CN109901397B - 一种使用粒子群优化算法的机械臂逆运动学方法 - Google Patents

Abstract

本发明涉及一种使用粒子群优化算法的机械臂逆运动学方法，作为一种不使用Jacobian矩阵的数值解法，该法对机械臂奇异性免疫。此外，由于PSO的性质，优化指标很容易纳入适宜函数，该法明确综合考虑了关节限制。该法可用于解决非冗余和冗余串联机械臂的IK问题，也可以用于在机械臂工作空间内跟踪连续路径。由于PSO的平面设计理念，与广泛使用的GA算法相比，该法计算高效，且具有寻找机械臂多个IK解的能力。这些特征使PSO‑IK求解法成为一种有吸引力的方法，具有广阔应用前景。未来的工作包括PSO‑IK求解在避障中的应用以及在路径规划、优化控制参数等方面的扩展。

Description

一种使用粒子群优化算法的机械臂逆运动学方法

技术领域

本发明属于多臂机械臂逆运动学技术，涉及一种使用粒子群优化算法的机械臂逆运动学方法。

背景技术

机械臂末端执行器在其工作空间内的运动需要计算高效的逆运动学(IK)解决方案。逆运动学在运动规划、实时动画和机械设计等领域扮演首要角色。因此，人们已经为寻找精确、高效的机械臂逆运动学方法做了很多努力。

通常机器人逆运动学问题的解决方法可分为三类：几何/解析、与Jacobian矩阵相关及基于优化。逆运动学最早和最佳的方法是解析解法，具有精度绝对和计算高效的优点。然而，仅当任三个相邻关节是平移的或任三个连续关节轴在同一点相交时，一个完整的解析解才存在。对于不满足这些条件的机械臂，特别是冗余机械臂，其逆运动学没有通用解析解。因此，额外的逆运动学技术必不可少。逆运动学与Jacobian矩阵相关的解是在关节速度水平上处理，而非关节位置。Jacobian矩阵及其逆/伪逆需要实时执行以获取所需的关节速度轨迹。必须采用闭环模型以保证跟踪误差收敛至零，这样末端执行器可在机械臂工作空间内精确跟踪预定路径。然而，由于关节轴的调整，任务空间内一个小的速度变化需要关节空间一个巨大的速度变化，会出现奇点。奇异性限制了机械臂可用工作空间的体积，并引起不可能的操作。在这样的奇异配置下，Jacobian矩阵将是秩亏且不可逆。因此，人们开发了一些技术来克服奇异性问题，如Jacobian矩阵转置、阻尼最小二乘法(DLS)、选择阻尼最小二乘法、Levenberg-Marquardt法、反馈逆运动学法等。这些方法使末端执行器即使在奇异配置下也可运动，但代价是跟踪精度下降。逆运动学解得第三种方法是基于优化^[9]。Newton-Raphson法简单，但由于只采用原非线性方程的一阶近似，计算负担很高且收敛速度慢。

另一种使用广泛的方法是循环坐标下降(CCD)法及其变种。CCD的流程是将某一时刻的一个关节与末端执行器及目标点对准，并迭代驱动末端执行器与目标。作为一种启发式方法，CCD与奇异性问题和计算效率无关。它的缺点是即使施加约束，其不连续性也不稳定。最近，逆运动学问题的进化算法受到广泛关注。有人提出了一种基于神经网络(NN)的方法，经训练确定神经网络的连接权重，但训练周期很恼人，且一旦训练完成，基于神经网络的方法只可以用于一个机器人。基于优化的逆运动学解还有一种流行的启发式算法遗传算法(GA)。遗传算法根据达尔文和孟德尔的进化理论，模仿自然行为。逆运动学解经复制操作、交叉、变异和进化一代代过滤，直到获得一个有希望的结果。它的主要缺点是复杂的编码和高昂的计算成本。

为克服上述方法的缺点，有人在逆运动学中采用了粒子群优化算法(PSO)。但他们大多只关注逆运动学的位置问题而不关注方向问题。PSO是一种基于智能的启发式优化技术，结合了社会心理学原理和进化计算，在一个复杂的空间内实现最优解。

