CN104422948A - 一种嵌入式组合导航系统及其方法 - Google Patents

Abstract

本发明公开了一种嵌入式组合导航系统及其方法。该系统包括传感器模块、数据采集模块、数据处理解算模块和外围通讯接口模块，其中传感器模块包括光纤惯导IMU和GNSS接收机，数据采集模块为FPGA芯片，数据处理解算模块为DSP芯片，传感器模块与数据处理解算模块通过数据采集模块的高速总线连接；组合导航方法为：光纤惯导IMU和GNSS接收机的量测数据通过FPGA的总线传输至DSP进行数据处理解算，建立SINS/GNSS组合导航系统数学模型进行多级容错组合导航，根据子系统状态判断，进行导航决策匹配，将得到的实时速度、位置及姿态信息通过外围通讯接口模块发送至外围设备。该系统具有高集成化、接口丰富、精度高及容错性能好的优点。

Description

一种嵌入式组合导航系统及其方法

一技术领域

本发明属于组合导航领域，特别是一种嵌入式组合导航系统及其方法。

二背景技术

随着航空技术对导航系统性能的要求越来越高，在工程应用上，组合导航系统逐步代替了纯惯性导航系统。卫星/惯性组合导航结合了惯性导航系统与全球卫星导航系统的优点，其通过后者对前者进行辅助修正，抑制了单一惯性系统的发散，实现了长航时下高精度的组合导航，已被广泛应用于军事领域，如制导武器的导航系统，在民用领域也得到了广泛应用，譬如车载导航系统等。

对于现有的传统分布式组合导航系统而言，SINS子系统、GNSS子系统及组合导航计算机之间采用设备间外围接口通讯的低级方式，组合导航系统性能易受各子系统间数据通讯品质制约，同时存在体积大、设计分散、集成度较低等问题，从而影响了导航系统的使用效能。

此外，对于以SINS为主、GNSS辅助定位的组合导航系统而言，从本质上SINS及GNSS子系统仍然是独立工作的，并且卡尔曼滤波是量测驱动的，在高动态及恶劣环境等情况下各子系统出现的极短异常均会降低组合系统的导航精度，使系统稳定性变差；同时，系统数学模型和噪声统计不准确、噪声突变等情况均可能导致常规卡尔曼滤波的收敛性变差、精度下降，甚至导致滤波发散，同样极大地影响了组合系统的导航精度。

三发明内容

本发明的目的在于提供一种容错性能好、精度高的嵌入式组合导航系统及其方法，能够在高动态及恶劣环境下进行稳定精确的导航。

实现本发明目的的技术解决方案为：一种嵌入式组合导航系统，包括传感器模块、数据采集模块、数据处理解算模块和外围通讯接口模块；其中传感器模块包括光纤惯导IMU和GNSS接收机，数据采集模块为FPGA芯片，数据处理解算模块为DSP芯片；传感器模块与数据处理解算模块通过数据采集模块的高速总线连接，光纤惯导IMU的ARM板通过FPGA外扩RS_422串口T2连接至DSP的UART_1，光纤惯导IMU的ARM板RS_422串口COM_1通过连接器引出IMU数据调试接口；GNSS接收机的板卡COM_A通过FPGA外扩RS_232串口T9连接至DSP的UART_2，GNSS接收机的板卡COM_B接T10并引出至连接器用于接收机模式设置；外围通讯接口模块包括RS_232串口和RS_422串口；光纤惯导IMU和GNSS接收机的量测数据通过FPGA的总线传输至DSP进行数据处理解算，将得到的实时速度、位置及姿态信息发送至外围通讯接口模块，外围接口模块将导航信息发送至外围设备。

一种嵌入式组合导航方法，包括以下步骤：

步骤1，组合导航系统上电后，完成系统硬件初始化工作，然后进行自检：根据传感器模块采集数据，检查组合系统是否异常；若无异常则进入步骤2，否则将发送异常信息，并继续进行自检；

步骤2，进行组合导航系统初始对准，为组合导航过程提供初始速度、位置及姿态角信息；

步骤3，捷联解算：将数据采集模块采集得到的传感器量测信息进行误差补偿后，经捷联解算得到载体实时的姿态、速度及位置信息；

步骤4，建立SINS/GNSS组合导航系统数学模型：建立SINS误差方程，利用自适应卡尔曼滤波方法对光纤惯导IMU对应的SINS子系统和GNSS接收机对应的GNSS子系统进行信息融合，估计光纤惯导IMU的各误差量并对其进行反馈校正；

步骤5，子系统状态判断：根据光纤惯导IMU的陀螺采样值和加速度计采样值判断SINS子系统的工作状态，根据GNSS接收机输出量测值判断GNSS子系统的工作状态；

步骤6，导航决策匹配：根据SINS子系统和GNSS子系统的工作状态进行导航决策匹配处理、输出导航信息，外围通讯接口模块将导航信息发送至外围设备，循环执行步骤3～6，即所述多级容错组合导航流程。

本发明与现有技术相比，其显著优点是：(1)在传感器模块中，IMU采用闭环光纤陀螺仪，寿命长、动态范围大、瞬时启动，没有闭锁问题，也不用在石英块精密加工出光路，成本更低；(2)数据处理解算模块采用高性能浮点型数字信号DSP，芯片采用模块化上电技术，运算及处理速度快、功耗低；基于DSP/FPGA浅并行架构，全系统采用集中式供电，传感器模块与数据处理模块通过高速总线通讯，数据的采集及处理将更加高效、稳定；(3)采用自适应滤波算法作为信息融合算法，同时采用了多级容错组合导航方法，使组合系统在复杂环境下具有更好的容错性、可靠性，更优的动态性能和更高的导航精度；(4)由于GNSS接收装置具备输出伪距、伪距率信息的功能，使得本系统具有更好的拓展性，实现紧耦合系统更为方便。

四附图说明

图1是本发明嵌入式组合导航系统的硬件结构示意图。

图2是本发明嵌入式组合导航系统的工作流程图。

图3是本发明嵌入式组合导航方法的原理框图。

图4是本发明嵌入式组合导航方法的组合滤波流程示意图。

