基于组合导航的整平机定位系统的设计与试验

朱 楠，黄 欢，金 鑫，秦海鹏

(江苏师范大学 电气工程及自动化学院，江苏 徐州 221000)

0 引言

我国是农业大国，水资源严重短缺，平整度较低的农田会导致农业灌溉用水的浪费。近年来，我国田间机械化技术发展迅速，不断将小面积的农田进行整合，提高了水资源和土地的利用率。对插播前的水田进行整平，可保证肥料或除草剂能够均匀地分布在水田底部，提高肥料的利用率，有效抑制杂草的生长，从而提高水稻的产量。整平机是保证农田在插播前平整的一类设备，传统的人工粗整平技术已经无法满足大面积水田的整平需求[1]，而整平机可大大提高平整精度和工作效率，因此得到了广泛应用。

航向角控制、运动轨迹监测及无人驾驶是整平机工作过程中的关键技术。获取整平机在工作时的偏航信息不仅能保证整平机始终处于直线运动状态，也可提供整平机驱动轮的转向角度，从而根据差速控制技术分配不同的转速。华南农业大学的杨伟伟等人利用卡尔曼滤波算法将陀螺仪与加速度计的数据进行融合，解算出的俯仰角可实现平地铲的姿态控制，代替高成本的倾角传感器[2]，但无法同时提供整平机的偏航信息。在无人驾驶的过程中，保证整平机以规定的速度沿规划好的路径行走尤为重要。三一重工路面机械研究院的胡宏秋和张中尧等人采用GPS系统实现了整平机的远程控制与定位[3]。然而，GPS在遇到室外复杂地形时信号易失锁，无法满足整平机在复杂工作环境下的定位要求。惯性导航是基于惯性技术的一种导航方法，能够实现运动体的自主控制和测量，提供运动体实时的姿态信息。但是，惯性导航主要依赖于对陀螺仪和加速度计输出的角速度和加速度进行积分来获得导航参数，随着时间的累积，陀螺仪的积分误差会越来越大。因此，提高惯性导航的定位精度主要是提高陀螺仪的精度。鉴于惯性导航和GPS各自的优缺点可以互补，它们之间的组合已成为组合导航中的“黄金搭档”，一般将卡尔曼滤波作为核心算法。目前，国内外研究较为深入的有松组合模式和紧组合模式。松组合模式是一种组合深度较浅的模式，也是GPS/INS最早使用的组合方式，在工程上易于实现[4]。

综上，本文将惯性导航使用的惯性传感器输出的数据与磁力计输出的数据进行融合，实现姿态解算，解算出的姿态角可同时提供整平机的航向角及实现平地铲的姿态控制。选择GPS/INS组合导航的定位方式对整平机进行定位，将惯性传感器输出的数据进行惯导解算后与GPS输出的导航参数进行融合，估计整平机的速度和位置信息。

1 系统设计

1.1 硬件设计

本文分为姿态解算算法设计和组合导航算法设计两个模块。磁力计选择HMC5983L，惯性传感器选择北微传感的BW-AH200高精度九轴航姿参考系统，其内部集成了高精度的MEMS陀螺仪和加速度计，能够提供用于进行惯导解算和姿态解算所需角速度和加速度的信息。GPS模块选择高精度的瑞士u-blox MAX-6Q GPS，利用C语言编程将姿态解算算法和组合导航算法在嵌入式系统中实现，主控制器选择STM32F103TB，具有数据处理能力强、资源丰富、性价比高等优点，是32位市场上功耗较低的产品，含有两个I2C接口以及USART接口[5]。磁力计输出的数据通过I2C接口发送给主控制器，惯性传感器和GPS的数据通过串口发送给主控制器。编译环境为MDK-Lite Version4.23。

选择卡尔曼滤波作为本系统的核心算法，进行信息融合。在姿态解算中，将惯性传感器和磁力计各自的输出进行融合，状态变量为四元数和陀螺仪的零点漂移，观测量为加速度计和磁力计的输出值[6]。在组合导航中，将惯性传感器与GPS各自输出的数据进行融合，状态方程为惯导误差方程，观测方程为两种导航子系统各自输出导航参数的差值。

1.2 姿态解算算法设计

姿态解算的状态方程为四元数微分方程，是一个非线性方程，需要对其求偏导以实现线性化，这种滤波方法称为扩展卡尔曼滤波(EKF)[7]。将各传感器的原始数据经滤波处理后进行融合。由于共有九轴的数据，需要进行大量维数较大的矩阵运算，主控制器的性能不能完全保证高精度的运算，这就导致每次运算都会出现相应的线性化误差。因此，协方差逐渐失去正定性，导致滤波发散，影响了姿态角的精度。因此，本文使用UD分解滤波算法与扩展卡尔曼滤波相结合，对卡尔曼滤波中的误差协方差进行UD分解，运算数量级的减少降低了线性化舍入误差的影响，增加了稳定性，使输出的姿态角更加精确[8]。用姿态误差的最优估计补偿陀螺仪提供的姿态参数，得到姿态的最优估计。滤波过程如图1所示。

