CN105116926A - 云台控制方法和装置 - Google Patents

Abstract

本发明公开了一种云台控制方法，获取云台上被控端当前姿态和目标姿态、固定端当前姿态，以及固定端与被控端的转动惯量比；根据所述被控端当前姿态和目标姿态、固定端当前姿态，以及固定端与被控端的转动惯量比，计算获取控制被控端的有效机械能；根据所述有效机械能，控制所述被控端保持在稳定状态。本发明还公开了一种云台控制装置。本发明使被控端达到目标姿态并保持在稳定状态，实现了对云台的精确控制，有效隔离机体扰动，达到了云台自稳控制效果，保障了云台负载相机的成像质量。

Description

云台控制方法和装置

技术领域

本发明涉及云台技术领域，尤其涉及一种云台控制方法和装置。

背景技术

云台是安装、固定像机的支撑设备，三轴云台可以控制相机进行俯仰、横滚、航向三个轴线的动作，具有很高的灵活性。

三轴云台固定在无人机上，作为机载相机的载体，可以用来隔离扰动，保证无人机机载相机的视轴稳定。理想状态下，在相机拍摄的过程中，无人机始终保持水平、匀速地飞行，并且还要保持无人机姿态的稳定，三轴云台控制相机达到目标姿态，使机载相机视轴能够均匀地扫描地面，从而能够获得高质量、无失真的图像。

但是，实际中存在着许多非线性干扰因素，空中的风速、气流以及风向的变化都会引起无人机的姿态发生变化，且当无人机体积越小、重量越轻时，这种影响就越为严重。无人机姿态变化的直接影响是三轴云台受到干扰不稳定，若三轴云台控制精度差，则不能有效隔离无人机的扰动，从而导致负载的相机光轴会产生抖动，获得的图像发生畸变。

因此，三轴云台控制精度越高，机载相机获取的图像的质量就越清晰。但是，当前的三轴云台控制精度较差，就会导致不能够精确调整相机姿态达到目标姿态，不能有效隔离机体的扰动，自稳效果差，无法保障云台负载相机的成像质量。

发明内容

本发明的主要目的在于提供一种云台控制方法和装置，旨在解决云台控制不精确、自稳效果差的技术问题。

为实现上述目的，本发明提供一种云台控制方法，所述云台控制方法包括以下步骤：

获取云台上被控端当前状态和目标状态、固定端当前状态，以及固定端与被控端的转动惯量比；

根据所述被控端当前状态和目标状态、固定端当前状态，以及固定端与被控端的转动惯量比，计算获取控制被控端的有效机械能；

根据所述有效机械能，控制所述被控端保持在稳定状态。

优选的，所述获取云台上被控端当前状态和目标状态、固定端当前状态，以及固定端与被控端的转动惯量比的步骤包括：

获取被控端的角速度和姿态四元数；

根据所述被控端的姿态四元数，获取被控端的旋转矩阵；

根据所述被控端的旋转矩阵，获取被控端的欧拉角，得到被控端当前姿态；

获取被控端的目标姿态角和目标角速度，作为被控端目标状态；

获取固定端的角速度和姿态角，作为固定端当前状态；

获取固定端与被控端的转动惯量比。

优选的，所述根据所述被控端当前状态和目标状态、固定端当前状态，以及固定端与被控端的转动惯量比，计算获取控制被控端的有效机械能的步骤包括：

根据所述被控端当前状态和目标状态，以及固定端当前状态，获取控制被控端的补偿角速度；

根据所述控制被控端的补偿角速度、固定端与被控端的转动惯量比以及固定端当前状态，计算获取控制被控端的有效机械能。

优选的，所述根据所述被控端当前状态和目标状态，以及固定端当前状态，获取控制被控端的补偿角速度的步骤包括：

根据所述固定端当前状态获取固定端在云台电机轴上的角速度；

根据所述被控端当前姿态和目标姿态，获取姿态角差值；

根据所述被控端的当前状态、固定端在云台电机轴上的角速度和所述姿态角差值，获取控制被控端的补偿角速度。

优选的，所述根据所述有效机械能，控制所述被控端保持在稳定状态的步骤包括：

根据所述有效机械能，获取控制所述被控端的目标电流；

根据所述目标电流，通过云台电机轴控制所述被控端保持在稳定状态。

此外，为实现上述目的，本发明还提供一种云台控制装置，所述云台控制装置包括：

获取模块，用于获取云台上被控端当前状态和目标状态、固定端当前状态，以及固定端与被控端的转动惯量比；

计算模块，用于根据所述被控端当前状态和目标状态、固定端当前状态，以及固定端与被控端的转动惯量比，计算获取控制被控端的有效机械能；

控制模块，用于根据所述有效机械能，控制所述被控端保持在稳定状态。

优选的，所述获取模块包括：

传感单元，用于获取被控端的角速度和姿态四元数；

矩阵单元，用于根据所述被控端的姿态四元数，获取被控端的旋转矩阵；

姿态单元，用于根据所述被控端的旋转矩阵，获取被控端的欧拉角，得到被控端当前姿态；

获取单元，用于获取被控端的目标姿态角和目标角速度，作为被控端目标状态；获取固定端与被控端的转动惯量比；

所述传感单元还用于，获取固定端的角速度和姿态角，作为固定端当前状态。

优选的，所述计算模块包括：

速度计算单元，用于根据所述被控端当前状态和目标状态，以及固定端当前状态，获取控制被控端的补偿角速度；

能量计算单元，用于根据所述控制被控端的补偿角速度、固定端与被控端的转动惯量比以及固定端当前状态，计算获取控制被控端的有效机械能。

优选的，所述速度计算单元包括：

干扰子单元，用于根据所述固定端当前状态获取固定端在云台电机轴上的角速度；

差值子单元，用于根据所述被控端当前姿态和目标姿态，获取姿态角差值；

补偿子单元，用于根据所述被控端的当前状态、固定端在云台电机轴上的角速度和所述姿态角差值，获取控制被控端的补偿角速度。

优选的，所述控制模块包括：

电流单元，用于根据所述有效机械能，获取控制所述被控端的目标电流；

控制单元，用于根据所述目标电流，通过云台电机轴控制所述被控端保持在稳定状态。

本发明实施例提出的一种云台控制方法和装置，通过获取的云台上被控端当前状态和目标状态，固定端当前状态，以及固定端与被控端的转动惯量比，计算得到控制被控端的有效机械能，根据该有效机械能控制所述被控端，使被控端达到目标姿态并保持在稳定状态，实现了对云台的精确控制，有效隔离机体扰动，达到了云台自稳控制效果，保障了云台负载相机的成像质量。

附图说明

图1为本发明云台控制方法第一实施例的流程示意图；

图2为本发明云台控制方法第二实施例的流程示意图；

图3为本发明云台控制方法第三实施例的流程示意图；

图4为本发明云台控制方法第四实施例的流程示意图；

图5为本发明云台控制方法第五实施例的流程示意图；

图6为本发明云台控制装置第一实施例的功能模块示意图；

图7为本发明云台控制装置第二实施例的功能模块示意图；

图8为本发明云台控制装置第三实施例的功能模块示意图；

图9为本发明云台控制装置第四实施例的功能模块示意图；

图10为本发明云台控制装置第五实施例的功能模块示意图。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

本发明实施例的主要解决方案是：获取云台上被控端当前状态和目标状态、固定端当前状态，以及固定端与被控端的转动惯量比；根据所述被控端当前状态和目标状态、固定端当前状态，以及固定端与被控端的转动惯量比，计算获取控制被控端的有效机械能；根据所述有效机械能，控制所述被控端保持在稳定状态。

由于现有技术对云台的控制精确度差，不能够精确调整相机姿态达到的目标姿态，不能有效隔离机体产生的扰动，自稳效果差。因此，在机体的扰动下云台不能够保持相机状态的稳定，也无法保障机载相机的成像质量。

