一种冗余机械臂避障路径规划方法、设备及存储设备

1.本发明涉及机器人路径规划领域，尤其涉及一种冗余机械臂避障路径规划方法、设备及存储设备。


背景技术：

2.随着工业自动化任务复杂程度的提高，传统的六自由度工业机器人已经不能满足很多作业场景的需求，因此近年来七自由度机械臂的运用也越来越广泛，逐渐出现在大众视野。七自由度机械臂相比于六自由度机械臂的主要特性为“自运动”，即保持末端位姿不变的情况下，仍然可以切换关节角度配置，因此也更加灵活，并且可以实现避奇异、避关节极限、力矩优化等。
3.机械臂在复杂环境进行作业时，需要进行路径规划获取机械臂运动过程中各个位置点的关节角度，防止机械臂与环境发生碰撞，传统的路径规划方法搜索维度高、收敛速度慢。


技术实现要素：

4.为了解决传统路径规划方法搜索维度高、收敛速度慢的技术问题，本技术提供一种冗余机械臂避障路径规划方法、设备及存储设备，其中方法具体包括以下步骤：
5.s1：采用rrt*算法对机械臂末端位置进行预搜索，得到初始规划；
6.s2：根据初始规划起始点、目标点以及空间障碍物信息，采用目标导向策略获取新的节点位姿；
7.s3、判断新节点是否处于无效规划区，若是则舍弃该节点，否则执行步骤s4；
8.s4、根据碰撞检测算法判断新节点的有效性，若无法通过碰撞检测，则添加对应的无效规划区，否则将新节点加入到规划路径中；
9.s5、根据优化父节点策略对新节点进行最优树确定以及父节点选择；
10.s6、重复步骤s2～s5，直至生成机械臂末端可行路径；
11.s7、根据基于三角不等式的路径优化方法对机械臂末端可行路径进一步优化，得到最终路径。
12.一种存储设备，所述存储设备存储指令及数据用于实现一种冗余机械臂避障路径规划方法。
13.一种冗余机械臂避障路径规划设备，包括：处理器及存储设备；所述处理器加载并执行存储设备中的指令及数据用于实现一种冗余机械臂避障路径规划方法。
14.本发明提供的有益效果是：1.使用关节空间路径规划，降低算法搜索维度；2.使用无效规划区机制防止在无效区域重复搜索；3.使用目标导向策略，避免算法盲目搜索；4.使用优化的父节点选择策略和基于三角不等式的路径优化方法，减少冗余节点的产生。
附图说明
15.图1是本发明方法的流程图；
16.图2是无效规划区机制原理示意图；
17.图3是预搜索与最优树选择示意图；
18.图4是父节点选择示意图；
19.图5是机器人受力示意图；
20.图6是基于三角不等式的路径优化算法示意图；
21.图7是本发明硬件设备工作图。
具体实施方式
22.为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地描述。
23.请参考图1，图1是本发明方法的流程图。
24.本发明提供了一种冗余机械臂避障路径规划方法、设备及存储设备，其中，方法具体包括以下步骤：
25.s1：采用rrt*算法对机械臂末端位置进行预搜索，得到初始规划；
26.s2：根据初始规划起始点、目标点以及空间障碍物信息，采用目标导向策略获取新的节点位姿；
27.需要说明的是，所述无效规划区，具体指：未通过碰撞检测的无效路径节点n
invalid
，以n
invalid
为中心，r
invalid
为半径作圆形区域，该圆形区域为无效规划区。
28.请参考图2，图2是无效规划区原理示意图。
29.传统rrt*算法节点扩展盲目性大，可能重复在无效区域进行新节点的扩展，大大降低了算法的搜索效率，因此引入无效规划区机制。图中n
start
为扩展树根节点即起始节点，红色节点为新生成的无效节点即未通过碰撞检测的节点，以该红色无效节点的父节点为圆心做半径为r
invalid
的圆形区域，该圆形区域即为无效规划区，一般r
invalid
小于随机搜索树算法的搜索步长。当生成新的节点时，首先判断该节点是否位于无效规划区内，如果位于无效规划区内则直接舍弃该节点，重新搜索新的节点，否则执行后续算法流程。无效规划区机制的引入，可以有效地去除无效节点，加快算法的搜索速度。
30.s3、判断新节点是否处于无效规划区，若是则舍弃该节点，否则执行步骤s4；
31.s4、根据碰撞检测算法判断新节点的有效性，若无法通过碰撞检测，则添加对应的无效规划区，否则将新节点加入到规划路径中；
32.s5、根据优化父节点策略对新节点进行最优树确定以及父节点选择；
33.步骤s5中所述优化父节点策略，具体如下：
34.s51、以起始节点和目标节点为根节点生成两颗子树；
35.s52、在两颗子树采用rrt*算法重选父节点计算代价值，获取两颗子树上代价最小的节点n
a_min
和n
b_min
，并计算n
a_min
和n
b_min
二者代价较小值对应的节点n
min
，将新节点加入n
min
对应的子树选择为最优树，每生成一个新节点都执行最优树选择。
36.需要说明的是，传统的rrt*算法选择父节点时仅考虑了与起始节点的代价，而未考虑与目标节点的代价，因此可能会错过离目标节点更近的父节点。为了对父节点选择策
略进行优化，首先我们使用rrt-connect算法进行预搜索，分别以起始节点与目标节点为根节点生成子树，在两棵子树上分别使用原始rrt*算法的重选父节点环节计算代价值，获取两棵树上代价最小的节点n
a_min
和n
b_min
，并计算n
a_min
和n
b_min
二者代价较小值对应的节点n
min
，并考虑将新节点加入n
min
对应的子树即选择最优树，每生成一个新节点都执行最优树选择过程，上述过程的原理示意请参考图3所示，图3是预搜索与最优树选择示意图。
37.完成预搜索与最优树选择以后，下面开始给出改进父节点优化策略，如图4所示，图4是父节点选择示意图。图4中n
parent
和n
ancestor
为n
near
的父节点和祖父节点。我们将n
parent
和n
ancestor
也加入新节点n
new
父节点的选择范围，相比于传统rrt*算法的父节点选择策略，优化后的策略添加了n
start
→nancestor
→nnew
和n
start
→nparent
→nnew
两条额外的路径，新加入的节点优先考虑n
start
→nancestor
→nnew
路径，碰撞检测无法通过再考虑路径n
start
→nparent
→nnew
和n
start
→nnear
→nnew
。父节点范围的扩大，会出现多个节点连接同一个父节点的情况，减少多余节点进而减少冗余路径的产生，减少机械臂在实际作业中可能出现的绕远或兜圈子的情况，也有利于路径长度的优化以及轨迹的平滑。
38.s6、重复步骤s2～s5，直至生成机械臂末端可行路径；
39.s7、根据基于三角不等式的路径优化方法对机械臂末端可行路径进一步优化，得到最终路径。
40.需要说明的是，步骤s2中，所述目标导向策略包括第一导向策略和第二导向策略。
41.本发明中，虽然添加了无效规划区机制，但是该机制只是为了防止算法重复在无效区域搜索，节点本身的生成依然是随机的，采样存在盲目性，因此我们需要添加目标导向策略，使采样点尽量朝着目标节点方向前进，节省算法的搜索时间。
42.针对rrt*算法搜索盲目性问题，引入第一导向策略：给定概率p，并以概率p使新节点朝着目标节点方向扩展，当环境中障碍物较密集时，概率p取较小值，否则取较大值，每次采样新节点时，将区间[0,1]内的随机数与p比较，根据比较结果确定随机节点如下
[0043][0044]
当目标节点无法作为随机节点时，我们考虑引入第二导向策略：基于人工势场法思想，在目标节点处添加引力势场，在障碍物处添加斥力势场，机器人在势场中受到目标节点的引力以及障碍物的斥力同时作用，如图5所示，引力与斥力的合力方向决定采样点的方向。
[0045]
传统的人工势场法存在障碍物附近目标不可达以及当合力为0时无法完成进一步搜索或来回震荡的问题。传统人工势场法中引力势场如下：
[0046][0047]
其中d(n,n
goal
)为机器人与目标节点的距离。对应的引力为
[0048][0049]
针对传统人工势场法存在的不足，给出改进后的斥力势场为：
[0050][0051]
其中d(n,n
obs
)为机器人与障碍物中心的距离。对应的斥力为：
[0052][0053]
其中
[0054][0055][0056]
因此机器人所受合力为
[0057][0058]
令斥力表达式中参数n取值范围为(1,+∞]，分析机器人在靠近目标点时所受斥力情况可以得到
[0059][0060][0061]
由式(9)和式(10)可知，当n》1时，f
rep_1
(n)和f
rep_2
(n)均趋近于0，此时机器人只受引力作用，因此改进后的斥力可以用于解决传统人工势场法中存在的终点不可达问题，当机器人引力和斥力合力为0时，此时认为第一导向策略和第二导向策略均不可用，此时随机生成新节点。
[0062]
最后，将传统rrt*算法加入上述优化步骤，生成可行路径后，我们继续采用基于三角不等式的路径优化方法进一步对生成的路径进行处理，算法原理如图6所示。在图6中，n1,n2,n3为路径中相邻三个节点，由三角形两边之和大于第三边可知。
[0063]
当路径n1→
n3满足碰撞检测时，我们认为中间节点n2是冗余的，因此将其从路径中删除，对路径中每相邻三个节点重复上述步骤，直到路径中节点个数不再发生变化。采用这样的优化方法，可以优化路径长度，使轨迹更加平滑，在一定程度上减小轨迹抖动。
[0064]
请参见图7，图7是本发明实施例的硬件设备工作示意图，所述硬件设备具体包括：一种冗余机械臂避障路径规划设备401、处理器402及存储设备403。
[0065]
一种冗余机械臂避障路径规划设备401：所述一种冗余机械臂避障路径规划设备401实现所述一种冗余机械臂避障路径规划方法。
[0066]
处理器402：所述处理器402加载并执行所述存储设备403中的指令及数据用于实现所述一种冗余机械臂避障路径规划方法。
[0067]
存储设备403：所述存储设备403存储指令及数据；所述存储设备403用于实现所述一种冗余机械臂避障路径规划方法。
[0068]
本发明的有益效果是：1.使用关节空间路径规划，降低算法搜索维度；2.使用无效规划区机制防止在无效区域重复搜索；3.使用目标导向策略，避免算法盲目搜索；4.使用优化的父节点选择策略和基于三角不等式的路径优化方法，减少冗余节点的产生。
[0069]
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

