基于RRT算法的智能包裹分拣系统路径规划方法与流程

本发明属于路径规划方法技术领域，涉及一种基于rrt算法的智能包裹分拣系统路径规划方法。

背景技术：

随着电子商务的迅猛发展，快递包裹数量的急剧增加，传统的人工分拣已不能满足要求，自动分拣系统能有效解决人工分拣所带来的问题，此系统具有能连续大批量的分拣运作，分拣误差率极低，分拣现场基本实现无人化特点，因此被广泛应用于现代物流领域。目前市场上传送带主要分为搬运机器人，agv小车，滚珠模块化传送带。但搬运机器人存在占地面积大，维护费用高等特点；agv小车对于快递包裹的尺寸重量有特定的要求；滚珠模块化传送带相比于传统的单向传输传送带具有维修方便，使用寿命长等特点，但包裹只能朝着特定的方向行进，具有转向不灵活的特点。因此，亟需发展智能模块化的传送带。基于智能传送带的快递包裹自动分拣系统涉及包裹分拣和路径规划两个重要的组成部分。

国内外研究学者对rrt算法做了很多改进。rrt算法具有对地图无需建模的优点，并且计算的复杂度不会随着障碍或威胁的增加而显著变化，可在复杂环境下找到路径规划的可行解，但在多数情况下，rrt算法规划出来的路径大多不是最优路径，最新位置点时是沿着选取子目标点的方向上生成的，因此形成的路径会有大量的转折点，另一个缺点是对于进入障碍区的位置点，需要重新生成该位置点，可能会存在多次迭代，影响位置点生成的质量。并且规划出来的路径需要做进一步平滑处理才能满足实际要求。

技术实现要素：

本发明的目的是提供一种基于rrt算法的智能包裹分拣系统路径规划方法，解决了现有技术中存在的路径生成随机性强，路径冗长迂回的问题。

本发明所采用的技术方案是，

基于rrt算法的智能包裹分拣系统路径规划方法，具体步骤为：

步骤1，提取包裹分拣系统平面图，获取平面图中包裹的入口、出口和障碍物的位置信息；

步骤2，以入口位置为当前位置，采用rrt算法进行路径规划初步获得包裹传送路径；其中引入人工势场引力分量，在节点扩展过程中引入随机生长函数与引力函数合力方向上的生长函数，设置步长，在生长函数方向上一个步长的位置即为下一步位置点；

步骤3，对初步获得的包裹传送路径利用贝塞尔曲线做进一步的平滑路径处理，获得最终的包裹传送路径。

本发明的特点还在于，

步骤2具体的为：

步骤2.1，设置入口位置为包裹的当前位置；

步骤2.2，在平面图区域内获取一个随机值q，0<q<1；若随机值小于设定的概率值0.6，则选择出口位置点作为当前子目标点，若随机值大于设置的概率值，则在平面图区域内随机生成一个坐标点为当前子目标点；

判断当前子目标点是否在障碍物区域中，若在障碍物区域则重新执行步骤2.2，即重新获取子目标点；否则执行步骤2.3；

步骤2.3，选择与当前子目标点和出口位置最近的坐标点作为当前位置点，引入随机生长函数与引力函数合力方向上的生长函数f(n)，设置步长，在f(n)方向上一个步长的位置即为下一步位置点；

步骤2.4，以当前位置点到下一步位置点做为初始向量，判断初始向量是否进入障碍物区域，若是，将初始向量作为待选向量并进行步骤2.5，否则进入步骤2.6；

步骤2.5，在待选向量逆时针方向和顺时针方向上以π/9的角度差分别扩展出5条向量，保留未进入障碍物区域的向量作为可选向量；将所有可选向量的长度作为启发函数，选择启发函数最小的向量对应的终点作为下一步位置点；

步骤2.6，将下一步位置点作为当前位置点，判断包裹当前位置是否在包裹出口区域内，若在包裹出口区域内，进行步骤2.7，否则转入步骤2.2；

步骤2.7，步骤2.1～步骤2.6中从入口位置走到出口区域内的所有当前位置点即为初步获得包裹传送路径。

步骤2.2中，判断当前子目标点是否在障碍物区域的方法为：设定障碍物安全距离，判断包裹当前子目标点位置与障碍物中心点的距离是否大于障碍物的安全距离；若是，则认为当前子目标点不在障碍物区域内，否则认为当前子目标点在障碍物区域内。

步骤2.3中生长函数f(n)计算方式为：

f(n)＝r(n)+g(n)(1)

其中r(n)为当前位置点到当前子目标点方向上的随机生长函数，公式为：

r(n)＝||qtarget-qnear||(2)

其中qtarget为当前子目标点的位置坐标，qnear为当前位置点的位置坐标；

出口位置点对于当前位置点的引力势场函数u为：