发明内容

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种使用粒子群优化算法的机械臂逆运动学方法，提出了非冗余和冗余机械臂基于PSO的逆运动学解。机械臂的逆运动学问题首先转化为优化问题，然后采用自适应惯性系数的PSO搜索逆运动学的最优解。是一种计算效率快、跟踪精度高，鲁棒性好，对奇异性不敏感的基于粒子群优化算法的机械臂逆运动学方法。

技术方案

一种使用粒子群优化算法的机械臂逆运动学方法，其特征在于步骤如下：

步骤1、建立n自由度串联机械臂的正运动学和逆运动学表达形式：

以f:θ→SE(3)表示机械臂正运动学，在所有关节位置已知的条件下确定末端执行器的位置和方向；以f^-1:SE(3)→θ∈Rⁿ表示逆运动学，在已知末端执行器姿态的条件下求各关节位置；正运动学表示为：

其中

为从坐标系i到i-1的旋转矩阵，

为坐标系i-1原点到坐标系i原点位置向量表示在坐标系i-1中；

有6个约束；

步骤2、建立逆运动学问题在指定搜索空间内的目标函数和约束条件：

采用以下多重标准：

其中β_i是各单独目标的权重因子，满足条件

g_i(θ)为指定用于优化目标的标量函数，有以下几种：

最大位置误差：

g₁(θ)＝||p_d-p||,g₂(θ)＝||φ_d-φ|

其中p_d，φ_d分别为期望的末端执行器位置和方向(欧拉角)向量，p和φ分别为末端执行器实际位置和方向向量；

目标为执行过程中的总关节角变化：

其中t₀和t_f分别为起始和终止时间，θ_s为机器人初始构型。W为正定权矩阵；

当机器人靠近机械臂中间构型操作，定义另一个目标函数：

其中

代表下界为θ_min、上界为θ_max的机器人中间构型；

步骤3、基于粒子群优化算法(PSO)进行逆运动学求解：首先在可行的搜索区域内初始化具有随机初始值的粒子群，各粒子对其适宜函数进行评估，在搜索空间内搜索目前已知的自身局部最佳位置和群整体最佳位置；各粒子的运动由局部及整体最佳位置指导，每一代更新一次。当发现更好的位置时，就选作粒子群运动的指导。此过程不断重复，直到满足一个特定条件或发现有前景的解决方案；

粒子维数代表设计变量的数量；

第i个粒子的位置和速度的更新形式如下：

其中(x_i1,x_i2,…,x_iK)和(υ_i1,υ_i2,…,υ_iK)分别表示第i个粒子的位置和速度，c₁、c₂是加速度常数，r₁、r₂为[0,1]间的均匀分布值。向量

存有第i个粒子目前的局部最佳位置，

表示粒子群目前总体最佳位置；δt为采样时间，ω为自适应惯性权重因子。ω采用线性递减策略

其中iter_max是最大迭代次数。w_min和w_max分别为惯性权重的下界和上界；

步骤4、在逆运动学求解中施加边界约束条件，如果不满足边界约束条件，返回步骤3继续搜索，直到找到满足条件的逆运动学解集；

设计变量上施加有约束，采用以下粒子位置的修复策略：

粒子群随机分为大小相等的两个子群S_s,s∈{1,2}并初始化如下：

c₃和c₄是两个加权标量，分别用来调节p_gbest附近的群分布和搜寻

附近的速度。rand(0,1)为0到1的正态分布随机数；

PSO算法稳定性的充要条件为0＜(c₁+c₂)＜4及

有益效果

本发明提出的一种使用粒子群优化算法的机械臂逆运动学方法，作为一种不使用Jacobian矩阵的数值解法，该法对机械臂奇异性免疫。此外，由于PSO的性质，优化指标很容易纳入适宜函数，该法明确综合考虑了关节限制。该法可用于解决非冗余和冗余串联机械臂的IK问题，也可以用于在机械臂工作空间内跟踪连续路径。由于PSO的平面设计理念，与广泛使用的GA算法相比，该法计算高效，且具有寻找机械臂多个IK解的能力。这些特征使PSO-IK求解法成为一种有吸引力的方法，具有广阔应用前景。未来的工作包括PSO-IK求解在避障中的应用以及在路径规划、优化控制参数等方面的扩展。

附图说明

图1为本发明实例中所述的4粒子PSO算法示意图