本发明提供一种解决方案，采集飞行器(也即与飞行器固定的那部分云台)和负载相机的状态(包括姿态和速度)信息，通过速度补偿，获取给与云台电机轴的有效机械能，通过云台电机轴的精准转动，控制相机达到目标姿态，并保持相机在稳定状态。

参照图1，本发明云台控制方法第一实施例提供一种云台控制方法，所述云台控制方法包括：

步骤S10、获取云台上被控端当前状态和目标状态、固定端当前状态，以及固定端与被控端的转动惯量比。

本实施例方案主要应用于三轴云台中，当然也不限于其他具有精确控制处理需求的单轴或二轴云台。云台可应用于飞行器、汽车、轮船、机器人、人体等，例如：当云台应用于飞行器时，为机载云台，可负载拍摄设备或摄影设备，进行动态拍摄或动态摄影。

本实施例以无人机三轴云台进行举例，三轴云台固定在无人机体上，且三轴云台搭载有相机。其中，三轴云台通过三个电机轴分别控制相机进行俯仰、横滚和航向方向的转动，具体的：俯仰电机轴控制相机进行俯仰转动，横滚电机轴控制相机进行横滚转动，航向电机轴控制相机进行航向转动。

本实施例在云台启动后，首先进行信息采集：获取被控端当前状态、固定端当前状态；获取被控端目标状态，获取固定端与被控端的转动惯量比。

其中，被控端为云台负载相机部分，与被控端相对，固定端可以为与无人机相连的云台部分，也可能包括与其刚性连接的无人机；以固定端为云台进行举例，云台通过云台上的三个电机轴与被控端连接，分别控制被控端绕俯仰电机轴、横滚电机轴和航向电机轴转动。

具体的，作为一种实施方式，在云台启动后，被控端当前姿态可以通过传感器获得。被控端的当前状态包括：被控端的角速度和被控端的姿态(即被控端的欧拉角)。

被控端目标姿态为预先设置的，可以直接获取。被控端目标状态包括：被控端的目标姿态角和目标角速度。

固定端当前状态可以通过传感器获得。固定端当前状态包括：固定端的角速度和当前姿态角。

固定端与被控端的转动惯量比为预设系数，可以通过测量得到。

需要说明的是，传感器可以为三轴陀螺仪和三轴加速度计，也可以为其他传感器，可根据实际需要灵活设置。

步骤S20、根据所述被控端当前状态和目标状态、固定端当前状态，以及固定端与被控端的转动惯量比，计算获取控制被控端的有效机械能。

在获取被控端当前状态和目标状态，固定端当前状态，以及固定端与被控端的转动惯量比之后，通过计算获取控制被控端的有效机械能。

具体的，作为一种实施方式，首先，根据被控端当前状态和目标状态，以及固定端当前状态，获取控制被控端的补偿角速度，该补偿角速度为：云台电机轴转动时所给予被控端的补偿角速度，能够使被控端隔离机体扰动，达到目标姿态并保持在稳定状态。

然后，根据控制被控端的补偿角速度、固定端与被控端的转动惯量比以及固定端当前状态，计算获取控制被控端的有效机械能，该有效机械能为：控制云台电机轴转动使被控端达到目标姿态并保持在稳定状态时，电机传出的有效机械能。

步骤S30、根据所述有效机械能，控制所述被控端保持在稳定状态。

在获取有效机械能后，根据获取的有效机械能，计算获得控制被控端的目标电流。由于是通过控制云台各电机轴转动，来控制被控端在俯仰、横滚和航向三个方向的转动，因此，控制被控端的目标电流也即控制云台电机轴的目标电流；

然后根据获取的目标电流，控制云台各电机轴转动，从而控制被控端达到目标姿态并保持在稳定状态。

在本实施例中，通过获取的云台上被控端当前状态和目标状态、固定端当前状态，以及固定端与被控端的转动惯量比，计算得到控制被控端的有效机械能，根据该有效机械能控制所述被控端，使被控端达到目标姿态并保持在稳定状态，有效隔离机体扰动，实现了对云台的精确控制，达到了云台自稳控制效果，保障了云台负载相机的成像质量。

进一步的，参照图2，本发明第二实施例提供一种云台控制方法，基于图1所示的实施例，所述步骤S10包括：

步骤S11、获取被控端的角速度和姿态四元数。

在云台启动后，可以通过传感器获取被控端的角速度和姿态四元数，也即云台负载相机的当前角速度和姿态四元数；其中，所获取的姿态四元数是由实数加上三个虚数单位i、j、k组成，i、j、k为相互正交的单位矢量；姿态四元数可以表征被控端在三维空间中的旋转。由此，所获取的姿态四元数包含了被控端的姿态信息。

上述被控端的角速度和姿态四元数是使用被控端的机体坐标系b作为基准得到的。

其中，机体坐标系b为空间直角坐标系，包括三个坐标轴，令x轴为俯仰轴，对应云台的俯仰电机轴，云台的俯仰电机轴转动，控制被控端绕俯仰轴进行俯仰转动；y轴为横滚轴，对应云台的横滚电机轴，云台的横滚电机轴转动，控制被控端绕横滚轴进行横滚转动；z轴为航向轴，对应云台的航向电机轴，云台的航向电机轴转动，控制被控端绕航向轴进行航向转动。

以下详细阐述使用被控端的机体坐标系b作为基准得到被控端的角速度和姿态四元数的具体过程：

作为一种实施例，通过传感器获取被控端的角速度为Ω：

$\mathrm{\Ω}=\left[\begin{array}{c}{{\mathrm{\ω}}^{\′}}_x\\ {{\mathrm{\ω}}^{\′}}_y\\ {{\mathrm{\ω}}^{\′}}_z\end{array}\right],$ 其中，ω′_x为被控端在俯仰轴上的角速度，ω′_x为被控端在横滚轴上的角速度，ω′_z为被控端在航向轴上的角速度。

为提高被控端当前姿态的精度，采用四元数乘法的近似算法计算得到被控端当前的姿态四元数，被控端的姿态四元数的计算过程如下：

任意选取姿态四元数满足模为1，记录为Q₀作为初始值，在前一时刻的姿态四元数的基础上进行下述计算：首先，通过传感器获取的被控端上一时刻姿态四元数为Q_n-1；其中，Q_n-1＝q′₀+q′₁×i+q′₂×j+q′₃×k，i、j、k为相互正交的单位矢量；

获取被控端的重力加速度：G₀＝2(q′₁·q′₃-q′₀·q′₂)^T；

计算当前传感器的校准值e为：

然后，计算得到被控端当前的姿态四元数Q_n，计算公式为：

Q_n＝Q_n-1+Q_n-1×(Ω+e)；其中，Q_n＝q₀+q₁i+q₂×j+q₃×k，i、j、k为相互正交的单位矢量。

由于传感器的校准值e的不断修正，在有限步后，姿态四元素会收敛到准确值，即可获得被控端的姿态四元数Q_n，也即该收敛后的准确值为姿态四元数Q_n。

步骤S12、根据所述被控端的姿态四元数，获取被控端的旋转矩阵。

在获取被控端的姿态四元数Q_n后，根据Q_n，计算被控端的姿态角由机体坐标系b转为大地坐标系R的转换矩阵计算公式为：

$C_b^R=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_2)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right];$

将Q_n中的q₀、q₁、q₂、q₃，代入上述公式计算，得到被控端的旋转矩阵记：

$C_b^R=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{\mathrm{22}}& T_{\mathrm{23}}\\ T_{31}& T_{\mathrm{32}}& T_{33}\end{array}\right].$

步骤S13、根据所述被控端的旋转矩阵，获取被控端的欧拉角，得到被控端当前姿态。

在获取被控端的旋转矩阵后，根据计算被控端的欧拉角；被控端的欧拉角包括：俯仰角θ、横滚角γ和航向角计算公式为：

