基于ARM的移动机器人组合导航系统设计与实现*

曹 超，马 磊，赵 舵，李 应

(西南交通大学电气学院，四川成都610031)

0 引言

对自主性、可靠性、精度等要求较高的室外自主移动机器人运用场合，常使用多传感器组合导航系统。惯性导航系统(INS)是一种能够提供全面的导航信息，完全自主的导航系统，但其误差随时间不断累计。全球卫星定位系统(GPS)定位范围广、精度高，误差不随时间累计，其不足之处在于自主性和可靠性差，信号易受外界遮挡和干扰，接收机数据更新率低。INS/GPS组合导航系统利用INS和GPS互补的特点，继承了2个独立系统的优点，被认为是导航领域最理想的组合方式，成为当前研究热点。随着组合水平的提高，INS与GPS之间相互辅助，传递使用信息的加强，组合系统的总体性能远远高于独立系统［1，2］。

1 硬件系统

为了实现室外移动机器的导航定位需求，需确定其精确的运动状态信息，通常包括三轴加速度信息、速度信息、位置信息、姿态信息。一般的，GPS可以提供载体的运行速度和位置(经纬度和高度)信息，惯性测量单元(IMU)可以得到三轴角速度和加速度信息，通过积分后可得到姿态与速度、位置信息。室外移动机器人组合导航系统，除了考虑导航精度外，还考虑导航系统成本和体积重量，故INS一般采用成本低、体积小、质量轻的微电子机械系统—惯性测量单元(mirco-electro-mechanical systems-inertial measurement unit，MEMS-IMU)构成的捷联惯性导航系统(strap-down inertial navigation system，SINS)。而GPS采用体积小、精度高的OEM板卡。根据以上原则选取硬件。

1)IMU ADIS16365:ADI公司MEMSIMU ADIS16365是一种由三轴陀螺仪和三轴加速度计组成的六自由度惯性感应系统，它能够准确地测量运动物体绕三轴的角速度和加速度，在医疗器械，平台控制，机器人技术和导航领域有着广泛的应用。该IMU内部采样率高达819.2 Hz，陀螺仪量程有 ±300°/s，±150°/s，±75°/s3个量程;加速度计分辨率为3.33 m gn。

2)磁力计GPSHMC5883L:HMC5833L是一种体积小、集成度高的三轴磁传感器，用于测量地球磁场分量。三轴磁阻传感器和ASIC被封装在3.0 mm×3.0 mm×0.9 mm LCC表面装配中，采用I2C接口，具有12 bit ADC数字输出功能。

3)GPSCrescent SX—2A:GPS 采用 Hemisphere公司2006年推出的一种GPS OEM模块SX—2A。该模块是一块单频12通道接收机，单点定位精度小于2.5 m，差分模式定位精度小于0.5 m。模块接上电源电线即可工作，数据通过RS—232接口进行通信。

4)处理器STM32F407VGT:处理器 STM32F407VGT是ST公司于2011年9月出的一种低功耗、高性能工业级最新处理器。内核使用ARM Cortex M—4内核，核心处理速度可达168 MHz，210DMIPS。与 M—3系列相比，M—4增加了单周期DSP指令和浮点运算单元(FPU)功能，能够满足捷联惯性解算与组合导航中的大量浮点运算需求。

组合导航系统的硬件设计按照功能可以划分7个模块:微惯性测量模块、GPS接收机模块、数据采集模块、数据存储模块、数据显示模块、导航计算机模块和电源模块。系统的总体结构如图1所示。

图1 组合导航系统总体结构图Fig 1 Overall structure diagram of integrated navigation system

由于采用低精度MEMS—IMU，无法感知地球自转角速度，不能利用IMU自身进行初始姿态对准，所以，加入三轴磁力计HMC5883L，结合三轴重力感应计，确定初始姿态和航向角信息。

2 INS/GPS组合导航

2.1 Kalman滤波

本文采用位置速度组合模式［3］，线性Kalman滤波，其原理框图如图2所示。

图2 基于位置速度组合Kalman滤波框图Fig 2 Block diagram of Kalman filtering based on location and velocity combination

滤波器以INS误差方程作为状态方程，GPS和INS输出东北天坐标系位置、速度差值作为量测值，经Kalman滤波估计INS误差，然后对INS输出参数进行校正。这种组合方式简单，有一定的冗余度，工程上易于实现，是目前运用最多的组合方式。IMU数据经过预处理(包含剔除野值、数字滤波、误差补偿等预处理)后［4］，捷联解算得出最佳导航参数。

选取东北天坐标系，以惯导系统平台角误差、速度误差、位置误差、陀螺和加速度计误差作为状态变量，忽略天向速度和位置分量，选取状态方程如下［5］

