CN115870990A - 一种机械臂避障路径规划方法 - Google Patents

Abstract

本发明公开了一种机械臂避障路径规划方法，包括以下步骤：S1、获取轨迹规划所需要的参数信息；S2、采用均匀Logistic混沌序列采样器生成随机点X_rand；S3、找到随机树上和点X_rand最近的点X_nearest，然后X_nearest向X_rand延伸一定距离L得到下一个拓展点X_new；S4、以步骤S3中的X_new为圆心，以一定的半径作圆，将随机树上落在圆内的节点作为潜在父节点形成集合Q_near；S5、起始点X_init和目标点X_goal同时向对方搜索，在两颗随机树搜索空间X₁和X₂中，X_r是起始点随机树T₁的一个拓展节点，T₂随机树将向随机树T₁拓展到X_r，最后通过代价判断得到最优节点Xr，将两颗随机树连接完成路径搜索；S6、基于滑动平均平滑算法对S5中生成的路径进行平滑处理，获得适合机械臂运行的实际路径。

Description

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

技术领域

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

背景技术

机械臂路径规划算法自从上世纪被提出来，便得到了快速发展和广泛应用，现在机械臂路径规划算法的算法性能以及十分高效，但在某些方面仍存在着一定的不足，例如全局规划难度大、实时性低等缺点。针对路径规划的问题，曾提出去多算法，如栅格法、蚁群算法、人工势场法、A*算法、快速随机扩展树算法等。其中，人工势场法是依靠机器人对障碍物的排斥力和目标点的吸引力的共同作用进行避障和路径规划，但在规划的过程中存在局部极小的陷阱，当排斥力和吸引力相互抵消时便不能找出路径。

快速随机扩展树算法自从提出便被广泛应用于机械臂的路径规划，其通过随机的采样与障碍物进行碰撞检测，简化了路径规划的算法模型和空间模型。但也快速随机扩展树算法也存在自己的缺点，由于采样点随机分布会导致无法找出合理路径，并且会产生很多无用节点，最终生成的路径会有很多转折并不平滑，将导致机械臂运动过程中震动加大、摩擦加剧。

发明内容

本发明的目的是解决上述问题，提供一种运动平稳，摩擦小，避障效果好的机械臂避障路径规划方法。

为解决上述技术问题，本发明的技术方案是：一种机械臂避障路径规划方法，包括以下步骤：

S1、获取轨迹规划所需要的参数信息，包括轨迹规划的空间范围X，以及X内的障碍物信息，机械臂的起始点X_init以及目标点X_goal；以起始点X_init以及目标点X_goal为根节点分别初始化随机树T1和T2，给定扩展步长L，最大迭代次数Max；

S2、采用均匀Logistic混沌序列采样器生成随机点X_rand；

S3、找到随机树上和点X_rand最近的点X_nearest，然后X_nearest向X_rand延伸一定距离L得到下一个拓展点X_new，判断X_nearest和X_new之间的连线是否发生碰撞，若没有发生碰撞，则将X_new加入随机树中；

S4、以步骤S3中的X_new为圆心，以一定的半径作圆，将随机树上落在圆内的节点作为潜在父节点形成集合Q_near，然后对从起始节点开始并经过集合Q_near内潜在父节点到X_new的路径长度与从起始点开始经过X_nearest到X_new的路径长度相比较，最终选择路径长度最小的节点作为X_new的父节点，然后将新的路径添加进随机树并删除原X_nearest与X_new连接路径。

S5、起始点X_init和目标点X_goal同时向对方搜索，在两颗随机树搜索空间X₁和X₂中，X_r是起始点随机树T₁的一个拓展节点，T₂随机树将向随机树T₁拓展到X_r，然后利用路径代价函数进行选择，代价函数如下所示：

最后通过代价判断得到最优节点Xr，将两颗随机树连接完成路径搜索；

S6、基于滑动平均平滑算法对S5中生成的路径进行平滑处理，获得适合机械臂运行的实际路径。

进一步地，所述步骤S1中，机械臂的起始点X_init以及目标点X_goal的确定方法是，根据内置信息和机械臂传输过来的相关信息初始化机械臂路径地图信息，包括地图的边界以及障碍物信息，同时利用地图信息技术构建地图模型，并在地图中确定机械臂的起始点X_init以及目标点X_goal，采样区域为全地图。

