基于自校准变结构Kalman的农机导航BDS失锁续航方法

张闻宇 王 进 张智刚 何 杰 胡 炼 罗锡文

(1.华南农业大学南方农业机械与装备关键技术教育部重点实验室，广州 510642;2.雷沃重工股份有限公司，潍坊 261206)

0 引言

针对旱地农机的导航系统[1-4]、针对水田农机的导航系统[5-9]和针对农业机械的导航系统[10-11]，大多以定位系统稳定可靠为设计前提，但是实际作业中存在定位数据不可靠现象，由于差分信号的无线通信不稳定、树木引起的多路径效应和高压线干扰等原因，导致北斗全球定位系统(Beidou Navigation Satellite System，BDS)差分解失锁，系统丢失固定解类，定位误差增大。该现象易导致系统突然失控，对导航作业影响较大，例如，棉花播种铺膜作业时的失控将导致切膜现象，需要揭膜、揭滴灌带和重播，费时、费力、费料。因此，研究农业机械自动导航系统BDS失锁解决方法具有实际意义。

BDS失锁问题存在于汽车、飞机和导弹等多个领域，国内外研究者对其进行了相关研究。曹娟娟等[12]提出一种基于径向基(Radial basis function，RBF)神经网络预测的MEMS-SINS误差反馈校正方法，在4个50 s以内的全球定位系统(Global positioning system，GPS)人为失锁过程中,该方法导航结果与参考系统比较,平均位置误差为3.8 m,平均速度误差为0.6 m/s,平均姿态误差为0.5°。谭红力[13]利用神经网络标定和补偿陀螺仪误差，结合Kalman滤波器设计了惯导快速传递对准算法，5次跑车试验结果表明，误差由40 m下降到10 m。鲍泳林等[14]针对飞行条件下惯性导航系统(Inertial navigation system，INS)/GPS 组合导航系统在 GPS 失锁时解算精度下降甚至发散的问题，采用神经网络辅助组合导航算法，飞行试验数据仿真表明，能够在一定程度上抑制解算发散。SHEN等[15]针对导航中GPS信号中断问题提出了基于径向基函数多层感知机的容积卡尔曼滤波，信号中断500 s，推算均方误差保持在23.11 m以内。刘庆元等[16]针对组合导航中载体处于恶劣环境下，或者载体处于大机动运动情况下导致的GPS失锁问题，提出了一种GPS/INS组合导航系统，通过模拟试验表明，经纬度与速度精度均提高了65%左右。BROWN等[17]研究的组合GPS/微机电系统(Micro electro mechanical system, MEMS)惯性导航组件应用于无人地面车辆，失锁后20 s内误差超过10 m。

上述研究对农机导航BDS失锁问题有一定的借鉴价值，但这些研究对精度的要求为米级，与农机农艺要求不匹配。农机导航系统具有高精度、较低的运动速度和有限缓冲距离需求等特点，上述研究无法直接应用于农业机械导航系统中。

本文根据农机农艺特点，分析INS传感器误差对失锁状态导航定位精度的影响规律，研究适应农机农艺要求的基于自校准变结构Kalman滤波器的农机导航BDS失锁续航方法，该方法能够实现常态与失锁态无缝切换、在有限时间内维持一定的直线跟踪精度(以棉花播种为例，实际偏差小于20 cm将不会出现切膜现象)，同时提示驾驶员，有足够的缓冲时间和距离进行停车处理，不影响重新获得固定解后继续进行自动导航作业。

1 4自由度农机运动学模型

融合BDS和INS的系统信息是克服BDS系统短时失锁问题的可行方案之一。信息融合需要构建农机简化的运动学模型来表示不同维度信息间的物理关系，依据文献[1]描述的关于横滚俯仰对定位精度的影响原理，设计了4自由度农机运动学模型，该模型包含4个自由度，分别是前进、转向、横滚和俯仰，农业机械4自由度运动学模型如图1所示。

图1 4自由度农机简化运动学模型

x′y′z′是农机车身坐标系，该坐标系随车身移动，x′方向与航向一致，x′y′平面为水平面，h为接收天线到底面的垂直距离(m)，α、β和γ分别是横滚角、俯仰角和偏航角(rad)。

