基于作业轨迹约束的机械臂多闭环标定方法

蒋周翔，秦鹏举，苏瑞，龙忠杰，宋宝，唐小琦

(1.北京信息科技大学机电工程学院，北京 100192；2.华中科技大学机械科学与工程学院，湖北武汉 430074)

0 前言

机械臂末端绝对定位精度是评价机械臂技术水平的重要指标，是能否准确完成抓取、装配、加工等作业任务的主要决定因素之一[1]。补偿连杆 (Denavit-Hartenberg，DH)参数误差是提高机械臂绝对定位精度的有效方式，关键问题之一是如何选取合适的测量构型进而建立良态辨识模型[2]，如此才能保证DH参数误差的准确求解。

目前常采用的测量构型主要分为开环和闭环2类[3]。其中，开环是指机械臂末端不受物理约束，允许在整个运动空间中选择测量构型。闭环是指机械臂末端受到物理约束，要求在末端名义位置基本不变的前提下选择测量构型。通常来讲，开、闭环构型方案的选择与测量装备的选择密切相关。例如，若采用Faro或Leica激光跟踪仪[4-5]或立体视觉[6-7]作为测量手段，则往往选用开环方案，原因是激光跟踪仪和双目相机具备较大的测量范围，并没有严格限制机械臂末端运动。如若采用双球杆仪[8]、触发式测头[9]或激光位移传感器[10]，则通常采用闭环方案，原因是这些测量设备的量程很小，要求机械臂末端只能在极小空间内运动。选定开环和闭环方案后，下一步需要确定具体的测量构型。目前一部分学者采用随机方式生成测量构型，另一部分则以辨识模型观测指数或条件数为优化目标，选用DETMAX等优化算法来选择测量构型[11-13]。还有学者在此基础上额外考虑了传感器的可视性对构型选择的约束[14]。研究表明：优化后的构型比随机构型能达到更高的DH误差辨识精度。

上述方法在面向实际工程应用时，在以下几个方面还需研究与改进：

(1)传统的开环方案在选择测量构型时具有更好的灵活性，但对运动空间的完整性要求更高，这通常与实际应用中的非结构工作环境相矛盾，降低了该类方案的实用性。

(2)传统的闭环方案虽然仅需较小的运动空间，但末端物理约束过于严格，导致测量设备安装调试难度较大，制约了此类方案的推广和应用。

(3)现有的开环、闭环方案得到的测量构型难以与机械臂实际作业轨迹保持一致，迫使机械臂停工来配合末端位姿采集，因此降低了生产效率，增加了维护成本。

面对上述现状，本文作者采用新方法，以双目相机作为测量设备，以机械臂实际作业轨迹为主要约束条件、以辨识矩阵形态水平为优化目标规划测量构型，并在此基础上生成新作业轨迹。当机械臂精度衰减至需要标定时，执行新作业轨迹并实时测量末端位姿，即可建立辨识模型解出DH参数误差用于关节指令补偿。新方法将视觉采集、作业轨迹与闭环方案相融合，既突破了闭环测量的强物理约束，又利用了闭环测量时末端点位相对集中的特点，由此提出的基于作业轨迹约束的多闭环测量方案使机械臂实时标定成为可能。

1 机械臂多闭环标定系统的设计

标定系统框架与流程如图1所示。

图1 标定系统框架与流程

图1中系统运作流程为：

(1)在工作轨迹上选取N个测量点；

(2)以各测量点位姿和传感器有效采集为约束条件，以辨识模型性态水平为优化目标，选择Nm1个测量构型；

(3)围绕测量构型重新规划若干条平滑的标定-作业共用轨迹；

(4)机械臂先按照原始作业轨迹进行作业，传感器实时监控末端位姿；

(5)当位姿误差超过阈值时，机械臂开始以新轨迹进行作业，即以选取的测量构型经过对应的测量点，同时传感器采集机械臂末端位姿，输入至上位机；

(6)在经过各测点时，机械臂控制系统同步发送关节指令至上位机；

(7)由上位机求出所有测量构型对应的末端位姿误差和辨识矩阵，并建立辨识模型，解算出DH参数误差；

(8)误差值输入至控制系统进行补偿进而提高机械臂末端位姿精度。

2 机械臂坐标系的建立

机械臂坐标系分布如图2所示。

图2 机械臂关节坐标分布

图2中：{Xi，Yi，Zi}为第i关节坐标系的3个坐标轴，i=1，2，…，6；{X0，Y0，Z0}为基坐标系；{Xt，Yt，Zt}为靶标坐标系。机械臂末端名义位姿是通过正运动学计算得到的，具体是采用式(1)所示齐次变换矩阵相乘：