图1 数据融合滤波过程Fig.1 The process of data fusion filtering

将算法在嵌入式系统中实现。首先设置初始值，对状态转移矩阵和测量雅克比矩阵赋值，利用加速度计测量的重力和磁力计测量的磁场对陀螺仪进行双矢量补偿；经卡尔曼滤波后，得出姿态误差的最优估计，从而计算出3个姿态角。数据通过串口发送给整平机主控制器。软件流程图如图2所示。

图2 姿态解算软件流程图Fig.2 Flow chart the attitude algorithm

1.3 组合导航算法设计

本文选择GPS/INS组合导航的定位方式，利用卡尔曼滤波估计整平机的速度及位置信息，选择惯导系统的误差方程作为卡尔曼滤波的状态方程，状态变量为速度、位置及姿态角在3个方向的误差，以及加速度计和陀螺仪各自的零点漂移。量测方程的观测量选择GPS和惯性导航分别输出的位置、速度信息之差。校正方式分为输出校正和反馈校正。由于输出校正在工程上易于实现，且滤波器发生故障后不会对系统造成影响，因此本系统选择输出校正的方式，即将滤波后的导航参数误差最优估计反馈给惯性导航系统，得到导航参数的最优值。数据融合之前必须进行初始化，即确定相关导航参数的初始值，包括初始姿态角的给定、初始速度的给定、位置的给定、初始对准时滤波所用的状态误差方差阵、量测噪声方差阵、初始误差方差阵的给定，以及GPS信息校正时滤波所用的状态误差方差阵、量测噪声方差阵和初始误差方差阵的给定[9]。惯性导航的导航参数由BW-AH200输出的角速度和加速度经惯导解算后得出。状态方程和量测方程的具体公式见文献[4]。算法流程图如图3所示。

由于姿态解算和组合导航的状态方程都是高阶时变的，在主控制器中进行卡尔曼滤波时包含了大量的矩阵计算。为了保证运行速度，本文调用了函数名为"Matrix.h"的头文件，它包含了矩阵加、减、乘、转置、求逆、计算行列式的值及生成单位矩阵等功能。部分程序如下：

void MatrixAdd( float* fMatrixA,float* fMatrixB,float* Result,

unsigned int m,unsigned int n )

{

unsigned int index_i = 0;

unsigned int index_j = 0;

unsigned int itemp = 0;

for (index_i=0;index_i

for (index_j=0;index_j

{

itemp = index_i*n+index_j;

*(Result+itemp) = *(fMatrixA+itemp) + *(fMatrixB+itemp);

}

}

图3 组合导航算法软件流程图Fig.3 Flow chart of the integrated navigation algorithm

2 试验结果及分析

本课题组设计了一套混凝土激光整平机的可移动实验平台，用来完成姿态解算实验，如图4所示。该实验平台由底座、刮平板、直流24V稳压电源和5个电推杆组成。其中，1用来调节前后水平，2用来模拟路况，3为24V直流稳压电源，4用来调节高程，5用来调节左右水平。

2.1 姿态解算试验结果分析

将惯性传感器和磁力计放置在刮平板上方，每隔5s将刮平板旋转一定的角度，多次记录实验数据，结果如图5所示。

图4 整平机实验平台Fig.4 Experimental platform of the leveling machine

图5 姿态解算实验结果图Fig.5 Experimental result diagram of attitude algorithm

由图5可知：解算出的航向角可精确提供整平机的偏航信息，俯仰角可实现平地铲的姿态控制。航向角与实际角度的偏差最大为4.8°，其余时间都在2°以内。俯仰角与实际角度的偏差不超过2°，满足实际要求。

2.2 组合导航试验结果分析

为了验证组合导航算法的可靠性，本文在整平机上进行了30s的实测，实测数据通过MatLab进行后处理。处理后的速度和位置的误差曲线如图6所示。

图6 误差曲线图Fig.6 Error curve

在实测的30s中，人为设置一段GPS失效的时间，误差曲线如图7所示。

图7 GPS失效时的误差曲线图Fig.7 Error curve when GPS failure

由图6、图7可知：本系统在实际应用中能够对整平机实现速度控制和精确定位。其速度和位置误差在30s的时间内始终趋近于“0”，且位置的最大误差不超过5m，相较于GPS10～15m的定位精度有了很大提高，同时满足组合导航0～5m定位精度的要求。当GPS信号失效时，仅由惯性导航提供的导航参数误差会在很短的时间内变得很大，与理论相符合。

3 结论

选择GPS/INS组合导航系统给出整平机实时的位置和速度信息，惯导解算中没有考虑初始对准和误差补偿。利用惯性传感器提供的数据与磁力计进行融合，解算出的姿态角可同时给出整平机的偏航信息及实现平地铲的姿态控制。本文提出的组合导航的方法不仅可以应用在整平机的水平定位上，也可以用在垂直定位(即高程测量)中，为今后提高整平机平地精度的研究打下了基础。