粒子：A,B,C,D目标：T gbest：全局最优pbest：局部最优

图2为本发明实例中所述PUMA560和7自由度拟人机械臂示意图

a：PUMA560机械臂；b：7DOF仿人手臂型机械臂

图3为本发明实例1中遗传算法(GA)和粒子群优化算法(PSO)仿真结果比较图

a：适应度函数收敛；b：平均执行时间

图4为本发明实例3中PUMA560机械臂PSO-IK求解仿真结果示意图

a：位置跟踪误差；b：姿态跟踪误差；c：关节轨迹；d：Jacobian矩阵的行列式；

图5为本发明实例5中7自由度拟人机械臂PSO-IK求解仿真结果示意图

a：位置跟踪误差；b：姿态跟踪误差；c：带优化的机械臂关节轨迹；d：不带优化的机械臂关节轨迹。

具体实施方式

现结合实施例、附图对本发明作进一步描述：

本发明基于粒子群优化算法和逆运动学理论，提出使用粒子群优化算法的机械臂逆运动学方法。关节轨迹求解基于非线性规划法PSO和机械臂递归正运动学。其中纳入了关节范围限制，并明确处理为边界约束。所提方法不仅可用于静力学IK任务，还可用于机器人的跟踪任务。此外，所提方法可避免奇异性问题。具体的逆运动学求解方法如下。

本发明是通过以下技术方案来实现：

本发明一种使用粒子群优化算法的机械臂逆运动学方法，包括，

步骤一，建立n自由度串联机械臂的正运动学和逆运动学表达形式；

步骤二，建立逆运动学问题在指定搜索空间内的目标函数和约束条件；

步骤三，基于粒子群优化算法(PSO)进行逆运动学求解。首先在可行的搜索区域内初始化具有随机初始值的粒子群。粒子维数代表设计变量的数量。各粒子对其适宜函数进行评估，在搜索空间内搜索目前已知的自身最佳位置(局部)和群最佳位置(整体)。各粒子的运动由局部及整体最佳位置指导，每一代更新一次。当发现更好的位置时，就选作粒子群运动的指导。此过程不断重复，直到满足一个特定条件或发现有前景的解决方案；

步骤四，在逆运动学求解中施加边界约束条件并保证其稳定性。

步骤一：建立n自由度串联机械臂的正运动学和逆运动学表达形式。

考虑一个n自由度串联机械臂，令f:θ→SE(3)表示机械臂正运动学，在所有关节位置已知的条件下确定末端执行器的位置和方向。与之相反，f^-1:SE(3)→θ∈Rⁿ表示逆运动学，在已知末端执行器姿态的条件下求各关节位置。若应用齐次矩阵，正运动学可表示为：

其中

为从坐标系i到i-1的旋转矩阵，

为坐标系i-1原点到坐标系i原点位置向量表示在坐标系i-1中。

有6个约束，当dim(θ)＝6时，逆运动学问题是适定的。若dim(θ)＜6，这个问题是过约束的，无可行解。相反，若dim(θ)＞6，问题缺约束，是冗余的。这种情况下，逆运动学问题有无数解。对于给定末端执行器姿态，冗余机械臂仍可在其关节空间内沿自运动流形运动，可应用于额外的优化目标，如碰撞规避或使机械臂远离关节运动边界或奇点。

步骤二：建立逆运动学问题在指定搜索空间内的目标函数和约束条件。

逆运动学解是在指定搜索空间内寻找最佳解。某些机械臂可用解析法解决，但对于其他机械臂，单个或多个目标必须为逆运动学解指定。这些目标可设计为最小或最大化多个/分散成本函数，如最小化末端执行器的位置和方向误差、最大化可操作性等。在本发明中，采用以下多重标准：

其中β_i是各单独目标的权重因子，满足条件

g_i(θ)通常是指定用于优化目标的标量函数。设p_d，φ_d分别为期望的末端执行器位置和方向(欧拉角)向量。最大位置误差可定义为：

g₁(θ)＝||p_d-p||,g₂(θ)＝||φ_d-φ|| (3)

其中p和φ分别为末端执行器实际位置和方向向量。此外，当执行跟踪任务时，不希望有激烈的关节变化。定义一个目标为执行过程中的总关节角变化如下：