将的值，代入上述公式计算，获得俯仰角θ、横滚角γ和航向角

由此，根据获取的被控端的角速度和欧拉角，得到被控端当前状态。

被控端当前状态包括：被控端的角速度Ω和被控端的欧拉角；其中，被控端的欧拉角包括：俯仰角θ、横滚角γ和航向角

步骤S14、获取被控端的目标姿态角和目标角速度，作为被控端目标状态。

被控端的目标姿态角可直接获取。被控端的目标姿态角可以是预设的目标姿态角，也可以是用户调节的目标姿态角，可根据实际需要灵活设置。被控端的目标姿态角包括：目标俯仰角θ′、目标横滚角γ′和目标航向角

被控端的目标角速度可直接获取。由于在被控端达到目标姿态后，若要保持在稳定状态，此时角速度应该为0，因此，在本实施例中，令被控端的目标角速度ω_t为预设的固定值：0。

由此，获取预先设置的被控端目标状态。所获取的被控端目标状态包括：被控端的目标角速度ω_t和被控端的目标姿态角；其中，被控端的目标姿态角包括：目标俯仰角θ′、目标横滚角γ′和目标航向角以及目标角速度ω_t。

步骤S15、获取固定端的角速度和姿态角，作为固定端当前状态。

固定端的角速度ω_b可以通过传感器获取，得到 ${\mathrm{\ω}}_b=\left[\begin{array}{c}{\mathrm{\ω}}_{bx}\\ {\mathrm{\ω}}_{by}\\ {\mathrm{\ω}}_{bz}\end{array}\right].$

固定端的旋转角度可以通过传感器获取，得到的固定端的姿态角为：横滚角θ_r，俯仰角θ_p，航向角θ_y。固定端的姿态角也即飞行器的姿态角。

由此，得到固定端当前状态。所得到的固定端当前状态包括：固定端的角速度ω_b和固定端的姿态角；其中，固定端的姿态角包括：横滚角θ_r，俯仰角θ_p，航向角θ_y。

步骤S16、获取固定端与被控端的转动惯量比。

固定端与被控端的转动惯量比为预设系数，可直接获取，得到固定端和被控端在云台电机轴上的转动惯量比k。得到的转动惯量比包括：固定端和被控端在俯仰电机轴上的转动惯量比k_p，固定端和被控端在横滚电机轴上的转动惯量比k_r，固定端和被控端在航向电机轴上的转动惯量比k_y。

需要说明的是，得到的转动惯量比为大量理论计算或实验数据所得，可根据实际需要进行数值调整。

在本实施例中，根据获取的被控端角速度和姿态四元数，获取被控端的旋转矩阵，从而获取所述被控端的欧拉角，所得到被控端当前姿态精度高；获取被控端目标状态、固定端当前状态、固定端与被控端的转动惯量比。本实施例中所获取的各项数据精度高，为后续控制被控端有效机械能的计算提供了精确的数据基础。

进一步的，参照图3，本发明第三实施例提供一种云台控制方法，基于上述图1或图2所示的实施例(本实施例以图1为例)，所述步骤S20包括：

步骤S21、根据所述被控端当前状态和目标状态，以及固定端当前状态，获取控制被控端的补偿角速度。

在机体扰动时，固定端会在云台电机轴上产生响应的干扰角速度，为隔离机体扰动，达到目标姿态，需控制云台电机轴转动对被控端产生补偿角速度，以使被控端达到目标姿态并保持状态稳定。

因此，在获取被控端当前状态和目标状态，固定端当前状态，以及固定端与被控端的转动惯量比之后，首先，计算获取控制被控端的补偿角速度，该补偿角速度为：云台电机轴转动时所给予被控端的补偿角速度，使被控端隔离机体扰动，能够达到目标姿态并保持稳定状态。

具体的，作为一种实施方式，首先，根据固定端当前的角速度：在俯仰电机轴上的角速度ω_pitch，在横滚电机轴上的角速度ω_roll，和在航向电机轴上的角速度ω_yaw。

根据被控端当前姿态角和目标姿态角，计算获取姿态角差值：俯仰角差值Δθ，横滚角差值Δγ和航向角差值

然后，根据被控端当前姿态角、固定端在云台三个电机轴上的角速度，以及姿态角差值，获取控制被控端的补偿角速度；控制被控端的补偿角速度包括：在俯仰电机轴上的补偿角速度ω_p，在横滚电机轴上的补偿角速度ω_r，和在航向电机轴上的补偿角速度ω_y。

步骤S22、根据所述控制被控端的补偿角速度、固定端与被控端的转动惯量比以及固定端当前状态，计算获取控制被控端的有效机械能。

在获取控制被控端的补偿角速度后，根据控制被控端的补偿角速度、固定端与被控端的转动惯量比以及固定端当前状态，计算获取控制被控端的有效机械能，该有效机械能为：控制云台电机轴转动，使被控端达到目标姿态并保持在稳定状态时，电机传出的有效机械能。

具体的，作为一种实施方式，可以根据转动惯量守恒和功能关系，列出方程组，计算控制被控端的有效机械能。由于是通过控制云台的各电机轴转动来控制被控端，因此，所得到的控制被控端的有效机械能也即控制云台各电机轴的有效机械能。

首先，获取的固定端与被控端的转动惯量比为：k，即三轴云台各轴上的k可表示为：k＝k_p，k_r，k_y；

一次控制后的云台各电机定子的角速度为ω_S，在后续的公式中可将其消掉；

获取的云台各电机轴上的补偿角速度为：即三轴云台各轴上的ω_c可表示为：ω_c＝ω_p，ω_r，ω_y；

根据固定端当前姿态获取的固定端在云台各电机轴上的角速度为：即三轴云台各轴上的ω_s可表示为ω_s＝ω_pitch，ω_roll，ω_yaw；

然后，令控制云台各电机轴的有效机械能为E，即三轴云台各轴上的E可表示为：E＝E_p，E_r，E_y；

根据转动惯量守恒和功能关系，则有方程组：

