一种机械臂动态避障路径规划方法

技术领域

本申请属于路径规划寻优方法技术领域，尤其涉及一种机械臂动态避障路径规划方法。

背景技术

在执行任务过程中，机械臂受外界环境的影响会导致当前的执行轨迹失效，如何快速地修复失效规划是机械臂在动态环境下作业的关键。因障碍物而导致规划失效的问题一般有两种解决方案：一种是放弃已有的规划，从当前位置重新进行规划；另一种是建立缓存区域，保留上次规划中有效的信息用于重规划修复。例如ERRT算法提出路径缓存的方法，利用缓存信息对已经失效的规划路径进行修复。

实验中的移动机器人在躲避障碍物、避免剧烈震荡和局部最小值等问题，例如DRRT算法在动态环境影响初始规划时，首先对失效搜索树的分支进行修剪，然后向目标点进行重新规划。

为了进一步提高路径规划过程的效率以及节省算力资源，考虑到重规划中的路径点选择存在一定盲目性，需要对该过程中的技术性能提出更多要求，如更少的运行时间和更短的修复轨迹。

发明内容

本申请的目的在于，通过对现有路径规划方法的基本过程中路径缓存方法的存在问题的优化，提出一种能够对修复过程中的路径点进行分析和选择，提高重规划的修复质量和成功率的机械臂动态避障路径规划方法。

为实现上述目的，本申请采用如下技术方案。

一种机械臂动态避障路径规划方法，包括如下步骤：

步骤一、基于初始规划路径控制机械臂运动，利用传感器检测可能存在的动态障碍，当监测到下一路径点被动态障碍阻断时，删除路径规划搜索树中的失效点，抛弃已经经过的路径点及其分支，整理后得到位于失效点前侧的路径V

步骤二、基于进行节点生长以生成新的路径，根据动态障碍物和机械臂当前的运动位置和状态，生成一个位于机械臂和障碍物之间的无碰撞中继节点q

其中

其中

计算各分支节点到V

为了在V