进一步地，所述步骤S2中的均匀Logistic混沌序列采样器，其中均匀Logistic混沌序列采样器生成的采样点Xrand由下面的公式确定：

x_i+1＝μ_xi(1-x_i) (1)

其中μ(0≤μ≤4)是混沌系统的李雅普诺夫指数，x_i(0＜x_i＜1)是混沌变量,根据混沌动力系统的研究，这里取μ取4，当i＝0时，x₀取0.630；

使用式(1)和式(2)相结合生成均匀分布Logistic混沌序列，这样生成随机采样点便具有均匀性的特点。

进一步地，所述步骤S2中，生成随机点X_rand时，先判断采样次数是否超过最大采样次数Max，如果超过结束采样，如果没有最大采样次数则采样次数加一，然后获取扩展采样点X_rand，采样过程为：基于初始化均匀Logistic混沌序列采样器在0-1内进行一次随机数生成，然后计算采样点X_rand是否在可行空间内，如果采样点X_rand有效则进行下一步。

进一步地，所述随机树T1和T2均采用上面步骤S1到步骤S4的过程进行拓展。

进一步地，所述步骤S6中基于滑动平均平滑算法对步骤S5中生成的路径进行平滑处理，其中基于滑动平均平滑算法由下面的公式确定：

设置滑动平均平滑算法的控制点为X_j,j＝0，1，2，…，滑动平均平滑算法的基函数表示为y_i,则滑动平均平滑算法的数学描述函数记为：

其中N表示当前节点进行平均平滑处理所使用相邻左右节点的个数；当N取3时如下所示：

y_a(1)＝y(1)

y_a(2)＝(y(1)+y(2)+y(3))/3

y_a(3)＝(y(1)+y(2)+y(3)+y(4)+y(5))/5

y_a(4)＝(y(1)+y(2)+y(3)+y(4)+y(5)+y(6)+y(7))/7)

y_a(5)＝(y(2)+y(3)+y(4)+y(5)+y(6)+y(7)+y(8))/7)；

将上面得到的平滑路径的数据传输给机械臂，机械臂根据得到的数据运动到目标点抓取目标物，然后再经过路径规划将目标物放置到指定位置。

本发明的有益效果是：本发明所提供的一种机械臂避障路径规划方法，步骤简单，避障效果好，能够有效降低机械臂运动过程中的震动和摩擦，节约机械臂的使用耗费成本。

附图说明

图1是本发明一种机械臂避障路径规划方法的RRT*-Connect算法流程图；

图2是本发明的拓展过程图。

具体实施方式

下面结合附图和具体实施例对本发明做进一步的说明：

如图1和图2所示，本发明提供的一种机械臂避障路径规划方法，包括以下步骤：

S1、获取轨迹规划所需要的参数信息，包括轨迹规划的空间范围X，以及X内的障碍物信息，机械臂的起始点X_init以及目标点X_goal；以起始点X_init以及目标点X_goal为根节点分别初始化随机树T1和T2，给定扩展步长L，最大迭代次数Max。

在本步骤中，机械臂的起始点X_init以及目标点X_goal的确定方法是，根据内置信息和机械臂传输过来的相关信息初始化机械臂路径地图信息，包括地图的边界以及障碍物信息，同时利用地图信息技术构建地图模型，并在地图中确定机械臂的起始点X_init以及目标点X_goal，采样区域为全地图。

在本实施例中，本发明的方法在现有的遨博机器人(AUBO-I5)进行使用，机器人的有效负载为5kg，最大工作半径为886.5mm。

首先初始化机械臂，然后机械臂末端移动到目标物的上方，通过相机获取目标物的位置X_goal以及空间中障碍物的信息，然后将机械臂的位置(起始点)X_init、目标物(目标点)的位置X_goal以及障碍物的信息传给RRT*-Connect算法。

根据内置信息和机械臂传输过来的相关信息初始化机械臂路径地图信息，包括地图的边界以及障碍物信息，同时利用地图信息技术构建地图模型，并在地图中确定机械臂的起始点X_init以及目标点X_goal，采样区域为全地图。然后开始进行机械臂路径规划的准备，以起始点Xinit以及目标点Xgoal为根节点分别初始化树T1和T2，给定扩展步长L，最大迭代次数Max。

