基于信息融合的拖拉机组合导航定位系统研究

2020-08-27李忠利刘小锋陈修魁高永升杨淑君

农业机械学报 2020年8期

李忠利刘小锋陈修魁高永升杨淑君

(河南科技大学车辆与交通工程学院, 洛阳 471003)

0 引言

农业工程领域导航定位系统的广泛应用在很大程度上提高了农业生产和作业效率。由于单一的车辆定位技术都存在一定的局限性和适用范围，往往不能满足拖拉机田间作业的精度要求，因此以全球定位系统(Global position system，GPS)和惯性导航联合的多传感器信息融合定位技术成为农业车辆导航定位系统的关键技术[1-4]。

华南农业大学研制的拖拉机自动导航定位控制系统在拖拉机速度为2.88 km/h时，拖拉机的跟踪误差保持在0.3 m[5-6]；中国农业大学研制的拖拉机自动导航控制系统能实时进行数据处理和定位导航，并且能够较好地进行路线追踪，最大横向偏差为11 cm[7-8]。这些拖拉机自动导航定位系统虽然具有较高的导航定位精度，也能进行直线和转弯跟踪，但忽略了拖拉机车身姿态引起的定位误差。为了修正这一误差，须获得拖拉机姿态信息。拖拉机姿态信息可通过加速度计或陀螺仪获得。文献[9]基于捷联惯导系统结合四元数进行了姿态解算，文献[10]基于RTK-GPS双天线进行了姿态解算，文献[11]采用自适应扩展卡尔曼进行姿态解算。上述研究成果对于姿态解算都获得了很好的测姿效果，但当拖拉机产生侧向加速度时，由侧向加速度引起姿态解算偏差，对偏差进行定位修正的研究较少。因此基于多传感器数据融合的拖拉机组合导航定位修正研究对于农业车辆导航具有重要意义。

本文采用载波相位差分全球定位系统(Real time kinematic differential global position system，RTK-GPS)和惯性测量单元(Inertial measurement unit，IMU)组合定位方式，将多传感器信息进行融合，充分发挥各个导航系统的优势，以修正拖拉机在田间作业时因地形倾斜、土质硬度不均或连续转弯而引起的定位误差，从而提高系统的定位精度和可靠性。

1 组合导航定位方案

本文设计的组合导航系统主要由RTK-GPS和IMU组成。其中RTK-GPS主要包括基站移动站接收机、基站移动站无线数传电台、GPS天线、基站移动站电台收发天线和移动电源；IMU由3轴的加速度计和3轴的陀螺仪组成。RTK-GPS[12]主要获得拖拉机的位置、速度、航向信息，拖拉机的侧倾角、俯仰角由加速度计、陀螺仪融合得到[13]。

RTK-GPS/IMU组合导航的核心在于多传感器数据的融合，从而得到最优估计的定位信息[14]。多传感器融合的常用算法是卡尔曼滤波。但卡尔曼滤波需要建立精确的系统模型并且状态噪声和观测噪声应该满足正态分布才能得到比较准确的估计效果。而拖拉机在田间工作过程中，由于田间环境复杂多变，将导致模型的状态噪声和观测噪声特性发生改变。因此，当建立的卡尔曼模型与实际的拖拉机工作状态存在较大偏差时，将导致卡尔曼滤波失败。

遗传算法反向传播(Genetic algorithm back propagation，GA-BP)神经网络可以完美拟合任意非线性系统，可以和卡尔曼滤波算法融合，弥补卡尔曼算法的局限性[15]。因此，采用GA-BP神经网络卡尔曼滤波算法实现RTK-GPS/IMU组合导航的信息融合。其组合导航定位方案如图1 所示。

图1 组合导航定位方案图Fig.1 Integrated navigation positioning plan diagram

2 基于GA-BP神经网络的卡尔曼滤波器设计

2.1 GA-BP神经网络模型

2.1.1BP神经网络模型

