CN111546347B - 一种适用于动态环境下的机械臂路径规划方法 - Google Patents

Abstract

本发明属于三维空间中机器人路径规划以及生成技术领域，尤其涉及一种用于动态环境下的机械臂路径规划方法。包括路径规划阶段以及重新规划阶段：在路径规划阶段，根据当前的环境和目标点通过路径规划算法得到搜索树和路径点；根据上述信息用轨迹规划的方法对路径点进行插值，获得初始规划路径，完成机械臂运动控制；在重新规划阶段：根据传感器监测的情况来判断机械臂当前工况是否需要重新规划路径；当前工况被检测到阻断时，通过路径规划算法对初始路径进行修复，并将更新后的路径重新进行轨迹插值，本发明更适应于复杂动态避障问题的处理，能够有效减少待处理数据总量，进而针对复杂工况，能够持续减少路径规划的时间，同时具有极高的有效性。

Description

一种适用于动态环境下的机械臂路径规划方法

技术领域

本发明属于三维空间中机器人路径规划以及生成技术领域，尤其涉及一种用于动态环境下的机械臂路径规划方法。

背景技术

在动态避障规划中，规划器需要根据更新后的环境重建或者修复路径。而动态环境类似于静态环，通常将其视为真实世界更新与规划器系统交替进行。动态避障的必要过程如下：1、在静态的环境下进行机械臂的运动规划。2、开始机械臂运动。3、更新环境信息。4、如果执行路径被监测到阻断，则规划器将重新规划或者修复当前路径以适应新的环境。5、机械臂继续运动。在机械臂执行任务时，外界传感器(如摄像头)监测其工作环境中存在运动的障碍物影响到机械臂执行路径时，如何快速地修正路径是机械臂在动态障碍物环境下进行作业规划的关键。但受不同规划方案侧重点以及数据处理效率的不同，经常难以在处理效率以及路径规划成功率上取得较好的平衡，特别是在动态避障问题上，传统方法不仅耗时良久，且难以保证路径规划的有效性，限制了机械臂等机器人的发展。

发明内容

本发明创造的目的在于，提供一种效率高于传统路径巡游方法，同时具有高有效性，同时适用于动态环境下的机械臂路径规划方法。

为实现上述目的，本发明创造采用如下技术方案。

一种适用于动态环境下的机械臂路径规划方法，包括路径规划阶段以及重新规划阶段：

路径规划阶段，是指根据当前的环境和目标点通过路径规划算法得到搜索树和路径点；根据上述信息用轨迹规划的方法对路径点进行插值，获得初始规划路径，完成机械臂运动控制；包括如下步骤：

步骤一、基于概率统计的运动不确定性建模来预测动态障碍物运动趋势：

A1.针对移动障碍物运动速度的大小和运动方向的不确定性，设每个移动障碍物的运动速度和方向的取值范围分别表示为[v_min，v_max]和[θ_min，θ_max]，在时间周期ΔT内速度和方向保持不变，在ΔT之后，二者可能会有改变，基于此，将障碍物速度的变化量和障碍物运动角度的变化量分别用正态分布描述：

采用正态分布描述移动障碍物速度v的变化量和障碍物运动角度θ的变化量：对于i≥0，有v(i+1)＝v(i)+Δv；θ(i+1)＝θ(i)+Δθ；其中

均服从正态分布：

A2.采用极大似然法构建移动障碍物Δv，Δθ的期望和方差；确定速度和方向保持不变的间周期ΔT，统计ΔT时间周期内移动障碍物运动的速度和方向信息v(i)、θ(i)，(i＝1、2......n)构建样本集；采用极大似然法构建Δv，Δθ的期望和方差

对Δv有：

对Δθ有：

在重新规划阶段：根据传感器监测的情况来判断机械臂当前工况是否需要重新规划路径；若当前工况被检测到阻断时，通过路径规划算法对初始路径进行修复，并将更新后的路径重新进行轨迹插值，使得机械臂能够连续平滑运行；

步骤二、初始路径预处理，具体是指：

B1按照初始规划的路径移动，当检测到初始路径被动态障碍物阻断时，删除掉被障碍物影响的无效路径点，抛弃已经经过的路径点及其分支；

具体而言：机械臂按照初始规划的路径移动，当传感器检测到初始路径被动态障碍物(即黑色圆圈)阻断时，删除掉被障碍物影响的无效路径点，抛弃已经经过的路径点及其分支，整理后，路径分为V_valid和V_backward，基于该步骤以减少查询次数和计算量；

