CN103217174B - 一种基于低精度微机电系统的捷联惯导系统初始对准方法 - Google Patents

Abstract

本发明是涉及的是一种捷联惯导系统的初始对准方法，具体涉及一种在仅利用GPS辅助设备的条件下基于低精度微机电系统的捷联惯导系统初始对准的方法。本发明包括：获取载体坐标系到水平坐标系的方向余弦矩阵；建立低精度微机电系统的捷联惯导系统粗对准卡尔曼滤波状态方程；建立低精度微机电系统的捷联惯导系统粗对准的卡尔曼滤波量测方程；对载体姿态进行第一次修正；建立捷联惯导系统精对准的卡尔曼滤波状态方程和量测方程；进行第二次卡尔曼滤波；对载体姿态进行第二次修正，得到微机电系统的捷联惯导系统的准确捷联矩阵。本发明利用两次卡尔曼滤波估计出低精度MEMS捷联惯导系统误差的方法，完成了系统的初始对准，使应用更便捷。

Description

一种基于低精度微机电系统的捷联惯导系统初始对准方法

技术领域

本发明是涉及的是一种捷联惯导系统的初始对准方法，具体涉及一种在仅利用GPS辅助设备的条件下基于低精度微机电系统的捷联惯导系统初始对准的方法。

背景技术

初始对准的主要任务就是给定捷联惯导的初始姿态阵。捷联惯导初始对准一般可分为粗对准和精对准两个阶段。粗对准的主要任务是快速将将姿态误差控制在比较小的范围内,为下一步精对准创造条件；精对准的任务是在小角度误差的条件下采用线性模型估计出剩余的捷联惯导系统误差，并对系统进行修正，为下一步的导航工作提供初始值。

对于高精度捷联惯导系统的初始对准一般采用经典对准方法，如解析式对准方法和罗经回路对准方法，但是这两种方法都要求高精度的陀螺仪，以便能够测出地球的自转角速度，因为以上两种方法都要用地球的自转角速度才能自主的完成初始对准过程。对于低精度MEMS的捷联惯导系统由于陀螺仪漂移速度比较大，一般为1-10deg/h，无法量测到地球的自转角速度，因此对高精度捷联惯导系统能完成的初始对准方法对低精度MEMS（micro-electro-mechanicalsystems，微机电系统）的捷联惯导系统是无效的，不能够完成初始对准。但是随着低精度MEMS由于低成本的优势在汽车等民用领域的广泛应用，其初始对准问题的研究就显得十分必要了。

目前对于基于低精度MEMS捷联惯导系统的研究，如哈尔滨工程大学的硕士论文《基于MTi的硅微系统初始对准技术研究》：文中采用MEMS磁强计和MEMS加速度计的测量信号解算出一个能够用于精对准的捷联矩阵,完成粗对准过程；然后将卡尔曼滤波技术运用到精对准过程，将平台误差角从随机误差和随机干扰中估计出来,并用其对粗对准过程得到的捷联矩阵进行修正完成精对准。还有发表在《中国惯性技术学报》的《基于加速度计余度配置的MEMS捷联惯导初始对准方法》：文中分析了在载体大俯仰角条件下采用余度配置加速度计的输出值，通过加权最小二乘算法进行数据融合完成水平方向的初始对准，而航向信息只是说明通过外观测方式获得。

但是他们在研究低精度MEMS的初始对准方法，对其初始对准的航向信息要采用在其他辅助设备，如磁罗盘或磁强计的条件下才能完成对准，若没有辅助设备在有GPS的条件下也不能完成对准；本发明则仅需GPS作为辅助即可完成低精度MEMS的捷联惯导系统的初始对准，应用起来更为方便。

发明内容

本发明的目的在于提供一种能够解决在仅利用GPS辅助设备的条件下完成基于低精度微机电系统的捷联惯导系统的初始对准方法。

本发明的目的是这样实现的：

本发明包括以下步骤：

（1）根据加速度计输出获取载体坐标系b到水平坐标系b′的方向余弦矩阵

（2）以位置误差、速度误差和姿态误差为参数建立低精度微机电系统的捷联惯导系统粗对准卡尔曼滤波状态方程；

（3）建立低精度微机电系统的捷联惯导系统粗对准的卡尔曼滤波量测方程；

