基于无量纲化辨识雅可比矩阵选取测量位姿的Stewart并联机构运动学标定

强红宾，薛大鹏，冯新宇，张立杰，2*

(1. 燕山大学 河北省重型机械流体动力传输与控制重点实验室，河北 秦皇岛 066004；2. 燕山大学 先进锻压成形技术与科学教育部重点实验室，河北 秦皇岛 066004)

1 引 言

并联机构具有结构紧凑、承载能力强、运动精度高、惯性低等优点，因此被广泛应用于调姿机构[1]、指向机构[2]、加工制造设备等领域。但由于并联机构制造及装配过程中产生的误差不可避免，导致并联机构的理论运动学参数与实际运动学参数之间也存在一定的误差，从而使并联机构的运动偏离理想轨迹，降低精度。通过运动学标定对运动学模型进行改进，可以提高并联机构的运动精度[3]。

运动学标定通常包括误差模型、位姿测量、参数辨识和误差补偿四个环节[4]。误差模型的研究主要包括误差参数的选择和基于机构运动学、智能算法等模型的建立等。位姿测量主要包括测量工具的选择和测量位姿的选择等。参数辨识主要研究基于最小二乘法、牛顿迭代算法及智能算法的误差参数求解。误差补偿包括对运动学模型的补偿和对控制系统的补偿。Gao等[5]建立了Stewart平台基于运动学模型的误差模型，并采用激光跟踪器LTD5OO进行位姿测量。Li等[6]采用双球棒(DBB)和三轴千分尺测量了三棱锥机器人末端执行器的位置，采用牛顿拉弗森迭代法和最小二乘法辨识出结构参数。Wu等[7]先采用牛顿迭代对并联机床进行初始误差参数辨识，再采用遗传算法进行精细误差参数辨识，得到了较高精度。

选择合理的测量位姿可以提高运动学标定对测量噪音的鲁棒性。测量位姿的选择主要包括工作空间内随机选择、工作空间内均布选择、基于正交理论的选择等方法。文献[8-9]在Stewart并联机构工作空间内随机选取了多个测量位姿。吴江宁等[10]根据Stewart并联机构关节空间选取了7个测量位姿。Bai等[11]在Delta并联机构工作空间的边界选择多个等距的姿态。GUO等[12]基于正交理论选择6PUS并联机械手的测量姿态。Sun等[13]基于三自由度旋转并联机器人运动轨迹选择测量位姿。

为了进一步提高标定对测量噪音的鲁棒性，许多学者研究了基于辨识雅克比矩阵的可观测性指标最大化的测量配置选择方法。文献[14-19]提出了5个基于辨识雅克比矩阵的可观测性指标O1～O5。Daney等[20]采用局部收敛法和禁忌搜索法获得了一组可观测性指标最大的测量位姿，并通过Stewart机构的仿真验证了此测量位姿对传感器测量噪声的鲁棒性有明显提升。Zhang等[21]利用DETMAX算法和禁忌搜索算法，提出了一种高效的测量位姿选择算法，并在5个可观测性指标下的仿真验证了该算法的有效性，结果表明通过所选测量位姿可以显著提高标定方法的鲁棒性。Huang等[22]以可观测性指标O2最大化选取四自由度并联机器人的测量位姿，通过标定保证并联机器人的定位和旋转精度。Gao等[23]以可观测性指标O4最大化选取六自由度冗余驱动的并联机器人的测量位姿。Saputra等[24]对Stewart平台利用群智能搜索算法分别以前4个可观测性指标最大化搜索了测量位姿，并采用千分尺测量位姿完成了运动学标定。Joubair等[25-26]研究了机器人标定中5项可观测性指标的有效性，结果表明不同可观测性指标对不同类型的机器人标定性能不同，整体来说可观测性指标O1的性能更好一些。

