葡萄园履带式移动平台自动导航系统的设计与试验

李伟，杨术明，李茂强，王乾

(1.宁夏大学机械工程学院，宁夏 银川 750021；2.北方民族大学机电工程学院，宁夏 银川 750021)

宁夏是酿酒葡萄的最佳产区之一，贺兰山东麓酿酒葡萄种植面积约3.53万hm2，且种植面积逐年扩大，年产量20万t，综合产值达166亿元[1].宁夏酿酒葡萄种植面积约占全国种植面积的四分之一[2]，酿酒葡萄的园间管理包含起藤、打药、除草、剪枝、灌溉、埋藤等生产作业环节，但随着酿酒葡萄种植面积的逐渐扩大，与其生产配套的自动化生产设备却相对滞后[3]，一定程度制约了酿酒葡萄产业的发展.为了推动宁夏酿酒葡萄作业装备的发展，研究宁夏酿酒葡萄园环境下的自动导航系统，对于实现酿酒葡萄场景下智能化作业具有重要意义.

农业环境下的自动导航是智能农业方面的重要研究内容，现有的自动导航技术主要通过卫星导航、激光雷达导航、超声波导航和视觉导航等实现[4].Diego等基于RGB-D摄像头和深度学习，研究了葡萄园自动导航运动规划器[5].Guy等[6]研究了用于葡萄园喷雾机器人的定位和导航数据融合算法.Backman等[7]研究了一种农业机械导航系统，通过二维激光雷达提取导航跟踪线，采用GPS和惯性测量单元识别机具的前进方位.谢永良等[8]应用滚动优化原理，以AGV偏差作为输入获得前轮期望导向角，提出轮式AGV沿葡萄园防碰撞避障算法.宋广虎等[9]基于深度学习网络模型分割葡萄园前景，优化葡萄园行间路径检测，提取导航基准线.杨舜禾等[10]基于GPS和激光雷达研究出一套葡萄园拖拉机自动导航系统，能够较为快速的修正导航横向偏差.

现有的导航技术对葡萄园生产作业适应性不强，由于葡萄生产伴随起藤和埋藤等复杂作业，对路面平整性破坏较大，容易造成机具作业的横向偏差，同时葡萄园各生产环节的精细化操作又需要快速修正横向偏差，避免碰撞篱架.针对此类问题，研究葡萄园履带式移动平台自动导航系统，能够快速修正环境造成的横向偏差，沿着规划路径实现自动导航，解决目前葡萄生产作业的导航需求，促进葡萄产业智能化发展.

1 材料与方法

1.1 导航系统总体方案设计

宁夏的葡萄多为篱架种植模式，篱架高约1.8 m，每行宽约3 m，葡萄藤依附于篱架生长，移动平台在两行篱架间进行作业.采用全局导航方式，确定葡萄园内的导航起点A及导航终点B，且规定葡萄园内主要路线的选择，将导航起点A、主要路线和导航终点B的路径作为自动导航的基准线，导航基准线示意如图1所示.

图1 导航基准线示意图

根据葡萄园导航作业需求，将自动导航系统从宏观上分为环境感知层、决策控制层和运动执行层，环境感知层进行对外界环境信息的感知，决策控制层对获取到的环境信息进行路径规划的决策，控制运动执行层实现导航运动，移动平台导航系统总体方案如图2所示.

图2 导航系统总体方案

1.2 导航系统硬件选型

根据导航系统总体方案对硬件进行选型，履带式移动平台整车分为机械部分和电气部分，机械部分由底盘部分和载体部分组成，电气部分由电源部分和电路部分组成，履带式移动平台总体组成如图3所示.

图3 履带式移动平台总体组成

移动平台采用两种控制模式，分别是自动导航模式和手动控制模式，2种控制模式可互相切换[11].葡萄园篱架种植模式呈行状态，且葡萄园道路坑泥低洼、高低起伏，结合作业的工作强度及环境条件，移动平台性能参数如表1所示.

表1 移动平台性能参数

1.3 环境感知层研究

1.3.1 数据获取 环境感知是导航系统研究的前提，分析导航传感器特性，选取激光雷达作为导航系统的环境感知传感器.激光雷达传感器为自动导航系统提供环境信息，如今激光雷达可以在葡萄园内大范围的静态环境下做到同步定位与地图构建，围绕导航系统的建图与定位展开研究，激光雷达扫描环境示意如图4所示.

图4 激光雷达扫描环境示意图

