一种云台姿态估计方法及装置制造方法及图纸

本发明专利技术公开了一种云台姿态估计方法及装置，属于云台技术领域。方法包括：分别获取第i+1时刻的云台中图像采集设备在云台坐标系三轴的角速度和线加速度、以及云台中航向轴电机的输出轴的角位移；基于第i+1时刻的云台中图像采集设备在云台坐标系三轴的角速度和线加速度、以及云台中航向轴电机的输出轴的角位移，计算第i+1时刻的云台平台误差角，获取第i时刻的云台姿态，基于第i+1时刻的云台平台误差角，更新第i时刻的云台姿态，得到第i+1时刻的云台姿态。装置：第一获取模块、计算模块、第二获取模块和更新模块。本发明专利技术能够提高云台姿态估计精度。

A method and device for attitude estimation of cloud platform

【技术实现步骤摘要】
一种云台姿态估计方法及装置
本专利技术涉及云台
，特别涉及一种云台姿态估计方法及装置。
技术介绍
航拍无人机一般通过云台搭载相机。在相机工作时，云台受无人机飞行环境的影响会出现震动、晃动等不稳定的情况，这时，不利于相机拍摄稳定的视频或者图像。为了保障相机能够获取到稳定视频或者图像，要实现云台姿态对目标云台姿态的良好跟随。在实现云台姿态的良好跟随时，首先需要对云台姿态进行精确估计。现有的云台姿态估计方法是通过在相机上安装陀螺仪，并通过陀螺仪的工作参数估计云台姿态。具体地，先给定云台的初始姿态，再对陀螺仪测量的角速度进行积分，以获取当前云台姿态相对于前次云台姿态的姿态增量，然后基于前次云台姿态与姿态增量得到当前云台姿态。在实现本专利技术的过程中，专利技术人发现现有技术至少存在以下问题：对陀螺仪的角速度进行积分得到的姿态增量存在积分运算的累积误差，该累积误差会随时间变化而增大，所计算得到的姿态增量不准确，从而降低了云台姿态的估计精度。
技术实现思路
本专利技术实施例提供了一种云台姿态估计方法及装置，能够提高云台姿态的估计精度。所述技术方案如下：第一方面，提供了一种云台姿态估计方法，所述方法包括：分别获取第i+1时刻的云台中图像采集设备在云台坐标系三轴的角速度和线加速度、以及所述云台中航向轴电机的输出轴的角位移；基于第i+1时刻的所述云台中图像采集设备在云台坐标系三轴的角速度和线加速度、以及所述云台中航向轴电机的输出轴的角位移，计算第i+1时刻的云台平台误差角；获取第i时刻的云台姿态；基于所述第i+1时刻的云台平台误差角，更新所述第i时刻的云台姿态，得到第i+1时刻的云台姿态。可选地，所述基于第i+1时刻的云台中图像采集设备在云台坐标系三轴的角速度和线加速度、以及所述云台中航向轴电机的输出轴的角位移，计算第i+1时刻的云台平台误差角，包括：基于第i+1时刻的所述图像采集设备在所述云台坐标系三轴的线加速度、以及所述航向轴电机的输出轴的角位移，计算第i+1时刻的云台姿态观测量；获取第i时刻的云台平台误差角；基于所述第i时刻的云台平台误差角和第i+1时刻的所述图像采集设备在云台坐标系三轴的角速度，计算第i+1时刻的预估云台平台误差角；基于所述第i+1时刻的云台姿态观测量，对所述第i+1时刻的预估云台平台误差角进行修正，得到所述第i+1时刻的云台平台误差角。可选地，所述基于第i+1时刻的所述图像采集设备在云台坐标系三轴的线加速度、以及所述航向轴电机的输出轴的角位移，计算第i+1时刻的云台姿态观测量，包括：基于第i+1时刻的所述图像采集设备在所述云台坐标系三轴的线加速度，计算第i+1时刻的云台横滚角；当所述云台横滚角大于第一阈值时，获取第i时刻的基座姿态、及第i+1时刻的所述云台中各个轴电机的输出轴的角位移，并基于所述第i时刻的基座姿态、及所述第i+1时刻的所述云台中各个轴电机的输出轴的角位移，计算所述云台姿态观测量；所述云台通过所述基座固定于载体，所述轴电机包括所述航向轴电机；当所述云台横滚角小于所述第一阈值时，获取第i+1时刻的载体航向角，并基于所述第i+1时刻的载体航向角、第i+1时刻的所述航向轴电机的输出轴的角位移、及第i+1时刻的所述图像采集设备在所述云台坐标系三轴的线加速度计算所述云台姿态观测量。可选地，所述获取第i时刻的基座姿态，包括：获取第i时刻的所述载体的姿态，并将第i时刻的所述载体的姿态作为所述第i时刻的基座姿态。可选地，所述获取第i时刻的基座姿态，包括：当i＝1时，基于第i时刻的所述基座在基座坐标系三轴的线加速度、以及载体航向角，计算第i时刻的基座姿态；当第i时刻不为第1时刻时，分别获取第i时刻的所述基座在基座坐标系三轴的角速度和线加速度；基于第i时刻的所述基座在基座坐标系三轴的角速度和线加速度，计算第i时刻的基座平台误差角；获取第i-1时刻的基座姿态；基于所述第i时刻的基座平台误差角，更新所述第i-1时刻的基座姿态，得到所述第i时刻的基座姿态。可选地，所述基于第i时刻的所述基座在基座坐标系三轴的线加速度、以及载体航向角，计算第i时刻的基座姿态，包括：按照如下公式计算所述第i时刻的基座姿态，其中，为第i时刻的基座的航向角，为第i时刻的基座的横滚角，为第i时刻的基座的俯仰角，ψbody,i为第i时刻的载体航向角；为第i时刻所述基座在所述基座坐标系三轴的线加速度。可选地，所述基于第i时刻的所述基座在基座坐标系三轴的角速度和线加速度，计算第i时刻的基座平台误差角，包括：基于第i时刻的所述基座在所述基座坐标系三轴的线加速度，计算第i时刻的基座姿态观测量；获取第i-1时刻的基座平台误差角；基于所述第i-1时刻的基座平台误差角和所述第i时刻的所述基座在所述基座坐标系三轴的角速度，计算第i时刻的预估基座平台误差角；基于所述第i时刻的基座姿态观测量，对所述第i时刻的预估基座平台误差角进行修正，得到所述第i时刻的基座平台误差角。可选地，所述基于第i时刻的所述基座在所述基座坐标系三轴的线加速度，计算第i时刻的基座姿态观测量，包括：基于第i时刻的所述基座在所述基座坐标系三轴的线加速度，计算第i时刻的基座横滚角；当所述基座横滚角大于第二阈值时，获取第i时刻的载体姿态，并将所述第i时刻的载体姿态作为所述第i时刻的基座姿态观测量；当所述基座横滚角小于所述第二阈值时，获取第i时刻的载体航向角，并基于所述第i时刻的载体航向角、及第i时刻的所述基座在所述基座坐标系三轴的线加速度计算所述第i时刻的基座姿态观测量。可选地，所述基于所述第i时刻的基座姿态、及所述第i+1时刻的所述云台中各个轴电机的输出轴的角位移，计算所述云台姿态观测量，包括：当所述轴电机还包括横滚轴电机和俯仰轴电机时，将所述第i时刻的基座姿态顺次按X轴-Y轴-Z轴、Y轴-Z轴-X轴、或者Z轴-X轴-Y轴的转动顺序转动对应轴电机的输出轴在第i+1时刻的角位移，得到第i+1时刻的云台姿态观测量，所述Z轴对应所述航向轴电机，所述X轴对应所述横滚轴电机，所述Y轴对应所述俯仰轴电机。可选地，当i＝1时，所述获取第i时刻的云台姿态，包括：按照如下公式计算所述第i时刻的云台姿态，其中，为第i时刻的云台的航向角，为第i时刻的云台的横滚角，为第i时刻的云台的俯仰角，ψbody,i为第i时刻的载体航向角；ψm,i为第i时刻的航向轴电机的输出轴的角位移，为第i时刻所述图像采集设备在所述云台坐标系三轴的线加速度。可选地，所述基于所述第i+1时刻的云台平台误差角，更新所述第i时刻的云台姿态，包括：基于所述第i+1时刻的云台平台误差角，计算第i+1时刻相对于第i时刻的云台姿态增量；采用所述第i+1时刻相对于第i时刻的云台姿态增量，更新所述第i时刻的云本文档来自技高网...