技术特征：
1.一种冗余机械臂避障路径规划方法，其特征在于：包括以下步骤：s1：采用rrt*算法对机械臂末端位置进行预搜索，得到初始规划；s2：根据初始规划起始点、目标点以及空间障碍物信息，采用目标导向策略获取新的节点位姿；s3、判断新节点是否处于无效规划区，若是则舍弃该节点，否则执行步骤s4；s4、根据碰撞检测算法判断新节点的有效性，若无法通过碰撞检测，则添加对应的无效规划区，否则将新节点加入到规划路径中；s5、根据优化父节点策略对新节点进行最优树确定以及父节点选择；s6、重复步骤s2～s5，直至生成机械臂末端可行路径；s7、根据基于三角不等式的路径优化方法对机械臂末端可行路径进一步优化，得到最终路径。2.如权利要求1所述的一种冗余机械臂避障路径规划方法，其特征在于：所述无效规划区，具体指：未通过碰撞检测的无效路径节点n
invalid
，以n
invalid
为中心，r
invalid
为半径作圆形区域，该圆形区域为无效规划区。3.如权利要求1所述的一种冗余机械臂避障路径规划方法，其特征在于：步骤s5中所述优化父节点策略，具体如下：s51、以起始节点和目标节点为根节点生成两颗子树；s52、在两颗子树采用rrt*算法重选父节点计算代价值，获取两颗子树上代价最小的节点n
a_min
和n
b_min
，并计算n
a_min
和n
b_min
二者代价较小值对应的节点n
min
，将新节点加入n
min
对应的子树选择为最优树，每生成一个新节点都执行最优树选择。4.如权利要求1所述的一种冗余机械臂避障路径规划方法，其特征在于：步骤s2中，所述目标导向策略包括第一导向策略和第二导向策略。5.如权利要求4所述的一种冗余机械臂避障路径规划方法，其特征在于：所述第一导向策略如下：给定概率p，并以概率p使新节点朝着目标节点方向扩展，当环境中障碍物多时，概率p取较小值，否则取较大值，每次采样新节点时，将区间[0,1]内的随机数与p比较，根据比较结果确定随机节点如下：当目标节点无法作为随机节点时，引入第二导向策略。6.如权利要求4所述的一种冗余机械臂避障路径规划方法，其特征在于：所述第二导向策略，基于人工势场法，在目标节点处添加引力势场，在障碍物处添加斥力势场，机械臂在势场中受到目标节点的引力以及障碍物的斥力同时作用，由引力与斥力的合力方向决定采样点的方向，此时将斥力势场中的参数n取值范围设置为(1,+∞)；其中斥力势场公式如下：u
rep
为斥力势场，n当前节点，k
rep
也是一个系数，n
obs
障碍物中心，d0为预设的距离阈值，n
goal
为目标节点；n为斥力势场公式中的一个参数；
当引力和斥力合力为0时，认为第一导向策略和第二导向策略均不可用，此时随机生成新节点。7.如权利要求1所述的一种冗余机械臂避障路径规划方法，其特征在于：步骤s7中所述基于三角不等式的路径优化方法具体为：对机械臂末端可行路径中的任意连续的三个相邻节点n1、n2、n3组成的三角形，由三角形两边之和大于第三边可知：pn
1-n
2 p+pn
2-n
3 p≥pn
1-n
3 p当路径n1→
n3满足碰撞检测时，认为中间节点n2是冗余的，将其从路径中删除，对路径中每相邻三个节点重复上述步骤，直到路径中节点个数不再发生变化，得到最终路径。8.一种存储设备，其特征在于：所述存储设备存储指令及数据用于实现权利要求1～7任一项所述的一种冗余机械臂避障路径规划方法。9.一种冗余机械臂避障路径规划设备，其特征在于：包括：处理器及存储设备；所述处理器加载并执行存储设备中的指令及数据用于实现权利要求1～7任一项所述的一种冗余机械臂避障路径规划方法。

技术总结
本发明公开了一种冗余机械臂避障路径规划方法、设备及存储设备，方法如下：采用RRT*算法对机械臂末端位置进行预搜索，得到初始规划；根据初始规划起始点、目标点以及空间障碍物信息，采用目标导向策略获取新的节点位姿；判断新节点是否处于无效规划区，若是则舍弃该节点，否则执行后续步骤；根据碰撞检测算法判断新节点的有效性，若无法通过碰撞检测，则添加对应的无效规划区，否则将新节点加入到规划路径；根据优化父节点策略对新节点进行最优树确定以及父节点选择；重复上述步骤直至生成机械臂末端可行路径；根据基于三角不等式的路径优化方法对机械臂末端可行路径进一步优化，得到最终路径。本发明有益效果是：有效减少了冗余路径的产生。余路径的产生。余路径的产生。


技术研发人员：陈鑫 梅义胜 秦朗 高旭 刘明哲 王行澳
受保护的技术使用者：中国地质大学（武汉）
技术研发日：2023.05.16
技术公布日：2023/8/16