基于视觉与惯性的农机组合导航的方法研究

马志艳，欧阳方熙，杨光友，余昌舜

(湖北工业大学 农业机械工程研究设计院，武汉 430068)

0 引言

现代农机自动导航系统一般由检测单元、控制单元、执行单元及监控单元4部分组成，主要任务是对农机进行横向位置控制，使其与预设路径之间的横向误差保持在一定的精度范围之内，满足农业自动化生产的需要[1]。农机作业在复杂多变、非结构化的野外农田环境中，每种位置测量方法都有一定的局限性。例如，基于全球导航卫星系统GNSS的导航方法应用成本较高，且信号传输易受外界环境干扰；基于机器视觉导航方法受视觉传感器分辨率、光照及作物生长状况的影响，导航路径易受干扰，从而导致定位失效[2]；基于惯性导航方法受传感器积分误差的影响，导航误差随时间的累积而增加，从而使得导航失效。采用组合导航的方式将各种导航信息进行融合，能够实现各导航方式优势互补，提升导航系统的检测精度和稳定性。

目前，市场上已有的农机导航系统大多基于GNSS。随着组合导航技术的发展，不同的导航信息组合已经应用到组合导航技术中，并取得了一定成果。如Nagasaka等研究的DGPS和陀螺的组合导航系统，例如，0.7m/s 的车载试验中，直线行走误差控制在12cm以内，但转弯误差较大[3]。Gao JC等基于卡尔曼滤波算法的GPS/INS车载式传感器组合导航算法，对比单独使用GPS信号的方法，在GPS信号中断40s的情况下，可提升定位精度15%[4]。Gao GJ等提出的INS辅助高精度GPS导航方法，能够在GPS信号受到较大干扰的情况下，提供有效的定位信息[5]。Wang等提出的基于模糊逻辑和专家系统的INS/ GPS卡尔曼滤波组合方案，证明了该方法的可行性[6]。高为广等设计的GPS/INS组合导航自适应滤波算法，能有效地控制观测异常和状态扰动异常对参数估计的影响[7]。相对于GNSS/INS组合导航系统，视觉/INS组合导航系统具有成本低、体积小、易维护等优点。视觉传感器除了能提取出导航信息之外还能提供丰富的感知信息(如作物生长情况、障碍物及实时路况等)，这为农机进一步的自主导航提供了可靠的信息来源。

本文以棚室电动微耕机为试验平台，在Raspberry Pi 3-B开发板上搭载JY901高精度惯性导航模块及320×480像素USB摄像头构建视觉/SINS组合导航系统。该导航系统一方面利用惯性测量模块，通过姿态矩阵将加速度计在载体坐标系下的检测量转换到地理坐标系中，进而积分得到载体在地理坐标系下的位置和速度信息；同时，利用视觉测量模块得到载体在导航坐标系下的位姿信息；最后，通过卡尔曼滤波对两类数据进行融合，实现导航信息的最优估计。通过微耕机实际行驶数据进行仿真，验证了算法的有效性。

1 组合导航系统架构

1.1 组合导航系统结构设计

导航系统按功能可分解为导航测量模块和导航控制模块：导航控制模块由导航控制器和执行机构组成；导航测量模块由视觉检测模块、惯性检测模块组成。该系统以电动微耕机为试验平台进行车载试验，系统框架如图1所示。

图1 组合导航系统框架示意图Fig.1 System framework of Integrated navigation

导航测量模块利用陀螺仪和加速度计这两个惯性测量单元，通过运动学方程获得农机在地理坐标系下的位姿信息，同时从视觉模块获取在导航坐标系下的位姿信息，采用卡尔曼滤波器进行信息融合，从而获得最终农机位姿信息，并且对惯性导航的累积误差进行修正。导航测量模块将当前农机位姿信息的最优估计同期望跟随路径对比，通过导航控制模块完成农机对期望路径的跟随任务。

1.2 组合导航硬件组成

