农机BDS/INS组合导航算法研究*

张晓寒，赵景波，董振振

(青岛理工大学，山东青岛，266520)

0 引言

起源于西方发达国家的“精细农业”概念当前越来越受到国内学者的重视，依托于现代化信息技术的快速发展，对提高我国农业机械智能化程度和加速我国农业现代化进程有着巨大作用[1]。自动驾驶技术符合农业机械程度智能化的发展要求，目前结合我国自主研发的北斗导航系统，在农机作业领域得到迅速发展[2]。研究农机自动驾驶技术，促进农业现代机械化和信息化融合[3]，是实现农业现代化、智能化的现实选择。

北斗导航系统是我国自主研发设计的导航定位系统，农业设备在BDS/INS辅助作业时自动导航，具有显著应用效果，在我国自动驾驶领域拥有良好发展前景[4]。20世纪70年代初期，美国最早对卫星导航领域展开深入研究，形成了现阶段全世界通用的GPS导航系统[5]，广泛应用于军事领域、民用设施、商业设备等领域。我国于2014年凭借北斗系统的优越性在世界海事领域取得了极大的尊重[6]，并获得了国际合法地位；2017年我国独立自主研发的“北斗三号”组网成功[7]。

20世纪90年代起，欧美等发达国家根据“精细农业”的概念开始发展农机导航于无人驾驶技术；美国最早将GPS导航与农业机械相融合，随后在欧美、日本等国家的发展下，逐步将农机导航技术产品化并投入市场[8-11]。我国的农业技术导航起步相对较晚，目前还没有进入产品化阶段；2011年刘向锋对田间拖拉机路径规划进行了深入研究；2013年贾全开发出农业机械自动导航监控系统；2016年广州某公司研发出包含监控终端的农机自动导航驾驶系统；随后多家企业和研究机构参与到农机导航的研究之中；2018年马志艳、欧阳方熙等提出了基于视觉与惯性农机组合导航方法，拓展了农机组合导航的研究思路。

本文应用北斗定位系统和惯性导航系统相结合的BDS/INS联合导航系统，以BDS/INS两者速度差值、位置差值为模型，提出改进蜂群算法优化BP神经网络辅助卡尔曼滤波的组合导航方法实现精准定位，解决复杂环境下BDS失调、INS单独导航误差大的难题。

1 导航系统分析

在BDS/INS组合导航过程中，BDS/INS可以提供载体的位置、速度、姿态信息。BDS精度较稳定无需实时校正和经常维护；INS系统是完全独立的自主式导航系统，不受外界干扰，具有良好的隐蔽性。BDS和INS之间优势互补，可明显改善导航系统的精度、输出频率及增强系统的可靠性。

在INS系统中，陀螺仪的角速度更新农机的姿态信息，即航向角[12]。初始速度、初始位置已知时，将加速度计测得的加速度经过一次积分得到运载体的线速度，再经过一次积分得到载体相对导航坐标系的位置信息。

当作业农机遇到障碍物信号遮挡时，BDS信号会减弱或丢失，无法给予满足精度的位置与速度值信息[13]，此时农机组合导航系统单独依靠INS系统进行定位。INS单独导航时，系统自身的积分原理使INS的速度、位置误差随时间积累，长时间工作会导致定位信息发散[14]。

BDS/INS组合导航时，BDS对INS的位置误差、速度误差进行补偿，BDS信息作为INS的量测输入，校正加速度的积分累计问题，完成精确定位。

本文引入BP神经网络辅助INS系统。当BDS信号良好时，采用BP神经网络辅助卡尔曼滤波完成训练，减少模型不准确造成的误差；当BDS信号失锁情况下利用BP神经网络进行预测，此时BP神经网络替代BDS校正INS系统。

2 导航算法研究

2.1 卡尔曼滤波

