果园绿篱修剪机械手路径规划算法研究

夏长高，许秋月，韩江义

(江苏大学 汽车与交通工程学院， 江苏 镇江 212013)

0 引言

绿篱是果园的屏障，起到了间隔保护的作用[1]，现有的手持式绿篱修剪机技术存在修剪工作繁重、效率低等缺点，实现绿篱的自动修剪已成为当下研究热点。能够使修剪机械手避开障碍物且能自动运行到修剪点，对提高机械手的智能化有很大的作用。

对于避障路径规划问题，现有的路径规划算法包括图搜索算法、人工势场法、智能算法和快速搜索随机树算法。基于图形的搜索方法[2-4]需要遍历有向图中的所有顶点，搜索效率低且不适用于高维空间。当障碍物对机器人的斥力与目标点对机器人的吸引力大小相等且方向相反时，人工势场法容易陷入局部极小值或目标不可达[5]。智能算法[6-7]通常与传统路径规划算法结合使用，其规划效率取决于参数的选择和算法的特性。

机械手是一个具有高度非线性的复杂链式结构，不能作为笛卡尔空间中的粒子进行路径规划，因此大多数路径规划算法不能直接应用于机械手。快速探究随机树RRT算法[8]是一种通过随机构建空间填充树来有效搜索非凸，高维空间的算法，可以有效避免创建三维障碍物模型的困难，广泛应用于机械手路径规划中，但是RRT算法存在采样过程随机、搜索时间长、路径代价高，难以在复杂狭小空间快速搜索到路径等缺点[9]。许多学者针对上述问题，在RRT算法的采样过程[10-12]和缩短路径长度方面[13-14]进行改进，但是改进后的算法不能兼顾搜索效率和路径质量，存在计算量大等问题。

本文针对RRT算法采样的缺点，主要研究了RRT算法的采样过程，提出了一种结合目标偏差概率和引力思想的改进采样过程，该采样过程使算法在采样时偏向目标点，加快了搜索速度。为缩短路径长度，减少路径点数，将RRT*算法与改进的采样方法相结合，使算法能够快速搜索渐进最优路径。通过在MATLAB中的机器人系统工具箱中进行仿真，验证算法的有效性。

1 算法改进

1.1 RRT*算法

RRT*算法是针对RRT算法搜索路径长的缺点提出的一种渐进最优快速搜索随机树算法，其重新选择父节点和重新布线过程，有效缩短了搜索路径的长度和减少了路径点[15]，重新选择父节点是以xnew为中心，建立半径为R的球形搜索空间，半径R可根据式(1)求得。在该球内的节点称为近邻节点xpotentialparent，依次计算xnew经过xpotentialparent到xstart路径代价，并对xnew和xpotentialparent之间的路径进行碰撞检测，选出使路径代价最小且无碰撞的近邻节点作为xnew新的父节点。之后，为进一步使随机树节点间的路径代价尽可能小，为扩展树重新布线，如果近邻节点的父节点改为新节点xnew可以使路径代价减小，则将近邻节点的父节点改为xnew。

(1)

式中：R为空间半径；γ为基于环境的规划常数；n为近邻节点的个数；d为规划空间维数。

RRT*算法选择父节点和重新布线的扩展过程如图1所示。

图1中，xpotentialparent代表邻近节点，xparent代表父节点。

图1 RRT*算法选择父节点和重新布线过程

虽然RRT*算法可以得到渐进最优路径，但是RRT*算法采样过程与RRT算法相同，采样过程具有随机性，算法效率低，不利于机器的实时控制，因此需要对算法进行改进，降低规划时间，提高规划效率。

1.2 a-bRRT*算法

人工势场法(APF)是Khatib[16]提出的一种机器人路径规划算法。该算法将目标点和障碍物分别看作对机器人有引力和斥力的物体，机器人沿引力与斥力的合力来进行运动。传统APF中机器人与目标点之间的引力场定义为[17]：

(2)

Fatt(X)=-▽Uatt(X)=-katt|X-Xg|

(3)

式中：Uatt为引力场；Fatt为引力；katt为增益系数；X为机器人的当前位置；Xg为目标点位置。

