基于改进双向RRT*的果园机器人运动规划算法

刘 慧 张世义 段云鹏 贾卫东 沈 跃

(1.江苏大学电气信息工程学院，镇江 212013; 2.江苏大学农业工程学院，镇江 212013)

0 引言

随着我国果园种植面积的不断扩大，果树种植正朝着规范化、标准化和规模化的方向不断发展，果园自主化作业的重要性日益凸显[1-3]。近几年人工智能、自动驾驶的迅速发展，也促使果园机器人朝着自主化、无人化作业方向快速发展[4]，果园机器人的广泛应用可以有效减轻农机操作人员的劳动强度，提高作业精度和作业效率[5-6]。

果园机器人自主化作业效率主要依赖于机器人运动规划所生成的作业轨迹。机器人运动规划是指在环境先验信息已知的情况下，生成一条从起始点到终止点的无碰撞且符合机器人运动学约束的作业轨迹。目前广泛使用的全局路径规划算法主要分为以A*算法为代表的基于搜索的路径规划算法[7-8]和以快速随机扩展树(RRT)算法为代表的基于采样的路径规划算法[9-11]。

RRT算法由LAVALLE[12]于1998年提出，该算法对整个环境空间进行采样，具有搜索快速、概率完备等特点，但是在复杂的地图中搜索效率低且采样点利用率低。针对此问题，一些学者提出多种RRT算法的改进方法。为提高搜索效率，偏向RRT[13]、RRT-Connect[14]、双向RRT[15]被提出，但是仍存在路径过于曲折且绕远的问题，不利于后期轨迹优化；KARAMAN等[16]提出RRT*算法，该算法具有渐进最优性，路径的长度随着迭代次数的增大而变短，最终只能无限逼近最优路径，存在着收敛速度慢的问题；JORDAN等[17]提出Bi-RRT*算法，在RRT*基础上，结合RRT-Connect和启发式的思想，保证RRT*渐进最优的同时也加快了路径收敛速度，但在复杂环境下节点计算量较大;刘奥博等[18]提出目标偏置双向RRT*算法，在双向RRT*基础上采用目标偏置策略，提高了搜索效率，但是生成路径存在尖峰状况；张伟民等[19]提出基于目标约束采样和目标偏置扩展的改进RRT*算法，并用三阶B样条进行平滑处理，提高了狭窄通道等特殊环境下的路径规划效率和平滑性，但是没有考虑到在路径转弯处机器人的曲率约束。

本文以果园作为环境地图，结合机器人运动学模型将离散果树转换为连续狭窄走廊模型，并根据此环境特点提出一种基于动态末梢节点导向和势场导向采样的双向RRT*算法。在寻路结束后路径回溯过程中，对已生成路径点进行路径点去冗余和相邻折线段转角约束处理，最后结合机器人曲率约束通过三阶准均匀B样条曲线[20-21]对已知路径点进行轨迹优化。

1 改进双向RRT*算法

1.1 传统双向RRT*算法

传统双向RRT*算法[15]将RRT*算法和RRT-Connect算法相结合，在保留RRT-Connect算法搜索快速、概率完备特点的同时，也继承了RRT*算法渐进最优的优点。但是，传统双向RRT*算法自身也存在着稠密障碍物或连续多层走廊环境下采样点利用率低、搜索效率低和生成路径折线多的缺点。

1.2 基于动态末梢点导向的偏置采样

本文针对带有障碍物的多重狭窄走廊的果园环境，对双向RRT*偏置采样算法做进一步改进，将静态目标点偏置导向优化为动态末梢目标点偏置导向，并重新定义偏置规则。

图1呈现了动态目标偏置导向的双向扩展RRT*搜索树扩展示意图，黄色树代表从起点生长的搜索树，用Ts表示，ninit是其根节点；蓝色树代表从终点生长的搜索树，用Te表示，ngoal为其根节点。为提高搜索效率，两棵树分别同时从节点ninit和节点ngoal进行扩展。在扩展树生长过程中，为提高搜索树的目标导向性，设置目标偏置概率pgoal(pgoal∈[0,1])，并设置随机概率p(p∈[0,1])，当p>pgoal时，在空间中随机产生一个采样点nrand；当p≤pgoal时，以末梢节点nlast(搜索树上距离根节点最远的节点)作为随机采样点nrand，即