BDS/INS组合导航中使用算法是基于卡尔曼滤波理论的相关算法，通过组合导航系统的误差滤波模型估计惯性导航误差值，进而利用误差估计值对惯性导航系统进行修正，使系统导航精准[15]。

本文研究模型为BDS和INS的速度与位置差值，将BDS与INS的速度差值、位置差值经过卡尔曼滤波器处理，得到组合导航误差估计值；然后利用误差估测值反馈校正INS系统，得到最优期望输出。本文在东北天坐标系下建立模型[16]，以INS的误差量作为状态量，BDS与INS的速度差值、位置差值作为量测量。

1) 状态方程。INS的误差状态向量由与BDS接收机位置误差和东北天三方向速度误差组成，姿态误差为东北天方向平台失准角，加速度计零偏误差，以及陀螺仪漂移误差组成。通过结合平台误差角方程、速度误差方程、位置误差方程与加速度计和陀螺仪误差，得到INS的误差状态方程

(1)

FI(t)——系统矩阵；

XI(t)——系统状态向量；

GI(t)——系统噪声矩阵；

WI(t)——系统噪声向量。

2) 量测方程。将INS与BDS测得的位置差值、速度差值信息作为量测信息。位置信息包括经度、纬度、高度信息，速度信息为所建东北天模型中东、北、天方向速度误差。结合INS系统解算出的位置信息与BDS解算出的位置信息可得到位置的量测方程；进一步结合INS系统解算出的速度信息和BDS解算出的速度信息得到速度的量测方程。将位置的量测方程与速度的量测方程结合得到BDS/INS组合模式下，组合导航的量测方程

=H(t)X(t)+V(t)

(2)

式中：Z(t)——组合导航测量矢量；

X(t)——系统状态向量；

H(t)——量测矩阵；

V(t)——量测噪声；

p——位置分量；

v——速度分量。

3) 卡尔曼滤波校正。BDS/INS联合导航系统为非线性系统，在系统的状态方程和观测方程的基础上，卡尔曼滤波算法可表示为以下7个步骤。

第一步：状态一步预测

(3)

Φk/k-1——k-1时刻到k时刻的状态转移矩阵；

第二步：状态一步预测均方误差

(4)

式中:Qk——k时刻系统噪声方差矩阵；

Γk——k时刻系统过程噪声。

第三步：利用k-1时刻协方差值，计算k时刻协方差一步预测值[17]。

(5)

第四步：滤波增益

(6)

式中:Hk——k时刻量测矩阵。

第五步：状态估计

(7)

式中:Zk——k时刻量测向量。

第六步：状态估计均方误差

(8)

式中:Rk——k时刻量测噪声方差矩阵。

第七步：通过不断迭代，更新各时刻状态估计值。

但是卡尔曼滤波也存在自身的缺陷。当增益计算维数很大时，误差会随时间积累而发散[18]；当系统参数不准确时，影响状态估计产生较大误差，滤波增益矩阵失去准确的计算能力[19]，使得卡尔曼滤波计算结果不准确，系统失去准确调控功能[20]。

2.2 神经网络辅助卡尔曼滤波

为解决BDS信号干扰失调问题，本文引入BP神经网络算法(图1)。当BDS信号正常时，BDS/INS组合导航系统对BP神经网络进行训练；当BDS信号中断时，BP神经网络输入发生变化，INS的加速度计输出及误差作为BP神经网络的输入，预测相关速度误差与位置误差。

图1 神经网络训练模式和预测模式Fig. 1 Neural network training mode and prediction mode

2.3 蜂群算法优化BP神经网络

为改善BP神经网络，将算法训练结果收敛到均方误差全局最小极值，避免输出陷入局部极小值，使用蜂群算法优化BP神经网络[21]。

2.3.1 人工蜂群算法

用人工蜂群算法(ABC-BP)为蜂群在D维权值空间求解。采蜜蜂与观察蜂的数量相等，等于食物源的数量，设为SN。食物源的位置为问题的可能解，采蜜蜂对应相应的食物源，食物源的吸引度为解的适应度。

