激光雷达机器人车辆地头转向路径规划*

封 永, 张欣欣, 黄林青, 薛金林

激光雷达机器人车辆地头转向路径规划*

封 永, 张欣欣, 黄林青, 薛金林

(南京农业大学 工学院，江苏 南京 210031)

对差速转向机器人车辆进行果园行尾地头转向路径规划研究，通过车辆运动分析，建立车辆运动学模型；进行U型地头转向和K型地头转向的路径生成与分析；采用层次分析法对生成路径优化选择，确定行尾地头转向路径；针对U型地头转向和K型地头转向进行实验验证。对于最佳生成路径对应的实测路径，U型地头转向时方向偏角与横向偏移均值为15.0°,13.7 cm，K型地头转向时方向偏角与横向偏移均值为13.7°，13.9 cm。

激光雷达传感器； 机器人车辆； 行尾地头转向； 路径规划

0 引 言

果园机器人车辆自主导航的主要任务为行引导与行尾地头转向，但是大多数研究集中于果树行内的自主导航，早期有些学者应用电磁导航与机械导航方式实现果园植保机械的自动行走[1,2]。由于在果园中不适宜采用全球定位系统(GPS)导航，目前大多数研究基于机器视觉、激光雷达等技术。冯娟等人利用机器视觉提取树干与地面交点生成果园行内直线导航基准线[3]；有学者基于激光雷达实现机器人车辆在果树行间的直线行驶[4～6]；还有学者采用机器视觉或激光雷达进行果园内弯曲路面的路径检测[7,8]；而Subramanian Vijay等人基于机器视觉与激光雷达开展柑橘园中直线与曲线路径的导航研究[9]；国内外一些学者还利用激光雷达进行障碍物检测，实现果园自主车辆的自动避障[10,11]。

目前，在果园行尾开展地头转向路径规划的研究较少，但是机器人车辆自主完成路径规划并正确地定位以进入下一行是至关重要的。

本文基于激光雷达开展差速转向机器人车辆行尾地头转向路径规划研究，这也是对果园车辆自主导航内容的重要补充。

1 机器人车辆运动学模型

图1为果园行尾地头转向研究用的机器人车辆，该机器人车辆可在果园内巡游进行果园信息采集与管理自动化[12～14]。车辆尺寸为1.2 m×0.6 m×0.75 m(长×宽×高)，轮距d=0.55 m，车轮半径rω=0.19 m。在机器人平台正前方距离地面0.6 m处安装LMS291—S05型激光雷达[15]。激光雷达检测到的果园行尾信息传输至上位机(便捷式计算机)；上位机对数据处理分析后计算转向路径，然后发送指令至下位机(BasicAtom微控制器)；下位机输出脉宽调制(PWM)信号至电机控制器以控制两侧驱动电机的角速度，实现机器人车辆的差速转向。由激光雷达同时获取果园地头宽度D大小(如图2)。上位机根据地头宽度D大小选择不同的转向方式，进行行尾地头转向路径规划。

图2 行尾地头宽度测定示意

建立车辆运动学模型，对其在地头转向过程中运动情况作出假设：1)转向时车速较低，车辆质心在转向过程中不变，忽略瞬时质心偏移和整车转动惯量的影响。2)仅考虑轮胎接地段地面变形阻力和转向阻力的影响，且两侧车轮轮胎的地面变形阻力相同。差速转向机器人车辆模型如图3所示，车辆质心C处坐标为(XC,YC)，位姿向量P=(XC,YC,θ)T。根据运动学知识可得其运动学方程为

(1)

图3 机器人车辆模型

将式(1)进一步整理为

(2)

2 行尾地头转向路径生成与分析

本文以种植行距为1.5m的主干形密植桃园为例开展行尾地头转向路径规划研究，树高不超过2m，树冠半径在1m以内。通过实验得知，当地头宽度D超过1m时，机器人车辆有相对宽阔的空间以U型转向方式进行地头转向；当地头宽度D小于1m时，地头空间较窄，机器人车辆通过三点地头转向，即K型转向。本文以地头宽度1m为界限，对果树地头宽度充足(D>1m)情况和地头宽度不足(D<1m)情况分别进行行尾地头转向路径研究。