（4）根据卡尔曼滤波状态方程和量测方程进行第一次卡尔曼滤波，估计捷联惯导系统的误差，获取水平坐标系b′到计算地理坐标系t的方向余弦矩阵对载体姿态进行第一次修正；

（5）建立捷联惯导系统精对准的卡尔曼滤波状态方程和量测方程；

（6）利用卡尔曼滤波状态方程和量测方程进行第二次卡尔曼滤波；获取地理坐标系t到导航坐标系n的方向余弦矩阵对载体姿态进行第二次修正，得到微机电系统的捷联惯导系统的准确捷联矩阵

载体坐标系b到水平坐标系b′的方向余弦矩阵为：

其中是根据初始时刻东向加速度计的输出得到的，γ是根据初始时刻北向加速度计的输出得到的，

低精度微机电系统的捷联惯导系统粗对准卡尔曼滤波的状态方程的状态向量为：

其中，捷联惯导系统输出的经度误差为δλ、纬度误差为和高度误差为δh；、载体在计算地理坐标系上的东向为北向和天向的速度误差，地理坐标系上的东向为E、北向为N和天向为U，上角标t表示计算地理坐标系，经过步骤1后剩余的载体东向纵摇误差角为Φ_E、北向横摇误差角为Φ_N；Y代表的是载体的粗略航向角。

低精度微机电系统的捷联惯导系统粗对准的卡尔曼滤波量测方程为：

Z₁＝H₁X₁+V₁

其中，Z₁为卡尔曼滤波量测方程的量测向量；H₁为卡尔曼滤波量测方程的量测矩阵； $V_1={\left[\begin{array}{ccc}{\mathrm{\δl}}_E^t& {\mathrm{\δl}}_N^t& {\mathrm{\δl}}_U^t\end{array}\right]}^T$ 为微机电系统中心到GPS的天线相位中心的杆臂长度误差在计算地理坐标系上的投影；

卡尔曼滤波量测方程的量测向量为：

$Z_1={\left[\begin{array}{ccc}{\mathrm{\δr}}_E^t& {\mathrm{\δr}}_N^t& {\mathrm{\δt}}_U^t\end{array}\right]}^T$

其中GPS输出的经度误差为纬度误差为和高度误差为在计算地理坐标系上的投影；

卡尔曼滤波量测方程的量测矩阵为：

H₁＝[I_3×30_3×3M_3×4]

其中I_3×3为三阶单位阵：

$M_{3\×4}=\left[\begin{array}{cccc}0& l_U^t& -l_N^t& -l_E^t\\ -l_U^t& 0& l_E^t& -l_N^t\\ l_N^t& -l_E^t& 0& 0\end{array}\right]$

其中 $l_1={\left[\begin{array}{ccc}{l}_E^t& l_N^t& l_U^t\end{array}\right]}^T$ 为微机电系统中心到GPS的天线相位中心的杆臂长度在计算地理坐标系上的投影。

水平坐标系b′到计算地理坐标系t的方向余弦矩阵即：

$C_{b\′}^t=\left[\begin{array}{ccc}\cos Y& \sin Y& -{\mathrm{\Φ}}_N\cos Y+{\mathrm{\Φ}}_E\sin Y\\ -\sin Y& \cos Y& {\mathrm{\Φ}}_N\sin Y+{\mathrm{\Φ}}_E\cos Y\\ {\mathrm{\Φ}}_N& -{\mathrm{\Φ}}_E& 1\end{array}\right]$

其中，Φ_E、Φ_N、sinY和cosY分别是步骤（4）中卡尔曼滤波方法估计的输出值。

对载体姿态进行第一次修正，即测量得到载体坐标系b到计算地理坐标系t的方向余弦矩阵为：

$C_b^t=C_{b^{\′}}^t{C}_b^{b^{\′}}$

捷联惯导系统精对准的卡尔曼滤波状态方程的状态向量为：

其中，捷联惯导系统输出的经度误差为δλ、纬度误差为和高度误差为δh；载体在导航坐标系上的东向为北向为和天向的速度误差；上角标n表示导航坐标系；经过步骤（4）修正后捷联惯导系统剩余的纵摇为ψ_E、横摇为ψ_N和航向误差角为ψ_U。

计算地理坐标系t到导航坐标系n的方向余弦矩阵为：