BP神经网络是一种以信号前向传递，误差逆向传播的多层前馈神经网络[16-17]。在BP神经网络学习训练过程中，输入信号经过权值、阈值和激活函数处理之后依次到达隐含层和输出层。如果输出层的输出值与期望值之间的均方根误差没有达到设定的网络精度，则误差将反向传播，以修改网络的权值和阈值。多次迭代训练，使得BP神经网路的预测输出不断逼近期望值，从而建立输入输出的非线性模型。BP神经网络模型可以采用3层网络结构，将JY61型六轴姿态角度传感器的X轴加速度Xacc、Y轴加速度Yacc、Z轴加速度Zacc、Z轴角速度w和RTK-GPS速度信息v的乘积wv作为网络的输入变量，由Kolmogorov公式计算得出隐含层神经元的个数为9，侧向加速度在X、Y、Z轴的分量Xnoise、Ynosie、Znoise作为BP神经网络的输出。

在构建的BP神经网络中，隐含层传递函数为正切S型tansig函数，输出层传递函数为线性purelin函数。权值学习训练函数为trainlm函数，此函数采用Levenberg-Marquardt算法[18]，可以使得权值快速收敛，达到网络训练目标的要求。

2.1.2权值与阈值优化

因为BP神经网络权值和阈值的随机初始化导致网络的泛化能力不强，容易陷入局部最优解，所以采用遗传算法优化网络的初始权值和阈值以提高神经网络的训练效果[19]。遗传算法优化BP神经网络的权值和阈值流程图如图2所示。

图2 遗传算法优化神经网络的权值和阈值流程图Fig.2 Genetic algorithm optimization of weight and threshold flow chart of neural network

2.1.3Matlab仿真训练

由于无法从传感器上得到BP神经网络的训练数据，需要用理论数据样本对神经网络进行训练。依据拖拉机在田间的工作情况大致可以确定拖拉机速度、角速度、俯仰角、侧倾角以及Z轴加速度的取值范围，并以表1中拖拉机角速度和速度乘积作为拖拉机的侧向加速度Lacc。已知侧倾角、俯仰角、Z轴加速度即可求出X轴加速度Xacc和Y轴加速度Yacc，将拖拉机侧向加速度Lacc根据勾股定理分解在X、Y、Z轴上，从而得到BP神经网络所需的输入Xinput、Yinput、Zinput和输出Xnoise、Ynoise、Znoise。根据表1中拖拉机参数的取值范围编写了BP神经网络样本生成软件，该软件以合适的间隔对这些特征信息的范围进行细化，遍历所有可能的组合，用理论值对神经网络进行训练。表2为生成BP神经网络的部分训练样本数据。Xnoise为侧向加速度在X轴的分量，Ynoise为侧向加速度在Y轴的分量，Znoise为侧向加速度在Z轴的分量。从而可得：Xinput、Yinput、Zinput、Lacc为BP神经网络的输入特征，Xnoise、Ynoise、Znoise为BP神经网络的输出。

表1 拖拉机参数的取值范围Tab.1 Value range of various tractor parameters

表2 BP神经网络样本数据Tab.2 BP neural network sample data m/s2

根据设置的取值范围生成满足条件的80 500组数据样本，其中200组作为测试样本，剩余的数据作为训练样本。为了使得神经网络快速收敛、训练时间短且激活函数的区分度明显，要对输入的样本数据进行归一化处理[20]。归一化公式为

(1)

式中S——样本归一化值

Ii——样本数据

Imax——样本数据最大值

Imin——样本数据最小值

经过多次仿真试验，设学习率Lr为 0.3，网络精度为 0.000 01[21]，迭代次数为2 000次时，网络训练较好。

遗传算法对于BP神经网络有很好的优化效果，在优化过程中需要对算法的一些参数进行设置。种群规模表示初始解集的大小，太少容易丢失种群多样性，导致无法寻找到最优解；太多则会增加计算量。种群规模设置为50；染色体编码采用浮点编码形式；选择函数为normGeomSelect，传递选择参数为0.09；交叉函数为arithXover，传递交叉参数为2；变异函数为nonUnifMutation，传递变异参数为[2，100，3]；迭代次数为100次[22]。

依据搭建的GA-BP神经网络模型在Matlab 2018工作环境下进行BP神经网络的训练。真实值与神经网络预测值对比如图3所示；侧向加速度在各个轴分量真实值与神经网络预测值误差如图4所示。

图3 真实值与神经网络预测值对比Fig.3 Comparison between true value and predicted value of neural network

图4 侧向加速度在各个轴分量真实值与神经网络预测值误差Fig.4 Error between true value of lateral acceleration component on each axis and predicted value of neural network