组合导航系统采用Raspberry Pi 3-B微型电脑为核心的嵌入式计算机，通过USB接口搭载JY901高精度惯性模块和320×480USB摄像头视觉模块，构成导航信息融合的主要硬件框架。该系统基于农机运动学分析，将多传感器的导航信息进行融合，估计出农机的位置、速度和航向信息，并将这些信息换算成电机的控制信号，最后通过Raspberry Pi 3-B微型电脑的I/O接口将控制数据发送到相应的电机驱模块实现农机自动导航，各硬件模块如图2所示。

1.RaspberryPi 3-B微型电脑 2.JY901惯性导航模块 3.USB摄像头 4.电动微耕机

2 农机运动学分析与建模

2.1 农机工作路径描述

在农机实际作业过程中，主要以完成农田环境中的直线行走任务为主。本文借鉴郭孔辉等提出的“预瞄-跟随”驾驶员建模理论[8]，选用位置偏差d和航向角偏差ψ两个变量，作为导航控制变量及导航过程中载体的位姿变量。其中，位置偏差为车辆行驶轨迹同预期规划路径的距离误差，航向角偏差为车辆行驶航向角同预期路径方向的夹角[9]。

假设农机的期望路径为既定直线，并以该直线为导航参考坐标系，通过视觉模块实时检测该直线相对载体的位姿信息，通过惯性模块实时检测载体相对起始点的位姿信息。在利用惯性模块测量信息时，需要把信息转换到参考坐标系下，再与视觉模块检测结果进行信息融合[10]。载体在参考坐标系下的位姿示意图如图3所示。

图3 载体位姿描述示意图Fig.3 Position and orientation of vehicle

2.2 视觉检测位姿确定

将相机与车体固定，并且使相机坐标系的yc-oc-zc平面垂直于地面，俯视角为φ。车载坐标系原点位于相机坐标系原点oc与地面的投影点o′轴指向载体的纵轴方向，Z轴方向垂直于地面向上，Y轴方向指向车辆方向的左侧。组合导航坐标系示意图如图4所示。

定义[xcyczc1]T为相机参考系下的坐标，[xwywzw]T为车载坐标系下的坐标，L为两坐标系原点的距离。相机坐标系与车载坐标系之间的转换关系为

(1)

如图5所示，导航线在车载坐标系下的直线表达式可表示为

(2)

其中，d为载体坐标系原点到导航线的距离，并且近似将其当作载体到导航线的偏离；ψ′为导航线与载体坐标系Z轴的夹角。

图5 导航路径在载体坐标系的投影Fig.5 Projection of the target path on the vehicle coordinate

导航线在相机成像平面的投影示意图如图6所示。由式(1) 、式(2) 可得导航线在xcocyc投影坐标系的投影表达式为

(3)

同时，在导航线L在图像中的投影为Lc，其表达式可写为

(4)

其中，λ为投影坐标系中导航线投影Lc到坐标原点的距离；ψc为投影坐标系中导航线投影Lc与Y轴偏角。由式(2)～式(4)可知，导航线L在平面o′x′z′的表达式为

(5)

在图像中容易得到导航线的投影与xc轴的截距a，又因xw、zw有无穷组解，解得

tanψ'=tanψc·sinφ

(6)

(7)

图6 导航路径在相机坐标系的投影Fig.6 Projection of the target path on the camera coordinate

由此，建立了图像检测模块对车辆载体的位姿描述方程。

2.3 微耕机运动学分析

通常微耕机工作在在平整的农田环境中，垂直方向上的变化可忽略。因此，由图7可得简化的水平面位姿推算模型为

(8)

其中，Vn为载体沿导航路径法向方向的速度；ax'和az'分别为载体沿载体坐标系x轴和z轴的加速度；ω为载体沿载体坐标系Y轴的角速度。公式中，ax'、az'和ω均可以由惯性导航模块测量得到，如图7所示。

图7 载体二维运动示意图Fig.7 Two-dimensional motion analysis of vehicle

陀螺仪和角加速度计测得的角速度和加速度存在着测量误差，将加速度和角速度的测量值表达为

(9)

