态四元数；其中，所获取的姿态四元数是由实数加上三个虚数单位 i、j、k组成，i、j、k为相互正交的单位矢量；姿态四元数可以表征被控端在三维空间中的旋 转。由此，所获取的姿态四元数包含了被控端的姿态信息。
[0207] 上述被控端的角速度和姿态四元数是使用被控端的机体坐标系b作为基准得到 的。
[0208] 其中，机体坐标系b为空间直角坐标系，包括三个坐标轴，令X轴为俯仰轴，对应云 台的俯仰电机轴，云台的俯仰电机轴转动，控制被控端绕俯仰轴进行俯仰转动；y轴为横滚 轴，对应云台的横滚电机轴，云台的横滚电机轴转动，控制被控端绕横滚轴进行横滚转动；z 轴为航向轴，对应云台的航向电机轴，云台的航向电机轴转动，控制被控端绕航向轴进行航 向转动。
[0209] 作为一种实施例，传感单元110获取被控端的角速度为Ω :
其中，ω ' x为被控端在俯仰轴上的角速度，ω ' 被控端在横滚 轴上的角速度，ω ' ζ为被控端在航向轴上的角速度。
[0211] 为提高被控端当前姿态的精度，传感单元110采用四元数乘法的近似算法计算得 到被控端当前的姿态四元数，被控端的姿态四元数的计算过程如下：
[0212] 任意选取姿态四元数满足模为1，记录为Q。作为初始值，在前一时刻的姿态四元数 的基础上进行下述计算：首先，传感单元110获取的被控端上一时刻姿态四元数为Qn 1;其 中，Qn iX i+q< 2χ j+V 3Xk，i、j、k为相互正交的单位矢量；
[0213] 获取被控端的重力加速度：G。= 2 W 3-qr0 · q' 2)τ；
[0214] 计算当前传感器的校准值e为：
[0215] 然后，计算得到被控端当前的姿态四元数Qn，计算公式为：
[0216] Qn= Qn !+Qn !X (Ω+e);其中，Qn= qo+q! i+q2X j+q3Xk，i、j、k 为相互正交的单 位矢量。
[0217] 由于传感器的校准值e的不断修正，在有限步后，姿态四元素会收敛到准确值，即 传感单元110获得被控端姿态四元数Qn，也即该收敛后的准确值为姿态四元数Qn。
[0218] 矩阵单元120,用于根据所述被控端的姿态四元数，获取被控端的旋转矩阵。
[0219] 在传感单元110获取被控端的姿态四元数％后，根据Q n，矩阵单元120计算被控 CN 105116926 A 说明书 14/20 页 端的姿态角由机体坐标系b转为大地坐标系
R的转换矩阵Cf，计算公式为：
[0221 ] 将Qn中的q。、qp q2、q3，代入上述公式计算，得到被控端的旋转矩阵记：
[0223] 姿态单元130,用于根据所述被控端的旋转矩阵，获取被控端的欧拉角，得到被控 端当前姿态。
[0224] 在矩阵单元120获取被控端的旋转矩阵Cf后，姿态单元130根据Cf计算被控端 的欧拉角；被控端的欧拉角包括：俯仰角Θ、横滚角γ和航向角计算公式为：
[0226] 将€^的值，代入上述公式计算，获得俯仰角Θ、横滚角γ和航向角警；
[0227] 由此，获取单元140根据获取的被控端的角速度和欧拉角，得到被控端当前状态。
[0228] 被控端当前状态表征了被控端当前所处于的状态，包括：被控端的角速度Ω和被 控端的欧拉角；其中，被控端的欧拉角包括：俯仰角Θ、横滚角γ和航向角φ。
[0229] 获取单元140,用于获取被控端的目标姿态角和目标角速度，作为被控端目标状 态；获取固定端与被控端的转动惯量比。
[0230] 获取单元140获取被控端的目标姿态角。被控端的目标姿态角可以是预设的目标 姿态角，也可以是用户调节的目标姿态角，可根据实际需要灵活设置。被控端的目标姿态角 包括：目标俯仰角θ'、目标横滚角γ'和目标航向角
[0231 ] 获取单元140获取被控端的目标角速度。由于在被控端达到目标姿态后，若要保 持在稳定状态，此时角速度应该为〇,因此，在本实施例中，令被控端的目标角速度为预 设的固定值：〇。
[0232] 由此，获取预先设置的被控端目标状态。所获取的被控端目标状态包括：被控端 的目标角速度和被控端的目标姿态角；其中，被控端的目标姿态角包括：目标俯仰角 Θ '、目标横滚角γ'和目标航向角φ'以及目标角速度c〇t。
[0233] 获取单元140获取固定端和被控端在云台电机轴上的转动惯量比k，固定端与被 控端的转动惯量比为预设系数。得到的转动惯量比包括：固定端和被控端在俯仰电机轴上 的转动惯量比kp，固定端和被控端在横滚电机轴上的转动惯量比k，固定端和被控端在航 向电机轴上的转动惯量比ky。
[0234] 需要说明的是，得到的转动惯量比为大量理论计算或实验数据所得，可根据实际 需要进行数值调整。
[0235] 传感单元110,还用于获取固定端的角速度和姿态角，作为固定端当前状态。
[0236] 传感单元110获取固定端的角速度wb，
[0237] 传感单元110获取固定端的旋转角度，得到的固定端的姿态角为：横滚角Θ p俯 仰角θρ，航向角0y。固定端的姿态角也即飞行器的姿态角。
[0238] 由此，传感单元110得到固定端当前状态。所得到的固定端当前状态包括：固定端 的角速度和固定端的姿态角；其中，固定端的姿态角包括：横滚角Θ p俯仰角θρ，航向 角Qy〇
[0239] 在本实施例中，根据传感单元110获取的被控端角速度和姿态四元数，矩阵单元 120获取被控端的旋转矩阵，姿态单元130从而获取所述被控端的欧拉角，所得到被控端当 前姿态精度高；获取单元140获取被控端目标状态、固定端当前状态、固定端与被控端的转 动惯量比。本实施例中所获取的各项数据精度高，为后续控制被控端有效机械能的计算提 供了精确的数据基础。
[0240] 进一步的，参照图8,本发明第三实施例提供一种云台控制装置，基于上述图6所 示的实施例，所述计算模块200包括：
[0241 ] 速度计算单元210，用于根据所述被控端当前状态和目标状态，以及固定端当前状 态，获取控制被控端的补偿角速度。
[0242] 在机体扰动时，固定端会在云台电机轴上产生响应的干扰角速度，为隔离机体扰 动，达到目标姿态，需控制云台电机轴转动对被控端补偿角速度，以使被控端达到目标姿态 并保持状态稳定。
[0243] 因此，在获取模块100获取被控端当前状态和目标状态，固定端当前状态，以及固 定端与被控端的转动惯量比之后，首先，速度计算单元210计算获取控制被控端的补偿角 速度，该补偿角速度为：云台电机轴转动时所给予被控端的补偿角速度，使被控端隔离机体 扰动，能够达到目标姿态并保持稳定状态。
[0244] 具体的，作为一种实施方式，首先，速度计算单元210根据被控端当前的角速度： 在俯仰电机轴上的角速度《piteh，在横滚电机轴上的角速度ωΜ?1，和在航向电机轴上的角 速度c^yaw。
[0245] 速度计算单元210根据被控端当前姿态角和目标姿态角，计算获取姿态角差值： 俯仰角差值A Θ，横滚角差值Δ γ和航向角差值&φ。
[0246] 然后，速度计算单元210根据被控端当前姿态角、固定端在云台三个电机轴上的 角速度，以及姿态角差值，获取控制被控端的补偿角速度；控制被控端的补偿角速度包括： 在俯仰电机轴上的补偿角速度ωρ，在横滚电机轴上的补偿角速度ω。，和在航向电机轴上 的补偿角速度《y。
[0247] 能量计算单元220,用于根据所述控制被控端的补偿角速度、固定端与被控端的转 动惯量比以及固定端当前状态，计算获取控制被控端的有效机械能。
[0248] 在速度计算单元210获取控制被控端的补偿角速度后，能量计算单元220根据控 制被控端的补偿角速度、固定端与被控端的转动惯量比以及固定端当前状态，计算获取控 制被控端的有效机械能，该有效机械能为：控制云台电机轴转动，使被控端达到目标姿态并 保持在稳定状态时，电机传出的有效机械能。
[0249] 具体的，作为一种实施方式，能量计算单元220可以根据转动惯量守恒和功能关 系，列出方程组，计算控制被控端的有效机械能。由于是通过控制云台的各电机轴转动来控 制被控端，因此，所得到的控制被控端的有效机械能也即控制云台各电机轴的有效机械能。
[0250] 首先，能量计算单元220得到以下数据：
[0251] 固定端与被控端的转动惯量比：k，即三轴云台各轴上的k可表示为：k = kp，I， ky；
[0252] -次控制后的云台各电机定子的角速度为cos，在后续的公式中可将其消掉；
[0253] 云台各电机轴上的补偿角速度：ω。，即三轴云台各轴上的ω。可表示为：ω ωρ，ωΓ，《y;
[0254] 根据固定端当前姿态获取的固定端在云台各电机轴上的角速度：ws，即三轴云台 各轴上的表示为 ^ s ^ pitch? ^ roll ? ^ yaw?
[0255] 然后，能量计算单元220令控制云台各电机轴的有效机械能为E，即三轴云台各轴 上的E可表示为：E = Ep，Er，Ey;
[0256] 根据转动惯量守恒和功能关系，则有方程组：
[0258] 其中，cot为被控端目标角速度，由于被控端到达目标姿态并保持稳定时，被控端 目标速度为零，因此，被控端在云台各电机轴上的目标角速度ω ^勺值均为〇
[0259] 然后，能量计算单元220求解上述方程组，得到：
[0261] 更为具体的：当计算控制俯仰电机轴的有效机械能时，令 ^ s ^ pitch? ^ c ^ p? k = kp;此时，根据上述方程组计算得到控制俯仰电机轴的有效机械能E p，能够使被控端在 俯仰轴方向达到目标姿态并保持稳定状态；
[0262] 当计算控制横滚电机轴的有效机械能时，令Ws= ω roll，ω。= ω = I;此时， 根据上述方程组计算得到控制横滚电机轴的有效机械能Ep能够使被控端在横滚轴方向达 到目标姿态并保持稳定状态；
[0263] 当计算控制航向电机轴的有效机械能时，令Ws= ω yaw，ω。= ω y，k = ky;此时， 根据上述方程组计算得到控制航向电机轴的有效机械能Ey，能够使被控端在航向轴方向达 到目标姿态并保持稳定状态。
[0264] 由此，得到控制云台各电机轴的有效机械能：控制俯仰电机轴的有效机械能Ep、控 制横滚电机轴的有效机械能Ep和控制航向电机轴的有效机械能Ey。也即，得到控制被控端 的有效机械能Ep、民和E y。
[0265] 其中，需要说明的是，E的符号代表电机轴转动方向，可根据实际情况具体确定。例 如：+(正）为顺时针转动，一（负）为逆时针转动。
[0266] 在本实施例中，根据获取的被控端当前状态和目标状态，固定端当前状态，以及固 定端与被控端的转动惯量比，首先速度计算单元210计算控制被控端的补偿角速度，然后 能量计算单元220计算控制被控端的有效机械能，能够控制云台电机轴转动使被控端达到 目标姿态并保持在稳定状态。本实施例实现了对云台的精确控制，使云台根据得到的有效 机械能，能够控制云台各电机轴转动，使被控端达到目标姿态并保持在稳定状态，达到了云 台自稳控制效果。
[0267] 进一步的，参照图9,本发明第四实施例提供一种云台控制装置，基于上述图6所 示的实施例，所述速度计算单元210包括：
[0268] 干扰子单元211，用于根