$\left\{\begin{array}{c}{\mathrm{k\ω}}_t+{\mathrm{\ω}}_s-({\mathrm{k\ω}}_c+{\mathrm{\ω}}_s)=0\\ {\mathrm{k\ω}}_t^2+{\mathrm{\ω}}_s^2-({\mathrm{k\ω}}_c^2+{\mathrm{\ω}}_s^2+E)=0\end{array}\right.;$

其中，ω_t为获取的被控端目标角速度，由于被控端到达目标姿态并保持稳定时，被控端目标速度为零，因此，被控端在云台各电机轴上的目标角速度ω_t的值均为0。

然后，求解上述方程组，得到：

$E=\frac{1}{2}{\mathrm{\ω}}_c^2(\frac{1}{k}-1)+{\mathrm{\ω}}_c{\mathrm{\ω}}_{\mathrm{\ϵ}};$

更为具体的：当计算控制俯仰电机轴的有效机械能时，令ω_s＝ω_pitch，ω_c＝ω_p，k＝k_p；此时，根据上述方程组计算得到控制俯仰电机轴的有效机械能E_p，能够使被控端在俯仰轴方向达到目标姿态并保持稳定状态；

当计算控制横滚电机轴的有效机械能时，令ω_s＝ω_roll，ω_c＝ω_r，k＝k_r；此时，根据上述方程组计算得到控制横滚电机轴的有效机械能E_r，能够使被控端在横滚轴方向达到目标姿态并保持稳定状态；

当计算控制航向电机轴的有效机械能时，令ω_s＝ω_yaw，ω_c＝ω_y，k＝k_y；此时，根据上述方程组计算得到控制航向电机轴的有效机械能E_y，能够使被控端在航向轴方向达到目标姿态并保持稳定状态。

由此，得到控制云台各电机轴的有效机械能：控制俯仰电机轴的有效机械能E_p、控制横滚电机轴的有效机械能E_r，和控制航向电机轴的有效机械能E_y。也即，得到控制被控端的有效机械能E_p、E_r和E_y。

其中，需要说明的是，E的符号代表电机轴转动方向，可根据实际情况具体确定。例如：+(正)为顺时针转动，—(负)为逆时针转动。

在本实施例中，根据获取的被控端当前状态和目标状态，固定端当前状态，以及固定端与被控端的转动惯量比，首先计算控制被控端的补偿角速度，然后计算控制被控端的有效机械能，能够控制云台电机轴转动使被控端达到目标姿态并保持在稳定状态。本实施例实现了对云台的精确控制，使云台根据得到的有效机械能，能够控制云台各电机轴转动，使被控端达到目标姿态并保持在稳定状态，达到了云台自稳控制效果。

进一步的，参照图4，本发明第四实施例提供一种云台控制方法，基于上述图3所示的实施例，所述步骤S21包括：

步骤S211、根据所述固定端当前状态获取固定端在云台电机轴上的角速度。

根据固定端当前状态可以获取固定端在云台电机轴上的角速度，固定端在云台电机轴上的角速度包括：在俯仰电机轴上的角速度ω_pitch，在横滚电机轴上的角速度ω_roll，和在航向电机轴上的角速度ω_yaw。计算固定端在云台各电机轴上的角速度，即为计算ω_pitch、ω_roll和ω_yaw。

具体的，作为一种实施方式，首先，固定端当前状态包括：固定端的角速度ω_b和固定端姿态角；其中， ${\mathrm{\ω}}_b=\left[\begin{array}{c}{\mathrm{\ω}}_{bx}\\ {\mathrm{\ω}}_{by}\\ {\mathrm{\ω}}_{bz}\end{array}\right];$ 固定端姿态角包括：横滚角θ_r，俯仰角θ_p，航向角θ_y。

然后，将固定端的角速度耦合到云台电机轴，根据坐标变换关系，得到固定端在航向电机轴上的角速度ω_yaw、ω_roll和ω_yaw：

$\begin{array}{c}\left[\begin{array}{ccc}{\mathrm{cos\θ}}_y& -{\mathrm{sin\θ}}_y& 0\\ {\mathrm{sin\θ}}_y& {\mathrm{cos\θ}}_y& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\θ}}_r& 0& -{\mathrm{sin\θ}}_r\\ 0& 1& 0\\ {\mathrm{sin\θ}}_r& 0& {\mathrm{cos\θ}}_r\end{array}\right]\left[\begin{array}{c}{w}_{pitch}\\ 0\\ 0\end{array}\right]\\ +\left[\begin{array}{ccc}{\mathrm{cos\θ}}_y& -{\mathrm{sin\θ}}_y& 0\\ {\mathrm{sin\θ}}_y& {\mathrm{cos\θ}}_y& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}0\\ w_{roll}\\ 0\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ w_{yaw}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_{bx}\\ {\mathrm{\ω}}_{by}\\ {\mathrm{\ωb}}_z\end{array}\right]\end{array};$

其中，θ_p，θ_r，θ_y分别为电机相对零位值的旋转角度，由磁编码器采集得到。根据公式可以得到ω_pitch、ω_roll和ω_yaw，由此，得到固定端在云台各电机轴上的角速度：ω_pitch、ω_roll和ω_yaw。

步骤S212、根据所述被控端当前姿态和目标姿态，获取姿态角差值。

姿态角差值为被控端由当前姿态达到目标姿态的角度差值，令姿态角差值为：俯仰角差值Δθ，横滚角差值Δγ和航向角差值

根据被控端目标姿态：目标俯仰角θ′、目标横滚角γ′和目标航向角和被控端当前的欧拉角：俯仰角θ、横滚角γ和航向角计算姿态角差值：

由此，得到姿态角差值，包括：俯仰角差值Δθ，横滚角差值Δγ和航向角差值

步骤S213、根据所述被控端的当前状态、固定端在云台电机轴上的角速度和所述姿态角差值，获取控制被控端的补偿角速度。

控制被控端的补偿角速度由隔离固定端干扰的反向补偿角速度和使被控端达到目标姿态的补偿角速度组成。

首先，根据被控端当前状态中的角速度 $\mathrm{\Ω}=\left[\begin{array}{c}{{\mathrm{\ω}}^{\′}}_x\\ {{\mathrm{\ω}}^{\′}}_y\\ {{\mathrm{\ω}}^{\′}}_z\end{array}\right],$ 固定端在云台电机轴上的角速度ω_pitch、ω_roll和ω_yaw，计算隔离固定端干扰的反向补偿角速度：

设反向补偿角速度为：横滚角速度δ_r、俯仰角速度δ_p、航向角速度δ_y；

则根据坐标变换关系，得到方程式：

$\begin{array}{c}\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\θ}}_p& {\mathrm{sin\θ}}_p\\ 0& -{\mathrm{sin\θ}}_p& {\mathrm{cos\θ}}_p\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\θ}}_r& 0& -{\mathrm{sin\θ}}_r\\ 0& 1& 0\\ {\mathrm{sin\θ}}_r& 0& {\mathrm{cos\θ}}_r\end{array}\right]\left[\begin{array}{c}0\\ 0\\ {\mathrm{\δ}}_y\end{array}\right]\\ +\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\θ}}_p& {\mathrm{sin\θ}}_p\\ 0& -{\mathrm{sin\θ}}_p& {\mathrm{cos\θ}}_p\end{array}\right]\left[\begin{array}{c}0\\ {\mathrm{\δ}}_r\\ 0\end{array}\right]+\left[\begin{array}{c}{\mathrm{\δ}}_p\\ 0\\ 0\end{array}\right]=\left[\begin{array}{c}{{\mathrm{\ω}}^{\′}}_x\\ {{\mathrm{\ω}}^{\′}}_y\\ {{\mathrm{\ω}}^{\′}}_z\end{array}\right]\end{array}$

解算上述方程，得到反向补偿角速度：横滚角速度δ_r、俯仰角速度δ_p和航向角速度δ_y。

然后，在本实施例中，取使被控端达到目标姿态的补偿角速度为：俯仰角速度A·Δθ，横滚角速度A·Δγ和航向角速度其中，A为预设系数，用于精确校准补偿角速度，可根据实际情况灵活设定。

然后，计算控制被控端的补偿角速度，取：在俯仰电机轴上的补偿角速度ω_p，在横滚电机轴上的补偿角速度和在航向电机轴上的补偿角速度ω_y。

则有如下计算公式：

由此，计算得到控制被控端的补偿角速度，包括：在俯仰电机轴上的补偿角速度ω_p，在横滚电机轴上的补偿角速度ω_r，和在航向电机轴上的补偿角速度ω_y。

在本实施例中，根据获取的被控端当前状态和目标状态、固定端当前状态，获取隔离固定端干扰的反向补偿角速度和使被控端达到目标角姿态的补偿角速度，得到控制被控端的补偿角速度。本实施例得到的补偿角速度，能够使控制云台电机轴转动时，实现被控端隔离机体扰动，达到目标姿态并保持稳定状态。

进一步的，参照图5，本发明云台控制方法第五实施例提供一种云台控制方法，基于上述图1所示的实施例，所述步骤S30包括：

步骤S31、根据所述有效机械能，获取控制所述被控端的目标电流。

在获取控制被控端的有效机械能后，根据控制被控端的有效机械能，计算控制被控端的目标电流，也即：根据控制云台各电机轴的有效机械能，分别计算控制云台各电机轴的目标电流。

具体的，作为一种实施例，获取的控制被控端的有效机械能为：控制俯仰电机轴的有效机械能E_P、控制横滚电机轴的有效机械能E_r，和控制航向电机轴的有效机械能E_y。

