具体实施方式

下文中将参考附图并结合实施例来详细说明本发明的实施例。

需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。

本申请实施例中所提供的方法实施例可以在移动终端、计算机终端或者类似的运算装置中执行。以运行在移动终端上为例，图1是本发明实施例的一种运动控制方法的移动终端的硬件结构框图。如图1所示，移动终端可以包括一个或多个(图1中仅示出一个)处理器102(处理器102可以包括但不限于微处理器MCU或可编程逻辑器件FPGA等的处理装置)和用于存储数据的存储器104，其中，上述移动终端还可以包括用于通信功能的传输设备106以及输入输出设备108。本领域普通技术人员可以理解，图1所示的结构仅为示意，其并不对上述移动终端的结构造成限定。例如，移动终端还可包括比图1中所示更多或者更少的组件，或者具有与图1所示不同的配置。

存储器104可用于存储计算机程序，例如，应用软件的软件程序以及模块，如本发明实施例中的运动控制方法对应的计算机程序，处理器102通过运行存储在存储器104内的计算机程序，从而执行各种功能应用以及数据处理，即实现上述的方法。存储器104可包括高速随机存储器，还可包括非易失性存储器，如一个或者多个磁性存储装置、闪存、或者其他非易失性固态存储器。在一些实例中，存储器104可进一步包括相对于处理器102远程设置的存储器，这些远程存储器可以通过网络连接至移动终端。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

传输设备106用于经由一个网络接收或者发送数据。上述的网络具体实例可包括移动终端的通信供应商提供的无线网络。在一个实例中，传输设备106包括一个网络适配器(Network Interface Controller，简称为NIC)，其可通过基站与其他网络设备相连从而可与互联网进行通讯。在一个实例中，传输设备106可以为射频(Radio Frequency，简称为RF)模块，其用于通过无线方式与互联网进行通讯。

在本实施例中提供了一种运动控制方法，图2是根据本发明实施例的运动控制方法的流程图，如图2所示，该流程包括如下步骤：

步骤S202，确定目标设备的第一运动状态；

步骤S204，确定目标子设备的测量运动参数，其中，所述目标子设备为所述目标设备中包括的设备，所述测量运动参数为通过目标传感器测量得到的参数；

步骤S206，基于所述第一运动状态确定所述测量运动参数的可信度；

步骤S208，基于所述可信度对所述测量运动参数进行目标滤波处理，以确定所述目标子设备的目标运动参数；

步骤S210，控制所述目标子设备基于所述目标运动参数运动。

在上述实施例中，目标设备可以为无人机，目标子设备可以为无人机中的云台。目标传感器可以为云台航向编码器、姿态检测传感器等。目标滤波处理可以为卡尔曼滤波。当目标设备为无人机时，第一运动状态可以包括悬停状态、运动状态等。测量运动参数可以为云台航向编码器测量得到的角度数据、飞行运动数据，如云台的航向角速度等。根据测量得到的航向角速度及目标设备的运动状态，确定目标子设备的航向角速度的偏差，并补偿该偏差确定目标运动参数，控制目标子设备按照目标运动参数运动。

在上述实施例中，在飞机处于悬停的情况下飞机是在静止状态，此时云台航向轴角速度随时间的积分等于云台航向轴编码器测量的角度数据。所以在飞机处于悬停状态时需要将航向轴编码器检测的角度数据作为航向轴角速度的参考值，参与计算航向轴角速度的漂移计算与补偿。当飞机处于飞行状态，飞机实在运动。此时，编码器检测的角度数据不等于航向轴加速度的积分，所以不能用于航向轴角速度的参考量参与漂移的估计与补偿。基于上述特性，当飞机处于悬停状态，航向轴编码器角度可信度较高，所以需要将卡尔曼滤波器参数调的较小，使其占更大比重，修正角速度漂移。相反，在飞机运动状态下云台编码器角度可信度较低，需要将卡尔曼滤波器参数调大，减小其站的比重。所以可以通过第一步获取飞机飞行状态，拟合关于飞机运动状态的曲线，动态调整参数使漂移补偿算法鲁棒更强，速度漂移补偿更加准确。

可选地，上述步骤的执行主体可以是后台处理器，或者其他的具备类似处理能力的设备，还可以是至少集成有数据处理设备的机器，其中，数据处理设备可以包括计算机、手机等终端，但不限于此。

