[发明专利]基于捷联惯性导航系统的姿态估计和融合的方法

摘要

本发明公开了一种基于捷联惯性导航系统的姿态估计和融合的方法，包括：(1)利用六自由度惯性测量组件(简称IMU)中的传感器感应载体运动特性；(2)进行捷联惯性解算；(3)在进行步骤(2)同时利用加速度估计系统的姿态信号；(4)判断姿态估计值的可信度；(5)进行姿态信息融合；(6)输出导航参数；本发明的方法具有以下优点：(1)不增加任何硬件成本并且不改变硬件安装结构；(2)具有完全的自主性，不受外界环境的影响，能够全天候工作；(3)能够有效提高系统的导航精度，特别是姿态精度；(4)系统能够为控制显示装置提供每秒钟50次以上的导航信号。

主权项

1、一种基于捷联惯性导航系统的姿态估计和融合的方法，包括下列步骤：(1)利用六自由度惯性测量组件中的传感器感应载体运动特性：通过陀螺仪敏感运动载体沿其轴向的角速度信号，通过加速度计测量沿载体轴向的线加速度信号，并将信号传输给导航计算机；(2)进行捷联惯性解算：导航计算机对陀螺仪敏感的角速度信号进行姿态解算，对姿态矩阵进行三角函数计算即提取出载体的姿态角和方位角，用加速度计测量的加速度进行由载体坐标系至地理坐标系的坐标变换计算，进而在地理坐标系中进行速度、位置等导航参数计算；其特征在于，在进行上述步骤(2)的同时进行下列步骤：(3)利用线加速度估计系统的姿态信号：根据三轴加速度计输出、地理坐标系和载体坐标系之间的关系，当系统加速度很小时，忽略速度和加速度对姿态的影响，得到加速度计输出矢量fb@[fbx fby fbz]T、$f_b@{\left[\begin{array}{ccc}{f}_{\mathrm{bx}}& f_{\mathrm{by}}& f_{\mathrm{bz}}\end{array}\right]}^T,$ 重力加速度g、系统横滚角的估计值γD、俯仰角估计值θD和航向角估计值ψD(中间变量)之间的关系：$\left[\begin{array}{c}{f}_{\mathrm{bx}}\\ f_{\mathrm{by}}\\ f_{\mathrm{bz}}\end{array}\right]\approx \left[\begin{array}{ccc}\cos {\psi }_D\cos {\theta }_D& \sin {\psi }_D\cos {\theta }_D& -\sin {\theta }_D\\ \cos {\psi }_D\sin {\theta }_D\sin {\gamma }_D& \sin {\psi }_D\sin {\theta }_D\sin {\gamma }_D& \\ -\sin {\psi }_D\cos {\gamma }_D& +\cos {\psi }_D\cos {\gamma }_D& \cos {\theta }_D\sin {\gamma }_D\\ \cos {\psi }_D\sin {\theta }_D\cos {\gamma }_D& \sin {\psi }_D\sin {\theta }_D\cos {\gamma }_D& \\ +\sin {\psi }_D\sin {\gamma }_D& -\cos {\psi }_D\sin {\gamma }_D& \cos {\theta }_D\cos {\gamma }_D\end{array}\right]\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]---\left(1\right)$ 从而可以得到：$\left\{\begin{array}{c}{f}_{\mathrm{bx}}=\sin {\theta }_D·g\\ f_{\mathrm{by}}=-\cos {\theta }_D·\sin {\gamma }_D·g\\ f_{\mathrm{bz}}=-\cos {\theta }_D·\cos {\gamma }_D·g\end{array}\right.---\left(2\right)$ 由此，可以得到内阻尼姿态估计的公式为：$\left\{\begin{array}{c}{\theta }_D=\arcsin \left(\frac{f_{\mathrm{bx}}}{\sqrt{f_{\mathrm{bx}}^2+f_{\mathrm{by}}^2+f_{\mathrm{bz}}^2}}\right)\\ {\gamma }_D=\left\{\begin{array}{c}\arctan (f_{\mathrm{by}}/f_{\mathrm{bz}});\left|f_{\mathrm{bz}}\right|\ge 0.1g\\ \arccos (-\frac{f_{\mathrm{bz}}}{\cos {\theta }_D·\sqrt{f_{\mathrm{bx}}^2+f_{\mathrm{by}}^2+f_{\mathrm{bz}}^2}});\left|f_{\mathrm{bz}}\right|<0.1g\end{array}\right.\end{array}\right.---\left(3\right)$ (4)判断姿态估计值的可信度：根据该加速度计测量值判断系统加速运动的情况，确定根据式(3)得到的姿态估计值的可信度；(5)进行姿态信息融合：如果根据式(4)得到的式(3)的姿态估计值是可信的，则进行卡尔曼滤波器的构建进行姿态信息融合，否则，直接跳到步骤(6)；本步骤包括如下步骤：①建立卡尔曼滤波的状态方程：使用一阶线性随机微分方程来描述捷联航姿系统的状态误差如下：X&(t)8×1＝A(t)8×8X(t)+G(t)8×9W(t)9×1 (5)式中，X(t)为t时刻系统的状态向量；A(t)，G(t)分别为系统状态矩阵和噪声矩阵；W(t)为系统的噪声向量；系统的状态矢量为：X＝[φn φe φd δvn δve εrx εry εrz]T (6)系统的白噪声矢量为：W＝[ωgx ωgy ωgz ωrx ωry ωrz ωax ωay ωaz]T (7)其中，φn φe φd分别表示系统的北向、东向和地向的姿态误差；δvn δve分别表示系统的北向和东向的速度误差；εrx εry εrz分别表示X、Y、Z轴陀螺的误差；ωgx ωgy ωgz分别表示X、Y、Z轴陀螺误差模型的一阶马儿可夫过程；ωrx ωry ωrz 分别表示X、Y、Z轴陀螺误差模型的白噪声误差；ωax ωay ωaz分别表示X、Y、Z轴加速度计误差模型的白噪声误差；系统噪声系数矩阵为：$G\left(t\right)={\left[\begin{array}{ccc}-C_b^n& 0_{3\times 3}& 0_{3\times 3}\\ 0_{2\times 3}& 0_{2\times 3}& C_{b2\times 3}^n\\ 0_{3\times 3}& I_{3\times 3}& 0_{3\times 3}\end{array}\right]}_{8\times 9}---\left(8\right)$ 系统的状态转移阵为：$A\left(t\right)={\left[\begin{array}{cc}{\left(A_{\mathrm{INS}}\right)}_{5\times 5}& {\left(A_S\right)}_{5\times 3}\\ 0_{3\times 5}& {\left(A_{\mathrm{IMU}}\right)}_{3\times 3}\end{array}\right]}_{8\times 8}---\left(9\right)$ 上式中，$A_{\mathrm{IMU}}=\mathrm{diag}\left[\begin{array}{ccc}-\frac{1}{T_{\mathrm{rx}}}& -\frac{1}{T_{\mathrm{ry}}}& -\frac{1}{T_{\mathrm{rz}}}\end{array}\right]---\left(10\right)$ $A_S={\left[\begin{array}{c}-C_b^n\\ 0_{2\times 3}\end{array}\right]}_{5\times 3}---\left(11\right)$ AINS是对应5个基本导航参数的矩阵，其非零项元素为：$\left\{\begin{array}{cc}A\left(\mathrm{1,3}\right)=\frac{v_N}{R_M+h}& A\left(\mathrm{1,5}\right)=\frac{1}{R_N+h}\\ A\left(\mathrm{2,3}\right)={\omega }_{\mathrm{ie}}+\frac{v_N}{R_N+h}& A(2,4)=-\frac{1}{R_M+h}\\ A(3,1)=-\frac{v_N}{R_M+h}& A(3,2)=-{\omega }_{\mathrm{ie}}-\frac{v_E}{R_N+h}\\ A\left(\mathrm{4,2}\right)=-f_D& A\left(\mathrm{4,3}\right)=f_E\\ A\left(\mathrm{5,1}\right)=f_D& A\left(\mathrm{5,3}\right)=-f_E\end{array}\right.---\left(12\right)$ 对8阶状态方程对应的白噪声矢量，取白噪声方差阵为：$Q\left(t\right)=\mathrm{diag}\left[\begin{array}{ccccccccc}{\sigma }_{\mathrm{gx}}^2& {\sigma }_{\mathrm{gy}}^2& {\sigma }_{\mathrm{gz}}^2& \frac{{2\beta }_x{\sigma }_{\mathrm{rx}}^2}{T_{\mathrm{kf}}}& \frac{2{\beta }_y{\sigma }_{\mathrm{ry}}^2}{T_{\mathrm{kf}}}& \frac{2{\beta }_z{\sigma }_{\mathrm{rz}}^2}{T_{\mathrm{kf}}}& {\sigma }_{\mathrm{ax}}^2& {\sigma }_{\mathrm{ay}}^2& {\sigma }_{\mathrm{az}}^2\end{array}\right]---\left(13\right)$ 其中，σgx，σgy，σgz为陀螺的白噪声漂移均方根值，σrx，σry，σrz为陀螺的一阶马儿可夫漂移均方根值，βx、βx、βx为相关频率，σax，σay，σaz为加速度计白噪声均方根值，Tkf是离散化的周期时间；②建立卡尔曼滤波的观测方程：使用一阶线性随机微分方程来描述航姿系统的测量误差，其方程如下：$Z\left(t\right)@\left[\begin{array}{c}{\theta }_I-{\theta }_D\\ {\gamma }_I-{\gamma }_D\end{array}\right]=\left[\begin{array}{c}{\mathrm{\delta \theta }}_I+{\mathrm{\delta \theta }}_D\\ {\mathrm{\delta \gamma }}_I+{\mathrm{\delta \gamma }}_D\end{array}\right]@H\left(t\right)X\left(t\right)+N\left(t\right)---\left(13\right)$ 式中，Z(t)为t时刻系统的量测向量；H(t)为系统量测矩阵；N(t)为系统的量测噪声向量。θ、γ表示系统真实水平姿态角；θI、γI表示由步骤2计算的惯性系统姿态角，δθI、δγI表示惯性系统姿态误差角；θD、γD表示由步骤3估计的姿态角，δθD、δγD表示姿态估计值的误差角度；系统量测矩阵为：H(t)＝[I2×2 02×6] (14)内阻尼姿态的测量噪声虽然和系统加速度、速度、纬度都有关系，但是经过补偿后的内阻尼姿态误差没有呈现明显的线性相关性，所以近似为白噪声矢量： N(t)＝[MN ME]T (15)式中MN ME分别表示北向和东向估计姿态的白噪声误差；(6)输出导航参数：导航计算机按照规定的格式和速率，将计算得到的导航参数传输给相应的控制单元或者显示仪器。