五具体实施方式

下面结合附图及具体实施例对本发明作进一步详细说明。

结合图1，本发明嵌入式组合导航系统，其整体尺寸为13cm×14cm×12cm，重量≤3kg；该系统包括传感器模块、数据采集模块、数据处理解算模块和外围通讯接口模块；其中传感器模块包括光纤惯导IMU(Inertial Measurement Unit，惯性测量单元)和GNSS(Global Navigation Satellite System，全球导航卫星系统)接收机，数据采集模块为FPGA芯片，数据处理解算模块为DSP芯片；传感器模块与数据处理解算模块通过数据采集模块的高速总线连接，光纤惯导IMU的ARM板通过FPGA外扩RS_422串口T2连接至DSP的UART_1，光纤惯导IMU的ARM板RS_422串口COM_1通过连接器引出IMU数据调试接口；GNSS接收机的板卡COM_A通过FPGA外扩RS_232串口T9连接至DSP的UART_2，GNSS接收机的板卡COM_B接T10并引出至连接器用于接收机模式设置；外围通讯接口模块包括RS_232串口和RS_422串口；

光纤惯导IMU和GNSS接收机的量测数据通过FPGA的总线传输至DSP进行数据处理解算，将得到的实时速度、位置及姿态信息发送至外围通讯接口模块，外围接口模块将导航信息发送至外围设备。

所述的光纤惯导IMU采用NV300系列闭环光纤陀螺惯性测量单元。NV300系列惯性测量产品为一套完整的惯性传感器处理系统，包括了三个闭环光纤陀螺和三个加速度计、内部数据采集、信号处理、温控(温补)及供电电源装置。产品出厂前经过了对加速度计、陀螺各项参数的标定和校准，使其动态性能达到最优化，从而为导航系统提供精确稳定的惯性量测信息。该产品x，y，z三轴的角速率动态测量范围分别为±500°/s、±100°/s、±100°/s，陀螺零偏重复性≤2°/h，零偏稳定性≤3°/h；三轴的加速度动态测量范围均为±15g，加速度计零偏重复性≤0.1mg，零偏稳定性≤0.05mg；光纤惯导IMU与数据采集模块采用RS_422单工方式连接，保证了数据操作的单向隔离，同时提供额外RS_422串口输出IMU调试信息。

所述的GNSS接收机采用JAVAD GG100系列，即JAVAD GPS/GLONASS二合一卫星接收机GG100，该接收机将来可升级至北斗二代；其动态平面定位精度≤3m(CEP)，高度定位精度≤6m(CEP)，测速精度≤O.1m/s，1PPS秒脉冲精度达50ns，失锁重捕时间≤1s，数据输出频率1-20Hz可调，具备性能好、抗干扰等特点。

所述数据处理解算模块的DSP采用TI TMS320C6747系列，即TI公司的32位高性能浮点型DSP TMS320C6747，该芯片具备模块化上电功能，相比其它C6000系列芯片具有更低的功耗，更小的尺寸；其运算速度高达300MHz，处理速度≥1200MFLOPS；内核使用两级缓存结构，片内RAM为448KB；同时，C6747具备丰富的外围模块，如三路UART，增强型DMA控制器，外部接口模块EMIFA、EMIFB，两路SPI，可以满足导航处理与解算的需要。

在数据处理解算模块中，本发明采用了复合式中断方式，通过读取DSP的GPIObank4和bank5中断状态寄存器，实现GNSS接收机1PPS秒脉冲及FPGA外扩串口中断的集中处理，降低了底层驱动配置及编写的难度。

所述外围通讯接口模块主要由RS_232和RS_422接口组成，可外扩为地面监控计算机、弹载计算机等设备。通过与外部监控设备相连(如台式计算机等)，可以实现数据实时显示与存储；也可以通过该接口将实时解算的导航信息传输至其它设备，用于进一步解算(如弹载任务计算机等)。

结合图2～4，本发明嵌入式组合导航方法，具体步骤如下：

步骤1，组合导航系统上电后，完成系统硬件初始化工作，然后进行自检：根据传感器模块采集数据，检查组合系统是否异常；若无异常则进入步骤2，否则将发送异常信息，并继续进行自检。

步骤2，进行组合导航系统初始对准，为组合导航过程提供初始速度、位置及精确的姿态角信息θ₀、γ₀、ψ₀，包括光纤惯导IMU子系统初始化、GNSS接收机子系统初始化，然后进入图3所示的容错组合导航过程，其中初始四元数Q₀可由θ₀、γ₀、ψ₀确定，公式如下：

$Q_0=\left[\begin{array}{c}\cos \frac{{\mathrm{\θ}}_0}{2}\cos \frac{{\mathrm{\γ}}_0}{2}\cos \frac{{\mathrm{\ψ}}_0}{2}+\sin \frac{{\mathrm{\θ}}_0}{2}\sin \frac{{\mathrm{\γ}}_0}{2}\sin \frac{{\mathrm{\ψ}}_0}{2}\\ \sin \frac{{\mathrm{\θ}}_0}{2}\cos \frac{{\mathrm{\γ}}_0}{2}\cos \frac{{\mathrm{\ψ}}_0}{2}+\cos \frac{{\mathrm{\θ}}_0}{2}\sin \frac{{\mathrm{\γ}}_0}{2}\sin \frac{{\mathrm{\ψ}}_0}{2}\\ \cos \frac{{\mathrm{\θ}}_0}{2}\sin \frac{{\mathrm{\γ}}_0}{2}\cos \frac{{\mathrm{\ψ}}_0}{2}+\sin \frac{{\mathrm{\θ}}_0}{2}\cos \frac{{\mathrm{\γ}}_0}{2}\sin \frac{{\mathrm{\ψ}}_0}{2}\\ \cos \frac{{\mathrm{\θ}}_0}{2}\cos \frac{{\mathrm{\γ}}_0}{2}\sin \frac{{\mathrm{\ψ}}_0}{2}+\sin \frac{{\mathrm{\θ}}_0}{2}\sin \frac{{\mathrm{\γ}}_0}{2}\cos \frac{{\mathrm{\ψ}}_0}{2}\end{array}\right]---\left(1\right)$

