基于构型空间先验知识引导点的柑橘采摘机械臂运动规划

马冀桐，王 毅,2※，何 宇，王 恺，张艺谭



基于构型空间先验知识引导点的柑橘采摘机械臂运动规划

马冀桐1，王 毅1,2※，何 宇1，王 恺1，张艺谭1

（1. 重庆理工大学机械工程学院，重庆 400054；2. 重庆大学机械工程学院，重庆 400044）

柑橘采摘过程中机械臂有时需要深入树冠内部进行采摘，而在树冠内众多枝干往往构成一个个封闭的多边形通道，比起单个枝条的障碍物，封闭多边形障碍物更加难以避开，需要更长的时间进行规划。针对此问题，该文通过对构型空间的离线构建，分析了封闭多边形障碍物在构型空间的拓扑结构性质，根据这一性质对双向快速扩展随机树算法（RRT-connect）进行改进，提出了一种基于构型空间先验知识引导点的RRT-connect算法（informed guidance point RRT-connect，IGPRRT-connect），并将RRT-connect与IGPRRT- connect进行了并行规划编程，提高在不同环境下的适应性。通过仿真：所提出的并行算法在各种环境下规划时间均处于较低水平，以边长为30 cm与25 cm的正方形封闭通道为例，与RRT-connect相比规划时间分别缩短了51%、86%。同时进行室内避障试验，试验结果表明，使用提出的并行算法，对封闭障碍物和未封闭障碍物均有较好的避障效果，平均规划时间为1.263 s左右，成功率为91%，可为柑橘采摘机器人在不同环境下的运动规划问题提供参考。

机器人；运动规划；避障；构型空间；引导点；RRT-connect；柑橘采摘

0 引 言

农业是中国经济的重要支柱之一[1]，果蔬产品作为农业生产的重要一部分，其收获采摘约占整个作业量的40%[2]，而柑橘等季节性水果的收获作业更是需要在短时间内密集的体力劳动，需要在短时间内找到大量劳动力，人力成本巨大[3]。为了能够使柑橘收获过程自动化，已经开发了一部分柑橘收获机械，如旋转拍打杆或枝干摇动设备。但是因为果实受到震动冲击的影响，这些方法只适合收获用于二次加工的柑橘(如制作果汁)，并不适合无损柑橘的收获[3]，随着计算机技术、人工智能等领域的快速发展，采摘机器人技术已从前瞻性研究开始成为现实需求[4]。采摘机器人的市场应用可以大幅减少人力成本。但是在非结构性的自然环境下，被采摘柑橘常常会被枝干遮挡，给机器人的自动采摘作业造成很大困难。这就需要机械臂具有自主运动规划的能力，即避开障碍，到达柑橘目标。因此机械臂的运动规划问题成为柑橘采摘机器人的关键问题之一[5]。

机械臂的运动规划与智能车等的路径规划不同，机械臂运动规划不是一个点的避障轨迹规划，而是在给定障碍物以及机械臂起始、目标位姿的条件下，搜寻一组连续的关节角度值序列，该角度序列能够驱动机械臂安全、无碰撞地从起始位姿运动到目标位姿[6]。机械臂的自由度往往更多，这就导致了在智能车上应用较多的A*、Dijkstra等算法应用在机械臂上时会出现“指数爆炸”难题[7]，为了应对这一难题国内外学者对机械臂避障路径规划问题开展了大量研究。祁若龙等[8]将空间机械臂运动规划问题转化为一个多目标优化求解的问题，然后利用遗传算法在构型空间下规划出一条无碰撞的动作序列。但是随着自由度增加遗传算法的待求解参数增加、碰撞检测的计算成本增加，计算时间将会随之成指数形式增长；贾庆轩等[9]先依据障碍物位姿坐标求解机械臂的无碰撞自由工作空间，然后利用A*算法在空间机械臂的自由工作空间进行无碰撞路径搜索；Gómez-Bravo等[10]首先利用快速扩展随机树法（rapidly exploring random tree,RRT）提供一组可行的路径，然后使用遗传算法根据适应度函数对原始解进行优化；蔡健荣等[11]利用概率地图法（probabilistic road map method，PRM）对多自由度机械臂进行避障路径规划，该方法不需要求障碍物在机械臂构形空间中的精确映射模型，只要通过采样获得部分机械臂构形空间内的无碰撞构型，通过连接无碰撞构型获得机械臂避障路径，但受限于迭代次数、搜索时间等因素，无法求解出所有可能的路径，也无法求解到最优解；姚立健等[12]对SCORBOT-ER4u型茄子采摘机械臂进行了避障运动规划，算法将障碍物映射到收获机械臂构形空间中，在构形空间中利用A*算法搜索避障路径，但该方法为对路径规划模型进行简化将空间障碍物投影到二维平面中，无法充分发挥高自由度采摘机械臂在避障方面的优势；尹建军等[13]将高自由度机械臂避障路径规划问题转化为多个平面2转动关节机械臂避障问题，但算法需要对高自由度机械臂整体进行逆运动学分析，并在每个无障碍的平面内搜寻关节角度，算法前期理论分析较复杂。罗陆锋等[14]则采用人工势场法对采摘机械臂进行无碰运动规划，在仿真试验中，34次规划成功了28次。谢龙等[15]对人工势场法进行改进，目标点对机械臂的末端施加引力，障碍物则对最近机械臂的连杆施加斥力，在两个力的合力下引导机械臂到达目标点。邹宇星等[16]通过对PRM算法进行改进，对采摘机器人机械臂进行运动规划，首先对碰撞检测数据库进行构建以减少每次规划的碰撞检测时间，再利用PRM进行运动规划。Nguyen等[3]对苹果采摘环境下多种算法进行了试验验证，发现双向连接快速扩展随机树算法（RRT-connect）效果最好。可以看到目前对于机械臂运动规划的研究主要集中在A*、人工势场法、PRM、RRT、RRT-connect等算法上，但A*、人工势场法在高维机械臂上存在计算量大等问题，而基于随机采样的PRM、RRT、RRT-connect等算法，尤其是RRT-connect在高自由度机械臂表现较好，但是针对柑橘采摘环境下的运动规划问题研究还较为缺乏。