设控制云台各电机轴的目标电流为：控制俯仰电机轴的目标电流I_p，控制横滚电机轴的目标电流I_r，控制航向电机轴的目标电流I_y；则有：

$\left\{\begin{array}{c}{I}_p=\sqrt{\left|E_p\right|}\\ I_r=\sqrt{\left|E_r\right|}\\ I_y=\sqrt{\left|E_y\right|}\end{array}\right.;$

根据上述公式，计算得到控制云台各电机轴的目标电流：I_p、I_r和I_y。也即，得到控制被控端的目标电流：I_p、I_r和I_y。

其中，需要说明的是，E的符号代表电机轴转动方向，可根据实际情况具体确定。例如：+(正)为顺时针转动，—(负)为逆时针转动。I_p、I_r和I_y的符号与E_p、E_r和E_y分别相同。

步骤S32、根据所述目标电流，通过云台电机轴控制所述被控端保持在稳定状态。

在获得控制被控端的目标电流后，以相应的目标电流控制云台各电机轴，使被控端达到目标姿态并保持在稳定状态。

具体的，以控制俯仰电机轴的目标电流I_p控制俯仰电机轴，从而使被控端在俯仰方向达到目标俯仰角，并保持俯仰方向的稳定状态；

以控制横滚电机轴的目标电流I_r控制横滚电机轴，从而使被控端在横滚方向达到目标横滚角，并保持横滚方向的稳定状态；

以控制航向电机轴的目标电流I_y控制航向电机轴，从而使被控端在航向方向达到目标航向角，并保持航向方向的稳定状态。

在本实施例中，根据获取的有效机械能，获取控制被控端的目标电流，并以控制被控端的目标电流，控制云台各电机轴，使被控端达到目标姿态并保持稳定，实现了云台的自稳控制效果。

参照图6，本发明云台控制装置第一实施例提供一种云台控制装置，所述云台控制装置包括：

获取模块100，用于获取云台上被控端当前状态和目标状态、固定端当前状态，以及固定端与被控端的转动惯量比。

本实施例方案主要应用于三轴云台中，当然也不限于其他具有精确控制处理需求的单轴或二轴云台。云台可应用于飞行器、汽车、轮船、机器人、人体等，例如：当云台应用于飞行器时，为机载云台，可负载拍摄设备或摄影设备，进行动态拍摄或动态摄影。

本实施例以无人机三轴云台进行举例，三轴云台固定在无人机体上，且三轴云台搭载有相机。其中，三轴云台通过三个电机轴分别控制相机进行俯仰、横滚和航向方向的转动，具体的：俯仰电机轴控制相机进行俯仰转动，横滚电机轴控制相机进行横滚转动，航向电机轴控制相机进行航向转动。

本实施例在云台启动后，获取模块100首先进行信息采集：获取被控端当前状态、固定端当前状态；获取被控端目标状态，获取固定端与被控端的转动惯量比。

其中，被控端为云台负载的相机部分，与被控端相对，固定端可以为与无人机相连的云台部分，也可能包括与其刚性连接的人机；以固定端为云台进行举例，云台通过云台上的三个电机轴与被控端连接，分别控制被控端绕俯仰电机轴、横滚电机轴和航向电机轴转动。

具体的，作为一种实施方式，在云台启动后，获取模块100获取被控端当前姿态。被控端的当前状态包括：被控端的角速度和被控端的状态(即被控端的欧拉角)。

获取模块100获取被控端目标状态。被控端目标状态包括：被控端的目标姿态角和目标角速度。

获取模块100获取固定端当前状态。固定端当前状态包括：固定端的角速度和当前姿态角。

获取模块100获取固定端与被控端的转动惯量比。固定端与被控端的转动惯量比为预设系数，可以通过测量得到。

计算模块200，用于根据所述被控端当前状态和目标状态、固定端当前状态，以及固定端与被控端的转动惯量比，计算获取控制被控端的有效机械能。

在获取模块100获取被控端当前状态和目标状态，固定端当前状态，以及固定端与被控端的转动惯量比之后，计算模块200计算获取控制被控端的有效机械能。

具体的，作为一种实施方式，首先，计算模块200根据被控端当前状态和目标状态，以及固定端当前状态，获取控制被控端的补偿角速度，该补偿角速度为：云台电机轴转动时所给予被控端的补偿角速度，能够使被控端隔离机体扰动，达到目标姿态并保持在稳定状态。

然后，计算模块200根据控制被控端的补偿角速度、固定端与被控端的转动惯量比以及固定端当前状态，计算获取控制被控端的有效机械能，该有效机械能为：控制云台电机轴转动使被控端达到目标姿态并保持在稳定状态时，电机传出的有效机械能。

控制模块300，用于根据所述有效机械能，控制所述被控端保持在稳定状态。

在计算模块200获取有效机械能后，控制模块300根据获取的有效机械能，计算获得控制被控端的目标电流。由于是通过控制云台各电机轴转动，来控制被控端在俯仰、横滚和航向三个方向的转动，因此，控制被控端的目标电流也即控制云台电机轴的目标电流；

然后根据获取的目标电流，控制云台各电机轴转动，从而控制被控端达到目标姿态并保持在稳定状态。

在本实施例中，通过获取模块100获取的云台上被控端当前状态和目标状态、固定端当前状态，以及固定端与被控端的转动惯量比，计算模块200计算得到控制被控端的有效机械能，控制模块300根据该有效机械能控制所述被控端，使被控端达到目标姿态并保持在稳定状态，有效隔离机体扰动，实现了对云台的精确控制，达到了云台自稳控制效果，保障了云台负载相机的成像质量。

进一步的，参照图7，本发明云台控制方法第二实施例提供一种云台控制装置，基于图6所示的实施例，所述获取模块100包括：

传感单元110，用于获取被控端的角速度和姿态四元数。

在云台启动后，传感单元110获取被控端的角速度和姿态四元数，也即云台负载相机的当前角速度和姿态四元数；其中，所获取的姿态四元数是由实数加上三个虚数单位i、j、k组成，i、j、k为相互正交的单位矢量；姿态四元数可以表征被控端在三维空间中的旋转。由此，所获取的姿态四元数包含了被控端的姿态信息。

上述被控端的角速度和姿态四元数是使用被控端的机体坐标系b作为基准得到的。

其中，机体坐标系b为空间直角坐标系，包括三个坐标轴，令x轴为俯仰轴，对应云台的俯仰电机轴，云台的俯仰电机轴转动，控制被控端绕俯仰轴进行俯仰转动；y轴为横滚轴，对应云台的横滚电机轴，云台的横滚电机轴转动，控制被控端绕横滚轴进行横滚转动；z轴为航向轴，对应云台的航向电机轴，云台的航向电机轴转动，控制被控端绕航向轴进行航向转动。

作为一种实施例，传感单元110获取被控端的角速度为Ω：

$\mathrm{\Ω}=\left[\begin{array}{c}{{\mathrm{\ω}}^{\′}}_x\\ {{\mathrm{\ω}}^{\′}}_y\\ {{\mathrm{\ω}}^{\′}}_z\end{array}\right],$ 其中，ω′_x为被控端在俯仰轴上的角速度，ω′_y为被控端在横滚轴上的角速度，ω′_z为被控端在航向轴上的角速度。

为提高被控端当前姿态的精度，传感单元110采用四元数乘法的近似算法计算得到被控端当前的姿态四元数，被控端的姿态四元数的计算过程如下：

任意选取姿态四元数满足模为1，记录为Q₀作为初始值，在前一时刻的姿态四元数的基础上进行下述计算：首先，传感单元110获取的被控端上一时刻姿态四元数为Q_n-1；其中，Q_n-1＝q′₀+q′₁×i+q′₂×j+q′₃×k，i、j、k为相互正交的单位矢量；

获取被控端的重力加速度：G₀＝2(q′₁·q′₃-q′₀·q′₂)^T；

计算当前传感器的校准值e为：

然后，计算得到被控端当前的姿态四元数Q_n，计算公式为：

Q_n＝Q_n-1+Q_n-1×(Ω+e)；其中，Q_n＝q₀+q₁i+q₂×j+q₃×k，i、j、k为相互正交的单位矢量。

由于传感器的校准值e的不断修正，在有限步后，姿态四元素会收敛到准确值，即传感单元110获得被控端姿态四元数Q_n，也即该收敛后的准确值为姿态四元数Q_n。

矩阵单元120，用于根据所述被控端的姿态四元数，获取被控端的旋转矩阵。

在传感单元110获取被控端的姿态四元数Q_n后，根据Q_n，矩阵单元120计算被控端的姿态角由机体坐标系b转为大地坐标系R的转换矩阵计算公式为：

$C_b^R=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_2)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right];$

