绳驱动连续体机器人标定方法

0 引言

与传统工业机器人相比，连续体机器人具有柔顺性高、能够实现连续变形等特点，越来越受到国内外学者的关注。

学者针对不同的应用场景提出了多种连续体机器人构型。KANG等[1]提出了一种将肌腱嵌入气动肌肉的连续体机器人，并建立了机器人的三维动力学模型。ZHANG等[2]以形状记忆合金(SMA)作为驱动模块，搭建了具有4个关节模块的模块化机器人臂，根据SMA的特点建立了自反馈控制系统，实现了机器人的控制。OTAKE等[3]研究了基于电活性聚合物凝胶的软连续体机器人，提出了电活性聚合物系统的动力学模型，并通过施加空间或时变电场来实现各种运动。周圆圆等[4]提出了一种新构型的6自由度连续体手术机器人，其形变骨架采用超弹性材料一体化成形加工，形成具有一系列十字交叉镂空结构的弹性铰链。MANTI等[5]提出了一种绳驱动连续体机械臂，该机械臂将驱动绳索与弹性流体制动器相结合，能够实现弯曲、拉伸和收缩运动。与其他连续体机器人构型相比，采用绳索驱动的连续体机器人通过绳索来传力，其传动装置可以安装于基座，这有利于实现机器人运动部件的轻量化，且绳驱动机器人的带载荷能力较大，因此，该构型适用于复杂环境内运动及本质安全的人机交互，被广泛应用在狭窄空间检测[6]、轻量化机械臂[7]、外科手术[8]等领域。

由于连续体机器人容易产生柔性变形，故其控制精度较传统机器人更容易受加工、安装以及外载荷等因素影响。与传统机器人相比，绳驱动连续体机器人更需要通过标定来提高其精度。

国内外学者针对传统工业机器人的标定做了大量研究。YANG等[9]针对并联机器人提出了标定方法，该方法将旋量、关节角误差都归纳到初始位姿的误差中，误差模型简单。谷乐丰等[10] 提出了一种由安装于机器人末端球心位置的测量装置和可移动球杆组成的新型便携式机器人标定装置，能够在较大空间中进行标定。KURBANHUSEN等[11]针对刚性关节绳驱动机器人标定方法进行了研究。CHEN等[12]针对7自由度绳驱动刚性机械臂，提出了一种集成化的两级自校准方法。虽然学者提出了大量的标定方法，但一般需在机器人末端安装测量装置，这使得多数方法不能直接用于易产生柔性变形的连续体机器人，且目前针对连续体机器人的标定方法研究较少。

本文设计了一种基于柔性支撑构型的绳驱动连续体机器人关节模块，该关节模块以柔性液压软管材料作为支撑骨架，通过4根绳索牵引，可实现2-DOF的连续弯曲运动。为提高定位精度，本文利用指数积(POE)公式建立了基于柔性支撑构型的绳驱动连续体机器人关节模块的运动学模型，然后推导出了基于运动学的误差传递模型。

1 基于柔性支撑的绳驱动关节模块设计

基于柔性支撑的绳驱动连续体机器人关节模块模型见图1。关节模块由基座、动平台、柔性支撑骨架及4根驱动绳索连接构成。为简化设计，动平台与基座具有相同结构。柔性支撑骨架采用液压软管材料，具有较高的抗拉压和扭转刚度，能够大幅减少运动过程中产生的拉伸、压缩、扭转等复杂变形。驱动绳索为刚闸绳，绳的连接点均匀分布在动平台与基座上，通过驱动电机牵引绳索，可实现单关节模块的两自由度弯曲运动。

图1 绳驱动单关节模块模型
Fig.1 Model of cable-driven segment

2 绳驱动连续体机器人运动学分析

2.1 单关节模块运动学分析

绳驱动单关节模块运动学简化模型如图2所示，点OB、OM分别为基座、动平台的圆心，Bi、Mi(i=1,2,3,4)为4根绳索连接点。在基座、动平台上分别建立坐标系OBXBYBZB、OMXMYMZM，两坐标系分别以OB、OM为原点，以OB、OM指向第一根绳索连接点的方向为轴XB、XM，以垂直于基座、动平台的方向为轴ZB、ZM。由于4根绳索均匀分布在平台上，所以轴YB、YM分别指向平台第二根绳索连接点。