B2、基于前述步骤对机械臂参数进行计算,不断用新采集来的数据对样本集进行更新,以保证估计的准确性；当使用一定数量的数据取得所需的参数后；再利用3σ规则,即对正态随机变量来说,它的取值落在区间[-3e,3e],并根据此规律以i＝1,2,...,n时刻的移动障碍物的运动状态对i＝n+1时刻障碍物的速度,方向和位置的不确定性进行描述，持续利用更新后的数据进行轨迹插值，得到新的路径规划。

对前述适用于动态环境下的机械臂路径规划方法的进一步改进，所述步骤B1还包括：

B1.1基于中继点的初始路径预处理

按照初始规划的路径移动，当检测到初始路径被动态障碍物阻断时，计算中继节点的位置同时在更新后的场景中规划新路径，若到达中继点之前没有完成新路径规划，则停止移动并等待新路径规划完成；否则机械臂将通过中继节点而不会暂停；

对于路径点V和中继节点q_relay，当前位置为q_current，q_obstacle为障碍物边界与V相交且距离最近的点，θ是动态障碍物和q_current的连线与动态障碍物速度方向v_obstacle之间的夹角，移动速度为v_agent(v_agent≤v_max)；当初始路径被中断时，动态障碍物的速度和运动角度的变换分别定义为v′_obstacle＝v_obstacle+Δv_andΔθ；找到障碍边界与路径相交的位置q_obstacle，中继节点的位置被定义为q_relay←q_current+k，表示机械臂沿执行轨迹从当前位置前进k的距离且

B1.2当初始路径被动态障碍物阻断时，先计算出中继点的位置，在到达中继点时重新判断障碍物与初始路径的关系，若检测到两者碰撞，则认为障碍物仍影响这初始路径，利用Reconnect方法获取修复路径；

具体而言，判断在V_vaild和V_backward所属的节点在拓展步长内，是否存在可以直接一步连接V_vaild和V_backward的节点且不会与障碍物发生碰撞，如果存在这样的节点，则将V_vaild的和V_backward的节点连接，以完成路径修复；当检测到路径被障碍物阻断时，生成中继节点并且删除无效节点，最后连接以修复路径；

当Reconnect策略无法完成任务时，使用Regrow策略利用中继节点，不再考虑中继点之前的搜索树分支各节点；只查询中继点之后搜索树的末端到V_valid代价最低的点作为新的起始点，同时设置V_valid为新目标，只要满足误差允许，则新的拓展树将扩展到V_valid中的任何点，进而修复路径方案；

对前述适用于动态环境下的机械臂路径规划方法的进一步改进，当使用Regrow方法开始重新规划时，增加路径缓存的步骤，具体而言：随机选择路径缓存区中的关键点；使用初始解决方案的信息来尽可能多地修复路径，定义Prand为在空间中随机采样的概率，则P_goal+P_way+P_rand＝1，其中P_goal表示向目标的拓展概率，P_way表示向V_vaild的拓展概率；在进行路径缓存过程中，设置两个随机数p(p∈(0,1))和i(i＝1，2,...,n)；其中N是路径缓存区中的点个数；若0<p<P_way，则新节点将沿V_vaild(i)的方向拓展，若P_way<p<P_way+P_goal，则新节点将沿q_goal的方向拓展，否则，新节点将沿随机方向拓展。

其有益效果在于：

本发明的用于动态环境下的机械臂路径规划方法，更适应于复杂动态避障问题的处理，能够有效减少待处理数据总量，进而针对复杂工况，能够持续减少路径规划的时间，同时具有极高的有效性，适应实际生产作业需求，避免危险发生。

附图说明

图1是本发明的机械臂动态路径规划器的原理示意图；

图2动态环境中机械臂按照初始规划的路径移动示意图；

图3是本发明中部分参数的定义参照图；

图4在机械臂工作空间中随机设置球形动态障碍物时机械臂动态避障动作分解示意图；

图5在map1工况下机械臂各关节点动作变化数据；

图6在机械臂工作空间中随机设置桌形动态障碍物时机械臂动态避障动作分解示意图；

图7在map2工况下机械臂各关节点动作变化数据。

具体实施方式

以下结合具体实施例对本发明创造作详细说明。