$C_t^n=\left[\begin{array}{ccc}1& -{\mathrm{\ψ}}_U& {\mathrm{\ψ}}_N\\ {\mathrm{\ψ}}_U& 1& -{\mathrm{\ψ}}_E\\ -{\mathrm{\ψ}}_N& {\mathrm{\ψ}}_E& 1\end{array}\right]$

对载体姿态的第二次修正，即测量得到载体坐标系b到导航坐标系n的方向余弦矩阵为：

$C_b^n=C_t^n{C}_b^t.$

本发明的有益效果在于：针对基于低精度微机电系统的捷联惯导系统初始对准的问题，提供了一种在仅需GPS作为辅助设备的条件下，利用两次卡尔曼滤波估计出低精度MEMS捷联惯导系统误差的方法，完成了系统的初始对准，使应用更便捷。

附图说明

图1为本发明的流程图。

具体实施方式

下面结合附图举例对本发明做更详细地描述：基于低精度MEMS的捷联惯导系统的初始对准方法

1、一种基于低精度MEMS捷联惯导系统的初始对准方法，其特征在于包括以下步骤：

步骤1、根据加速度计的初始时刻的输出得到得到载体坐标系b到水平坐标系b′的方向余弦矩阵

（1）利用东向加速度计的初始时刻的输出得到：

其中是东向加速度计初始时刻输出。

（2）利用北向加速度计的初始时刻的输出得到：

$\mathrm{\γ}=f_N^b/9.78049$

其中是北向加速度计初始时刻输出。

（3）利用（1）、（2）得到的角度计算得到载体坐标系b到水平坐标系b′的方向余弦矩阵即：

步骤2、利用位置误差、速度误差和姿态误差建立低精度MEMS捷联惯导系统粗对准的卡尔曼滤波状态方程，即

$\frac{d}{\mathrm{dt}}{X}_1=F_1{X}_1+G_1{W}_1$

其中，X₁为卡尔曼滤波状态方程的状态向量；F₁和G₁分别为卡尔曼滤波状态方程的状态矩阵和偏差矩阵；W₁为卡尔曼滤波状态方程的偏差向量；

卡尔曼滤波状态方程的状态向量为：

卡尔曼滤波状态方程的偏差向量为：

W₁＝[▽_E▽_N▽_Uε_Eε_N]^T

其中，δλ、和δh分别代表的是捷联惯导系统输出的经度误差、纬度误差和高度误差；和分别代表载体在计算地理坐标系上的东向、北向和天向的速度误差；下角标E、N、U分别表示计算地理坐标系上的东向、北向和天向，上角标t表示计算地理坐标系，Φ_E和Φ_N分别代表的是经过步骤1后剩余的载体东向（纵摇）误差角和北向（横摇）误差角；Y代表的是载体的粗略航向角；▽_E、▽_N和▽_U分别代表东向、北向和天向轴加速度计的零偏；ε_E和ε_N分别代表东向和北向轴陀螺仪的常值漂移。将步骤1中的作为该阶段的初始方向余弦矩阵即载体坐标系b到计算地理坐标系t的初始方向余弦矩阵为：

$\tilde{T}=C_b^{b^{\′}}=\left[\begin{array}{ccc}{\tilde{C}}_{11}& {\tilde{C}}_{12}& {\tilde{C}}_{13}\\ {\tilde{C}}_{21}& {\tilde{C}}_{22}& {\tilde{C}}_{23}\\ {\tilde{C}}_{31}& {\tilde{C}}_{32}& {\tilde{C}}_{33}\end{array}\right]$

卡尔曼滤波状态方程的状态矩阵：

$F_1=\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{et}}^t\×& I_{3\×3}& 0_{3\×4}\\ 0_{3\×3}& {\mathrm{\ω}}_{\mathrm{it}}^t\×& B_{3\×4}\\ 0_{2\×3}& 0_{2\×3}& A_{2\×4}\\ 0_{2\×3}& 0_{2\×3}& 0_{2\×4}\end{array}\right]$

其中，I_3×3为三阶单位阵；为的反对称矩阵，是计算地理坐标系相对地球坐标系的角速率，R为地球半径，为当地的纬度值；为的反对称矩阵， ${\mathrm{\ω}}_{\mathrm{it}}^t={\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{itE}}^t& {\mathrm{\ω}}_{\mathrm{itN}}^t& {\mathrm{\ω}}_{\mathrm{itU}}^t\end{array}\right]}^T$ 是陀螺仪输出的载体坐标系相对惯性坐标系的角速率在计算地理坐标系上的投影；

