利用粒子群优化方法标定机械臂关节参数

时间：2024-07-28

朱衡，杨东超，常旭，孙可平

（清华大学机械工程系，北京 100084）

1 引言

工业机器人在装配、维修和长时间使用后会由于杆件变形、磨损、间隙等原因使关节参数发生变化[1]，这导致控制器中存储的关节参数精度降低，使机械臂无法完成精准的操作任务。为了提升末端执行器位姿的控制精度，不得不重新进行标定。机器人关节参数的标定过程，分为建模、测量、识别和校正四个阶段[2]。

目前，对于机械臂的标定已经进行了大量的研究。Nubiola提出了一个29参数的校准模型，该模型考虑了包含关节柔顺度在内的所有可能的几何误差，来提高ABB IRB 1600工业机器人的绝对精度。基于最小二乘法对29个参数进行识别，并最终对末端执行器上的若干个标志点进行了1000组测量，验证了机器人的精度[3]。文献[4]提出用三个激光测距传感器向机械臂发射激光束，综合不同传感器测距，利用机械臂末端位姿的改变所引起的激光测距传感器的输出实现末端位姿信息的采集，用以代替传统的位姿信息采集方法，提升了后续关节参数标定的精度。文献[5]研究了对六自由度机械臂的视觉标定，使用简单的标定板，基于OpenCV中的霍夫变换等图像算法及线性回归拟合，获得较高的标定精度。

从某种意义上来说，机械臂的标定是一个寻找关节参数最优值的过程，它能使末端执行器的标称位姿和实际位姿间的误差最小化。因此，优化算法可用于机械臂的标定。近年来，针对群体智能（SI）的研究迅速展开，所谓群体智能，是一种基于对高效、高度组织化的自然系统进行观测而得到的优化方法。在整个系统中，个体间通过相对简单的规则进行交流与合作，从而使群体表现出特定的复杂行为。因此，群体智能可以为不同的应用程序带来更有效的算法，可以解决具有非线性、多极值等特点的复杂函数及组合优化问题，尤其是针对不易写出目标函数解析式的应用也有较好的优化结果[6]。典型的群体智能算法包括蚁群算法、遗传算法、模拟退火算法、禁忌搜索算法和粒子群算法等。其中，粒子群优化算法（Particle Swarm Optimization，PSO）作为一种基于种群的随机优化算法，最早由Kennedy和Eberhart提出，源于对鸟群捕食的行为研究[7]，是一种启发式群智能算法。类似遗传算法，粒子群算法也是执行多点搜索，可以在多样化和集中化之间建立均衡。相较于用其他智能优化算法，如遗传算法进行串联机械臂的运动学参数标定[8]，粒子群算法的原理和编程实现简单，收敛速度较快，搜索精细、方向明确，且具有并行性[9]。

另外，传统的机械臂标定方法中，大多采用D-H参数法进行建模，这一方法在串联机械臂相邻关节两轴平行或近似平行时会出现问题：两轴线相对位置的误差的建模不符合误差模型，这导致部分重要的关节参数误差无法辨识[10]。为了解决这一参数耦合的问题，文献[11]提出了一种MDH参数建模方法，额外增加了一个参数来保证标定得到正确的结果，然而这也增加了算法的复杂性。粒子群算法标定的原理使其能简单、有效地解决由于两轴平行导致的参数耦合问题。因此，选择粒子群算法研究其在机械臂标定中的应用。

粒子群算法在机械臂标定中的应用目前已得到了一定的研究。例如，文献[12]提出用粒子群算法产生误差估计函数来预测机械臂的定位误差。然而，实验结果表明在对近似函数使用低次的正规多项式时，误差的预测不能得到明显的改善。文献[13]提出了一种基于粒子群算法的以单点的重复精度作为适应度函数的方法，去识别六自由度铰链臂坐标测量机（AACMM）的关节零偏，但标定方法较为繁琐，且选用适应度函数时仅考虑了末端探针位置的重复精度。以上这些方法均没有考虑粒子群优化方法与传统标定方法的优劣比较，且标定过程缺乏普适性。

对此，提出了一种相对传统方法更简单有效的、基于粒子群算法的标定方法，用以识别建模中使用Denavit-Hartenberg（DH）方法[14]建立的实际关节参数，针对ABB IRB 120工业机器人进行了实验，给出了迭代优化解，结合实验结果进行误差分析。此外，本法易于实现长度参数和转角参数的分步标定，且可规避相邻关节扭角为零的发散问题。

2 问题描述

2.1 机械臂运动模型

研究对象—ABB IRB 120工业机器人由一个基座和六个旋转关节组成。该机器人的有效载荷为3kg，工作空间范围可达580mm。机器人的尺寸参数和生成运动模型所需的坐标系，如图1～图2所示。其中机械臂各关节坐标系的建立采用了Denavit-Hartenberg（D-H）方法。机械臂的标称D-H 参数，如表1 所示。表中的4个D-H 参数的分别为：杆件长度ɑ，关节距离d，杆件扭角α，关节转角θ。