由图3可知，虽在一些样本点处拟合的效果不理想，但整体预测值拟合结果较好，这也验证了所构建的GA-BP神经网络具有较好的预测效果。

从图4可以看到，侧向加速度在X、Y、Z轴分量的真实值和预测值误差在±0.04 m/s2的范围内波动，主要集中在±0.02 m/s2的区间范围内。

2.2 卡尔曼滤波器设计

拖拉机姿态解算中的卡尔曼滤波是建立姿态角的状态方程和观测方程，然后通过预测更新过程得到较准确的姿态角[23-24]。将陀螺仪积分得到的姿态角A和陀螺仪偏差B作为卡尔曼滤波状态方程的状态量，加速度计得到的姿态角Aacc作为观测值。该组合导航系统的状态方程和观测方程为

(2)

(3)

式中wgyro——陀螺仪角速度输出

wg——陀螺仪噪声

vacc——加速度计的测量噪声

设wg与vacc相互独立且为满足正态分布的白噪声。令系统的采样时间为dt，则离散系统的状态方程和观测方程为

(4)

(5)

(6)

(7)

式中θ、φ——陀螺仪得到的俯仰角、侧倾角

wXb——X轴角速度偏差

wYb——Y轴角速度偏差

wX、wY——陀螺仪X、Y轴的噪声

N(0，Rk)——加速度计噪声的正态分布特征

θacc、φacc——加速度计得到的仰俯角、侧倾角

k——离散系统时间，下标表示时刻

Q、R——过程噪声和观测噪声的协方差矩阵

qθ、qφ——陀螺仪积分得到俯仰角和侧倾角的噪声协方差

qbX、qbY——陀螺仪X、Y轴偏差噪声的协方差

rθ、rφ——加速度计得到的俯仰角和侧倾角噪声协方差

Q、R表示卡尔曼滤波器对其传感器得到数据的信任程度，值越小，表明信任程度越高。通过对采集到的大量试验数据进行分析，可得：qθ为0.002、qφ为0.005、qbX为0.01、qbY为0.01、rθ为3、rφ为3，代入即可得到Q和R。卡尔曼滤波整个工作过程分为时间更新和观测更新。时间更新即根据前一时刻的状态估计值推算当前时刻的状态变量先验估计值和误差协方差先验估计值；观测更新负责将先验估计和新的测量值结合起来构造改进的后验估计，以次递归找到最优结果。

3 试验准备及误差分析

3.1 坐标系建立

选用高斯平面坐标系，y轴指向坐标东为正，x轴指向坐标北为正。车体坐标系为右手坐标系，X′轴指向车辆前进的方向，Y′轴垂直于车辆前进方向且指向驾驶员右侧，Z′轴为通过汽车质心指向上方。由于GPS的定位信息所使用的坐标系为WGS84大地坐标系统，所以需要对WGS84大地坐标系进行高斯投影正解计算，得到相对应的高斯平面坐标[25-27]。坐标系示意图如图5所示。

图5 坐标系示意图Fig.5 Schematic of coordinate system

3.2 拖拉机导航参数

图6 拖拉机导航参数Fig.6 Tractor navigation parameters

将车体前进方向与真北方向的夹角定义为拖拉机的航向角ψ，规定正方向为顺时针方向。将X′轴与水平面之间的夹角定义为拖拉机的俯仰角θ，规定拖拉机前轮下倾为正，上倾为负。将车体Y′轴与水平面之间的夹角定义为拖拉机的侧倾角φ，规定沿车体前进方向右倾为正，左倾为负。拖拉机的导航参数如图6所示。

3.3 RTK-GPS和IMU安装

试验选用的拖拉机为东方红MK904型轮式拖拉机，安装设备主要包括GPS双天线、移动站接收机、移动站无线数传电台、IMU，GPS天线安装在拖拉机顶部中心横轴线上，两边各一个，距离地面约2 580 mm。移动站无线数传电台安装在拖拉机前盖上，移动站接收机放在驾驶室内。基站GPS天线、接收机、无线数传电台放在田间试验场，RTK-GPS设备全程采用移动电源供电。IMU选用JY61型六轴姿态角度传感器，输出3轴加速度信息、3轴角速度信息和拖拉机姿态信息，安装在拖拉机的质心位置，X轴与X′轴重合、Y轴与Y′轴重合、垂直于模块向外为Z轴。本次试验需要使用编写的上位机数据采集软件将原始的RTK-GPS和IMU的定位信息进行保存，用于后续在Matlab中使用已经训练的GA-BP神经网络进行处理以及对GA-BP神经网络卡尔曼滤波算法的验证。GPS和IMU安装位置图如图7所示。