(1)

如图1所示，针对目标导向，文献[19]采用的是红色虚线箭头所示方向，本文采用的是绿色虚线箭头所示方向，将对方搜索树上的末梢节点nlast作为目标约束偏置节点，当p≤pgoal时，本文方法为：

图1 以末梢节点为目标导向的偏置采样示意图

(1)将nlast作为采样点nrand，并根据扩展步长确定点B，若点C到点B无障碍物，则将点B作为新节点nnew来扩展搜索树。

(2)若点C到点B之间有障碍物，则调整搜索树扩展方向，按照由D到C的方向进行扩展，过点B作垂线垂直DC于点E，若点C到点E之间无障碍物，则将点E作为新节点nnew来扩展搜索树。

(3)若以上全不成立，则重新采样(resample)。

综上可得

(2)

1.3 基于势场导向采样点偏置处理

为进一步提高采样点的利用率并提高路径搜索效率，结合人工势场法思想[22-24]，在采样时利用势场导向将落在障碍物区域的采样点沿着该点势场下降方向排斥至无障碍区，以实现采样点再利用。本文将果树和障碍物所在区域均看作障碍物区域，根据喷雾机器人本体尺寸对障碍物膨胀化处理，保证机器人与障碍物间的安全区域，障碍物膨胀尺寸Lexpend为喷雾机器人宽度Wcar的一半,即

(3)

障碍物膨胀后喷雾机器人被理想化为一个点。

人工势场法中合势场是由引力场和斥力场叠加得到，本文所使用的势场导向法只需要考虑斥力场。将障碍物区域设为高势场，非障碍物区域设为低势场，并设置一个安全距离，在安全距离外不受斥力场影响。落在障碍物区域的采样点在斥力的作用下会被排斥到非障碍区。

根据人工势场法建立斥力势场函数

(4)

式中Q*——斥力势场临界值，Q*∈R

q——双向RRT*随机采样点

η——斥力势场增益(决定势场变化快慢)

D(q)——采样点距最近障碍物中心的距离函数

Urep(q)——采样点处的斥力势场

斥力势场梯度函数为

(5)

1.4 回溯路径点处理

改进双向RRT*算法生成的原始路径点由一系列稠密稀疏不均但有序的点集组成，且不满足阿克曼运动学曲率约束。因此，在进行轨迹优化前，需要对已生成路径做两点处理——路径点去冗余和相邻折线段转角约束。

1.4.1路径点去冗余

从实际应用角度分析，并不是每个路径点都是必需的，冗余拥挤的路径点可以被舍弃掉，使用具有代表性且可以保留原有路径信息的少数点代替。

本文对路径点去冗余所遵循的条件为

(6)

式中 Δd——相邻路径点距离

Δθ——相邻折线段夹角

dthr——相邻路径点距离阈值

θthr——相邻折线段夹角阈值

当满足式(6)时，根据给定阈值动态修改此段路径点，路径点修改公式为

(7)

式中ni——第i个待修改的路径点

n′i——第i个修改后的路径点

ni-1,i——从第i-1个待修改点指向第i个待修改点的向量

ni,i+1——从第i个待修改点指向第i+1个待修改点的向量

1.4.2相邻折线段转角约束

阿克曼底盘具有非完整性约束的特点，可以将其理想化为自行车模型进行分析，如图2所示。若要改变航向，需要满足曲率约束条件

图2 车辆自行车模型

(8)

式中κ——转弯曲率α——前轮转角

L——轴距αmax——前轮转角最大值

因此，在换行转弯情况下，对曲率不满足喷雾机器人曲率约束条件的路径点进行修正，通过在转弯起止点之间插入路径点来约束每次转弯角度，从而减小转弯曲率。

