控端隔离机体扰动，达到目标姿态并保持在稳定状态。
[0078] 然后，根据控制被控端的补偿角速度、固定端与被控端的转动惯量比以及固定端 当前状态，计算获取控制被控端的有效机械能，该有效机械能为：控制云台电机轴转动使被 控端达到目标姿态并保持在稳定状态时，电机传出的有效机械能。
[0079] 步骤S30、根据所述有效机械能，控制所述被控端保持在稳定状态。
[0080] 在获取有效机械能后，根据获取的有效机械能，计算获得控制被控端的目标电流。 由于是通过控制云台各电机轴转动，来控制被控端在俯仰、横滚和航向三个方向的转动，因 此，控制被控端的目标电流也即控制云台电机轴的目标电流；
[0081] 然后根据获取的目标电流，控制云台各电机轴转动，从而控制被控端达到目标姿 态并保持在稳定状态。
[0082] 在本实施例中，通过获取的云台上被控端当前状态和目标状态、固定端当前状态， 以及固定端与被控端的转动惯量比，计算得到控制被控端的有效机械能，根据该有效机械 能控制所述被控端，使被控端达到目标姿态并保持在稳定状态，有效隔离机体扰动，实现了 对云台的精确控制，达到了云台自稳控制效果，保障了云台负载相机的成像质量。
[0083] 进一步的，参照图2,本发明第二实施例提供一种云台控制方法，基于图1所示的 实施例，所述步骤SlO包括：
[0084] 步骤S11、获取被控端的角速度和姿态四元数。
[0085] 在云台启动后，可以通过传感器获取被控端的角速度和姿态四元数，也即云台负 载相机的当前角速度和姿态四元数；其中，所获取的姿态四元数是由实数加上三个虚数单 位i、j、k组成，i、j、k为相互正交的单位矢量；姿态四元数可以表征被控端在三维空间中 的旋转。由此，所获取的姿态四元数包含了被控端的姿态信息。
[0086] 上述被控端的角速度和姿态四元数是使用被控端的机体坐标系b作为基准得到 的。
[0087] 其中，机体坐标系b为空间直角坐标系，包括三个坐标轴，令X轴为俯仰轴，对应云 台的俯仰电机轴，云台的俯仰电机轴转动，控制被控端绕俯仰轴进行俯仰转动；y轴为横滚 轴，对应云台的横滚电机轴，云台的横滚电机轴转动，控制被控端绕横滚轴进行横滚转动；z 轴为航向轴，对应云台的航向电机轴，云台的航向电机轴转动，控制被控端绕航向轴进行航 向转动。
[0088] 以下详细阐述使用被控端的机体坐标系b作为基准得到被控端的角速度和姿态 四元数的具体过程：
[0089] 作为一种实施例，通过传感器获取被控端的角速度为Ω :
[0090]
其中，ω ^ x为被控端在俯仰轴上的角速度，,为被控端在横滚 轴上的角速度，ω ' z为被控端在航向轴上的角速度。
[0091] 为提高被控端当前姿态的精度，采用四元数乘法的近似算法计算得到被控端当前 的姿态四元数，被控端的姿态四元数的计算过程如下：
[0092] 任意选取姿态四元数满足模为1，记录为Q。作为初始值，在前一时刻的姿态四元 数的基础上进行下述计算：首先，通过传感器获取的被控端上一时刻姿态四元数为Qn 1;其 中，Qn iX i+q< 2χ j+V 3Xk，i、j、k为相互正交的单位矢量；
[0093] 获取被控端的重力加速度：G。= 2 (q< 3-qr0 · q' 2)τ；
[0094] 计算当前传感器的校准值e为：
[0095] 然后，计算得到被控端当前的姿态四元数Qn，计算公式为：
[0096] Qn= Qn !+Qn !X (Ω+e);其中，Qn= qo+q! i+q2X j+q3Xk，i、j、k 为相互正交的单 位矢量。
[0097] 由于传感器的校准值e的不断修正，在有限步后，姿态四元素会收敛到准确值，即 可获得被控端的姿态四元数Qn，也即该收敛后的准确值为姿态四元数Qn。
[0098] 步骤S12、根据所述被控端的姿态四元数，获取被控端的旋转矩阵。
[0099] 在获取被控端的姿态四元数％后，根据Qn，计算被控端的姿态角由机体坐标系b转 为大地坐标系R的转换矩阵Cf,汁算公式为：
[0101] 将Qn中的q。、Ql、q2、q3，代入上述公式计算，得到被控端的旋转矩阵记：
[0103] 步骤S13、根据所述被控端的旋转矩阵，获取被控端的欧拉角，得到被控端当前姿 〇
[0104] 在获取被控端的旋转矩阵Cf后，根据Cf计算被控端的欧拉角；被控端的欧拉角 包括：俯仰角Θ、横滚角γ和航向角Φ;计算公式为：
[0106] 将Cf的值，代入上述公式计算，获得俯仰角Θ、横滚角γ和航向角
[0107] 由此，根据获取的被控端的角速度和欧拉角，得到被控端当前状态。
[0108] 被控端当前状态包括：被控端的角速度Ω和被控端的欧拉角；其中，被控端的欧 拉角包括：俯仰角θ、横滚角γ和航向角
[0109] 步骤S14、获取被控端的目标姿态角和目标角速度，作为被控端目标状态。
[0110] 被控端的目标姿态角可直接获取。被控端的目标姿态角可以是预设的目标姿态 角，也可以是用户调节的目标姿态角，可根据实际需要灵活设置。被控端的目标姿态角包 括：目标俯仰角Θ'、目标横滚角γ'和目标航向角φ、
[0111] 被控端的目标角速度可直接获取。由于在被控端达到目标姿态后，若要保持在稳 定状态，此时角速度应该为0,因此，在本实施例中，令被控端的目标角速度ω t为预设的固 定值：〇。
[0112] 由此，获取预先设置的被控端目标状态。所获取的被控端目标状态包括：被控端 的目标角速度和被控端的目标姿态角；其中，被控端的目标姿态角包括：目标俯仰角 θ '、目标横滚角γ'和目标航向角以及目标角速度《t。
[0113] 步骤S15、获取固定端的角速度和姿态角，作为固定端当前状态。
[0114] 固定端的角速度cob可以通过传感器获取，得到
[0115] 固定端的旋转角度可以通过传感器获取，得到的固定端的姿态角为：横滚角Θ y 俯仰角ΘΡ，航向角Gy。固定端的姿态角也即飞行器的姿态角。
[0116] 由此，得到固定端当前状态。所得到的固定端当前状态包括：固定端的角速度cob和固定端的姿态角；其中，固定端的姿态角包括：横滚角Θ P俯仰角θρ，航向角9y。
[0117] 步骤S16、获取固定端与被控端的转动惯量比。
[0118] 固定端与被控端的转动惯量比为预设系数，可直接获取，得到固定端和被控端在 云台电机轴上的转动惯量比k。得到的转动惯量比包括：固定端和被控端在俯仰电机轴上 的转动惯量比kp，固定端和被控端在横滚电机轴上的转动惯量比k，固定端和被控端在航 向电机轴上的转动惯量比ky。
[0119] 需要说明的是，得到的转动惯量比为大量理论计算或实验数据所得，可根据实际 需要进行数值调整。
[0120] 在本实施例中，根据获取的被控端角速度和姿态四元数，获取被控端的旋转矩阵， 从而获取所述被控端的欧拉角，所得到被控端当前姿态精度高；获取被控端目标状态、固定 端当前状态、固定端与被控端的转动惯量比。本实施例中所获取的各项数据精度高，为后续 控制被控端有效机械能的计算提供了精确的数据基础。
[0121] 进一步的，参照图3,本发明第三实施例提供一种云台控制方法，基于上述图1或 图2所示的实施例（本实施例以图1为例），所述步骤S20包括：
[0122] 步骤S21、根据所述被控端当前状态和目标状态，以及固定端当前状态，获取控制 被控端的补偿角速度。
[0123] 在机体扰动时，固定端会在云台电机轴上产生响应的干扰角速度，为隔离机体扰 动，达到目标姿态，需控制云台电机轴转动对被控端产生补偿角速度，以使被控端达到目标 姿态并保持状态稳定。
[0124] 因此，在获取被控端当前状态和目标状态，固定端当前状态，以及固定端与被控端 的转动惯量比之后，首先，计算获取控制被控端的补偿角速度，该补偿角速度为：云台电机 轴转动时所给予被控端的补偿角速度，使被控端隔离机体扰动，能够达到目标姿态并保持 稳定状态。
[0125] 具体的，作为一种实施方式，首先，根据固定端当前的角速度：在俯仰电机轴上的 角速度《piteh，在横滚电机轴上的角速度ωΜ?1，和在航向电机轴上的角速度coyaw。
[0126] 根据被控端当前姿态角和目标姿态角，计算获取姿态角差值：俯仰角差值Δ Θ， 横滚角差值A γ和航向角差值麵&
[0127] 然后，根据被控端当前姿态角、固定端在云台三个电机轴上的角速度，以及姿态角 差值，获取控制被控端的补偿角速度；控制被控端的补偿角速度包括：在俯仰电机轴上的 补偿角速度ωρ，在横滚电机轴上的补偿角速度ωρ和在航向电机轴上的补偿角速度coy。
[0128] 步骤S22、根据所述控制被控端的补偿角速度、固定端与被控端的转动惯量比以及 固定端当前状态，计算获取控制被控端的有效机械能。
[0129] 在获取控制被控端的补偿角速度后，根据控制被控端的补偿角速度、固定端与被 控端的转动惯量比以及固定端当前状态，计算获取控制被控端的有效机械能，该有效机械 能为：控制云台电机轴转动，使被控端达到目标姿态并保持在稳定状态时，电机传出的有效 机械能。
[0130] 具体的，作为一种实施方式，可以根据转动惯量守恒和功能关系，列出方程组，计 算控制被控端的有效机械能。由于是通过控制云台的各电机轴转动来控制被控端，因此，所 得到的控制被控端的有效机械能也即控制云台各电机轴的有效机械能。
[0131] 首先，获取的固定端与被控端的转动惯量比为：k，即三轴云台各轴上的k可表示 为：k = kp，kr，ky;
[0132] -次控制后的云台各电机定子的角速度为cos，在后续的公式中可将其消掉；
[0133] 获取的云台各电机轴上的补偿角速度为：，即三轴云台各轴上的ω。可表示 为：ω。= ω ρ，ωΓ，coy;
[0134] 根据固定端当前姿态获取的固定端在云台各电机轴上的角速度为：即三轴 云台各轴上的表示为ω s ^ pitch? ^ roll ? ^ yaw?
[0135] 然后，令控制云台各电机轴的有效机械能为E，即三轴云台各轴上的E可表示为：E =Ep, Er, Ey；
[0136] 根据转动惯量守恒和功能关系，则有方程组：
[0138] 其中，cot为获取的被控端目标角速度，由于被控端到达目标姿态并保持稳定时， 被控端目标速度为零，因此，被控端在云台各电机轴上的目标角速度ω ^勺值均为〇。
[0139] 然后，求解上述方程组，得到：
[0141] 更为具体的：当计算控制俯仰电机轴的有效机械能时，令 ^ s ^ pitch? ^ c ^ p? k = kp;此时，根据上述方程组计算得到控制俯仰电机轴的有效机械能E p，能够使被控端在 俯仰轴方向达到目标姿态并保持稳定状态；
[0142] 当计算控制横滚电机轴的有效机械能时，令Ws= ω roll，ω。= ω = I;此时， 根据上述方程组计算得到控制横滚电机轴的有效机械能Ep能够使被控端在横滚轴方向达 到目标姿态并保持稳定状态；
[0143] 当计算控制航向电机轴的有效机械能时，令Ws= ω yaw，ω。= ω y，k = ky;此时， 根据上述方程组计算得到控制航向电机轴的有效机械能Ey，能够使被控端在航向轴方向达