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

说明书

本发明实施例提供了一种云台姿态调整方法及装置，按照预设周期，获取云台设备的角速度量测值及姿态量测四元数；计算云台设备的姿态预测四元数及协方差预测值；判断姿态量测四元数及姿态预测四元数是否满足姿态奇异条件；若满足，将姿态量测四元数转换为欧拉角，对欧拉角中的航向角取反，对航向角取反后的欧拉角进行转换，得到更新后的姿态量测四元数，计算更新后的姿态量测四元数与姿态预测四元数之差，得到第二量测新息值；计算得到云台设备在当前周期的姿态估计四元数；基于云台设备在当前周期的姿态估计四元数对云台设备的姿态进行调整。从而减少云台设备的姿态发散的问题，实现对云台设备的全姿态调整。

技术领域

本发明涉及云台控制技术领域，特别是涉及一种云台姿态调整方法及装置。

背景技术

随着摄像机的应用场景越来越复杂，对搭载摄像机的云台对各种应用场景的适应性要求也越来越高。在一些场景中，需要云台能够不受结构与载体姿态的限制，进行全姿态工作。

而在相关技术中，通常采用欧拉角量测的EKF(Extended Kalman Filter，扩展卡尔曼滤波)模型进行云台姿态调整，但是，这种模型会存在姿态奇异点，当云台到达姿态奇异点时，很容易出现发散、乱转现象，难以实现对云台姿态的控制。

因此，目前亟需一种能够实现云台全姿态调整的云台姿态调整方法。

发明内容

本发明实施例的目的在于提供一种云台姿态调整方法及装置，以实现对云台的全姿态调整。具体技术方案如下：

本发明实施例提供了一种云台姿态调整方法，所述方法包括：

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

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

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

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

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

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

可选的，在所述姿态量测四元数及所述姿态预测四元数不满足姿态奇异条件的情况下，所述方法还包括：

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

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

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

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

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

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