[发明专利]一种云台姿态调整方法及装置

权利要求书

1.一种云台姿态调整方法，其特征在于，所述方法包括：

按照预设周期，获取云台设备的角速度量测值及姿态量测四元数；

根据所述角速度量测值、所述预设周期、所述云台设备在上一周期的姿态估计四元数、所述云台设备的角速度偏置及所述云台设备在上一周期的协方差估计值，计算所述云台设备的姿态预测四元数及协方差预测值；

判断所述姿态量测四元数及所述姿态预测四元数是否满足姿态奇异条件；

若满足，将所述姿态量测四元数转换为欧拉角，对所述欧拉角中的航向角取反，对航向角取反后的欧拉角进行转换，得到更新后的姿态量测四元数，计算更新后的姿态量测四元数与所述姿态预测四元数之差，得到第二量测新息值；

利用扩展卡尔曼滤波算法，基于所述角速度偏置、所述姿态预测四元数、所述协方差预测值及所述第二量测新息值，计算得到所述云台设备在当前周期的姿态估计四元数；

基于所述云台设备在当前周期的姿态估计四元数，对所述云台设备的姿态进行调整。

2.根据权利要求1所述的方法，其特征在于，在所述姿态量测四元数及所述姿态预测四元数不满足姿态奇异条件的情况下，所述方法还包括：

利用所述扩展卡尔曼滤波算法，基于所述角速度偏置、所述姿态预测四元数、所述协方差预测值及第一量测新息值，计算得到所述云台设备的当前姿态估计四元数，所述第一量测新息值为所述姿态量测四元数及所述姿态预测四元数之差。

3.根据权利要求1所述的方法，其特征在于，所述角速度量测值为三轴角速度量测值；所述根据所述角速度量测值、所述预设周期、所述云台设备在上一周期的姿态估计四元数、所述云台设备的角速度偏置及所述云台设备在上一周期的协方差估计值，计算所述云台设备的姿态预测四元数及协方差预测值，包括：

根据所述三轴角速度量测值、所述预设周期及所述云台设备的角速度偏置，计算所述云台设备的三轴角度增量；

将所述三轴角度增量转换为旋转角度；

根据所述三轴角度增量及所述旋转角度，计算所述云台设备的旋转轴；

根据所述旋转角度及所述旋转轴，计算所述云台设备的姿态变化量；

根据所述姿态变化量及所述云台设备在上一周期的姿态估计四元数，计算得到所述云台设备的姿态预测四元数；

根据所述姿态预测四元数及所述云台设备在上一周期的协方差估计值，计算得到所述云台设备的协方差预测值。

4.根据权利要求3所述的方法，其特征在于，所述根据所述三轴角速度量测值、所述预设周期及所述云台设备的角速度偏置，计算所述云台设备的三轴角度增量，包括：

针对所述三轴角速度量测值的每个方向，计算该方向的角速度量测值与所述云台设备在该方向的角速度偏置之差与所述预设周期的乘积，得到所述云台设备在该方向的角度增量；

所述将所述三轴角度增量转换为旋转角度，包括：

计算所述云台设备的三轴角度增量的平方和，计算所述平方和的平方根，得到旋转角度；

所述根据所述三轴角度增量及所述旋转角度，计算所述云台设备的旋转轴，包括：

分别计算所述云台设备在每个方向的角度增量与所述旋转角度的比值，得到所述云台设备的旋转轴；

所述根据所述旋转角度及所述旋转轴，计算所述云台设备的姿态变化量，包括：

计算所述旋转角度的半角的余弦值，并计算所述旋转角度的半角的正弦值与所述旋转轴的乘积，将所述余弦值及得到的乘积作为所述云台设备的姿态变化量；

所述根据所述姿态变化量及所述云台设备在上一周期的姿态估计四元数，计算得到所述云台设备的姿态预测四元数，包括：

对所述姿态变化量及所述云台设备在上一周期的姿态估计四元数进行四元数相乘运算，得到所述云台设备的姿态预测四元数；

所述根据所述姿态预测四元数及所述云台设备在上一周期的协方差估计值，计算得到所述云台设备的协方差预测值，包括：

获取所述云台设备的状态系数矩阵和系统噪声矩阵；

计算所述状态系数矩阵、所述云台设备在上一周期的协方差估计值及所述状态系数矩阵的转置矩阵之间的乘积，计算得到的乘积与所述系统噪声矩阵之和，得到所述云台设备的协方差预测值。