通过式(2)可以生成地头转向路径，且生成的路径纯粹与左右两侧车轮角速度有关。因此，机器人车辆采用角速度控制模式，即通过改变车轮角速度ωr，ωl大小控制路径形状。图4为生成的任意地头转向路径。对于差速转向车辆，理论上可以实现原地转向，但实际上会出现较大的滑移。为了减少滑移，同时考虑到机器人车辆最大设计车速，对其地头转向时的左、右两轮角速度进行限定，即左右两轮最小角速度不低于0.5rad/s，最大角速度不高于5rad/s。

图4 地头转向路径

图5(a)为地头宽度充足情况下机器人车辆U型地头转向的生成路径，路径1、路径2、路径3的右侧车轮角速度分别为3，4.25，5rad/s，左侧车轮角速度分别为1.5，2，2rad/s。

图5(b)为地头宽度不足情况下机器人车辆K型地头转向的生成路径。每条路径都由前进转向、后退转向和再前进转向三个转向过程组成。路径4、路径5、路径6前进转向时右侧车轮角速度分别为2.5，2.5，2rad/s，左侧车轮角速度分别为2，1.55，1.5rad/s；后退转向时右侧车轮角速度分别为1.5，1，1.5rad/s，左侧车轮角速度分别为2，1.4，2rad/s；再前进转向时右侧车轮角速度都为2rad/s，左侧车轮角速度都为2rad/s。

图5 U型和K型地头转向生成路径

3 机器人车辆生成路径评价

3.1 路径评价方法

本文采用层次分析法(analytichierarchyprocess，AHP)对所生成的路径进行评价。如图6所示。与前文对应，在地头宽度充足情况下，方案层D包含路径1、路径2和路径3；在地头宽度不足情况下，方案层D包含路径4、路径5和路径6。

各层次因素间的相对重要性分为同等重要、稍微重要、明显重要、强烈重要、极度重要5等级(对应标度值为1,3,5,7,9)。假设某层次中元素ai与元素aj的相对重要性用ai/aj表示，则有

进行全面规划，应充分考虑长远利益与当前利益，积极推进省内重点水利工程建设，适时推动鄱阳湖水利枢纽工程建设；持续推进大型灌区续建配套工程，加强建设和改造中小型灌区，加快排灌泵站更新改造，不断完善灌溉排涝系统；[2，3]大力开展县级以上城市防洪排涝体系建设，以及山洪灾害防治、中小河流治理和鄱阳湖蓄滞洪区安全建设，从而提高设防中心城市防洪标准。

(3)

为了防止层次分析法分析问题产生偏差，需要进行一致性检验，用一致性比例C.R.表示，即

C.R.=C.I./R.I.

(4)

式中 C.I.=(λmax-n)/(n-1)为一致性指标，其中n为判断矩阵的阶数，λmax为矩阵的最大特征值；R.I.为平均随机一致性指标，由查表确定，对于5阶判断矩阵，R.I.=1.12。

当C.R.<0.1时，认为判断矩阵的一致性是可以接受的;否则,应该修改矩阵使之符合一致性要求。

图6 转向路径层次结构模型图

3.2 行尾地头转向路径评价与选择

3.2.1U型地头转向路径评价与选择

构造准则层C相对目标层A的判断矩阵

通过计算判断矩阵的特征值λmax=4.174和特征向量W=[0.567 0.05 0.151 0.232]，判断矩阵一致性比例C.R.=0.066<0.1，说明判断矩阵的一致性可以接受。

构造方案层D各元素相当于准则层C各元素的判断矩阵

方案层D相对于目标层A的权重是通过准则层C相对于目标层A和方案层D相对于准则层C的权重组合而得到，计算结果如表1。路径1,2,3的总权重向量W=[0.138 0.720 0.142]T，即生成路径2为最优路径。总排序一致性比例C.R.为

