CN111283681B - 一种基于切换scapso的六自由度机械臂逆解方法 - Google Patents

Abstract

本发明公开一种基于切换SCAPSO的六自由度机械臂逆解方法，建立六自由度机械臂模型；利用DH建模获取末端执行器位置与旋转关节角的关系表达式；在约束范围内随机设置NUM个第一代逆解粒子值

使用目标函数fit(P,θ)计算每个逆解粒子目标函数值；根据逆解粒子历史最优θ_pbesti和当前全局最优逆解粒子θ_gbest，使用粒子群算法规则更新得到每一代逆解粒子值

当逆解粒子历史最佳粒子超过Limit代数没更新，并且目标函数改变值大于EPS，切换成基于正余弦函数的粒子群算法，在当前解周围产生随机解；当全局最优解的目标函数在迭代中改变值小于限度值，结束迭代得到全局最优解。此种方法在精准求解机械臂逆运动学的同时，能够解决传统算法中存在的陷入缓慢收敛和局部最优的弊端。

Description

一种基于切换SCAPSO的六自由度机械臂逆解方法

技术领域

本发明属于工业机械臂控制技术领域，特别涉及一种基于切换SCAPSO的六自由度机械臂逆运动学求解方法。

背景技术

近年来，机械臂研究发展迅猛，在工业生产和技术研究中应用广泛，机械臂轨迹规划和位置设计是机械臂控制的主要问题，而机械臂逆运动学问题是以上两关键问题的基础。机械臂逆运动学是将末端执行器的笛卡尔坐标系位置映射到关节坐标系中的一种求解问题，其中存在复杂的非线性映射关系，还存在逆解不唯一和高计算复杂等矛盾。机械臂逆运动学传统方案主要分为三类：解析法、几何法和数值法。解析法存在非线性多耦合问题，导致求解难度大甚至无解现象；几何法构造困难且通用性不高，局限于特定系统；数值法通过迭代实现求解，存在误差累积弊端。如何构造精准有效的机械臂逆运动学求解方案是机械臂控制基础，设计通用型方案对各种类型机械臂控制的改进有重要作用。

将机械臂逆运动学问题归类于多元函数最优问题，智能算法陆续应用于此类问题求解，元式启发算法因其对机械臂模型无特殊限制、求解速度快、通用性强等优势被广泛推广。但智能算法在最优求解过程中容易产生过早收敛从而陷入局部最优的情况，也容易发生收敛过慢不能尽快达到最优解的弊端，对智能算法中存在缺陷的改进，是此类研究的重点所在。粒子群算法作为一种群体智能优化算法，在多元多目标优化问题中达到良好效果，但是上述两类弊端会导致算法陷入收敛缓慢和局部最优的情况。设计改进算法，在迭代陷入局部最优时施加合理扰动，并在不同阶段调整搜索方向和步长，以保证快速精准达到最优值，对解算出符合优化目标的机械臂逆运动学具有重要意义，值得深入研究。

基于以上分析，本案由此产生。

发明内容

本发明的目的，在于提供一种基于切换SCAPSO的六自由度机械臂逆解方法，其是对传统粒子群算法的改进，在精准求解机械臂逆运动学的同时，能够解决传统算法中存在的陷入缓慢收敛和局部最优的弊端。

为了达成上述目的，本发明的解决方案是：

一种基于切换SCAPSO的六自由度机械臂逆解方法，包括如下步骤：

步骤1，基于六自由度机械臂模型，利用DH建模建立末端执行器位置P与机械臂旋转关节角θ之间的关系表达式；

步骤2，在约束范围内随机设置NUM个第一代逆解粒子值

步骤3，根据步骤2中得到的粒子，使用目标函数fit(P,θ)计算每个逆解粒子目标函数值；

所述目标函数fit(P,θ)包括约束函数f(P)和优化函数f(θ)：