本文基于团队所研发的柑橘采摘机器人，针对机械臂有时需要深入树冠内部进行采摘，树冠内众多枝干往往构成一个个封闭的多边形通道的情况，对采摘环境进行合理归纳，机械臂需要深入到封闭多边形内部，对果实进行采摘。本文提出了一种构型空间投影降维方法将六维构型空间投影为三维进行可视化，方便对构型空间进行分析，为运动规划算法的开发提供指导。针对封闭多边形投影后的构型空间进行分析，提出了一种针对该采摘环境的基于构型空间先验知识引导点的双向连接快速扩展随机树运动规划算法，并进行了改进前后算法的并行编程，以提高不同环境适应性，最后通过仿真试验验证所提出的方法与现有RRT-connect算法相比具有显著优越性。

1 机械臂构型空间降维映射建模

针对研发的采摘机器人，使用本文提出的基于降维投影的构型空间映射方法对底盘（本文底盘是对底盘和外壳的统称）在构型空间的映射进行了建模，然后对封闭多边形障碍物在构型空间的映射进行了离线构建，为规划算法的改进提供依据。

1.1 采摘机器人原理

本文所采用的采摘机器人为团队研发，结构如图1所示，该机器人主要由机械臂、移动底盘及视觉系统等组成，机械臂采用遨博公司生产的aubo_i5六自由度机械臂，臂展1 m，安装位置距离地面1 m，采摘范围为0～2 m，可以对矮化的柑橘树进行采摘，移动底盘采用山东极创机器人公司Komodo-2型履带底盘，上方安装有自主设计的外壳，视觉系统由BB2双目相机获取柑橘及障碍物信息[17]，实验室前期研究中双目相机精度达到1.58%[18]，能够满足采摘作业中的定位要求。机械臂经过运动规划到达采摘位置，末端执行器进行果柄剪切及果实夹持[19-20]，然后通过外壳上方的开孔将柑橘放入果篮。由于本文在于研究机械臂避障方法的可行性，因此对末端执行器的影响暂未考虑。

1. 履带底盘 2. 双目相机 3. 机械臂 4. 末端执行器 5. 外壳 6. 果篮 7. 工控机

1.2 构型空间降维映射建模

对于机械臂，点的坐标则表示为每个关节的角度值(1,2,…,θ)，六自由度的机械臂对应六维构型空间：6。过高的维度导致了难以准确表达构型空间[21]，且无法对高维空间进行可视化观察，增大了分析难度。因此−的降维处理对于算法的前期理论分析尤为重要，本文提出一种降维投影的方法：对避障影响最大的前3个大关节进行构型空间构建并依据后3个关节进行修正，将构型空间由六维投影为三维：≈6×-3。

具体过程为：使用蒙特卡洛方法对前3个关节（肩关节、大臂关节、小臂关节）角度值进行随机采样，后3个关节（腕关节1、腕关节2、腕关节3）角度值为0，每次采样对机械臂进行碰撞检测，判断该点在free或obs，若无碰撞则该构型在free，为了减少投影过程中后3个关节的信息丢失量，再以随机采样的方式更改碰撞构型的后3个关节角度值，循环500次进行碰撞检测，若有无碰撞构型，说明前3个关节角度值为当前值时，存在与障碍物的无碰撞构型，则将该点列入free，反之认为前3个关节角度值为当前值时所有构型均为碰撞构型，列入obs得到点云数据，将obs的点进行绘制，应用alphashape算法提取点云轮廓[22]，使构型空间可视化，便于分析，流程如图2所示。