其中ρ(N

对前述机械臂动态避障路径规划方法的进一步改进或者优选实施方案，为便于确定动态障碍物与传感器运动关系，还包括用于构建运动不确定的动态障碍物运动模型的步骤，具体而言：采用正态分布描述障碍物的实时速度v(i)和障碍物运动角度θ(i)：建立第i+1个运动时间节点运动模型v(i+1)＝v(i)+Δv；θ(i+1)＝θ(i)+Δθ；

其中Δv为移动速度变化量；其中Δθ为移动方向变化量；

且

其中

μ

对Δv有

对Δθ有

不断采集数据更新样本集以保证模型的准确性，基于正态随机变量的3σ规则进行取值，建立运动障碍物的运动模型。

对前述机械臂动态避障路径规划方法的进一步改进或者优选实施方案，所述步骤二中，中继节点基于如下方式确定：

定义中继节点q

当路径被中断时，确定动态障碍物的速度v′

确定障碍边界与路径相交的位置q

记录当前序列中的路径点的位置，利用二分法计算q

对前述机械臂动态避障路径规划方法的进一步改进或者优选实施方案，若基于步骤一可以完成路径规划，则在机械臂运动至下一节点时重新判断障碍物与初始路径规划的关系，若检测到障碍物与初始路径规划碰撞，则重复步骤一；若基于步骤一无法完成路径规划，则执行步骤二，并在机械臂运动至下一节点时重新判断障碍物与初始路径规划的关系，若检测到障碍物与初始路径规划碰撞，则返回步骤一。

其有益效果在于：

本申请的机械臂动态避障路径规划方法以中继点方法和连接策略为修复方法实现机械臂的动态避障，实现了更少的运行时间和更短的修复轨迹。通过对修复过程中的路径点进行分析和选择，提高重规划的修复质量和成功率。

附图说明

图1是本申请的机械臂动态避障路径规划方法的主要原理示意图；

图2是路径规划搜索树修剪过程示意图；

图3是对照实验中每组实验的平均规划时间；

图4是每组实验在动态避障阶段中修复搜索树的平均节点个数；

图5是每组实验在动态避障阶段中修复路径的平均距离；

图6是每组实验在动态避障阶段中修复路径点的平均数量；

图7是现有路径规划方案不同步长下的路径点分布示意图；

图8是本申请的规划方法不同步长下的路径点的分布示意图；

图9是δ＝5的路径规划仿真过程；

图10是δ＝10的路径规划仿真过程；

图11是δ＝15的路径规划仿真过程；

图12是δ＝20的路径规划仿真过程。

具体实施方式

以下结合具体实施例对本申请作详细说明。

机械臂的工作空间内可能会有运动障碍物或操作人员误入，亦或有相关协作的任务，在运动规划的过程中都可以将其视为障碍物，这些障碍物存在一定的运动不确定性，如运动障碍物速度的大小和方向。

本申请的机械臂动态避障路径规划方法在进行机械臂路径规划过程中，为描述障碍物的不确定性，以便于对机械臂路径的规划和设计，基于概率理论方法描述障碍物的不确定性，具体而言，采用正态分布描述障碍物的实时速度v(i)和障碍物运动角度θ(i)：建立第i+1个运动时间节点运动模型v(i+1)＝v(i)+Δv；θ(i+1)＝θ(i)+Δθ；

其中Δv为移动速度变化量；其中Δθ为移动方向变化量；

且

其中

μ

对Δv有

对Δθ有

通过上述方法，通过不断采集到的数据更新样本集以保证估计的准确性。基于正态随机变量的3σ规则，根据i＝1,2,...,n时刻运动障碍物的状态可以描述i＝n+1时刻运动障碍物的速度和方向，建立运动障碍物的运动模型。

如图1所示，本申请的机械臂动态避障路径规划方法的主要原理包括基于路径规划的初始规划，该过程通过设置目标点实现路径点的搜索；基于动态策略的重新规划，该过程通过传感器监测环境的变化，根据机械臂当前的运动状态，判断初始规划是否适应当前环境，再使用算法修复失效规划；基于路径点或更新后的路径点实现的轨迹规划，规划出能够满足机械臂执行条件的轨迹，具体而言，包括如下步骤：

步骤一、基于初始规划路径控制机械臂运动，利用传感器检测可能存在的动态障碍，当监测到下一路径点被动态障碍阻断时，删除路径规划搜索树中的失效点，抛弃已经经过的路径点及其分支，如图2所示，整理后得到位于失效点前侧的路径V

若基于步骤一可以完成路径规划，则在机械臂运动至下一节点时重新判断障碍物与初始路径规划的关系，若检测到障碍物与初始路径规划碰撞，则重复步骤一；

步骤二、基于进行节点生长以生成新的路径，根据动态障碍物和机械臂当前的运动位置和状态，生成一个位于机械臂和障碍物之间的无碰撞中继节点q

其中

其中

计算各分支节点到V

为了在V

其中ρ(N

若基于步骤一无法完成路径规划，则执行步骤二，并在机械臂运动至下一节点时重新判断障碍物与初始路径规划的关系，若检测到障碍物与初始路径规划碰撞，则返回步骤一。

所述步骤二中，中继节点基于如下方式确定：

定义中继节点q

当路径被中断时，确定动态障碍物的速度v′

确定障碍边界与路径相交的位置q

记录当前序列中的路径点的位置，利用二分法计算q

为了验证本申请的机械臂动态避障路径规划方法地有效性，设置三组对比实验：

方案一：基于ERRT算法的路径规划方法；

方案二：基于DRRT算法中直接向目标点生长修复的路径规划方法；

方案三：本申请的机械臂动态避障路径规划方法。

为保证算法外资源消耗的一致性，在进行搜索树扩展过程中，均使用RRT算法作为重生长策略的修复算法；记录每组实验中，1000次成功使用重生长策略的动态避障规划仿真数据，这些数据包括：每组实验的规划时间；每组实验在动态避障阶段中修复搜索树的节点个数；每组实验在动态避障阶段中修复路径的距离；每组实验在动态避障阶段中修复路径点的数量，得到的结果如图3、图4、图5、图6所示

通过对照实验可知，基于本申请的机械臂动态避障路径规划方法在上述几方面的表现明显优于其它两种方法。

具体来看，如图3所示，前50％的数据中，本申请方法与方案一方法在路径修复的过程中的规划时间相近，然而由于方案一方法在扩展的过程中存在一定的随机性，在部分仿真中，使得方案一方法在更新后的环境中无法解决问题，从而整体在规划时间上的表现比较极端。另外一个值得关注的现象是在前70％的仿真实验中，方案一方法需要较少的规划时间，但是与方案二方法相比，后30％的仿真实验中，方案一方法所需较多的规划时间来修复路径。这是因为方案一方法在复杂的环境中或扩展被障碍物阻碍的情况下，无法正常实现路径修复，而方案二方法可以直接向目标点生长，可以避免方案一方法中因强制扩展而导致失败的问题，如图4所示。

因此，方案一方法在动态环境中表现极端，当其扩展陷入某个方向时，由于扩展概率的影响，使其不容易跳出局部限制。如图5所示，本申请方法能够规划出较短的路径，这是因为选择函数找出N中到Vvaild代价最低的点作为新的生长点。

如图6所示，本申请方法在完成动态避障的基础上，能够产生较少的路径点，从而使得查询过程中的计算量相对减少。

进一步的，设置1000组对比仿真，在机械臂运动过程中，设置动态障碍物以影响机械臂的初始规划，在不同扩展步长δ下对比不同常用的路径规划方法分析的性能进行仿真分析，对仿真结果分析统计，可以总结本申请的机械臂动态避障路径规划方法的扩展特性如下：

1、路径点在前期分布密集，而在后期则分布稀疏，且总路径点数量数量明显少于现有其他算法，如图7、图8所示；

2、在扩展树的每个分支中，各末端节点都偏向于目标点，这使机械臂的位姿路径修复更容易；

3、在动态避障过程中的查询次数减少；

采用不同的步长δ验证本申请的机械臂动态避障路径规划方法，分别进行1000次动态避障仿真，整理数据如表1所示。

总的来看，本申请的机械臂动态避障路径规划方法算法在不同δ情况下的平均规划时间和平均重规划时间都令人满意。重规划过程中，搜索树的节点数和路径点数也比较少，这是由于代价函数选择和扩展特性所影响。

表1不同δ的EBG-RRT算法的平均数据

图9～12是不同δ的仿真过程示意图，图中立方体1是静态障碍物、立方体2是动态障碍、3是中继点、4是当前位置点4、5是起始点、6是目标点、7是初始规划搜索树、8是初始规划路径、9是修复路径。

由图可知，当δ＝5时，初始规划所产生的节点会占用大量缓存，虽然该步长下的平均重规划的路径长度较短，但是造成平均重规划时间较长，如图9所示。当δ＝20时，初始规划产生的节点较少，虽然不会占用过多的缓存，但是较大的δ会使得平均重规划路径长度变长，如图10所示。结合表4.1分析可以得出，本申请的算法适合在δ＝15下执行。

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