步骤3，捷联解算：数据采集模块采集得到的传感器量测信息通过误差补偿后，经捷联解算得到载体实时的姿态、速度及位置信息，具体如下：

在所有上下标中，i表示地球惯性坐标系，n表示导航坐标系即东北天坐标系，e为地球坐标系，b为载体坐标系。

(3.1)姿态更新算法采用传统四元数算法，其方法如下：

设载体坐标系相对于导航坐标系的转动四元数为：

Q=q₀+q₁i+q₂j+q₃k       (2)

则相应的四元数微分方程为：

$\stackrel{\·}{Q}=\frac{1}{2}\mathrm{\ΩQ}---\left(3\right)$

其中：

$\mathrm{\Ω}=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& 0& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& 0& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& 0\end{array}\right],Q=\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]---\left(4\right)$

式中分别表示载体坐标系相对导航坐标系的角速率在载体坐标系x、y、z轴的分量；

由陀螺量测到的角速率可以计算得到载体系相对导航系的角速率在载体下的投影角速率公式如下：

${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-{\mathrm{\ω}}_{\mathrm{in}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b{\mathrm{\ω}}_{\mathrm{in}}^n={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)---\left(4\right)$

其中，为地球角速率，为导航坐标系相对地球坐标系的角速率在导航坐标系下的投影，为导航坐标系到载体坐标系的姿态转换矩阵，为导航坐标系相对地球惯性坐标系的角速率在载体坐标系的投影，为导航坐标系相对地球惯性坐标系的角速率在导航坐标系的投影；

设为陀螺直接测量到的角速率值在载体坐标系x、y、z轴的分量，则存在以下关系：

$\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\ω}}_{\mathrm{ibx}}^b\\ {\mathrm{\ω}}_{\mathrm{iby}}^b\\ {\mathrm{\ω}}_{\mathrm{ibz}}^b\end{array}\right]-C_n^b\left(\begin{array}{c}\end{array}\left[\begin{array}{c}0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]+\left[\begin{array}{c}-\frac{V_N}{R_M+h}\\ \frac{V_E}{R_N+h}\\ \frac{V_E}{R_N+h}\tan L\end{array}\right]\right)---\left(5\right)$

其中V_E、V_N、V_U分别为载体东北天方向的速度；L、λ、h分别为载体的纬度、经度、高度；R_M、R_N分别为地球子午面和卯酉面主曲率半径，ω_ie为地球自转角速度。

四元数微分方程的求解采用四阶龙格-库塔法，其中T为解算周期，t+T时刻的四元数q(t+T)如下：

$q(t+T)=q\left(t\right)+\frac{T}{6}(K_1+2K_2+2K_3+K_4)---\left(6\right)$

其中：

$\left\{\begin{array}{c}{K}_1=\frac{1}{2}\mathrm{\Ω}\left(t\right)q\left(t\right)\\ K_2=\frac{1}{2}\mathrm{\Ω}(t+\frac{T}{2})[q\left(t\right)+\frac{K_{1}T}{2}]\\ K_3=\frac{1}{2}\mathrm{\Ω}(t+\frac{T}{2})[q\left(t\right)+\frac{K_{2}T}{2}]\\ K_4=\frac{1}{2}\mathrm{\Ω}(t+\frac{T}{2})[q\left(t\right)+K_{3}T]\end{array}\right.---\left(7\right)$

然后，对四元数进行周期性的规范化处理，保证捷联矩阵的正交性，公式如下(8)所示：

$Q=\frac{q_0+q_{1}i+q_{2}j+q_{3}k}{\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}}---\left(8\right)$

根据姿态矩阵和Q之间的关系，得到载体坐标系到导航坐标系的姿态转换矩阵

$C_b^n=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_0{q}_3+q_1{q}_2)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(9\right)$

导航所需的姿态角信息即俯仰角θ、横滚角γ、航线角ψ，从姿态矩阵得到，如下式所示：