将Q_n中的q₀、q₁、q₂、q₃，代入上述公式计算，得到被控端的旋转矩阵记：

$C_b^R=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{\mathrm{22}}& T_{\mathrm{23}}\\ T_{31}& T_{\mathrm{32}}& T_{33}\end{array}\right].$

姿态单元130，用于根据所述被控端的旋转矩阵，获取被控端的欧拉角，得到被控端当前姿态。

在矩阵单元120获取被控端的旋转矩阵后，姿态单元130根据计算被控端的欧拉角；被控端的欧拉角包括：俯仰角θ、横滚角γ和航向角计算公式为：

将的值，代入上述公式计算，获得俯仰角θ、横滚角γ和航向角

由此，获取单元140根据获取的被控端的角速度和欧拉角，得到被控端当前状态。

被控端当前状态表征了被控端当前所处于的状态，包括：被控端的角速度Ω和被控端的欧拉角；其中，被控端的欧拉角包括：俯仰角θ、横滚角γ和航向角

获取单元140，用于获取被控端的目标姿态角和目标角速度，作为被控端目标状态；获取固定端与被控端的转动惯量比。

获取单元140获取被控端的目标姿态角。被控端的目标姿态角可以是预设的目标姿态角，也可以是用户调节的目标姿态角，可根据实际需要灵活设置。被控端的目标姿态角包括：目标俯仰角θ′、目标横滚角γ′和目标航向角

获取单元140获取被控端的目标角速度。由于在被控端达到目标姿态后，若要保持在稳定状态，此时角速度应该为0，因此，在本实施例中，令被控端的目标角速度ω_t为预设的固定值：0。

由此，获取预先设置的被控端目标状态。所获取的被控端目标状态包括：被控端的目标角速度ω_t和被控端的目标姿态角；其中，被控端的目标姿态角包括：目标俯仰角θ′、目标横滚角γ′和目标航向角以及目标角速度ω_t。

获取单元140获取固定端和被控端在云台电机轴上的转动惯量比k，固定端与被控端的转动惯量比为预设系数。得到的转动惯量比包括：固定端和被控端在俯仰电机轴上的转动惯量比k_p，固定端和被控端在横滚电机轴上的转动惯量比k_r，固定端和被控端在航向电机轴上的转动惯量比k_y。

需要说明的是，得到的转动惯量比为大量理论计算或实验数据所得，可根据实际需要进行数值调整。

传感单元110，还用于获取固定端的角速度和姿态角，作为固定端当前状态。

传感单元110获取固定端的角速度ω_b， ${\mathrm{\ω}}_b=\left[\begin{array}{c}{\mathrm{\ω}}_{bx}\\ {\mathrm{\ω}}_{by}\\ {\mathrm{\ω}}_{bz}\end{array}\right].$

传感单元110获取固定端的旋转角度，得到的固定端的姿态角为：横滚角θ_r，俯仰角θ_p，航向角θ_y。固定端的姿态角也即飞行器的姿态角。

由此，传感单元110得到固定端当前状态。所得到的固定端当前状态包括：固定端的角速度ω_b和固定端的姿态角；其中，固定端的姿态角包括：横滚角θ_r，俯仰角θ_p，航向角θ_y。

在本实施例中，根据传感单元110获取的被控端角速度和姿态四元数，矩阵单元120获取被控端的旋转矩阵，姿态单元130从而获取所述被控端的欧拉角，所得到被控端当前姿态精度高；获取单元140获取被控端目标状态、固定端当前状态、固定端与被控端的转动惯量比。本实施例中所获取的各项数据精度高，为后续控制被控端有效机械能的计算提供了精确的数据基础。

进一步的，参照图8，本发明第三实施例提供一种云台控制装置，基于上述图6所示的实施例，所述计算模块200包括：

速度计算单元210，用于根据所述被控端当前状态和目标状态，以及固定端当前状态，获取控制被控端的补偿角速度。

在机体扰动时，固定端会在云台电机轴上产生响应的干扰角速度，为隔离机体扰动，达到目标姿态，需控制云台电机轴转动对被控端补偿角速度，以使被控端达到目标姿态并保持状态稳定。

因此，在获取模块100获取被控端当前状态和目标状态，固定端当前状态，以及固定端与被控端的转动惯量比之后，首先，速度计算单元210计算获取控制被控端的补偿角速度，该补偿角速度为：云台电机轴转动时所给予被控端的补偿角速度，使被控端隔离机体扰动，能够达到目标姿态并保持稳定状态。

具体的，作为一种实施方式，首先，速度计算单元210根据被控端当前的角速度：在俯仰电机轴上的角速度ω_pitch，在横滚电机轴上的角速度ω_roll，和在航向电机轴上的角速度ω_yaw。

速度计算单元210根据被控端当前姿态角和目标姿态角，计算获取姿态角差值：俯仰角差值Δθ，横滚角差值Δγ和航向角差值

然后，速度计算单元210根据被控端当前姿态角、固定端在云台三个电机轴上的角速度，以及姿态角差值，获取控制被控端的补偿角速度；控制被控端的补偿角速度包括：在俯仰电机轴上的补偿角速度ω_p，在横滚电机轴上的补偿角速度ω_c，和在航向电机轴上的补偿角速度ω_y。

能量计算单元220，用于根据所述控制被控端的补偿角速度、固定端与被控端的转动惯量比以及固定端当前状态，计算获取控制被控端的有效机械能。

在速度计算单元210获取控制被控端的补偿角速度后，能量计算单元220根据控制被控端的补偿角速度、固定端与被控端的转动惯量比以及固定端当前状态，计算获取控制被控端的有效机械能，该有效机械能为：控制云台电机轴转动，使被控端达到目标姿态并保持在稳定状态时，电机传出的有效机械能。

具体的，作为一种实施方式，能量计算单元220可以根据转动惯量守恒和功能关系，列出方程组，计算控制被控端的有效机械能。由于是通过控制云台的各电机轴转动来控制被控端，因此，所得到的控制被控端的有效机械能也即控制云台各电机轴的有效机械能。

首先，能量计算单元220得到以下数据：

固定端与被控端的转动惯量比：k，即三轴云台各轴上的k可表示为：k＝k_p，k_r，k_y；

一次控制后的云台各电机定子的角速度为ω_S，在后续的公式中可将其消掉；

云台各电机轴上的补偿角速度：ω_c，即三轴云台各轴上的ω_c可表示为：ω_c＝ω_p，ω_r，ω_y；

根据固定端当前姿态获取的固定端在云台各电机轴上的角速度：ω_s，即三轴云台各轴上的ω_s可表示为ω_s＝ω_pitch，ω_roll，ω_yaw；

然后，能量计算单元220令控制云台各电机轴的有效机械能为E，即三轴云台各轴上的E可表示为：E＝E_p，E_r，E_y；

根据转动惯量守恒和功能关系，则有方程组：

其中，ω_t为被控端目标角速度，由于被控端到达目标姿态并保持稳定时，被控端目标速度为零，因此，被控端在云台各电机轴上的目标角速度ω_t的值均为0