图2 单关节模块运动学模型
Fig.2 Kinematics model of segment

假定柔性支撑骨架的弯曲形变理想化，则OBOM为一条定长为l的圆弧。平面OBOMO由轴ZB、ZM以及圆弧OBOM组成，分别交基座、动平台于直线OBO、OMO，且OBO与OMO相交于O点，该平面始终垂直于基座。

定义关节变量旋转角α和弯曲角θ，通过两关节变量α、θ来描述单关节模块的弯曲运动。其中，α为轴XB与直线OBO的夹角，表示关节弯曲的方向；θ为∠OMOOB，表示关节弯曲的角度。

2.1.1 关节变量与动平台末端位姿运动学分析

根据螺旋理论，绳驱动单关节模块的弯曲运动可表示为沿瞬时旋转轴旋转θ的旋转运动。利用局部指数积公式，建立2-DOF绳驱动单关节模块的运动学模型：

(1)

ξ=(v,ω)∈R6×1

式中，T(α,θ)∈SE(3),为坐标系OMXMYMZM相对于OBXBYBZB的末端位姿；T(0)∈SE(3)，为坐标系OMXMYMZM相对于OBXBYBZB的初始位姿；为单关节模块的刚体运动；为坐标系OMXMYMZM相对于OBXBYBZB的运动螺旋；ξ为刚体运动的旋量坐标。

另外，SO(3)表示三维特殊正交群(special orthogonal group)，SE(3)表示三维特殊欧氏群(special Euclidean group)。若ω=[ω1 ω2 ω3]T，则表示ω的反对称矩阵：

根据正向运动学变换关系，T(α,θ)可表示为

(2)

式中，R(α,θ)∈SO(3),为坐标系OMXMYMZM相对于OBXBYBZB的方向；p(α,θ)为坐标系OMXMYMZM相对于OBXBYBZB的位置向量。

由式(1)可得

(3)

由文献[13]可知，也可表示为

(4)

由式(3)、式(4)可得

(5)

(6)

式中，L为柔性骨架的长度。

综上所述，若给定关节变量α、θ，则绳驱动关节模块的动平台相对于基座的位姿也确定。

2.1.2 关节变量与绳长的运动学分析

‖BiMi‖表示第i根绳索的长度，根据几何关系，有

BiMi=BiOB+OBOM+OMMi

(7)

则第i根绳索的表达式为

(8)

(9)

式中，βi为OBO与OBBi的夹角；r为绳连接点所在圆的半径。

由式(8)可得绳长与关节角的数学关系：

(10)

(11)

2.2 连续体机器人运动学分析

绳驱动连续体机器人是由多个相同的2-DOF绳驱动单关节模块连接构成。对于n个单关节模块组成的连续体机器人，其正向运动学模型为

T0,n=T0,1(α1,θ1)…Ti-1,i(αi,θi)…Tn-1,n(αn,θn)

(12)

式中，Ti-1,i(αi,θi)为第i个关节的运动学模型。

给定每个单关节模块的旋转角α、弯曲角θ，则绳驱动连续体机器人的末端动平台相对于基座系的位姿便可确定。

3 绳驱动连续体机器人标定算法

3.1 误差模型

对于由n个关节模块构成的绳驱动连续体机器人，由式(12)可知，其运动学模型是关于初始位姿T(0)、旋量ξ、关节角q的函数，即

T=f(T(0),ξ,q)

(13)

T(0)=[T0,1(0) T1,2(0) … Tn-1,n(0)]T

ξ=[ξ1 ξ2 … ξn]T

q=[q1 q2 … qn]T

对式(13)进行微分，有

(14)

由式(14)可知，末端位姿误差δT·T-1由δT(0)、δξ和δq三个参数决定。由于ξ是关节角q=[α θ]T的函数，这里暂时不考虑旋量带来的误差，故只需推导出最终与δT(0)和δq相关的末端误差模型。式(14)可简化为

(15)

对式(12)进行微分，可得

(16)

δqT-1=Ad(T0,1(0))J1δq1+
Ad(T0,1)Ad(T1,2(0))J2δq2+…+
Ad(T0,n-1)Ad(Tn-1,n(0))Jnδqn