$\left\{\begin{array}{c}\mathrm{\θ}=\mathrm{si}{n}^{-1}{C}_{32}\\ \mathrm{\γ}={\tan }^{-1}(-C_{31}/C_{33})\\ \mathrm{\ψ}={\tan }^{-1}(-C_{12}/C_{22})\end{array}\right.$

其中C_ij为的第i行第j列元素，i，j=1，2，3，最终通过姿态角象限判断准则，得到当前时刻更新后的姿态角。

(3.2)载体速度计算，其方法如下：

由比力方程：

${\stackrel{\·}{V}}^n=f^n-(2{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×V^n+g^n---\left(10\right)$

式中：是在导航坐标系下载体速度矢量的变化率，fⁿ为加速度计的测量值f^b转换到导航坐标系下的加速度值，为哥氏加速度，加速度是载体保持在地球表面运动(圆周运动)引起的对地向心加速度；gⁿ为重力加速度。

把式(10)表示为矩阵形式可得：

$\left[\begin{array}{c}{\stackrel{\·}{V}}_E\\ {\stackrel{\·}{V}}_N\\ {\stackrel{\·}{V}}_U\end{array}\right]=\left[\begin{array}{c}{f}_E\\ f_N\\ f_U\end{array}\right]-\left[\begin{array}{ccc}0& -(2{\mathrm{\ω}}_{\mathrm{iez}}^n+{\mathrm{\ω}}_{\mathrm{enz}}^n)& ({2\mathrm{\ω}}_{\mathrm{iey}}^n+{\mathrm{\ω}}_{\mathrm{eny}}^n)\\ (2{\mathrm{\ω}}_{\mathrm{iez}}^n+{\mathrm{\ω}}_{\mathrm{enz}}^n)& 0& -(2{\mathrm{\ω}}_{\mathrm{iex}}^n+{\mathrm{\ω}}_{\mathrm{enx}}^n)\\ -(2{\mathrm{\ω}}_{\mathrm{iey}}^n+{\mathrm{\ω}}_{\mathrm{eny}}^n)& (2{\mathrm{\ω}}_{\mathrm{iex}}^n+{\mathrm{\ω}}_{\mathrm{enz}}^n)& 0\end{array}\right]\left[\begin{array}{c}{V}_E\\ V_N\\ V_U\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ -g^n\end{array}\right]---\left(11\right)$

式中分别为地球角速率在导航坐标系x、y、z轴的分量，f_E、f_N、f_U是加速度计测量到的比力通过坐标转换后得到的导航系下东、北、天三个方向上的比力分量，即：

$\left[\begin{array}{c}{f}_E\\ f_N\\ f_U\end{array}\right]=C_b^n\left[\begin{array}{c}{f}_x^b\\ f_y^b\\ f_z^b\end{array}\right]---\left(12\right)$

其中，分别为加速度计的测量值在载体系x、y、z轴的分量；

将式(10)离散化可得速度递推求解方程：

$V^n(k+1)=V^n\left(k\right)+T(f^n\left(k\right)-(2{\mathrm{\ω}}_{\mathrm{ie}}^n\left(k\right)+{\mathrm{\ω}}_{\mathrm{en}}^n\left(k\right))\×V^n\left(k\right)+g^n)---\left(13\right)$

求解式(10)所代表的方程，即可得到载体在导航坐标系下的三维速度信息。

(3.3)载体位置计算。其方法如下：

由式

$\stackrel{\·}{L}=\frac{V_N}{R_M+h},\stackrel{\·}{\mathrm{\λ}}=\frac{V_E\sec L}{R_N+h},\stackrel{\·}{h}=V_U$

得：

$\left\{\begin{array}{c}L\left(k\right)=L(k-1)+\frac{V_N(k-1)}{R_M(k-1)+h(k-1)}T\\ \mathrm{\λ}\left(k\right)=\mathrm{\λ}(k-1)+\frac{V_E(k-1)}{(R_N(k-1)+h(k-1))\cos L(k-1)}T\\ h\left(k\right)=h(k-1)+V_U(k-1)T\end{array}\right.---\left(14\right)$

其中，T为解算周期，在计算得到最新的纬度L、经度λ数值之后，根据纬度可以更新KT时刻的子午圈曲率半径R_M(k)、卯酉圈的曲率半径R_N(k)。

步骤4，建立SINS/GNSS组合导航系统数学模型：建立SINS误差方程，利用自适应卡尔曼滤波方法对光纤惯导IMU对应的SINS子系统和GNSS接收机对应的GNSS子系统进行信息融合，估计SINS的各误差量并对其进行反馈校正；

(4.1)建立SINS误差方程，具体如下：

(a)系统状态方程：

$\stackrel{\·}{X}=\mathrm{FX}+\mathrm{GW}---\left(15\right)$

其中X为系统状态矢量，表示系统状态矢量的导数，F为系统状态转移矩阵，G为系统噪声驱动矩阵，W为系统噪声矢量，具体如下：

$X={\left[\begin{array}{ccccccccccccccc}{\mathrm{\ψ}}_E& {\mathrm{\ψ}}_N& {\mathrm{\ψ}}_U& {\mathrm{\δV}}_E& {\mathrm{\δV}}_N& \mathrm{\δ}{V}_U& \mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z& {\▿}_x& {\▿}_y& {\▿}_z\end{array}\right]}^T$

$F_{15\×15}=\left[\begin{array}{cc}{F}_N& F_S\\ 0_{6\×9}& F_M\end{array}\right],F_S=\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{3\×3}& 0_{3\×3}\end{array}\right],F_M=\left[0_{6\×6}\right],G=\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{9\×3}& 0_{9\×3}\end{array}\right]$

其中ψ_E、ψ_N、ψ_U分别为东、北、天方向的平台失准角误差；δV_E、δV_N、δV_U分别为东、北、天方向的速度误差；δL、δλ、δh分别为纬度、经度及高度误差；ε_x、ε_y、ε_z分别为陀螺常值漂移在x、y、z轴上的分量；分别为加速度计常值偏置在x、y、z轴上的分量；F_N为对应9个基本导航参数的系统阵；为姿态矩阵； 分别为陀螺噪声在x、y、z轴上的分量；分别为加表噪声在x、y、z轴上的分量；

(b)系统量测方程：

Z=HX+V                    (16)

其中，Z为观测矢量，H为观测矩阵，V为观测噪声矩阵，具体如下：

Z=[δL δλ δh δV_E δV_N δV_U]^T，H=[H_P H_V]^T

H_P=[0_3×6 diag[R_M R_NcosL 1]0_3×6]

H_V=[0_3×3 I_3×3 0_3×9]

$V={\left[\begin{array}{cccccc}{V}_L& V_{\mathrm{\λ}}& V_h& V_{V_E}& V_{V_N}& V_{V_U}\end{array}\right]}^T$

其中R_M为椭球子午圈上各点的曲率半径，R_N为卯酉圈(它所在的平面与子午面垂直)上各点的曲率半径，L、λ、h分别表示纬度、经度及高度，V_E、V_N、V_U分别为东、北、天方向的速度，V_L、V_λ、V_h分别表示GNSS的纬度、经度、高度的观测误差，分别表示GNSS的东、北、天方向上的速度观测误差。

(4.2)利用自适应卡尔曼滤波方法对SINS子系统和GNSS子系统进行信息融合，估计SINS各误差量并对其进行反馈校正，具体为：

如图4所示，根据系统状态方程和系统量测方程，采用基于单步量测新息矢量r_k的自适应卡尔曼滤波信息融合算法，其算法公式编排具体如下：

①状态一步预测方程：

${\hat{X}}_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}---\left(17\right)$

其中，为k时刻系统状态一步预测值，为k-1时刻系统状态估计值，Ф_k，k-1为k-1时刻到k时刻的系统状态转移矩阵；

②一步预测均方误差方程：

$P_{k,k-1}=S_k{\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(18\right)$

$S_k=\frac{\mathrm{Trace}[r_k{r}_k^T-H_k{Q}_{k-1}{H}_k^T-R_k]}{\mathrm{Trace}\left[H_k{\mathrm{\Φ}}_{k,k-1}{P}_{k,k-1}{\mathrm{\Φ}}_{k,k-1}^T{H}_k^T\right]}---\left(19\right)$

其中，P_k，k-1为k-1时刻到k时刻的系统状态协方差阵，P_k-1为k-1时刻的系统状态协方差阵，S_k为自适应因子，Q_k-1为k-1时刻系统噪声矩阵、Γ_k-1为k-1时刻系统噪声驱动矩阵；

③卡尔曼滤波增益方程：

$K_k=P_{k,k-1}{H}_k^T{(H_k{P}_{k,k-1}{H}_k^T+R_k)}^{-1}---\left(20\right)$

其中，K_k为k时刻系统增益矩阵，H_k为k时刻系统量测矩阵，R_k为k时刻系统量测噪声矩阵；

④状态估计值更新方程：

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k{r}_k---\left(21\right)$

$r_k=Z_k-H_k{\hat{X}}_{k,k-1}---\left(22\right)$

为k时刻系统状态估计值，r_k为新息矢量，K_k为k时刻系统增益矩阵，Z_k为k时刻量测向量；

⑤估计均方误差方程：

$P_k=(I-K_k{H}_k)P_{k,k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T---\left(23\right)$

P_k为k时刻的系统状态协方差阵，I是单位阵；

基于单步量测新息r_k的滤波发散判据为：

$r_k^T{r}_k>{\mathrm{\λ}}^{\′}\mathrm{Trace}[H_k{P}_{k,k-1}{H}_k^T+R_k]---\left(24\right)$

其中λ′为常值系数，取1～5，若该式成立，则滤波发散；反之不发散。在滤波发散情况下，组合导航系统的各误差增大，降低了滤波精度。所以根据滤波发散判据，如果判断滤波发散，则通过求出自适应权重因子S_k以扩大P_k，k-1的作用，增大滤波增益，从而增强新量测值的修正作用，进而抑制滤波发散；如果判断滤波不发散，即S_k=1，则可以直接使用常规的卡尔曼滤波算法进行修正。

最后，通过状态误差估计量对SINS／GNSS组合导航系统中的速度误差、位置误差、姿态误差以及惯导内部的加速度零偏和陀螺漂移进行反馈校正。

步骤5，子系统状态判断：根据SINS的陀螺采样值和加速度计采样值判断SINS子系统的工作状态，根据GNSS输出量测值判断GNSS子系统的工作状态；

子系统状态判断的具体步骤为：

(5.1)根据SINS的陀螺仪采样值和加速度计采样值判断SINS子系统的工作状态，设A_(axis)max为加速度阈值、ω_(axis)max为角速度阈值(通常根据惯性测量器件或载体的动态极限设定)，判断加速度计采样值A_axis和陀螺采样值ω_axis是否满足以下条件：

|A_axis|<A_(axis)max

|ω_axis|<ω_(axis)max

当满足上式时，则SINS子系统的工作状态正常，否则SINS子系统的工作状态异常；

(5.2)根据GNSS输出量测值判断GNSS子系统的工作状态，先后进行外层判断和内层判断：

(a)外层判断为收星条件判断：设定n为最小收星数、dop为精度因子门限，判断收星数N_sats和精度因子xDOP是否满足以下条件：

N_sats≥n

xDOP≤dop

当满足上式时，继续内层判断，否则认为GNSS子系统的工作状态异常；

(b)内层判断主要依靠SINS数据短时间内具有较高的稳定性和精度这一特性，内层判断对GNSS量测粗大误差进行判断，分为位置粗大误差判断、速度粗大误差判断以及GNSS误差带判断；设定下标GNSS、SINS分别代表GNSS接收机输出值和惯导输出值，下标1、0分别代表当前时刻值和前一时刻值，具体判断方法如下：

①位置粗大误差判断：

位置粗大误差d_err判断方法具体公式如下，其中，L、λ、h分别代表载体在地理系下的纬度、经度及高度，R_m为椭球子午圈上各点的曲率半径，R_n为卯酉圈(它所在的平面与子午面垂直)上各点的曲率半径：

d_err={[R_m((L_GNSS1-L_GNSS0)-(L_SINS1-L_SINS0))]²

+[R_ncos(L_SINS1)·((λ_GNSS1-λ_GNSS0)-(λ_SINS1-λ_SINS0))]²    (25)

+[(h_GNSS1-h_GNSS0)-(h_SINS1-h_SINS0)]²}^1/2

当d_err大于所设定的位置误差阀值(如100m)时，则GNSS定位信息受到干扰，位置数据异常；否则，GNSS位置数据正常；

②速度粗大误差判断：

速度粗大误差V_err判断方法具体公式如下，其中V表示速度数据，下标E、N、U代表东、北、天方向：

$\begin{array}{c}{V}_{\mathrm{err}}=\{{[(V_{\mathrm{GNSS}1}^N-V_{\mathrm{GNSS}0}^N)-(V_{\mathrm{SINS}1}^N-V_{\mathrm{SINS}0}^N)]}^2+{[(V_{\mathrm{GNSS}1}^E-V_{\mathrm{GNSS}0}^E)-(V_{\mathrm{SINS}1}^E-V_{\mathrm{SINS}0}^E)]}^2\\ +{{[(V_{\mathrm{GNSS}1}^U-V_{\mathrm{GNSS}0}^U)-(V_{\mathrm{SINS}1}^U-V_{\mathrm{SINS}0}^U)]}^2\}}^{1/2}\end{array}---\left(26\right)$

当V_err大于设定的速度误差阈值(如10m／s)时，则认为GNSS测速信息受到干扰，速度数据异常；否则，GNSS速度数据正常；

③GNSS误差带判断：

采用下列公式判断当前时刻GNSS输出的位置数据是否处于SINS误差范围内，具体公式如下：

d_perr={[R_m(L_GNSS1-L_SINS1)]²+[R_ncos(L_SINS1)·(λ_GNSS1-λ_SINS1)]²+(h_GNSS1-h_SINS1)²}^1/2

                                        (27)

设e_GNSS为GNSS位置误差阈值、e_SINS为SINS位置误差阈值(如均设定为50m)；若d_perr≤e_GNSS+e_SINS成立，GNSS数据处于SINS误差范围内，GNSS数据有效；否则，GNSS数据无效；

根据上述判断方法，当位置、速度数据均正常并且GNSS数据处于SINS误差范围内时，GNSS子系统正常；否则，GNSS子系统工作异常，数据存在突变。

步骤6，导航决策匹配：根据SINS子系统和GNSS子系统的工作状态进行导航决策匹配处理、输出导航信息，根据GNSS、IMU工作状态的不同匹配相应导航策略，以增强系统适应性和鲁棒性，并循环执行步骤3～6，即所述多级容错组合导航流程。具体如下：

(6.1)当SINS子系统、GNSS子系统工作状态均正常时，采用SINS／GNSS组合导航：将SINS和GNSS进行位置速度误差组合得到误差方程，经卡尔曼滤波估计出载体的位置、速度和姿态误差，对SINS的位置、速度、横滚角和俯仰角进行反馈校正，其中滤波算法采用自适应滤波，滤波周期为1s；

(6.2)当SINS子系统工作状态异常、GNSS子系统工作状态正常时，放弃当前时刻SINS中陀螺仪和加速度计的量测值，用前一时刻的量测值替代：

ω(k)_axis=ω(k-1)_axis

A(k)_axis=A(k-1)_axis

其中ω(k)_axis为k时刻的角速度，ω(k-1)_axis为k-1时刻的角速度，A(k)_axis为k时刻的加速度，A(k-1)_axis为k-1时刻的加速度；

(6.3)当SINS子系统工作状态正常、GNSS子系统工作状态异常时，采用丢星算法处理；从丢星状态恢复收星时，利用状态误差转移矩阵F估计导航误差并对导航输出进行修正。

(a)在捷联解算后，采用照常进行状态及均方误差的时间更新，省略量测更新的方法处理短时间丢星情况，方法如下：

${\hat{X}}_{k,k-1}={\mathrm{\&Phi;}}_{k,k-1}{\hat{X}}_{k-1}---\left(28\right)$

$P_{k,k-1}={\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T+{\mathrm{\&Gamma;}}_{k-1}{Q}_{k-1}{\mathrm{\&Gamma;}}_{k-1}^T---\left(29\right)$

P_k=P_k，k-1    (30)

其中X_k为k时刻的n维状态向量，X_k-1为k-1时刻的n维状态向量，为状态X_k的卡尔曼滤波估计值，为状态X_k-1的卡尔曼滤波估计值，为利用计算得到的对X_k的一步预测；Φ_k，k-1为k-1到k时刻的系统一步转移矩阵(n×n阶)，为Φ_k，k-1的转置矩阵；P_k为估值的均方误差阵，P_k-1为估值的均方误差阵，P_k，k-1为估值的均方误差阵；Γ_k-1为系统噪声矩阵(n×r阶)，它表征由k-1到k时刻的各个系统噪声分别影响k时刻各个状态的程度，为Γ_k-1的转置矩阵；Q_k-1为k-1时刻的系统噪声的方差矩阵。

(b)但是当量测缺失时间过长或者由此导致的状态估计均方误差过大时，应当否定滤波结果的有效性，甚至重置卡尔曼滤波器。

当GNSS从长时间异常恢复时，用GNSS给出的位置、速度作为当前时刻组合导航的位置、速度值；同时当采用的惯性器件稳定性较好时，可采用误差状态转移阵估计出导航误差并进行修正。首先，在丢星期间计算与滤波周期相应的Φ_k，k-1并连乘，得到丢星前一时刻t₀到当前时刻t₁状态转移矩阵下式所示：

${\mathrm{\&Phi;}}_{t_1,t_0}={\mathrm{\&Phi;}}_{t_1,t_1-T^{\&prime;}}...{\mathrm{\&Phi;}}_{t_0+2T^{\&prime;},t_0+T^{\&prime;}}{\mathrm{\&Phi;}}_{t_0+T^{\&prime;},t_0}---\left(31\right)$

其中，T′为滤波周期；然后由此及t₀时刻的误差状态量推算出当前t₁时刻的误差状态量，并修正导航输出，如下式所示：

$\left[\begin{array}{c}{\mathrm{\&Psi;}}_{t_1}\\ {\mathrm{\&delta;V}}_{t_1}\\ {\mathrm{\&delta;P}}_{t_1}\\ \mathrm{\&epsiv;}\\ \&dtri;\end{array}\right]={\mathrm{\&Phi;}}_{t_1,t_0}\left[\begin{array}{c}{\mathrm{\&Psi;}}_{t_0}\\ {\mathrm{\&delta;V}}_{t_0}\\ {\mathrm{\&delta;P}}_{t_0}\\ \mathrm{\&epsiv;}\\ \&dtri;\end{array}\right]---\left(32\right)$

其中为t₀到t₁时刻的系统一步转移矩阵(n×阶)，ε、分别为t₀时刻的平台失准角误差、速度误差、位置误差、陀螺漂移、加速度计零偏， ε、分别为t₁时刻的平台失准角误差、速度误差、位置误差、陀螺漂移、加速度计零偏。

(6.4)当SINS子系统、GNSS子系统工作状态均异常时，采用机动目标的轨迹预测方法对载体运动状态进行估计，对载体当前时刻运动状态进行估计，削弱系统无法正常工作带来的不利影响。

综上所述，本发明嵌入式组合导航系统中IMU采用闭环光纤陀螺仪，基于DSP／FPGA浅并行架构，全系统采用集中式供电，传感器模块与数据处理模块通过高速总线通讯，数据的采集及处理将更加高效、稳定；采用自适应滤波算法作为信息融合算法，同时采用了多级容错组合导航方法，使组合系统在复杂环境下具有更好的容错性、可靠性，更优的动态性能和更高的导航精度。

Claims (6)

1.一种嵌入式组合导航系统，其特征在于，包括传感器模块、数据采集模块、数据处理解算模块和外围通讯接口模块；其中传感器模块包括光纤惯导IMU和GNSS接收机，数据采集模块为FPGA芯片，数据处理解算模块为DSP芯片；传感器模块与数据处理解算模块通过数据采集模块的高速总线连接，光纤惯导IMU的ARM板通过FPGA外扩RS_422串口T2连接至DSP的UART_1，光纤惯导IMU的ARM板RS_422串口COM_1通过连接器引出IMU数据调试接口；GNSS接收机的板卡COM_A通过FPGA外扩RS_232串口T9连接至DSP的UART_2，GNSS接收机的板卡COM_B接T10并引出至连接器用于接收机模式设置；外围通讯接口模块包括RS_232串口和RS_422串口；

光纤惯导IMU和GNSS接收机的量测数据通过FPGA的总线传输至DSP进行数据处理解算，将得到的实时速度、位置及姿态信息发送至外围通讯接口模块，外围接口模块将导航信息发送至外围设备。

2.根据权利要求1所述的嵌入式组合导航系统，其特征在于，所述的光纤惯导IMU采用NV300系列闭环光纤陀螺惯性测量单元。

3.根据权利要求1所述的嵌入式组合导航系统，其特征在于，所述的GNSS接收机采用JAVAD GPS/GLONASS二合一卫星接收机GG100。

4.根据权利要求1所述的嵌入式组合导航系统，其特征在于，所述数据处理解算模块的DSP采用TI公司的32位浮点型DSP TMS320C6747。

5.一种嵌入式组合导航方法，其特征在于，包括以下步骤：

步骤1，组合导航系统上电后，完成系统硬件初始化工作，然后进行自检：根据传感器模块采集数据，检查组合系统是否异常；若无异常则进入步骤2，否则将发送异常信息，并继续进行自检；

步骤2，进行组合导航系统初始对准，为组合导航过程提供初始速度、位置及姿态角信息；

步骤3，捷联解算：将数据采集模块采集得到的传感器量测信息进行误差补偿后，经捷联解算得到载体实时的姿态、速度及位置信息；

步骤4，建立SINS/GNSS组合导航系统数学模型：建立SINS误差方程，利用自适应卡尔曼滤波方法对光纤惯导IMU对应的SINS子系统和GNSS接收机对应的GNSS子系统进行信息融合，估计光纤惯导IMU的各误差量并对其进行反馈校正；

步骤5，子系统状态判断：根据光纤惯导IMU的陀螺采样值和加速度计采样值判断SINS子系统的工作状态，根据GNSS接收机输出量测值判断GNSS子系统的工作状态；

步骤6，导航决策匹配：根据SINS子系统和GNSS子系统的工作状态进行导航决策匹配处理、输出导航信息，外围通讯接口模块将导航信息发送至外围设备，循环执行步骤3～6，即所述多级容错组合导航流程。

6.根据权利要求5所述的嵌入式组合导航方法，其特征在于，步骤3所述的捷联解算的具体步骤如下：

(3.1)姿态更新算法采用传统四元数算法，其方法如下：

设载体坐标系相对于导航坐标系的转动四元数为：

Q＝q₀+q₁i+q₂j+q₃k

则相应的四元数微分方程为：

$\stackrel{\&CenterDot;}{Q}=\frac{1}{2}\mathrm{\&Omega;Q}$

其中：

$\mathrm{\&Omega;}=\left[\begin{array}{cccc}0& -{\mathrm{\&omega;}}_{\mathrm{nb}}^{\mathrm{bx}}& -{\mathrm{\&omega;}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\&omega;}}_{\mathrm{nb}}^{\mathrm{bz}}\\ {\mathrm{\&omega;}}_{\mathrm{nb}}^{\mathrm{bx}}& 0& {\mathrm{\&omega;}}_{\mathrm{nb}}^{\mathrm{bz}}& -{\mathrm{\&omega;}}_{\mathrm{nb}}^{\mathrm{by}}\\ {\mathrm{\&omega;}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\&omega;}}_{\mathrm{nb}}^{\mathrm{bz}}& 0& {\mathrm{\&omega;}}_{\mathrm{nb}}^{\mathrm{bx}}\\ {\mathrm{\&omega;}}_{\mathrm{nb}}^{\mathrm{bz}}& {\mathrm{\&omega;}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\&omega;}}_{\mathrm{nb}}^{\mathrm{bx}}& 0\end{array}\right],Q=\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]$

