阵列式多MEMS-IMU协同导航系统与算法

沈 凯，左健文，左思琪，李岳伦，刘雅萱，郭文君

（北京理工大学自动化学院，北京 100081）

目前无人车辆上广泛使用的组合导航系统是以全球卫星导航系统（Global Navigation Satellite System,GNSS）和惯性导航系统（Inertial Navigation System,INS）为基础的导航定位系统。实践表明，GNSS/INS组合导航系统在开阔无遮挡的环境中表现良好，可以为无人车辆提供高精度和高可靠性的导航定位信息，但当车辆行驶到反介入/区域拒止环境下,全球卫星导航系统能力被严重削弱[1]，此时通过惯性导航系统[2,3]实现无人车辆高精度的自主导航。与全球卫星导航系统相比，惯性导航系统是一种不依赖于外部信息、也不向外部辐射能量的自主式导航系统，其精确度、可靠性等性能直接影响着极端环境下系统部署和导航任务的完成。随着微机电系统（Micro-Electro-Mechanical System, MEMS）技术的快速发展，成本低、体积小、重量轻的微惯性测量单元（MEMS-IMU）精确度、可靠性不断提升，逐步在智能车辆、无人机和机器人导航与定位等领域应用[4-6]。

在全球卫星导航系统拒止环境下，MEMS-IMU误差随时间不断累积。为了抑制MEMS-IMU误差发散，一方面从硬件上（即硬件方式）改良惯性敏感元件的生产工艺或使用新材料，但研制周期长、成本高、难度大；另一方面从系统和算法设计上（即算法方式）来补偿误差，从而提升MEMS-IMU的精度。与硬件方式相比，采用算法方式能够在很大程度上缩短研制周期，具有成本低、见效快、效果好等优点。

当前，为了实现惯导误差补偿，主要采取以下两种算法方式：一是旋转调制技术，通过采用旋转或者翻转等物理方式，周期性地改变IMU相对于导航系的方位，从而将IMU的常值误差调制成周期性变化的物理量，促使其在一个周期内积分为零或一个极小的值，达到抑制IMU误差发散的目的[7]；二是协同导航技术，通过联合利用多个IMU的测量信息，采用互补滤波或信息融合算法[8]，精确地估计IMU的各项误差，并对IMU测量误差进行补偿，从而提高IMU的导航定位性能。

为了实现不依赖卫星的高精度位置、速度和姿态等导航信息解算，本文从微惯性器件自身工作机理出发，分析影响其精度和可靠性的关键因素，综合利用MEMS-IMU、旋转调制和协同导航技术，分析对比了基于旋转调制技术的双MEMS-IMU协同导航方案，并设计了适用于自主无人系统导航的阵列式四MEMS-IMU协同导航系统，从而提升了MEMS-IMU的在线使用精度、可靠性等性能。

1 基于旋转调制技术的微惯性测量单元

1.1 单轴旋转调制型MEMS-IMU系统

旋转调制型惯导是在IMU的外部加装转动机构和测角装置，并采用一定的旋转方案而构成的系统。旋转调制型惯导按转轴数目的多少可以分为单轴系统、双轴系统和三轴系统等类型[9,10]，而每种类型又可以采用不同的转动方案，例如连续旋转、多位置转停方案等[11,12]。

为了提高MEMS-IMU长时间工作的导航精度，采用旋转调制技术将惯导误差调制成周期性变化信号，能够有效抑制MEMS-IMU误差发散。图1展示了单轴旋转调制型MEMS-IMU，其主要由三部分组成，即MEMS-IMUA、转台以及电机。MEMS-IMUA与转台固联，电机内部集成了光电编码器作为测角机构，并采取绕平台y轴正反180 °旋转调制方案。

在图1所示的单轴旋转调制型MEMS-IMU中，影响MEMS-IMU测量误差的主要因素包括：陀螺仪漂移、加速度计零偏误差、标度因数误差、安装误差以及测量噪声等。为了建立单轴旋转调制型MEMS-IMU模型，需要建立载体坐标系、平台坐标系与传感坐标系之间的转换关系。

图1 单轴旋转调制型MEMS-IMUFig.1 Single-axis rotary modulation MEMS-IMU

在单轴旋转调制型MEMS-IMU中，定义载体坐标系b的原点在载体的重心位置，坐标系与载体固联，Xb轴指向载体的右方，Yb轴指向载体的前方，Zb轴指向载体的上方，构成右手系；定义平台坐标系p与转动机构固联，其旋转时与载体系在平面上的夹角定义为旋转机构的旋转角度；定义传感器坐标系s与MEMS-IMU 固联，其三个轴的方向分别与MEMS-IMU传感器的三个敏感轴方向相重合，但与平台系之间存在安装误差。

单轴旋转调制型MEMS-IMU中平台坐标系p到传感器坐标系s的转换矩阵为：