第i个采蜜蜂搜寻食物源

(9)

式中:φid——区间(-1,1)上的随机数。

标准ABC算法根据蜂群的贪婪法则，比较新生成的解X′i={x′i1,x′i2,…,x′id}和原来的解Xi={xi1,xi2,…,xid},依据食物源质量保留其中更好的解。

观察蜂选择食物源的概率

(10)

式中:fiti——可能解的适应值。

当蜂群中采蜜蜂和观察蜂完成D维空间搜索后，如果食物源的适应值在limit步骤内没有提高，则放弃此食物源。此食物源对应的采蜜蜂变为侦察蜂，侦察蜂通过式(11)搜索新可能解。

(11)

2.3.2 改进人工蜂群算法

通过改进采蜜蜂的搜索食物方式和观察蜂选择食物源概率，进一步改进蜂群算法优化BP神经网络，组成改进ABC-BP模型。

1) 改进搜寻食物源方式。经研究，采蜜蜂与观察蜂以式(10)搜索食物源，在原有食物源的附近随机搜索获得新食物源，并引入全局最优解加快收敛速度。采蜜蜂搜索公式如式(12)所示。

(12)

式中:r1——区间(0,1)间随机数；

r2——区间(0,1.5)间随机数；

yd——全局最优解的第d个变量。

引入全局最优解，虽然使蜂群迅速向适应度高的食物源集中，提高搜寻解的收敛速度，但破坏了蜂群种群的多样性，易使得搜索最优解变为局部最小值。所以进一步引入调节因子w1与w2，改进蜂群算法。此时采蜜蜂搜索

w2×r2×(yd-xid)

(13)

(14)

(15)

式中:iter——当前的迭代次数；

maxcycle——最大迭代次数；

wmin——调整因子的最小值；

wmax——调整因子的最大值。

开始阶段w2>w1，选中的食物源使采蜜蜂向全局最优解靠近的速度较快，搜寻的食物源更快逼近收敛全局最优解；后期阶段w1>w2，此时采蜜蜂若以较快的收敛速度向最优解靠近时，会陷入局部最优。因此在实验后期加大食物源的随机性，避免种群陷入局部最优。

2) 改进观察蜂选择概率。标准ABC算法中，观察蜂选择食物源时，蜂群在搜索更新时会向适应度较高的解运动，寻到的解易为局部极值；在常用的选择概率中，开始阶段观察蜂选择适应度较小的食物源，减小蜂群向更高食物源靠近的速度，保持了种群多样性，但是算法初期搜索速度较慢。改进的蜂群算法使用新的自适应判断因子

(16)

改进的选择概率公式

(17)

根据φid与τ比较选择概率pi,保证种群完整性，并大大提高的搜索初期的搜索速度。

3 算法仿真与数据分析

3.1 神经网络辅助BDS/INS导航效果仿真

本文采用MATLAB软件对BP神经网络辅助卡尔曼滤波算法组合导航进行仿真验证。将试验农机的地理经度、纬度、高度和东向、北向、天向的实际速度作为对象，进行仿真测试得出输出结果。对显示结果与导航数据进行对比，得到最优导航精度。

本文测试轨迹起始点设为东经143.65°，北纬49.26°，海拔高度设置为100 m；初始东向速度为2 m/s，北向速度为0 m/s；其他姿态信息均置零。INS系统的仿真误差参数如表1所示。

表1 INS系统仿真参数设置Tab. 1 INS system simulation parameter setting

本次仿真设置在前700 s内，锁定组合导航系统的BDS系统，并在此过程中同步BP神经网络实时在线训练；700 s后，试验农机收到外界不利环境影响，此时BDS信号减弱或失锁，本次仿真设置为BDS失锁状态。此时将先前700 s训练的BP神经网络代替BDS系统，BP神经网络进行预测模式，对INS进行补偿校正，分别得到东、北、天向速度误差与经、纬、高向位置误差变化图。图2、图3为东向速度误差仿真图和经度位置误差仿真图。