首先对底座在构型空间内的映射base进行建模。为方便观察，base为半透明，颜色较深处为free与base交界面，如图3a，可以看到构型空间边界由障碍物包围，内部为free空腔，机械臂在内部运行。

注：为6个关节角度值。

底座构型空间的映射有助于分析底座外壳的合理性，是否对机械臂运动规划造成了不良影响，对于改进底座的外形有指导意义。

构型空间中封闭多边形的拓扑性质是同胚的[23-24]。对封闭多边形枝干障碍物在构型空间的映射limb进行了建模分析。图3b、图3c分别为正方框形障碍物、圆环形障碍物在构型空间映射的不同角度视图。

通过图3b、图3c发现正方框形与圆环形在构型空间内的投影拓扑的性质：封闭多边形障碍物的构型空间投影可以简化为上下2部分，上下2部分会有部分联结，中间部分为空腔与外界free相通，且只有上下两个开口，若机械臂终点位姿位于空腔内（机械臂伸入封闭多边形）则只能通过上下开口（1为3.14和0时，实际为同一个构型，在图中显示为开口，实为封闭）。

注：从左至右代表不同视角下构型空间的视图，视角方向依次为(1,1,1)、(0,1,1.9)、(1,0,0)。

为了对构型空间有整体观察，将底盘和障碍物（40 cm框型为例）的映射进行并集操作如图3d所示，空白部分为机械臂可以无碰撞运动的自由空间。

对构型空间的降维映射建模为离线构建，由于封闭多边形障碍物的拓扑结构相似性，对构型空间的建模目的在于探寻不同的封闭多边形障碍物的共同特性，为规划算法提供改进依据，无需每次规划都对构型空间进行构建。

2 基于构型空间引导点的RRT-connect算法

首先对RRT-connect算法及桥测试原理进行阐述，然后对构型空间的映射进行简化分析，以此作为先验知识，提出了针对深入树冠内部采摘任务的基于先验知识的桥测试策略，以此策略对RRT-connect算法进行改进，提出基于构型空间引导点的RRT-connect算法，并对RRT及改进后的算法进行并行编程，解决2种算法在不同环境下适应性较差的问题。

2.1 RRT-connect算法及桥测试原理

RRT-connect算法[25]的主要思想是：从初始点和目标点出发，并行构建2棵搜索树；在每次迭代过程中，2棵搜索树总是彼此朝着对方扩展，直至2棵搜索树相遇为止。RRT-connect构建过程如下：在每次迭代中，先扩展其中一棵搜索树，然后尝试将另一棵搜索树扩展到当前搜索树扩展的新节点；2棵搜索树init和goal交替扩展，直至2棵搜索树相遇为止[26]。

对柑橘采摘作业中常见的封闭多边形障碍物降维投影到了构型空间后，对空间进行进一步抽象简化，如图4，起始构型init点，到达终点goal，只能通过上下开口，这种环境为窄通道环境。由于RRT-connect对于窄通道的通过性较差，针对这一问题有研究人员提出使用桥测试算法寻找中间点引导通过窄通道[27]，桥测试算法是在构型空间随机采样2个端点，若2个端点位于obs且它们的中点mid位于free则认为通过了桥测试，mid被认为位于窄通道处，但往往该算法会在free进行大量无用的搜索，且实际的构型空间内往往有很多凹区域，会导致桥测试算法找到不合适的中间构型。

注：qinit为起始构型，qgoal为目标构型,qmid为引导构型。

2.2 基于先验知识的桥测试策略

由于传统的桥测试进行全局搜索，会浪费大量时间在无用区域上，且由于凹区域的存在，使得使用桥测试往往无法找到正确的窄通道位置。若机械臂伸入封闭多边形框内，在构型空间内则体现为由障碍物外部空间进入到空腔中，需要通过上下开口处的窄通道位置，这可以视为先验知识。

有了这个先验知识，就可以制定策略，缩小桥测试的搜索范围，已经知道窄通道的粗略位置，则只需在这2个开口处进行检测，如图4所示。为了减少碰撞检测量，只在、两条线段上进行桥测试：由于底盘的构型空间映射总是固定的，所以将线段端点设在处将不必对底盘进行碰撞检测，只需对障碍物进行碰撞检测，在线段上随机取点mid，若mid位于free，则mid把分为2段mid,mid。若这2段进行碰撞检测都无法通过，则认为mid位于窄通道位置。

具体流程为：

1）获取起点、终点关节角度值init、goal。

2）确定mid的1起始值，为尽可能缩短总的路径，mid的1初始值取init及goal的中值。

3）由于1值已经被赋予初始值，此时线段位于mid®1的横切面上，随机取(01)区间内的数值，作为对插值比例。

4）对mid进行碰撞检测，判断mid位于free还是obs。

5）若mid位于free则分别对mid、mid进行碰撞检测，反之进入步骤7。

6）若mid,mid均有碰撞则mid位于窄通道位置，结束；反之，返回步骤3）。若重复30次无法找到窄通道处mid则进入步骤7）。