在进行相邻折线段转角约束的同时，也要考虑碰撞检测，以保证生成的新路径点以及相邻路径点的连线均在可行区域内。碰撞检测示意图如图3所示，矩形ABCD表示障碍物区域，EF表示任意一段路径段，若线段EF上存在一点M，满足

图3 碰撞检测示意图

(9)

式中lAB、lAD——路径AB、AD长度

nAM、nAB、nAD——对应向量

则说明路径段EF经过障碍物，该路径点不可用。

1.5 改进双向RRT*算法流程

综上所述，本文提出的改进双向RRT*算法经动态末梢点导向采样、势场导向采样以及回溯路径点处理后可生成作业路径，但考虑到路径平滑性和喷雾机器人最大曲率约束，本文采用三阶准均匀B样条曲线[25-26]优化轨迹，从而得到最终作业轨迹，改进双向RRT*算法整体流程如图4所示。

图4 改进双向RRT*算法实现流程图

2 仿真验证

2.1 果园场景模拟地图

为验证本文改进双向RRT*算法的有效性，进行了基于Matlab的仿真试验。

首先根据果园中果树行分布特点设计果园场景模拟地图，如图5a所示。根据树干位置确定黑色边框作为搜索采样边界，为路径规划提供封闭场景。在该场景下，果树树干、障碍物和四周边界统一构成障碍物区域，并对障碍物区域进行膨胀化处理，膨胀尺寸理论上应为喷雾机器人本体宽度的一半。

为解决果树离散性问题，保证寻找出适合条件的路径，在对图5a中障碍物区域进行膨胀化处理后，还需对每一列果树进行连续性处理，形成封闭的连续狭窄走廊，如图5所示。

2.2 改进双向RRT*算法验证

在图5b中采用人工势场思想对二维地图做处理，每一列果树中轴线处是势场的高势点，每一列可行区域的中轴线处是势场低势点，将整幅地图势场化，如图5c所示。图5d为封闭果园势场三维仿真图，是根据图5c的二维图再结合z轴势场值所确定，从图5d中可以看出整幅果园地图的势场分布以及变化趋势。

图5 运动规划仿真试验图

基于这一思想，在双向RRT*算法采样过程中，当采样点落到障碍物区域，该采样点便会沿着该点势场下降方向被排斥到非障碍区，接下来再继续通过RRT*算法对新的采样点进行后续处理，此方法大大提高了路径搜索效率和采样点利用率。

经1.2、1.3节所述改进后的双向RRT*算法规划生成一条可行路径，并经1.4节所述思想进行路径点处理，最终生成的路径如图5e所示。

2.3 三阶准均匀B样条轨迹优化

图5e所示路径虽然满足位置点连续，但无法满足路径平滑和曲率约束，会产生运动状态不连续、轨迹跟踪效果差的问题。为解决以上问题，采用三阶准均匀B样条优化路径，生成平滑且满足最大曲率约束的作业轨迹。

B样条曲线定义为

(10)

式中C(u)——B样条曲线

Pi——第i个控制点

Ni,p(u)——具有i个节点的p次多项式

第i个p次B样条基函数记为Ni,p(u)，B样条基函数递归定义为

(11)

式中ui——第i个节点

为使优化生成的轨迹通过给定路径点，应将路径点作为型值点而非控制点，所以首先需要通过型值点反求控制点。三阶B样条曲线的型值点Q和控制点P存在关系

(12)

为使生成的轨迹曲线经过起点和终点，需满足

(13)

由式(12)、(13)可得型值点与控制点关系的线性方程

(14)

可算出所求控制点为

(15)

为满足轨迹曲线的起始阶段和终止阶段分别与起始线段P0P1、终止线段Pn-1Pn相切，保证构造出的B样条曲线在两端曲率为0，需满足

(16)

