[发明专利]基于角速率输入的高阶增强姿态方法

说明书

本发明公开一种基于角速率输入的高阶增强姿态方法，属于捷联惯性导航领域。该方法利用前一姿态更新周期和当前姿态更新周期内的角速率，首先通过对角速率进行拉格朗日插值并在当前姿态更新周期内积分来估计角增量，然后利用角速率二叉积的线性组合来估计不可交换性误差中的二阶旋转矢量项，接着利用角速率三叉积的线性组合来估计不可交换性误差中的三阶旋转矢量项，最后用求得的角增量、二阶旋转矢量项和三阶旋转矢量项的近似值来代替姿态更新周期内的旋转矢量，有效降低了刚体转动引起的不可交换性误差，进一步提高了姿态解算精度。

技术领域

本发明涉及一种基于角速率输入的高阶增强姿态方法，属于捷联惯性导航姿态解算技术领域。

背景技术

在捷联式惯性导航系统(Strapdown Inertial Navigation System,SINS)中，陀螺仪直接固联于载体，敏感载体相对惯性空间的角运动。姿态算法的性能直接影响SINS的导航精度，由于有限转动的不可交换性影响，姿态解算中不可避免地引入圆锥误差，尤其是在高动态环境下更为恶劣。Savage认为导航算法的误差低于惯性器件所带来的误差的5％以内是合理的，所以随着惯性器件不断发展的同时，导航算法的精度也需要改进，因此有必要研究一种高精度姿态算法来补偿圆锥误差。

1971年Bortz最早提出了等效旋转矢量概念，在理论上解决了不可交换性误差的补偿问题。在此基础上，Miller、Lee和Ignagni等人陆续提出了多子样圆锥误差补偿算法。王茂松考虑了三叉积项的影响，提出了高阶姿态补偿算法以适应振动与旋转并存的环境，但该算法计算量太大，无法用于实时处理系统。以上算法的实现都是利用陀螺仪输出的角增量信息，但当陀螺仪输出信号为角速率时，若利用数值积分方法将角速率信息转换为角增量信息，然后再应用传统圆锥误差补偿算法，算法精度会明显降低，无法满足惯性器件对姿态解算精度的要求。于是也有学者研究了纯角速率输入下的姿态误差补偿算法，然而这些算法的精度有限，无法满足高精度导航性能的要求。

采用毕卡逼近法求解四元数微分方程时，首先要计算出载体运动时刻所对应的姿态四元数Q(t)，再将姿态四元数Q(t)转化为姿态矩阵最后从姿态矩阵中求出姿态角。

求解四元数微分方程的毕卡逼近法：

式中：

由式(11)可知，需要对角速率矢量ω进行积分才能得到角增量θ，虽然在一个姿态更新周期内角增量θ较小，但矢量的有限转动不满足矢量相加的交换律，所以对角速率矢量的积分会产生不可交换性误差。为了消除四元数求姿中的不可交换性误差，Bortz引入等效旋转矢量的概念来描述刚体相对惯性空间的转动，并提出了著名的旋转矢量微分方程用于求解旋转矢量Φ：

式中，Φ是旋转矢量，Φ＝(Φ^TΦ)^1/2为旋转矢量的模，ω是载体角速率矢量。

由于在一个姿态更新周期H内Φ较小，忽略Φ的高次项，式(12)可简化为：

式(13)中等号后的第2项被称为圆锥校正项，第3项被称为三叉积项。传统的基于角速率输入的圆锥算法忽略了三叉积项的影响，只针对圆锥校正项进行了补偿，故传统算法所采用的旋转矢量微分方程在一个姿态更新周期H内的积分近似为：

式中，Φ(H)、θ(H)、δΦ_c(H)分别为一个姿态更新周期H内的旋转矢量、角增量和二阶旋转矢量项的理论值，姿态更新周期H＝t_m-t_m-1。

考虑不可交换性误差中的三阶旋转矢量项的影响，将式(14)代入到式(13)，可得：