7）步骤6）中没有找到窄通道，则认为在当前1下不存在通道，对1进行“震荡”调整，返回步骤3。“震荡”调整即以增量的方式左右摇摆：1、1+01、1−01、1+02、1−02、1+03、…，直到找到窄通道或1超出±3.14。

8）若步骤7）无法找到mid，则将线段换为线段，重复步骤3）。

通过上述步骤找到窄通道处的引导构型后分别对initmid、midgoal运行RRT-connect算法进行运动规划：由引导构型mid向init规划使得RRT-connect不用花费大量的时间去探索寻找窄通道的出入口，mid位于出入口处，再结合RRT-connect的双向生长特性使得mid与init、goal相连接都比较容易。由此得到基于构型空间先验知识引导点的RRT-connect算法（RRT-connect with informed guidance point, IGPRRT-connect），算法整体流程如图5所示。

图5 IGPRRT-connect流程图

2.3 2种算法的并行编程

IGPRRT-connect只针对封闭多边形这一特定环境，在单根枝干的情况下会消耗大量时间去寻找并不存在的中间构型，这就需要人为去判断在当前环境下需要RRT-connect算法还是IGPRRT-connect算法。为了解决这一问题，采用并行编程的方法，创建2个线程：一个线程运行RRT-connect，另一个线程运行IGPRRT-connect算法，当其中一个线程完成规划，2个线程全部停止，输出规划路径，这样就避免了人为选择算法，提高了采摘机器人的智能程度，并行编程有利于简化程序解决方案：无需再编写算法判断当前环境下该采用RRT-connect还是IGPRRT-connect。并行计算框架如图6所示。

图6 RRT-connect和IGPRRT-connect算法并行规划框架

3 仿真及试验验证

本文搭建了柑橘采摘机器人仿真与控制平台，然后对IGPRRT-connect与并行算法进行仿真与室内试验 验证。

3.1 仿真与控制平台搭建

基于机器人操作系统（robot operating system，ROS）[28]开发了柑橘采摘机器人仿真与控制平台，其界面如图7所示，进行柑橘采摘机器人运动规划仿真与控制方面的研究。该平台集成了碰撞检测、正逆运动学求解、轨迹平滑、轨迹可视化等功能，部分功能的插件选择及参数设置如表1所示。

整个系统避障任务工作流程如图8，逆运动学算法求解任务信息中目标位置对应的机械臂逆解，提供给RRT- connect算法作为构型空间规划目标，RRT-connect在节点扩展过程中调取碰撞检测插件进行路径碰撞检测。得到无碰撞路径后，对路径应用5次样条曲线进行路径平滑。然后平滑后的路径数据通过CAN总线发送给驱动器，由驱动器驱动电机转动完成机器人的采摘动作。机器人采集的位置、速度等信息实时反馈到规划场景中，对规划场景中的机械臂各关节位置进行更新。

图7 柑橘采摘机器人仿真与控制平台界面（移动底盘以包围盒方法进行了简化）

表1 集成的插件及参数设置

图8 柑橘采摘机器人避障运动规划仿真及控制系统

3.2 仿真试验

3.2.1 试验设计

为验证所提出IGPRRT-connect算法以及RRT- connect与IGPRRT-connect并行计算算法的性能（下文简称并行算法），在搭建的仿真平台下进行仿真试验，试验环境为在windows下安装的Ubuntu虚拟机，分配的内存大小为4 GB、4核，GPU为NVIDIA GeForce GTX 1060 6 GB。

为验证本文提出的IGPRRT-connect算法的适应范围和使用并行算法的效果，进行了2个仿真试验：1）设定不同正方形边长障碍物试验，来验证不同大小封闭障碍物下各算法的表现，将作为障碍物的正方形框的边长逐次减小，以5 cm为间隔，从50到25 cm，共进行6组试验，每组试验分别应用IGPRRT-connect、RRT-connect及并行算法进行20次规划，并计算平均规划时间；2）设置不同形状障碍物试验，来进一步验证验证所提出算法的优越性，障碍物共分为6组：4组不同形状位置的封闭多边形框、1组两根平行障碍物，1组V型障碍物，每组试验同样应用3个算法进行20次规划，统计规划时间。

3.2.2 结果对比分析

不同正方形框边长障碍物试验过程如图9所示，图10为使用RRT-connect、IGPRRT-connect和并行算法3种算法进行运动规划出的平均时间。

a. 起始构型 a. Initial configurationb. 目标构型 b. Target configurationc. 运动轨迹 c. Trajectoryd. 引导构型 d. Guided configuration

注：并行算法为RRT-connect与IGPRRT-connect双线程并行规划算法。