最终，在Matlab中仿真生成的喷雾机器人作业轨迹如图5f、5g所示，图5f表示在果树膨胀化处理后的地图里生成的轨迹，从图中可以看出，轨迹保持平滑性且整体转弯少的同时，也和障碍物有一定间距，这保证喷雾机器人在作业过程中始终是安全的。然后将生成的轨迹在原始果园环境中表示出来，如图5g所示。其中障碍物膨胀尺寸Lexpend为15像素，改进算法扩展步长为25像素，目标偏置概率pgoal为0.3，斥力势场临界值Q*为45像素，斥力势场增益η为20，相邻路径点距离阈值dthr为60像素，相邻折线段夹角阈值θthr为45°。

表1分别从寻路时间、成功搜索路径所需迭代次数、采样点利用率、生成路径的节点数和路径成本5个方面对比了双向RRT算法、双向RRT*算法和本文改进双向RRT*算法。由表1可知，改进双向RRT*算法在以上5个方面均优于双向RRT算法和双向RRT*算法，可以通过相对较少的时间和节点数量生成相对更优的路径。

3 试验验证

3.1 试验平台

本文算法验证使用阿克曼底盘的喷雾机器人作为试验平台，如图6a所示。喷雾机器人主要由机器人底盘(hunter 2.0型，松灵机器人(深圳)有限公司)、中央计算机(英特尔酷睿八代i7-8565u)、组合导航系统(高精度组合导航系统X1，北云科技)、二维激光雷达传感器(LR-1BS2型，欧镭激光)和精准变量喷雾系统组成。其中，机器人底盘后轮为直流无刷电机驱动，前轮为阿克曼转向结构，轮距0.61 m，轴距0.65 m，最小转弯半径1.5 m，最大速度1.5 m/s，车体三维坐标系如图6b所示；组合导航系统支持双天线定向，通过接收基站差分信号实现厘米级定位精度，X1安装在机器人后轮中轴线上，X1坐标系和车体坐标系重合；二维激光雷达传感器感知角度范围0°～270°，距离范围0～10 m，厘米级精度，安装在喷雾机器人前方，距地面高度约0.40 m，激光雷达局部坐标系和车体坐标系的XY平面在同一平面，且X轴重合。

图6 喷雾机器人示意图

本研究使用中央计算机装载Ubuntu 18.04LTS操作系统，并基于机器人操作系统(Robot operating system，ROS)进行开发。

喷雾机器人运动规划系统框图如图7所示。首先通过二维激光雷达采集树干相对喷雾机器人的相对坐标，喷雾机器人的全局坐标可由组合导航系统X1精确定位，经坐标转换后计算出树干在全局坐标系下的坐标，根据转换后的树干坐标建立果园全局地图，然后根据喷雾机器人的尺寸参数和转弯半径对全局地图做封闭和膨胀处理，并在此地图上对改进双向RRT*算法进行验证，最后通过轨迹跟踪控制算法生成控制喷雾机器人的运动指令，经CAN协议与底层控制系统通信，实现对喷雾机器人的行为控制。

图7 喷雾机器人系统框图

3.2 数据采集与地图生成

本文算法验证所选试验地点位于江苏大学校园宽阔场地(32.203 557 4°N,119.512 970 3°E)，如 图8 所示。模拟果园场景中使用仿真树和PVC水管排成4行4列模拟果树，模拟树干直径约为0.15 m，果树行间距为3 m，同一行中果树间距为1.5 m。通过机载前置水平二维激光雷达采集果园树干位置信息，并根据机载全局定位系统将树干位置信息从机体坐标系转换到全局坐标系，在ROS中进行处理生成以喷雾机器人当前位置为原点的二维平面地图并在Rviz中显示，可视化地图中栅格分辨率为0.5 m，所生成果园二维地图如图9所示，为方便观察，图中对树干进行放大处理。

图8 试验场景图

图9 果园二维地图

3.3 改进双向RRT*算法验证与分析

对于果园环境下喷雾机器人的运动规划，在具有树干位置信息的二维地图基础上，根据2.1节所述方法和喷雾机器人的运动学参数(长0.98 m，宽0.745 m，最小转弯半径1.5 m)对果树尺寸进行膨胀化处理并添加边界约束，将整幅地图处理为一个封闭的连续狭窄走廊场景。