采用与辨识雅可比矩阵相关的可观测性指标最大化的方法选择测量位姿，可以提高并联机构运动学标定对传感器测量噪声的鲁棒性。但是，辨识雅可比矩阵是并联机构位移和姿态的函数，且位移和姿态对应的辨识雅可比矩阵中的元素的数量级不同。直接求解辨识雅可比矩阵求取性能指标选择出的位姿，其位置和姿态对测量噪音的鲁棒性不同。因此，本文提出先对辨识雅可比矩阵拆分为位置误差雅克比矩阵和姿态误差雅克比矩阵，在通过特征长度使位移和姿态对应的辨识雅可比矩阵中的元素的数量级相同，再通过可观测性指标选择测量位姿，保证位移和姿态对测量噪音具有相同的鲁棒性。提高机构的标定整体性能和精度。

2 Stewart并联机构分析

图1为Stewart并联机构结构简图。该机构的基本构件为静平台、动平台和六个支腿。静平台固定，六个支腿为驱动器，通过胡克铰连接静平台与动平台，动平台为运动末端。支腿与静平台连接的铰点为Ai(i=1，2，…，6)，与动平台连接的铰点为Bi(i=1，2，…，6)。动、静平台的胡克铰分布如图2所示。

铰点Ai，Bi的坐标ai，bi的表达式为：

ai=[RacosηiRasinηi0]，

(1)

bi=[RbcosφiRbsinφi0]，

(2)

图1 Stewart并联机构结构简图Fig.1 Structure diagram of Stewart parallel manipulator

式中：

2.1 逆运动学分析

逆运动学为已知动平台的位移和姿态求解六个支腿的长度。支腿长度的求解公式为：

li=Rbi+P-ai，

(3)

其中：P=[x，y，z]是动平台的位移。R为动平台的旋转矩阵，本文采用RPY角γ，β，α来表示，具体表达式为：

(4)

其中γ，β，α分别为动平台绕x轴，y轴，z轴的转动。

2.2 正运动学分析

正运动学为已知6个支腿的长度求解动平台的位移和姿态。正解公式为：

f(T)=‖(Rbi+p-ai)‖2-‖li‖2=0.

(5)

并联机构的位置反解可以通过公式(3)直接求解，而位置正解公式(5)由于包含非线性方程组却相当复杂。目前常用数值方法对公式(5)进行求解。

3 误差模型

对于Stewart并联机构，每个分支有7 个误差参数，包括上、下平台的铰点沿x，y，z3个方向的定位误差和驱动杆的轴向误差，则6个分支共有42项误差。则基于运动学的误差模型建立对公式(3)两边微分：

dliui+lidui=d(R)bi+Rdbi+dp-dai.

(6)

JTdT=Jede，

(7)

其中：

dT=[dxdydzdαdβdγ]T∈R6×1，

de=[dl1da1db1… dl6da6db6]T∈R42×1.

由于在六自由度并联机构工作空间内，JT为非奇异矩阵，则：

dT=Jde，

(8)

公式(8)即为单位姿运动学误差模型，1个位姿可得到6个方程，理论上讲只需测量7个位姿，即能构造出42 个约束方程，辨识出机构的42个误差参数。实际标定时由于测量噪音等的存在，需要测量m(m>7)个位姿，以便于更好地辨识误差参数。则m个位姿的误差模型为：

dTz=Jzde，

(9)

其中：Tz∈R6m×1，Jz∈R6m×42。

4 基于无量纲化辨识雅克比矩阵的

可观测性指标

为了提高测量位姿对测量噪音的鲁棒性，我们希望误差参数de的一个非常小的变化能够对位姿误差dT产生尽可能大的影响，即‖dT‖/‖de‖尽可能大。

Nahvi等[18]利用辨识雅克比矩阵Jz的奇异值来表‖dT‖/‖de‖：

(10)

其中σL和σ1分别是辨识雅克比矩阵Jz的非零最小奇异值和最大奇异值。

从几何角度看，如果定义de为一个单位球，那么dT为一个椭球体，其半轴为辨识雅克比矩阵的奇异值[18]，如图3所示。通过扩大椭球体的体积，可以提高标定位姿误差对参数误差的灵敏度。