其中qgoal为出口位置点的坐标点，kp为出口位置对于包裹当前位置点的引力系数；g(n)为当前位置点到出口位置点的引力函数，即为对应引力场函数u的负梯度，其公式为：

g(n)＝kp||qgoal-qnear||(4)

联合公式(1)～(4)即可得到：

f(n)＝||qtarget-qnear||+kp×||qgoal-qnear||(5)

综上，下一步位置点的计算公式为：

qnew＝qnear+q_dist×(||qtarget-qnear||+kp×||qgoal-qnear||)(6)

其中qnew为下一步位置点，q_dist为步长。

步骤2.4中，判断初始向量是否进入障碍物区域的方法为：在初始向量上选取0.5cm间隔坐标位置点，设置障碍物安全距离，比较间隔坐标位置点到障碍区中心的距离是否大于安全距离，若大于安全距离，则认为初始向量未进入障碍物区域；否则认为初始向量进入障碍物区域。

步骤3的具体方法为：

步骤3.1，设初步获得的包裹传送路径中的所有位置点为{xi,i＝1,2λn}，从i＝1开始，判断是否在障碍物区域内，若在则令i＝i+1，继续判断是否在障碍物区域内，否则删除xi+1，令i＝i+2，继续判断是否在障碍物区域内；直到i＞i+2，停止操作，获取新的优化路径；

步骤3.2，选择一阶贝塞尔曲线对新的优化路径作进一步的平滑处理，获得最终的包裹传送路径。

步骤3.2具体的为：假设新的优化路径节点数为n个，依次在两节点之间取中间节点，则可取n-1个节点，判断相连节点之间的线段是否在障碍物区域内，若不在障碍物区域内，用中间节点替换两端节点；若在障碍物区域内，则保留原有节点，依次类推，直到最后一个节点，更新后的所有节点组成的路径即为最终的包裹传送路径。

本发明的有益效果是

一、在rrt算法的基础上引入人工势场引力分量，在节点扩展过程中引入随机生长函数与引力函数合力方向上的生长函数，设置步长，在生长函数方向上一个步长的位置即为下一步位置点；引力分量的作用可以限制节点生成的范围，其次在此范围内提高路径的生成质量，可使得每次仿真结束后路径轨迹差异不大，一定程度上提高算法的速度；

二、在避障过程中加入避障处理过程，生成扇形区域，加入启发函数，在多条待选向量中选择最优向量迭代初始向量，对路径做进一步优化，得到最近的避障向量；

三、在得到初步包裹传送路径后做进一步平滑处理，提高路径灵活度，使得规划后的路径更短，传输效率更快。

附图说明

图1是本发明基于rrt算法的智能包裹分拣系统路径规划方法的流程图；

图2是本发明基于rrt算法的智能包裹分拣系统路径规划方法实施例1所获得的平面图；

图3是本发明基于rrt算法的智能包裹分拣系统路径规划方法实施例1获得的初步包裹传送路径图；

图4是本发明基于rrt算法的智能包裹分拣系统路径规划方法实施例1获得的最终包裹传送路径图；

图5是采用传统的rrt算法得到的包裹传送路径图；

图6为采用rrt+人工势场法得到的包裹传送路径图。

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

本发明基于rrt算法的智能包裹分拣系统路径规划方法，如图1，具体步骤为：

步骤1，提取包裹分拣系统平面图，获取平面图中包裹的入口、出口和障碍物的位置信息；

步骤2，以入口位置为当前位置，采用rrt算法进行路径规划获得初步包裹传送路径；其中引入人工势场引力分量，在节点扩展过程中引入随机生长函数与引力函数合力方向上的生长函数，设置步长，在生长函数方向上一个步长的位置即为下一步位置点；

步骤3，对初步获得的包裹传送路径利用贝塞尔曲线做进一步的平滑路径处理，获得最终的包裹传送路径。

其中步骤2具体的为：

步骤2.1，设置入口位置为包裹的当前位置；

步骤2.2，在平面图区域内获取一个随机值q，0<q<1；若随机值小于设定的概率值0.6，则选择出口位置点作为当前子目标点，若随机值大于设置的概率值，则在平面图区域内随机生成一个坐标点为包裹当前子目标点；

判断当前子目标点是否在障碍物区域中，若在障碍物区域则重新执行步骤2.2，即重新获取子目标点；否则执行步骤2.3；

判断当前子目标点是否在障碍物区域的方法为：设定障碍物安全距离，判断包裹当前子目标点位置与障碍物中心点的距离是否大于障碍物的安全距离；若是，则认为当前子目标点不在障碍物区域内，否则认为当前子目标点在障碍物区域内。

步骤2.3，选择与当前子目标点和出口位置最近的坐标点作为当前位置点，引入随机生长函数与引力函数合力方向上的生长函数f(n)，设置步长，在f(n)方向上一个步长的位置即为下一步位置点；

生长函数f(n)计算方式为：