式中分别表示载体坐标系相对导航坐标系的角速率在载体坐标系x、y、z轴的分量；

由陀螺量测到的角速率计算得到载体系相对导航系的角速率在载体下的投影角速率公式如下：

${\mathrm{\&omega;}}_{\mathrm{nb}}^b={\mathrm{\&omega;}}_{\mathrm{ib}}^b-{\mathrm{\&omega;}}_{\mathrm{in}}^b={\mathrm{\&omega;}}_{\mathrm{ib}}^b-C_n^b{\mathrm{\&omega;}}_{\mathrm{in}}^n={\mathrm{\&omega;}}_{\mathrm{ib}}^b-C_n^b({\mathrm{\&omega;}}_{\mathrm{ie}}^n+{\mathrm{\&omega;}}_{\mathrm{en}}^n)$

其中，为地球角速率，为导航坐标系相对地球坐标系的角速率在导航坐标系下的投影，为导航坐标系到载体坐标系的姿态转换矩阵，为导航坐标系相对地球惯性坐标系的角速率在载体坐标系的投影，为导航坐标系相对地球惯性坐标系的角速率在导航坐标系的投影；

设为陀螺直接测量到的角速率值在载体坐标系x、y、z轴的分量，则存在以下关系：

$\left[\begin{array}{c}{\mathrm{\&omega;}}_{\mathrm{nb}}^{\mathrm{bx}}\\ {\mathrm{\&omega;}}_{\mathrm{nb}}^{\mathrm{by}}\\ {\mathrm{\&omega;}}_{\mathrm{nb}}^{\mathrm{bz}}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\&omega;}}_{\mathrm{ibx}}^b\\ {\mathrm{\&omega;}}_{\mathrm{iby}}^b\\ {\mathrm{\&omega;}}_{\mathrm{ibz}}^b\end{array}\right]-C_n^b(\left[\begin{array}{c}0\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\sin L\end{array}\right]+\left[\begin{array}{c}-\frac{V_N}{R_M+h}\\ \frac{V_E}{R_N+h}\\ \frac{V_E}{R_N+h}\tan L\end{array}\right])$

