CN116872212A - 一种基于A-Star算法和改进人工势场法的双机械臂避障规划方法 - Google Patents

Abstract

本发明属于自动化装卸技术领域，尤其涉及一种基于A‑Star算法和改进人工势场法的双机械臂避障规划方法，包括以下步骤：S1、采用D‑H建模法建立双机械臂的运动学模型，得到D‑H参数以及关节之间的转换矩阵；S2、通过双机械臂关节的转换矩阵作为人工势场法的搜索空间，构建立体栅格区域；S3、采用人工势场法对双机械臂末端进行路径规划，其中主机械臂优先进行路径规划，从机械臂将主机械臂作为障碍进行路径规划；S4、当有机械臂陷入局部极小值点时，采用A‑Star算法混合人工势场法，使该机械臂末端跳出局部极小值点，规划全局最优的三维避障路径。本发明对于复杂且任意变化的三维障碍物空间环境均能规划全局最优的三维避障路径，满足了机械臂避障路径规划的要求。

Description

一种基于A-Star算法和改进人工势场法的双机械臂避障规划 方法

技术领域

本发明属于自动化装卸技术领域，尤其涉及一种基于A-Star算法和改进人工势场法的双机械臂避障规划方法。

背景技术

当前，伴随着现代经济飞速发展，人力资源日益昂贵，能代替传统的工业发展模式的机器人产业逐渐崭露头角。现代工业开始加强智能自动化方向的发展，逐步开始引入机器人投入生产环境，但现代工业自动化装卸车中的使用的机器人仍然多为在指定场所完成作业的单机械臂。这类单机械臂的优点是成本低，重复工作精度高，能替代大量的人力资源；但基于这种方式的机械臂系统在生产中的缺点也极为明显，只能按照其原有既定程序做重复的工作，且由于不配备传感器从而无法针对外界环境做出相应调整。

机械臂的避障路径规划方法主要有两种：自由空间法和人工势场法。自由空间法需要预先得到全局的信息，算法开销较大，如A-Star算法(A*算法)以及快速搜索随机树等。相较于自由空间法，人工势场法是一种常用于机器人路径规划的算法，它不需要大量数据，而是实时检测机器人的工作环境。然而，该算法也被广泛证明存在不可避免的缺点，即当引力与斥力的合力为零时，机器人容易陷入局部极小值点，从而无法到达目标点。

经检索，现有专利中，CN114610076B，公开了结合人工势场法与A-star算法角度约束的航迹规划方法，其将人工势场法与改进A-star算法结合分别指导全局路径与局部路径：采用人工势场法进行全局路径规划，无人机的飞行方向由引力场控制，采用改进的A-star算法进行局部路径规划，避让大型障碍物，通过提高避障效果，缩短寻路时间来优化规划路径、提高规划效率，且利用双向搜索模型解决了端点进入角度的约束，更符合实际应用。

但是该专利适用的为无人机的飞行方向路径规划，与机械臂的避障规划相差巨大。

此外，CN114166235A公开了一种基于优化A-STAR算法的全局动态平滑路径规划方法，前期将障碍物进行矩形化处理，无障碍区域应用传统A*算法寻路，在遇到障碍物时，采用优化后的A*算法，在初次完成路径规划后，按照所选取的路径点，加到路径列表进行计算，然后应用人工势场法进行路径平滑处理。本发明不但能够保证搜索到的路径具有平滑性和实时避障能力，还在确保全局最优的基础上提高了寻路效率。

该专利主要适用于自主移动机器人领域，主要应用在静态环境中机器人小车的全局动态平滑路径规划，是在平面上的移动、避障和路径规划，并不适用于三维移动的机械臂。

发明内容

根据以上现有技术中的不足，本发明提供了一种将A-Star算法与改进人工势场法相结合并应用于双机械臂避障规划中，对于复杂且任意变化的三维障碍物空间环境均能规划全局最优的三维避障路径，满足了机械臂避障路径规划的要求双机械臂避障规划方法。

本发明所述的一种基于A-Star算法和改进人工势场法的双机械臂避障规划方法，包括以下步骤：

S1、采用D-H建模法建立双机械臂的运动学模型，得到双机械臂的D-H参数以及双机械臂关节之间的转换矩阵，其中双机械臂包括主机械臂和从机械臂；