f(n)＝r(n)+g(n)(1)

其中r(n)为当前位置点到当前子目标点方向上的随机生长函数，公式为：

r(n)＝||qtarget-qnear||(2)

其中qtarget为当前子目标点的位置坐标，qnear为当前位置点的位置坐标；

包裹出口位置点对于当前位置点的引力势场函数u为：

其中qgoal为出口位置点的坐标点，kp为出口位置对于当前位置点的引力系数；g(n)为当前位置点到出口位置点的引力函数，即为对应引力场函数u的负梯度，其公式为：

g(n)＝kp||qgoal-qnear||(4)

联合公式(1)～(4)即可得到：

f(n)＝||qtarget-qnear||+kp×||qgoal-qnear||(5)

综上，下一步位置点的计算公式为：

qnew＝qnear+q_dist×(||qtarget-qnear||+kp×||qgoal-qnear||)(6)

其中qnew为下一步位置点，q_dist为步长。

步骤2.4，以当前位置点到下一步位置点做为初始向量，判断初始向量是否进入障碍物区域，若是，将初始向量作为待选向量并进行步骤2.5，否则进入步骤2.6；

判断初始向量是否进入障碍物区域的方法为：在初始向量上选取0.5cm间隔坐标位置点，设置障碍物安全距离，比较间隔坐标位置点到障碍区中心的距离是否大于安全距离，若大于安全距离，则认为初始向量未进入障碍物区域；否则认为初始向量进入障碍物区域。

步骤2.5，在待选向量逆时针方向和顺时针方向上以π/9的角度差分别扩展出5条向量，保留未进入障碍物区域的向量作为可选向量；将所有可选向量的长度作为启发函数，选择启发函数最小的向量对应的终点作为下一步位置点；

上述步骤2.4到步骤2.6为局部避障方法，在避障过程中加入避障处理过程，生成扇形区域，加入启发函数，在多条待选向量中选择最优向量迭代初始向量，对路径做进一步优化。

步骤2.6，将下一步位置点作为当前位置点，判断包裹当前位置是否在包裹出口区域内，若在包裹出口区域内，进行步骤2.7，否则转入步骤2.2；

判断包裹当前位置是否在包裹出口区域内的方法为：设置阈值e，若包裹出口位置与包裹当前位置点之间的距离小于阈值e，则判定包裹到达出口位置，否则继续进行路径规划。

步骤2.7，所述步骤2.1～步骤2.6中从入口位置走到出口区域内的所有当前位置点即为初步包裹传送路径。

步骤3的具体方法为：

步骤3.1，设初步的包裹传送路径中的所有位置点为{xi,i＝1,2λn}，从i＝1开始，判断是否在障碍物区域内，若在则令i＝i+1，继续判断是否在障碍物区域内，否则删除xi+1，令i＝i+2，继续判断是否在障碍物区域内；直到i＞i+2，停止操作，获取新的优化路径；

步骤3.2，选择一阶贝塞尔曲线对新的优化路径作进一步的平滑处理，获得最终的包裹传送路径；具体的，假设新的优化路径节点数为n个，依次在两节点之间取中间节点，则可取n-1个节点，判断相连节点之间的线段是否在障碍物区域内，若不在障碍物区域内，用中间节点替换两端节点；若在障碍物区域内，则保留原有节点，依次类推，直到最后一个节点，更新后的所有节点组成的路径即为最终的包裹传送路径。

实施例1

执行步骤1，得到的平面图如图2，其中右上角菱形为出口位置点，左下角菱形为入口位置点、黑色为障碍物；

执行步骤2，得到的初步获得包裹传送路径，如图3；在加入引力分量的基础上采用局部区域避障策略，使得包裹对于环境障碍物信息敏感，在碰到障碍物的时候可快速改变方向绕开障碍物行走。

执行步骤3，得到最终的包裹传送路径，如图4，采用二次平滑策略使得rrt算法生成的节点数目减少，并且得到较为平滑的路径。此策略对于有尖锐夹角的节点有很好的平滑效果，使得包裹的转向符合实际情况中的包裹转向。

对于相同的包裹分拣系统平面图，采用传统的rrt算法，得到的路径如图5，可以看到节点扩展方式随机，对于环境信息感知不敏感。并且会出现回溯的现象；采用rrt+人工势场法得到的路径如图6，rrt+人工势场法即在传统rrt的基础上，对于进入障碍物区域的点采用人工势场法确定下一步位置点，如图可知，上述方法也可得到良好路径，但是对于仿真结果，如表1所示，可以看出其仿真时间更长，路径的长度也更长，另外人工势场法需要人工合理设置以满足转向要求，人为因素增加了得到最优路径的难度。

表1仿真结果对比

综上所述，本发明基于rrt算法的智能包裹分拣系统路径规划方法的路径不存在回溯现象、路径转向更加灵活、路径更短，传输效率更快。