图1 机械臂工作空间Fig.1 The Size Parameters of the Manipulator

图2 ABB IRB 120工业机器人坐标系Fig.2 ABB IRB 120 Industrial Robot Coordinate System

表1 机械臂标称D-H参数Tab.1 Actual D-H Parameters

该机械臂的关节坐标系i与关节坐标系i-1之间的齐次变换矩阵为：

坐标系i绕Z轴旋转后的齐次变换为：

式中：θ—关节变量；β—关节电机驱动的旋转角度。两个连续关节坐标系i和i-1之间的齐次变换可以由Zi和给出：

由相邻两坐标系的齐次变换矩阵，可以累乘得到基坐标系与固定在机械臂末端执行器上的坐标系之间的整体变换矩阵：

2.2 标定过程

与传统标定方法类似，首先利用测量精度较高的测量臂等装置测量工业机械臂末端执行器上的3个标志点的坐标，同时获得末端执行器的Z轴（法线方向）和Y轴方向矢量，X轴的方向矢量可通过叉乘获得，由此并可确定机器人的末端位姿。这一过程要重复多次，关节转角可从示教器中读出。此后，基于所获得的测量数据，即可利用粒子群优化算法对机械臂运动学参数进行识别，最终获得实际的机械臂D-H 参数矩阵，完成对机械臂的标定，并评估标定结果。

3 原理与方法

在粒子群算法中，群表示为由粒子组成的主体，并可以通过群内各组成部分之间的通信进行求解。而粒子是群的无质量、无体积的组成部分，具有速度和加速度信息。在空间中寻找最优解时，每个粒子的位置可以通过适应度函数来评估。粒子交换彼此的位置信息，通过这种相互作用来更新自己的位置，最终达到所有粒子的最佳位置。在D维空间中搜索时，粒子i的飞行速度和位置通过以下的式子更新：

以上两式中：Xi=(xi1，xi2，xi3，...，xiD)和Vi=(vi1，vi2，vi3，...，viD)是粒子i的当前位置和当前飞行速度，Pi=(pi1，pi2，pi3，...，piD)表示粒子i到当前迭代次数时的最佳位置，即个体极值；G=(g1，g2，g3，...，gD)表示在所有粒子中的适应度函数最小的粒子到当前迭代次数时的最佳位置，即全局极值。粒子飞行速度的迭代式由三个部分组成，第一部分代表了前一速度对当前速度的影响，由惯性权重w来量化；第二部分可理解为粒子当前位置与自身历史最好位置之间的距离，这一差值对当前速度的影响可由加速度系数c1来控制；第三部分可理解为粒子当前位置与群体的历史最好位置之间的距离，这一差值对当前速度的影响可由加速度系数c2来控制。rand为随机函数，取值范围为[0，1]，用以增加搜索的随机性。惯性权重w可以控制粒子的搜索能力。一般来说，权值很大时粒子搜索空间的范围会大大提高，而权值很小时粒子的局部搜索能力会提高，但搜索空间会被限制。

具体到标定机械臂的过程中，每个粒子代表着ABB IRB 120工业机器人在校准过程中的一组D-H参数估计值。首先根据DH参数的初始值在一定范围内进行随机化，作为粒子的初始位置。基于每一粒子，可获得执行器的末端齐次变换矩阵估计值T～：

式中：Zi—各关节绕其Z轴的旋转矩阵—各关节依据D-H参数估计值确定的坐标变换矩阵。末端变换矩阵的形式如下：

在标定的计算过程中，需要确定一个适应度函数来衡量末端变换矩阵实际值T和理论值T～的差异。合适的适应度函数是标定成功的关键：适应度函数代表着迭代前进的方向，控制着每次迭代中粒子群个体极值和全局极值的更新；适应度函数应该包含各关节的信息，通过对所有D-H参数的约束来控制迭代寻优的方向，并且保证D-H参数的微小变化不会引起函数的跃变从而避免奇异。在传统标定方法中，可利用系数矩阵的广义逆左乘误差向量的积作为适应度函数，但引入了所测机械臂的模型；而粒子群算法由于“黑箱”的优势，不必引入实际模型，但必须选择一个有普适性的适应度函数用于迭代。考虑到实际物理意义，选择末端位置理论值与实际值间的空间欧氏距离，以及末端姿态的横滚-俯仰-偏航（RPY）欧拉角与实际值的差值作为适应度函数f：

此外，基于粒子群的优化标定方法非常容易实现分步式标定，无需重新推导模型。这个特点对相机姿态的校准和标定等应用非常有意义。由于串联机械臂末端执行器的姿态仅取决于12个角度参数，可以先确定12个与角度相关的参数，再确定剩下的12个参数，即每一步粒子的维度都降为12。利用粒子群算法标定时，第一步先将12个长度参数固定为初始的D-H参数（长度参数的取值对姿态误差没有影响），将式（12）中的forientation作为适应度函数。迭代至收敛后，再将12个角度参数固定，以式（11）中的fposition作为适应度函数，迭代确定剩余的12个长度参数。