由图10可看出，当障碍物边长≥40 cm时，3种算法规划时间相差不大，均为1.5 s左右，当障碍物边长≤35 cm时，RRT-connect算法规划时间开始大幅增加，IGPRRT-connect与并行算法规划时间增加的幅度较小：边长为35、30、25 cm时，RRT-connect规划时间分别为1.7、3.1、18.1 s，IGPRRT-connect规划时间分别为1.2、1.6、2.6 s，相比RRT-connect减少了29%、48%、85%，并行算法规划时间分别为1.3、1.6、2.3 s，相比RRT-connect减少了24%、51%、86%。在不同障碍物大小下，IGPRRT-connect平均规划时间1.56 s相比RRT-connect的4.33 s减少了64%，说明IGPRRT-connect算法具有较大的优越性。

不同形状的障碍物仿真规划如图11a所示。其中不透明机械臂为起始构型，半透明为终点构型，曲线为某次试验的运动轨迹。数据如图11b所示，可以看到并行算法在各种环境下相比RRT-connect和IGPRRT- connect更加稳定，在第1、3组时所消耗的平均时间最少，虽然第2、4、5、6组试验并行算法平均时间并不是最少，但是与消耗时间最少的算法相差不大。在第1、2、3、4组试验中RRT-connect算法规划时间大幅大于IGPRRT- connect与并行算法，进一步验证了本文所提出的IGPRRT-connect算法优越性。而第6组试验说明在障碍物不封闭的情况下，IGPRRT-connect失去优势，时间大幅增加，时间主要消耗在寻找并不存在的引导构型， 6组试验中RRT-connect平均耗时为2.94 s，IGPRRT-connect为1.37 s，相比RRT-connect降低了53%，并行算法为1.26 s，相比RRT-connect降低了57%。

注：6组障碍物形状分别为长方框形障碍物、不规则多边框形障碍物、平行障碍物和V形障碍物。

仿真结果验证了IGPRRT-connect算法在障碍物为封闭多边形时的优势，以及将RRT-connect、IGPRRT- connect并行编程的并行算法的适应性。

3.3 室内试验

3.3.1 试验设计

为了验证采摘机器人平台的可行性，本节应用并行算法进行室内避障试验，控制主机内存大小为4GB、4核。由铝质型材搭建坐标系框架，原点为机械臂安装位置垂直于地面的点，障碍物位置由导轨调整，如图12所示。共进行2组试验，一组为封闭多边形障碍物，封闭多边形障碍物采用30、40 cm边长的正方形框及30、40、50 cm边长的菱形框模拟树冠内部的封闭多边形，另一组为边长为30、40、50 cm的未封闭V形障碍物，模拟分叉枝干，障碍物由木条组成，布置范围距离机器人底座0.5～0.8 m，每组由随机起始构型试验规划50次，统计平均规划时间及成功率。

3.3.2 结果及分析

图13为采摘机械臂在仿真及实验室环境下的避障试验运动过程。试验统计结果如表2。

图12 室内试验环境

a. 仿真运动过程a. Simulation for motion process b. 实际运动过程b. Actual motion process

表2 并行算法室内试验结果

注：每组共进行50次试验。

Note: Fifty experiments are conducted in each group.

可以看到并行算法在各种环境下表现稳定，失败的原因主要为障碍物布置过程中产生的误差及机械臂运动误差所导致的碰撞。

室内试验结果表明：使用本文提出的并行算法，对封闭障碍物和未封闭障碍物的均有较好的避障效果，平均规划时间为1.263 s，避障成功率为91%。可为柑橘采摘机器人的采摘作业提供新的运动规划算法选择，为进一步的研究工作奠定了基础。

4 结 论

本文提出一种构型空间映射方法将高维构型空间进行降维投影并可视化，针对柑橘采摘环境，将封闭多边形障碍物映射到构型空间进行分析，并提出了基于构型空间先验知识的引导点RRT-connect算法（IGPRRT-connect），并针对IGPRRT-connect在非封闭环境下适应性差的问题，将RRT-connect与IGPRRT-connect算法进行并行规划编程。

所提出的构型空间映射方法主要考虑了对机械臂影响较大的前3个关节，同时也将后3个关节考虑对避障的影响考虑在内，减小了降维过程中的信息丢失量，构型空间的可视化对于后期的规划算法分析具有一定指导意义。对RRT-connect算法改进的仿真结果表明，所提出的IGPRRT-connect算法在封闭多边形环境下规划时间大幅减少，在不同边长正方形框障碍物仿真下，平均规划时间为1.56 s，降低了64%，而RRT-connect与IGPRRT- connect的并行算法则在不同的形状障碍物环境下都具有很好的表现，平均规划时间为1.26 s，降低了57%。在室内试验中，并行算法平均规划时间为1.263 s，避障成功率为91%。所提出的IGPRRT-connect算法解决了在RRT-connect封闭通道下规划时间长的问题，进一步的并行规划算法则提高了对不同环境的适应性，对柑橘采摘机器人的研究具有重要意义。