(17)

δti=[dxi dyi dzi δxi δyi δzi]

Ji4=ω δqi=[δαi δθi]T

式中，Ad(T)表示求矩阵T的伴随变换矩阵，δti为第i个关节末端运动学误差；Ji为第i个关节的雅可比矩阵[14]；δqi为第i个关节的角度误差；ω′为ω的导数。

根据李群李代数相关理论[13]，可得

(18)

式中，为末端实际位姿；T0,n为末端理论位姿，计算符∨表示∧的逆运算。

将式(16)～式(18)代入式(15)，得

Ad(T0,1)Ad(T1,2(0))J2δq2+…+
Ad(T0,n-1)Ad(Tn-1,n(0))Jnδqn

(19)

将式(19)线性化表示为

y=Ax

(20)

其中，y为末端位姿误差，x为机器人的运动学误差，具体表达式为

[dx0,n dy0,n dz0,n δx0,n δy0,n δz0,n]
A=[Ad(T) Ad(Tq)]
x=[δt δq]T
Ad(T)=[Ad(T0,1) Ad(T0，2) … Ad(T0,n)]

Ad(Tq)=[Ad(T0,1(0))J1 Ad(T0,1)Ad(T1,2(0))J2

… Ad(T0,n-1)Ad(Tn-1,n(0))Jn]
δt=[δt1 δt2 … δtn]T
δq=[δq1 δq2 … δqn]T

若测量m组数据，则式(20)可扩展为

(21)

由式(21)得

(22)

式中，为矩阵的伪逆。

3.2 模型参数的最小二乘法求解

为得到高精度的运动学模型参数，采用最小二乘法对式(22)进行迭代计算，算法流程如图3所示。

图3 标定算法流程图
Fig.3 Flow chart of the calibration algorithm

根据理论与实际位姿误差Y求出运动学误差参数x，便可得新的运动学模型参数通过循环不断更新，直到误差偏差度量ΔT小于公差容限ε，循环结束。最终求得的参数为机器人真实的运动学参数。其中，ε为趋近于0的常数，具体表示为

4 仿真分析

4.1 仿真过程

本文利用MATLAB作为计算工具，对上述绳驱动连续体机器人进行仿真验证。

选择的仿真模型为绳驱动连续体机器人单关节模块，其主要模型参数为：柔性支撑长度L=120 mm，绳索连接点到平台圆心距离r=67 mm，初始位姿矩阵为

对运动学初始参数T0,1(0)、ξ、q分别预设误差Δt、Δξ、Δq，其中:

Δt=[0 0.1 mm 0.1 mm -0.1 mm 0 0]T

Δξ=[0.02 0 0 0 sin 0.02 cos 0.02-1]

Δq=[0.008° 0.008°]T

则实际模型参数表示为

ξa=ξ+Δξ qa=q+Δq

此时，绳驱动单关节模块的运动学模型为

(23)

4.2 仿真结果分析

仿真导入30组关节角，其中前20组用于迭代求解标定后的运动学参数，后10组用于验证标定后运动学参数的正确性。

定义平均位置误差Ep 和平均姿态误差Eo，具体表示为

图4所示为前20组数据的平均误差随迭代次数收敛情况。位置误差经6次迭代，从11.727 mm下降到0.0978 mm，姿态误差从0.0929 rad下降到0.0027 rad，且第二次迭代时位置误差就已下降到1.043 mm，姿态误差下降到0.0068 rad，说明算法具有较好的收敛性。

(a)位置误差迭代过程

(b)姿态误差迭代过程
图4 误差迭代过程的收敛性(仿真)
Fig.4 Convergence of error iterative process(simulation)

通过标定算法得到新的运动学模型参数：

利用后10组数据对标定后的运动学模型进行验证，结果如图5所示。10组数据的平均位置误差为0.109 mm，平均姿态误差为0.0029 rad，验证了标定算法的可行性。

(a)位置误差

(b)姿态误差
图5 标定后末端位姿误差
Fig.5 Pose error after calibration

4.3 算法对比