在机械臂执行任务时，外界传感器(如摄像头)监测其工作环境中存在运动的障碍物影响到机械臂执行路径时，如何快速地修正路径是机械臂在动态障碍物环境下进行作业规划的关键。因障碍物而导路径失效的问题，目前一般有两种做法：一种是放弃已经规划的路径，从当前位置重新规划一条路径；另一种做法是建立缓存空间，保留上次规划路径中有用的信息，为下一次规划打下基础。

而在动态避障规划中，规划器需要根据更新后的环境重建或者修复路径。而动态环境类似于静态环，通常将其视为真实世界更新与规划器系统交替进行。动态避障的必要过程如下：1、在静态的环境下进行机械臂的路径规划；2、开始机械臂运动；3、更新环境信息；如果执行路径被监测到阻断；4、则规划器将重新规划或者修复当前路径以适应新的环境；5、机械臂继续运动；重复上述步骤。

在路径规划的基础上，可以将避障规划分为静态环境下的避障规划和动态环境下的避障规划，动态环境中的避障规划包括已知环境和未知或部分未知环境。当环境或目标发生变化时，在运动过程中对路径进行重新规划，使新轨迹与原轨迹顺利连接，这个过程中机械臂不会停止。设环境中有一个静态且先验障碍物集合O(c)，其中c∈R。另外设置一个动态障碍物集合O(t)，其中t≥0。在大多数情况下，动态环境中预测障碍物运动是困难的。

因此对于机械臂而言，在其工作空间中假设认为先验信息由传感器接收。对于其建模四路：给定一个构型时间混合空间，路径规划仅仅是在混合空间规划一条几何路径，而动态障碍物则为C-space带来时间信息。设可度量空间的状态问题描述为

其中d∈N,d≥2。因此

，则障碍物统一描述为

,在S_obs的范围内机械臂是发生碰撞的。则无障碍物碰撞范围被定为S_free＝S\S_obs，这些区域是机械臂可以达到的。设搜索树T(N,V,E)是S的一部分,其中N,V,E分别是搜索树节点集合,路径点集合和搜索树边集合。在任意时间t的路径点被定义为V(t),其中V:[t₀,t_end]→S是从起点时间t₀到终点时间t_end的离散函数。设机械臂可执行轨迹为τ(q_start,q_goal)，τ是在S上连续的映射表示为：τ:[0,1]→S，0→q_start并且1→q_goal。τ是由机械臂的kinodynamic¹或其他限制决定的,例如最大加速度。起始点q_start和终点q_goal都属于V。设d(q_i,q_j)是在S上的欧氏距离,其中所有的q_i,q_j∈S,i≠j。

基于上述思路以及理论基础之上，针对移动障碍物运动速度的大小和运动方向的不确定性，本发明提出一种基于概率统计的方法为移动障碍物的不确定性进行建模，根据中心极限理论，将移动障碍物运动的速度和方向用正态分布来描述，通过数理统计方法对正态分布的参数进行估计，保证证快速收敛的同时避免常见的路径规划问题(如局部最小值)，结合中继节点方法和Connection理论为动态修复原理，同时采用路径缓存和KD-tree四路提高搜索速度和搜索效率，以更高效快捷的实现动态环境下的机械臂路径规划。

基于本发明的机械臂动态路径规划器基本原理如图1所示，具体而言，本发明的一种适用于动态环境下的机械臂路径规划方法，包括路径规划阶段以及重新规划阶段：

路径规划阶段，是指根据当前的环境和目标点通过路径规划算法得到搜索树和路径点；根据上述信息用轨迹规划的方法对路径点进行插值，获得初始规划路径，完成机械臂运动控制；包括如下步骤：

步骤三、基于概率统计的运动不确定性建模来预测动态障碍物运动趋势：

A3.采用正态分布描述移动障碍物速度v的变化量和障碍物运动角度θ的变化量：对于i≥0，有v(i+1)＝v(i)+Δv；θ(i+1)＝θ(i)+Δθ；其中

均服从正态分布：

A4.采用极大似然法构建移动障碍物Δv，Δθ的期望和方差；确定速度和方向保持不变的间周期ΔT，统计ΔT时间周期内移动障碍物运动的速度和方向信息v(i)、θ(i)，(i＝1、2......n)构建样本集；采用极大似然法构建Δv，Δθ的期望和方差

对Δv有：

对Δθ有：

A5.建立移动障碍物运动不确定性描述；不断更新数据对样本集进行更新,在数据量足够取得所需的参数后；利用正态随机变量的值落在区间[-3e,3e]的规律根据i＝1,2,...,n时刻的移动障碍物的运动状态对i＝n+1时刻障碍物的速度,方向和位置的不确定性进行描述；