通过本发明，确定目标设备的第一运动状态，并确定目标设备中包括的目标子设备的测量运动参数，根据第一运动状态确定测量运动参数的可信度，根据可信度对测量运动参数进行目标滤波处理，以确定目标子设备的目标运动参数，控制目标子设备根据目标运动参数运动。由于根据目标设备的第一运动状态确定测量运动参数的可信度，并根据可信度对目标测量运动参数进行目标滤波处理，可以准确地确定出目标运动参数，不会受到干扰，并且无需利用其他装置，节约了成本。因此，可以解决相关技术中存在的运动参数调节不准确、容易受到干扰且成本高的问题，提高运动参数调节的准确率，降低了成本。

在一个示例性实施例中，确定目标设备的第一运动状态包括：获取所述目标设备的第一运动参数；使用第一模型对所述第一运动参数进行分析，以确定所述目标设备的第一运动状态，其中，所述第一模型为使用多组第一训练数据通过机器学习训练出的，所述多组第一训练数据中的每组数据包括：设备的运动参数和运动状态。在本实施例中，可以通过第一模型确定目标设备的第一运动状态，其中，第一模型可以为DNN模型，第一运动参数可以为目标设备的运动状态。当目标设备为无人机时，可以从飞机获取飞行状态数据，包括飞机的三轴角速度(Gx，Gy，Gz)、三轴线速度(Vx，Vy，Vz)、三轴加速度(Ax，Ay，Az)以及飞机位置坐标(Px，Py，Pz)。将飞机的12个数据传入训练好的深度学习模型，通过前向推理得到飞机的状态Y(稳定悬停或运动)。将状态Y传递给卡尔曼滤波器，动态调节参数(对应于上述可信度)。可信度可以用公式表示，Y表示目标设备的运动状态(用0-1中包括的任意一个数据表示)，F为预先确定的参数。其中，Y可以通过(0,1)中任意一个数据表示，数据越靠近1，表示第一运动状态为运动状态的概率越大，越靠近0，表示第一运动状态为悬停状态的概率越小。利用第一模型确定目标设备的第一运动状态示意图可参见附图3。其中，第一模型可以为DNN模型、CNN模型等。

在一个示例性实施例中，在基于所述可信度及目标滤波器确定目标运动参数之前，所述方法还包括：基于所述测量运动参数确定所述目标子设备的第一运动状态函数；基于所述目标子设备所上报的运动参数确定所述目标子设备的第二运动状态函数；对所述第一运动状态函数及所述第二运动状态函数进行目标处理以得到所述目标滤波处理中所包括的处理操作。在本实施例中，在基于可信度及目标滤波器确定目标运动参数之前，可以根据测量运动参数确定目标子设备的第一运动状态函数，根据目标子设备所上报的运动参数确定所述目标子设备的第二运动状态函数。即建立目标子设备的状态空间方程，第一运动状态函数可以表示为z＝h(x)+w_z(1)，第二运动状态函数可以表示为其中，h(x)表示目标子设备的测量运动函数，f(x)表示目标子设备的实际运动函数，ω_x、ω_z为噪声误差。目标处理可以为对状态空间方程进行一阶段泰勒展开，同时离散化得到标准的卡尔曼滤波。

其中，卡尔曼滤波如下：

P_k＝(I-K_kH_k)P_k,k-1………………………………(8)

其中，云台航向轴角速度，K_k为滤波增益，为云台编码器测出旋转角速度的残差(量测误差)，z_k云台编码器得到的旋转角度，Φ_k,k-1为状态转移矩阵(通过f(x)求偏导得到)，H_k为量测矩阵(通过h(x)求偏导得到)，P_k为协方差矩阵，为系统噪声(预先确定)，为量测噪声(对应于上述可信度)，I为单位矩阵。一般的卡尔曼融合中会取一个定值，过大更多相信递推，量测数据不能及时起作用；过小更多相信量测，量测数据能够及时起作用。