S2、采用均匀Logistic混沌序列采样器生成随机点X_rand。

在本步骤中的均匀Logistic混沌序列采样器，其中均匀Logistic混沌序列采样器生成的采样点Xrand由下面的公式确定：

x_i+1＝μ_xi(1-x_i) (1)

其中μ(0≤μ≤4)是混沌系统的李雅普诺夫指数，x_i(0＜x_i＜1)是混沌变量,根据混沌动力系统的研究，这里取μ取4，当i＝0时，x₀取0.630。

使用式(1)和式(2)相结合生成均匀分布Logistic混沌序列，这样生成随机采样点便具有均匀性的特点。

本步骤中，生成随机点X_rand时，先判断采样次数是否超过最大采样次数Max，如果超过结束采样，如果没有最大采样次数则采样次数加一，然后获取扩展采样点X_rand，采样过程为：基于初始化均匀Logistic混沌序列采样器在0-1内进行一次随机数生成，然后计算采样点X_rand是否在可行空间内，如果采样点X_rand有效则进行下一步。

S3、找到随机树上和点X_rand最近的点X_nearest，然后X_nearest向X_rand延伸一定距离L得到下一个拓展点X_new，判断X_nearest和X_new之间的连线是否发生碰撞，若没有发生碰撞，则将X_new加入随机树中。

S4、以步骤S3中的X_new为圆心，以一定的半径作圆，将随机树上落在圆内的节点作为潜在父节点形成集合Q_near，然后对从起始节点开始并经过集合Q_near内潜在父节点到X_new的路径长度与从起始点开始经过X_nearest到X_new的路径长度相比较，最终选择路径长度最小的节点作为X_new的父节点，然后将新的路径添加进随机树并删除原X_nearest与X_new连接路径。

在本实施例中，若Q_near为空则跳过直接进入下一步，若Q_near不为空，则遍历所有从起始节点经过集合Q_near节点到X_new所有路径，然后与从起始节点经X_nearest到X_new的路径相比较，选出路径最短的一条。

如果从起始节点经X_nearest到X_new的路径是最短的那条线路，直接进入下一步，如果不是则断开X_nearest到X_new的连线，然后将最短路径上X_new的上一个节点作为其父节点，并将X_new加入这条路径当中。

随机树T1和T2均采用上面步骤S1到步骤S4的过程进行拓展。

S5、起始点X_init和目标点X_goal同时向对方搜索，在两颗随机树搜索空间X₁和X₂中，X_r是起始点随机树T₁的一个拓展节点，T₂随机树将向随机树T₁拓展到X_r，然后利用路径代价函数进行选择，代价函数如下所示：

最后通过代价判断得到最优节点Xr，将两颗随机树连接完成路径搜索。

S6、基于滑动平均平滑算法对S5中生成的路径进行平滑处理，获得适合机械臂运行的实际路径。

本步骤中在得到最优路径后，基于滑动平均平滑算法对步骤S5中生成的路径进行平滑处理，获得最终路径。其中基于滑动平均平滑算法由下面的公式确定：

设置滑动平均平滑算法的控制点为X_j,j＝0，1，2，…，滑动平均平滑算法的基函数表示为y_i,则滑动平均平滑算法的数学描述函数记为：

其中N表示当前节点进行平均平滑处理所使用相邻左右节点的个数；当N取3时如下所示：

y_a(1)＝y(1)

y_a(2)＝(y(1)+y(2)+y(3))/3

y_a(3)＝(y(1)+y(2)+y(3)+y(4)+y(5))/5

y_a(4)＝(y(1)+y(2)++y(3)+y(4)+y(5)+y(6)+y(7))/7

y_a(5)＝(y(2)++y(3)+y(4)+y(5)+y(6)+y(7)+y(8))/7；

将上面得到的平滑路径的数据传输给机械臂，机械臂根据得到的数据运动到目标点抓取目标物，然后再经过路径规划将目标物放置到指定位置。

本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。

Claims (6)

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

S1、获取轨迹规划所需要的参数信息，包括轨迹规划的空间范围X，以及X内的障碍物信息，机械臂的起始点X_init以及目标点X_goal；以起始点X_init以及目标点X_goal为根节点分别初始化随机树T1和T2，给定扩展步长L，最大迭代次数Max；