图3 奇异值的几何表示图Fig.3 Geometric interpretation of singular values

4.1 可观测性指标

Menq和Borm[14-15]定义了第1个可观测性指标O1，辨识雅克比矩阵奇异值的几何平均值。通过最大化O1，可以增加椭球体的体积[18]。根据指标O1可以找到由机构参数误差导致位姿误差最大的位姿，从而使参数估计的效果更好。该指标的表达式如式(11)：

(11)

式中：m是测量位姿的数量，L为待识别的参数数量。

Driels和Pathre[16]提出了第2个指标O2，辨识雅克比矩阵条件数的倒数。最大化O2，可以提高辨识雅克比矩阵奇异值的一致性，提高了椭圆偏心率。该指标的表达式如式(12)：

O2=σL/σ1.

(12)

Nahvi和Hollerbach[17]提出了第3个指标O3，辨识雅克比矩阵奇异值的最小值。最大化O3，会增加椭球的最小半径和体积。使所选择的校准配置对参数误差更敏感。该指标的表达式如式(13)：

O3=σL.

(13)

Nahvi和Hollerbach[18]提出了第4个指标O4，辨识雅可比矩阵的最小非零奇异值的平方除以最大奇异值。最大化O4，会增加椭球体的短轴，最大轴变小(即偏心率变小)。该指标的表达式如式(14)：

(14)

Sun和Hollerbach[19]提出第5个指O5，辨识雅可比矩阵的非零奇异值的调和平均值。该指标的表达式如式(15)：

(15)

4.2 辨识雅可比矩阵无量纲化

目前学者们采用辨识雅克比矩阵Jz的奇异值求解可观测性指标，但是辨识雅可比矩阵是位移和姿态的函数，但位移和姿态对应的辨识雅可比矩阵中的元素的数量级不同，直接求解辨识雅可比矩阵得到可观测性指标，获得的测量配置会导致位移和姿态对测量误差的鲁棒性不同。因此，本文提出先对辨识雅可比矩阵进行无量纲化，再通过可观测性指标选择测量位姿，保证位移和姿态对测量噪音具有相同的鲁棒性。

根据误差模型公式(8)，可以将动平台位置误差矢量和姿态误差矢量分开表示：

(16)

式中：Jp∈R3×42为位置误差传递子矩阵，Jp为J的前3行；Jθ∈R3×42为姿态误差传递子矩阵，Jθ为J的后3行。

当对m个位姿进行测量时，位置误差传递矩阵为Jpm∈R3m×42， 姿态误差传递矩阵为Jθm∈R3m×42。为了使Jpm与Jθm中元素数量级相等，对矩阵进行无量纲化：

(17)

Jzg即为无量纲化辨识雅克比矩阵。将无量纲化的辨识雅克比矩阵Jzg代替原有的辨识雅克比矩阵Jz，去求解可观测指标Oi，再通过最大化指标Oi选取测量位姿。

5 测量位姿选择算法

许多学者对基于可观测指标Oi的位姿选择算法进行了研究。其中，Daney等[20]提出的局部收敛和禁忌搜索的方法，具有不易陷入局部最优的特点。其工作原理及算法流程如图4所示。

图4 最优位姿选择流程图Fig.4 Flow chart of selecting the measurement poses

(1)生成待选位姿Ω0。由于任务工作空间为、互相耦合的六维，直接生成的位姿点不一定位于工作空间内，而关节空间是相互独立的，因此通过关节空间6个杆长n-1等分，产生n6个杆长组合，再通过正解公式(2)获得n6个位姿Ω0。

(2)设置初始位姿Ω1。在Ω0中任意选择m个位姿作为初始位姿。

(5)终止条件。重复步骤(3)和步骤(4)，直到步骤(3)中添加的位姿T+为步骤(4)中被删除的位姿T-。

6 数值仿真验证

6.1 仿真流程