(5)

式中C.I.i为准则层Ci相应的方案层D中判断矩阵的一致性指标；R.I.i为准则层Ci相应的方案层D中判断矩阵的平均随机一致性指标；ci是准则层C相对目标层A的权重值。层次总排序一致性比例C.R.=0.018<0.1，说明层次总排序一致性可以接受。

表1 转向路径1,2,3总权重

方案C1C2C3C40．5670．0500．1510．232总权重D10．1190．6490．1130．0910．138D20．7470．2790．7090．7580．720D30．1340．0720．1780．1510．142

3.2.2 K型地头转向路径评价与选择

构造准则层C相对目标层A的判断矩阵

通过计算判断矩阵的特征值λmax=4.258和特征向量W=[0.140 0.052 0.250 0.558]，判断矩阵一致性比例C.R.=0.096<0.1，说明判断矩阵的一致性可以接受。

构造方案层D各元素相当于准则层C各元素的判断矩阵

同理，计算出方案层D相对于目标层A的权重，如表2。路径4,5,6的总权重向量W=[0.147 0.727 0.126]T，即生成路径5为最优路径。经计算层次总排序一致性可以接受(C.R.=0.018<0.1)。

表2 转向路径4,5,6总权重

方案C1C2C3C40．1400．0520．2500．558总权重D10．2580．2790．1190．1190．147D20．6370．6490．7470．7470．727D30．1050．0720．1340．1340．126

4 实验与结果分析

实验场地是江浦农场的密植桃园，每次机器人车辆在行中定点出发，初始转向角为0°±1°，初始偏移量为(0±1)cm。在地头宽度D=1.4 m对路径1,2,3进行实验；在地头宽度D=0.8 m对路径4,5,6进行实验。每条路径实验3次，分别记录左右轮角速度、转向时间、转向次数、最终位置(位置与方向角),实验数据如表3。

表3 地头转向路径实验数据

路径实验次数右侧车轮角速度/(rad/s)左侧车轮角速度/(rad/s)转向时间/s转向次数/次最终位置(与行中心线的夹角)/(°)最终位置(与行中心线的距离)/cm1131．57．6213035．21231．57．5712326．31331．57．5512022．4214．2525．1111512．7224．2524．9911210．3234．2525．6111818．231523．8213539．832523．7613336．633523．5013033．1412．5，-1．5，22，-2，213．003-30-36．2422．5，-1．5，22，-2，212．523-25-28．8432．5，-1．5，22，-2，212．983-26-30．0512．5，-1，21．55，-1．4，29．3531515．6522．5，-1，21．55，-1．4，29．3231211．6532．5，-1，21．55，-1．4，29．3531414．4612，-1．5，21．5，-2，212．5532528．8622，-1．5，21．5，-2，212．3132225．0632，-1．5，21．5，-2，212．4532326．3

注：车辆纵向中心线偏向果树行中心线左侧为正，反之为负；车头偏向左为正，反之为负；角速度值为负表示车辆倒退行驶。

根据表3，在3次实验中机器人车辆都能够在地头宽度充足情况下以U型转向生成转向路径1，2,3，及在地头宽度不足情况下以K型转向生成转向路径4,5,6，很好地完成果园行尾地头转向操作，尽管有不同程度的方向偏角与横向偏移，这主要是因为转向中存在微小的滑移。在U型转向时路径2的方向偏角与横向偏移均值都最小，分别为15.0°与13.7 cm；在K型转向时路径5的偏角与偏移均值都最小，分别为13.7°与13.9 cm。根据上位机对各层次因素相对重要性的分析结果，在U型转向中路径2的总权重最大，为0.720；在K型转向中路径5的总权重最大，为0.727。

5 结 论

针对差速转向机器人车辆开展了果园行尾地头转向路径规划研究，分析了在行尾地头宽度充足与不足情况下的行尾地头转向路径，并通过层次分析法对生成路径进行优化选择。对果树行尾地头宽度充足或不足的情况分别进行实验验证。结果表明:尽管有一定的方向偏角与横向偏移，但是机器人车辆能很好地完成果园行尾的地头转向。