0Tt=0T11T22T33T44T55T66Tt

(1)

式中：0Tt为机械臂基座标系到末端靶标坐标系的齐次变换矩阵；i-1Ti为第i-1关节坐标系到i关节坐标系的齐次变换矩阵。i≠2时，i-1Ti为

i-1Ti=Rz(θi)Tz(di)Tx(ai)Rx(αi)

(2)

而i=2时，i-1Ti为

1T2=Rz(θ2)Tx(a2)Rx(α2)Ry(β2)

(3)

6Tt为第6关节坐标系到末端传感器坐标系的齐次变换矩阵。6Tt表示为

6Tt=Tx(xt)Ty(yt)Tz(zt)Rx(αt)Ry(βt)Rz(γt)

(4)

式中：Rx(*)、Ry(*)、Rz(*)为旋转矩阵；Tx(*)、Ty(*)、Tz(*)为平移矩阵；{θi，di，ai，αi}为第i关节的标准DH参数；{xt，yt，zt，αt，βt，γt}为传感器安装参数；β2为针对相邻平行关节而增加的偏转角，目的是在机械臂标定中保持DH参数的数值连续性[15]。

3 机械臂DH误差辨识模型的建立

令{δθi，δdi，δai，δαi，δβi}为DH参数误差，ΔσDH，i表示i关节DH参数误差的向量，则有：

将所有ΔσDH，i按关节顺序合并得到：

ΔσDH=[ΔσDH，1，ΔσDH，2，…，ΔσDH，6]T

假定机械臂建立辨识模型共需M个测量构型，令{δxj，δyj，δzj，δαj，δβj，δγj}为第j个测量构型对应的传感器坐标系位姿偏差分量，则位姿偏差ΔEj为

ΔEj=[δxj，δyj，δzj，δαj，δβj，δγj]j=1，2，…，M

其中，各位姿偏差分量计算式如下：

(5)

式(5)中的元素来源于传感器名义坐标系到实际坐标系的齐次变换矩阵：

(6)

式中：0Ttnj、0Ttaj分别为第j个测量构型对应的传感器名义坐标系矩阵和实际坐标系矩阵。

将M个测量构型对应的ΔEj合并得到：

ΔE=[ΔE1，ΔE2，…，ΔEM]T

(7)

再结合微分变换原理和机械臂正运动学，可建立ΔσDH到ΔE的线性映射关系：

ΔE=JΔσDH

(8)

若用J+表征雅克比矩阵J的广义逆，则由上式可建立机械臂DH参数误差的辨识模型：

ΔσDH=J+ΔE

(9)

4 测量构型的确定

4.1 建立测量构型库

测量构型库的建立分为2个步骤：首先是在原作业轨迹上选择若干测量点，并以各测量点坐标为约束求运动学逆解，以此得到若干测量构型；然后在每个测量构型下检验靶标是否能被相机有效采集，将不满足要求的构型剔除，得到初始的测量构型库。下面分别对2个步骤进行阐述：

(1)以测量点作为约束

测量点选取的原则首先是该点位处光照稳定，这是视觉系统准确采集的前提。此外，还应尽量使测量点间隔均匀，以保证点位具有一般性。

令选择的测量点在基坐标系下的位置为[pxk，pyk，pzk]T，其中k=1～N。在±π范围内均匀取m个值，由3组m个角度值交叉组合生成m3个不同的角度组合[α6，β6，γ6]T，与[pxk，pyk，pzk]T组合成Nm3个靶标位姿向量[px，py，pz，α6，β6，γ6]T，然后由齐次变换运算得到每个位姿向量对应的靶标坐标系矩阵0Tt。最后通过机械臂逆运动学求出所有0Tt对应的关节指令集合{θ1，θ2，θ3，θ4，θ5，θ6}q1，其中q1=1～Nm3。

(2)以靶标的可视性作为约束

令靶面法向量为靶标坐标系Zt轴向量，计算每个0Tt的Zt轴与相机光轴(Zc轴)间夹角θq1。由于θq1∈[-π/2，π/2]对应的靶标姿态无法被相机识别，因此与之对应的测量构型不符合标定要求，将它剔除后得到测量构型库{θ1，θ2，θ3，θ4，θ5，θ6}q2。

4.2 测量构型优化

采用DETMAX方法，在测量构型库{θ1，θ2，θ3，θ4，θ5，θ6}q2中进行筛选，最终得到能够用于标定的测量构型{θ1，θ2，θ3，θ4，θ5，θ6}q3。