图7 RTK-GPS和IMU安装位置图Fig.7 RTK-GPS and IMU installation diagram1.电台天线 2.移动站无线数传电台 3.计算机 4.移动站接收机 5.JY61型六轴姿态角度传感器 6.GPS天线

3.4 上位机数据采集软件

上位机数据采集软件是基于Visual Studio开发软件使用C#语言开发。上位机数据采集软件同时对多传感器的信息进行采集并同步传感器数据，用于后续的算法验证和处理。上位机数据采集软件主要有两大功能：①对传感器信息的显示，这也更加直观地了解到传感器当前的状态信息。②对传感器的信息进行保存，方便后续的算法处理。上位机数据采集软件界面如图8所示。从图8可以看到，姿态传感器3轴加速度、3轴角速度、绕3轴转动的角度、UTC时间、定位方式、定位信息、速度信息以及航向信息，还能通过曲线实时看到数据的动态变化，从而判断采集到的数据是否正确。

图8 上位机数据采集软件界面Fig.8 PC data acquisition software

3.5 GPS误差分析

由于田间情况复杂、地面起伏不定、土质硬度不均，拖拉机在田间耕作或连续转弯期间，车身极易受到这些因素影响而发生倾斜，从而导致GPS接收天线发生倾斜，产生定位误差。为了提高拖拉机自主耕作的导航精度，实现精细作业，必须要对车体倾斜误差进行修正，从而得到较准确的GPS定位信息。当车体发生倾斜时，其在车体坐标系上X′、Y′轴在水平面上投影的偏差为

e′x=Hsinθ

(8)

e′y=Hsinφ

(9)

式中H——GPS天线距离水平地面的高度

由于e′x、e′y是居于车体坐标系下X′、Y′轴水平面上投影的偏差，而定位所采用的坐标系是高斯平面坐标系，因此需要进行坐标变换，将车体坐标系下的坐标转换到高斯平面坐标系下的坐标[24-27]。设在高斯平面下坐标为(ex,ey)，其对应的车体坐标系下的坐标为(e′x,e′y)，它们的转换关系为

(10)

其中，航向角由RTK-GPS双天线测得，具有很高的精度。得到了高斯平面坐标系下的偏差，就可以用此偏差修正原始的RTK-GPS定位信息，从而提高导航精度。

4 田间试验

为了验证所设计的基于GA-BP神经网络卡尔曼滤波算法的有效性，利用搭建的组合导航系统试验平台，在洛阳孟津一拖产品试验基地选取一块未被耕种的试验田进行试验。主要包括拖拉机的直线行驶试验和拖拉机连续转弯试验。在拖拉机直线行驶试验中对拖拉机进行了4次的数据采集，在拖拉机连续转弯试验中对拖拉机进行了2次数据的采集。将GA-BP神经网络卡尔曼滤波算法的组合导航方案对拖拉机的位置信息进行实时估计，同时，记录RTK-GPS和IMU的原始定位信息。RTK-GPS具有双天线，且横向安装只能得到拖拉机的侧倾角，其动态精度为0.08°。由于RTK-GPS侧倾角的更新频率为1 Hz，将RTK-GPS测得的侧倾角进行插值处理后作为参考角度来验证算法的效果。图9为拖拉机直线行驶过程中侧倾角在不同算法下的对比。从图9可以得到，在拖拉机直线行驶情况下，GA-BP卡尔曼和卡尔曼2种算法获得的侧倾角基本跟随GPS参考角度变换，且GA-BP卡尔曼和卡尔曼2种算法得到的侧倾角曲线变化趋势大致相同。图10为拖拉机直线行驶过程中不同算法下的侧倾角与GPS参考角度的误差图。从图10可以得到，GA-BP卡尔曼算法得到的侧倾角与GPS参考角度的最大误差为0.07 rad，最小值为-0.04 rad，大部分误差的范围为-0.01～0.04 rad；卡尔曼算法得到的侧倾角与GPS参考角度的最大误差为0.09 rad，最小误差为-0.05 rad，大部分误差的范围为-0.02～0.05 rad。由于GA-BP神经网络已经训练完毕且数据处理设备为计算机CPU，所以GA-BP卡尔曼算法和传统卡尔曼算法的响应时间差距为纳秒级，可忽略不计，拖拉机转弯行驶也如此。由此可得，拖拉机在直线行驶情况下，GA-BP卡尔曼算法获得的侧倾角比卡尔曼得到的侧倾角精度提高了0.01 rad。

