[发明专利]一种运动载体姿态实时测量方法、装置

说明书

本申请涉及一种运动载体姿态角实时测量方法、装置。所述方法包括：获取惯性传感器实时采集的运动载体的角速度和加速度；根据所述角速度和加速度，通过满足下降条件的共轭梯度法对姿态四元数进行寻优估计，获得寻优估计值；分别建立角速度与姿态角的系统模型、加速度与姿态角的观测模型，并根据系统模型和观测模型建立无迹卡尔曼滤波递推方程；将寻优估计值导入无迹卡尔曼滤波递推方程中，计算得到运动载体姿态角。采用本方法能够提高运动载体姿态角计算的准确度。

技术领域

本申请涉及姿态测量技术领域，特别是涉及一种运动载体姿态实时计算方法、装置。

背景技术

运动载体包括飞行器、机器人、两轮平衡车等技术项目的研究取得了巨大的进步，这些项目能够完成侦察、监控以及危险环境下的搜救等任务。而在这些项目中，每一个都需要对载体或部件的姿态进行实时测量，这是实现姿态精确控制的基础。

传统测量运动载体姿态是采用两台高速摄像机进行测量，不仅设备价格昂贵，而且不能实现实时测量，只能进行后处理，而且操作繁琐，不易快速得到结果；或单独使用陀螺仪完成姿态角测量，误差会随时间积累，产生较大的误差，短时间结果可靠长时间不可靠；或单独使用加速度计完成姿态角测量在非平稳状态下(静止或匀速运动)结果有干扰，长时间结果可靠短时间不可靠。

总之，现有的运动载体姿态测量方式，不能准确的获得实时姿态角。

发明内容

基于此，有必要针对上述技术问题，提供一种能够提高运动载体实时姿态角计算的准确度的运动载体姿态角实时测量方法、装置。

一种运动载体姿态角实时测量方法，所述方法包括：

获取惯性传感器实时采集的运动载体的角速度和加速度；

根据所述角速度和加速度，通过满足下降条件的共轭梯度法对姿态四元数进行寻优估计，获得寻优估计值；

分别建立角速度与姿态角的系统模型、加速度与姿态角的观测模型，并根据系统模型和观测模型建立无迹卡尔曼滤波递推方程；

将寻优估计值导入无迹卡尔曼滤波递推方程中，计算得到运动载体姿态角。

在其中一个实施例中，所述根据所述角速度和加速度，通过满足下降条件的共轭梯度法对姿态四元数进行寻优估计，获得寻优估计值，包括：

将运动载体的姿态角采用四元数表示，根据四元数与姿态矩阵的关系和归一化重力加速度矢量，计算归一化重力加速度矢量在k次采样时刻载体坐标系下的投影值；

将第k次采样时刻的加速度计在x、y、z轴上采集的加速度进行归一化处理，得到加速度向量；

将投影值与加速度向量相减，得到k次采样时刻测量误差；

根据测量误差构建关于姿态角的误差函数，获得在误差函数最小值时的寻优估计值；

其中，在对四元数进行更新迭代时，根据最优步长和搜索方向作为迭代参数，修改搜索方向的方向调控参数，使得共轭梯度法满足下降条件。

在其中一个实施例中，所述将运动载体的姿态角采用四元数表示，根据四元数与姿态矩阵的关系和归一化重力加速度矢量，计算归一化重力加速度矢量在k次采样时刻载体坐标系下的投影值，具体为：

采用四元数表示姿态角用矩阵形式表达： q_k＝[q_k0 q_k1 q_k2 q_k4]^T；

其中，q_k表示k次采样时刻的四元数，根据四元数q_k与姿态矩阵的关系得到姿态矩阵