[发明专利]一种姿态角的恢复方法、装置及设备

权利要求书

1.一种姿态角的恢复方法，其特征在于，应用于包括测量传感器和云台的设备，所述测量传感器周期性采集云台的加速度值和角速度值，该方法包括：

在确定所述云台发生碰撞后，基于当前周期的角速度值确定所述云台的预测方向向量，基于所述当前周期的加速度值确定所述云台的实际方向向量，并基于所述预测方向向量和所述实际方向向量确定目标修正值；

基于所述目标修正值对所述角速度值进行修正，得到修正后的角速度值，并基于所述修正后的角速度值确定所述云台的姿态角是否已恢复；

若是，则基于所述修正后的角速度值确定所述云台的目标姿态角，在所述当前周期的后一个周期，基于所述目标姿态角确定所述云台的姿态角；

若否，则将所述当前周期的后一个周期作为当前周期，返回执行基于当前周期的角速度值确定所述云台的预测方向向量的操作。

2.根据权利要求1所述的方法，其特征在于，

所述基于当前周期的角速度值确定所述云台的预测方向向量，包括：

基于当前周期的角速度值及所述当前周期的上一个周期的初始四元数，确定当前周期的初始四元数，所述当前周期的初始四元数包括4个向量值；

基于所述初始四元数中的4个向量值确定所述云台的预测方向向量；

其中，若当前周期是所述云台发生碰撞后的第一个周期，则所述当前周期的上一个周期的初始四元数，是所述云台发生碰撞时的初始四元数，且该初始四元数是基于所述云台发生碰撞时的初始姿态角确定的。

3.根据权利要求1所述的方法，其特征在于，

所述基于所述当前周期的加速度值确定所述云台的实际方向向量，包括：

基于所述当前周期的加速度值确定加速度模长；

基于所述加速度值和所述加速度模长确定所述云台的实际方向向量。

4.根据权利要求1所述的方法，其特征在于，

所述基于所述预测方向向量和所述实际方向向量确定目标修正值，包括：

确定所述预测方向向量与所述实际方向向量之间的误差向量；

基于已配置的修正参数值和所述误差向量确定所述目标修正值。

5.根据权利要求1所述的方法，其特征在于，

所述基于所述修正后的角速度值确定所述云台的姿态角是否已恢复，包括：

基于所述修正后的角速度值及当前周期的上一个周期的目标四元数，确定当前周期的目标四元数；其中，若当前周期是所述云台发生碰撞后的第一个周期，则当前周期的上一个周期的目标四元数，是所述云台发生碰撞时的目标四元数，且目标四元数是基于所述云台发生碰撞时的初始姿态角确定的；

若当前周期的目标四元数与当前周期的上一个周期的目标四元数之间的差值的绝对值小于预设阈值，则确定所述云台的姿态角已恢复；

若当前周期的目标四元数与当前周期的上一个周期的目标四元数之间的差值的绝对值不小于预设阈值，则确定所述云台的姿态角未恢复。

6.根据权利要求1-5任一项所述的方法，其特征在于，

所述确定所述云台发生碰撞，包括：

基于所述当前周期的加速度值确定加速度模长；

基于所述当前周期的角速度值确定角速度模长；

基于所述角速度模长确定角速度模长目标值；

若所述角速度模长目标值大于角速度模长阈值，和/或，所述加速度模长与指定加速度值的差值的绝对值大于加速度模长阈值，则确定所述云台发生碰撞。