在重新规划阶段：根据传感器监测的情况来判断机械臂当前工况是否需要重新规划路径；若当前工况被检测到阻断时，通过路径规划算法对初始路径进行修复，并将更新后的路径重新进行轨迹插值，使得机械臂能够连续平滑运行；

步骤四、初始路径预处理，具体是指：

B1按照初始规划的路径移动，当检测到初始路径被动态障碍物阻断时，删除掉被障碍物影响的无效路径点，抛弃已经经过的路径点及其分支；

在动态环境中，如图2，机械臂按照初始规划的路径移动，当传感器检测到初始路径被动态障碍物(即黑色圆圈)阻断时，删除掉被障碍物影响的无效路径点，抛弃已经经过的路径点及其分支，整理后，路径分为V_valid和V_backward，基于该步骤，可以有效减少查询次数和计算量。

B1.1基于中继点的初始路径预处理

按照初始规划的路径移动，当检测到初始路径被动态障碍物阻断时，计算中继节点的位置同时在更新后的场景中规划新路径，若到达中继点之前没有完成新路径规划，则停止移动并等待新路径规划完成；否则机械臂将通过中继节点而不会暂停，由于中继节点的计算只需花费极短的时间，因此前一种情况几乎不发生。

如图3所示，对于路径点V(实心小黑点)和中继节点q_relay(空心小圆圈)，当前位置为q_current，q_obstacle为障碍物边界与V相交且距离最近的点，θ是动态障碍物和q_current的连线与动态障碍物速度方向v_obstacle之间的夹角，移动速度为v_agent(v_agent≤v_max)；当初始路径被中断时，动态障碍物的速度和运动角度的变换分别定义为v′_obstacle＝v_obstacle+ΔvandΔθ；找到障碍边界与路径相交的位置q_obstacle，中继节点的位置被定义为q_relay←q_current+k，表示机械臂沿执行轨迹从当前位置前进k的距离且

B1.2当初始路径被动态障碍物阻断时，先计算出中继点的位置，在到达中继点时重新判断障碍物与初始路径的关系，若检测到两者碰撞，则认为障碍物仍影响这初始路径，利用Reconnect方法获取修复路径；

具体而言，判断在V_vaild和V_backward所属的节点在拓展步长内，是否存在可以直接一步连接V_vaild和V_backward的节点且不会与障碍物发生碰撞，如果存在这样的节点，则将V_vaild的和V_backward的节点连接，以完成路径修复，如图2所示，图2(a)描绘了机械臂在初始环境中沿路径移动的情况。当检测到路径被障碍物阻断时，生成中继节点如图2(b)所示，并且删除无效节点，该过程只需要在r_R的范围内搜索即可，如图2(c)所示。如图2(d)，最后直接一步连接以修复路径。

当Reconnect策略无法完成任务时，使用Regrow策略。再次利用中继节点如图2(b)，不再考虑中继点之前的搜索树分支各节点，降低查询过程中计算量。所以只需要查询中继点之后搜索树的末端到V_valid代价最低的点作为新的起始点，同时V_valid被设置为新目标，只要满足误差允许，则新的拓展树将扩展到V_valid中的任何点，进而修复路径方案。

进一步地，当使用Regrow方法开始重新规划时，增加路径缓存的步骤，具体而言是指：随机选择路径缓存区中的关键点。其中P_goal和P_way分别表示向目标的拓展概率和向V_vaild的拓展概率，通过这样的方法，可以使用初始解决方案的信息来尽可能多地修复路径，定义Prand为在空间中随机采样的概率，则P_goal+P_way+P_rand＝1。在进行路径缓存过程中，设置两个随机数p(p∈(0,1))和i(i＝1，2,...,n)。其中N是路径缓存区中的点个数。若0<p<P_way，则新节点将沿V_vaild(i)的方向拓展，若P_way<p<P_way+P_goal，则新节点将沿q_goal的方向拓展，否则，新节点将沿随机方向拓展。

B2、基于前述步骤对机械臂参数进行计算,不断用新采集来的数据对样本集进行更新,以保证估计的准确性；当使用一定数量的数据取得所需的参数后；再利用“3σ”规则,即对正态随机变量来说,它的值落在区间[-3e,3e],并根据此规律以i＝1,2,...,n时刻的移动障碍物的运动状态对i＝n+1时刻障碍物的速度,方向和位置的不确定性进行描述，持续利用更新后的数据进行轨迹插值，得到新的路径规划。