在前进和转向两个自由度上采用简化二轮车模型[4]。为将横滚、俯仰自由度上的卫星天线漂移统一到二维平面上来，依据4自由度运动学模型对横滚、俯仰引起的定位漂移进行定义。将三维的BDS天线定位投影到车身坐标的二维平面(x′y′)，并转换到全局坐标系(XY)中，用于消除横滚、俯仰导致的等效定位误差干扰[1]。投影和转换的几何关系如图2所示。

图2 卫星天线平面投影与转换几何关系图

XYZ为全局坐标系，该坐标系不随车身移动，其中XY平面与x′y′平面平行，px为BDS的天线坐标在局部坐标系x′轴投影，py为BDS的天线坐标在局部坐标系y′轴投影，θ为全局坐标系航向角(rad)。

由于农机行驶时α和β均较小，所以将sinα和sinβ用α和β进行等效替换，获得横滚俯仰引起的定位投影相对位置漂移微分方程

(1)

式中x″——全局坐标系横滚俯仰引起的定位相对位置X轴投影

y″——全局坐标系横滚俯仰引起的定位相对位置Y轴投影

2 自校准变结构Kalman滤波器设计

2.1 基础Kalman滤波器设计

Kalman滤波器是BDS/INS数据融合的常用方法[18-19]。针对BDS失锁、固定解丢失定位误差增大导致的BDS定位数据失效的问题，基于4自由度运动学模型设计了一种变参数Kalman滤波器，以同时适应正常状态下的数据融合功能和失锁状态下的INS定位功能。

首先依据简化二轮车模型和横滚、俯仰相对位置漂移模型(式(1))构建状态估计方程和观测方程[20-21]

(2)

zkal=Hkalxkal+vkal

(3)

(4)

(5)

(6)

(7)

式中xkal——滤波器状态量

ukal——滤波器状态输入量

zkal——状态观测变量

wkal——过程激励噪声

Akal——系统状态转移矩阵

Bkal——系统控制量矩阵

vkal——观测噪声t——离散时间间隔,s

Hkal——观测增益矩阵，为6阶单位矩阵

v——农机行驶速度,m/s

ax′——车辆坐标系x′轴加速度,m2/s

ay′——车辆坐标系y′轴加速度,m2/s

(8)

(9)

当BDS系统失锁时，减少BDS系统数据的权重，依赖AHRS系统数据进行定位滤波观测，等待BDS系统恢复锁定固定解时切换回式(8)、(9)。失锁状态BDS信息噪声增大时减小BDS系统观测值权重，使滤波器系统对卫星定位的依赖降低，定位误差由厘米级提高到米级，定位方差提高100～200倍，所以对BDS观测相关系数进行调整，将BDS观测权重降低至极低的水平，设定失锁状态观测噪声协方差矩阵R′为

(10)

2.2 INS导航系统定位误差不确定度分析

BDS系统失锁后需依赖INS系统实现有限直线导航行驶，这里有限直线行驶是指在直线作业过程中出现发散性跟踪偏差，但是该偏差在作业要求的接受范围内。由于INS导航系统受零位漂移和噪声等因素的影响，运用惯性导航系统方程推算位置存在累计误差。可以通过不确定度(理论正负最大误差)来表示最大误差最大累计程度。从而分析零位漂移和噪声对农机在GPS失锁的状态下保持有限直线行驶的时间和定位误差影响。惯性导航系统方程见文献[22]。

根据惯性导航系统方程可知角速率测量误差以累计方式传递到定位误差上，角速率测量误差w为

w(t)=b+e(t)

(11)

式中b——零偏，主要受温度和环境影响变化缓慢，(°)/s

e(t)——带宽固定的高频白噪声，(°)/s

再根据惯性导航系统仿真模型[22]分析噪声不确定度传递过程。设定行驶速度v为常规播种作业速度1.0 m/s，车辆初始位置为(0，0)，初始航向为0 rad，设角速率测量零偏b不确定度为±0.03(°)/s，角速率白噪声e(t)幅值为±0.05(°)/s，通过仿真模型计算获得100 s内的系统累计误差的不确定度如图3所示。20 s时定位不确定度为±0.1 m，100 s时定位不确定度为±2.6 m。

图3 INS导航不确定度仿真结果