通常情况下，为了加快RRT算法的随机搜索速度，通常设置目标偏移概率λ，即bias-RRT[18]，本文称为bRRT。虽然bRRT算法相对于RRT算法的路径规划速度有所加快，但是当生成的随机数大于偏转概率时，采样依然随机。如果在RRT*中引入目标偏转概率λ和APF法的引力思想，对RRT*算法的采样过程进行改进。算法中的rand函数随机生成0～1的随机数，如果随机数小于λ，则将目标点xgoal设置为采样点Xrand，如果随机数大于λ，则在采样空间中随机采样之后得到xrad，目标点对扩展树上距离采样点最近的点xnear产生吸引力，根据目标点对最近点xnear的吸引力，得到临时路径点xtp,由随机采样点xrand和临时路径点xtp求取新的采样点Xrand，a-bRRT*算法采样过程如图2所示，临时路径点xtp和新采样点Xrand可以用以下公式进行表示：

图2 a-RRT*算法采样过程

xtp=-Δl·katt·|xnear-xgoal|=

(4)

(5)

则新节点xnew可通过式(5)求得：

(6)

式中：xnear为最近树节点；xtp为临时路径点；xrand为随机采样点；Xrand为新采样点；xnew为新节点；xgoal为目标点；Δl为扩展步长。

将改进后的采样过程与RRT*算法中的重新选择父节点和重新布线相结合，给出a-bRRT*算法流程，如图3所示。

图3 a-bRRT*算法流程框图

2 算法验证

为了验证改进算法的搜索和寻优能力，创建包括9个边长为2 cm正方体障碍物的验证环境，它们位于起始点(5，5，2)和目标点之间。为了验证算法在复杂空间中的有效性，将目标点设置为(11，10，13)。为了验证算法在狭窄空间中的寻优能力，将目标设置在2个障碍物之间的狭窄空间中，目标点为(11，8，10)，3种算法在边长为20 cm的正方体空间中进行随机采样，步长设置为1，当目标点与扩展树xnear节点距离小于1时，将目标点添加到扩展树中，算法结束。模拟结果如图4—6所示，其中红线表示搜索无碰撞路径，绿线表示扩展的树枝，红色点表示扩展树节点。

从图4—6可以看出，bRRT、RRT*和a-bRRT*都可以在复杂环境和狭窄空间中搜索无碰撞路径。在图4中，由于bRRT算法采样方向的随机性，扩展树充满采样空间，规划效率低，路径最长。从图5可以看出，RRT*的路径长度明显短于bRRT算法，在复杂环境中，RRT*算法可以搜索到渐进最优路径。由于其采样过程的随机性，重新选择父节点和重新布线的过程致使当目标点在狭小空间时，算法很难搜索到路径，搜索效率低。从图6可以看出，改进算法a-bRRT*可以有效减少扩展树节点且向目标点方向进行扩展。在复杂的环境和狭小的空间中，该算法都能够高效地搜索路径，且搜索路径较短。

图4 bRRT算法验证仿真图

图5 RRT*算法验证仿真图

图6 a-bRRT*算法验证仿真图

为了详细比较bRRT、RRT*和a-bRRT*3种算法，在Matlab中对3种算法进行了多次仿真，记录其搜索时间、路径长度、采样点和树节点的数量，其平均数据如表1所示。

表1 3种算法的仿真平均数据

2种空间条件下，与bRRT算法相比，a-bRRT*算法的路径长度分别缩短了29.9%和23.8%；与RRT*算法相比，时间分别减少了52.8%和98.6%。改进后的算法提高了规划效率，缩短了路径长度，采样点和树节点数量显著减少，加入引力思想算法的采样点数量减少了约90.44%。改进后的算法采样方向性强，易于快速搜索无碰撞路径。

3 机械手避障路径规划仿真

在关节空间中进行路径规划，机械臂自初始位姿至目标位姿的运动不可预知，且搜索速度远慢于笛卡尔空间规划法，但是计算量小，不需要时刻进行机械臂逆运动学求解，路径是在机械手各个关节范围内规划出的，因此关节角度变化必定连贯，排除出现奇异点的可能[19]。本文在关节空间中进行路径规划，在笛卡尔空间中进行碰撞检测。

3.1 碰撞检测