其中t₀和t_f分别为起始和终止时间，θ_s为机器人初始构型。W为正定权矩阵。如果我们希望机器人尽可能靠近其中间构型操作，可定义另一个目标函数：

其中

代表下界为θ_min、上界为θ_max的机器人中间构型。同样，当需要新目标时，还可纳入额外的目标函数。

约束是另一个需考虑的问题。实际上，所有的过程都受到约束。传感器有范围限制，执行器有行动区域限制。此外，出于安全或经济原因，往往还有人为限制。设h(θ)为线性及非线性方程，在一系列约束下的逆运动学解描述如下：

步骤三：基于粒子群优化算法(PSO)进行逆运动学求解。

与遗传算法相同，粒子群优化算法也是一种随机搜索法，但原理更简单。它是受到了群体动物协调运动的启发，如鸟和鱼等。在群中的每一个粒子的状态调整都考虑到随机的，认知的和社会的影响。PSO算法首先在可行的搜索区域内初始化具有随机初始值的粒子群。粒子维数代表设计变量的数量。各粒子对其适宜函数进行评估，在搜索空间内搜索目前已知的自身最佳位置(局部)和群最佳位置(整体)。各粒子的运动由局部及整体最佳位置指导，每一代更新一次。当发现更好的位置时，就选作粒子群运动的指导。此过程不断重复，直到满足一个特定条件或发现有前景的解决方案。图1展示了一个4粒子PSO流程图。

设第i个粒子的位置和速度分别表示为(x_i1,x_i2,…,x_iK)和(υ_i1,υ_i2,…,υ_iK)。按PSO算法原则，第i个粒子的位置和速度的更新形式如下：

其中c₁、c₂是加速度常数，r₁、r₂为[0,1]间的均匀分布值。向量

存有第i个粒子目前的局部最佳位置，

表示粒子群目前总体最佳位置。δt为采样时间，ω为自适应惯性权重因子，有效地控制搜索范围。本发明中ω采用线性递减策略

其中iter_max是最大迭代次数。w_min和w_max分别为惯性权重的下界和上界。由式(7)，第i个粒子的更新速度由三部分组成：其以前速度的动量、由局部最佳及整体最佳位置决定的速度增量。最终，粒子的位置随其以前位置和新的速度在采样时间δt内引起的位移更新。

步骤四：在逆运动学求解中施加边界约束条件并保证其稳定性。

选择局部和整体最优粒子是为了指导整个群的运动。因此PSO中采用适宜函数评估群中粒子质量并驱动它们朝向目标。如何选择适宜函数取决于机器人类型、逆运动学任务及优化目标。若只考虑位置误差，可考虑目标函数g₁(θ)作为适宜函数，不用考虑方向误差g₂(θ)，反之亦然。若冗余机械臂要求跟踪任务，可将式(2)表示的目标函数组合作为PSO的适宜函数。

实际中，设计变量上施加有约束。应对PSO中约束的策略有补偿函数、修复算法等。在PSO逆运动学解中，考虑关节范围限制并用修复不可行方案应对。采用以下粒子位置的修复策略：

注意在修复算法中位置和速度的处理都要考虑。若x_ik＜x_min而没有速度变化，υ_ik＜0将驱动粒子再次移出可行搜索空间(对于x_ik＞x_max，υ_ik＞0类似)。速度处理另一个原因是速度对PSO性能的显著影响。当速度太大时，可能会跳过可行解决方案；当速度太小时，收敛速度会很慢。因此，有必要将速度设为零或将第k个速度分量反转以使粒子返回可行搜索区域。

PSO算法中另一个问题是当p_gbest和p_pbestj不再改善时，粒子群发生停滞。经过固定数量的优化步骤，适宜函数的变化率仍然低于一个给定阈值，接近于零。为应对这种情况，粒子群随机分为大小相等的两个子群S_s,s∈{1,2}并初始化如下

c₃和c₄是两个加权标量，分别用来调节p_gbest附近的群分布和搜寻p_pbestj附近的速度。rand(0,1)为0到1的正态分布随机数。

最后简短地讨论PSO算法的稳定性问题。已有人导出了PSO算法稳定性的充要条件。这些条件可表述为0＜(c₁+c₂)＜4及