其中，P_xi,P_yi,P_zi是第i个逆解粒子根据步骤1中末端执行器位置与机械臂旋转关节角之间的关系表达式计算得到的末端执行器位置；P_x0,P_y0,P_z0是机械臂末端执行器的初始位置坐标；μ_j表示关节角权重系数；θ_ij是第i个逆解粒子的第j个变量，θ_j0是θ_j的初值；

目标函数fit(P,θ)的表达式为：

其中，α、β分别是约束函数与优化函数的权重系数；

步骤4，根据步骤3得到的逆解粒子历史最优θ_pbesti和当前全局最优逆解粒子θ_gbest，使用粒子群算法规则更新得到每一代逆解粒子值

步骤5，当逆解粒子在Limit代数范围内，未更新其历史最佳粒子θ_pbesti，且全局最优逆解粒子θ_gbest的目标函数值fit(θ_gbest)的增量超过最小步长EPS时，切换成基于正余弦函数的粒子群算法，基于SCAPSO更新逆解粒子值，在当前解周围产生随机解；

其中，基于正余弦函数的粒子群算法的更新规则描述如下：

其中，r₂是(0,2π)之间的随机数，用于产生随机解的方向；r₃,r₄是(0,1)之间的随机数，r₄小于等于0.5时使用sin函数产生随机解，r₄大于0.5时使用cos函数产生随机解，r₁是介于(0,1)之间的系数，其表达式描述如下：

其中，R是常数；

步骤6，当全局最优解的目标函数在迭代中改变值小于限度值MAXEPS时，结束迭代得到全局最优解。

上述步骤1中，建立的六自由度机械臂模型是具有三个旋转关节的机械臂，并且具有一个三自由度的末端执行器，属于非冗余机械臂。

上述步骤2中，末端执行器位置与机械臂旋转关节角之间的关系表达式为：

其中，P_x,P_y,P_z是末端执行器的三维坐标，θ₁,θ₂,θ₃是机械臂三个关节角，a₂、a₃、a₄、d₄是机械臂DH建模参数。

上述步骤2的具体内容是：

首先在约束条件内随机产生NUM个粒子作为第一代逆解粒子，其中每个粒子包含三个变量θ₁,θ₂,θ₃，且约束条件根据关节角转动范围限制：

θ_min<θ_j<θ_max

其中，

其中，

分别为θ₁,θ₂,θ₃的最小设定值；

其中，

分别为θ₁,θ₂,θ₃的最大设定值；

粒子通过随机数rand(1)在约束范围内生成，其生成机制是：

上述步骤4的具体过程是：

粒子群算法的更新规则描述如下：

其中，

是第t代第i个逆解粒子的第j个分量的增长量，

是对应的位置，即新一代逆解粒子值；ω是惯性常数系数；

c₁,c₂是逆解粒子自我学习和社会学习系数，其值随迭代次数改变而改变，其表达式描述如下：

其中，c_1min,c_1max,c_2min,c_2max分别是系数c₁,c₂的极值，当迭代次数超过T时，c₁,c₂值不再改变，T是总迭代数。

采用上述方案后，本发明能够有效解决机械臂逆运动学过程中存在的逆解不唯一和高计算复杂等问题，基于正余弦算法的粒子群算法(SCAPSO)可以防止粒子迭代过程中陷入局部最优，也能加快收敛速度，保证找到机械臂最优逆运动学。

本发明在拥有传统智能算法拥有的对机械臂模型无特殊限制、逆运动学求解速度快、无需对机械臂进行复杂设计与计算、通用性强等优势之外，还具有以下两点优势：

(1)当算法迭代陷入局部最优时，使用正余弦算法施加合理扰动，使其在逆解粒子历史最优解和全局最优解共同作用下产生新的随机解；

(2)随着迭代次数增加，使用动态参数调整在不同阶段修正搜索方向和步长，以保证快速精准达到最优值。

附图说明

图1是本发明的原理实现框图；

图2是粒子群算法(PSO)下的全局最优解收敛图；

图3是粒子群算法(PSO)下的全局最优解三维坐标变化图；