$B_{3\×4}=\left[\begin{array}{cccc}0& -f_U^t& f_N^t& f_E^t\\ f_U^t& 0& -f_E^t& f_N^t\\ -f_N^t& f_E^t& 0& 0\end{array}\right]$

其中 $f^t={\left[\begin{array}{ccc}{f}_E^t& f_N^t& f_U^t\end{array}\right]}^T$ 是加速度计输出的载体坐标系相对惯性坐标系的加速度在计算地理坐标系上的投影；

$A_{2\×4}=\left[\begin{array}{cccc}0& {\mathrm{\ω}}_{\mathrm{itU}}^t& -{\mathrm{\ω}}_{\mathrm{itN}}^t& -{\mathrm{\ω}}_{\mathrm{itE}}^t\\ -{\mathrm{\ω}}_{\mathrm{itU}}^t& 0& {\mathrm{\ω}}_{\mathrm{itE}}^t& -{\mathrm{\ω}}_{\mathrm{itN}}^t\end{array}\right]$

卡尔曼滤波状态方程的偏差矩阵：

$G_1=\left[\begin{array}{cc}{0}_{3\×3}& 0_{3\×2}\\ C_{3\×3}& 0_{3\×2}\\ 0_{2\×3}& C_{2\×2}\\ 0_{2\×3}& 0_{2\×2}\end{array}\right]$

其中， $C_{2\×2}=\left[\begin{array}{cc}{\tilde{C}}_{11}& {\tilde{C}}_{12}\\ {\tilde{C}}_{21}& {\tilde{C}}_{22}\end{array}\right],$ $C_{3\×3}=\left[\begin{array}{ccc}{\tilde{C}}_{11}& {\tilde{C}}_{12}& {\tilde{C}}_{13}\\ {\tilde{C}}_{21}& {\tilde{C}}_{22}& {\tilde{C}}_{23}\\ {\tilde{C}}_{31}& {\tilde{C}}_{32}& {\tilde{C}}_{33}\end{array}\right]$

步骤3、建立低精度MEMS捷联惯导系统粗对准的卡尔曼滤波量测方程，即

Z₁＝H₁X₁+V₁

其中，Z₁为卡尔曼滤波量测方程的量测向量；H₁为卡尔曼滤波量测方程的量测矩阵； $V_1={\left[\begin{array}{ccc}{\mathrm{\δl}}_E^t& {\mathrm{\δl}}_N^t& {\mathrm{\δl}}_U^t\end{array}\right]}^T$ 为MEMS中心到GPS的天线相位中心的杆臂长度误差在计算地理坐标系上的投影；

卡尔曼滤波量测方程的量测向量为：

$Z_1={\left[\begin{array}{ccc}{\mathrm{\δr}}_E^t& {\mathrm{\δr}}_N^t& {\mathrm{\δr}}_U^t\end{array}\right]}^T$

其中和分别代表的是GPS输出的经度误差、纬度误差和高度误差在计算地理坐标系上的投影；

卡尔曼滤波量测方程的量测矩阵为：

H₁＝[I_3×30_3×3M_3×4]

其中I_3×3为三阶单位阵

$M_{3\×4}=\left[\begin{array}{cccc}0& l_U^t& {-l}_N^t& {-l}_E^t\\ {-l}_U^t& 0& l_E^t& {-l}_N^t\\ l_N^t& {-l}_E^t& 0& 0\end{array}\right]$

其中 $l^t={\left[\begin{array}{ccc}{l}_E^t& l_N^t& l_U^t\end{array}\right]}^T$ 为MEMS中心到GPS的天线相位中心的杆臂长度在计算地理坐标系上的投影。

步骤4、利用步骤2、步骤3中的卡尔曼滤波状态方程和量测方程进行第一次用卡尔曼滤波方法估计捷联惯导系统的误差，并计算得到水平坐标系b′到计算地理坐标系t的方向余弦矩阵对载体姿态进行第一次修正，完成捷联惯导系统的粗对准。

水平坐标系b′到计算地理坐标系t的方向余弦矩阵为：