其中V_E、V_N、V_U分别为载体东北天方向的速度；L、λ、h分别为载体的纬度、经度、高度；R_M、R_N分别为地球子午面和卯酉面主曲率半径，ω_ie为地球自转角速度；

四元数微分方程的求解采用四阶龙格-库塔法，其中T为解算周期，t+T时刻的四元数q(t+T)如下：

$q(t+T)=q\left(t\right)+\frac{T}{6}(K_1+{2K}_2+{2K}_3+K_4)$

其中：

$\left\{\begin{array}{c}{K}_1=\frac{1}{2}\mathrm{\&Omega;}\left(t\right)q\left(t\right)\\ K_2=\frac{1}{2}\mathrm{\&Omega;}(t+\frac{T}{2})[q\left(t\right)+\frac{K_{1}T}{2}]\\ K_3=\frac{1}{2}\mathrm{\&Omega;}(t+\frac{T}{2})[q\left(t\right)+\frac{K_{2}T}{2}]\\ K_4=\frac{1}{2}\mathrm{\&Omega;}(t+\frac{T}{2})[q\left(t\right)+K_{3}T]\end{array}\right.$

然后，对四元数进行周期性的规范化处理，公式如下所示：

$Q=\frac{q_0+q_{1}i+q_{2}j+q_{3}k}{\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}}$