【技术保护点】
1.一种云台姿态估计方法，其特征在于，所述方法包括：/n分别获取第i+1时刻的云台中图像采集设备在云台坐标系三轴的角速度和线加速度、以及所述云台中航向轴电机的输出轴的角位移；/n基于第i+1时刻的所述云台中图像采集设备在云台坐标系三轴的角速度和线加速度、以及所述云台中航向轴电机的输出轴的角位移，计算第i+1时刻的云台平台误差角；/n获取第i时刻的云台姿态；/n基于所述第i+1时刻的云台平台误差角，更新所述第i时刻的云台姿态，得到第i+1时刻的云台姿态。/n

【技术特征摘要】
1.一种云台姿态估计方法，其特征在于，所述方法包括：
分别获取第i+1时刻的云台中图像采集设备在云台坐标系三轴的角速度和线加速度、以及所述云台中航向轴电机的输出轴的角位移；
基于第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-1时刻的基座姿态，得到所述第i时刻的基座姿态。


6.根据权利要求5所述的方法，其特征在于，所述基于第i时刻的所述基座在基座坐标系三轴的线加速度、以及载体航向角，计算第i时刻的基座姿态，包括：
按照如下公式计算所述第i时刻的基座姿态，









其中，为第i时刻的基座的航向角，为第i时刻的基座的横滚角，为第i时刻的基座的俯仰角，ψbody,i为第i时刻的载体航向角；为第i时刻所述基座在所述基座坐标系三轴的线加速度。