图4是切换正余弦函数的粒子群算法(SCAPSO)下的全局最优解收敛图；

图5是切换正余弦函数的粒子群算法(SCAPSO)下的全局最优解三维坐标变化图。

具体实施方式

以下将结合附图，对本发明的技术方案及有益效果进行详细说明。

如图1至图5所示，本发明所述的一种基于切换SCAPSO的六自由度机械臂逆解方法，包括以下步骤：

步骤(1)：基于六自由度机械臂模型，利用DH建模建立末端坐标系位置P_d与旋转关节向量θ的关系表达式；

步骤(2)：在约束范围内随机设置NUM个第一代逆解粒子值

步骤(3)：根据步骤(3)中得到的粒子，使用目标函数fit(P,θ)计算每个逆解粒子目标函数值；

步骤(4)：根据步骤(4)得到的逆解粒子历史最优θ_pbesti和当前全局最优逆解粒子θ_gbest，使用粒子群算法(PSO)规则更新得到每一代逆解粒子值

步骤(5)：当逆解粒子历史最佳粒子θ_pbesti超过Limit代数没更新，并且目标函数改变值大于EPS时，切换成基于正余弦函数的粒子群算法(SCAPSO)，在当前解周围产生随机解；

步骤(6)：当全局最优解的目标函数在迭代中改变值小于限度值MAXEPS时，结束迭代得到全局最优解。

进一步的，所述步骤(1)中建立六自由度机械臂是具有三个旋转关节的机械臂，并且具有一个三自由度的末端执行器，属于非冗余机械臂。机械臂的DH模型的具体步骤如下：

末端执行器的位置和机械臂关节角之间有如下表达式：

其中，(P_x,P_y,P_z)是末端执行器的三维坐标，(θ₁,θ₂,θ₃)是机械臂三个关节角，a₂、a₃、a₄、d₄是机械臂DH建模参数。

进一步的，所述步骤(2)中应用粒子群算法求解机械臂逆运动学的具体步骤如下：

首先需要在约束条件内随机产生NUM个粒子作为第一代逆解粒子，其中每个粒子包含三个变量(θ₁,θ₂,θ₃)值，且约束条件根据关节角转动范围限制：

θ_min<θ_j<θ_max

其中，

其中，

分别为θ₁,θ₂,θ₃的最小设定值；

其中，

分别为θ₁,θ₂,θ₃的最大设定值；

粒子通过随机数在约束范围内生成，其生成机制是：

进一步的，所述步骤(3)中目标函数的具体步骤如下：

目标函数fit(P,θ)是用来衡量选择逆解粒子的标准，其值得大小代表逆解粒子优劣程度，即与最优解的相关程度。目标函数包括两方面内容：

一是约束函数，保证末端执行具有最小位置误差。使用笛卡尔坐标系下，当前位置与末端执行器初始坐标的距离来定义，其值越小，越符合目标位置，具体步骤如下：

其中，P_xi,P_yi,P_zi是第i个逆解粒子根据步骤1中末端执行器位置与机械臂旋转关节角之间的关系表达式计算得到的末端执行器位置；P_x0,P_y0,P_z0是机械臂末端执行器的初始位置坐标；

二是优化函数，在满足距离条件基础上，选择关节角总和最小的逆解粒子，以达到时间最优和能量最优的优化目标，考虑到各关节角电机特性等因素，其旋转时间和消耗能量有所差别，μ_j表示关节角权重系数，具体步骤如下：

其中，θ_ij是第i个逆解粒子的第j个变量，θ_j0是θ_j的初值；

整体目标函数表达式综合以上两点，根据要求可依据权重系数α、β更改约束函数与优化函数比例。目标函数可重新描述如下：

进一步的，所述步骤(4)中传统PSO迭代更新的具体步骤如下：