[1] Torii T.Research in autonomous agriculture vehicles in Japan[J].Computers and Electronics in Agriculture,2000,25(1-2):133-153．

[2] Gabriely Y,Rimon E.C-space characterization of contact preserving paths with application to tactile-sensor based mobile robot navigation[C]∥2008 IEEE International Conference on Robotics and Automation,Pasadena,CA:IEEE,2008:1792-1797.

[3] 冯 娟,刘 刚,司永胜,等.果园视觉导航基准线生成算法[J].农业机械学报,2012,43(7):185-189,184.

[4] 刘 沛,陈 军,张明颖.基于激光导航的果园拖拉机自动控制系统[J].农业工程学报,2011,27(3):196-199.

[5] 薛金林,张顺顺.基于激光雷达的农业机器人导航控制研究[J].农业机械学报,2014,45(9):55-60.

[6] Barawid O C,Mizushima A,Ishii K,et al.Development of an autonomous navigation system using a two-dimensional laser scanner in an orchard application[J].Biosystems Engineering,2007,96(2):139-149.

[7] 林桂潮,邹湘军,罗陆锋,等.改进随机样本一致性算法的弯曲果园道路检测[J].农业工程学报,2015,31(4):168-174.

[8] 陈 军,蒋浩然,刘 沛,等.果园移动机器人曲线路径导航控制[J].农业机械学报,2012,43(4):179-182,187.

[9] Subramanian Vijay,Burks F T,Arroyo A A.Development of machine vision and laser radar based autonomous vehicle guidance systems for citrus grove navigation[J].Computers and Electronics in Agriculture,2006,53:130-143.

[10] 郝永军.果园打药机自动行走控制系统研究[D].保定:河北农业大学,2013.

[11] Freitas G,Hamner B,Bergerman M,et al.A practical obstacle detection system for autonomous orchard vehicles[C]∥IEEE/RSJ International Conference on Intelligent Robots and Systems,Algarve,Portugal,2012:3391-3398.

[12] 朱磊磊.果园管理机器人平台的自动导航系统研究[D].西安:西北农林科技大学,2010.

[13] 王海波,刘凤之,王孝娣,等.我国果园机械研发与应用概述[J].果树学报,2013,30(1):165-170.

[14] 董希玲.苹果果园管理信息系统的构建[D].保定:河北农业大学,2010.

[15] 王 斌,刘昭度,何 玮,等.车用测距雷达研究进展[J].传感器与微系统,2006,25(3):7-9.

Path planning of end-of-row turning for robot vehicle based on laser radar*

FENG Yong, ZHANG Xin-xin, HUANG Lin-qing, XUE Jin-lin

(College of Engineering,Nanjing Agricultural University,Nanjing 210031,China)

Path planning of end-of-row turning for robot vehicle with differential steering in orchard is researched.Kinematics model is established in light of an analysis on movement for robot vehicle.U-turn and K-turn paths are generated and analyzed according to model.Analytic hierarchy process(AHP)is adopted to select generated paths, thus to choose the optimal path for end-of-row turning.Experiments are carried out to verify in the case with U-turn and K-turn.For the measured paths corresponding to the optimal generated paths,mean value of direction angle and lateral shift are 15.0°,13.7 cm for U-turn and 13.7°,13.9 cm for K-turn.

laser radar sensor; robot vehicle; end-of-row turning; path planning

10.13873/J.1000—9787(2017)02—0028—04

2016—02—24

中央高校基本科研业务费专项资金资助项目(KYZ2011)； 江苏省自然科学基金资助项目(BK20151436)

TP 242

A

1000—9787(2017)02—0028—04

封 永(1987-)，男，硕士研究生，主要研究方向为车辆自主转向智能化。

薛金林(1974-)，男，通讯作者，教授，主要从事农业智能化研究工作，E-mail:xuejinlin@njau.edu.cn。