以下对本申请的适用于动态环境下的机械臂路径规划方法的效果进行实例验证：

以Aubo-i5协作机械臂为研究对象，对本申请的适用于动态环境下的机械臂路径规划方法及进行性能进行分析。为了验证可靠性，在ROS中实现可视化工具Rviz。将本申请的适用于动态环境下的机械臂路径规划方法通过算法编辑置入到OMPL规划器中，将动态策略设计为move-group接口的节点运行。

一般而言，机械臂工作在一个复杂的环境中，包括静止障碍物和动态障碍物。因此，在Map1和Map2中设置了动态避障环境，以测试基于本申请方法的路径规划能力。对实验进行了100次仿真，设置一个随机设置的球形障碍物，最大速度和最大加速度控制在0.3以内。各关节的起始位姿和目标位姿设置如表2所示。

表1各关节的起始位姿和目标位姿

	关节点0	关节点1	关节点2	关节点3	关节点4	关节点5
							起始位姿	1.546	0.007	-1.040	0.486	-1.548	1.594
目标位姿	1.546	0.363	-2.215	-1.045	-1.548	1.539

两个地图的环境设置如下：

地图一

在图4中，map1在机械臂工作空间中随机设置球形动态障碍物，一次完整的机械臂动态避障仿真被分为了六个部分，默认在图4(b)中，机械臂已经准备好执行初始规划，此时动态障碍物位置发生了变化。图中线条是机械臂的执行轨迹，当机械臂沿着规划路径移动检测到机械臂将与动态障碍物发生碰撞，如图4(c)所示。然后规划新的路径，机械臂随之移动，如图4(d)所示。之后，操纵器到达目标，并计划开始的路径。各关节的变化如图5所示。在避障过程中，机械臂的关节运动平稳。

地图二

如图6所示，map2场景中引入了一个小桌子作为静态障碍，使得环境比Map1更复杂。其他类似于情形1。各关节的变化如图7所示。由于环境的复杂性，每一次都考虑到障碍物。这使得机械臂的角度变化更大。

针对上面两个地图的路径规划问题，分别采用传统的RRT-connect路径规划方法、P-RRT路径规划方法以及本申请的路径规划方法分别进行规划实验(各统计100次实验数据)，所得结果如表2所示：

表2不同路径规划方法消耗时间以及成功率

由表2可知，本申请的适用于动态环境下的机械臂路径规划方法在更复杂的环境中，与其他动态算法相比，在路径规划所消耗的时间更段，成功率明显上升，能够更好的实现在动态障碍物存在的复杂环境下提高机械臂的规划效率，减少失误概率，保证路径规划的有效性。

最后应当说明的是，以上实施例仅用以说明本发明创造的技术方案，而非对本发明创造保护范围的限制，尽管参照较佳实施例对本发明创造作了详细地说明，本领域的普通技术人员应当理解，可以对本发明创造的技术方案进行修改或者等同替换，而不脱离本发明创造技术方案的实质和范围。

Claims (3)

1.一种适用于动态环境下的机械臂路径规划方法，其特征在于，包括路径规划阶段以及重新规划阶段：

路径规划阶段，是指根据当前的环境和目标点通过路径规划算法得到搜索树和路径点；根据搜索树和路径点用轨迹规划的方法对路径点进行插值，获得初始规划轨迹，完成机械臂运动控制；包括如下步骤：

步骤一、基于概率统计的运动不确定性建模来预测动态障碍物运动趋势：

A1.针对移动障碍物运动速度的大小和运动方向的不确定性，设每个移动障碍物的运动速度和方向的取值范围分别表示为[v_min，v_max]和[θ_min，θ_max]，在时间周期ΔT内速度和方向保持不变，在ΔT之后，二者可能会有改变，基于此，将障碍物速度的变化量和障碍物运动角度的变化量分别用正态分布描述：

采用正态分布描述移动障碍物速度v的变化量和障碍物运动角度θ的变化量：对于i≥0，有v(i+1)＝v(i)+Δv；θ(i+1)＝θ(i)+Δθ；其中

均服从正态分布：