逆解粒子历史最优θ_pbesti和当前全局最优逆解粒子θ_gbest的选择依据是步骤(4)中的目标函数。逆解粒子历史最优θ_pbesti是某粒子在迭代过程中产生的最优逆解粒子，θ_gbest表示所有粒子的所有代数中产生的最优逆解粒子。粒子群算法用于更新每一代中的逆解粒子值，其更新规则可以描述如下：

其中，

是第t代第i个逆解粒子的第j个分量的增长量，

是对应的位置，即新一代逆解粒子值。

另外，ω是惯性常数系数。c₁,c₂是逆解粒子自我学习和社会学习系数，其值随迭代次数改变而改变，其表达式可以描述如下：

其中，c_1min,c_1max,c_2min,c_2max分别是系数c₁,c₂的极值，当迭代次数超过T时，c₁,c₂值不再改变；T是选取的总迭代数，可调节系数变化速度。

进一步的，所述步骤(5)中基于正余弦函数的粒子群算法(SCAPSO)的具体步骤如下：

传统PSO和SCAPSO用来更新逆解粒子值，但是当传统PSO中某个逆解粒子在Limit代数范围内，未更新其历史最佳粒子θ_pbesti，且此时全局最优逆解粒子θ_gbest的目标函数值fit(θ_gbest)的增量超过最小步长EPS时，使用基于SCAPSO更新逆解粒子值。其更新规则可以描述如下：

其中，r₂是(0,2π)之间的随机数，用于产生随机解的方向；r₃,r₄是(0,1)之间的随机数，r₄小于等于0.5时使用sin函数产生随机解，r₄大于0.5时使用cos函数产生随机解，r₁是介于(0,1)之间的系数，其表达式可以描述如下：

其中，R是常数，取R＝0.1。此步骤用于当传统PSO中某个逆解粒子在Limit代数范围内，未更新其历史最佳粒子θ_pbesti，且此时全局最优逆解粒子θ_gbest的目标函数值fit(θ_gbest)的增量超过最小步长EPS时，使用基于SCAPSO更新逆解粒子值。

进一步的，所述步骤(6)中迭代寻找全局最优解的具体步骤如下：

迭代过程中，产生当前全局最优解，当全局最优解的目标函数在迭代中改变值小于限度值MAXEPS时，结束迭代得到全局最优解，其表达式可以描述如下：

在具体实施例中，整个控制系统的原理图如图1所示，本发明针对六自由度机械臂设计基于传统PSO和改进SCAPSO的逆运动学求解方法，其具体实例如下：

六自由度机械臂DH参数如表1所示，得到末端执行器和机械臂关节角之间有如下表达式：

表1六自由度机械臂DH参数

其中，a₂＝0.180m,a₃＝0.600m,a₄＝0.130m,d₄＝0.630m.

系统迭代中，每一代逆解粒子数设置为NUM＝40，限度值MAXEPS＝10^-6。粒子群算法中机械臂关节约束角度为：

各参数设置为：c_1min＝1,c_1max＝2,c_2mun＝1,c_2max＝2,ω＝0.7298,T＝50.基于正余弦算法的粒子群算法中：R＝1,Limit＝3,EPS＝0.01，末端执行器目标设置为P_f＝(0.529,-0.306,-0.471)。

为了验证本发明的效果，进行了以上实验。图2是传统粒子群算(PSO)下的全局最优解收敛图；图3是传统粒子群算法(PSO)下的全局最优解三维坐标变化图；图4是基于切换正余弦函数的粒子群算法(SCAPSO)下的全局最优解收敛图；图5是基于切换正余弦函数的粒子群算法(SCAPSO)下的全局最优解三维坐标变化图。两种算法均能满足题设要求，解得机械臂逆运动学最优解，SCAPSO方法相较于传统PSO方法能更快达到解的收敛，且有效防止陷入局部最优解。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (5)

1.一种基于切换SCAPSO的六自由度机械臂逆解方法，其特征在于包括如下步骤：

步骤1，基于六自由度机械臂模型，利用DH建模建立末端执行器位置P与机械臂旋转关节角θ之间的关系表达式；

