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

1.本发明属于机械臂路径规划技术领域，具体涉及一种机械臂避障路径规划方法。


背景技术：

2.机械臂路径规划算法自从上世纪被提出来，便得到了快速发展和广泛应用，现在机械臂路径规划算法的算法性能以及十分高效，但在某些方面仍存在着一定的不足，例如全局规划难度大、实时性低等缺点。针对路径规划的问题，曾提出去多算法，如栅格法、蚁群算法、人工势场法、a*算法、快速随机扩展树算法等。其中，人工势场法是依靠机器人对障碍物的排斥力和目标点的吸引力的共同作用进行避障和路径规划，但在规划的过程中存在局部极小的陷阱，当排斥力和吸引力相互抵消时便不能找出路径。
3.快速随机扩展树算法自从提出便被广泛应用于机械臂的路径规划，其通过随机的采样与障碍物进行碰撞检测，简化了路径规划的算法模型和空间模型。但也快速随机扩展树算法也存在自己的缺点，由于采样点随机分布会导致无法找出合理路径，并且会产生很多无用节点，最终生成的路径会有很多转折并不平滑，将导致机械臂运动过程中震动加大、摩擦加剧。


技术实现要素：

4.本发明的目的是解决上述问题，提供一种运动平稳，摩擦小，避障效果好的机械臂避障路径规划方法。
5.为解决上述技术问题，本发明的技术方案是：一种机械臂避障路径规划方法，包括以下步骤：
6.s1、获取轨迹规划所需要的参数信息，包括轨迹规划的空间范围x，以及x内的障碍物信息，机械臂的起始点x
init
以及目标点x
goal
；以起始点x
init
以及目标点x
goal
为根节点分别初始化随机树t1和t2，给定扩展步长l，最大迭代次数max；
7.s2、采用均匀logistic混沌序列采样器生成随机点x
rand
；
8.s3、找到随机树上和点x
rand
最近的点x
nearest
，然后x
nearest
向x
rand
延伸一定距离l得到下一个拓展点x
new
，判断x
nearest
和x
new
之间的连线是否发生碰撞，若没有发生碰撞，则将x
new
加入随机树中；
9.s4、以步骤s3中的x
new
为圆心，以一定的半径作圆，将随机树上落在圆内的节点作为潜在父节点形成集合q
near
，然后对从起始节点开始并经过集合q
near
内潜在父节点到x
new
的路径长度与从起始点开始经过x
nearest
到x
new
的路径长度相比较，最终选择路径长度最小的节点作为x
new
的父节点，然后将新的路径添加进随机树并删除原x
nearest
与x
new
连接路径。
10.s5、起始点x
init
和目标点x
goal
同时向对方搜索，在两颗随机树搜索空间x1和x2中，xr是起始点随机树t1的一个拓展节点，t2随机树将向随机树t1拓展到xr，然后利用路径代价函数进行选择，代价函数如下所示：
[0011][0012]
最后通过代价判断得到最优节点xr，将两颗随机树连接完成路径搜索；
[0013]
s6、基于滑动平均平滑算法对s5中生成的路径进行平滑处理，获得适合机械臂运行的实际路径。
[0014]
进一步地，所述步骤s1中，机械臂的起始点x
init
以及目标点x
goal
的确定方法是，根据内置信息和机械臂传输过来的相关信息初始化机械臂路径地图信息，包括地图的边界以及障碍物信息，同时利用地图信息技术构建地图模型，并在地图中确定机械臂的起始点x
init
以及目标点x
goal
，采样区域为全地图。
[0015]
进一步地，所述步骤s2中的均匀logistic混沌序列采样器，其中均匀logistic混沌序列采样器生成的采样点xrand由下面的公式确定：
[0016]
x
i+1
＝μ
xi
(1-xi)
ꢀꢀ
(1)
[0017][0018]
其中μ(0≤μ≤4)是混沌系统的李雅普诺夫指数，xi(0＜xi＜1)是混沌变量,根据混沌动力系统的研究，这里取μ取4，当i＝0时，x0取0.630；
[0019]
使用式(1)和式(2)相结合生成均匀分布logistic混沌序列，这样生成随机采样点便具有均匀性的特点。
[0020]
进一步地，所述步骤s2中，生成随机点x
rand
时，先判断采样次数是否超过最大采样次数max，如果超过结束采样，如果没有最大采样次数则采样次数加一，然后获取扩展采样点x
rand
，采样过程为：基于初始化均匀logistic混沌序列采样器在0-1内进行一次随机数生成，然后计算采样点x
rand
是否在可行空间内，如果采样点x
rand
有效则进行下一步。
[0021]
进一步地，所述随机树t1和t2均采用上面步骤s1到步骤s4的过程进行拓展。
[0022]
进一步地，所述步骤s6中基于滑动平均平滑算法对步骤s5中生成的路径进行平滑处理，其中基于滑动平均平滑算法由下面的公式确定：
[0023]
设置滑动平均平滑算法的控制点为xj,j＝0，1，2，
…
，滑动平均平滑算法的基函数表示为yi,则滑动平均平滑算法的数学描述函数记为：
[0024][0025]
其中n表示当前节点进行平均平滑处理所使用相邻左右节点的个数；当n取3时如下所示：
[0026]
ya(1)＝y(1)
[0027]
ya(2)＝(y(1)+y(2)+y(3))/3
[0028]
ya(3)＝(y(1)+y(2)+y(3)+y(4)+y(5))/5
[0029]
ya(4)＝(y(1)+y(2)+y(3)+y(4)+y(5)+y(6)+y(7))/7)
[0030]
ya(5)＝(y(2)+y(3)+y(4)+y(5)+y(6)+y(7)+y(8))/7)；
[0031]
将上面得到的平滑路径的数据传输给机械臂，机械臂根据得到的数据运动到目标点抓取目标物，然后再经过路径规划将目标物放置到指定位置。
[0032]
本发明的有益效果是：本发明所提供的一种机械臂避障路径规划方法，步骤简单，
避障效果好，能够有效降低机械臂运动过程中的震动和摩擦，节约机械臂的使用耗费成本。
附图说明
[0033]
图1是本发明一种机械臂避障路径规划方法的rrt*-connect算法流程图；
[0034]
图2是本发明的拓展过程图。
具体实施方式
[0035]
下面结合附图和具体实施例对本发明做进一步的说明：
[0036]
如图1和图2所示，本发明提供的一种机械臂避障路径规划方法，包括以下步骤：
[0037]
s1、获取轨迹规划所需要的参数信息，包括轨迹规划的空间范围x，以及x内的障碍物信息，机械臂的起始点x
init
以及目标点x
goal
；以起始点x
init
以及目标点x
goal
为根节点分别初始化随机树t1和t2，给定扩展步长l，最大迭代次数max。
[0038]
在本步骤中，机械臂的起始点x
init
以及目标点x
goal
的确定方法是，根据内置信息和机械臂传输过来的相关信息初始化机械臂路径地图信息，包括地图的边界以及障碍物信息，同时利用地图信息技术构建地图模型，并在地图中确定机械臂的起始点x
init
以及目标点x
goal
，采样区域为全地图。
[0039]
在本实施例中，本发明的方法在现有的遨博机器人(aubo-i5)进行使用，机器人的有效负载为5kg，最大工作半径为886.5mm。
[0040]
首先初始化机械臂，然后机械臂末端移动到目标物的上方，通过相机获取目标物的位置x
goal
以及空间中障碍物的信息，然后将机械臂的位置(起始点)x
init
、目标物(目标点)的位置x
goal
以及障碍物的信息传给rrt*-connect算法。
[0041]
根据内置信息和机械臂传输过来的相关信息初始化机械臂路径地图信息，包括地图的边界以及障碍物信息，同时利用地图信息技术构建地图模型，并在地图中确定机械臂的起始点x
init
以及目标点x
goal
，采样区域为全地图。然后开始进行机械臂路径规划的准备，以起始点xinit以及目标点xgoal为根节点分别初始化树t1和t2，给定扩展步长l，最大迭代次数max。
[0042]
s2、采用均匀logistic混沌序列采样器生成随机点x
rand
。
[0043]
在本步骤中的均匀logistic混沌序列采样器，其中均匀logistic混沌序列采样器生成的采样点xrand由下面的公式确定：
[0044]
x
i+1
＝μ
xi
(1-xi)
ꢀꢀ
(1)
[0045][0046]
其中μ(0≤μ≤4)是混沌系统的李雅普诺夫指数，xi(0＜xi＜1)是混沌变量,根据混沌动力系统的研究，这里取μ取4，当i＝0时，x0取0.630。
[0047]
使用式(1)和式(2)相结合生成均匀分布logistic混沌序列，这样生成随机采样点便具有均匀性的特点。
[0048]
本步骤中，生成随机点x
rand
时，先判断采样次数是否超过最大采样次数max，如果超过结束采样，如果没有最大采样次数则采样次数加一，然后获取扩展采样点x
rand
，采样过程为：基于初始化均匀logistic混沌序列采样器在0-1内进行一次随机数生成，然后计算采
样点x
rand
是否在可行空间内，如果采样点x
rand
有效则进行下一步。
[0049]
s3、找到随机树上和点x
rand
最近的点x
nearest
，然后x
nearest
向x
rand
延伸一定距离l得到下一个拓展点x
new
，判断x
nearest
和x
new
之间的连线是否发生碰撞，若没有发生碰撞，则将x
new
加入随机树中。
[0050]
s4、以步骤s3中的x
new
为圆心，以一定的半径作圆，将随机树上落在圆内的节点作为潜在父节点形成集合q
near
，然后对从起始节点开始并经过集合q
near
内潜在父节点到x
new
的路径长度与从起始点开始经过x
nearest
到x
new
的路径长度相比较，最终选择路径长度最小的节点作为x
new
的父节点，然后将新的路径添加进随机树并删除原x
nearest
与x
new
连接路径。
[0051]
在本实施例中，若q
near
为空则跳过直接进入下一步，若q
near
不为空，则遍历所有从起始节点经过集合q
near
节点到x
new
所有路径，然后与从起始节点经x
nearest
到x
new
的路径相比较，选出路径最短的一条。
[0052]
如果从起始节点经x
nearest
到x
new
的路径是最短的那条线路，直接进入下一步，如果不是则断开x
nearest
到x
new
的连线，然后将最短路径上x
new
的上一个节点作为其父节点，并将x
new
加入这条路径当中。
[0053]
随机树t1和t2均采用上面步骤s1到步骤s4的过程进行拓展。
[0054]
s5、起始点x
init
和目标点x
goal
同时向对方搜索，在两颗随机树搜索空间x1和x2中，xr是起始点随机树t1的一个拓展节点，t2随机树将向随机树t1拓展到xr，然后利用路径代价函数进行选择，代价函数如下所示：
[0055][0056]
最后通过代价判断得到最优节点xr，将两颗随机树连接完成路径搜索。
[0057]
s6、基于滑动平均平滑算法对s5中生成的路径进行平滑处理，获得适合机械臂运行的实际路径。
[0058]
本步骤中在得到最优路径后，基于滑动平均平滑算法对步骤s5中生成的路径进行平滑处理，获得最终路径。其中基于滑动平均平滑算法由下面的公式确定：
[0059]
设置滑动平均平滑算法的控制点为xj,j＝0，1，2，
…
，滑动平均平滑算法的基函数表示为yi,则滑动平均平滑算法的数学描述函数记为：
[0060][0061]
其中n表示当前节点进行平均平滑处理所使用相邻左右节点的个数；当n取3时如下所示：
[0062]
ya(1)＝y(1)
[0063]
ya(2)＝(y(1)+y(2)+y(3))/3
[0064]
ya(3)＝(y(1)+y(2)+y(3)+y(4)+y(5))/5
[0065]
ya(4)＝(y(1)+y(2)++y(3)+y(4)+y(5)+y(6)+y(7))/7
[0066]
ya(5)＝(y(2)++y(3)+y(4)+y(5)+y(6)+y(7)+y(8))/7；
[0067]
将上面得到的平滑路径的数据传输给机械臂，机械臂根据得到的数据运动到目标点抓取目标物，然后再经过路径规划将目标物放置到指定位置。
[0068]
本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发
明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。