基于RRT*FN算法的机械臂避障路径规划方法

基于rrt*fn算法的机械臂避障路径规划方法
技术领域
1.本发明涉及机器人、计算机视觉、人工智能技术领域，具体涉及一种基于rrt*fn算法的机械臂避障路径规划方法。


背景技术：

2.随着技术的进步与需求的增长机械臂在许多领域发挥着越来越重要的作用，比如物流行业货物的搬运、分拣等都是机械臂的用武之地。而随着领域的拓展家庭用或是物流用机械臂周围的环境不再只是如工业机械臂所处的固定简单环境，更多的是复杂多样的，所以对机械臂路径规划适应多种复杂环境的能力提出更高的要求。目前，针对机械臂高维空间路径规划通常采用基于采样的规划方法中更适用于处理高维空间路径规划问题的rrt算法。rrt算法旨在从起始点开始创建一棵树，通过向构型空间随机采样生长树来探索一条到达目标点，且不发生碰撞的路径，然而rrt随机性大这必然导致规划时间长且不稳定。rrt*fn算法是在rrt*基础上限制树中节点总数的最大值，即当节点数达到最大值时移除相应节点以保证增量采样过程中添加新节点而总节点数目保持不变，此方法虽然避免了树的规模无限增长节约了内存，但其规划速度并未得到较大改善并且其收敛精度低。
3.此外，环境的复杂必然会引起狭窄通道路径规划的问题，目前在这方面的路径规划研究尚处于探索阶段。现有技术中，kuffner等提出rrt-connect算法，该算法采用双树搜索状态空间极大地提高了搜索效率同时也为存在一侧狭窄通道的路径规划提供了解决方案，但其无法提供最优解并且在双侧狭窄通道环境下其算法几乎与rrt单支树没有差异。wu zhenping等提出随机转向扩展策略，该方法会在由随机采样点方向引导的扩容失败后随机生成一个方向进行扩容，如果随机点与新采样点组成的边是无障碍的，那么它们将被添加到树中，否则将采样另一个随机状态。随机转向扩展策略对解决狭窄通道路径规划问题有积极作用但存在很大的随机性。li bh等在解决狭窄通道问题上提出先判断环境类型再选择合适的方向重试扩展，极大地改善了含有狭窄通道环境的路径规划效果，由于环境类型存在多样性而li bh等所提方法并没有比较彻底解决环境类型多样问题。仍然存在复杂环境下路径处理时间较长且不能满足实时性要求，同时在狭窄通道环境下基本无法搜索到机械臂的有效路径的问题。


技术实现要素：