$C_{b^{\′}}^t=\left[\begin{array}{ccc}\cos Y& \sin Y& 0\\ -\sin Y& \cos Y& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& -{\mathrm{\Φ}}_N\\ 0& 1& {\mathrm{\Φ}}_E\\ {\mathrm{\Φ}}_N& {-\mathrm{\Φ}}_E& 1\end{array}\right]=\left[\begin{array}{ccc}\cos Y& \sin Y& -{\mathrm{\Φ}}_N\cos Y+{\mathrm{\Φ}}_E\sin Y\\ -\sin Y& \cos Y& {\mathrm{\Φ}}_N\sin Y+{\mathrm{\Φ}}_E\cos Y\\ {\mathrm{\Φ}}_N& -{\mathrm{\Φ}}_E& 1\end{array}\right]$

其中，Φ_E、Φ_N、sinY和cosY分别是步骤4中卡尔曼滤波方法估计的输出值。

对载体姿态进行第一次修正，即得到载体坐标系b到计算地理坐标系t的方向余弦矩阵为：

$C_b^t=C_{b^{\′}}^t{C}_b^{b^{\′}}$

步骤5、建立捷联惯导系统精对准的卡尔曼滤波状态方程和量测方程。

（1）捷联惯导系统精对准的卡尔曼滤波状态方程为：

$\frac{d}{\mathrm{dt}}{X}_2=F_2{X}_2+G_2{W}_2$

其中，X₂为卡尔曼滤波状态方程的状态向量；F₂和G₂分别为卡尔曼滤波状态方程的状态矩阵和偏差矩阵；W₂为卡尔曼滤波状态方程的偏差向量；

卡尔曼滤波状态方程的状态向量为：

卡尔曼滤波状态方程的偏差向量为：

$W_2={\left[\begin{array}{cccccc}{\▿}_E& {\▿}_N& {\▿}_U& {\mathrm{\ϵ}}_E& {\mathrm{\ϵ}}_N& {\mathrm{\ϵ}}_U\end{array}\right]}^T$

其中，δλ、和δh分别代表的是捷联惯导系统输出的经度误差、纬度误差和高度误差；和分别代表的是载体在导航坐标系上的东向、北向和天向的速度误差；上角标n表示导航坐标系；ψ_E、ψ_N和ψ_U分别代表的是经过步骤4修正后捷联惯导系统剩余的平台东向、北向和天向的失准角；▽_E、▽_N和▽_U分别代表东向、北向和天向轴加速度计的零偏；ε_E、ε_N和ε_U分别代表东向、北向和天向轴陀螺仪的常值漂移。将步骤4中得到修正的载体姿态作为该阶段的初始方向余弦矩阵T，即载体坐标系b到导航坐标系n的初始方向余弦矩阵T为：

$T=C_b^t=\left[\begin{array}{ccc}{\tilde{T}}_{11}& {\tilde{T}}_{12}& {\tilde{T}}_{13}\\ {\tilde{T}}_{21}& {\tilde{T}}_{22}& {\tilde{T}}_{23}\\ {\tilde{T}}_{31}& {\tilde{T}}_{32}& {\tilde{T}}_{33}\end{array}\right]$

卡尔曼滤波状态方程的状态矩阵：

$F_2=\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{en}}^n\×& I_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& {\mathrm{\ω}}_{\mathrm{in}}^n\×& D_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& {\mathrm{\ω}}_{\mathrm{in}}^n\×\end{array}\right]$

其中，I_3×3为三阶单位阵；为的反对称矩阵，是导航坐标系相对地球坐标系的角速率，R为地球半径，为当地的纬度值；为的反对称矩阵， ${\mathrm{\ω}}_{\mathrm{in}}^n={\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{inE}}^n& {\mathrm{\ω}}_{\mathrm{inN}}^n& {\mathrm{\ω}}_{\mathrm{inU}}^n\end{array}\right]}^T$ 是陀螺仪输出的载体坐标系相对惯性坐标系的角速率在导航坐标系上的投影；

$D_{3\×3}=\left[\begin{array}{ccc}0& -f_U^n& f_N^n\\ f_U^n& 0& -f_E^n\\ -f_N^n& f_E^n& 0\end{array}\right]$

