CN113547523B - 基于人工势场法和灰狼算法的冗余机械臂求逆解方法 - Google Patents

Abstract

本发明公开了一种基于人工势场法和灰狼算法冗余机械臂求逆解方法，包括第一步、建立冗余机械臂的D‑H参数模型；第二步、构造待优化的目标函数；第三步、计算冗余机械臂各关节在运动空间内各关节角度下的人工势场合力，并将各关节角度下的人工势场合力转换为取到对应关节角度的概率；第四步、利用灰狼算法对目标函数进行优化；冗余机械臂各关节都初始化狼群，根据第三步得到的取到对应关节角度的概率对狼群进行随机取值，灰狼个体的初始位置取大概率对应的关节角度。该方法在狼群初始化时各灰狼个体的初始位置取大概率对应的关节角度，可以有效降低冗余机械臂各关节求得危险位置的概率。

Description

基于人工势场法和灰狼算法的冗余机械臂求逆解方法

技术领域

本发明属于机械臂运动学逆解求解技术领域，尤其涉及一种基于人工势场法和灰狼算法冗余机械臂求逆解方法。

背景技术

目前一般的机械臂多为六自由度机械臂，主要通过机械臂的特殊构型采用几何方法进行求逆解，解法简单，实用性较高，但通用性不高。六自由度及以上的为冗余机械臂，常见的为七自由度机械臂，其具有无数组逆解，各个关节之间的耦合关系复杂，通过几何方法找到关节角度之间的关系会产生较大的计算量，而且求解过程复杂，实时性较差。

文献《构形平面方法求解冗余机械臂逆运动学》在求机械臂逆解中运用了构形平面几何参数方法，将机械臂运动的三维问题转化为二维问题进行求解。文献《一种冗余自由度机械臂逆运动学解析算法》提出了“自由度模块化”的概念，通过将复杂的空间机构运动学问题转化为平面机构问题，大大提高了计算速度，但该方法仅适用于文中的类似构型的机械臂，通用性不强。

灰狼算法通过对灰狼群体捕食行为进行模拟，以狼群合作机制为基础来对目标函数进行优化，不仅结构简单，而且在求解精度方面有很大优势，被广泛应用于诸多领域。通过灰狼算法对冗余机械臂进行求逆解，可以很好地克服了冗余机械臂求解通用性差的缺陷。传统灰狼算法中初始狼群的位置随机，使得个体的位置容易陷入局部最优，因此在冗余机械臂求逆解过程中容易得到危险的关节位置，因此本申请融合人工势场法，降低了灰狼算法求得危险关节位置的概率。

发明内容

针对现有技术的不足，本发明拟解决的技术问题是，提出一种基于人工势场法和灰狼算法的冗余机械臂求逆解方法。

本发明解决所述技术问题采用的技术方案如下：

一种基于人工势场法和灰狼算法冗余机械臂求逆解方法，其特征在于，该方法包括以下步骤：

第一步、建立冗余机械臂的D-H参数模型；

第二步、构造如式(3)所示的待优化的目标函数；

式(3)中，

为冗余机械臂正向运动学的一般方程，

为冗余机械臂末端关节位姿；

第三步、计算冗余机械臂各关节在运动空间内各关节角度下的人工势场合力，并将各关节角度下的人工势场合力转换为取到对应关节角度的概率；

第四步、利用灰狼算法对目标函数进行优化；冗余机械臂各关节都初始化狼群，根据第三步得到的取到对应关节角度的概率对狼群进行随机取值，灰狼个体的初始位置取大概率对应的关节角度。

第四步中灰狼算法的收敛因子a的表达式为：

式(16)中，t为当前迭代次数，t_max为最大迭代次数。

第三步的具体过程为：

对于同一关节，将每个关节角度下的引力和斥力进行求和，得到关节在运动空间内每个关节角度下的人工势场合力；按照式(8)将各关节角度的人工势场合力转换为取到对应关节角度的概率P(λ)；

式(8)中，∑F为关节在运动空间内的人工势场总合力，F(λ)为对应关节角度下的人工势场合力。

与现有技术相比，本发明的有益效果是：

1.本发明首先利用人工势场法计算冗余机械臂各关节在运动空间内各关节角度下的人工势场合力，再将人工势场合力转换为取到对应关节角度的概率；灰狼算法狼群初始化时，各灰狼个体的初始位置取大概率对应的关节角度，而不是采用现有技术狼群初始化过程中在关节的运动空间内随机取值，容易陷入局部最优，因此本发明可以有效降低冗余机械臂各关节求得危险位置的概率。

2.收敛因子的函数为单调非线性递减函数，而且递减速度由快到慢，即在全局优化过程中搜索速率加快，在局部优化过程中搜索速率减慢，进行更加精细的搜索，不仅提高了精度，而且也加快了搜索速度。

附图说明

图1为本发明的整体流程图；

图2为本发明的人工势场合力的变化曲线图；

图3为未改进的灰狼算法与本发明改进后的灰狼算法得到的冗余机械臂末端关节位姿与实际位姿的均方误差曲线图。

具体实施方式

