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

权利要求书

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

分别获取第i+1时刻的云台中图像采集设备在云台坐标系三轴的角速度和线加速度、以及所述云台中航向轴电机的输出轴的角位移；

基于第i+1时刻的所述云台中图像采集设备在云台坐标系三轴的角速度和线加速度、以及所述云台中航向轴电机的输出轴的角位移，计算第i+1时刻的云台平台误差角；

获取第i时刻的云台姿态；

基于所述第i+1时刻的云台平台误差角，计算第i+1时刻相对于第i时刻的云台姿态增量，基于所述第i+1时刻相对于第i时刻的云台姿态增量和所述第i时刻的云台姿态，得到第i+1时刻的云台姿态；

所述基于所述第i+1时刻的云台平台误差角，计算第i+1时刻相对于第i时刻的云台姿态增量，包括：

基于所述第i+1时刻的云台平台误差角，按照如下公式，计算第i+1时刻相对于第i时刻的云台姿态增量，

其中，所述为所述第i+1时刻相对于第i时刻的云台姿态增量，所述为所述第i+1时刻的云台平台误差角。

2.根据权利要求1所述的方法，其特征在于，所述基于第i+1时刻的云台中图像采集设备在云台坐标系三轴的角速度和线加速度、以及所述云台中航向轴电机的输出轴的角位移，计算第i+1时刻的云台平台误差角，包括：

基于第i+1时刻的所述图像采集设备在所述云台坐标系三轴的线加速度、以及所述航向轴电机的输出轴的角位移，计算第i+1时刻的云台姿态观测量；

获取第i时刻的云台平台误差角；

基于所述第i时刻的云台平台误差角和第i+1时刻的所述图像采集设备在云台坐标系三轴的角速度，计算第i+1时刻的预估云台平台误差角；

基于所述第i+1时刻的云台姿态观测量，对所述第i+1时刻的预估云台平台误差角进行修正，得到所述第i+1时刻的云台平台误差角。

3.根据权利要求2所述的方法，其特征在于，所述基于第i+1时刻的所述图像采集设备在云台坐标系三轴的线加速度、以及所述航向轴电机的输出轴的角位移，计算第i+1时刻的云台姿态观测量，包括：

基于第i+1时刻的所述图像采集设备在所述云台坐标系三轴的线加速度，计算第i+1时刻的云台横滚角；

当所述云台横滚角大于第一阈值时，获取第i时刻的基座姿态、及第i+1时刻的所述云台中各个轴电机的输出轴的角位移，并基于所述第i时刻的基座姿态、及所述第i+1时刻的所述云台中各个轴电机的输出轴的角位移，计算所述云台姿态观测量；所述云台通过所述基座固定于载体，所述轴电机包括所述航向轴电机；

当所述云台横滚角小于所述第一阈值时，获取第i+1时刻的载体航向角，并基于所述第i+1时刻的载体航向角、第i+1时刻的所述航向轴电机的输出轴的角位移、及第i+1时刻的所述图像采集设备在所述云台坐标系三轴的线加速度计算所述云台姿态观测量。

4.根据权利要求3所述的方法，其特征在于，所述获取第i时刻的基座姿态，包括：

获取第i时刻的所述载体的姿态，并将第i时刻的所述载体的姿态作为所述第i时刻的基座姿态。

5.根据权利要求3所述的方法，其特征在于，所述获取第i时刻的基座姿态，包括：

当i＝1时，基于第i时刻的所述基座在基座坐标系三轴的线加速度、以及载体航向角，计算第i时刻的基座姿态；

当第i时刻不为第1时刻时，分别获取第i时刻的所述基座在基座坐标系三轴的角速度和线加速度；基于第i时刻的所述基座在基座坐标系三轴的角速度和线加速度，计算第i时刻的基座平台误差角；获取第i-1时刻的基座姿态；基于所述第i时刻的基座平台误差角，计算第i时刻相对于第i-1时刻的基座姿态增量，基于所述第i时刻相对于第i-1时刻的基座姿态增量和所述第i-1时刻的基座姿态，得到所述第i时刻的基座姿态。