1.3.2 同步定位与地图构建 导航系统订阅多传感器信息并发布地图和位置信息，配置成对应的工作模式则调用Cartographer核心算法[12]，葡萄园建图与定位算法框图如图5所示.三维激光雷达扫描的一系列的子图构成了葡萄园地图，通过回环检测来减少长时间累积的误差，全局优化构建葡萄园地图[13]，其中葡萄园导航地图中白色区域为工作区域，黑色区域为葡萄篱架区域，而灰色区域是未探测区域.

图5 建图与定位算法框图

二维激光雷达通过感知道路信息，从而确定自身位置.激光雷达拟合篱架如图6所示，建立移动平台的动态坐标系OXY，其中点(XL,YL)和点(XR,YR)表示拟合直线与篱架的交点，lL和lR表示葡萄园拟合的左右两侧种植行边界线.激光雷达与左右篱架拟合越好，则表明定位越好，为了获取准确位置信息，需要估计移动平台的位置和姿态，通过不断地运动和测量，逐渐降低自身位置的不确定性[14]，获取移动平台所处葡萄园环境中的准确位姿信息，保证环境感知层稳定且可靠.

图6 激光雷达拟合篱架示意图

1.4 决策控制层研究

决策控制层是导航系统研究的核心，在此基础上，决策控制层将获取到的环境信息进行路径规划.导航路径规划主要有两个层次的模块组成，分为全局路径规划和局部路径规划，全局路径规划将导航路径直接规划至终点，局部路径规划跟随全局路径并对遇到的障碍实时进行规避，这两个模块组成了葡萄园作业的路径规划部分.

1.4.1 全局路径规划 全局路径规划采用图搜索的方法，引入了图的概念可以很好的计算葡萄园地图中导航点之间的路径规划问题[15]，生成一条从起点A到终点B的葡萄园全局路径.全局路径规划的本质上是一种贪心算法，通过二维激光雷达感知葡萄园环境信息，在已知环境中使用迪杰斯特拉算法进行全局路径规划[16]，确定葡萄园2个基本要素，分别是顶点a和边s，用V来保存导航起点A到各个路口区域转向点的最短距离，用数学语言描述G为：

G=(V，E)

(1)

假定葡萄园篱架和藤为直线且平行等长，若对于导航起点A存在能直接到达的边s，则把其纳入数组S中，同时把其余所有移动平台不能直接到达的路径长度假设为无穷远.通过算法的每一次迭代，直到路径中包含了地图中所有顶点，且抵达了导航终点B，这样所有到达的边就组成了导航的全局路径.

1.4.2 局部路径规划 在全局路径规划把移动平台拉向导航目标终点时，局部路径规划则实时根据路况施力进行避障.局部路径规划引入TEB算法，施加外力改变局部路径以实时规避障碍[17]，局部路径规划示意如图7所示.

图7 局部路径规划示意图

局部路径约束的2个目标分别为跟随已知全局路径和根据实时路况进行避障[18]，这种约束使得移动平台以若干弧段组成的平滑的轨迹运动.定义移动平台的横纵位置和姿态，则空间内序列为：

Q={xi}i=-0,1,…,nn∈N

(2)

移动平台运动时两个位姿间的时间间隔定义为ΔTi，来表示移动平台由一个状态运动到下一个状态所需时间，则时间序列为：

τ={ΔTi}i=0,1,2,…,n-1

(3)

移动平台在平面运动具有三个自由度，将位姿及时间序列合并后，通过加权多目标优化获取最优的路径点，结合导航全局路径，实时规划出一条无碰撞的葡萄园局部最优路径，实现导航作业的路径规划.

1.5 运动执行层研究

运动执行层是导航系统研究的关键，控制移动平台跟踪规划的路径，实现自动导航运动.根据葡萄种植模式，分析移动平台的运动状态，建立移动平台运动学模型，研究导航系统转向控制算法.

1.5.1 建立运动学模型 为了更好地适应葡萄园作业环境，移动平台选取履带式运动方式，分析移动平台的运动状态，从几何学角度考虑移动平台的运动规律，使其在导航过程中满足几何约束.移动平台采取左右履带轮差速运动方式，在此基础上，基于移动平台的运动学特性建立了相关运动学模型[19]，运动学模型及转向示意图如图8所示.

图8 运动学模型及转向示意图

坐标系OXY表示惯性坐标系，点C(x，y)表示移动平台的中心点坐标，θ表示移动平台的行驶方位角，移动平台左右履带的速度分别表示为VL和VR，左右履带转动角速度分别用ωL和ωR表示，移动平台运动状态可用Xt表示：

Xt=[xt,yt,θt]T

(4)

通过运动学分析，假定移动平台驱动履带与地面接触运动为纯滚动无滑动，也没有侧移情况下，移动平台的运动学模型可以表示为：

(5)