传统工业机器人标定方法一般仅考虑初始位姿误差对精度的影响，如文献[9]针对并联机器人提出的标定算法。本文对文献[9]的传统工业机器人标定算法进行仿真计算，并将其结果与本文所提算法进行比较，对比结果如图6所示。

(a)位置误差对比

(b)姿态误差对比
图6 仿真结果对比
Fig.6 Comparison of simulation results

由图6可得，传统算法经迭代后位置误差下降到0.2292 mm，姿态误差下降到0.0041 rad。根据仿真结果对比可得，本文所提算法能够获得更小的收敛误差，得到的运动学模型精度相对较高，这进一步验证所提算法的可行性。

5 实验验证

为验证标定算法的有效性，利用绳驱动单关节模块样机进行了实验。实验环境布置如图7所示，实验设备由运动捕捉系统(qualisys track manager，QTM)、上位机、绳驱动样机、驱动电机组成，其中运动捕捉系统测量关节末端位姿，上位机用于控制绳驱动单关节模块，驱动电机牵引绳索运动。如图8所示，样机采用4根绳索驱动，柔性支撑的长度L=120 mm，绳索连接点所在圆周的半径r=67 mm。

图7 实验装置
Fig.7 Experimental equipment

图8 绳驱动单关节样机
Fig.8 Prototype of cable-drive segment

5.1 实验步骤

实验中，在基座、动平台上放置标记小球(3个)，通过QTM测得标记小球的坐标。根据3个小球坐标分别计算基座、末端动平台在QTM坐标系下的位姿TQ,B、TQ,E，则可得到末端在基座坐标系下的实际位姿TB,E：

(24)

实验具体步骤如下：

(1)运用QTM测量基于初始运动学模型的120组数据，其中前80组通过标定算法计算新的理论模型参数，后40组计算位姿误差用于数据对比；

(2)将步骤(1)中计算出的标定后的模型参数重新导入上位机；

(3)测量基于标定后运动学模型的40组数据(与步骤(1)中40组关节角相同)，计算末端位姿误差；

(4)比较步骤(1)、步骤(3)中40组数据的末端位姿误差，验证算法有效性。

5.2 实验数据分析

实验步骤(1)中80组数据迭代的收敛性如图9所示，通过4次迭代初始位置误差从3.836 mm下降到1.153 mm，姿态误差从0.123 rad下降到0.022 rad，算法的收敛性较好。

(a)位置误差

(b)姿态误差
图9 误差迭代过程的收敛性(实验)
Fig.9 Convergence of error iterative process(experiment)

标定算法最终得到的新运动学参数为

将标定后的新运动学模型导入上位机，采集实验步骤(3)中40组数据，将该数据与实验步骤(1)中40组数据进行对比，结果如图10所示。标定前位置平均误差为3.955 mm，姿态平均误差为0.122 rad。标定后位置平均误差为2.68 mm，姿态平均误差为0.0224 rad。

(a)位置误差

(b)姿态误差
图10 标定前后误差对比
Fig.10 Positioning errors before and after calibration

5.3 实验结果分析

根据实验数据分析可得：绳驱动连续体单关节模块位置平均误差从标定前的3.955 mm下降到2.68 mm，位置精度提高了32.23%；平均姿态误差从0.122 rad下降到0.0224 rad，姿态精度提高了81.64%，证明了标定算法的有效性。

根据实验数据，绳驱动单关节样机在标定后位置、姿态仍存在一定误差。绳驱动连续体机器人为弱刚度系统，自重是导致机器人存在误差重要原因之一。另外，驱动绳索在运动过程中存在的摩擦力也会影响机器人控制精度。

6 结束语

提出了基于柔性支撑的绳驱动连续体机器人构型，根据指数积公式建立了机器人的运动学模型。针对机器人控制精度较差的问题，提出了误差标定算法，通过标定算法计算出准确的运动学模型。仿真结果显示，该算法能够快速收敛，并正确得到标定后的运动学参数，证明了该标定算法的可行性。最后，制作了绳驱动连续体机器人单关节样机，对误差标定算法进行了验证。实验结果显示，机器人末端位置精度提高了32.23%，姿态精度提高了81.64%，验证了标定算法的有效性。未来，将针对自重和摩擦等非几何因素导致的误差问题进行研究，进一步提高绳驱动连续体机器人的控制精度。