为分析100 s时不确定度与转速标定残差不确定度之间的关系，设定行驶速度v为1.0 m/s，车辆初始位置为(0，0)，初始航向为0 rad，白噪声幅值为±0.05(°)/s，分别设定角速率测量零偏b的不确定度为±0.01(°)/s、±0.02(°)/s、±0.03(°)/s、±0.04(°)/s和±0.05(°)/s。计算获得100 s时定位不确定度分别为±0.9 m、±1.7 m、±2.6 m、±3.5 m和±4.3 m，运用Matlab中corrcoef函数进行相关性分析，相关系数为0.99，显著相关。以相同方法分析角速率白噪声e(t)，幅值分别为0.03、0.05、0.07、0.09、0.11(°)/s，零偏b的不确定度设为±0.03(°)/s。于100 s时，定位不确定度分别为±2.606 m、±2.633 m、±2.656 m、±2.614 m和±2.580 m，相关系数为-0.392 5，相关性不显著。

模型推导和仿真试验结果表明：零偏b对推算定位不确定度的影响远大于高频噪声e(t)。在导航系统校准时着重校准INS传感器零偏b，将会有效提高农机INS续航定位精度。

2.3 INS导航参数自校准方法

由于基于INS的导航系统跟踪误差受初始状态信息和零偏b的影响较大，所以针对这两类参数的校准将有助于控制INS导航误差。

2.3.1基于自回归模型的航向校准方法

INS导航需要定义初始航向，根据BDS报文可以实时获得信号解类，失锁时为非固定解类。以信号解类为失锁标志，在失锁的同时进行初始航向θ0校准，校准方法为10阶自回归(Autoregressive,AR)模型，模型定义为

Xt=a1Xt-1+a2Xt-2+…+a10Xt-10+εt

(12)

式中a——AR模型参数

X——历史BDS航向数据序列

εt——白噪声

本文采用伯格(Burg)算法，依据已知时间序列递推计算AR模型参数，该算法以前项预测误差ef和后项预测误差eb均方误差之和最小为目标求取反射系数Km，再利用反射系数Km递推求解10阶参数an，获得AR模型具体形式。计算方法参见文献[23]。

2.3.2INS传感器角速率测量零偏实时校准方法

BDS数据为正常固定解时，利用定位信息通过时间窗进行INS传感器零速校准和基于一次线性回归最小二乘模型(Least squares matching,LSM)的零偏动态校准。

设计实时滑动时间窗口为30 s，按队列方式定义，新数据载入则删除30 s前的数据，先进先出。时间窗口内BDS的定位数据最远点之间的距离Lmax小于0.1 m且速度v最大值小于0.1 m/s，则判定当前窗口内导航系统为零速，由于该值远大于农机最小转弯半径不可能旋转，根据式(11)可得MEMES传感器零偏为

(13)

其中

式中n——时间窗内数据集数量

k——零偏数组编号

农机导航系统正常工作时处于直线跟踪状态，该状态的特点为行驶目标航向不变。依据该特点设计动态校准方法，由于时间窗口内轨迹近似直线，航向在有限范围内围绕目标航向小范围波动，累计航向变化近似为零，所以同样可以采用式(13)进行转向率传感器的零偏b估计。

判断车辆是否处于直线行驶状态的标准为跟踪轨迹的LSM拟合，采用最小二乘法获得，前期非道路试验表明：直线跟踪状态下30 s时间窗口的高斯投影坐标一次最小二乘拟合R2检验结果均大于0.995，所以判断标准定为R2检验结果大于0.995时导航系统处于直线行驶状态，采用式(13)进行零偏b实时校准，流程如图4所示。

图4 零偏实时校准流程图

由于农机导航系统曲线行驶状态的一般时间较短，卫星天线获取的航向存在误差，用于零偏b校准影响较大，所以非直线状态不进行校准处理。

2.4 自校准变结构位姿信息处理方法

针对正常状态和BDS失锁状态的位姿处理无缝切换的需求，设计了一种自校准变结构位姿信息处理方法。该方法包括变结构参数、INS导航参数校准模块和信息融合Kalman滤波器。INS导航参数校准模块采用的方法如第4节所述。其中Kalman滤波器设计原理如2.1节所述。该方法输入为解类、BDS和INS信号，输出为融合滤波后的位姿信息，具体输入输出信息参数为式(4)、(5)。