A2.采用极大似然法构建移动障碍物Δv，Δθ的期望和方差；确定速度和方向保持不变的间周期ΔT，统计ΔT时间周期内移动障碍物运动的速度和方向信息v(i)、θ(i)，(i＝1、2......n)构建样本集，i是指ΔT时间周期内的不同时刻；采用极大似然法构建Δv，Δθ的期望和方差

对Δv有：

对Δθ有：

在重新规划阶段：根据传感器监测的情况来判断机械臂当前工况是否需要重新规划路径；若当前工况被检测到阻断时，通过路径规划算法对初始路径进行修复，并将更新后的路径重新进行轨迹插值，使得机械臂能够连续平滑运行；

步骤二、初始路径预处理，具体是指：

B1、按照初始规划的轨迹移动，当检测到初始路径被动态障碍物阻断时，删除掉被障碍物影响的无效路径点，抛弃已经经过的路径点及其分支；

具体而言：机械臂按照初始规划的路径移动，当传感器检测到初始路径被动态障碍物阻断时，删除掉被障碍物影响的无效路径点，抛弃已经经过的路径点及其分支，整理后，路径分为V_valid和V_backward，基于该步骤以减少查询次数和计算量；

B2、基于前述步骤对机械臂参数进行计算,不断用新采集来的数据对样本集进行更新,以保证估计的准确性；当使用一定数量的数据取得所需的参数后；再利用3σ规则,即对正态随机变量来说,它的取值落在区间[-3e,3e],并根据此规律以i＝1,2,...,n时刻的移动障碍物的运动状态对i＝n+1时刻障碍物的速度,方向和位置的不确定性进行描述，持续利用更新后的数据进行轨迹插值，得到新的轨迹。

2.根据权利要求1所述一种适用于动态环境下的机械臂路径规划方法，其特征在于，所述步骤B1还包括：

B1.1、基于中继点的初始路径预处理：

按照初始规划的路径移动，当检测到初始路径被动态障碍物阻断时，计算中继节点的位置同时在更新后的场景中规划新路径，若到达中继点之前没有完成新路径规划，则停止移动并等待新路径规划完成；否则机械臂将通过中继节点而不会暂停；

对于路径点V和中继节点q_relay，当前位置为q_current，q_obstacle为障碍物边界与V相交且距离最近的点，θ是动态障碍物和q_current的连线与动态障碍物速度方向v_obstacle之间的夹角，移动速度为v_agent(v_agent≤v_max)；当初始路径被中断时，动态障碍物的速度和运动角度的变换分别定义为v′_obstacle＝v_obstacle+Δv_andΔθ；找到障碍边界与路径相交的位置q_obstacle，中继节点的位置被定义为q_relay←q_current+k，表示机械臂沿执行轨迹从当前位置前进k的距离且

B1.2、当初始路径被动态障碍物阻断时，先计算出中继点的位置，在到达中继点时重新判断障碍物与初始路径的关系，若检测到两者碰撞，则认为障碍物仍影响这初始路径，利用Reconnect方法获取修复路径；

具体而言，判断在V_vaild和V_backward所属的节点在拓展步长内，是否存在可以直接一步连接V_vaild和V_backward的节点且不会与障碍物发生碰撞，如果存在这样的节点，则将V_vaild的和V_backward的节点连接，以完成路径修复；当检测到路径被障碍物阻断时，生成中继节点并且删除无效节点，最后连接以修复路径；

当Reconnect策略无法完成任务时，使用Regrow策略利用中继节点，不再考虑中继点之前的搜索树分支各节点；只查询中继点之后搜索树的末端到V_valid代价最低的点作为新的起始点，同时设置V_valid为新目标，只要满足误差允许，则新的拓展树将扩展到V_valid中的任何点，进而修复路径方案。

3.根据权利要求1所述一种适用于动态环境下的机械臂路径规划方法，其特征在于，当使用Regrow方法开始重新规划时，增加路径缓存的步骤，具体而言：随机选择路径缓存区中的关键点；使用初始解决方案的信息来尽可能多地修复路径，定义P_rand为在空间中随机采样的概率，则P_goal+P_way+P_rand＝1，其中P_goal表示向目标的拓展概率，P_way表示向V_vaild的拓展概率；在进行路径缓存过程中，设置两个随机数p(p∈(0,1))和i(i＝1，2,...,n)；其中N是路径缓存区中的点个数；若0<p<P_way，则新节点将沿V_vaild(i)的方向拓展，若P_way<p<P_way+P_goal，则新节点将沿q_goal的方向拓展，否则，新节点将沿随机方向拓展。

Images