图9 拖拉机直线行驶过程中在不同算法下侧倾角对比Fig.9 Angle comparison chart of tractor’s roll angle by different algorithms during straight driving

图10 拖拉机直线行驶过程中不同算法下的侧倾角与GPS参考角度的误差曲线Fig.10 Error graph of roll angle and GPS reference angle by different algorithms during tractor straight driving

图11 拖拉机连续转弯过程中在不同算法下侧倾角对比Fig.11 Comparison of roll angle of tractor during continuous turning by different algorithms

图11为拖拉机连续转弯过程中侧倾角在不同算法下的对比。从图11可以得到，在拖拉机连续转弯过程中，2种算法获得的侧倾角的动态变换趋势同GPS参考角度一致，但卡尔曼算法得到的侧倾角的变换幅度相对于GA-BP卡尔曼算法变化幅度较剧烈。图12为拖拉机连续转弯过程中不同算法下的侧倾角与GPS参考角的误差。从图12可得，GA-BP卡尔曼算法得到的侧倾角与GPS参考角度的最大误差为0.08 rad，最小值为-0.04 rad，大部分误差的范围为-0.02～0.04 rad；卡尔曼算法得到的侧倾角与GPS参考角度的最大误差为0.11 rad，最小误差为-0.06 rad，大部分误差的范围为-0.04～0.08 rad。由此可得，拖拉机在连续转弯过程中，GA-BP卡尔曼算法获得的侧倾角比卡尔曼得到的侧倾角在左倾时精度提高了0.02 rad，右倾时精度提高了0.04 rad。即在转弯过程中GA-BP卡尔曼算法比卡尔曼算法得到的侧倾角具有更好的精度。

图13 拖拉机直线行驶轨迹及局部放大图Fig.13 Straight track of tractor and partial enlarged view

图12 拖拉机连续转弯过程中不同算法下的侧倾角与GPS参考角度的误差曲线Fig.12 Error graph of roll angle and GPS reference angle by different algorithms during tractor continuous turning

将原始的RTK-GPS定位信息和经过误差修正之后的定位信息进行比较从而验证误差修正的效果。由于试验环境限制且拖拉机连续转弯行驶无法精确规划路径，所以没有以规划的直线路径和转弯路径作为参考，进而通过分析定位信息的数字特征来间接验证修正效果。图13为拖拉机直线行驶轨迹及局部放大图。图14为拖拉机连续转弯行驶轨迹及局部放大图。图中所示都为高斯平面相对坐标，x轴对应坐标北向，y轴对应东向。在图13a、13b、13c、13d、14a、14b中圆圈对应的放大图13e、13f、13g、13h、14c、14d中可以看到，拖拉机实际轨迹会出现曲折的波动，这些波动主要是由于田间地面起伏不定、土质硬度不均导致的车身倾斜而造成的定位偏差。经过GA-BP神经网络卡尔曼滤波算法及误差修正后的拖拉机行驶轨迹相对于原始拖拉机轨迹整体更加平稳。表3为拖拉机在直线和转弯行驶过程中定位信息的协方差和相关系数。从表3可以得到，只有第1次转弯的定位信息为正相关，其余情况都为负相关。从其对应的协方差和相关系数的原始和修正值可以得到修正后的定位信息协方差和相关系数比原始定位信息都有明显减小。即修正后的定位信息比起原始信息幅值变化小、集中且平稳。根据图13、14和表3可得GA-BP卡尔曼算法及误差分析在一定程度上修正了车身姿态倾斜引起的定位误差，使拖拉机在复杂的地形或连续转弯情况下都有较好的定位精度。