为避免机械手从起始位姿到达修剪位姿触碰绿篱，需要进行机械手避障。本文中绿篱修剪机械手的修剪状况有2种：修剪绿篱顶面时，机械手由初始位姿运行至绿篱顶面修剪位姿；完成顶面修剪后，机械手从顶面修剪位姿运行至侧面修剪位姿。

机械手碰撞包括机械手连杆之间的碰撞以及整个机械手和障碍物之间的碰撞[20]，果园绿篱墙高度一般在1.2～2 m，宽0.8 m，以绿篱墙为障碍物，采用矩形包络法对障碍物进行简化。将机械手连杆的最大半径叠加在障碍物上，将连杆与障碍物的空间碰撞检测问题转化为空间直线与平面位置关系的检测，如图7所示。

图7 机械手连杆和障碍物简化过程

3.2 仿真结果

机械手在Matlab中的机器人系统工具箱(RST)中进行仿真。为了更好地验证算法，在SolidWorks中建立了四自由度修剪机械手模型，如图8所示。将机械手模型通过插件转化为URDF格式，导入到RST工具箱中，进行机械手路径规划仿真。

图8 修剪机械手三维结构示意图

机械手第一个修剪状况是从收缩状态到达顶部修剪位置，起始点的关节坐标为(0，2.094 4，-2.094 4，-1.047 2)，目标坐标为(0,1.275 4，-0.850 1，-1.996 0)。第二个修剪工况为机械手顶端修剪完毕，从顶端运行到侧面修剪点的位置，起点的关节坐标为(0，1.275 4，-0.850 1，-1.996 0)，终点的关节坐标为(0，1.807 0，-2.443 5，0.636 5)，机械手路径规划采样范围限定在机械手各个关节转动范围内。

以从修剪顶面运行至修剪侧面为例，设置高 2 m，长2 m，宽0.8 m的绿篱墙障碍物，在RST工具箱中进行三维仿真，获得机械手末端执行器路径，验证a-bRRT*算法可以在起始点和终点之间搜索到一条无碰撞路径，且路径长度得到了优化，如图9—11所示，图中绿色立方体为障碍物模型。

图9 bRRT算法末端执行器路径

图10 RRT*算法末端执行器路径

图11 a-bRRT*算法末端执行器路径

为验证改进a-bRRT*算法的优越性，在关节空间对3种路径规划算法进行多次仿真验证，结果如表2所示。

表2 机械手路径规划仿真平均数据

从表2可以看出，修剪顶面时，与bRRT相比，a-bRRT*的平均路径长度减少了66.32%，规划时间减少44.19%。修剪侧面时，a-bRRT*算法的路径长度比bRRT*算法短67.17%，规划时间比RRT*算法少73.87%。结果表明，改进后的算法兼顾路径长度和规划效率，达到了预期目的。每个算法模拟数据的标准差用于评估算法的稳定性。各算法数据的标准差如表3所示，在仿真结果中，改进后的a-bRRT*算法在2种修剪条件下的时间标准差和长度标准差都是最小，因此改进后的算法比原算法在保证规划效率和路径长度方面更加稳定。

表3 各算法仿真数据的标准差

从图9—11，以及表3可以看出，这3种算法都可以顺利完成避障路径规划，但bRRT算法搜索时间过长，路径长。虽然RRT*算法的路径长度缩短，但搜索效率低。改进的a-bRRT*算法偏向目标点采样，不仅搜索速度快，规划效率高，而且继承了RRT*算法搜索路径长度短的优点，兼顾了搜索效率和路径长度。

4 结论

1) 针对RRT算法采样随机、效率低、规划路径长的缺点，对算法采样过程进行改进，结合目标偏移概率和引力的思想，提出了一种改进的RRT*算法。改进后的算法采样偏向标点，规划效率高、路径短，采样点数减少约90.44%。

2) 在RST工具箱中验证a-bRRT*算法在机械手路径规划中的有效性。结果表明，a-bRRT*算法能够成功规划无碰撞路径，效率高、路径短，且改进后的算法更稳定。

3) 针对修剪机械手避障路径规划问题，仅在静态已知障碍物环境下进行路径规划，因此在后续研究中，需要考虑动态障碍物的实时避障，以适应更多的修剪场景。

4) 由于条件有限，本文的研究仅在MATLAB/RST中进行仿真验证，后期还需进行实验验证。