下面结合附图和具体实施方式对本发明的技术方案进行详细说明，但并不以此限定本申请的保护范围。

以七自由度冗余机械臂为例，对本发明的基于人工势场法和灰狼算法冗余机械臂求逆解方法进行详细说明，该方法包括以下步骤：

第一步、利用D-H参数法，根据表1所示的参数建立冗余机械臂的D-H参数模型，即在冗余机械臂的每个关节处都建立一个坐标系；其中，i表示关节编号，

表示第i个关节对应的连杆的扭转角度，l_i表示第i个关节对应的连杆长度，d_i表示第i个关节对应的连杆的偏距，θ_i表示第i个关节角度；

表1冗余机械臂的参数

第二步、构造待优化的目标函数；

在冗余机械臂正向运动学中，根据D-H参数模型得到相邻两根连杆对应的坐标系之间的转换矩阵

为：

式(1)中，Rot(z,θ_i)表示第i个关节坐标系绕z轴的旋转矩阵；Trans(z,d_i)表示第i个关节坐标系沿z轴的平移矩阵；Trans(x,l_i)表示第i个关节坐标系沿x轴的平移矩阵；

表示第i个关节坐标系绕x轴的旋转矩阵；

将所有转换矩阵相乘，得到冗余机械臂正向运动学的一般方程为：

式(2)中，(n_x,n_y,n_z)为冗余机械臂末端关节坐标系的x轴在基坐标系中的投影向量；(o_x,o_y,o_z)为冗余机械臂末端关节坐标系的y轴在基坐标系中的投影向量；(f_x,f_y,f_z)为冗余机械臂末端关节坐标系的z轴在基坐标系中的投影向量；(p_x,p_y,p_z)为冗余机械臂末端关节坐标系原点在基坐标系的位置；

根据冗余机械臂正向运动学一般方程，建立如式(3)所示的用于优化的目标函数Y；

式(3)中，

为冗余机械臂末端关节位姿；

第三步、利用人工势场法计算冗余机械臂各关节在运动空间内各关节角度下的人工势场合力，并将各关节角度下的人工势场合力转换为取到对应关节角度的概率；

由于冗余机械臂的各个关节由电机驱动，因此存在关节限位，当关节靠近关节限位时会使得冗余机械臂处于危险状态，因此需要使冗余机械臂避开关节限位；根据冗余机械臂的工作要求，对各关节进行限位，限位点之间的空间即为关节的运动空间，本实施例中各关节的运动空间为[-π,π]；人工势场包含引力场和斥力场，将限位点设有斥力源，即将-π和π对应的位置分别设为斥力源，将关节角度为零的位置设为引力源，则引力场函数U_att(λ)为：

式(4)中，ρ(λ,λ_g)为关节在运动空间内的位置λ与引力源λ_g之间的距离，A为引力场正比例增益；

根据式(4)的引力场函数，得到引力F_att(λ)为：

F_att(λ)＝A(λ-λ_g) (5)

斥力场函数U_rep(λ)为：

式(6)中，ρ(λ,λ₀)为关节在运动空间内的位置λ与斥力源λ₀之间的距离；B为斥力场正比例增益；ρ₀为斥力场作用半径，当关节在运动空间内的位置λ与斥力源λ₀之间的距离大于斥力场作用半径时，斥力视为零；

根据式(6)的斥力场函数，得到斥力F_rep(λ)为：

对于同一关节，将每个关节角度下的引力和斥力进行求和，得到关节在运动空间内每个关节角度下的人工势场合力，图2为人工势场合力的变化曲线图，在运动空间的中间位置人工势场合力最大，运动空间的两端人工势场合力逐渐减小，并在关节限位点处人工势场合力最小；按照式(8)将各关节角度的人工势场合力转换为取到对应关节角度的概率P(λ)，概率与人工势场合力的变化趋势相同；

式(8)中，∑F为关节在运动空间内的人工势场总合力，F(λ)为对应关节角度下的人工势场合力；

冗余机械臂各关节都按照此步骤计算得到运动空间内各关节角度下的人工势场合力以及取到相应关节角度的概率。

第四步、利用灰狼算法对目标函数进行优化；

对于冗余机械臂各关节都初始化狼群，根据第三步得到的取到对应关节角度的概率对狼群进行随机取值，灰狼个体的初始位置取大概率对应的关节角度，即灰狼个体的初始位置尽量去较大概率对应的关节角度，得到狼群中所有灰狼个体的位置向量Q＝(Q₁,Q₂,…,Q_j,…Q_m)，j＝1,2,3,…m，m为狼群数量，本实施例中狼群数量为1000；灰狼个体的位置向量Q_j＝(q₁,q₂,…,q_j,…q_dim)，dim为狼群维度；

整个狼群被分为四组，将狼群中位置最优的前三匹灰狼个体依次记为α、β、δ，其中α为头狼，灰狼个体β、δ对头狼α进行协助，其余灰狼个体记为ω且无决策能力，α、β、δ用于指导其余灰狼个体ω朝向猎物的方向进行搜索，逐渐靠近猎物，在靠近猎物的过程中不断更新α、β、δ、ω的位置；