在一个示例性实施例中，基于所述可信度及对所述测量运动参数进行目标滤波处理，以确定所述目标子设备的目标运动参数包括：基于所述第一运动状态函数确定所述目标子设备的量测矩阵；基于所述量测矩阵、所述可信度以及历史测量运动参数的协方差矩阵确定所述测量运动参数的滤波增益，其中，所述运动参数调节的操作是循环执行，在当前次运动参数调节的操作为首次执行时，所述历史测量运动参数为预设测量运动参数，在当前次运动参数调节的操作为非首次执行时，所述历史测量运动参数为前一次执行所述运动参数调节的操作时所得到的测量运动参数；基于量测矩阵确定所述测量运动参数的量测误差；基于所述滤波增益及所述量测误差确定所述目标运动参数。在本实施例中，可以根据第一运动状态函数z＝h(x)+w_z中包括的h(x)求偏导得到目标子设备的量测矩阵，根据量测矩阵、可信度以及历史测量运动参数的协方差矩阵确定测量运动参数的滤波增益。当运动参数调节的操作是首次执行时，目标滤波初始化，可以得到初始化的协方差矩阵，则可以利用初始化的协方差矩阵确定量测运动参数的滤波增益。当运动参数调节的操作时非首次执行的，可以通过前一次执行得到的协方差矩阵，则可以利用前一次执行得到的协方差矩阵确定测量运动参数的滤波增益。还可以通过量测矩阵确定云台编码器测出旋转角速度的残差(即量测误差)。通过量测误差和滤波增益确定目标运动参数。

在一个示例性实施例中，基于所述量测矩阵、所述可信度以及历史测量运动参数的协方差矩阵确定所述测量运动参数的滤波增益包括：确定所述量测矩阵、所述协方差矩阵、所述量测矩阵的转置矩阵三者的乘积；确定所述乘积以及所述可信度的和的逆矩阵；将所述协方差矩阵、所述量测矩阵的转置矩阵以及所述逆矩阵的乘积确定为所述滤波增益。在本实施例中，在确定量测矩阵、可信度以及历史测量运动参数的协方差矩阵之后，可以通过公式确定滤波增益。

在一个示例性实施例中，基于量测矩阵确定所述测量运动参数的量测误差包括：确定所述测量运动参数中包括的偏转角度；确定所述目标子设备所上报的所述运动参数中包括的第一速度；将所述偏转角度与第一乘积的差确定为所述量测误差，其中，第一乘积为所述量测矩阵及所述第一速度的乘积。在本实施例中，可以通过公式确定量测误差。其中，z_k云台编码器得到的旋转角度，为目标子设备所上报的运动参数中包括的第一速度，其中，第一速度可以为角速度。

在一个示例性实施例中，确定所述目标子设备的运动参数中包括的第一速度包括：基于所述第二运动状态函数确定所述目标子设备的状态转移矩阵；将所述状态转移矩阵与历史第一速度的乘积确定为所述第一速度，其中，所述运动参数调节的操作是循环执行，将所述状态转移矩阵与历史第一速度的乘积确定为所述第一速度，其中，所述运动参数调节的操作是循环执行，在当前次运动参数调节的操作为首次执行时，所述状态转移矩阵为预设转移矩阵，在当前次运动参数调节的操作为非首次执行时，所述状态转移矩阵为前一次执行所述运动参数调节的操作时所得到的转移矩阵。在本实施例中，可以用公式表示目标子设备的运动参数中包括的第一速度，其中，Φ_k,k-1为状态转移矩阵。在当前次运动参数调节的操作为首次执行时，状态转移矩阵为预测转移矩阵，即初始化时预先设定的预测转移矩阵。在当前次运动参数调节为非首次执行时，状态转移矩阵为前一次执行运动参数调节操作时所得到的转移矩阵。

在一个示例性实施例中，基于所述滤波增益及所述量测误差确定所述目标运动参数包括：确定所述滤波增益及所述量测误差的乘积；将所述滤波增益及所述量测误差的乘积与所述第一速度的和确定为所述目标运动参数。在本实施例中，可以通过公式表示目标运动参数，即根据更新后的滤波增益及量测误差，更新状态量得到融合角速度，即目标运动参数。

在上述实施例中，在计算出目标运动参数后，可以控制目标子设备按照目标运动参数运动，同时计算此时的协方差矩阵，可以通过公式P_k＝(I-K_kH_k)P_k,k-1计算协方差矩阵，基于计算得到的协方差矩阵计算滤波增益，循环执行上述步骤，确定出新的目标运动参数，控制目标子设备按照新的目标运动参数运动。

下面结合具体实施方式对运动控制方法进行说明：

图4是根据本发明具体实施例的运动控制方法流程图，如图4所示，该流程包括：

(1)获取飞机的运动数据，以及云台的编码器角度数据；

(2)将获取的飞机运动数据导入到基于深度学习模型的无人机状态估计器中。

(3)得到上一步飞机的状态是稳定悬停还是运动；

(4)根据飞机的运动状态，动态调节参数