S2、通过双机械臂关节的转换矩阵作为人工势场法的搜索空间，构建立体栅格区域；

S3、采用人工势场法对双机械臂末端进行路径规划，其中主机械臂优先进行路径规划，从机械臂将主机械臂作为障碍进行路径规划；

S4、当有机械臂陷入局部极小值点时，采用A-Star算法混合人工势场法，使该机械臂末端跳出局部极小值点，规划全局最优的三维避障路径。

S2中，当转换矩阵(作为人工势场法的搜索空间)确定后，采用人工势场法构建立体栅格区域为现有技术。

所述的S1中，双机械臂的关节数量为m，关节为i，i＝1,2，……，m，经D-H建模法得到双机械臂的D-H参数θ_i、d_i、α_i和a_i，通过坐标变换得到双机械臂相邻两关节之间的转换矩阵。

D-H参数一般使用四个参数：Z轴旋转，使用θ表示；Z轴平移，使用d表示；X轴旋转，使用α表示；X轴平移，使用a表示。

本发明采用的是改进D-H参数，即为两个连杆坐标系变换是先绕X轴转动、平移，再绕Z轴转动、平移。即Z-X顺序，先转动后平移。

所述的S1中，对于主机械臂，为关节i与其上一关节之间转换矩阵，主机械臂末端与主机械臂基座之间的转换矩阵为T_trans：

根据转换矩阵，可以通过主机械臂的关节角度，得到主机械臂末端的位姿；

对于从机械臂，从机械臂由主机械臂平移而来，X_primary为主机械臂的基坐标，X_secondary为从臂的基坐标原点，X_secondary＝X_primary·T_trans。

所述的S3中，采用的人工势场法为将机械臂视为质点，在其转换矩阵中建立由引力场和斥力场构成的人工势场，两者产生的引力和斥力作用在机械臂末端点上使其趋近目标点，以此进行路径规划，其中：

机械臂末端点的坐标为p_n(x_n，y_n，z_n)，目标点坐标为p_g(x_g，y_g，z_g)，其斥力势场函数U_rep(p_n)如下：

式中，d_n为机械臂末端点p_n与障碍物之间的距离，d₀为障碍物斥力场的作用距离，k_rep为斥力常数；

机械臂末端点p_n所受引力势场函数U_att(p_n)为：

式中，k_att为引力常数，p_g为三维环境中的目标点；

机械臂末端所受人工势场函数U_sum(p_n)之和为：

U_Sum(p_n)＝U_att(p_n)+U_rep(p_n)；

机械臂末端点所受的引力与斥力的合力F_sum为：

F_sum＝F_att+F_rep；

式中，F_att为目标点物体对机械臂末端点的引力；F_rep为三维工作空间中障碍物对机械臂末端点的斥力。

主机械臂与从机械臂均按照上述方法进行路径规划，区别为坐标不同以及从机械臂将主机械臂作为障碍。

所述的S4中，当路径规划中有机械臂陷入局部极小值点时，引入A-Star算法，A-Star算法是启发式搜索算法，能够实现全局最优路径规划，在搜索时加入了与问题有关的估价函数，机械臂每走一步都需要计算该估价函数的代价，最小估计代价值的节点就是机械臂末端下一步将到达的位置，从而能够搜索一条从起始点至目标点的最短路径，其中A-Star算法启发式估计代价函数表达式为：

f(n)＝g(n)+h(n)；

式中，f(n)为机械臂末端从起始点到目标点的最优路径代价之和，g(n)为从起始点到当前节点n的实际代价，h(n)为从当前节点n到目标节点的估计代价；节点即为路径中经过的路径点。

A-Star算法使用的估计代价函数h(n)为机械臂末端点位置p_n(x_n，y_n，z_n)与目标点p_g(x_g，y_g，z_g)之间的欧氏距离，h(n)为：

本发明所具有的有益效果是：