其中解类信号为变结构判断标准，来源是BDS实时GPGGA报文，固定解(第4类)为卫星定位正常状态，差分定位误差为±(10+1×10-6D)mm，D为基站到移动站的距离，其他解类均定义为失锁状态，定位误差从米级到亚米级不等，无法用于农业高精度导航控制。当解类为固定解时采用正常状态参数(式(8)、(9))，否则采用失锁状态观测噪声协方差矩阵R′(式(10))。解类同时控制校准模块，正常状态时，为校准零偏和初始航向模块采集历史数据，在失锁同时计算初始航向用于INS定位处理，同时启动零偏补偿。总体结构如图5。

图5 自校准变结构位姿信息处理方法结构图

3 BDS失锁续航机器人平台试验

3.1 BDS失锁续航导航控制器

为构建机器人失锁续航系统，基于BDS失锁变结构位姿信息处理方法设计了BDS失锁续航导航控制器，该控制器能够依据解类信息无缝切换BDS/INS组合导航和INS导航模式，运用自校准变结构位姿信息处理模型进行无缝位姿信息处理，与目标路径相差获得横向偏差和航向偏差，输入到基于深度强化学习网络的导航控制模型中计算出目标行驶曲率[24]。模型输入为BDS、INS和目标路径信息，输出为目标曲率。控制器结构如图6。

图6 BDS失锁续航导航控制器结构图

3.2 机器人导航试验系统

为研究BDS失锁续航方法的性能，搭建轮式差速机器人平台。该平台硬件部分包括轮式差速机器人(Husky Robotics Inc.)和卫星定位系统(Swift company)；定位信息获取频率为10 Hz，水平定位精度±(10+1×10-6D)mm，AHRS惯性传感器(MicroStrain公司 3DM-GX5-25型)，陀螺仪非线性度0.02%，横滚/俯仰精度±0.25°，采样频率30 Hz；控制终端为NVIDIA公司的 Jetson TX1 嵌入式计算机和触控显示屏。软件部分为基于BDS失锁续航导航控制模型的机器人导航控制软件系统，该软件系统运用QT5.0编写加载于Ubuntu 16.04 LTS系统中。RKT-GNSS(BDS)定位信息和AHRS(INS)惯性航姿信息通过串口输入到导航控制系统。该系统将计算获得目标曲率和设定行驶速度，通过ROS机器人操作系统传输给Husky机器人，机器人自动执行目标控制信号实现自动导航行驶。系统结构如图7，试验平台实物如图8。

图7 机器人失锁续航导航试验系统结构图

图8 机器人失锁续航导航试验系统

图9 BDS失锁状态续航性能对比试验实际横向偏差

3.3 BDS失锁状态续航性能对比试验

为研究BDS信号失锁状态下，控制器的无缝处理能力，在机器人系统直线跟踪时人为软件屏蔽BDS定位点信息的输入和给出失锁信号。只记录BDS定位数据而不作为导航控制输入。使用INS递推轨迹作为导航控制器输入进行路径跟踪控制。同时记录BDS数据获得实际轨迹，分析失锁续航导航系统性能。

试验设计：规划直线路径，使用机器人导航系统在草地进行直线导航跟踪，行驶速度设置为1 m/s。当系统进入直线跟踪的稳定状态时，切换系统输入为INS定位数据和非4类解信号。分别采用基于自校准变结构(Self-calibrating variable structure)Kalman滤波位姿信息处理方法(SCVS-K)的BDS失锁续航导航控制器和未校准的变结构(Variable structure)Kalman滤波(VS-K)进行信息处理，失锁续航控制的时间为20 s，每组控制器进行3次重复试验。

3.4 机器人矩形路径(含有转弯环节)BDS失锁状态续航试验

转弯跟踪时航向变化率较大，INS测量误差增大，INS定位航位推算误差累计较快，为测试矩形路径(含有转弯环节)导航失锁续航控制器性能，采用与3.3节直线续航试验相同的处理方法获取BDS数据获得实际轨迹与INS续航轨迹数据，分析失锁续航导航系统性能。

试验设计：规划矩形路径，使用机器人导航系统在草地进行矩形路径导航跟踪，直线行驶部分速度设置为1 m/s，转弯上线部分速度设置为0.5 m/s。当系统进入直线跟踪的稳定状态时，切换系统输入为INS定位续航状态数据和非4类解信号。采用基于SCVS-K方法的BDS失锁续航导航控制器进行信息处理，失锁续航控制对矩形路径进行续航，进行3次重复试验。

表1 BDS失锁状态续航性能对比试验结果

图10 机器人矩形路径BDS失锁续航试验1轨迹