7.根据权利要求5所述的方法，其特征在于，所述基于第i时刻的所述基座在基座坐标系三轴的角速度和线加速度，计算第i时刻的基座平台误差角，包括：
基于第i时刻的所述基座在所述基座坐标系三轴的线加速度，计算第i时刻的基座姿态观测量；
获取第i-1时刻的基座平台误差角；
基于所述第i-1时刻的基座平台误差角和所述第i时刻的所述基座在所述基座坐标系三轴的角速度，计算第i时刻的预估基座平台误差角；
基于所述第i时刻的基座姿态观测量，对所述第i时刻的预估基座平台误差角进行修正，得到所述第i时刻的基座平台误差角。


8.根据权利要求7所述的方法，其特征在于，所述基于第i时刻的所述基座在所述基座坐标系三轴的线加速度，计算第i时刻的基座姿态观测量，包括：
基于第i时刻的所述基座在所述基座坐标系三轴的线加速度，计算第i时刻的基座横滚角；
当所述基座横滚角大于第二阈值时，获取第i时刻的载体姿态，并将所述第i时刻的载体姿态作为所述第i时刻的基座姿态观测量；
当所述基座横滚角小于所述第二阈值时，获取第i时刻的载体航向角，并基于所述第i时刻的载体航向角、及第i时刻的所述基座在所述基座坐标系三轴的线加速度计算所述第i时刻的基座姿态观测量。


9.根据权利要求3所述的方法，其特征在于，所述基于所述第i时刻的基座姿态、及所述第i+1时刻的所述云台中各个轴电机的输出轴的角位移，计算所述云台姿态观测量，包括：
当所述轴电机还包括横滚轴电机和俯仰轴电机时，将所述第i时刻的基座姿态顺次按X轴-Y轴-Z轴、Y轴-Z轴-X轴、或者Z轴-X轴-Y轴的转动顺序转动对应轴电机的输出轴在第i+1时刻的角位移，得到第i+1时刻的云台姿态观测量，所述Z轴对应所述航向轴电机，所述X轴对应所述横滚轴电机，所述Y轴对应所述俯仰轴电机。


10.根据权利要求1-9中任一项所述的方法，其特征在于，当i＝1时，所述获取第i时刻的云台姿态，包括：
按照如下公式计算所述第i时刻的云台姿态，









其中，为第i时刻的云台的航向角，为第i时刻的云台的横滚角，θic为第i时刻的云台的俯仰角，ψbody,i为第i时刻的载体航向角；ψm,i为第i时刻的航向轴电机的输出轴的角位移，为第i时刻所述图像采集设备在所述云台坐标系三轴的线加速度。


11.根据权利要求1-9中任一项所述的方法，其特征在于，所述基于所述第i+1时刻的云台平台误差角，更新所述第i时刻的云台姿态，包括：
基于所述第i+1时刻的云台平台误差角，计算第i+1时刻相对于第i时刻的云台姿态增量；
采用所述第i+1时刻相对于第i时刻的云台姿态增量...

【专利技术属性】
技术研发人员：王洁，
申请(专利权)人：杭州海康机器人技术有限公司，
类型：发明
国别省市：浙江;33