然后，能量计算单元220求解上述方程组，得到：

$E=\frac{1}{2}{\mathrm{\ω}}_c^2(\frac{1}{k}-1)+{\mathrm{\ω}}_c{\mathrm{\ω}}_{\mathrm{\ϵ}};$

更为具体的：当计算控制俯仰电机轴的有效机械能时，令ω_s＝ω_pitch，ω_c＝ω_p，k＝k_p；此时，根据上述方程组计算得到控制俯仰电机轴的有效机械能E_p，能够使被控端在俯仰轴方向达到目标姿态并保持稳定状态；

当计算控制横滚电机轴的有效机械能时，令ω_s＝ω_roll，ω_c＝ω_r，k＝k_r；此时，根据上述方程组计算得到控制横滚电机轴的有效机械能E_r，能够使被控端在横滚轴方向达到目标姿态并保持稳定状态；

当计算控制航向电机轴的有效机械能时，令ω_s＝ω_yaw，ω_c＝ω_y，k＝k_y；此时，根据上述方程组计算得到控制航向电机轴的有效机械能E_y，能够使被控端在航向轴方向达到目标姿态并保持稳定状态。

由此，得到控制云台各电机轴的有效机械能：控制俯仰电机轴的有效机械能E_p、控制横滚电机轴的有效机械能E_r，和控制航向电机轴的有效机械能E_y。也即，得到控制被控端的有效机械能E_p、E_r和E_y。

其中，需要说明的是，E的符号代表电机轴转动方向，可根据实际情况具体确定。例如：+(正)为顺时针转动，—(负)为逆时针转动。

在本实施例中，根据获取的被控端当前状态和目标状态，固定端当前状态，以及固定端与被控端的转动惯量比，首先速度计算单元210计算控制被控端的补偿角速度，然后能量计算单元220计算控制被控端的有效机械能，能够控制云台电机轴转动使被控端达到目标姿态并保持在稳定状态。本实施例实现了对云台的精确控制，使云台根据得到的有效机械能，能够控制云台各电机轴转动，使被控端达到目标姿态并保持在稳定状态，达到了云台自稳控制效果。

进一步的，参照图9，本发明第四实施例提供一种云台控制装置，基于上述图6所示的实施例，所述速度计算单元210包括：

干扰子单元211，用于根据所述固定端当前状态获取固定端在云台电机轴上的角速度。

干扰子单元211可以根据固定端当前状态获取固定端在云台电机轴上的角速度，固定端在云台电机轴上的角速度包括：在俯仰电机轴上的角速度ω_pitch，在横滚电机轴上的角速度ω_roll，和在航向电机轴上的角速度ω_yaw。干扰子单元211计算固定端在云台各电机轴上的角速度，即为计算ω_pitch、ω_roll和ω_yaw。

具体的，作为一种实施方式，首先，固定端当前状态包括：固定端的角速度ω_b和固定端的姿态角；其中， ${\mathrm{\ω}}_b=\left[\begin{array}{c}{\mathrm{\ω}}_{bx}\\ {\mathrm{\ω}}_{by}\\ {\mathrm{\ω}}_{bz}\end{array}\right];$ 固定端的姿态角包括：横滚角θ_r，俯仰角θ_p，航向角θ_y。

然后，干扰子单元211将固定端的角速度耦合到云台电机轴，根据坐标变换关系，得到固定端在航向电机轴上的角速度ω_yaw、ω_roll和ω_yaw：

$\begin{array}{c}\left[\begin{array}{ccc}{\mathrm{cos\θ}}_y& -{\mathrm{sin\θ}}_y& 0\\ {\mathrm{sin\θ}}_y& {\mathrm{cos\θ}}_y& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\θ}}_r& 0& -{\mathrm{sin\θ}}_r\\ 0& 1& 0\\ {\mathrm{sin\θ}}_r& 0& {\mathrm{cos\θ}}_r\end{array}\right]\left[\begin{array}{c}{w}_{pitch}\\ 0\\ 0\end{array}\right]\\ +\left[\begin{array}{ccc}{\mathrm{cos\θ}}_y& -{\mathrm{sin\θ}}_y& 0\\ {\mathrm{sin\θ}}_y& {\mathrm{cos\θ}}_y& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}0\\ w_{roll}\\ 0\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ w_{yaw}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_{bx}\\ {\mathrm{\ω}}_{by}\\ {\mathrm{\ωb}}_z\end{array}\right]\end{array};$

其中，θ_p，θ_r，θ_y分别为电机相对零位值的旋转角度，由磁编码器采集得到。由此，干扰子单元211得到固定端在云台各电机轴上的角速度：ω_pitch、ω_roll和ω_yaw。

差值子单元212，用于根据所述被控端当前姿态和目标姿态，获取姿态角差值。

姿态角差值为被控端由当前姿态达到目标姿态的角度差值，差值子单元212令姿态角差值为：俯仰角差值Δθ，横滚角差值Δγ和航向角差值

根据获取的被控端目标姿态：目标俯仰角θ′、目标横滚角γ′和目标航向角和获取的被控端当前的欧拉角：俯仰角θ、横滚角γ和航向角差值子单元212计算姿态角差值：

由此，差值子单元212得到姿态角差值，包括：俯仰角差值Δθ，横滚角差值Δγ和航向角差值

补偿子单元213，用于根据所述被控端的当前状态、固定端在云台电机轴上的角速度和所述姿态角差值，获取控制被控端的补偿角速度。

控制被控端的补偿角速度由隔离固定端干扰的反向补偿角速度和使被控端达到目标姿态的补偿角速度组成。

首先，补偿子单元213根据被控端当前状态中的角速度 $\mathrm{\Ω}=\left[\begin{array}{c}{{\mathrm{\ω}}^{\′}}_x\\ {{\mathrm{\ω}}^{\′}}_y\\ {{\mathrm{\ω}}^{\′}}_z\end{array}\right],$ 固定端在云台电机轴上的角速度ω_pitch、ω_roll和ω_yaw，计算隔离固定端干扰的反向补偿角速度：

设反向补偿角速度为：横滚角速度δ_r、俯仰角速度δ_p、航向角速度δ_y；

则根据坐标变换关系，得到方程式：

$\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\θ}}_p& {\mathrm{sin\θ}}_p\\ 0& -{\mathrm{sin\θ}}_p& {\mathrm{cos\θ}}_p\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\θ}}_r& 0& -{\mathrm{sin\θ}}_r\\ 0& 1& 0\\ {\mathrm{sin\θ}}_r& 0& {\mathrm{cos\θ}}_r\end{array}\right]\left[\begin{array}{c}0\\ 0\\ {\mathrm{\δ}}_y\end{array}\right]$

$+\left[\begin{array}{ccc}1& 0& O\\ 0& {\mathrm{cos\θ}}_p& {\mathrm{sin\θ}}_p\\ 0& -{\mathrm{sin\θ}}_p& {\mathrm{cos\θ}}_p\end{array}\right]\left[\begin{array}{c}0\\ {\mathrm{\δ}}_r\\ 0\end{array}\right]+\left[\begin{array}{c}{\mathrm{\δ}}_p\\ 0\\ 0\end{array}\right]=\left[\begin{array}{c}{{\mathrm{\ω}}^{\′}}_x\\ {{\mathrm{\ω}}^{\′}}_y\\ {{\mathrm{\ω}}^{\′}}_z\end{array}\right]$

补偿子单元213解算上述方程，得到反向补偿角速度：横滚角速度δ_r、俯仰角速度δ_p和航向角速度δ_y。

然后，在本实施例中，补偿子单元213取使被控端达到目标姿态的补偿角速度为：俯仰角速度A·Δθ，横滚角速度A·Δγ和航向角速度其中，A为预设系数，用于精确校准补偿角速度，可根据实际情况灵活设定。