包围行为：发现猎物后头狼α对狼群进行召唤，狼群向着头狼α的方向进行奔袭包围猎物，狼群包围猎物满足式(9)：

D＝|E·X_s(t)-X(t)| (9)

X(t+1)＝X_s(t)-L·D (10)

式(9)、(10)中，D为灰狼个体与猎物之间的距离，E为[0,2]之间的随机向量，t为当前迭代次数，X(t)为灰狼个体当前的位置向量，X(t+1)为灰狼个体下一次迭代的位置向量，L为包围步长；X_s(t)为猎物当前的位置向量，s取α、β、δ；

L＝2a(r₁-1) (11)

E＝2r₂ (12)

式(11)、(12)中，r₁、r₂均为标量在[0,1]之间的随机向量，a为收敛因子；

狩猎行为：当头狼α判断出猎物位置后，头狼α带领灰狼个体β、δ指导其余灰狼个体ω进行狩猎，使狼群逐渐逼近猎物；α、β、δ按照式(13)～(15)进行位置更新；

式(13)～(15)中，D_α、D_β、D_δ为α、β、δ与其余灰狼个体ω之间相对距离；E₁、E₂、E₃为随机向量；X_α(t)、X_β(t)、X_δ(t)为α、β、δ当前的位置向量；X(t)为其余灰狼个体ω的位置；X₁(t+1)、X₂(t+1)、X₃(t+1)为α、β、δ下一次迭代的位置向量，决定了其余灰狼个体ω的移动方向和步长；X(t+1)为其余灰狼个体ω的最终位置；

攻击猎物：灰狼算法的迭代主要是参数E和L的动态平衡，随着迭代次数的增加，收敛因子a不断减小，L的取值与收敛因子a同频波动；当|L|＜1时，狼群将包围圈收缩至最小，目标函数有最优解，即算法收敛，搜索得到猎物位置，完成冗余机械臂求逆解；其中收敛因子a的表达式为：

式(16)中，t_max为最大迭代次数，本实施例中最大迭代次数为20000；

式(16)为单调非线性递减函数，而且递减速度由快到慢，故L的取值也由快到慢递减，即在全局优化过程中搜索速率加快，在局部优化过程中搜索速率减慢，进行更加精细的搜索，不仅提高了精度，而且也加快了搜索速度。图3为未改进的灰狼算法(GWO)与本发明改进后的灰狼算法(IGWO)得到的冗余机械臂末端关节位姿与实际位姿的均方误差曲线图，未改进的灰狼算法的收敛因子线性递减，从图中可以看出本发明的均方误差明显小于未改进的灰狼算法，精度更高，即改进后的灰狼算法得到的冗余机械臂末端关节位姿更加接近于实际位姿。

未改进的灰狼算法得到七个关节角度值为：(单位：弧度)

[2.289 -0.067481 -2.547 2.0944 -0.021879 -0.7813 -0.17603]

改进后的灰狼算法得到七个关节角度值为：(单位：弧度)

[-0.27757 0.056286 -0.0005953 2.0944 -0.000729 0.059522 1.0001]

从上述结果可以看出，对于前三个关节角度，未改进的灰狼算法大于改进后的灰狼算法，即未改进的灰狼算法的关节运动范围大，因此更加容易运动到关节限位处，更加容易接近危险位置。在仿真过程中发现，无论其余关节角度如何设置，改进后的灰狼算法得到的冗余机械臂末端关节位姿始终与目标位姿相同，保证了结果的可靠性，同时验证了本方法的精度高。

本发明未述及之处适用于现有技术。

Claims (3)

1.一种基于人工势场法和灰狼算法冗余机械臂求逆解方法，其特征在于，该方法包括以下步骤：

第一步、建立冗余机械臂的D-H参数模型；

第二步、构造如式(3)所示的待优化的目标函数；

式(3)中，

为冗余机械臂正向运动学的一般方程，

为冗余机械臂末端关节位姿；

第三步、计算冗余机械臂各关节在运动空间内各关节角度下的人工势场合力，并将各关节角度下的人工势场合力转换为取到对应关节角度的概率；

第四步、利用灰狼算法对目标函数进行优化；冗余机械臂各关节都初始化狼群，根据第三步得到的取到对应关节角度的概率对狼群进行随机取值，灰狼个体的初始位置取大概率对应的关节角度。

2.根据权利要求1所述的基于人工势场法和灰狼算法冗余机械臂求逆解方法，其特征在于，第四步中灰狼算法的收敛因子a的表达式为：

式(16)中，t为当前迭代次数，t_max为最大迭代次数。

3.根据权利要求1所述的基于人工势场法和灰狼算法冗余机械臂求逆解方法，其特征在于，第三步的具体过程为：

对于同一关节，将每个关节角度下的引力和斥力进行求和，得到关节在运动空间内每个关节角度下的人工势场合力；按照式(8)将各关节角度的人工势场合力转换为取到对应关节角度的概率P(λ)；

式(8)中，∑F为关节在运动空间内的人工势场总合力，F(λ)为对应关节角度下的人工势场合力。

Images