MINS/BDS组合导航系统设计与实现

Design and Implementation of MINS/BDS Integrated Navigation System

摘要: 为满足军用无人探测车对低成本、轻重量导航系统需求，应用MEMS 惯性导航系统(MINS)和三天线北斗接收机(BDS)，完成MINS/BDS组合导航系统设计。组合导航计算模块基于ARM + FLASH设计完成，实现多传感器通信、姿态解算、定向定位及与上位机人机交互等功能。将计算模块、电源模块、滤波模块、MIMU及北斗数据接收模块集成到嵌入式MIMU/北斗组合导航系统中，完成的系统直径78 mm，高度60 mm，质量不超过600 g。对集成后系统进行定点动姿态实验，结果表明: 该组合导航系统输出频率为200 Hz，定点位置测量精度在1 m以内，动姿态测量精度姿态角在0.1˚以内，航向角在0.5˚以内。

Abstract: IIn order to meet the requirement of low cost and light weight navigation system for Military Un-manned detection vehicle, the design of MINS/BDS integrated navigation system is completed by using the MEMS inertial navigation system (MINS) and three-antenna Beidou receiver (BDS). The integrated navigation calculation module is designed based on ARM + FLASH, which realizes the functions of multi-sensor communication, attitude calculation, orientation and positioning, and human-computer interaction with the host computer. Computing module, power module, filter module, MIMU and Beidou data receiving module are integrated into the embedded MIMU/Beidou integrated navigation system. The completed system has a diameter of 78 mm, a height of 60 mm and a quality of no more than 600 g. The fixed-point dynamic attitude experiment of the integrated system shows that the output frequency of the integrated navigation system is 200 Hz, the fixed-point position measurement accuracy is less than 1 m, the attitude angle of the dynamic at-titude measurement accuracy is less than 0.1 degree and the heading angle is less than 0.5 degree.

1. 引言

在捷联惯导系统中一般应用的陀螺器件有光纤陀螺、激光陀螺、MEMS陀螺等，近年来，MEMS技术的发展使得捷联惯导系统向着体积更小、质量更轻、成本更低的方向发展。基于MEMS技术的微惯性测量单元( MEMS inertial measurement unit，MIMU)不仅可被用在军用制导炸弹、手持制导武器上，在民用的车载定位导航和小型无人机导航等方面也有着巨大潜力。由于惯性传感器有积分误差特性，在长时间导航中误差积累较严重，需要频繁的更新外部辅助源如卫星信号、里程计、气压计等等，GNSS/MIMU组合则是目前公认的最理想、应用最为广泛的组合方式 [1] 。由于GPS在军事通信领域受制于美国，中国自主研发的全球卫星导航定位系统——北斗卫星导航系统，在我国的应用中具有突出优势，MIMU/BDS组合导航系统在我国军事领域发挥着重要作用。

2. 组合导航系统方案

MIMU/BDS组合导航系统包括4个部分，其中嵌入式电源模块为MIMU、组合导航计算模块和BDS接收机模块提供电源，MIMU和BDS接收机模块分别通过标准RS-422和RS-232与计算模块进行通信，组合导航计算模块接收到IMU和BDS的测量信息，采用基于卡尔曼滤波的观测控制器方式进行初始对准，之后采用松组合方式进行导航解算，并通过标准RS-422串口和上位机监测软件进行通信。上述4个部分集合成一体，系统框图如图1所示。

3. 算法设计

3.1. 卡尔曼滤波器原理

卡尔曼滤波(Kalman filter)是一种高效率的递归滤波器(自回归滤波器)，也是一种递推线性最小方差估计，它能够从一系列的不完全包含噪声的测量中，估计动态系统的状态 [2] 。

不考虑控制系统作用，设随机线性离散系统的状态方程和观测方程为 [3] ：

Figure 1. Block diagram of embedded integrated navigation system

图1. 嵌入式组合导航系统框图

$X_k={\Phi }_{k,k-1}{{\rm X}}_{k-1}+{\Gamma }_{k,k-1}{W}_{k-1}$ (1)

${{\rm Z}}_k={{\rm H}}_k{{\rm X}}_k+V_k$ (2)

式中： ${{\rm X}}_k$ 是系统的n维状态向量； ${{\rm Z}}_k$ 是系统的m维观测序列； $W_k$ 是p维系统过程噪声序列； $V_k$ 是m维的系统观测噪声序列； ${\Phi }_{k,k-1}$ 是系统的 $n\times n$ 维状态转移矩阵； ${\Gamma }_{k,k-1}$ 是 $n\times p$ 维系统过程噪声输入序列； ${{\rm H}}_k$ 是 $m\times n$ 维观测矩阵。