针对人工势场法容易陷入局部极小值点，从而无法到达目标点问题，本发明采用了混合势场法，即为将A-Star算法与改进人工势场法相结合并应用于双机械臂避障规划中，先采用人工势场法对机械臂末端进行路径规划，主机械臂优先进行路径规划，从机械臂将主机械臂作为障碍进行路径规划，当机械臂陷入局部极小值点时，采用具有全局规划特性的A-Star算法混合人工势场法的方法，使机械臂末端跳出局部极小值点，规划全局最优的三维避障路径，对于复杂且任意变化的三维障碍物空间环境均有效，满足了机械臂避障路径规划的要求。

附图说明

图1为本发明的流程原理图；

图2为本发明实施例1的仿真实验结果图。

具体实施方式

下面结合附图对本发明的实施例做进一步描述：

如图1所示，一种基于A-Star算法和改进人工势场法的双机械臂避障规划方法，包括以下步骤：

S1、采用D-H建模法建立双机械臂的运动学模型，得到双机械臂的D-H参数以及双机械臂关节之间的转换矩阵，其中双机械臂包括主机械臂和从机械臂；

S2、通过双机械臂关节的转换矩阵作为人工势场法的搜索空间，构建立体栅格区域；

S3、采用人工势场法对双机械臂末端进行路径规划，其中主机械臂优先进行路径规划，从机械臂将主机械臂作为障碍进行路径规划；

S4、当有机械臂陷入局部极小值点时，采用A-Star算法混合人工势场法，使该机械臂末端跳出局部极小值点，规划全局最优的三维避障路径。

S1中，双机械臂的关节数量为6，即为采用六自由度的双机械臂，关节为i，i＝1,2，……，6，对于主机械臂，经D-H建模法得到机械臂的D-H参数如表1所示：

表1 D-H参数表

由机械臂D-H参数，通过坐标变换得到相邻两关节之间的转换矩阵为：

为关节i与其上一关节之间转换矩阵，θ_i、d_i、α_i和a_i为机械臂的D-H参数。

机械臂末端与机械臂基座之间的转换矩阵为T_trans：

根据转换矩阵，可以通过机械臂的关节角度，得到机械臂末端的位姿，即正运动学；反之，可以反解公式，通过机械臂末端的位姿，求得机械臂关节角度，即逆运动学。

对于从机械臂，从机械臂由主机械臂平移而来，X_primary为主机械臂的基坐标，X_secondary为从臂的基坐标原点，X_secondary＝X_primary·T_trans。

S3中，采用的人工势场法为将机械臂视为质点，在其转换矩阵中建立由引力场和斥力场构成的人工势场，两者产生的引力和斥力作用在机械臂末端点上使其趋近目标点，以此进行路径规划，其中：

机械臂末端点的坐标为p_n(x_n，y_n，z_n)，目标点坐标为p_g(x_g，y_g，z_g)，其斥力势场函数U_req(p_n)如下：

式中，d_n为机械臂末端点p_n与障碍物之间的距离，d₀为障碍物斥力场的作用距离，k_rep为斥力常数；

机械臂末端点p_n所受引力势场函数U_att(p_n)为：

式中，k_att为引力常数，p_g为三维环境中的目标点；

机械臂末端所受人工势场函数U_sum(p_n)之和为：

U_sum(p_n)＝U_att(p_n)+U_rep(p_n)；

机械臂末端点所受的引力与斥力的合力F_sum为：

F_sum＝F_att+F_rep；

式中，F_att为目标点物体对机械臂末端点的引力；F_rep为三维工作空间中障碍物对机械臂末端点的斥力。

S4中，当路径规划中有机械臂陷入局部极小值点时，引入A-Star算法，A-Star算法在搜索最优路径时加入了与问题有关的估价函数，机械臂每走一步都需要计算该估价函数的代价，最小估计代价值的节点就是机械臂末端下一步将到达的位置，从而能够搜索一条从起始点至目标点的最短路径，其中A-Star算法启发式估计代价函数表达式为：

f(n)＝g(n)+h(n)；

式中，f(n)为机械臂末端从起始点到目标点的最优路径代价之和，g(n)为从起始点到当前节点n的实际代价，h(n)为从当前节点n到目标节点的估计代价；

A-Star算法使用的估计代价函数h(n)为机械臂末端点位置p_n(x_n，y_n，z_n)与目标点p_g(x_g，y_g，z_g)之间的欧氏距离，h(n)为：

