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

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

背景技术：

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

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

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

技术实现思路

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

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

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

4、步骤一、基于初始规划路径控制机械臂运动，利用传感器检测可能存在的动态障碍，当监测到下一路径点被动态障碍阻断时，删除路径规划搜索树中的失效点，抛弃已经经过的路径点及其分支，整理后得到位于失效点前侧的路径vbackward和位于失效点后侧的路径vvalid；基于搜索树查询vbackward和vvalid的路径点中是否存在相临近的点vl和vm，能够在检索范围内直接连接且不会与更新后的障碍发生碰撞，所述检索范围是指以vl和vbackward中vm两点的中点oe＝(vl+vm)/2为圆心，预设参数ε为半径的范围；如果存在这样的点，则将vl和vm连接，完成路径规划；若无法获得有效路径，则执行步骤二；

5、步骤二、基于进行节点生长以生成新的路径，根据动态障碍物和机械臂当前的运动位置和状态，生成一个位于机械臂和障碍物之间的无碰撞中继节点qrelay；查询qrelay到vbackward之间搜索树的最末端分支节点，并将这些节点根据父子关系的顺序设为n＝[n1,n2,......nm]t；为了从分支节点中确定一个生长起始节点qstart且使其生长到vvalid＝[v1,v2,......vn]t的代价更小，定义选择代价函数

6、其中

7、其中是指v1和vn两点的连线，x为障碍物安全系数；d表示两点之间的距离，d(nm,v1)表示为nm和v1之间的距离，表示点线之间的距离；

8、计算各分支节点到vvalid＝[v1,v2,......vn]t的代价，选择代价函数值最小的点作为生长点qstart；

9、为了在vvalid＝[v1,v2,......vn]t中确定最佳的路径点以简化规划距离和时间，定义消耗代价函数选择在vvalid＝[v1,v2,......vn]t中消耗代价最小的点为路径修复的生长目标节点；

10、其中ρ(nm,vi)是指nm,vi和vm的离散点曲率且ρ(nm,vn)＝1；

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

12、其中δv为移动速度变化量；其中δθ为移动方向变化量；

13、且均服从正态分布；

14、其中

15、μv,分别为δv的期望和方差；μθ,分别为δθ的期望和方差；

16、对δv有

17、对δθ有

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

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

20、定义中继节点qrelay、机械臂当前位置qcurrent、障碍物和qcurrent的连线与障碍物速度方向vobstacle之间的夹角θ，机械臂移动速度vagent(vagent≤vmax)；

21、当路径被中断时，确定动态障碍物的速度v′obstacle＝vobstacle+δv和运动角度偏转δθ；

22、确定障碍边界与路径相交的位置qobstacle，

23、记录当前序列中的路径点的位置，利用二分法计算qreplay的位置，中继节点的位置定义为n(qreplay)＝binarysearch(k)+1；qrelay←qcurrent+k，k表示机械臂沿执行轨迹从当前位置前进的距离且

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

25、其有益效果在于：

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

技术特征：

1.一种机械臂动态避障路径规划方法，其特征在于，包括如下步骤：

2.根据权利要求1所述机械臂动态避障路径规划方法，其特征在于，为便于确定动态障碍物与传感器运动关系，还包括用于构建运动不确定的动态障碍物运动模型的步骤，具体而言：采用正态分布描述障碍物的实时速度v(i)和障碍物运动角度θ(i)：建立第i+1个运动时间节点运动模型v(i+1)＝v(i)+δv；θ(i+1)＝θ(i)+δθ；

3.根据权利要求1所述机械臂动态避障路径规划方法，其特征在于，所述步骤二中，中继节点基于如下方式确定：

4.根据权利要求1所述机械臂动态避障路径规划方法，其特征在于，若基于步骤一可以完成路径规划，则在机械臂运动至下一节点时重新判断障碍物与初始路径规划的关系，若检测到障碍物与初始路径规划碰撞，则重复步骤一；若基于步骤一无法完成路径规划，则执行步骤二，并在机械臂运动至下一节点时重新判断障碍物与初始路径规划的关系，若检测到障碍物与初始路径规划碰撞，则返回步骤一。

技术总结
本申请属于路径规划寻优方法技术领域，尤其涉及一种机械臂动态避障路径规划方法。包括如下步骤：检测障碍，删除失效点，筛选连接有效点完成路径规划；基于进行节点生长以生成新的路径，生成中继节点；查询分支节点，选择代价函数值最小的点作为生长点；选择在消耗代价最小的点为路径修复的生长目标节点；本申请的机械臂动态避障路径规划方法以中继点方法和连接策略为修复方法实现机械臂的动态避障，实现了更少的运行时间和更短的修复轨迹。通过对修复过程中的路径点进行分析和选择，提高重规划的修复质量和成功率。

技术研发人员：张文群,蒋飞,袁成人,余良武
受保护的技术使用者：中国人民解放军海军工程大学
技术研发日：
技术公布日：2024/1/13