仿真流程包括选择位姿、生成测量位姿的测量值、基于牛顿迭代的参数辨识过程、误差补偿、检验标定结果，如图5所示，详细具体过程如下。

(1)选择测量位姿。设定Stewart并联机构理论结构参数e(1)，根据图4选择测量位姿Tm，并通过运动学反解公式(3)得到测量位姿Tm的支腿长度L0。

(2)生成测量位姿的测量值。设定并联机构误差参数e0，与理论结构参数e(1)相加得到实际结构参数ea，根据实际结构参数ea和支腿长度L0正解出实际位姿Ta，在实际位姿Ta上添加测量噪声Te获得实际测量位姿Tam。

(4)误差补偿。直接对运动学模型进行补偿，将辨识结构参数e(k+1)代替理论结构参数e(1)。

(5)标定结果检验。随机给定一组位姿Ts反解出支腿长度，再根据实际结构参数e0+e(1)正解出实际位姿TJ，通过实际位姿TJ减去位姿Ts得到误差参数辨识补偿后的位姿误差dTs。

图5 仿真流程图Fig.5 Flowchart of simulation

通过比较基于Jzg选取测量位姿的Stewart稳定平台运动学标定位姿误差dTs与基于Jz选取测量位姿的标定位姿误差dTs的大小，来判断哪种方法的标定精度更高。此外，还可以通过判断最终的迭代位姿误差dT(k)的大小、最终辨识误差e(k)-e(1)与实际设定误差e0差值的大小，来判断哪种方法的标定精度更高。

6.2 数值验证

给定一组六自由度并联机构结构参数，如表1所示。对支腿进行二等分，求出36个候选位姿。

表1 六自由度并联机构结构参数Tab.1 Theoretical structural parameters

设定标定位姿数m=18。分别基于未无量纲化的雅克比矩阵Jz和无量纲化的雅克比矩阵Jzg以可观测性指标O1为指标，根据图4选取测量位姿，其中可观测性指标O1的迭代过程如图6所示。

图6 指标O1的迭代过程Fig.6 Values of O1 in the iterative

迭代18次后均可达到可观测性指标O1的最大值。所选择的测量位姿，如表2、表3所示。其中，对辨识雅克比矩阵进行无量纲化时，位移对应的辨识雅可比矩阵中的元素的数量比姿态大，特征长度K=600。

表2 基于Jz选取的测量位姿Tab.2 Measurement poses base on Jz

表3 基于Jzg选取的测量位姿Tab.3 Measurement poses base on Jzg

设置结构误差参数e0，如表4所示，单位为毫米。正解求出18个实际位姿。设定3个水平的测量噪音，如表5所示。

表4 机构误差参数e0Tab.4 Structure parameter error e0

表5 3个水平的测量噪音Tab.5 Measurement sensor noise of three levels

图7 位姿误差和结构误差迭代过程Fig.7 Iterative process of pose and structure parameter error

在测量噪音1下，根据图5迭代求解位姿误差dT和结构参数误差de，其迭代求解过程如图7所示。迭代5次后迭代结果不再变化。表6给出了不同测量噪音下的辨识结果，其中P为位移，θ为姿态误差，S为结构参数误差。由表6可得，3种测量噪音下基于本文提出方法标定的结构参数精度相对于以前方法均有大幅提高，分别由1.007 5 mm提高到0.336 7 mm，0.100 9 mm提高到0.033 7 mm，0.010 1 mm提高到0.003 4 mm；三种测量噪音下基于本文提出方法标定的位置精度相对于以前方法基本不变，但姿态精度有大幅提高，分别由0.015 2°提高到0.003 3°，0.003 3°提高到0.000 3°，0.000 15°提高到0.000 033°，证明了本文方法的有效性。

表6 不同测量噪音下的辨识结果Tab.6 Iterative results under different measurement sensor noise