(5)使用卡尔曼滤波器对云台航向编码器角度数据和云台航向角速度进行融合，对速度漂移进行补偿；

(6)得到漂移补偿后的航向轴角速度用于云台控制。

其中，卡尔曼角度融合方案详细的算法如下：

建立状态空间方程(1)、(2)式。

z＝h(x)+w_z………………………………………(2)

将(1)、(2)一阶段泰勒展开同时离散化的得到标准的卡尔曼滤波。

P_k＝(I-K_kH_k)P_k,k-1………………………………(8)

结合第一部分估计的无人机飞行状态Y动态计算

其中，云台航向轴角速度，K_k为滤波增益，为云台编码器测出旋转角速度的残差(量测误差)，z_k云台编码器得到的旋转角度，Φ_k,k-1为状态转移矩阵(通过f(x)求偏导得到)，H_k为量测矩阵(通过h(x)求偏导得到)，P_k为协方差矩阵，为系统噪声(预先确定)，为量测噪声(对应于上述可信度)，I为单位矩阵。一般的卡尔曼融合中会取一个定值，过大更多相信递推，量测数据不能及时起作用；过小更多相信量测，量测数据能够及时起作用。

利用卡尔曼滤波器对云台航向编码器角度数据和云台航向角速度进行融合，对速度漂移进行补偿具体步骤如下：

1)卡尔曼初始化——P、K及状态预测(即状态转移矩阵)初始化；

2)获取飞机状态估计器输出的飞机运动状态Y；

3)利用P、K数值计算预测协方差——公式(7)，同时获取实际姿态传感器反馈的航向角速度——公式(4)；

4)利用飞机的运动状态Y计算量测噪声——公式(9)，得到动态通过公式(6)更新滤波增益，同时将云台航向编码器角度数据输入公式(5)，更新量测误差；

5)利用跟新后的滤波增益及量测误差，更新状态量得到融合角速度——公式(3)；

6)计算协方差矩阵——公式(8)；

7)云台航向编码器角度数据，云台航向轴角速度数据更新后重复2)～6)步骤；

8)得到补偿温度漂移后的航向轴角速度。

在前述实施例中，通过利用飞机飞行数据，使用深度学习模型估计当前飞机运动状态。使用云台航向编码器获得角度和云台上面姿态检测传感器检测的速度进行卡尔曼融合，并随着飞机的运动不同动态切换实现对编码器获得角度和姿态检测传感器的信任度的动态调节，得到补偿漂移的航向轴速度。在卡尔曼融合时根据飞行状态，拟合曲线动态改变参数进而动态调整融合增益，增加算法适应性。将补偿漂移后的航向速度用于图像增稳系统中提高图像稳定性。解决了在存在环境干扰的场景，也可以对云台航向轴速度漂移补偿。也解决了航向轴没有角度环航向轴漂移问题，提高了画面的稳定性。通过计算补偿传感器缺陷带来的漂移问题，使云台控制更加稳定。此外，本发明无需外加任何传感器，降低了成本。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到根据上述实施例的方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如ROM/RAM、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，或者网络设备等)执行本发明各个实施例所述的方法。

在本实施例中还提供了一种运动控制装置，该装置用于实现上述实施例及优选实施方式，已经进行过说明的不再赘述。如以下所使用的，术语“模块”可以实现预定功能的软件和/或硬件的组合。尽管以下实施例所描述的装置较佳地以软件来实现，但是硬件，或者软件和硬件的组合的实现也是可能并被构想的。

图5是根据本发明实施例的运动控制装置的结构框图，如图5所示，该装置包括：

第一确定模块502，用于确定目标设备的第一运动状态；

第二确定模块504，用于确定目标子设备的测量运动参数，其中，所述目标子设备为所述目标设备中包括的设备，所述测量运动参数为通过目标传感器测量得到的参数；

第三确定模块506，用于基于所述第一运动状态确定所述测量运动参数的可信度；

滤波模块508，用于基于所述可信度对所述测量运动参数进行目标滤波处理，以确定所述目标子设备的目标运动参数；

控制模块510，用于控制所述目标子设备基于所述目标运动参数运动。

在一个示例性实施例中，第一确定模块502可以通过如下方式实现确定目标设备的第一运动状态：获取所述目标设备的第一运动参数；使用第一模型对所述第一运动参数进行分析，以确定所述目标设备的第一运动状态，其中，所述第一模型为使用多组第一训练数据通过机器学习训练出的，所述多组第一训练数据中的每组数据包括：设备的运动参数和运动状态。