DETMAX方法的基本步骤是：

(1)在构型库中初选q个构型组建对应的辨识矩阵Jq，其维度为6q×24；

(2)计算其观测指数O1(q)；

(3)在剩余q2-q个构型中任选一个构型建立J1，与Jq组成Jq+1，其维度为6(q+1)×24，并计算其观测指数O1(q+1)；

(4)若O1(q+1)>O1(q)， 则保留添加的构型，否则弃置，该构型不再回到测量构型库中；

(5)从q+1构型中减去除新添加构型之外的其他任一构型，建立辨识矩阵Jq′并计算其观测指数O1(q′)；

(6)若O1(q′)>O1(q+1)则删除该构型，否则保留，该构型不再回到剩余构型库中；

(7)对所有剩余的q2-q个构型均依次执行步骤(3)—(6)，最终得到测量构型{θ1，θ2，θ3，θ4，θ5，θ6}q3及其对应的辨识矩阵Jq3。

4.3 由测量构型生成新作业轨迹

开环方案的测量构型彼此差异明显，然而闭环方案的机械臂末端运动受到约束，因此部分构型较为近似，可通过K-means聚类分析法得到若干构型簇{Θ1，Θ2，…，ΘK}。每个簇内的测量构型相对接近，与机械臂指定任务点(完成抓取、装配等任务的点位)的构型结合，输入至机械臂控制系统，即可规划K条新作业轨迹。

机械臂开始作业后，预先架设好的双目相机持续采集末端靶标位姿。当发现位姿精度衰减超过设定阈值时，机械臂不再执行初始作业轨迹，而是依次沿K条新作业轨迹完成工作任务。机械臂途径新作业轨迹的N个测量点时，双目相机实时采集靶标位姿，与对应的靶标名义位姿以及测量构型{θ1，θ2，θ3，θ4，θ5，θ6}q3结合，即可建立式(9)所示辨识模型。

5 仿真验证

5.1 确定测量点

首先为机械臂DH参数设定如表1所示的名义值用于建立其运动学模型，与之对应的机械臂关节坐标系布局与图2一致。第6关节坐标系到靶标坐标系的平移和旋转如表2所示，由此可根据式(4)得到变换矩阵6Tt。

表1 机械臂DH参数赋值

表2 第6关节到靶标的坐标系变换矩阵赋值

至此可指定测量点。为满足测量点间隔均匀和一般性要求，在各关节旋转范围内均匀取4个转角值，得到各关节指令如表3所示。根据式(1)得到与测量点对应的机械臂名义位姿如图3所示。

表3 所选测量点对应的关节指令 单位：rad

图3 机械臂姿态

5.2 建立测量构型库

令第4.1节中的m=10，在每个测量点处得到1 000个靶标位姿向量及其对应的变换矩阵0Tt。再通过机械臂逆运动学求得所有0Tt对应的共4 674个有效可达的关节指令集合{θ1，θ2，θ3，θ4，θ5，θ6}q1。然后以靶标可有效采集作为约束条件，进一步从中筛选出1 930个可行的关节指令集合{θ1，θ2，θ3，θ4，θ5，θ6}q2，并用DETMAX算法从中挑选出24个测量构型{θ1，θ2，θ3，θ4，θ5，θ6}q3。

最后采用K-means算法将24个测量构型进行聚类，使同类别构型的关节指令差别相对较小，以便于机械臂控制系统生成平滑的新作业轨迹。令K=6，构型聚类结果如图4—9所示。

图4 第1类测量构型

图5 第2类测量构型

图6 第3类测量构型

图7 第4类测量构型

图9 第6类测量构型

5.3 建立辨识模型

至此可由式(9)得到J。为进一步提高模型性态水平，采用矩阵平衡法优化矩阵J。首先按下式找出矩阵J各列的最大1-范数C：

(10)

式中：J(：，c)为J矩阵的第c列。然后用下式得到列平衡矩阵B的元素bc，c：

(11)

即可得到列平衡对角阵B：

B=diag(b1，1，b2，2，…，b24，24)

(12)

类似的，找出JB矩阵各行最大1-范数R：

(13)

式中：JB(r，：)为JB矩阵的第r行。然后用下式得到行平衡矩阵A的元素ar，r：

(14)

即可得到行平衡对角阵A：

A=diag(a1，1，a2，2，…，a6M，6M)

(15)

将式(12)(14)(9)相结合即可得到经过矩阵平衡处理后的辨识模型：

ΔσDH=B(AJB)+AΔE

(16)