步骤2，在约束范围内随机设置NUM个第一代逆解粒子值

步骤3，根据步骤2中得到的粒子，使用目标函数fit(P，θ)计算每个逆解粒子目标函数值；

所述目标函数fit(P，θ)包括约束函数f(P)和优化函数f(θ)：

其中，P_xi，P_yi，P_zi是第i个逆解粒子根据步骤1中末端执行器位置与机械臂旋转关节角之间的关系表达式计算得到的末端执行器位置；P_x0，P_y0，P_z0是机械臂末端执行器的初始位置坐标；μ_j表示关节角权重系数；θ_ij是第i个逆解粒子的第j个变量，θ_j0是θ_j的初值，θ_j是第j个机械臂旋转关节角；

目标函数fit(P，θ)的表达式为：

其中，α、β分别是约束函数与优化函数的权重系数；

步骤4，根据步骤3得到的逆解粒子历史最优θ_pbesti和当前全局最优逆解粒子θ_gbest，使用粒子群算法规则更新得到每一代逆解粒子值

步骤5，当逆解粒子在Limit代数范围内，未更新其历史最佳粒子θ_pbesti，且全局最优逆解粒子θ_gbest的目标函数值fit(θ_gbest)的增量超过最小步长EPS时，切换成基于正余弦函数的粒子群算法，基于SCAPSO更新逆解粒子值，在当前解周围产生随机解；

其中，基于正余弦函数的粒子群算法的更新规则描述如下：

其中，r₂是(0，2π)之间的随机数，用于产生随机解的方向；r₃，r₄是(0，1)之间的随机数，r₄小于等于0.5时使用sin函数产生随机解，r₄大于0.5时使用cos函数产生随机解，r₁是介于(0，1)之间的系数，其表达式描述如下：

其中，R是常数，t表示第t代，T是总迭代数；

步骤6，当全局最优解的目标函数在迭代中改变值小于限度值MAXEPS时，结束迭代得到全局最优解。

2.如权利要求1所述的基于切换SCAPSO的六自由度机械臂逆解方法，其特征在于：所述步骤1中，建立的六自由度机械臂模型是具有三个旋转关节的机械臂，并且具有一个三自由度的末端执行器，属于非冗余机械臂。

3.如权利要求1所述的基于切换SCAPSO的六自由度机械臂逆解方法，其特征在于：所述步骤1中，末端执行器位置P与机械臂旋转关节角θ之间的关系表达式为：

其中，P_x，P_y，P_z是末端执行器的三维坐标，θ₁，θ₂，θ₃是机械臂三个旋转关节角，a₂、a₃、a₄、d₄是机械臂DH建模参数。

4.如权利要求1所述的基于切换SCAPSO的六自由度机械臂逆解方法，其特征在于：所述步骤2的具体内容是：

首先在约束条件内随机产生NUM个粒子作为第一代逆解粒子，其中每个粒子包含三个变量θ₁，θ₂，θ₃，且约束条件根据关节角转动范围限制：

θ_min＜θ_j＜θ_max

其中，

其中，

分别为θ₁，θ₂，θ₃的最小设定值；

其中，

分别为θ₁，θ₂，θ₃的最大设定值；

粒子通过随机数rand(1)在约束范围内生成，其生成机制是：

5.如权利要求1所述的基于切换SCAPSO的六自由度机械臂逆解方法，其特征在于：所述步骤4的具体过程是：

粒子群算法的更新规则描述如下：

其中，

是第t代第i个逆解粒子的第j个分量的增长量，

是对应的位置，即新一代逆解粒子值；ω是惯性常数系数；

c₁，c₂是逆解粒子自我学习和社会学习系数，其值随迭代次数改变而改变，其表达式描述如下：

其中，c_1min，c_1max，c_2min，c_2max分别是系数c₁，c₂的极值，当迭代次数超过T时，c₁，c₂值不再改变，T是总迭代数。

Images