4.本发明的目的在于克服上述缺点而提出了一种能够在满足实时最优路径条件的同时，能适应多种场景的基于rrt*fn算法的机械臂避障路径规划方法。
5.本发明的一种基于rrt*fn算法的机械臂避障路径规划方法，该方法包括以下步骤:
6.s1：建立运动学模型：建立机械臂运动学模型，并根据建立的所述运行学模型进行机械臂运动学分析；
7.所述机械臂满足pieper原则，采用解析法进行机械臂运动学求逆解。
8.s2：碰撞检测：采用球包络法和方盒包络法，进行机械臂与障碍物以及机械臂连杆之间的碰撞检测；
9.s3：初始化工作空间：初始化机械臂运动的工作空间以及工作空间中的环境信息，所述工作空间分为自由区和障碍物区，并给定机械臂移动的初始点s
init
和机械臂移动的目标点s
goal
；
10.s4：利用改进的适用多场景的rrt*fn算法为机械臂规划全局路径，具体步骤如下：
11.s41：初始化搜索树，将初始点s
init
设为随机树根节点t
init
，初始化已扩展随机树区域t_r，将已扩展树上的节点距离目标点最近距离r设为s
init
到s
goal
之间距离大小；设定总节点数；
12.s42：生成随机采样点s
rand
，其过程如下：
13.s421：将整个采样空间标定为[0,1]区间，通过设定概率值p
goal_point
、概率值p
unexplored
和概率值p
t_r
，将[0,1]区间划分为四个子区间，分别为目标点区间[0,p
goal_point
]、未探索区间[p
goal_point
,p
unexplored
]、目标导向区间[p
unexplored
,p
t_r
]、已扩展树所在区间[p
t_r
,1]；
[0014]
s422：若随机产生的概率在[0,p
goal_point
]之间，则在以目标点为球心，一定半径的球内均匀采样；若随机产生的概率在[p
goal_point
,p
unexplored
]之间，则在未探索区内采样；若随机产生的概率在[p
t_r
,1]之间，则在已扩展树所在区域均匀采样；若以上均不符则从目标导向区间采样随机采样点；
[0015]
s43：遍历随机树，搜索随机树距离s
rand
最近随机树节点s
nearest
；
[0016]
s44：判断最近随机树节点s
nearest
的属性：若s
nearest
沿s
nearest
指向s
rand
方向行走距离step_to_random所得位置点，位于障碍物内并且边界扩展标记为0，则判断s
nearest
为未扩展的边界点，同时采用基于局部环境采样边界扩展策略生成新节点；若s
nearest
的边界扩展标记为1，则判断s
nearest
为已扩展边界点，新节点的扩展朝远离障碍物方向，也就是局部采样中障碍区点集的均值点指向最近随机树节点s
nearest
方向，此时步长设为step_to_random；否则，采用二分法贪婪扩展产生新节点；
[0017]
所述基于局部环境采样边界扩展策略生成新节点，包括以下步骤：
[0018]
步骤1：为获得足够多满足要求的采样点来描述局部结构，以s
nearest
为中心，分别将其空间坐标的每一维坐标独立取正负距离
±
step_to_random，形成s
nearest
为中心的空间坐标维数的两倍个采样点；然后分别以获取的所述采样点为中心，分别将其空间坐标的每一维坐标独立取正负距离的一半值
±
step_to_random/2，再形成新的采样点；
[0019]
步骤2：获得采样点集s
sampling_points
后将这些采样点划分为自由区采样点集s
loc_free
和障碍区采样点集s
loc_obs
，然后计算障碍区采样点集的均值点s
avepoint
；
[0020]
步骤3：若所述均值点在障碍物内，则将沿障碍区点集相距最远的两采样点s
obs1
和s
obs2
连线单方向或双向扩展形成新节点；若所述均值点在障碍物内并且按所述单方向或双方向扩展失败，则取自由区中距离s
nearest
最远的点作为新节点；
[0021]
所述沿障碍区点集相距最远的两点s
obs1
和s
obs2
连线单方向或双向扩展：若s
nearest
的父节点是边界点，朝两点s
obs1
和s
obs2
连线，并远离s
nearest
的父节点的单方向扩展，否则，以两点s
obs1
和s
obs2
连线进行双向扩展。
[0022]
步骤4：若所述均值点在自由区内并且与s
nearest
距离大于阈值λ，则判定s
nearest
为狭
窄通道外入口处的边界点，向所述均值点方向扩展形成新节点；若所述均值点在自由区内并且与s
nearest
距离小于λ，则沿自由区内相距最远的两采样点s
free1
和s
free2
连线朝向目标点s
goal
方向扩展。
[0023]
所述阈值λ小于距离step_to_random。
[0024]
所述采用二分法贪婪扩展产生新节点：该新节点的产生是在目标点引力和随机采样点引力共同下产生，并且其中的权重值采用二分法动态调节，初始步长选取随机采样点s
rand
与最近随机树节点s
nearest
之间的距离或最近随机树节点s
nearest
与目标点的距离中的最小值，当步长小于step_to_random时还发生碰撞，则将目标点引力大小设为步长值step_to_goal、随机采样点引力大小设为步长值step_to_random来扩展新节点，若碰撞检测结果还是处于碰撞状态，则返回s42重新产生随机采样点。
[0025]
s45：将产生的所述新节点朝目标点位置二次扩展，步长取障碍物之间的间隙的平均值；
[0026]
s46：筛查新节点扩展可行性，若新节点在树中已扩展节点的管辖区域内并且与已扩展节点之间无碰撞，则待扩展新节点视为不必要扩展点，舍弃处理并返回s42；
[0027]
所述树中已扩展节点的管辖区域：以已扩展节点为球心，以已扩展节点与其父节点之间距离为半径的球形区域；
[0028]
s47：判断搜索随机树的总节点数量是否超出设定的总节点数，若超出了，将以初始点和目标点为焦点的椭球外的叶子节点删除，并判断是否到达目标点位置，若没有，则返回s42重复执行；
[0029]
s5：路径优化：将所述改进rrt*fn规划的全局路径设置为当前路径，并根据路径代价公式计算代价值，以代价值最小化原则，采用基于三角不等式的方法对当前路径进行判断优化，所述代价公式为：
[0030][0031]
其中p(si)表示节点si实际路径成本，即从初始点到节点si走过的总步长；1/f(si)表示安全成本，f(si)为节点si所在位置与附近障碍物平均距离；t(si)表示稳定性成本，其值为si的父节点移动到节点si机械臂各关节平均变化率；k1、k2、k3分别为三个成本的系数代表它们占总成本的比重；
[0032]
所述采用基于三角不等式的方法对当前路径进行判断优化：从当前路径的目标点开始优化，如果节点si父节点的父节点作为si的父节点，在无碰撞条件下减小了si的成本时也即满足三角不等式法则时，则将si的父节点调整为si的父节点的父节点，当si父节点的父节点与si之间发生碰撞，则si这一节点优化完成，进入下一个si父节点的优化，直到初始点s
init
，完成优化进程。
[0033]
s6：平滑处理：采用三次b样条曲线光滑路径，并输出最优路径。
[0034]
本发明与现有技术的相比，具有明显的有益效果，由以上方案可知，本发明的采样方法，采用启发式抽样方法，引入目标区域的采样特性，从而更快速地逼近目标点位置。将机械臂工作空间划分为四个区域，在这些区域内按照一定的规则完成启发式抽样。采用二分法的贪婪展开法和二次展开法来提高算法的收敛速度。此外，优先删除椭球外的叶节点，解决节点移除问题，得到了更好的搜索路径和更快的搜索速度。节点扩展时，根据目标重力
和随机点重力生成新节点，并通过二分法动态调整权重值，将新节点扩展到目标点两次，以更快地获得更接近目标点的树扩展方向。对于边界点，通过扩展局部环境采样边界，可以有效解决通道狭窄、阶梯式障碍物路径规划的问题。为了优化路径，基于三角形不等式对冗余中间节点进行了简化。
[0035]
总之，本发明具有满足实时最优路径条件的同时，能够适应多种场景的特点。
[0036]
以下通过具体实施方式，进一步说明本发明的有益效果。
附图说明
[0037]
图1为本发明的流程结构图；
[0038]
图2为本发明的采样空间区域划分图；
[0039]
图3为本发明的基于局部环境采样边界扩展策略生成新节点图；
[0040]
图4为本发明的采用二分法贪婪扩展产生新节点图；
[0041]
图5为本发明的筛查新节点扩展可行性图；
[0042]
图6为本发明的基于三角不等式方法对当前路径进行判断优化图；
[0043]
图7为本发明实施案例仿真图。
具体实施方式
[0044]
以下结合附图及较佳实施例，对依据本发明提出的一种基于rrt*fn算法的机械臂避障路径规划方法的具体实施方式、特征及其功效，详细说明如下。
[0045]
参见图1，本发明的一种基于rrt*fn算法的机械臂避障路径规划方法，包括以下具体步骤：
[0046]
s1：建立机械臂运动学模型，并根据建立的模型进行机械臂运动学分析，所采用的机械臂满足pieper原则，因此采用解析法的求逆方法完成机械臂运动学求逆解；机器人运动学中的pieper准则是：机器人的三个相邻关节轴交于一点或三轴线平行；
[0047]
s2：采用球包络法和方盒包络法，进行机械臂与障碍物以及机械臂连杆之间的碰撞检测；
[0048]
s3：初始化机械臂运动的工作空间以及工作空间中的环境信息，给定机械臂移动的初始点s
init
和机械臂移动的目标点s
goal
；
[0049]
s4：利用改进的适用多场景的rrt*fn算法为机械臂规划全局路径，具体步骤如下：
[0050]
s41：初始化搜索树，将初始点s
init
设为随机树根节点t
init
，初始化已扩展随机树区域t_r，将已扩展树上的节点距离目标点最近距离r设为s
init
到s
goal
之间距离大小；
[0051]
s42：生成随机采样点s
rand
，其过程如下：
[0052]
s421：将整个采样空间划分为如图2所示四个区域，分别为目标导向区域、目标点区域、已扩展树所在区域和其他未探索区域；
[0053]
s422：通过p
goal_point
,p
unexplored
和p
t_r
三个参数将[0,1]区间划分成四个区间，依次表示目标点区、未探索区、目标导向区和已扩展树所在区的采样概率；
[0054]
s423：若随机产生的概率在[0,p
goal_point
]之间，则在以目标点为球心ε为半径的球内均匀采样，若随机产生的概率在[p
goal_point
,p
unexplored
]之间，则在未探索区内采样，若随机产生的概率在[p
t_r
,1]之间，则在已扩展树所在区域均匀采样，若以上均不符则从目标引导
区采样随机采样点。
[0055]
s43：遍历随机树，搜索随机树距离s
rand
最近随机树节点s
nearest
；
[0056]
s44：判断最近随机树节点s
nearest
的属性，若s
nearest
沿s
nearest
指向s
rand
方向行走step_to_random距离所得位置点位于障碍物内并且边界扩展标记为0，则判断s
nearest
为未扩展的边界点，同时采用基于局部环境采样边界扩展策略生成新节点，如图3所示；
[0057]
s441：为获得足够多满足要求的采样点来描述局部结构，以s
nearest
为中心分别将其每一维坐标独立
±
step_to_random形成s
nearest
维数的两倍个采样点，然后分别以这些采样点为中心重复之前采样方法只是这一次每一维坐标变化
±
step_to_random/2；
[0058]
s442：获得采样点集s
sampling_points
后将这些点划分为自由区点集s
loc_free
和障碍区点集s
loc_obs
，然后计算障碍区点集的均值点s
avepoint
；
[0059]
s443：若均值点在障碍物内，则新点将沿障碍区点集相距最远的两点s
obs1
和s
obs2
连线单方向或双向扩展，这取决于s
nearest
的父节点是否为已边界扩展的边界点，若是则朝两点连线远离s
nearest
的父节点的单方向扩展，若均值点在障碍物内并且按前述扩展失败，则取自由区中距离s
nearest
最远的点作为新点。
[0060]
s444：若均值点在自由区内并且与s
nearest
距离大于一个略小于step_to_random的阈值λ，则判定边界点s
nearest
在狭窄通道外入口处，新点尝试向均值点s
avepoint
方向扩展，若均值点在自由区内并且与s
nearest
距离小于λ，则沿自由区内相距最远的两点连线朝向目标点方向扩展。
[0061]
s45：判断最近随机树节点s
nearest
的属性，若s
nearest
的边界扩展标记为1，则判断s
nearest
为已扩展边界点，于是考虑到此时尽可能走出边界区是当下首选，所以在这种情况下新节点的扩展只朝远离障碍物方向，也就是局部采样中障碍区点集的均值点s
avepoint
指向最近邻节点方向，此时步长设为step_to_random。
[0062]
s46：判断最近随机树节点s
nearest
的属性，若既不满足s44也不满足s45条件，则采用二分法贪婪扩展产生新节点，如图4所示，新节点的产生是在目标引力和随机点引力共同作用下产生，并且其中的权重值采用二分法动态调节，初始步长选取随机点与最近节点之间的距离或最近节点距离目标点大小当中最小值，一直二分到步长刚小于step_to_random，若还是碰撞，则将目标引力大小设为step_to_goal、随机点引力大小设为step_to_random来扩展新节点，若碰撞检测结果还是处于碰撞状态则返回s42重新产生随机点。
[0063]
s47：将前述产生的新节点朝目标点二次扩展，步长取障碍物之间的间隙的平均值。
[0064]
s48：筛查新节点扩展可行性，如图5所示，若新节点在树中已扩展节点的管辖区域即已扩展节点与其父节点之间距离大小的圆区域内(这与重选父节点以及重写例程中的邻近节点s
near
求取时涉及的区域不同)并且与已扩展节点之间无碰撞，则待扩展新节点视为不必要扩展点，舍弃处理并返回s42；
[0065]
s49：判断搜索随机树的总节点数量是否超出了固定值，若超出了将以初始点和目标点为焦点的椭球外的叶子节点删除，并判断是否到达目标点位置，若没有返回s42重复执行；
[0066]
s5：将改进rrt*fn规划的全局路径path设置为当前路径，并根据路径代价公式1最小化原则而采用基于三角不等式的方法对当前路径进行判断优化，如图6所示；
[0067]
所述公式1为：
[0068][0069]
其中p(si)表示si点实际路径成本也就是从初始点到si点走过的总步长，1/f(si)表示安全成本且f(si)值为si所在位置与附近障碍物平均距离，t(si)表示稳定性成本其值为si父节点移动到当前点si机械臂各关节平均变化率。k1、k2、k3分别为三个子成本的系数代表它们占总成本的比重。
[0070]
s51：从当前路径的目标点开始优化，如果si父节点的父节点作为si的父节点在无碰撞条件下减小了si的成本时也即满足三角不等式法则时则将si的父节点调整为si的父节点的父节点，当si父节点的父节点与si之间发生碰撞则si这一节点优化完成进入下一个si父节点的优化，依次类推直到s
init
初始节点才结束整个优化进程；
[0071]
s52：在完成整个一轮优化后还可以在新得到的路径上插入相应节点进一步重复上面三角不等式优化过程。相应插入点根据以待优化点为基点沿所得路径移动一定距离获得，该距离为用球表示的所有障碍物体积的平均半径和目标点到待优化点si之间的直线长度的一半当中最小值。
[0072]
s6：采用三次b样条曲线光滑路径并生成最终也即当下最优路径。
[0073]
s7：通过所述规划的路径解来控制机械臂运动到目标点位置。
[0074]
性能分析：
[0075]
在python中构建三维避障环境，并与rrt*fn和rrt-connect算法进行了三维仿真比较。为了测试本发明方法在三维环境下对复杂狭窄通道的适应性，设置半径为2cm的小球为导航对象，安全距离delta为0.2cm，整个空间布局为100cm
×
100cm
×
100cm，狭窄通道之间的最小间隙为5cm。每种环境共进行1000组实验，得到的结果如表1所示。最终路径与障碍物之间的最短距离为0.3cm。本发明方法由于采用启发式抽样算法，在使用采样点方面更加准确有效。此外，将贪心展开法与二次展开策略相结合，可以使树更快地收敛到目标点位置，同时产生最优展开过程路径。特别地，对边界点采用了一种特殊而有效的边界展开方法。最后，基于三角形不等式对路径进行优化。因此，本发明方法不仅保证了实时性，而且在各种场景下都能比其他算法产生更好的路径。
[0076]
表1三维实验结果
[0077][0078]
为了验证本发明所述机械手路径规划方法的可行性，将所提算法应用于ros平台控制的六自由度串行机械手，预设的安全距离、扩展步长等超参数：起始点为(0，0，168)，目标点为(15，22，60)，通用步长step_to_random＝0.8cm，step_to_goal＝0.4cm，p
goal_point
＝0.1，初始时p
t_r
＝0.95，p
unexplored
＝0.2，固定最大节点数为2000，最大迭代次数为10000。
[0079]
如图7所示，本发明的路径规划方法引导机械手完成从起始位置到存放工件的货架一层指定位置的移动的实施案例仿真图，并完成工件排序任务t，对于任务t，使用rrt*、rrt*fn、rrt-connect和本发明方法进行了500次实验，提供的数据如表2所示。在完成任务t的整个规划过程中，机械手的运行时间不应超过80s；否则，该规划被视为无效。从表2的数据来看，本发明方法的规划效率更高，每次测试都是成功的。机械手一次规划的整个关节平均旋转角度越小，说明该方法具有更强的路径优化能力和节能能力。最大转角与平均转角相差较小，表明机械臂的运动更加稳定。
[0080]
表2在ros仿真中各种方法控制下完成任务t的结果
[0081][0082]
以上所述，仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，任何未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与修饰，均仍属于本发明技术方案的范围内。