然后，补偿子单元213计算控制被控端的补偿角速度，令：在俯仰电机轴上的补偿角速度ω_p，在横滚电机轴上的补偿角速度和在航向电机轴上的补偿角速度ω_y；

则有如下计算公式：

由此，补偿子单元213计算得到控制被控端的补偿角速度，包括：在俯仰电机轴上的补偿角速度ω_p，在横滚电机轴上的补偿角速度ω_r，和在航向电机轴上的补偿角速度ω_y。

在本实施例中，根据获取的被控端当前状态和被控端目标状态、固定端当前状态，获取隔离固定端干扰的反向补偿角速度和使被控端达到目标姿态的补偿角速度，补偿子单元213得到控制被控端的补偿角速度。本实施例得到的补偿角速度，能够使控制云台电机轴转动时，实现被控端隔离机体扰动，达到目标姿态并保持稳定状态。

进一步的，参照图10，本发明第五实施例提供一种云台控制装置，基于上述图1所示的实施例，所述控制模块300包括：

电流单元310，用于根据所述有效机械能，获取控制所述被控端的目标电流。

计算模块200获取控制被控端的有效机械能后，电流单元310根据控制被控端的有效机械能，计算控制被控端的目标电流，也即：根据控制云台各电机轴的有效机械能，分别计算控制云台各电机轴的目标电流。

具体的，作为一种实施例，获取的控制被控端的有效机械能为：控制俯仰电机轴的有效机械能E_p、控制横滚电机轴的有效机械能E_r，和控制航向电机轴的有效机械能E_y。

电流单元310设云台各电机轴的目标电流为：控制俯仰电机轴的目标电流I_p，控制横滚电机轴的目标电流I_r，控制航向电机轴的目标电流I_y；则有：

$\left\{\begin{array}{c}{I}_p=\sqrt{\left|E_p\right|}\\ I_r=\sqrt{\left|E_r\right|}\\ I_y=\sqrt{\left|E_y\right|}\end{array}\right.;$

根据上述公式，电流单元310计算得到控制云台各电机轴的目标电流：I_p、I_r和I_y。也即，得到控制被控端的目标电流：I_p、I_r和I_y。

其中，需要说明的是，E的符号代表电机轴转动方向，可根据实际情况具体确定。例如：+(正)为顺时针转动，—(负)为逆时针转动。I_p、I_r和I_y的符号与E_p、E_r和E_y分别相同。

控制单元320，用于根据所述目标电流，通过云台电机轴控制所述被控端保持在稳定状态。

在电流单元310获得控制被控端的目标电流后，控制单元320以相应的目标电流控制云台各电机轴，使被控端达到目标姿态并保持在稳定状态。

具体的，控制单元320以控制俯仰电机轴的目标电流I_p控制俯仰电机轴，从而使被控端在俯仰方向达到目标俯仰角，并保持俯仰方向的稳定状态；

控制单元320以控制横滚电机轴的目标电流I_r控制横滚电机轴，从而使被控端在横滚方向达到目标横滚角，并保持横滚方向的稳定状态；

控制单元320以控制航向电机轴的目标电流I_y控制航向电机轴，从而使被控端在航向方向达到目标航向角，并保持航向方向的稳定状态。

在本实施例中，根据获取的有效机械能，电流单元310获取控制被控端的目标电流，控制单元320以控制被控端的目标电流，控制云台各电机轴，使被控端达到目标姿态并保持稳定，实现了云台的自稳控制效果。

以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (10)

1.一种云台控制方法，其特征在于，所述云台控制方法包括以下步骤：

获取云台上被控端当前状态和目标状态、固定端当前状态，以及固定端与被控端的转动惯量比；

根据所述被控端当前状态和目标状态、固定端当前状态，以及固定端与被控端的转动惯量比，计算获取控制被控端的有效机械能；

根据所述有效机械能，控制所述被控端保持在稳定状态。

2.如权利要求1所述的云台控制方法，其特征在于，所述获取云台上被控端当前状态和目标状态、固定端当前状态，以及固定端与被控端的转动惯量比的步骤包括：

获取被控端的角速度和姿态四元数；

根据所述被控端的姿态四元数，获取被控端的旋转矩阵；

根据所述被控端的旋转矩阵，获取被控端的欧拉角，得到被控端当前姿态；

获取被控端的目标姿态角和目标角速度，作为被控端目标状态；

获取固定端的角速度和姿态角，作为固定端当前状态；

获取固定端与被控端的转动惯量比。

3.如权利要求1或2所述的云台控制方法，其特征在于，所述根据所述被控端当前状态和目标状态、固定端当前状态，以及固定端与被控端的转动惯量比，计算获取控制被控端的有效机械能的步骤包括：

根据所述被控端当前状态和目标状态，以及固定端当前状态，获取控制被控端的补偿角速度；

根据所述控制被控端的补偿角速度、固定端与被控端的转动惯量比以及固定端当前状态，计算获取控制被控端的有效机械能。

4.如权利要求3所述的云台控制方法，其特征在于，所述根据所述被控端当前状态和目标状态，以及固定端当前状态，获取控制被控端的补偿角速度的步骤包括：

根据所述固定端当前状态获取固定端在云台电机轴上的角速度；

根据所述被控端当前姿态和目标姿态，获取姿态角差值；

根据所述被控端的当前状态、固定端在云台电机轴上的角速度和所述姿态角差值，获取控制被控端的补偿角速度。

5.如权利要求1所述的云台控制方法，其特征在于，所述根据所述有效机械能，控制所述被控端保持在稳定状态的步骤包括：

根据所述有效机械能，获取控制所述被控端的目标电流；

根据所述目标电流，通过云台电机轴控制所述被控端保持在稳定状态。

6.一种云台控制装置，其特征在于，所述云台控制装置包括：

获取模块，用于获取云台上被控端当前状态和目标状态、固定端当前状态，以及固定端与被控端的转动惯量比；

计算模块，用于根据所述被控端当前状态和目标状态、固定端当前状态，以及固定端与被控端的转动惯量比，计算获取控制被控端的有效机械能；

控制模块，用于根据所述有效机械能，控制所述被控端保持在稳定状态。

7.如权利要求6所述的云台控制装置，其特征在于，所述获取模块包括：

传感单元，用于获取被控端的角速度和姿态四元数；

矩阵单元，用于根据所述被控端的姿态四元数，获取被控端的旋转矩阵；

姿态单元，用于根据所述被控端的旋转矩阵，获取被控端的欧拉角，得到被控端当前姿态；

获取单元，用于获取被控端的目标姿态角和目标角速度，作为被控端目标状态；获取固定端与被控端的转动惯量比；

所述传感单元还用于，获取固定端的角速度和姿态角，作为固定端当前状态。

8.如权利要求6所述的云台控制装置，其特征在于，所述计算模块包括：

速度计算单元，用于根据所述被控端当前状态和目标状态，以及固定端当前状态，获取控制被控端的补偿角速度；

能量计算单元，用于根据所述控制被控端的补偿角速度、固定端与被控端的转动惯量比以及固定端当前状态，计算获取控制被控端的有效机械能。

9.如权利要求8所述的云台控制装置，其特征在于，所述速度计算单元包括：

干扰子单元，用于根据所述固定端当前状态获取固定端在云台电机轴上的角速度；

差值子单元，用于根据所述被控端当前姿态和目标姿态，获取姿态角差值；

补偿子单元，用于根据所述被控端的当前状态、固定端在云台电机轴上的角速度和所述姿态角差值，获取控制被控端的补偿角速度。

10.如权利要求6所述的云台控制装置，其特征在于，所述控制模块包括：

电流单元，用于根据所述有效机械能，获取控制所述被控端的目标电流；

控制单元，用于根据所述目标电流，通过云台电机轴控制所述被控端保持在稳定状态。