其中 $f^n={\left[\begin{array}{ccc}{f}_E^n& f_N^n& f_U^n\end{array}\right]}^T$ 是加速度计输出的载体坐标系相对惯性坐标系的加速度在导航坐标系上的投影；

卡尔曼滤波状态方程的偏差矩阵：

$G_2=\left[\begin{array}{cc}{0}_{3\×3}& 0_{3\×3}\\ C_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& C_{3\×3}\end{array}\right]$

其中， $C_{3\×3}=\left[\begin{array}{ccc}{\tilde{T}}_{11}& {\tilde{T}}_{12}& {\tilde{T}}_{13}\\ {\tilde{T}}_{21}& {\tilde{T}}_{22}& {\tilde{T}}_{23}\\ {\tilde{T}}_{31}& {\tilde{T}}_{32}& {\tilde{T}}_{33}\end{array}\right]$

（2）捷联惯导系统精对准的卡尔曼滤波量测方程为：

Z₂＝H₂X₂+V₂

其中，Z₂为卡尔曼滤波量测方程的量测向量；H₂为卡尔曼滤波量测方程的量测矩阵； $V_2={\left[\begin{array}{ccc}{\mathrm{\δl}}_E^n& {\mathrm{\δl}}_N^n& {\mathrm{\δl}}_U^n\end{array}\right]}^T$ 为MEMS中心到GPS的天线相位中心的杆臂长度误差在导航坐标系上的投影；

卡尔曼滤波量测方程的量测向量为：

$Z_2={\left[\begin{array}{ccc}{\mathrm{\δr}}_E^n& {\mathrm{\δr}}_N^n& {\mathrm{\δr}}_U^n\end{array}\right]}^T$

其中和分别代表的是GPS输出的经度误差、纬度误差和高度误差在导航坐标系上的投影；

卡尔曼滤波量测方程的量测矩阵为：

H₂＝[I_3×30_3×3M_3×3]

其中I_3×3为三阶单位阵

$M_{3\×3}=\left[\begin{array}{ccc}0& l_U^n& {-l}_N^n\\ {-l}_U^n& 0& l_E^n\\ l_N^n& {-l}_E^n& 0\end{array}\right]$

其中 $l^n={\left[\begin{array}{ccc}{l}_E^n& l_N^n& l_U^n\end{array}\right]}^T$ 为MEMS中心到GPS的天线相位中心的杆臂长度在导航坐标系上的投影

步骤6、利用步骤5中得到的卡尔曼滤波状态方程和量测方程进行第二次用卡尔曼滤波方法估计捷联惯导系统的误差捷联惯导系统的误差；并计算得到计算地理坐标系t到导航坐标系n的方向余弦矩阵对载体姿态进行第二次修正，得到捷联惯导系统初始对准的捷联矩阵完成捷联惯导系统的精对准，为下一步的导航提供初始条件。

计算地理坐标系t到导航坐标系n的方向余弦矩阵为：

$C_t^n=I+(\mathrm{\ψ}\×)=\left[\begin{array}{ccc}1& -{\mathrm{\ψ}}_U& {\mathrm{\ψ}}_N\\ {\mathrm{\ψ}}_U& 1& -{\mathrm{\ψ}}_E\\ -{\mathrm{\ψ}}_N& {\mathrm{\ψ}}_E& 1\end{array}\right]$

其中，I是三阶的单位矩阵； $\mathrm{\ψ}={\left[\begin{array}{ccc}{\mathrm{\ψ}}_E& {\mathrm{\ψ}}_N& {\mathrm{\ψ}}_U\end{array}\right]}^T$ 是用卡尔曼滤波方法估计得到经过步骤4修正后捷联惯导系统剩余的平台东向、北向和天向的失准角；(ψ×)是ψ的反对称矩阵。

对载体姿态的第二次修正，即得到载体坐标系b到导航坐标系n的方向余弦矩阵为：

$C_b^n=C_t^n{C}_b^t=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]$

其中，T₁₁＝cosHcosγ+ψ_UsinHcosγ-Φ_NcosHsinγ+Φ_EsinHsinγ+ψ_Nsinγ

T₂₁＝ψ_UcosHcosγ-sinHcosγ+Φ_NsinHsinγ+Φ_EcosHsinγ-ψ_Esinγ

T₃₁＝-ψ_UcosHcosγ-ψ_EsinHcosγ+Φ_Ncosγ+sinγ