技术特征：
1.一种基于rrt*fn算法的机械臂避障路径规划方法，其特征在于：该方法包括以下步骤:s1：建立运动学模型：建立机械臂运动学模型，并根据建立的所述运行学模型进行机械臂运动学分析；s2：碰撞检测：进行所述机械臂与障碍物以及所述机械臂连杆之间的碰撞检测；s3：初始化工作空间：初始化所述机械臂运动的工作空间以及工作空间中的环境信息，所述工作空间分为自由区和障碍物区，并给定所述机械臂移动的初始点s
init
和所述机械臂移动的目标点s
goal
；s4：利用改进的适用多场景的rrt*fn算法为所述机械臂规划全局路径，具体步骤如下：s41：初始化搜索树，将初始点s
init
设为随机树根节点t
init
，初始化已扩展随机树区域t_r，将已扩展树上的节点距离目标点最近距离r设为s
init
到s
goal
之间距离大小；设定总节点数；s42：生成随机采样点s
rand
，其过程如下：s421：将整个采样空间标定为[0,1]区间，通过设定概率值p
goal_point
、概率值p
unexplored
和概率值p
t_r
，将[0,1]区间划分为四个子区间，分别为目标点区间[0,p
goal_point
]、未探索区间[p
goal_point
,p
unexplored
]、目标导向区间[p
unexplored
,p
t_r
]、已扩展树所在区间[p
t_r
,1]；s422：若随机产生的概率在[0,p
goal_point
]之间，则在以目标点为球心，一定半径的球内均匀采样；若随机产生的概率在[p
goal_point
,p
unexplored
]之间，则在未探索区内采样；若随机产生的概率在[p
t_r
,1]之间，则在已扩展树所在区域均匀采样；若以上均不符则从目标导向区间采样随机采样点；s43：遍历随机树，搜索随机树距离s
rand
最近随机树节点s
nearest
；s44：判断最近随机树节点s
nearest
的属性：若s
nearest
沿s
nearest
指向s
rand
方向行走距离step_to_random所得位置点，位于障碍物内并且边界扩展标记为0，则判断s
nearest
为未扩展的边界点，同时采用基于局部环境采样边界扩展策略生成新节点；若s
nearest
的边界扩展标记为1，则判断s
nearest
为已扩展边界点，新节点的扩展采用朝远离障碍物方向的安全扩展策略，也就是局部采样中障碍区点集的均值点指向最近随机树节点s
nearest
方向，此时步长设为step_to_random；否则，采用二分法贪婪扩展产生新节点：该新节点的产生是在目标点引力和随机采样点引力共同下产生，并且其中的权重值采用二分法动态调节，初始步长选取随机采样点srand与最近随机树节点snearest之间的距离或最近随机树节点snearest与目标点的距离中的最小值，当步长小于step_to_random时还发生碰撞，则将目标点引力大小设为步长值step_to_goal、随机采样点引力大小设为步长值step_to_random来扩展新节点，若碰撞检测结果还是处于碰撞状态，则返回s42重新产生随机采样点；s45：将产生的所述新节点朝目标点位置二次扩展，步长取障碍物之间的间隙的平均值；s46：筛查新节点扩展可行性，若新节点在树中已扩展节点的管辖区域内并且与已扩展节点之间无碰撞，则待扩展新节点视为不必要扩展点，舍弃处理并返回s42；所述树中已扩展节点的管辖区域：以已扩展节点为球心，以已扩展节点与其父节点之间距离为半径的球形区域；s47：判断搜索随机树的总节点数量是否超出设定的总节点数，若超出了，将以初始点
和目标点为焦点的椭球外的叶子节点删除，并判断是否到达目标点位置，若没有，则返回s42重复执行；s5：路径优化：将所述改进rrt*fn规划的全局路径设置为当前路径，并根据路径代价公式计算代价值，以代价值最小化原则，采用基于三角不等式的方法对当前路径进行判断优化，所述代价公式为：其中p(s
i
)表示节点s
i
实际路径成本，即从初始点到节点s
i
走过的总步长；1/f(s
i
)表示安全成本，f(s
i
)为节点s
i
所在位置与附近障碍物平均距离；t(s
i
)表示稳定性成本，其值为s
i
的父节点移动到节点s
i
机械臂各关节平均变化率；k1、k2、k3分别为三个成本的系数代表它们占总成本的比重；s6：平滑处理：采用三次b样条曲线光滑路径，并输出最优路径。2.如权利要求1所述的基于rrt*fn算法的机械臂避障路径规划方法，其特征在于：在步骤s1中，所述机械臂满足pieper原则，采用解析法进行机械臂运动学求逆解。3.如权利要求1所述的基于rrt*fn算法的机械臂避障路径规划方法，其特征在于：在步骤s44中，所述基于局部环境采样边界扩展策略生成新节点，包括以下步骤：步骤1：为获得足够多满足要求的采样点来描述局部结构，以s
nearest
为中心，分别将其空间坐标的每一维坐标独立取正负距离
±
step_to_random，形成s
nearest
为中心的空间坐标维数的两倍个采样点；然后分别以获取的所述采样点为中心，分别将其空间坐标的每一维坐标独立取正负距离的一半值
±
step_to_random/2，再形成新的采样点；步骤2：获得采样点集s
sampling_points
后将这些采样点划分为自由区采样点集s
loc_free
和障碍区采样点集s
loc_obs
，然后计算障碍区采样点集的均值点s
avepoint
；步骤3：若所述均值点在障碍物内，则将沿障碍区点集相距最远的两采样点s
obs1
和s
obs2
连线单方向或双向扩展形成新节点；若所述均值点在障碍物内并且按所述单方向或双方向扩展失败，则取自由区中距离s
nearest
最远的点作为新节点；步骤4：若所述均值点在自由区内并且与s
nearest
距离大于阈值λ，则判定s
nearest
为狭窄通道外入口处的边界点，向所述均值点方向扩展形成新节点；若所述均值点在自由区内并且与s
nearest
距离小于λ，则沿自由区内相距最远的两采样点s
free1
和s
free2
连线朝向目标点s
goal
方向扩展。4.如权利要求3所述的基于rrt*fn算法的机械臂避障路径规划方法，其特征在于：在步骤3中，所述沿障碍区点集相距最远的两点s
obs1
和s
obs2
连线单方向或双向扩展：若s
nearest
的父节点是边界点，朝两点s
obs1
和s
obs2
连线，并远离s
nearest
的父节点的单方向扩展，否则，以两点s
obs1
和s
obs2
连线进行双向扩展。5.如权利要求3所述的基于rrt*fn算法的机械臂避障路径规划方法，其特征在于：在步骤4中，所述阈值λ小于距离step_to_random。6.如权利要求1所述的基于rrt*fn算法的机械臂避障路径规划方法，其特征在于：在步骤s5中，所述采用基于三角不等式的方法对当前路径进行判断优化：从当前路径的目标点开始优化，如果节点s
i
父节点的父节点作为s
i
的父节点，在无碰撞条件下减小了s
i
的成本时也即满足三角不等式法则时，则将s
i
的父节点调整为s
i
的父节点的父节点，当s
i
父节点的父
节点与s
i
之间发生碰撞，则s
i
这一节点优化完成，进入下一个s
i
父节点的优化，直到初始点s
init
，完成优化进程。7.如权利要求1所述的基于rrt*fn算法的机械臂避障路径规划方法，其特征在于：在步骤s2中，所述碰撞检测，采用球包络法和方盒包络法。

技术总结
本发明的一种基于RRT*FN算法的机械臂避障路径规划方法，包括以下步骤:建立机械臂运动学模型；碰撞检测；初始化工作空间；利用改进的适用多场景的RRT*FN算法为机械臂规划全局路径，其中随机采样点s


技术研发人员：袁庆霓 齐建友 王晨 高清扬 杜晓英 陈启鹏 杜飞龙 吕健 吴杨东 蓝伟文
受保护的技术使用者：贵州大学
技术研发日：2023.05.30
技术公布日：2023/8/13