只要参数c₁、c₂和ω满足以上条件，系统就保证收敛到一个稳定的平衡。PSO算法参数的选择列在表1中。PSO算法实现快速收敛，并已在不同条件下和不同机械臂上通过许多测试。

表1遗传算法和粒子群优化算法参数

在以下实例中，采用本发明所述的逆运动学方法得到的求解效果分别如下。

实例1：本发明在仿真中用C/C++语言编程实现通用机械臂的两种逆运动学求解。一种是基于遗传算法的逆运动学求解(GA-IK)，另一种是基于粒子群优化算法的求解(PSO-IK)，用GA-IK解作为对照。两种求解都在PC/WinXP/2GB RAM/Intel Atom1.6GHz CPU中运行。有研究表明群的规模并无一定的价值。在本发明中，GA的个体数和PSO的粒子数分别设为14，作为计算成本和群多样性的折中。GA和PSO包含的参数列在表1中。

表2 PUMA560机械臂的D-H参数

在本例中，用于证明PSO-IK解的为PUMA560机械臂(图2(a))。它的Denavit-Hartenberg(D-H)参数(扭转角α、连杆长度a、偏移长度d和旋转角度θ)以及关节限制列在表2中。选择PUMA560的原因是存在逆运动学解析解，可用于验证数值解的精度。本例选择的适宜函数是末端执行器位置和方向误差的组合。收敛标准设为10^-k，其中k从-2到-8变化。对于施加收敛标准为10^-k的给定末端执行器姿态，基于GA和PSO的逆运动学求解都初始化为关节限制内的任意值，并实施100次以评估GA和PSO算法的性能。两种逆运动学求解的最佳适宜收敛和平均执行时间绘制在图3中。由图可知，PSO-IK求解的适宜收敛速度远快于GA-IK求解。值得注意的是，当收敛标准增加时，PSO和GA的平均执行时间都增长，而由于PSO设计原理简单，它的执行速度比GA更快。因此，基于PSO的方法更适合逆运动学求解。

实例2：在本例中，我们要表现PSO-IK求解寻找多个逆运动学解得能力。对于给定末端执行器姿态，如果不考虑关节约束，PUMA560最多有8个逆运动学解。

本例中PUMA560的期望末端执行器姿态设为

(37.2319°,34.8296°，69.0820°,0.5746,0.2009,0.3261)，

确定解为θ＝(5°,10°,15°,20°,25°,30°)。收敛标准设为10^-8。经过87次迭代，所有8个解都被找到，列在表3中。

表3 PUMA560给定末端执行器姿态的8个逆运动学解

实例3：在案例1和2说明了静态应用的PSO-IK求解，它也可用于末端执行器连续轨迹跟踪。本次仿真用PUMA560的末端执行器在其工作区间中跟踪一个预定义的抛物线。采样时间为0.1s，机器人初始构型为θ₀＝(0°,-60°,180°,0°,-60°,0°)。期望末端执行器方向和位置轨迹设为：

(0,π/3-2πt/3T,0)和

采样时间T＝10s。图4展示了采用PSO-IK求解的PUMA560跟踪任务仿真结果。可以看到，位置跟踪误差小于0.1mm，方向跟踪误差小于0.0055°。即使在机械臂经过其奇异点时，PSO法产生的关节轨迹也很平滑。由图4(d)，机械臂平稳地通过奇异点，Jacobian矩阵的行列式等于零。值得注意的是，跟踪误差与奇异性不相关，说明该法对奇异性不敏感。

实例4：本例展示了PSO-IK求解对冗余分辨率的潜力。采用7自由度拟人机械臂(图2(b))，其D-H参数和关节限制列在表4中，收敛标准设为10^-8。PSO-IK求解成功找到一个解θ＝(-85.4129°,36.3543°,-89.3915°,-14.7510°,-115.1104°,-29.7095°)。事实上，冗余机械臂有无数个逆运动学解，找到的只是其中一个。该解可作为后续优化的初始可行解。

表4 7自由度拟人机械臂的D-H参数