单轴旋转调制型MEMS-IMU中载体坐标系b到平台坐标系p的转换矩阵为：

式中：φA为MEMS-IMUA的旋转角。

此时，传感器坐标系s下的角速率和加速度的量测模型为：

考虑MEMS-IMU的误差通常都是较小的值，我们可以忽略安装误差与标度因数的乘积，作式(4)所示的近似处理，陀螺漂移误差和测量误差同理。

因此，平台坐标系p下的角速率和加速度的量测模型为：

由于惯性导航系统的导航信息需要在导航坐标系下进行解算，为了方便讨论，本文假设导航坐标系与载体坐标系重合。因此，式(5)所示的量测模型可以通过坐标变换矩阵转化到导航坐标系下：

单轴旋转调制型MEMS-IMU是最简单、最易实现的系统之一，然而单轴旋转调制型系统具有一个最大的缺点：旋转轴方向上的误差无法被调制，导致其误差仍然随时间累积而发散。

2.2 正交式旋转调制型双MEMS-IMU协同系统

为了解决单轴旋转调制型MEMS-IMU系统旋转轴方向上的误差累积问题，采用绕不同轴旋转的双MEMS-IMU协同导航方法[12]，以提高导航系统误差状态的可观测度，从而在线标定和补偿各个MEMS-IMU的误差。如图2所示，MEMS-IMUA旋转调制而MEMS-IMUB静止（记为型系统），构成型正交式旋转调制双MEMS-IMU系统。

图2 型正交式旋转调制双MEMS-IMU系统Fig.2 Dual MEMS-IMU cooperative navigation system based onorthogonal rotary modulation

为构建正交式旋转调制双MEMS-IMU误差模型，将系统中旋转调制 MEMS-IMUA 和静止MEMS-IMUB的量测模型相减，得到：

为了进一步提高双MEMS-IMU姿态误差的可观测度，可以采用MEMS-IMUA和B同时旋转调制的正交式旋转调制双MEMS-IMU（记为型系统）方案。如图3所示，MEMS-IMUA的旋转调制方式为：绕其平台坐标系y轴进行正反180 °旋转，旋转的角度为Aφ；MEMS-IMUB的旋转调制方式为：绕其平台坐标系的x轴进行正反180 °旋转，旋转的角度为Bφ。

图3 型正交式旋转调制双MEMS-IMU系统Fig.3 Dual MEMS-IMU cooperative navigation system based onorthogonal rotary modulation

2 阵列式四MEMS-IMU协同导航系统及算法

2.1 阵列式四MEMS-IMU协同导航系统

针对无人车等自主无人系统的行驶特点，在利用双MEMS-IMU获取精准姿态信息的基础上，为了进一步解算出更高精度的定位定向信息，本文提出了如图4所示的阵列式四MEMS-IMU协同导航系统方案。

图4 阵列式四MEMS-IMU协同导航系统Fig.4 Array four MEMS-IMU cooperative navigation system

为了充分发挥旋转调制技术与协同导航系统技术的综合优势，对定向和定位误差进行补偿，最大限度地提升导航系统的精度和可靠性，把阵列式四MEMS-IMU协同导航系统划分为3个子系统，每个子系统包含两个MEMS-IMU，且子系统内部可以独立工作，其中子系统划分如表1所示。

表1 阵列式四MEMS-IMU协同导航系统的子系统划分Tab.1 Subsystem division of array four MEMS-IMU cooperative navigation system

2.2 联邦卡尔曼滤波算法

为了高效准确地处理阵列式四MEMS-IMU协同导航系统中各子系统的导航信息，本文采用联邦卡尔曼滤波算法（Federated Kalman Filter, FKF）融合各子系统信息，如图5所示。

图5 联邦卡尔曼滤波算法框架图Fig.5 Framework of federated Kalman filter algorithm

以东-北-天地理坐标系作为导航坐标系，在本课题中，导航坐标系与载体坐标系重合，在卡尔曼滤波算法框架下，各子滤波器的状态空间模型为：

式中：xi(t)为子系统各项误差组成的状态变量，fi(t)为状态转移矩阵，ui(t)为控制量，gi(t)为输入矩阵，wi(t)为过程噪声；zi(t)为测量输出，hi(t)为测量矩阵，vi(t)为测量噪声；i表示第i个子系统。

同理，系统的量测方程为：

进而，得到各个子滤波器的离散时间状态方程和量测方程：

此时，联邦卡尔曼滤波算法迭代过程如下。