根据姿态矩阵和Q之间的关系，得到载体坐标系到导航坐标系的姿态转换矩阵

$C_b^n=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_0{q}_3+q_1{q}_2)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$

导航所需的姿态角信息即俯仰角θ、横滚角γ、航线角ψ从姿态矩阵得到，如下式所示：

$\left\{\begin{array}{c}\mathrm{\&theta;}={\sin }^{-1}{C}_{32}\\ \mathrm{\&gamma;}={\tan }^{-1}(-C_{31}/C_{33})\\ \mathrm{\&psi;}={\tan }^{-1}(-C_{12}/C_{22})\end{array}\right.$

其中C_ij为的第i行第j列元素，i，j＝1，2，3，最终通过姿态角象限判断准则，得到当前时刻更新后的姿态角；

(3.2)载体速度计算，其方法如下：

由比力方程：

${\stackrel{\&CenterDot;}{V}}^n=f^n-({2\mathrm{\&omega;}}_{\mathrm{ie}}^n+{\mathrm{\&omega;}}_{\mathrm{en}}^n)\&times;V^n+g^n$

式中：是在导航坐标系下载体速度矢量的变化率，fⁿ为加速度计的测量值f^b转换到导航坐标系下的加速度值，为哥氏加速度，加速度是载体保持在地球表面运动引起的对地向心加速度；gⁿ为重力加速度；

上式表示为矩阵形式得：

$\left[\begin{array}{c}{\stackrel{\&CenterDot;}{V}}_E\\ {\stackrel{\&CenterDot;}{V}}_N\\ {\stackrel{\&CenterDot;}{V}}_U\end{array}\right]=\left[\begin{array}{c}{f}_E\\ f_N\\ f_U\end{array}\right]-\left[\begin{array}{ccc}0& -({2\mathrm{\&omega;}}_{\mathrm{iez}}^n+{\mathrm{\&omega;}}_{\mathrm{enz}}^n)& ({2\mathrm{\&omega;}}_{\mathrm{iey}}^n+{\mathrm{\&omega;}}_{\mathrm{eny}}^n)\\ ({2\mathrm{\&omega;}}_{\mathrm{iez}}^n+{\mathrm{\&omega;}}_{\mathrm{enz}}^n)& 0& -({2\mathrm{\&omega;}}_{\mathrm{iex}}^n+{\mathrm{\&omega;}}_{\mathrm{enx}}^n)\\ -({2\mathrm{\&omega;}}_{\mathrm{iey}}^n+{\mathrm{\&omega;}}_{\mathrm{eny}}^n)& ({2\mathrm{\&omega;}}_{\mathrm{iex}}^n+{\mathrm{\&omega;}}_{\mathrm{enz}}^n)& 0\end{array}\right]\left[\begin{array}{c}{V}_E\\ V_N\\ V_U\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ {-g}^n\end{array}\right]$

式中分别为地球角速率在导航坐标系x、y、z轴的分量，f_E、f_N、f_U是加速度计测量到的比力通过坐标转换后得到的导航系下东、北、天三个方向上的比力分量，即：

$\left[\begin{array}{c}{f}_E\\ f_N\\ f_U\end{array}\right]=C_b^n\left[\begin{array}{c}{f}_x^b\\ f_y^b\\ f_z^b\end{array}\right]$

其中，分别为加速度计的测量值在载体系x、y、z轴的分量；

将比力方程离散化得速度递推求解方程：

$V^n(k+1)=V^n\left(k\right)+T(f^n\left(k\right)-({2\mathrm{\&omega;}}_{\mathrm{ie}}^n\left(k\right)+{\mathrm{\&omega;}}_{\mathrm{en}}^n\left(k\right))\&times;V^n\left(k\right)+g^n)$

得到载体在导航坐标系下的三维速度信息；

(3.3)载体位置计算，其方法如下：

由式 $\stackrel{\&CenterDot;}{L}=\frac{V_N}{R_M+h},\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}=\frac{V_E\sec L}{R_N+h},\stackrel{\&CenterDot;}{h}=V_U$

得：

$\left\{\begin{array}{c}L\left(k\right)=L(k-1)+\frac{V_N(k-1)}{R_M(k-1)+h(k-1)}T\\ \mathrm{\&lambda;}\left(k\right)=\mathrm{\&lambda;}(k-1)+\frac{V_E(k-1)}{(R_N(k-1)+h(k-1))\cos L(k-1)}T\\ h\left(k\right)=h(k-1)+V_U(k-1)T\end{array}\right.$

其中，T为解算周期，在计算得到最新的纬度L、经度λ数值之后，根据纬度更新KT时刻的子午圈曲率半径R_M(k)、卯酉圈的曲率半径R_N(k)。