基于宏微结合的田间作业机器人路径规划.pdf

2023年9 月第54卷第9 期农报业机械学doi:10.6041/j.issn.1000-1298.2023.09.002基于宏微结合的田间作业机器人路径规划郑路张啸王建国吴悦李海涛（中国农业大学工学院，北京10 0 0 8 3）摘要：农业机器人对推动农业现代化加速变革和实现智慧农业有重要作用。高精度定位技术是保障机器人安全高效完成各类作业的基础，而作业路径准确规划是实现农业场景下导航的核心。针对田间作业机器人复杂环境下因测绘误差与局部障碍进而造成作物损伤率较大这一问题，本文提出一种基于宏微结合的路径规划算法，该算法首先基于作业区域宏观测绘信息生成全局静态作业路径，同时利用雷达传感器对机器人局部作业环境进行实时动态监测进而生成局部动态最优路径，将全局静态路径与局部动态路径进行有机融合以实现作业路径优化修正，保障田间作业的顺利进行，最终应用MPC算法控制机器人对规划后的路径进行追踪。经试验验证，当机器人田间作业两侧安全距离分别为0.2、0.1m时，本算法可将作业过程中平均作物损伤率由3.40 58%、1.2 7 6 3%降低到0.6772%、0.18 8 9%，保证了机器人作业的安全可靠，为大田稳产条件下的高效作业奠定基础。同时，本算法提升了精准农业要求下田间作业精度，对实现农业高产高效高质目标有重要意义。关键词：农业机器人；路径规划；宏微结合；全局静态；局部动态中图分类号：S24文献标识码：A文章编号：10 0 0-12 9 8（2 0 2 3)0 9-0 0 13-14OSID：Path Planning of Field Robot Based on Macro-micro CombinationZHENG LuZHANG XiaoWANG JianguoWU YueLI Haitao(College of Engineering,China Agricultural University,Beijing 100083,China)Abstract:Agricultural robot plays an important role in accelerating transformation of agriculturalmodernization and achieving intelligent agriculture.Field positioning and navigation technology is thefoundation for ensuring the safe and efficient completion of various agricultural tasks by robots,andcombining high-precision positioning information of robots to achieve efficient planning of work paths isthe technical core of field positioning and navigation.A path planning algorithm based on macro-microintegration was proposed.Firstly,the algorithm generated a global static work path based on the macromapping information of the operation area.While implementing robot operations,various radar sensorswere used to dynamically monitor the micro work environment and path information of the robot in realtime.Finally,path tracking algorithms such as MPC were applied to real-time process local and globalwork environment information to achieve real-time job path optimization and correction to ensure thesmooth progress of field work.Experimental verification showed that when the adjustable distances onboth sides of the robot during field operations were 0.2 m,0.1 m respectively,the algorithm can reducethe average crop compaction rate during the operation process from 3.405 8%and 1.276 3%to0.677 2%and 0.188 9%.Meanwhile,the algorithm improved the precision of field operations under therequirements of precision agriculture and had important significance for achieving the goal of high yield,efficiency,and quality in agriculture.Key words:agricultural robot;path planning;macro-micro combination;global static;local dynamic0引言随着智慧农业的不断发展，农业机器人作为解决农村务农人口骤减、农业人力成本上涨的关键技术而备受关注1。高精度定位与导航是农业机器人进行自主田间作业的先决条件，在导航作业中，机收稿日期：2 0 2 3-0 4-14修回日期：2 0 2 3-0 6-2 1基金项目：国家自然科学基金项目（52 0 7 2 40 7）作者简介：郑路（19 9 0 一），男，讲师，博士，主要从事农业智能装备研究，E-mail：1.z h e n g 5 c a u.e d u.c n通信作者：李海涛（19 6 8 一），男，教授，博士，主要从事农业智能装备研究，E-mail：h 8 9 533 c a u.e d u.c n农142023年机业报学械器人首先需确定自身的绝对或相对位置以进行后续路径规划和轨迹追踪等各项工作2-3。绝对位置多指机器人所在位置的全球定位系统坐标，其获取主要依靠全球导航卫星系统（Globalnavigation satellite system，G NSS）。随着自动驾驶行业的蓬勃发展，搭载载波相位差分技术（Realtimekinematic，R T K）的定位系统协同以卡尔曼滤波为代表的滤波器可将定位精度维持在分米级范围，足以满足田间作业需求4-5。相对位置是指作业过程中机器人相对于作物或障碍物等标志的位置，对相对位置的获取主要依靠以激光雷达、毫米波雷达和视觉系统等为主的多种传感器，在条件允许的情况下定位精度可达厘米级甚至毫米级6-8 。位置信息得以获取后，对农业机器人作业路径进行合理规划是决定农业机器人能否顺利完成相关作业的另一重要环节。根据对所需信息和适用场景的不同，田间作业路径又分为全局静态路径和局部动态路径9 。全局静态路径的规划主要依托作业区域特征、作物种植特征等现有信息，以机器人运行总里程最少、路径重复率最低为原则对田间作业道路进行静态规划，且规划后路径信息无法实时改变10-1。局部动态路径的规划依托相对位置信息，在一定区域以不与障碍物发生碰撞为前提规划出最优路径，可减小环境中存在的未知障碍物对机器人作业的影响12-13 针对田间作业环境，FAUST等14 提出一种分层式机器人路径规划方法PRM-RL,通过强化学习代理完成短程障碍躲避的任务，然后利用基于采样的规划器映射这些代理从而进行可靠的路径规划。JENSEN等15 提出一种农业作业中涉及田内和田间运输的单元路径规划方法，结合时间或行驶距离的优化准则,生成一个“度量地图”并通过Dijkstra算法解决了单源路径规划问题。魏爽等16 提出了农机自动导航路径搜索方法和预瞄点搜索的纯追踪模型,可以得到较好的导航与路径追踪控制精度。刘刚等17 提出了一种基于GNSS 的农田平整自动导航路径规划方法，将全局路径规划和局部路径规划相结合进而实现路径规划。上述的路径规划方法在全局静态路径和局部动态路径规划上进行了区分，位置信息仅应用于对应路径的规划，无法将两者进行融合。由于全局路径对高精度测绘信息即绝对位置信息的依赖较大，测绘精度将直接影响规划得到的路径质量，当机器人位于田间作物行间特殊位置时，局部路径规划可能会导致机器人运行方向发生错误，造成重复作业甚至迷失方向的问题。因此，全局静态路径需要进行实时修正，局部动态路径需要指明每一时刻的运动趋势，通过对位置信息的交叉利用来保障机器人田间作业的正常运行。本文提出“全局+局部”的宏微融合路径规划模式。全局静态路径规划为机器人运行指明关键方向与整体运动趋势，动态规划依托激光雷达对作业环境进行实时感知，通过对作业行与作物特征点的提取进行相对定位，配合一定的修正系数确定机器人行驶行两侧作物的相对位置并规划局部动态路线。通过对两种路径的实时融合可避免因种植规范化程度低，测绘误差大等因素所带来的静态规划路线偏差较大的问题，为机器人指明运动方向并保障运动趋势的正确性。最后，基于模型预测控制（M o d e l p r e d i c t i v e c o n t r o l，M PC）算法实现对机器人运行路径的实时追踪。1全局静态路径规划1.1绝对位置（经纬度）坐标获取绝对位置的精确获取是实现精准作业的先决条件，包括路径追踪在内的多个环节均对位置精度有较高要求。不同于海拔落差幅度较大的丘陵山地，大田作业地面平整度较高，因此海拔变化可忽略，仅保留经度与纬度作为绝对位置。在忽略海拔变化的情况下,绝对位置坐标表现为由经度和纬度所组成的二维坐标。组合导航作为在自动驾驶中常用的导航方式，被广泛应用于各种搭载定位系统的载具上，主要指利用GNSS和惯性测量单元（Inertialmeasurement unit，IM U）为代表的多种传感器进行松/紧/深组合，通过不同传感器之间的相互配合，获得更为精准的载体绝对位置信息，并由计算机对载具进行控制以完成制导18 。GNSS 频率较低，但定位长期精度高，基本不受地域、时间限制，在空旷无遮挡，载体运动平稳的理想环境下具有较好的稳定性，可以提供可信度较高的位置信息19 ，在引人RTK技术后理论定位精度可达厘米级2 0 。考虑到农业作业场景的特殊性，农业机器人常工作在遮挡较少的开阔环境，多径效应和材质反射所带来的影响更小,对RTK等定位设备更为友好2 1。IMU频率高，短期累计误差小，可获取短时间内较为准确的位置信息。在自动驾驶领域，两种传感器进行融合后通过频率特性互补可以在一定程度上降低定位误差，从而提升定位精度。何秀凤等2 2 提出了一种基于UD分解的快速卡尔曼滤波算法，给出了IMU/GPS组合系统的软硬件设计和试验结果，相较于单一传感器定位，融合定位精度大幅提升。张百强等2 3 设计了基于RaoBl a c k w e l l i s e d 无迹卡尔曼滤波（RB15郑路等：基于宏微结为田间作业机器人路径规划第9 期UKF)的组合导航算法，显著提升了组合导航系统GPS信号中断再恢复后的滤波精度，且易于工程实现。经试验验证，利用现有组导算法足以满足农业作业精度需求。1.2作业区域路径规划1.2.1路径规划范围确定组合导航大大降低了定位误差，可以使机器人获取到满足作业要求的绝对位置信息。为明确机器人全局静态路径规划的区域范围，本文面向二维平面内田间作业进行全局静态路径规划，不考虑地势变化对作业的影响。图1为作业平面及作业区域特征点选取示意图，首先对作业区域特征点进行选取，并通过多种测绘手段相结合的方式，再对作业区域特征点的经纬度信息进行获取和确认，进而构建多边形作业区域并确认机器人作业范围。工作区域特征点330m工作区域特征点420工作区域特征点510工作区域特征点2010203040工作区域特征点1X/m图1作业区域特征点及作业平面Fig.1Operating area feature points and operating plane受田间道路铺设、灌溉系统搭建等因素的影响，在实际农田中，作物种植区域周边往往会存在一圈非种植区域，如图2 所示，此区域无法承载机器人的作业路径。为此，考虑到地图精度和潜在误差，需将作业范围划分为种植区和非种植区两部分2 4。如图3所示，其中黄色区域为非种植区,绿色区域为种植区。实际农田中非种植区内外两边的距离或内外顶点间的距离多为定值，在路径规划中可作静态参数直接参与规划运算。通过定义“种植区域特征点”，即图3中红色点，与作业区域特征点相对应，最终确定机器人在种植区作业的全局静态路径规划范围。非种植区域图2实际农田情况Fig.2Actual farmlandconditionsYA种植区域特征点3种植区域特征点4种植区域特征点5种植区域特征点2种植区域特征点1X图3种植区域与非种植区域Fig.3Planted and non-planted areas1.2.2路径规划安全区域确定作业规划区域范围内作物有序种植，种植行列之间均留有一定的间隙，可利用此间隙对路径规划范围进行细分。以玉米大田为例，在种植区内，依托于北斗卫星定位系统进行的精准播种使得玉米行距和株距得以保持在一个特定值上下小幅度波动。因此种植区内每列玉米可以近似拟合为矩形区域，如图4中深绿色矩形所示。矩形长边矢量方向即每列玉米与坐标轴的夹角在不同种植要求的农田中差别较大，需要通过参考点给出。每两行玉米之间的距离称为玉米种植行距。图4对玉米大田进行了模拟，由种植区和矩形区域选择出来的浅绿色部分即为机器人可以正常通过的安全区域。为保证机器人工作时对作物的影响或破坏程度最小，机器人行驶路径必须限制在安全域内。3020F1010203040机器人出发点X/m图4玉米大田模型Fig.4Model of corn field机器人未工作时，假设其位于种植区域左下角点，该点可作为机器人运动和路径规划的出发点，在进行田间作业时，需要明确的主要参数为机器人的行驶方向和速度，综合考虑作业精度和作物损伤程度，机器人应沿矩形长边延伸方向行驶，且在直线行驶过程中速度可近似认为是确定值。因此，路径规划应结合作物矩形区域特征，通过给出机器人行驶方向，进而确定在图4所示农田中作业时的行驶轨迹，为保证作业效率，避免重复作业造成的能源浪费等问题,最佳行驶路径重复率为0，即路径中没有二次经过的区段，这也是将机器人运动和规划的出发点定在左下角点的原因之一。农162023年报机业学械1.2.3全局静态路径规划在圈定机器人行驶的安全区域后，需对全局静态路径进行规划。规划原理为在安全区域内，标定种植方向后在垂直方向上衍生一定数量的等距直线。如图5所示，等距直线与种植区边界会存在多个交点，这些点便是轨迹引导点，且轨迹引导点应出现在安全区域内。对点进行顺序选择后，利用折线连接即可得到规划后的路径。轨迹引导点图5等距直线与边界线交点Fig.5Intersections of equidistant lines and boundary lines种植方向决定了规划后路径直行段的方向，且种植区形状同样会影响规划效果，如图6 所示，等距直线与种植区边界线相交出现交点，对于种植区形成的多边形而言，如果为凸多边形，会与直线存在一个或两个交点，当直线上有两个及以上的点落在凸多边形内，便会出现两个交点，当直线上只有一点落在多边形内且刚好为顶点时便只有一个交点。但对于凹多边形而言，第1种情况可能导致交点多于两个，此时路径规划方法需要进行调整，以上便是作业区域形状对规划的影响。本文以凸多边形作业区域为例进行规划。图6凸多边形和凹多边形区域Fig.6Convex and concave polygonal areas在确定第1行/列作物的种植方向后,基于后续种植方向均与第1行/列平行的假设，需要计算给定区域最大允许作物种植行数即等距线条数。通过遍历种植区域特征点，得到最大距离后与行距相除且向下取整即可得到最大允许行数，计算式为-maxN：(1)H其中Lmx=max(L;)(2)L,=(x,-x,)+(y2-yr)2(3)其中式中L;特征点间距离L特征点间最大距离maxTT一H一行距N一约给定区域最大允许作物种植行数如图7 所示，为对点进行顺序选择，方便路径形成，将等距线分为上行线和下行线两类，上行线即路径矢量方向在Y轴的分量为正值，下行线即路径矢量方向在Y轴的分量为负值，为保证总路线长度最短，规划后的路线应当为蛇形折线，即上行线和下行线交替出现。上下行线与边界线的交点即为轨迹引导点，为确定折线路径中交点出现的顺序，利用上下行线斜率特征进行判断。在图7 所示情况下，针对上行线，由于先出现的交点应为上行线与下方边界线的交点，后出现的是与上方边界线的交点，即点1、5、9 先出现,2、6、10 后出现，下行线恰好相反。为计算交点，必须得知边界线和上下行线的直线特征，具体表现为K和b值。YA上行线108X下行线图7上行线和下行线Fig.7Uplines and downlines如图8 所示,边界线由种植区特征点确定，在计算时有顺时针计算和逆时针计算的区别，为达到交点有序，在计算交点坐标时，与上行线特征匹配的是逆时针边界线特征，与下行线特征匹配的是顺时针边界线特征。如此往复，分别计算不同等距线的交点即可实现交点有序化。Y个逆时针计算顺时针计算一11一X图：顺时针计算和逆时针计算Fig.8Clockwise calculation and counterclockwise calculation轨迹引导点确定后，路径规划便可以实现。以上提出的引导点有序化方法是在种植方向直线特征17第9 期郑路子等：基于宏微结合的田间作业机器人路径规划K为正值的前提下实现的，如果种植方向直线特征K为负值，则上下行特征与边界线特征顺逆匹配恰好相反。依据上述全局静态路径规划原理，经规划后最终形成的路径如图9 所示。图9全局静态路径规划演示Fig.9Global path planning demonstration2基于激光雷达的局部动态路径规划针对实际农田种植和生产要求，提出了基于GNSS+IMU组合导航的全局静态路径规划方法，即依托高精度位置信息，获取机器人作业范围，结合实际农艺，通过多种测绘手段获得大田种植参数后，由系统自动规划得到一条重复率最低的作业路径，在误差允许范围内，全局静态路径规划可以给定机器人运动的大致轨迹。考虑到土壤起伏程度，作物种植行距波动，测绘精度等因素的影响，机器人在田间作业时依靠单一的全局静态路径规划显然不足以支撑自动驾驶的正常运行。其次，全局静态规划通常需要在已知环境中进行，即上述“人为划定”，属于一种事前规划，可以找到最优解，一旦环境发生变化，未能及时更新地图，或由于对已知环境把控的误差，便无法保证作业效果。以玉米大田为例，玉米行距维持在50 6 0 cm范围内，拟定机器人宽度为40cm，由于GNSS+IMU组合系统可以提供的精度范围只能稳定维持在分米级，随着行驶过程中累计误差的增大，可能出现机器人行驶轨迹与实际种植区重叠即发生碰撞进而导致局部作物损伤的不利情况。为解决这一问题，引人局部动态路径规划并对全局静态路径进行实时修正显得尤为重要。局部动态路径规划通常用在未知或部分未知的环境中，系统根据传感器实时获取到环境障碍物的信息，并做出相应规划，在设定的规划阈值范围内，参照全局静态路径规划做出的初步判断，进行再判断，矫正纠偏，这对系统的实时计算处理能力有着较高的要求。但若缺乏全局环境信息，在一些特定场景中其规划效果也同样不尽人意。两者的宏微结合，协同融合，可大幅提高机器人运行的精准性和平稳性。2.1点云数据剪切基于现有的局部动态路径规划算法，本文提出一种基于激光雷达的规划方法。在作物种植行间，通过单线激光雷达对某一合适高度的平面进行扫描，同时对不符合计算要求的区域进行剪切处理，通过保留的数据可以计算在当前环境下机器人与作物茎秆的相对位置和自身位姿，对茎秆特征点（位置坐标)进行曲线拟合后，规划出布局最优路径，协同全局静态路径并对其进行局部修正，做出最终规划。单线激光雷达工作原理如图10 所示，激光雷达原始输出经处理后成为2 个数组，分别记录0 36 0 范围内每一个环境反射点的距离和角度信息，经可视化处理后最终显示效果如图10 所示。00,360%270900180(a)思岚S1雷达(b)雷达扫描演示图10雷达扫描可视化Fig.10Radar scanning visualization在实际应用中，由于安放位置不同以及机器人外形限制，0 36 0 的扫描信息往往有一部分需要剪切。假设雷达置于机器人最前方，故其扫描范围为9 0 2 7 0，而2 7 0 36 0 和0 9 0 范围的点云数据由于设备遮挡等因素被裁剪。在消除机器人自身障碍部分对雷达测距产生的影响的前提下，减小数据计算量，方便更快地做出规划。2.2范围值设定点云数据经过裁剪之后，还要设定扫描范围阈值，因为雷达的扫描半径远大于作物种植行距，雷达发出的激光难免会透过作物株间的空隙，照射到非当前行驶行的作物，采集异常信息，进而对局部动态规划造成干扰。范围阈值的设定保证了机器人进行行间作业时，局部动态路径规划的依据始终为左右两侧最近作物种植行/列而避免出现漂移和越界规划行为，进而导致局部动态路径规划异常。如图11所示，范围阈值设定的计算过程如下：设L，为作物种植的行距，计算式为L,=L2+2L4+L3(4)式中L2作物拟合矩形宽度L3一机器人宽度L4机器人边界到作物拟合矩形内边距离L4可以作为机器人运行安全距离。图中机器人恰好位于左右两侧作物列中央，此时式（4）成立。在计算范围阈值时，需要将机器人置于边界位置，如图12 所示，范围阈值由最大距离Lmax和最小距离Lmin组成，其计算式为农2023年报机18学械业L雷达1图11相对位置及距离标定Fig.11Relative position and distance calibrationLLminmax送图12范围阈值计算Fig.12Range threshold calculation1L(5)+min22一(6)max2假设在某一时刻机器人受到影响，行驶轨迹存在偏移，导致机器人偏离正常航线，紧贴右边作物拟合矩形，即安全距离为0,存在碰撞和碾压作物风险。此时雷达被安置于机器人中线位置处，扫描范围L应设定在（Lmin，Lma x）区间内，参照式（4）和图13可将点云数据进行处理，可得L=rangecos(degree)(7)其中range、d e g r e e 均为雷达内置数据格式。Lreerange点云雷达数据(a)雷达扫描原理(b)点云分布情况图13点云裁剪Fig.13Point cloud cutting经过范围阈值设定后，最终呈现的点云分布如图13所示，可以看到雷达只保留了两侧阈值范围内的点云数据并进行显示，不合适的角度和点云均被剪切。2.3特征化识别完成点的筛选后，当前点云数据间除了位置信息以外没有任何差别，没有一个点可以作为局部动态路径规划的特征点或参考点参与运算，故需要对单一的点云数据进行再筛选，目的是找出符合规划思路的特征点并标定。在实际的大田中以玉米为例，玉米机械化种植后存在规定的行距和株距，在进行全局静态路径规划时依托玉米大田全局特征进行计算和匹配，可以得到图14中的红色路径，但在进行局部动态路径规划时更关心图中微观特征。(a)全局特征(b)局部特征图14玉米大田种植情况Fig.14Corn field planting conditions在同一行/列内，每株玉米之间都有一定距离，称为株距，雷达进行扫描时，株距的存在会导致扫描到非当前行驶行的作物信息，对规划造成干扰，可以通过雷达角度和点云裁剪对其进行修正。行驶行两侧的玉米会被雷达扫描到并可视化呈现，图14即为局部动态规划时的微观情况，红色箭头指向为行驶方向。进行局部动态路径规划时，由于雷达扫描特性，在某一特定角度下会首先收到距离最近点的距离信息，规划的首要目的是定位玉米茎秆的位置，但由于不同品种的玉米节距，叶片数量、分布、大小等情况均不相同，可能出现在扫描高度有叶片遮挡，进而无法准确定位茎秆的问题。因此需要对多种玉米生长情况调研，并对生长特征类似的玉米进行三维模型重建,如图15 所示。如图16 所示，将雷达扫描高度的玉米生长情况拟合为双同心圆，图中红黄色同心圆即代表作物，红色表示茎秆位置，黄色表示玉米下部，茎秆周边可能出现的叶片，只要机器人行驶过程中不会与红色部分发生碰撞即可。该模型将叶片考虑在内，雷达扫描得到的点经拟合认为是圆弧，通过计算找到圆心即茎秆位置，避免出现因叶片干扰导致茎秆定位失误，造成规划错误的问题，机器人两侧矩形框为作物拟合范围，浅绿色矩形框代表雷达扫描范围，虚线箭头代表雷达发出的激光，与雷达不同距离的植株经扫描后，反馈的信息19郑路等：基于宏微结的田间作业机器人路径规划第9 期(a)玉米整体模型(b)玉米局部细节特征图15玉米三维扫描模型Fig.15Three-dimensional scanning model of corn雷达图16扫描平面示意图及特征圆拟合Fig.16Scanning plane diagram and characteristiccircle fitting并不完全相同，以紫色弧线表示反馈信息，即扫描到的玉米特征信息。紫色弧线由多个点云数据组成，任选其中4个点可以拟合一个特征圆，取多个圆心均值标定为玉米茎秆位置，即红色同心圆圆心位置。通过对作物三维建模着重对雷达扫描平面即玉米下半部分进行分析。由图15b可知，在扫描平面内，玉米叶片可拟合成一个半径为R,的圆，同理，茎秆区域也可以拟合为一个半径为R,的圆，在理想情况下，两圆的中轴线会重合，现实情况中往往会出现长度为d的间隙。在误差允许的范围内，d可不做考虑。玉米茎秆位置确定之后便可以进行局部动态规划，假设扫描后有2 m个符合的特征点（茎秆位置）且分布在机器人左右两侧，两两配对并取中点坐标，符合要求的中点有个,采用最小二乘法对曲线进行拟合，选择多项式拟合模式2my=ao+a,x+azx+.+amx(8)离散的各点（特征点中点）到这条曲线的平方和F(ao,ai,.,am)为F(ao,ai,am)=Z y:-(ao+ax+a2x+.+amx)2(9)对F(ao,ai,am)分别求对a4的偏导数,得到m+1个方程-2y:-(a+a:+a2x+a)=0ni=i-2y-(ao+ajx:+a2x+.+a.x)x:=0-2Zy:-(a+ajx:+a2x+am)x=0(10)将其整理为针对ao，a i，,a m的正规方程组，最终结果为n2aon+al;+a2m+aym=i=1nnn2ao;+a1a23+i=1i=innm+1,二a;y;mi=1i=innaomam+1a2m+2+n2mxyimam(11)利用高斯消元法求解即可得到o，a，,m，拟合曲线如图17 所示。40402020001-20020406080-20020406080X/mX/m(a)拟合曲线1(b)拟合曲线24040r2020001-20020 406080-200204060 80X/mX/m(c)拟合曲线3(d)拟合曲线4图17最小二乘法拟合曲线Fig.17Fitting curves of least square3全局与局部规划的宏微融合基于测绘信息和雷达扫描信息，全局静态路径规划与局部动态路径规划得以实现，为优化机器人田间作业效果，两者的宏微结合是极为重要的环节。显然，全局静态路径与局部动态路径的区别在农202023年机报学业械于规划范围即路径对机器人指导范围的不同，全局静态路径更多的对机器人大范围内（如整块作业田）的行驶方向进行限定，而局部动态路径更倾向于对行间直行前提下的轨迹纠偏进行指导。受限于田间测绘信息的精确度，如果只将全局状态下规划得到的作业路径直接投人使用会产生累积误差进而影响作业质量，同理，虽然局部动态路径可以为机器人行间作业提供高精度路径信息，却无法在诸如起始行、转角、终止行等特殊位置处给出合理路径，且极易出现错误规划的问题。因此通过全局静态与局部动态路径规划的宏微融合，即作业过程中，机器人的作业行驶方向始终由宏观状态下的全局静态路径给出，而当机器人顺利通过特殊作业位置，行驶在作物行内时，依托微观状态下的局部路径对其进行更为精细，作物损伤率更低的行驶指导，宏微融合从而完成高质量田间作业。在机器人操作系统（Robot operating system，ROS）中，路径作为Path消息被发布并显示在可视化工具（Robot visualization tool，R VIZ）中。按照种植规律首先放置虚拟作物，利用Python中高斯分布函数np.random.normal在保证不违背种植规律的前提下对作物位置进行微调以模拟实际作物种植情况。利用宏微结合算法，将宏观路径和微观路径进行融合后得到如图18 中的路径效果。绿色实线表示规划路径，红色带箭头虚线表示全局静态规划路径。显然，宏微结合路径相较于全局静态路径和局部动态路径而言更贴合实际田间作业场景，更符合实际生产需要。(a)全局静态路径规划(b)局部动态路径规划()局部动态路径规划图18路径规划仿真Fig.18Path planning simulation田间作业往往会受到不同障碍物阻挡等情况，通过增大虚拟作物体积将其中某一作物定为如树木、水井等的障碍物。在障碍物存在的情况下,若行间仍允许机器人通过，即当前作业行间最小宽度大于机器人宽度，则出现如图18 的规划路径；若无法保证机器人安全通过，则绿色路径会在障碍物处终止，而红色路径不受影响。由此可见宏微融合对实际作业中保证安全、高质量作业的重要性。4琴基于MPC算法的路径跟踪为保证机器人在实际作业时可以对宏微融合后的路径进行有效追踪，使其运行轨迹最大程度上贴合规划后的路线2 5-2 6 ,可基于 MPC算法对这一环节进行实现。MPC算法的作用机理可以表述为，在每一个采用时刻，根据当前获得的测量信息，在线实时求解一个有限时间下开环优化问题，并将得到的控制序列的第1个元素作用于被控对象。在下一个采样时刻，重复上述过程：用新的测量值作为此时预测系统未来动态的初始条件，刷新优化问题并重新求解。相对于线性二次型调节器（LQR)方法而言，MPC算法可以考虑空间状态变量的各种约束，而LQR、PID 等控制算法只能够考虑输人输出变量的各种约束2 7 试验所用农业机器人为两轮差速机器人，其底盘为两轮差速底盘，由两个轮毂电机作为动力轮，后部由一个万向轮支撑。基于左右轮速度变化进而实现转弯和前进动作。在ROS独有的两轮差速模型中，机器人运动学模型描述为x=vcos0(12)j=vsing(13)(14)式中机器人横向速度，m/s机器人纵向速度，m/s机器人航向角，radW-机器人航向角速度，rad/s为方便实际应用，将上述连续的微分模型进行离散化处理，成为差分模型，差分间隔定义为dt。则k+1时刻的机器人横向、纵向位移与航向角便可由k时刻的对应量与差分间隔内的微小变化量相加得到，可得Xk+1=x,+V,cos0,dt(15)yk+1=yk+U,sino,dt(16)0k+1=0+W;dt(17)角与真实航的差值,则有e表示k时刻MPC算法规划路径上对应点的航向刻下MPC算法规划的路径点与真实路径点距离差，设机器人运行偏差敏感方向为Y方向，e表示k时定义横向跟踪误差e（m）与航向误差e（r a d）并假考虑到路面情的位移偏差21郑路等：基于宏微结田间作业机器人路径规划第9 期式中-k时刻机器人横向位移，m-k时刻机器人纵向位移，mk-k时刻机器人航向角，radk时刻机器人航向角速度，rad/se:=f(x)-yk(18)e=arctan(f()-0)(19)其中f(x）为机器人运行到k时刻时真实路径的位置在Y方向上的投影，相应的，arctan（f（）为k时刻的航向角，同样在离散化处理后有ek+1=ek+Vesino,dt(20)+1=eh+dt(21)通过对机器人运动学模型进行分析并计算离散后的位移与误差，再根据实际应用场景对目标函数进行设置与优化,最终可以得到基于MPC的控制函数，以目标函数最小为原则，可得到N个预测节点以指导机器人的运行。如图19 所示,在ROS中,利用C+编制了部署于虚拟机平台的仿真环境。在引入机器人统一机器人描述格式（Unified robot description format，URDF)模型后，机器人运动基础参数如轮径、轮距等均被定义，之后导入机器人运动学模型与对应的MPC控制函数并以图18 中的仿真路径为基础进行MPC算法仿真试验。其中绿色路径为融合规划路径，红色轨迹为MPC算法对机器人前方1.5m范围内实时规划所得路径。以机器人当前位置为原点，将运行方向上前1.5m内的融合规划路径与MPC实时规划路径离散化后得到如表1所示的位置点，路径效果图如图2 0 所示。在MPC算法控制下，1.5m内的最大横向误差为0.0 2 8 m,1.0m内的最融合规划路径MPC下实时规划路径图19基于MPC算法的路径跟踪仿真Fig.19Path tracking simulation based on MPC algorithm大横向误差为0.0 0 6 m，可见，合理选择MPC算法中动态窗口可以保证良好的跟踪效果。表1MPC规划路径离散化坐标点Tab.1MPC planning path discretizationcoordinate pointsm融合路径离散化MPC规划路径离散化离散点距离坐标点坐标点10.05(0.023,0.05)(0.023,0.05)20.10(0.017,0.10)(0.017,0.10)30.15(0.028,0.15)(0.030,0.15)40.20(0.028,0.20)(0.030,0.20)50.25(0.022,0.25)(0.029,0.25)60.30(0.024,0.30)(0.024,0.30)70.35(0.007,0.35)(0.009,0.35)80.40(-0.008,0.40)(-0.002,0.40)90.45(-0.012,0.45)(0.012,0.45)100.50(-0.020,0.50)(0.020,0.50)110.55(-0.028,0.55)(0.030,0.55)120.60(-0.021,0.60)(0.027,0.60)130.65(0.019,0.65)(-0.022,0.65)140.70(-0.008,0.70)(-0.008,0.70)150.75(0.017,0.75)(0.013,0.75)160.80(0.025,0.80)(0.025,0.80)170.85(0.032,0.85)(0.032,0.85)180.90(0.034,0.90)(0.036,0.90)190.95(0.037,0.95)(0.037,0.95)201.00(0.031,1.00)(0.036,1.00)211.05(0.029,1.05)(0.034,1.05)221.10(0.034,1.10)(0.034,1.10)231.15(0.035,1.15)(0.032,1.15)241.20(0.037,1.20)(0.031,1.20)251.25(0.031,1.25)(0.026,1.25)261.30(0.021,1.30)(0