联邦卡尔曼滤波算法步骤一：子滤波器i初始化0 0 ˆ [ ]i i X =EX T 0 0 0 0 0 ˆ ˆ[()()]i i i i i P EX X X X= - -步骤二：子滤波器i时间更新| 1 | 1 1 ˆ ˆ i i i kk kk k X X-- -=Φ T T| 1 | 1 1 | 1 1 1 1 ˆi i i i i i i kk kk k kk k k k P P-- - - - - -=Φ Φ+GQG步骤三：子滤波器i量测更新T T| 1 | 1[]i i i i i i i k kk k k kk k k K P H HP H R-1--=+| 1 | 1 ˆ ˆ ˆ()i i i i i i k kk k k k kk-X X KZ HX-= + -| 1 ˆ ˆ(I)i i i i k k k kk P KH P-= -步骤四：主滤波器信息融合1 1 1 ˆ ˆ[ ()]N g i k k i=P P --=∑ ； 1 ˆ ˆ ˆ ˆ[ () ]N g g i i k k k k i=1 X P P-X= ∑步骤五：信息共享与全局状态估计ˆ ˆ i g k k X=X；N i i=1∑β=1 1ˆ i g k i k P=β-P； 1 i g k i k Q=β-Q

3 MEMS-IMU实验平台搭建及仿真试验分析

3.1 旋转调制型MEMS-IMU实验平台

为了研究旋转调制型MEMS-IMU的工作性能，基于ADIS16405型MEMS-IMU，搭建了如图6所示的实验平台。

图6 旋转调制型MEMS-IMU实验平台图Fig.6 Experimental platform of rotary modulation MEMS-IMU

该系统主要由MEMS-IMU旋转调制平台、电源稳压模块、控制器模块和数据采集与处理模块等构成。MEMS-IMU旋转调制平台测量旋转角速率和加速度；电源稳压模块为ADIS16405、控制器等提供工作电压；控制器模块提供所需的旋转调制方案；数据采集与处理模块用于读取角速率、加速度、旋转角等信息，并进行导航解算。

通过将搭建好的旋转调制型MEMS-IMU实验平台分别设置为静止状态和旋转调制状态，然后读取加速度、旋转角速率的位姿测量信息。最后，利用高通滤波器对所得的系统量测数据滤波，从而获得微惯性测量单元ADIS16405的实际噪声。

3.2 无人车辆运动仿真设置

本实验模拟的无人车辆运动情景如图7所示。

图7 无人车辆运动模拟情景示意图Fig.7 Simulation of unmanned vehicle driving environment

无人车辆从GNSS信号良好路段驶入城市区域，由于城市高楼建筑遮挡GNSS信号，因此无人驾驶车辆主要依靠纯MEMS-IMU进行惯性导航，以GNSS导航路段与MEMS-IMU导航路段交接处为起点，设置无人车辆的姿态角运动状态为：

其中ψ为无人车辆的航向角，θ为俯仰角，γ为翻滚角，并设置无人车辆的东向和北向的运动状态为：

在上述设置的无人车辆运动状态条件下，惯性导航采用本文设计的阵列式多MEMS-IMU协同导航算法，并注入惯性器件ADIS16405采集的实际噪声数据，然后仿真测试各导航方案的定向定位性能。

3.3 姿态解算实验及对比分析

图8 定向性能及对比分析图Fig.8 Orientation performance and comparative analysis

为了更直观地对比分析不同导航系统所能达成的定向性能，在表2中列出了均方根（Root Mean Square,RMS）指标下各方案姿态角解算误差。

表2 导航系统姿态角解算误差对比表Tab.2 Comparison of attitude angle calculation error of navigation system

3.4 位置解算实验及对比分析

图9 定位性能及对比分析图Fig.9 Positioning performance and comparative analysis

表3展示了各个方案的位置解算误差。

表3 导航系统位置解算误差对比表Tab.3 Comparison of positioning calculation error of navigation system

3.5 定向定位综合性能分析

表4 各方案导航误差的RMS指标改善百分比Tab.4 Improvement percentage of navigation error of each system

4 结 论

在全球卫星导航系统失效后，惯性导航是实现自主无人系统高精度自主导航与定位的重要途径。MEMS-IMU作为惯性导航的一种低成本途径，其长时间工作的精确度和可靠性较差，为了解决这一问题，本文综合利用MEMS-IMU、旋转调制和协同导航技术，设计了阵列式四MEMS-IMU协同导航方案，并利用惯性器件ADIS16405采集的实际噪声数据进行了仿真实验。实验结果表明：本文设计的阵列式四MEMS-IMU协同导航方案能够有效抑制低成本MEMS-IMU的各项导航误差；在自主无人系统运行120 s内，姿态角解算误差不大于0.25 °；自主行驶1000 m时，东向和北向位置误差不大于3.07%d和1.04%d。因此，阵列式四MEMS-IMU协同导航方案具有一定的实用性，能够显著提升低成本MEMS-IMU的导航性能，实现不依赖卫星的高精度位置、速度和姿态等解算。未来研究工作将进一步优化多MEMS-IMU协同导航系统相关算法，开展实验验证，致力于系统的具体实现。