对移动平台的行驶过程中的转向模型进行简化，分析建立移动平台转向示意图，θτ表示平台行驶方位角，Φ表示平台中心点转向夹角，通过控制左右履带轮的速度实现导航运动的转向.

1.5.2 运动状态分析 移动平台采用差速驱动方式，由于左右履带轮速度VL和VR存在速度差关系，通过控制左右履带轮的驱动速度，可以实现移动平台不同的运动状态，所以移动平台具有不同的运动状态.当左右履带轮分别以VL>VR、VL=VR和VL=-VR的速度运动时，移动平台可以实现曲线运动、直线运动、旋转运动，为导航系统提供了转向运动基础，移动平台运动状态如图9所示.

图9 移动平台运动状态图

1.5.3 转向控制设计 移动平台转向的被控对象是所分析的左右履带轮，为了更一般的描述平台运动，直观的控制量是运动学建模中的左右履带轮速度.转向控制采用比例积分微分控制(PID)的方式[20]，控制系统主要由控制器、执行机构、被控对象等组成，运动执行机构控制框图如图10所示.导航系统控制决策给定速度信息，由左右电机控制实现移动平台的转向运动，编码器反馈实现导航系统的闭环控制，以此来控制跟踪导航路径线.

图10 运动执行机构控制框图

控制移动平台由当前点导航到指定目标点，其核心是令移动平台在控制器作用下，持续地沿规划路径朝向导航目标点运动，这样可实现葡萄园导航至目标点的运动控制过程，移动平台导航运动示意如图11所示.

图11 导航运动示意图

2 结果与分析

围绕导航系统进行关键技术研究，为验证导航系统的可行性，开展葡萄园环境下履带式移动平台自动导航系统试验，选取宁夏贺兰山东麓酿酒葡萄种植基地作为试验场地，试验地点环境如图12所示.

图12 试验地点环境图

葡萄园行间距约为3 m，葡萄藤高约为1.8 m，葡萄园路口宽约5 m.移动平台初始航向与葡萄种植方向偏差不应大于45°，试验预先规定移动平台位于导航线的右侧时横向偏差为正，左侧时为负.开展履带式移动平台自动导航系统性能试验，葡萄园导航试验如图13所示.

图13 葡萄园导航试验

根据葡萄园机具生产作业的速度，试验选取以0.5 m/s速度和1 m/s速度下的导航效果.记录导航横向偏差数据，根据葡萄园导航试验横向偏差数据，导航试验横向偏差如图14所示.

A：以0.5 m/s速度下导航横向偏差图；B：以1 m/s速度下导航横向偏差图；C：以0.5 m/s速度下导航横向偏差图；D：以1 m/s速度下导航横向偏差图.

导航横向偏差控制在10 cm范围内具有一定的可行性，由试验横向偏差图可知，移动平台分别以0.5 m/s和1 m/s的速度试导航时，导航初始位置与与中心路径存在一定偏差，但在行驶约10 m后，消除横向偏差驶入导航中心路径，并且当移动平台遇到坑洼地势偏离预定路径时，能够消除横向偏差驶入导航中心路径.

分析导航试验横向偏差数据，移动平台分别以0.5 m/s和1 m/s速度导航时，平均横向偏差分别为3.4 cm和3.6 cm，最大横向偏差分别为9.1 cm和9.8 cm，均方差分别为4.2 cm和4.2 cm，试验数据统计如表2所示.

表2 导航系统试验横线偏差统计

3 讨论与结论

为了更好地实现葡萄园环境下作业的自动导航，解决复杂地形造成导航偏离路径后能快速修正偏差，以免碰撞篱架等问题.以履带式移动平台导航控制为研究对象，研究了一套葡萄园自动导航系统，通过葡萄园导航试验，认为该自动导航系统能够较快的修正导航横向偏差，实现葡萄园内自动导航系统路径规划及转向控制，具有一定的可行性.

分析试验结果可知，移动平台分别以0.5 m/s和1 m/s速度导航时，导航平均横向偏差分别为3.4 cm和3.6 cm，最大横向偏差分别为9.1 cm和9.8 cm，均方差分别为4.2 cm和4.2 cm.导航横向偏差控制在合理范围内，当移动平台遇到坑洼地形出现较大的位置偏差时，导航控制系统能较为快速、准确地修正横向偏差，使移动平台驶入中心路径.

围绕自动导航系统进行研究，目前虽取得一定程度上成果，但分析试验数据可知，导航的最大横向偏差主要出现在速度较高的情况下.为了确保导航系统在葡萄园环境下具有更好的准确定及稳定性，保证导航路径与中心线拟合更优，本文后续将对平台的导航控制算法进行优化研究，提高移动平台路径拟合性及运行稳定性，以更好地适应葡萄园自动导航作业需求.