假设过程噪声 $W_k$ 和观测噪声 $V_k$ 是不相关的、白噪声序列，其统计特性如下：

$\{\begin{array}{c}{\rm E}\left[W_k\right]=\text{0}，{\rm E}\left[W_k{W}_j^T\right]=Q_k{\delta }_{kj}\\ {\rm E}\left[V_k\right]=\text{0}，{\rm E}\left[V_k{V}_j^T\right]=R_k{\delta }_{kj}\\ {\rm E}\left[W_k{V}_k^T\right]=\text{0}\end{array}$ (3)

式中： $Q_k$ 是系统过程噪声 $W_k$ 的 $p\times p$ 维对称非负定方差矩阵； $R_k$ 是系统观测噪声 $V_k$ 的 $m\times m$ 维对称正定方差阵；而 ${\delta }_{kj}$ 是Kronecker- $\delta$ 函数。

常规kalman滤波方程，可描述如下：

状态一步预测：

${\widehat{{\rm X}}}_{k,k-1}={\Phi }_{k,k-1}{\widehat{{\rm X}}}_{k-1}$ (4)

状态估计：

${\widehat{{\rm X}}}_k={\widehat{{\rm X}}}_{k,k-1}+{{\rm K}}_k\left[{\rm Z}{}_k-{{\rm H}}_k{{\rm X}}_{k-1}\right]$ (5)

一步预测误差阵：

${{\rm P}}_{k,k-1}={\Phi }_{k,k-1}{{\rm P}}_{k-1}{\Phi }_{k,k-1}^T+T_{k,k-1}{Q}_{k-1}{T}_{k,k-1}^T$ (6)

增益矩阵：

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

估计误差方阵：

${{\rm P}}_k=\left[{\rm I}-{{\rm K}}_k{{\rm H}}_k\right]{{\rm P}}_{k,k-1}{\left[{\rm I}-{{\rm K}}_k{{\rm H}}_k\right]}^T+{{\rm K}}_k{\text{R}}_k{{\rm H}}_k^T$ (8)

3.2. 扩展卡尔曼滤波原理

前面介绍的离散Kalman滤波器只适用于线性系统,但是很多实际系统，如本设计所采用的组合导航系统的航姿系统是一个非线性系统，其状态方程和观测方程中存在一定的非线性，所以本文拟采用非线性滤波器，即扩展卡尔曼滤波(Extended Kalman Filter，简称EKF) [4] 。

假设一个非线性系统如下：

$X_k=f\left(X_{k-1},k-1\right)+\Gamma \left(X_{k-1},k-1\right)W_{k-1}$ (9)

$Z_k=h\left(X_k,k\right)+V_k$ (10)

式中： $f(·)$ 是n维的向量函数，是自变量的非线性函数且对 $X_k$ 是可微的； $h(·)$ 是m维向量函数，是自变量的非线性函数； $W_{k-1}$ 和 $V_k$ 分别为p维系统噪声向量和m维观测噪声向量；假设 $W_k$ 和 $V_k$ 为零均值且互不相关的白噪声序列。

如果真实状态 $X_k$ 与标称状态 $X_k^{\ast }$ 之差(状态偏差)足够小，可以围绕滤波值 $$ 将系统状态方程(9)中的非线性函数 $f(·)$ 进行泰勒级数展开，并略去二阶及以上的项可得：

$X_k\approx f\left(\hat{X},k-1\right)+\frac{\partial f\left({\hat{X}}_{k-1},k-1\right)}{\partial {\hat{X}}_{k-1}^T}\cdot \left(X_{k-1}-{\hat{X}}_{k-1}\right)+\Gamma \left({\hat{X}}_{k-1},k-1\right)W_{k-1}$ (11)

令 $$

且 $f\left(\hat{X},k-1\right)-{\frac{\partial f\left(X_{k-1},k-1\right)}{\partial X_{k-1}^T}|}_{X_{k-1}={\hat{X}}_{k-1}}{\hat{X}}_{k-1}\text{=}{\phi }_{k-1}$

则状态方程为：

$X_k={\Phi }_{k,k-1}+\Gamma \left({\hat{X}}_{k-1},k-1\right)W_{k-1}+{\phi }_{k-1}$ (12)

针对系统观测方程(10)，将非线性函数 $h(·)$ 围绕滤波值 ${\hat{X}}_{k,k-1}$ 展成泰勒级数，省略二阶及以上的项可得：

$Z_k\approx h\left({\hat{X}}_{{}_{k|k-1}},k\right)+\frac{\partial h\left({\hat{X}}_{{}_{k|k-1}},k\right)}{\partial {\hat{X}}_{k|k-1}^T}\cdot \left(X_k-{\hat{X}}_{{}_{k|k-1}}\right)+V_k$ (13)

令 $A_{k,k-1}={\frac{\partial f\left(X_{k-1},k-1\right)}{\partial X_{k-1}^T}|}_{X_{k-1}={\hat{X}}_{k-1}}$

且 $H_k={\frac{\partial h\left(X_{k|k-1},k\right)}{\partial X_{k|k-1}^T}|}_{X_{k|k-1}={\hat{X}}_{k|k-1}}$

则观测方程为：

$Z_k=H_k{X}_k+y_k+V_k$ (14)

利用3.1节中推导的线性离散时间系统卡尔曼滤波基本方程，我们可以得到非线性离散时间系统的扩展卡尔曼滤波的一组递推公式。

状态一步预测：

${\hat{X}}_{k|k-1}=f\left[{\hat{X}}_{k-1},k-1\right]$ (15)

估计误差方差阵：

$P_{k|k-1}=A_{k|k-1}{P}_{k-1}{A}_{k|k-1}^T+B_{k-1}{Q}_{k-1}{B}_{k-1}^T$ (16)

状态估计：

${\hat{X}}_k={\hat{X}}_{k|k-1}+K_k\left[Z_k-h\left(X_{k|k-1},k\right)\right]$ (17)

卡尔曼滤波增益矩阵：

$K_k=P_{k|k-1}{H}_k^T{\left[H_k{P}_{k|k-1}{H}_k^T+R_k\right]}^{-1}$ (18)

估计误差方差阵：

$P_k=\left(I-K_k{H}_k\right)P_{k|k-1}$ (19)

3.3. MINS/BDS组合方案

本文设计的是一种MEMS-SINS/BDS的微型组合导航系统，由MEMS-SINS系统和北斗接收机构成。BDS/SINS系统组合原理框图如图2所示，采用两级组合的方式，MEMS惯性导航系统能提供姿态角、速度、位置等导航参数，而BDS系统能提供航向、姿态和位置信息，建立扩展卡尔曼滤波方程，可以完成信息融合 [5] 。第一级组合是由MEMS加速度计和陀螺仪组合完成姿态的测量，在静态下，用MEMS加速度计和陀螺仪通过积分测得姿态角，三天线北斗接收机可利用差分信号测得初始的航向，将两者信息进行组合来完成系统的初始对准，得到组合导航系统的初始姿态角和方位信息，并将这三个量作为量测值，来估计MEMS陀螺的姿态测量误差；第二级在这种组合水平上，组合卡尔曼滤波器以及扩展卡尔曼滤波器对INS误差建模 [6] ，为MEMS-SINS系统误差提供最小方差估计，利用这些误差的估计值去修正MEMS-SINS系统，减少组合导航系统的误差，将BDS输出的位置、速度与INS输出的位置、速度之差作为滤波器的量测，相互补偿、反馈最终得到最优组合解 [7] 。在位置组合的基础上，加上速度组合，可以提高速度误差、方位误差和惯性器件误差的估计精度和收敛速度，增强INS的地面导航性能 [8] 。

Figure 2. Schematic diagram of integrated navigation system

图2. 组合导航系统原理图

4. 硬件设计

组合导航系统IMU模块采用了美国ADI公司的惯性传感器ADIS16445。该传感器是一款完整的惯性系统，内置一个三轴陀螺仪和一个三轴加速度计，内部集成信号调理技术，可提供动态优化的动态性能。传感器采用SPI数字接口，传输速度快、输出稳定，为本组合导航系统的精度提供了保障。系统实物图如图3所示：

Figure 3. Picture of real product

图3. 系统实物图

组合导航系统计算模块采用ARM公司Cortex M4架构的处理器STM32F407，该芯片是基于32位总线设计，工作频率最高可达168 MHz，具有丰富的中断外设等资源，能够完成初始对准和组合导航的计算。

硬件系统的工作框图如图4所示，系统上电后，外部12M晶振经过PLL锁相环，频率倍增至168 MHz，主控ARM芯片启动SPI控制器，通过SPI接口采集IMU的三轴加速度和三轴角速度，采样频率为800 Hz。主控芯片启动UART控制器，通过串口中断方式采集北斗的信息，北斗的输出频率约为10 Hz，在采集完成后。用上述的扩展卡尔曼滤波原理将数据进行组合导航，最后主控芯片启动DMA控制器，通过串口输出导航数据，输出频率为200 Hz。

Figure 4. Hardware system block diagram

图4. 硬件系统框图

5. 软件设计

基于软件的稳定性和可扩展性考虑，软件设计采用了高内聚低耦合的设计思想，即面向对象。软件划分为了多个模块，模块与模块之间的接口统一化，易于维护和升级。具体软件设计的流程如图5所示：

ARM芯片开始工作后首先对系统进行初始化，主要工作是关闭看门狗、设置时钟、初始化IO口、初始化UART、SPI、DMA控制器，初始化SDRAM和Flash，记录当前时间等。

初始化后，首先检测传感器工作是否正常，ADIS16445这款传感器内置工作状态寄存器，主控芯片读取状态寄存器的当前值，判断传感器工作状态。所以在启动的过程中借鉴了计算机的硬件自检过程，主控制器先检测硬件能否满足运行的基本条件，如果传感器硬件出现问题，系统将中止启动。如果没有问题，则进行传感器标定以及初始对准等过程。

然后，系统将进入主程序运行，在主程序中读取SPI数据寄存器和UART缓存中的数据，采集IMU和北斗数据。对IMU数据和北斗原始数据进行自检，若数据有效，则对原始数据进行滤波，并实现对原始信号的编解码；若数据持续3s无效则上报对应的故障代码。

得到标定后的数据后，启动卡尔曼滤波器，然后惯性导航系统进行持续工作，输出载体的位置、姿态和航向信息，同时系统将计算F矩阵和G矩阵并计算状态估计误差的协方差矩阵Pk [9] ，若北斗信息没有更新，则重复上述过程。当北斗信号有更新，系统将计算H矩阵，然后计算状态估计的观测值 ${\stackrel{\rightharpoonup }{z}}_k$ ，最后更新滤波序列，得到估计的系统状态量。

最后，主控芯片启动DMA控制器，开启DMA传输。将最终的导航数据打包发送至DMA控制器，由DMA与串口直接通讯，再通过串口发送数据至上位机显示。

Figure 5. Software design flow chart

图5. 软件设计流程图

6. 试验与结果

6.1. 定点位置、速度、姿态航向实验

定点位置、速度、姿态航向实验地点为哈尔滨工程大学，BDS天线放置在61号楼阳台，每组测试时间约为6 min，反复测量多次。

连续静态测试3次，将3次测量结果的位置、速度和姿态的最大值、最小值、平均值和标准差整理到表1中。从下述实验结果可以看出：MIMU/BDS组合导航系统水平定位精度可以达到1 m以内，相比单独BDS接收机模块水平定位2.5 m有了一定提高。BDS接收机本身高度通道即有一定的误差，这项误差会被带入组合导航高度解算过程，所以，相比水平定位高度解算结果精度达到2.85 m。系统解算出北向和东向的水平速度均小于1 cm/s，标准差在1 cm/s左右，同样的向速度的平均值和标准差受BDS接收机精度的影响略差，平均值在6 cm/s。系统的俯仰角和横滚角平均值均在0.3˚以内，标准差小于0.05˚。

Table 1. Results of fixed point static test

表1. 静态定点试验结果

6.2. 定点动姿态航向实验

实验结果如表2、表3，俯仰角和横滚角的测量误差基本在0.1˚以内，航向角输出结果在0.5˚左右，这主要因为：1) 在BDS接收机天线静止情况下，系统航向角可观测性较差；2) 静态条件下测试，系统航向角存在长期漂移。以上2 点都会使系统静态航向测量精度产生影响。

Table 2. Error of fixed point dynamic posture test

表2. 定点动姿态误差

Table 3. Error of fixed point dynamic heading test

表3. 定点动航向误差

7. 结论

本文基于美国ADI的MIMU和国内某研究所研发的BDS接收机，设计完成了MIMU/BDS组合导航系统。组合导航算法的理论基础为扩展卡尔曼滤波算法，结合主控ARM内嵌的浮点运算器，有效的控制了导航的精度。后面介绍了硬件与软件的设计实现，从系统的角度描述了整个系统的实现过程。

将各硬件部分集成在一起，完成的系统高度60 mm，直径78 mm，重量不超过600 g。以200 Hz频率输出导航信息，提升了北斗的输出频率。基于设计完成的MIMU/BDS组合导航系统进行了静态实验，分为定点静姿态实验和定点动姿态实验，实验结果表明：该组合导航系统静态水平定位精度可达到1 m以内，姿态测量精度在0.1˚以内，航向测量精度也在0.5˚左右。