图2 神经网络辅助东向速度误差仿真图Fig. 2 Neural network-assisted eastward velocity error simulation

图3 神经网络辅助经度误差仿真图Fig. 3 Neural network assisted longitude error simulation

图2和图3中，若在700 s处未受外界不利环境影响，BDS信号正常时误差仿真图如图中深色线1所示；若在700 s之后受外界不利环境影响，BDS信号减弱或者失效误差仿真图如图中浅色线2所示。分析误差数据可得，在BDS失锁后，东向速度误差在0.5 m/s之内，经纬度的位置误差小于30 m。而在无神经网络校正的实际生产生活中，纯INS在失去BDS信号700 s后普遍速度误差大于2 m/s，位置误差大于500 m。证明了使用训练的BP神经网络进行INS校正，将速度误差和位置误差控制在与BDS有效时相近的范围，相比较于单独INS导航的速度误差发生较为明显的改善。

3.2 改进的ABC-BP模型训练效果仿真

为验证改进的人工蜂群算法对神经网络的作用，分别使用ABC-BP和改进的ABC-BP算法，以东向速度为对象进行训练仿真，设置最大的迭代次数为100次，利用目标函数和适应度值，对优化后的参数进行训练，训练上限为300次。ABC-BP模型和改进的ABC-BP模型训练试验仿真结果如图4、图5所示。

图4 ABC-BP训练结果Fig. 4 ABC-BP training results

图5 改进ABC-BP训练结果Fig. 5 Improve ABC-BP training results

通过对比图4、图5可以看出，ABC-BP模型在第30次迭代左右达到收敛，最终误差约为0.08；改进的ABC-BP模型在23次迭代左右达到收敛，最终误差约为0.04。可见改进的ABC-BP模型相比于的ABC-BP模型，收敛速度快，且精度较高，克服了算法中存在的搜索能力较弱难以跳出局部最优解的弊端。

3.3 改进的ABC-BP模型预测试验仿真

在一次农机作业中，让其沿正东方向直线行驶，测得其实际的东向速度，并同时用已训练好的ABC-BP模型和改进的ABC-BP模型对农机东向速度分别进行预测。两模型的预测东向速度与实际东向速度对比图如图6、图7所示。

图6 ABC-BP预测值与实际值比较Fig. 6 ABC-BP predicted value compared with actual value

图7 改进ABC-BP预测值与实际值比较Fig. 7 Improve the comparison of ABC-BP predicted values with actual values

对比图6、图7中的数据进行分析，为评价估计量与被估计量存在的差异程度，分别计算两种算法各60组数据的平均绝对误差(MAE)和均方误差(MSE)，结果如表2所示。

表2 误差分析Tab. 2 Error analysis

由此可得，使用改进的人工蜂群算法误差相对于之前有了明显提高，平均误差小于0.6 m/s，其预测值更加接近于实际数值，预测精度有了明显改善。综上所述，改进ABC-BP模型具有更加精确快速的预测效果。

4 结论

1) 本文针对农机在环境干扰情况下BDS失调、INS单独导航误差大的不足，以BDS/INS两者间位置、速度差值为模型，提出蜂群算法优化BP神经网络辅助卡尔曼滤波的算法组合导航，精准定位农机位置速度信息。

2) 使用了BDS/INS组合导航，结合了两者短时间内定位精度高、定位没有误差积累的优势，提高了定位精度及稳定性；引入BP神经网络辅助卡尔曼滤波，在BDS有效时BP神经网络训练，BDS失效时BP神经网络预测速度位置差，避免单独INS导航时误差不断增大。

3) 改进人工蜂群算法，使系统更快更准确的更新神经网络的权值阈值，提高了训练收敛速度，在速度预测仿真中将平均误差控制在了0.6 m/s之内，改善了农机导航精准度。