图11 续航试验BDS定位与INS定位差值变化曲线

图12 续航试验实际横向误差变化曲线

表2 机器人矩形路径BDS失锁续航试验结果

4 农机导航系统BDS失锁续航田间试验

4.1 农机导航系统

为验证BDS失锁续航控制方法田间工作效果，将BDS失锁续航控制方法移植到雷沃重工AGCS-Ⅰ自动导航系统中，搭载于雷沃重工M-1104型拖拉机，在雷沃阿波斯智慧农业示范基地的试验田进行田间试验。RTK-GNSS定位模块(司南公司的K728)，定位信息获取频率为10 Hz，水平定位精度±(10+1×10-6D)mm；AHRS惯性传感器(XSENS公司MTi-30 AHRS)，陀螺仪非线性度0.01%，横滚/俯仰精度±0.3°，采样频率25 Hz；前轮转角传感器为BEI-9902120CW型，非线性度为±2%，A/D采样精度为12位；转向执行机构为力士乐公司的HT801053；控制终端为AGCS-Ⅰ控制器和触控显示屏。运用Metrowerks Code Warrior for ARM Developer Suite v1.2进行软件开发。系统结构如图13所示，试验平台实物如图14所示。

图13 农机导航系统试验平台结构图

图14 农机导航系统田间试验

图15 BDS失锁状态田间续航试验实际横向偏差变化曲线

4.2 BDS失锁状态田间续航试验

为研究BDS信号失锁系统的田间导航性能，在农机导航系统直线跟踪时人为软件屏蔽BDS定位点信息的输入和给出失锁信号。采用与机器人试验相同方法，记录BDS数据和INS推算数据获得实际轨迹和递推轨迹，分析失锁续航导航系统性能。

试验设计：规划直线路径，使用机器人导航系统在试验田地进行直线导航跟踪，直线行驶速度设置为1 m/s。当系统进入直线跟踪的稳定状态时，切换系统输入为INS定位续航状态数据和非4类解信号。采用基于SCVS-K方法和未校准的 VS-K方法分别对直线路径进行BDS失锁续航处理，分别进行3次重复试验。

试验分析：农机田间试验中，3组SCVS-K方法失锁续航过程的实际横向偏差ed如图15a所示，3组VS-K方法失锁续航过程的实际横向偏差如图15b所示。对比试验结果如表3，SCVS-K处理方法平均L20 cm达到16.65 m，满足棉花播种失锁应激处理的要求。

文献[12]中采用基于神经网络预测的误差反馈矫正方法，在车辆行驶速度为16 m/s时，失锁时间25 s内位置误差为3.5 m。文献[15]中采用基于立方Kalman滤波器和神经网络的失锁定位方法，速度为1.48 m/s时，500 s内定位误差为23.11 m。在文献[17]的研究中使用MEMS惯性传感器在GPS失锁时进行续航，20 s内误差超过10 m。上述研究没有对分米级精度失锁续航特性进行细分研究。文献[25]采用扩展Kalman滤波器进行拖拉机失锁续航，横向误差达到20 cm时，行驶距离小于10 m。使用本文方法续航，横向误差达到20 cm时，行驶距离大于15 m。

表3 BDS失锁状态田间续航试验结果

5 结论

(1)针对农机导航作业过程中因存在BDS信号失锁而导致突然失控的问题，提出了一种基于自校准变结构Kalman滤波的农机导航BDS失锁续航方法，使操作人员有足够的时间和缓冲距离进行进一步处理，减轻由于突然失控导致的农资损失和额外劳动强度。该方法为棉花播种导航作业过程中BDS失锁应急续航处理提供了研究基础。

(3)根据农机导航系统低速、高精度的特点，进行了BDS失锁续航方法的田间试验，试验结果表明：在实际偏差小于20 cm的条件下，农机在路径上的行驶平均距离达到16.65 m，预留反应距离能够满足农机导航作业需求。

(4)BDS失锁试验结果表明，INS导航航向角速率测量零偏对直线路径失锁续航影响较大，进一步提高零偏的校准效率和精度能够提高直线路径失锁续航质量和可靠性。

(5)机器人矩形路径BDS失锁续航试验表明，系统在转弯环节续航误差迅速增加，并影响后续续航效果，这主要是测量误差、采样率和系统延时等因素导致，进一步研究导航航向和航向变化率的测量、采样、预测和插值方法，能够提高非直线续航质量。