式(9)和式(16)中的J和AJB2个辨识矩阵的观测指数和条件数如表4所示。

表4 矩阵平衡前后的辨识矩阵性态指标

可见经过矩阵平衡后，辨识矩阵性态水平得到显著提升，表明新辨识模型下的靶标位姿误差能够更显著反映出DH参数误差的影响，且辨识精度对测量噪声的敏感度明显降低。

5.4 DH参数误差辨识精度验证

令DH参数误差真实值ΔσDH，i如表5所示。

表5 机械臂DH参数误差赋值

再根据下式计算出靶标在测量构型处的实际坐标系：

0Ttaj=0T1a1T2a2T3a3T4a4T5a5T6a6Tt

(17)

式中：0Ttaj为在第j个测量构型处的机械臂基座标系到机械臂末端靶标坐标系的实际齐次变换矩阵；i-1Tia表示第i-1关节坐标系到i关节坐标系的实际齐次变换矩阵。其中i≠2时，i-1Tia为

i-1Tia=Rz(θi+δθi)Tz(di+δdi)·Tx(ai+δai)Rx(αi+δαi)

(18)

i=2时，i-1Tia为

1T2a=Rz(θ2+δθ2)Tx(a2+δa2)·Rx(α2+δα2)Ry(β2+δβ2)

(19)

然后由式(1)计算出各测量构型对应的靶标名义变换矩阵0Tt，将它和式(17)所得0Ttaj一并代入式(6)即可得到Δ0Ttj，进而可由式(5)和式(7)得到所有测量构型对应的靶标位姿偏差向量ΔE。

由于新方法拟采用双目视觉系统，因此根据主流视觉测量设备性能参数，假设采用高性能双目相机时测量噪声分布规律如表6所示。

表6 采用高性能双目相机时测量噪声分布规律

而假设采用普通双目相机时，测量噪声分布规律如表7所示。

表7 采用普通双目相机时测量噪声分布规律

(20)

采用表6所示测量噪声时的DH误差辨识结果和辨识精度如表8所示。

表8 低测量噪声下的DH误差辨识精度

采用表7所示测量噪声时的DH误差辨识结果和辨识精度如表9所示。

表9 高测量噪声下的DH误差辨识精度

结果显示，低测量噪声下DH误差辨识精度在0%～5.1%，其中，角度误差辨识精度在0%～0.6%，明显优于距离误差辨识精度0.1%～5.1%。高测量噪声下DH误差辨识精度整体上有所降低，为0%～12.2%，但其中角度误差辨识精度几乎没有变化，为0%～0.7%。与之相比，大多数距离误差辨识精度无明显变化，仅δa2和δa3辨识精度降低至11.5%和12.2%，但仍在可接受范围内。由此可知{δθi，δαi，δβi}相比{δdi，δai}而言，其辨识精度更不易受测量噪声的影响，这是因为优化后的测量构型能够使{δθi，δαi，δβi}对末端靶标位姿的影响最大化，而在{δdi，δai}上并不产生类似的倍增效果。此外，距离测量噪声标准差由0.1/3 mm提高至1/3 mm时，DH误差平均辨识偏差仅由1.1%增大至1.4%，表明新方法能在不同量级噪声下维持较高的辨识精度。

6 结语

此研究首先简单阐述了由微分变换和正运动学理论建立DH误差辨识模型的原理，再以作业轨迹和靶标可视性作为约束条件生成测量构型库，并采用DETMAX算法从中挑选出合适的构型用于建立辨识模型，然后提出了一种矩阵平衡方法进一步改善了模型性态水平。为满足实时标定需求，还采用K-means聚类算法将所选测量构型划分为若干新作业轨迹。最后通过仿真对新方法的DH误差辨识精度进行了验证。

研究结果表明：

(1)由于需要同时满足作业轨迹和靶标可视性的约束，因此由DETMAX算法所得测量构型建立的辨识模型仍然相对病态，而采用矩阵平衡法能显著改善其性态水平。

(2)当测量噪声增强时，角度DH误差的辨识偏差依旧维持在0.7%以下，并未产生明显波动，因此与距离DH误差相比具有更好的抗噪声干扰能力。

(3)新方法在2种典型测量噪声强度下均可满足机械臂DH参数标定精度要求，因此即使采用普通性能的测量装备，该方法也具有良好应用潜力。

下一步研究计划是搭建试验平台验证新方法在现实场景下可行性和标定精度，以此开展实际工况下的优化与完善。其中将重点关注新作业轨迹运行的流畅性以及与四周障碍物间的干涉问题。