其中，δφE，δφN，δφU为平台角误差，δVE，δVN为速度误差，δλ，δL 为位置误差，εrx，εry，εrz为陀螺一阶马尔可夫噪声，εbx，εby，εbz为陀螺随机漂移，Δ x，Δy，Δz为加速度计随机漂移。

将INS输出位置速度信息与GPS输出相减得到组合导航系统观测方程如下

系统噪声和测量噪声为

其中，W(t)方差矩阵 Q(t)，V(t)方差矩阵 R(t)。W(t)，V(t)符合高斯统计特性，通过实验统计分析得出。系统白噪声和测量白噪声方差矩阵为

其中，σrx，σry，σrz为陀螺一阶马尔可夫驱动白噪声方差;σgx，σgy，σgz为陀螺随机白噪声方差;Trx，Try，Trz陀螺一阶马尔可夫相关时间;σax，σay，σaz为加速度计随机白噪声方差，Tay，Tay，Taz为加速度计一阶马尔可夫过程相关时间;σλ，σL为 GPS 位置误差白噪声方差;σve，σvn为 GPS 速度误差白噪声方差。将以上建立的随机线性系统式(1)、式(3)的状态方程离散化后，按照Kalman滤波器的计算步骤递归更新［6］。

主程序开始后初始化配置，进行初始对准，确定初始姿态、速度、位置等信息。GPS数据中断优先级高于IMU数据，GPS信号有效时输出数据与相邻IMU输出导航参数进行Kalman数据融合，滤波器输出校正INS输出导航参数;GPS信号无效时，INS单独导航。

2.2 仿真测试

为了验证上述方案，采集静止状态下500 s数据在Matlab上进行分析，IMU采样率为81.92 Hz，GPS采样率为1 Hz，组合周期为1 s。初值设定如下:经度103°59'15.9″;纬度30°40'6.6″;东向速度0 m/s;北向速度0 m/s;俯仰角2°;横滚角0°;航向角156.2°，误差初值假设均为 0。实验结果的各导航参数估计误差曲线如图3、图4和图5所示。

图3 位置误差曲线比较Fig 3 Comparison of position error curve

图4 速度误差曲线比较Fig 4 Comparison of velocity error curve

图5 组合导航姿态误差曲线Fig 5 Attitude error curve of integrated navigation

由图3可知，INS单独导航时，短时精度较高，但误差由于时间累计发散，几分钟内位置误差甚至达到几百米至几千米，不适合于长时间定位，而INS/GPS组合导航时，位置误差得到校正呈收敛状，定位精度得到了明显提高;由图4可看出:组合导航时，速度误差曲线收敛，优于INS单独导航。由图5所示，组合导航姿态角误差曲线，在大约40 s时稳定。

3 实验测试

该组合导航系统安装于旅行者II号移动机器人，绕某教学楼进行实验。小车速度设置为1.32 m/s，沿道路中线运动，绕教学楼一周。由GPS提供初始位置与标称轨迹，评估组合导航系统性能，根据静止时ADIS16365加速度计数据与HMC5883L数据解出载体初始姿态与航向角，经组合导航后解算轨迹如图6所示。

图6 GPS和INS/GPS输出轨迹对比Fig 6 Comparison of output trajectory INS/GPS and GPS

4 结论

该嵌入式车载组合导航系统以嵌入式ARM为硬件平台，采用Kalman滤波组合导航信息融合算法，提高了系统导航的可靠性和精度。嵌入式导航系统采用GPS辅助INS，能够提供连续全面的导航参数，在短期GPS信号失锁情况下，纯惯性导航仍然在一段时间内(约15s)保持导航精度，使该系统具有一定的抗干扰能力。实际实验测试表明:该系统能够较好地跟踪姿态速度位置等状态信息，满足对室外移动机器人实时定位功能，达到了设计要求。

［1］秦永元.惯性导航［M］.北京:科学出版社，2006:12-36.

［2］李宝林，李勇建，刘 勇，等.INS/GPS多传感器车载组合导航系统算法研究［J］.压电与声光，2012(1):37-41，80.

［3］尚 捷，顾启泰.MIMS/GPS组合导航系统中卡尔曼滤波器设计与实验研究［J］.中国惯性技术学报，2005(2):45-48，53.

［4］赵伟臣，付梦印，张启鸿，等.微机械IMU数据建模与滤波方法研究［J］.中国惯性技术学报，2005(6):13-17.

［5］林敏敏，房建成，高国江.GPS/SINS组合导航系统混合校正卡尔曼滤波方法［J］.中国惯性技术学报，2003(3):30-34.

［6］卞鸿巍，李 安，覃方君.现代信息融合技术在组合导航中的应用［M］.北京:国防工业出版社，2010:70-73.