如图2所示，在仿真实验结果图中，按照本避障规划方法，主机械臂和从机械臂的末端均避开了圆柱形的障碍物并成功到达了目标点，且选择的路径为最优。

Claims (5)

1.一种基于A-Star算法和改进人工势场法的双机械臂避障规划方法，其特征在于包括以下步骤：

S1、采用D-H建模法建立双机械臂的运动学模型，得到双机械臂的D-H参数以及双机械臂关节之间的转换矩阵，其中双机械臂包括主机械臂和从机械臂；

S2、通过双机械臂关节的转换矩阵作为人工势场法的搜索空间，构建立体栅格区域；

S3、采用人工势场法对双机械臂末端进行路径规划，其中主机械臂优先进行路径规划，从机械臂将主机械臂作为障碍进行路径规划；

S4、当有机械臂陷入局部极小值点时，采用A-Star算法混合人工势场法，使该机械臂末端跳出局部极小值点，规划全局最优的三维避障路径。

2.一种根据权利要求1所述的基于A-Star算法和改进人工势场法的双机械臂避障规划方法，其特征在于：所述的S1中，双机械臂的关节数量为m，关节为i，i＝1，2，……，m，经D-H建模法得到双机械臂的D-H参数θ_i、d_i、α_i和a_i，通过坐标变换得到双机械臂相邻两关节之间的转换矩阵。

3.一种根据权利要求2所述的基于A-Star算法和改进人工势场法的双机械臂避障规划方法，其特征在于：所述的S1中，对于主机械臂，为关节i与其上一关节之间转换矩阵，主机械臂末端与主机械臂基座之间的转换矩阵为T_trans：

根据转换矩阵，可以通过主机械臂的关节角度，得到主机械臂末端的位姿；

对于从机械臂，从机械臂由主机械臂平移而来，X_primary为主机械臂的基坐标，X_secondary为从臂的基坐标原点，X_secondary＝X_primary·T_trans。

4.一种根据权利要求1所述的基于A-Star算法和改进人工势场法的双机械臂避障规划方法，其特征在于：所述的S3中，采用的人工势场法为将机械臂视为质点，在其转换矩阵中建立由引力场和斥力场构成的人工势场，两者产生的引力和斥力作用在机械臂末端点上使其趋近目标点，以此进行路径规划，其中：

机械臂末端点的坐标为p_n(x_n，y_n，z_n)，目标点坐标为p_g(x_g，y_g，z_g)，其斥力势场函数U_rep(p_n)如下：

式中，d_n为机械臂末端点p_n与障碍物之间的距离，d₀为障碍物斥力场的作用距离，k_rep为斥力常数；

机械臂末端点p_n所受引力势场函数U_att(p_n)为：

式中，k_att为引力常数，p_g为三维环境中的目标点；

机械臂末端所受人工势场函数U_sum(p_n)之和为：

U_sum(p_n)＝U_att(p_n)+U_rep(p_n)；

机械臂末端点所受的引力与斥力的合力F_sum为：

F_sum＝F_att+F_rep；

式中，F_att为目标点物体对机械臂末端点的引力；F_rep为三维工作空间中障碍物对机械臂末端点的斥力。

5.一种根据权利要求4所述的基于A-Star算法和改进人工势场法的双机械臂避障规划方法，其特征在于：所述的S4中，当路径规划中有机械臂陷入局部极小值点时，在人工势场法中引入A-Star算法，A-Star算法在搜索最优路径时加入了与问题有关的估价函数，机械臂每走一步都需要计算该估价函数的代价，最小估计代价值的节点就是机械臂末端下一步将到达的位置，从而能够搜索一条从起始点至目标点的最短路径，其中A-Star算法启发式估计代价函数表达式为：

f(n)＝g(n)+h(n)；

式中，f(n)为机械臂末端从起始点到目标点的最优路径代价之和，g(n)为从起始点到当前节点n的实际代价，h(n)为从当前节点n到目标节点的估计代价；

A-Star算法使用的估计代价函数h(n)为机械臂末端点位置p_n(x_n，y_n，z_n)与目标点p_g(x_g，y_g，z_g)之间的欧氏距离，h(n)为：