实例5：本例中，用表4描述的冗余机械臂利用PSO-IK求解在其工作空间内跟踪一个连续路径。仿真的目标是展示PSO-IK求解在连续路径跟踪和冗余分辨率上的潜力。跟踪路径是一条直线，从末端执行器姿态(60°,3°,180°,0.8855,1.1977,-0.0737)到终止末端执行器姿态(60°,3°,18°0,0.8855,1.3977,1.1(在基座坐标系中)，跟踪时间T＝5s。相应机器人的初始配置θ₀＝(60°,45°,0°,72°,0°,-30°,0°)，采样时间为0.1s。由于机械臂的冗余性，公式(4)和(5)可纳入一个额外的优化目标。这里PSO-IK求解选择最小关节角变化g₃(θ)。仿真结果绘制在图5中。位置和方向最大跟踪误差分别为0.123和0.0077°。同样值得注意的是跟踪误差低振幅、高频率地振荡。这种现象仅发生在仿真中，在实际机械臂中并不明显。它将因惯性和机械臂阻尼而衰减。与未优化的关节轨迹比较，额外的优化目标g₃(θ)产生关节轨迹更平滑、变化更少，特别是关节1和3。在本次仿真中，未优化案例总关节角变化计算为218.3897°，优化案例为201.4290°。关节运动减少降低了机械臂的能量消耗，并产生更平滑的关节轨迹，保护驱动机构免于剧烈的关节变化。

Claims (1)

1.一种使用粒子群优化算法的机械臂逆运动学方法，其特征在于步骤如下：

步骤1、建立n自由度串联机械臂的正运动学和逆运动学表达形式：

以f:θ→SE(3)表示机械臂正运动学，在所有关节位置已知的条件下确定末端执行器的位置和方向；以f^-1:SE(3)→θ∈Rⁿ表示逆运动学，在已知末端执行器姿态的条件下求各关节位置；正运动学表示为：

其中

为从坐标系i到i-1的旋转矩阵，

为坐标系i-1原点到坐标系i原点位置向量表示在坐标系i-1中；

有6个约束；

步骤2、建立逆运动学问题在指定搜索空间内的目标函数和约束条件：

采用以下多重标准：

其中β_i是各单独目标的权重因子，满足条件

g_i(θ)为指定用于优化目标的标量函数，有以下几种：

最大位置误差：

g₁(θ)＝||p_d-p||,g₂(θ)＝||φ_d-φ||

其中p_d，φ_d分别为期望的末端执行器位置和方向(欧拉角)向量，p和φ分别为末端执行器实际位置和方向向量；

目标为执行过程中的总关节角变化：

其中t₀和t_f分别为起始和终止时间，θ_s为机器人初始构型，W为正定权矩阵；

当机器人靠近机械臂中间构型操作，定义另一个目标函数：

其中

代表下界为θ_min、上界为θ_max的机器人中间构型；

步骤3、基于粒子群优化算法(PSO)进行逆运动学求解：首先在可行的搜索区域内初始化具有随机初始值的粒子群，各粒子对其适宜函数进行评估，在搜索空间内搜索目前已知的自身局部最佳位置和群整体最佳位置；各粒子的运动由局部及整体最佳位置指导，每一代更新一次；当发现更好的位置时，就选作粒子群运动的指导；此过程不断重复，直到满足条件；

粒子维数代表设计变量的数量；

第i个粒子的位置和速度的更新形式如下：

其中(x_i1,x_i2,…,x_iK)和(υ_i1,υ_i2,…,υ_iK)分别表示第i个粒子的位置和速度，c₁、c₂是加速度常数，r₁、r₂为[0,1]间的均匀分布值；向量

存有第i个粒子目前的局部最佳位置，

表示粒子群目前总体最佳位置；δt为采样时间，ω为自适应惯性权重因子；ω采用线性递减策略

其中iter_max是最大迭代次数，w_min和w_max分别为惯性权重的下界和上界；

步骤4、在逆运动学求解中施加边界约束条件，如果不满足边界约束条件，返回步骤3继续搜索，直到找到满足条件的逆运动学解集；

设计变量上施加有约束，采用以下粒子位置的修复策略：

粒子群随机分为大小相等的两个子群S_s,s∈{1,2}并初始化如下：

c₃和c₄是两个加权标量，分别用来调节p_gbest附近的群分布和搜寻

附近的速度，rand(0,1)为0到1的正态分布随机数；

PSO算法稳定性的充要条件为0＜(c₁+c₂)＜4及

Images