为验证无障碍果园环境下的改进双向RRT*算法规划效果，设计试验场景如图10所示。以机器人当前位置为起点，根据右前方的树干位置推算终点，并根据当前喷雾机位姿信息和采集树干坐标时喷雾机器人的位姿信息，对地图进行旋转平移变换，在转换后的地图中分别运行传统双向RRT*算法和改进双向RRT*算法，在Rviz中显示所生成轨迹，如图11红色曲线所示。

图10 喷雾机器人期望运动路径

图11 无障碍环境下轨迹对比图

通过对比图11a、11b可知，改进双向RRT*算法相较于传统双向RRT*算法，可以生成更加平滑且转弯次数更少的轨迹，只在每次换行时具有转弯现象，更符合喷雾机器人的真实作业轨迹。

由表2可知，在无障碍果园环境下，改进双向RRT*算法相较于传统双向RRT*算法，路径规划用时缩短62%，采样点利用率提高了33.97个百分点，最终所生成运动路径长度平均缩短6.54%。图12所示为以上2种算法经过三阶准均匀B样条优化后生成轨迹的曲率随轨迹上与起点距离的变化曲线，由图12可以看出，改进双向RRT*生成轨迹比传统双向RRT*有更小的曲率波动且有一定规律，这一波动主要出现在换行转弯处，基本上满足最大曲率约束。

表2 无障碍环境下算法效果比较

图12 无障碍环境下轨迹曲率对比

为验证存在行人障碍物的果园环境下改进双向RRT*规划算法，设计如图13所示试验场景，模拟具有行人障碍物的果园环境。在该地图中分别运行传统双向RRT*算法和改进双向RRT*算法，在Rviz中显示该环境下生成的作业轨迹，如图14所示。

图13 存在行人障碍物的果园环境

图14 行人障碍环境下轨迹对比图

由图14可知，当果园环境中存在行人障碍物时，本文算法生成的轨迹只在换行转弯和障碍物附近存在轨迹弯曲波动的现象，在其他无障碍物树行中，仍然基本保持直线轨迹，从表现形式来看，仍优于传统双向RRT*算法。

由表3可以看出，相对于无障碍物环境下，具有行人障碍物的环境下规划时间整体延长，延长的时间主要花费在障碍物附近的狭窄走廊处。在行人障碍物环境下，改进双向RRT*算法相较于传统双向RRT*算法，规划用时缩短53%，采样点利用率提高23.12个百分点，优化后路径平均长度缩短7.73%，改进算法整体性能优于传统算法。图15为具有行人障碍环境下2种算法生成路径经三阶准均匀B样条优化后生成轨迹的曲率随轨迹上与起点距离的变化曲线，在该环境下，所生成轨迹仍满足喷雾机器人作业的最大曲率约束，在30 m附近的波动是由绕开障碍物附近轨迹引起的。以上2种试验场景试验参数如表4所示。

表3 行人障碍环境下算法效果比较

图15 行人障碍环境下轨迹曲率对比

表4 试验参数

4 结论

(1)为提高果园环境下喷雾机器人作业效率与自主性，设计了一种搭载有组合导航系统、二维激光雷达、中央计算机和精准变量喷雾系统的果园喷雾机器人，并基于此平台，提出了一种改进双向RRT*的全局路径规划算法，相较于传统双向RRT*规划算法，改进算法规划时间平均减少57.5%，采样点利用率平均提高28.55个百分点，最终路径长度平均缩短7.14%(2种情况取平均)。

(2)搜索路径成功后，经回溯路径点处理并结合三阶准均匀B样条曲线，生成可供果园喷雾机器人运行的精准平滑轨迹，在无障碍物环境和行人障碍物环境下，生成的轨迹整体转弯较少，仅在换行转弯和躲避障碍物处出现轨迹弯曲现象，且曲率均满足喷雾机器人底盘最大曲率约束。

(3)为进一步提高果园喷雾机器人的自主性和智能性，在实现全局路径规划的基础上，还需考虑在喷雾机器人作业过程中的实时动态路径规划以及遇到动态障碍物的决策规划方法。