为了进一步验证本文所提方法的有效性，表7、表8分别给出基于Jz和Jzg的测量噪音2下的结构参数误差标定结果，并对运动学模型进行补偿。随机给定60组位姿，计算出位姿误差，如图8所示。可以看出位置精度基本不变，姿态精度大幅提高。

表7 基于Jz的测量噪音2下结构参数误差标定结果Tab.7 Identified structural parameter error base on Jz

表8 基于Jzg的测量噪音2下结构参数误差标定结果Tab.8 Identified structural parameter error base on Jzg

图8 60组随机位姿下的位姿误差

表9～表12给出了基于可观测性指标O2～O5选取测量位姿的标定结果。由表9～表12可得，不同可观测性指标下，基于本文提出方法标定的位置精度相对于以前方法基本不变或有所下降，但姿态精度和结构参数精度均有大幅提高，证明了本文方法的有效性。

表9 基于可观测性指标O2的辨识结果Tab.9 Iterative results under observability index O2

表10 基于可观测性指标O3的辨识结果Tab.10 Iterative results under observability index O3

表11 基于可观测性指标O4的辨识结果Tab.11 Iterative results under observability index O4

表12 基于可观测性指标O5的辨识结果Tab.12 Iterative results under observability index O5

7 标定实验

标定实验与标定仿真的流程相同，主要包括选择测量位姿、牛顿迭代求解辨识参数、误差补偿和检验标定结果。其中，对于测量位姿的选择，Stewart并联机构的理论结构参数与仿真设定的结构参数相同，因此选用表3基于Jzg选取的测量位姿作为标定实验的测量位姿。

对于测量位姿，本实验采用徕卡AT960激光跟踪仪(定位精度为5 μm+6 μm/m)对Stewart稳定平台进行位姿测量，首先测量出上平台坐标系{p}和下平台坐标系{e}位于激光跟踪仪坐标系{g}中的位姿，然后再通过坐标变换获得上平台坐标系{p}相对于下平台坐标系{e}的位姿，如图9所示。

图9 基于激光跟踪仪的标定实验Fig.9 Calibration experiment using a laser tracker

激光跟踪仪只能测量空间某一点位于激光跟踪仪坐标系中的位置，需要上、下平台上多个测量点才能计算出上平台坐标系{p}和下平台坐标系{e}位于激光跟踪仪坐标系{g}中的位姿。稳定平台的上平台坐标系{p}、下平台坐标系{e}和测量点分布，如图10所示。

图10 上下平台的测量点分布Fig.10 Measurement point distribution of the platform

(18)

(19)

式中：Atan 2(x，y)是双变量反正切函数，γ为绕x轴的转角，β为绕y轴的转角，α为绕z轴的转角。

根据测量得到的位姿与理论位姿的差值进行牛顿迭代求解，辨识出机构参数的误差值，进行运动学模型补偿。随机给出20组位姿，进行标定结果的检验，图11给出了标定前后的位姿误差。位置和姿态误差的均值分别从2.321 mm降至0.242 mm，0.246°降至0.025°。标定后的位姿精度有了明显提高，但距离仿真标定精度还有差距，主要是本文没有考虑关节间隙、机构刚度、位姿控制精度等对位姿误差的影响。

图11 位姿误差标定前后对比Fig.11 Pose error contrast before and after calibration

8 结 论

本文提出对辨识雅克比矩阵进行无量纲化，再根据基于无量纲化辨识雅克比矩阵的可观测性指标选择测量位姿，保证位移和姿态对测量噪音具有相同的鲁棒性。通过数值算例验证了，3种测量噪音下该方法标定的结构参数精度相对于传统方法均有大幅提高；该方法标定的位置精度相对于传统方法基本不变，但姿态精度有大幅提高，证明了该方法的有效性。

在以该方法选取的测量位姿对Stewart并联机构的标定试验中发现，位姿精度有了明显提高，但距离仿真标定精度还有差距，主要是本文没有考虑关节间隙、机构刚度、位姿控制精度等对位姿误差的影响，后续将对其进行研究，以提高标定试验精度。