S2、采用均匀Logistic混沌序列采样器生成随机点X_rand；

S3、找到随机树上和点X_rand最近的点X_nearest，然后X_nearest向X_rand延伸一定距离L得到下一个拓展点X_new，判断X_nearest和X_new之间的连线是否发生碰撞，若没有发生碰撞，则将X_new加入随机树中；

S4、以步骤S3中的X_new为圆心，以一定的半径作圆，将随机树上落在圆内的节点作为潜在父节点形成集合Q_near，然后对从起始节点开始并经过集合Q_near内潜在父节点到X_new的路径长度与从起始点开始经过X_nearest到X_new的路径长度相比较，最终选择路径长度最小的节点作为X_new的父节点，然后将新的路径添加进随机树并删除原X_nearest与X_new连接路径；

S5、起始点X_init和目标点X_goal同时向对方搜索，在两颗随机树搜索空间X₁和X₂中，X_r是起始点随机树T₁的一个拓展节点，T₂随机树将向随机树T₁拓展到X_r，然后利用路径代价函数进行选择，代价函数如下所示：

最后通过代价判断得到最优节点Xr，将两颗随机树连接完成路径搜索；

S6、基于滑动平均平滑算法对S5中生成的路径进行平滑处理，获得适合机械臂运行的实际路径。

2.根据权利要求1所述的一种机械臂避障路径规划方法，其特征在于：所述步骤S1中，机械臂的起始点X_init以及目标点X_goal的确定方法是，根据内置信息和机械臂传输过来的相关信息初始化机械臂路径地图信息，包括地图的边界以及障碍物信息，同时利用地图信息技术构建地图模型，并在地图中确定机械臂的起始点X_init以及目标点X_goal，采样区域为全地图。

3.根据权利要求1所述的一种机械臂避障路径规划方法，其特征在于：所述步骤S2中的均匀Logistic混沌序列采样器，其中均匀Logistic混沌序列采样器生成的采样点Xrand由下面的公式确定：

x_i+1＝μx_i(1-x_i) (1)

其中μ(0≤μ≤4)是混沌系统的李雅普诺夫指数，x_i(0＜x_i＜1)是混沌变量,根据混沌动力系统的研究，这里取μ取4，当i＝0时，x₀取0.630；

使用式(1)和式(2)相结合生成均匀分布Logistic混沌序列，这样生成随机采样点便具有均匀性的特点。

4.根据权利要求1所述的一种机械臂避障路径规划方法，其特征在于：所述步骤S2中，生成随机点X_rand时，先判断采样次数是否超过最大采样次数Max，如果超过结束采样，如果没有最大采样次数则采样次数加一，然后获取扩展采样点X_rand，采样过程为：基于初始化均匀Logistic混沌序列采样器在0-1内进行一次随机数生成，然后计算采样点X_rand是否在可行空间内，如果采样点X_rand有效则进行下一步。

5.根据权利要求1所述的一种机械臂避障路径规划方法，其特征在于：所述随机树T1和T2均采用上面步骤S1到步骤S4的过程进行拓展。

6.根据权利要求1所述的一种机械臂避障路径规划方法，其特征在于：所述步骤S6中基于滑动平均平滑算法对步骤S5中生成的路径进行平滑处理，其中基于滑动平均平滑算法由下面的公式确定：

设置滑动平均平滑算法的控制点为X_j,j＝0，1，2，…，滑动平均平滑算法的基函数表示为y_i,则滑动平均平滑算法的数学描述函数记为：

其中N表示当前节点进行平均平滑处理所使用相邻左右节点的个数；当N取3时如下所示：

y_a(1)＝y(1)

y_a(2)＝(y(1)+y(2)+y(3))/3

y_a(3)＝(y(1)+y(2)+y(3)+y(4)+y(5))/5

y_a(4)＝(y(1)+y(2)+y(3)+y(4)+y(5)+y(6)+y(7))/7)

y_a(5)＝(y(2)++y(3)+y(4)+y(5)+y(6)+y(7)+y(8))/7)；

将上面得到的平滑路径的数据传输给机械臂，机械臂根据得到的数据运动到目标点抓取目标物，然后再经过路径规划将目标物放置到指定位置。

Images