[1] 王亚珍，汪安明，赵坤，等. 基于薄壁圆环理论的机器人用柔性轴承变形特征快速求解[J]. 农业工程学报，2019，35(3)：60－66. Wang Yazhen, Wang Anming, Zhao Kun, et al. Fast solution for deformation characteristics of flexible bearing of robot based on thin-walled ring theory[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2019, 35(3): 60－66. (in Chinese with English abstract)

[2] 宋健，张铁中，徐丽明，等. 果蔬采摘机器人研究进展与展望[J]. 农业机械学报，2006，37(5): 158－162. Song Jian, Zhang Tiezhong, Xu Liming, et al. Research actuality and prospect of picking robot for fruits and vegetables[J]. Transactions of the Chinese Society for Agricultural Machinery, 2006, 37(5): 158－162. (in Chinese with English abstract)

[3] Nguyen T T, Kayacan E, Baedemaeker J D, et al. Task and motion planning for apple harvesting robot[J]. Ifac Proceedings Volumes, 2013, 46(18): 247－252.

[4] 刘继展. 温室采摘机器人技术研究进展分析[J]. 农业机械学报，2017，48(12): 1－18. Liu Jizhan. Research progress analysis of robotic harvesting technologies in greenhouse[J]. Transactions of The Chinese Society for Agricultural Machinery, 2017, 48(12): 1－18. (in Chinese with English abstract)

[5] 阳涵疆，李立君，高自成. 基于关节构形空间的混联采摘机械臂避障路径规划[J]. 农业工程学报，2017，33(4): 55－62. Yang Hanjiang, Li Lijun, Gao Zicheng. Obstacle avoidance path planning of hybrid harvesting manipulator based on jointconfiguration space[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2017, 33(4): 55－62. (in Chinese with English abstract)

[6] Lozano-P, Rez T. Spatial Planning: A Configuration Space Approach[M]. New York: Springer, 1990.

[7] 唐华斌，王磊，孙增圻. 基于随机采样的运动规划综述[J]. 控制与决策，2005，20(7): 4－9. Tang Huabin, Wang Lei, Sun Zengqi. A survey on randomized sampling-based motion planning[J]. Control and Decision, 2005, 20(7): 4－9. (in Chinese with English abstract)

[8] 祁若龙，周维佳，王铁军. 一种基于遗传算法的空间机械臂避障轨迹规划方法[J]. 机器人，2014，36(3)：263－270. Qi Ruolong, Zhou Weijia, Wang Tiejun. An obstacle avoidance trajectory planning scheme for space manipulators based on genetic algorithm[J]. Robot, 2014, 36(3): 263－270. (in Chinese with English abstract)

[9] 贾庆轩，陈钢，孙汉旭，等. 基于A*算法的空间机械臂避障路径规划[J]. 机械工程学报，2010，46(13)：109－115. Jia Qingxuan, Chen Gang, Sun Hanxu, et al. Path planning for space manipulator to avoid obstacle based on A* algorithm[J]. Journal of Mechanical Engineering, 2010, 46(13): 109－115. (in Chinese with English abstract)

[10] Gómez-Bravo F, Carbone G, Fortes J C. Collision free trajectory planning for hybrid manipulators[J]. Mechatronics, 2012, 22(6): 836－851.

[11] 蔡健荣，赵杰文，Thomas Rath，等. 水果收获机器人避障路径规划[J]. 农业机械学报，2007，38(3)：102－105. Cai Jianrong, Zhao Jiewen, Thomas Rath, et al. Path planning of fruits harvesting robot[J]. Transactions of the Chinese Society for Agricultural Machinery, 2007, 38(3): 102－105. (in Chinese with English abstract)

[12] 姚立健，丁为民，陈玉仑，等. 茄子收获机器人机械臂避障路径规划[J]. 农业机械学报，2008，39(11)：94－98. Yao Lijian, Ding Weimin, Chen Yulun, et al. Obstacle avoidance path planning of eggplant harvesting robot manipulator[J]. Transactions of the Chinese Society for Agricultural Machinery, 2008, 39(11): 94－98. (in Chinese with English abstract)

[13] 尹建军，武传宇，Yang Simon X，等. 番茄采摘机器人机械臂避障路径规划[J]. 农业机械学报，2012，43(12): 171－175. Yin Jianjun, Wu Chuanyu, Yang Simon X, et al. Obstacle- avoidance path planning of robot arm for tomato-picking robot[J]. Transactions of the Chinese Society for Agricultural Machinery, 2012, 43(12): 171－175. (in Chinese with English abstract)

[14] 罗陆锋，邹湘军，程堂灿，等. 采摘机器人视觉定位及行为控制的硬件在环虚拟试验系统设计[J]. 农业工程学报，2017，33(4)：39－46. Luo Lufeng, Zou Xiangjun, Cheng Tangcan, et al. Design of virtual test system based on hardware-in-loop for picking robot vision localization and behavior control[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2017, 33(4): 39－46. (in Chinese with English abstract)

[15] 谢龙，刘山. 基于改进势场法的机械臂动态避障规划[J]. 控制理论与应用，2018，35(9): 1239－1249. Xie Long, Liu Shan. Dynamic obstacle-avoiding motion planning for manipulator based on improved artificial potential filed[J]. Control Theory & Applications, 2018, 35(9): 1239－1249. (in Chinese with English abstract)

[16] 邹宇星，李立君，高自成. 基于改进PRM的采摘机器人机械臂避障路径规划[J]. 传感器与微系统，2019，38(1): 52－56. Zou Yuxing, Li Lijun, Gao Zicheng. Obstacle avoidance path planning for harvesting robotarm based on improved PRM[J]. Transducer and Microsystem Technologies, 2019, 38(1): 52－56. (in Chinese with English abstract)

[17] Liu Y P, Yang C H A, Huang L, et al. Visual system of citrus picking robot using convolutional neural networks[C]// International Conference on Systems and Informatics, [S.l.]: IEEE, 2018: 344－349. DOI: 10.1109/ICSAI.2018.8599325

[18] 李扬，杨长辉，胡友呈，等. 基于凸壳及距离变换的重叠柑橘目标识别与定位方法[J]. 现代制造工程，2018(9): 82－87. Li Yang, Yang Changhui, Hu Youcheng, et al. Overlapping citrus target recognition and localization method based on convex shell and distance transformation[J]. Modern Manufacturing Engineering, 2018(9): 82－87. (in Chinese with English abstract)

[19] 王毅，许洪斌，张茂，等. 仿蛇嘴咬合式柑橘采摘末端执行器设计与实验[J]. 农业机械学报，2018，49(10)：61－71. Wang Yi, Xu Hongbin, Zhang Mao, et al. Design and experiment of bite-model end-effector for citrus harvesting by simulating with mouth of snake[J]. Transactions of the Chinese Society for Agricultural Machinery, 2018, 49(10): 61－71. (in Chinese with English abstract)

[20] 王毅，付舜，张哲，等. 柑橘采摘机器人末端执行器设计与试验[J]. 中国农业科技导报，2018，20(1)：69－77. Wang Yi, Fu Shun, Zhang Zhe, et al. Design and experimentation on end-effector of citrus picking robot[J]. Journal of Agricultural Science and Technology, 2018, 20(1): 69－77. (in Chinese with English abstract)

[21] Pan Jia. Efficient configuration space construction and optimization for motion planning[J]. Engineering, 2015, 1(1): 46－57.

[22] 范启忠，陈是扦，刁燕，等. 基于Alpha Shapes三维重建原理的机器人工作空间求解方法[J]. 机械传动，2014，38(9): 27－29，59. Fan Qizhong, Chen Shiqian, Diao Yan, et al. Solution method of robot workspace based on Alpha Shapes 3D reconstruction principle[J]. Journal of Mechanical Transmission, 2014,38(9): 27－29,59. (in Chinese with English abstract)

[23] 张凤茹. 同胚映射在拓扑空间中的应用[J]. 哈尔滨师范大学自然科学学报，2010，26(4): 19－21. Zhang Fengru. The application of homeomorphic mapping in topological space[J]. Natural Science Journal of Harbin Normal University, 2010, 26(4): 19－21. (in Chinese with English abstract)

[24] Lavalle S M. Planning Algorithms[M]. Cambridge: Cambridge University Press, 2006: 127－133.

[25] Lozano-Perez T. Automatic Planning of Manipulator Transfer Movements[J]. IEEE Transactions on Systems Man & Cybernetics, 1980, 11(10): 681－698.

[26] Kuffner J J, Lavalle S M. RRT-connect: An efficient approach to single-query path planning[C]//IEEE International Conference on Robotics and Automation, 2000. Proceedings. ICRA. IEEE, 2002: 995－1001.

[27] Sun Z, Hsu D, Jiang T, et al. Narrow passage sampling for probabilistic roadmap planning[J]. IEEE Transactions on Robotics, 2005, 21(6): 1105－1115.

[28] ROS|Powering the world's robots[EB/OL]. http://www.ros. org/

[29] Pan J, Chitta S, Manocha D. FCL: A general purpose library for collision and proximity queries[C]//International Conference on Robotics and Automation. Saint Paul: IEEE, 2012: 3859－3866.

[30] Beeson P, Ames B. TRAC-IK: An open-source library for improved solving of generic inverse kinematics[C]// International Conference on Humanoid Robots. [S.l.]: IEEE, 2015: 928－935.

Motion planning of citrus harvesting manipulator based on informed guidance point of configuration space

Ma Jitong1, Wang Yi1,2※, He Yu1, Wang Kai1, Zhang Yitan1

(1.400054,; 2.400044,)

Harvesting robot is the representative of agricultural intellectualization. In the process of citrus harvesting, the manipulator sometimes needs to go deep into the canopy to harvest citrus. Many branches inside the canopy often form a closed polygon channel. Compared with single branch obstacle, the closed polygon obstacle is more difficult to avoid and it takes longer time to plan trajectory for obstacle avoidance. In order to solve this problem, an off-line configuration space mapping method is proposed in this paper, which can reduce the dimension and visualize the high-dimensional configuration space. The first three joints which have great influence on the manipulator are mainly considered, and the impact of the latter three joints on obstacle avoidance also taken into account, thus reducing the information lost in the process of dimension reduction. The visualization of configuration space for crawler chassis and obstacles has certain guiding significance for the later planning algorithm analysis. The topological properties of closed polygonal obstacles in configuration space are analyzed. The projection of closed polygonal obstacles in configuration space can be simplified into upper and lower parts. The upper and lower parts will be partially connected. The middle part is what the cavity is connected with the outside non-collision configuration space, with only two openings. If the end position of the manipulator is located in the cavity (the manipulator extends into the closed polygon), it can only pass through the upper and lower openings. According to this property, the bidirectional fast extended random tree algorithm (RRT-connect) is improved, and an RRT-connect algorithm with informed guidance point (IGPRRT-connect) based on prior knowledge guidance points in configuration space is proposed. The algorithm searches for narrow-channel guidance points in configuration space according to the topological properties of closed polygons and applies the bridge test algorithm to find the correct narrow-channel. Planning from the guiding point to the starting point and the end point respectively greatly speeds up the planning speed of the RRT-connect algorithm in a closed polygon environment. Taking the square box obstacle as an example, the simulation results show that RRT is faster than IGPRRT-connect when the side length is larger than 40 cm, while less than 40 cm, the IGPRRT-connect has advantages as follows: it takes 1.7 s and 1.2 s for RRT-connect and IGPRRT-connect algorithm respectively to motion planning for the obstacle with side length of 35 cm; that of 3.1 s and 1.6 s respectively for side length of 30 cm; when the edge length is reduced to 25 cm, the planning time for RRT-connect algorithm is as high as 18.1 s, while that of IGPRRT-connect is only 2.6 s, which decreases by 86%. At the same time, simulation experiments are carried out under different shapes of obstacles. The results show that IGPRRT-connect algorithm often takes less time than RRT-connect in closed polygon environment. Because IGPRRT-connect algorithm spends a lot of time in searching for non-existent boot configurations in unclosed polygon environment, RRT-connect algorithm performs better than IGPRRT-connect algorithm in unclosed polygon environment. In order to solve the problem of IGPRRT-connect algorithm in unclosed polygon environment, parallel programming for RRT-connect and IGPRRT-connect is carries out in this paper, and two threads are created: one thread runs RRT-connect algorithm and the other thread runs IGPRRT-connect algorithm. When one thread completes the planning, both threads stop completely and output the planned path, thus avoiding the artificial choice of which algorithm to use, which improves the intelligence of the harvesting robot. Parallel programming is beneficial to simplify the program solution: it is not needed to write the algorithm to judge whether RRT-connect or IGPRRT-connect should be used in the current environment. Simulation results show that the parallel algorithm performs well in various environments. Finally, an indoor obstacle avoidance experiment is carried out using parallel algorithm on the prototype of Citrus harvesting robot. In the experiment, the average planning time in the closed polygon obstacle environment is 1.431 s, the successful rate of obstacle avoidance is 88%, while that in the unclosed polygon obstacle environment are 1.064 s and 94%. The experimental results show that the IGPRRT-connect algorithm proposed in this paper has a good obstacle avoidance effect on both closed and unclosed obstacles, which is of great significance to the research of Citrus harvesting robot.

robots; motion planning; obstacle avoidance; configuration space; guidance point; RRT-connect; citrus harvesting

2018-11-14

2019-04-11

重庆市重点产业共性关键技术创新专项（cstc2015zdcyztzx70003）；重庆市基础科学与前沿技术研究一般项目（cstc2016jcyjA0444）资助

马冀桐，主要从事智能农业机械研究。Email：mjt0419@163.com

王 毅，博士生，副教授，主要从事智能农业机械研究。 Email：wangyi_cqut@163.com

10.11975/j.issn.1002-6819.2019.08.012

S24

A

1002-6819(2019)-08-0100-09

马冀桐，王 毅，何 宇，王 恺，张艺谭. 基于构型空间先验知识引导点的柑橘采摘机械臂运动规划[J]. 农业工程学报，2019，35(8)：100－108. doi：10.11975/j.issn.1002-6819.2019.08.012 http://www.tcsae.org

Ma Jitong, Wang Yi, He Yu, Wang Kai, Zhang Yitan.Motion planning of citrus harvesting manipulator based on informed guidance point of configuration space[J]. Transactions of the Chinese Society of Agricultural Engineering (Transactions of the CSAE), 2019, 35(8): 100－108. (in Chinese with English abstract) doi：10.11975/j.issn.1002-6819.2019.08.012 http://www.tcsae.org