在一个示例性实施例中，所述装置可以用于在基于所述可信度及目标滤波器确定目标运动参数之前，基于所述测量运动参数确定所述目标子设备的第一运动状态函数；基于所述目标子设备所上报的运动参数确定所述目标子设备的第二运动状态函数；对所述第一运动状态函数及所述第二运动状态函数进行目标处理以得到所述目标滤波处理中所包括的处理操作。

在一个示例性实施例中，所述装置可以通过如下方式实现基于所述可信度及对所述测量运动参数进行目标滤波处理，以确定所述目标子设备的目标运动参数：基于所述第一运动状态函数确定所述目标子设备的量测矩阵；基于所述量测矩阵、所述可信度以及历史测量运动参数的协方差矩阵确定所述测量运动参数的滤波增益，其中，所述运动参数调节的操作是循环执行，在当前次运动参数调节的操作为首次执行时，所述历史测量运动参数为预设测量运动参数，在当前次运动参数调节的操作为非首次执行时，所述历史测量运动参数为前一次执行所述运动参数调节的操作时所得到的测量运动参数；基于量测矩阵确定所述测量运动参数的量测误差；基于所述滤波增益及所述量测误差确定所述目标运动参数。

在一个示例性实施例中，所述装置可以通过如下方式实现基于所述量测矩阵、所述可信度以及历史测量运动参数的协方差矩阵确定所述测量运动参数的滤波增益：确定所述量测矩阵、所述协方差矩阵、所述量测矩阵的转置矩阵三者的乘积；确定所述乘积以及所述可信度的和的逆矩阵；将所述协方差矩阵、所述量测矩阵的转置矩阵以及所述逆矩阵的乘积确定为所述滤波增益。

在一个示例性实施例中，所述装置可以通过如下方式实现基于量测矩阵确定所述测量运动参数的量测误差：确定所述测量运动参数中包括的偏转角度；确定所述目标子设备所上报的所述运动参数中包括的第一速度；将所述偏转角度与第一乘积的差确定为所述量测误差，其中，第一乘积为所述量测矩阵及所述第一速度的乘积。

在一个示例性实施例中，所述装置可以通过如下方式实现确定所述目标子设备的运动参数中包括的第一速度：基于所述第二运动状态函数确定所述目标子设备的状态转移矩阵；将所述状态转移矩阵与历史第一速度的乘积确定为所述第一速度，其中，所述运动参数调节的操作是循环执行，在当前次运动参数调节的操作为首次执行时，所述状态转移矩阵为预设转移矩阵，在当前次运动参数调节的操作为非首次执行时，所述状态转移矩阵为前一次执行所述运动参数调节的操作时所得到的转移矩阵。

在一个示例性实施例中，所述装置可以通过如下方式实现基于所述滤波增益及所述量测误差确定所述目标运动参数：确定所述滤波增益及所述量测误差的乘积；将所述滤波增益及所述量测误差的乘积与所述第一速度的和确定为所述目标运动参数。

需要说明的是，上述各个模块是可以通过软件或硬件来实现的，对于后者，可以通过以下方式实现，但不限于此：上述模块均位于同一处理器中；或者，上述各个模块以任意组合的形式分别位于不同的处理器中。

本发明的实施例还提供了一种计算机可读存储介质，该计算机可读存储介质中存储有计算机程序，其中，该计算机程序被设置为运行时执行上述任一项方法实施例中的步骤。

在一个示例性实施例中，上述计算机可读存储介质可以包括但不限于：U盘、只读存储器(Read-Only Memory，简称为ROM)、随机存取存储器(Random Access Memory，简称为RAM)、移动硬盘、磁碟或者光盘等各种可以存储计算机程序的介质。

本发明的实施例还提供了一种电子装置，包括存储器和处理器，该存储器中存储有计算机程序，该处理器被设置为运行计算机程序以执行上述任一项方法实施例中的步骤。

在一个示例性实施例中，上述电子装置还可以包括传输设备以及输入输出设备，其中，该传输设备和上述处理器连接，该输入输出设备和上述处理器连接。

本实施例中的具体示例可以参考上述实施例及示例性实施方式中所描述的示例，本实施例在此不再赘述。

显然，本领域的技术人员应该明白，上述的本发明的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，并且在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本发明不限制于任何特定的硬件和软件结合。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。