其中，ωc、ax′c、az′c为陀螺仪和加速度计在X、Z轴的测量值；ω、ax′、az′为对应的真值，wω、wax′、waz′为对应的测量噪声。由式(8)、式(9)可得水平面位姿推算带测量噪声的方程，即

(10)

3 基于卡尔曼滤波的组合导航

3.1 卡尔曼滤波

卡尔曼滤波是一种递推的状态空间方法。在离散过程中，该方法利用状态方程描述系统状态的转移过程，并结合上次的最优估计获取系统状态的当前估计；利用观测方程获取状态的观测值，再结合二者得出当前的最优估计。它只用状态的前一个估计值和最近一个观察值，就可以在线性无偏最小方差估计准则下对当前状态做出最优估计。其递推公式[11]为

(11)

其中，Kk为卡尔曼增益矩阵，Pk/k-1为预测误差协方差矩阵，Pk为估计误差协方差矩阵，Rk为测量方差矩阵,Qk-1为过程方差矩阵。

3.2 组合导航系统方程

经典的卡尔曼滤波只适用于线性且满足高斯分布的系统，但此时的系统并不是线性的，需要应用扩展卡尔曼滤波(EKF)来解决这种情况。设置系统的采样时间为Δt，在估计状态值处进行泰勒级数展开，忽略二次级以上的高阶项，一阶线性化处理后的系统状态方程[12]为

Xk=ΦXk-1+ΓW

(12)

由系统的视觉测量模块的测量变量可确定，卡尔曼滤波的测量向量为

(13)

根据选定的状态向量和量测向量可得系统的量测方程为

Zk=HXk+V

(14)

其中

根据状态方程(12)和测量方程(14)，即可结合前面的卡尔曼滤波公式(11)进行扩展卡尔曼滤波器的设计，进而对系统的状态进行估计。

4 仿真与试验

4.1 仿真初始条件

1)系统噪声和测量噪声均为高斯白噪声，陀螺漂移设为均值0.01(°)/h、标准差为0.005(°)/h的随机偏差，加速度计零偏值设为10-2g(g=9.5m/s2)、标准差为5×10-3g的随机偏差。

2)卡尔曼滤波初始条件：X0取0向量，则

P0=diag{(0.2)2m,(0.2)2m/s,(0.001672)°}

(15)

Q=diag{(5×10-3)2m,(5×10-3)2m/s,(0.00082)°}

(16)

人工控制载体在预订的路径上行驶，并实时采集行驶过程中的导航数据，同时对真实的行驶轨迹进行采样、拟合。将卡尔曼滤波器融合的结果与行驶轨迹的拟合结果进行对比，即可得到试验结论。

4.2 仿真结果分析

实际试验过程中，选择较为平整的温室大棚田地作为场地，设定车辆作业的行进速度为1m/s，信息采样的周期为200ms，保证预设路径的图像始终出现在相机的成像平面中。整个试验过程，车辆的作业距离大约为50m。对采集的一组数据进行实验仿真，计算结果表明：该算法能保证组合导航系统状态收敛，有效提高导航的精度。图8为融合导航信息与实时行驶参数的对比结果。其中，实线为对行驶轨迹真实采样点的拟合，由图8(a)到图8(c)分别记录了载体行进过程中，载体相对于目标导航线的法向偏移、法向速度和行偏角。由图8可以看出：虽然载体在复杂的农田环境中行驶轨迹、车身姿态难以达到理想状态，但融合的导航数据基本能快速收敛于真实数据且具有较高精度。其中，法向偏移的最大误差不超过10cm，航偏角的最大误差不超过1.0°。

图8 仿真最优估计与真实状态对比图Fig.8 Result of simulation and actual

5 结论

介绍了一种农机多传感器组合导航方法。该方法通过建立以导航线为参考系的位姿描述方程及对载体进行运动学建模，完成了基于视觉和惯性传感器组合导航系统平台搭建，并采用卡尔曼扩展滤波方法进行导航信息融合，能够实时可靠地计算出车辆的位姿与速度信息。实验结果表明：该方法在一定条件下能较好地满足农机作业高精度自动导航的要求。