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

权利要求书

1.一种运动载体姿态角实时测量方法，其特征在于，所述方法包括：

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

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

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

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

2.根据权利要求1所述的方法，其特征在于，所述根据所述角速度和加速度，通过满足下降条件的共轭梯度法对姿态四元数进行寻优估计，获得寻优估计值，包括：

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

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

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

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

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

3.根据权利要求2所述的方法，其特征在于，所述将运动载体的姿态角采用四元数表示，根据四元数与姿态矩阵的关系和归一化重力加速度矢量，计算归一化重力加速度矢量在k次采样时刻载体坐标系下的投影值，具体为：

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

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

设归一化重力加速度矢量为gⁿ＝[0 0 1]^T，则归一化重力加速度矢量在k次采样时刻载体坐标系下的投影值为：

其中，(g^b)_k为归一化重力加速度矢量在k次采样时刻载体坐标系下的投影值；

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

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

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

通过投影值(g^b)_k与加速度向量相减，得到k次采样时刻测量误差e_k：

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

根据测量误差构建关于姿态角的误差函数f_g(q_k)；

计算误差函数f_g(q_k)取最小值时的四元数q_k的值，作为在误差函数最小值时的寻优估计值。