至此就完成了基于低精度MEMS捷联惯导系统初始对准，即为系统导航时所用的初始捷联矩阵。

Claims (6)

1.一种基于低精度微机电系统的捷联惯导系统初始对准方法，其特征在于，包括以下步骤：

(1)根据加速度计输出获取载体坐标系b到水平坐标系b′的方向余弦矩阵

(2)以位置误差、速度误差和姿态误差为参数建立低精度微机电系统的捷联惯导系统粗对准卡尔曼滤波状态方程；

(3)建立低精度微机电系统的捷联惯导系统粗对准的卡尔曼滤波量测方程；

(4)根据卡尔曼滤波状态方程和量测方程进行第一次卡尔曼滤波，估计捷联惯导系统的误差，获取水平坐标系b′到计算地理坐标系t的方向余弦矩阵对载体姿态进行第一次修正；

(5)建立捷联惯导系统精对准的卡尔曼滤波状态方程和量测方程；

(6)利用卡尔曼滤波状态方程和量测方程进行第二次卡尔曼滤波；获取地理坐标系t到导航坐标系n的方向余弦矩阵对载体姿态进行第二次修正，得到微机电系统的捷联惯导系统的准确捷联矩阵

所述的低精度微机电系统的捷联惯导系统粗对准的卡尔曼滤波量测方程为：

Z₁＝H₁X₁+V₁

其中，Z₁为卡尔曼滤波量测方程的量测向量；H₁为卡尔曼滤波量测方程的量测矩阵；为微机电系统中心到GPS的天线相位中心的杆臂长度误差在计算地理坐标系上的投影；

卡尔曼滤波量测方程的量测向量为：

其中分别为GPS输出的经度误差、纬度误差和高度误差在计算地理坐标系上的投影；

卡尔曼滤波量测方程的量测矩阵为：

H₁＝[I_3×30_3×3M_3×4]

其中I_3×3为三阶单位阵：

其中为微机电系统中心到GPS的天线相位中心的杆臂长度在计算地理坐标系上的投影。

2.根据权利要求1所述的一种基于低精度微机电系统的捷联惯导系统初始对准方法，其特征在于：所述的载体坐标系b到水平坐标系b′的方向余弦矩阵为：

其中是根据初始时刻东向加速度计的输出得到的，γ是根据初始时刻北向加速度计的输出得到的，

3.根据权利要求2所述的一种基于低精度微机电系统的捷联惯导系统初始对准方法，其特征在于：所述的低精度微机电系统的捷联惯导系统粗对准卡尔曼滤波的状态方程的状态向量为：

其中，捷联惯导系统输出的经度误差为δλ、纬度误差为和高度误差为δh；载体在计算地理坐标系上的东向为北向和天向的速度误差，地理坐标系上的东向为E、北向为N和天向为U，上角标t表示计算地理坐标系，经过步骤1后剩余的载体东向纵摇误差角为Φ_E、北向横摇误差角为Φ_N；Y代表的是载体的粗略航向角。

4.根据权利要求3所述的一种基于低精度微机电系统的捷联惯导系统的初始对准方法，其特征在于：所述的水平坐标系b′到计算地理坐标系t的方向余弦矩阵即：

其中，Φ_E、Φ_N、sinY和cosY分别是步骤(4)中卡尔曼滤波方法估计的输出值；

对载体姿态进行第一次修正，即测量得到载体坐标系b到计算地理坐标系t的方向余弦矩阵为：

5.根据权利要求4所述的一种基于低精度微机电系统的捷联惯导系统的初始对准方法，其特征在于：所述的捷联惯导系统精对准的卡尔曼滤波状态方程的状态向量为：

其中，捷联惯导系统输出的经度误差为δλ、纬度误差为和高度误差为δh；载体在导航坐标系上的东向为北向为和天向的速度误差；上角标n表示导航坐标系；经过步骤(4)修正后捷联惯导系统剩余的纵摇误差角为ψ_E、横摇误差角为ψ_N和航向误差角为ψ_U。

6.根据权利要求5所述的一种基于低精度微机电系统的捷联惯导系统的初始对准方法，其特征在于：所述的计算地理坐标系t到导航坐标系n的方向余弦矩阵为：

对载体姿态的第二次修正，即测量得到载体坐标系b到导航坐标系n的方向余弦矩阵为：