根据机械臂D-H参数的意义可知，在标定角度参数时，如果机械臂前一关节扭角为零，这一关节的转角会与相邻的后一关节的转角耦合，从而造成发散。对这一现象的原因和解决方法分析如下：

相邻两关节间由D-H参数得到的转换矩阵通式为：

其中，左上角的3阶矩阵为旋转矩阵。根据式（7）可知，其与关节绕Z轴的旋转矩阵共同决定了相邻关节的旋转变换矩阵，如式（14）所示。

式中：β—关节驱动器产生的转角。

在串联多关节机械臂中，相邻关节的旋转矩阵相乘，如果前一关节的扭角为0，即αi=0，则相邻关节的旋转变换矩阵变为：

这说明如果前一关节的扭角为0，即两轴平行时，则其与相邻后一关节的旋转矩阵相乘会造成两关节转角参数的耦合，即通过与末端齐次变换矩阵相关的适应度函数进行迭代寻优时，只能确定式（15）中θi与θj的和，而无法区分每一个转角的值。传统标定方法利用误差函数模型优化D-H参数值，这一模型要求末端执行器位姿的微小误差必须能由模型参数的微小误差来表示（否则系数矩阵的广义逆矩阵发生奇异），显然当两关节转角参数发生耦合时，这一要求不能被满足，即不满足误差模型，因此必须增加第五个参数。而基于粒子群算法的标定并不分析机械臂的误差模型。

在使用粒子群算法迭代前对关节参数的初始值进行了一定范围内的随机化处理，并且如式（5）所示。在每一步迭代过程中对关节参数也进行了随机化处理，因此不会出现扭角为零导致发散的结果；而且事实上，对于机器人的零位而言，几乎不可能出现扭角精确为0的情况，所以这并不会影响实际标定操作中程序的使用，不需要额外增加参数就能标定出精确结果。

4 标定实验

为了验证基于粒子群优化算法的标定方法在实际应用中的效果，采用传统标定方法和粒子群算法整体式标定分别对一台实际工作环境中的ABB IRB 120六自由度工业机器人进行了标定实验。

首先，遵循D-H 规则建立该工业机器人的坐标系，如图2所示。以厂家给出的初始D-H 参数作为实验初始值，如表1所示。

之后利用测量臂测量机械臂基坐标系，多次测量后拟合，获得基坐标系相对于测量坐标系的齐次变换矩阵。然后利用测量系统获得机械臂的50 组转角数据以及对应的末端位姿测量值。利用传统标定方法迭代计算机械臂的实际D-H参数，以D-H 参数估计值的增量矩阵的二范数小于阈值10-10作为停止准则，获得标定结果，如表2所示。

表2 传统方法标定实验结果Tab.2 Calibration Results of Traditional Method

利用粒子群优化算法整体法标定，参数设置如下：粒子数N=300，惯性权重w=0.85，加速度常数c1=c2=2，最大迭代次数k=2000；标定结果，如表3所示。

表3 粒子群算法标定实验结果Tab.3 Calibration Results of PSO Method

以标定过程中的粒子群适应度函数值的常用对数作为指标，迭代过程，如图3所示。已迭代至收敛：

图3 粒子群算法迭代过程Fig.3 Iterative Process of PSO Method

对粒子群算法获得的D-H参数标定结果，与标定前的初始D-H 参数、利用传统方法标定得到的D-H 参数进行误差分析。利用测量臂测量得到的机械臂末端位姿作为参考值。定义利用标定结果计算得到的末端执行器位置与参考位置的平均空间欧氏距离作为位置误差；利用末端执行器坐标系相对于基坐标系的RPY欧拉角来描述末端姿态，定义标定结果计算得到的末端姿态与参考姿态的平均差值作为姿态误差，结果如下，如图4所示。

图4 标定位姿误差分析Fig.4 Posture Error Analysis of Calibration

在位置误差方面，根据传统方法的标定结果计算得到的机械臂末端位置与测量得到的参考位置（实际位置）的误差约为标定前的2.6%，而根据粒子群算法整体式标定得到的末端位置与实际位置的误差约为标定前的2.4%，如表4所示。在姿态误差方面，以欧拉角—横滚、俯仰、偏航角误差平均值为参考，根据传统方法标定结果计算得到的末端姿态与测量得到的参考姿态（实际姿态）的误差约为标定前的56.7%，而根据粒子群算法整体式标定得到的末端姿态与实际姿态的误差约为标定前的54.0%。根据以上分析可知，利用粒子群算法整体式标定得到的D-H参数，已经在位置误差和姿态误差上均略优于传统方法标定结果，且远远优于未标定前的D-H参数。这证明了粒子群优化算法完成多自由度机械臂运动学参数标定的可行性和优越性。另外，对相同的测量数据进行多次优化标定，在充分迭代至收敛后，都能得到相同的标定结果，这证明了粒子群算法标定的稳定性。

表4 两种标定方法的位姿误差分析Tab.4 Posture Error Analysis of Two Calibration Methods