基于导纳的机械臂实时柔顺轨迹规划

王庆辉,江 超,2

(1.沈阳化工大学信息工程学院,沈阳 110142;2.中国科学院沈阳自动化研究所,沈阳 110016)

0 引言

机械臂轨迹规划问题一直是机械臂运动控制研究的重点内容,轨迹规划的效果对机械臂工作的稳定性、安全性和能量消耗等有重大影响,因此国内外学者针对机械臂的使用场合提出了多种轨迹规划方法。林威等[1]提出了利用抛物线速度平滑的笛卡尔空间直线和圆弧轨迹插补算法。为了进一步保证轨迹连续性,减少运动冲击,潘海鸿等[2]提出对机器人笛卡尔空间轨迹进行等距粗插补以及加减速预处理,再使用Hermite插值法对各关节运动进行轨迹规划。SU、RIAZI等[3-5]在基本插值曲线的基础上,使用优化算法求解时间、能耗和冲击等优化目标的最小值,以此优化机械臂的运行轨迹。

上述研究只是根据任务需求计算得到机械臂时间、连续性等方面最优的离线式轨迹,而未考虑机械臂实际作业时控制、测量误差和环境不确定因素的存在,机械臂末端可能会与环境发生碰撞造成损坏。离线式轨迹规划期望的是高精度位置控制,体现的是刚性需求。而在机械臂与外界环境发生接触时又期望机械臂表现一定的顺应性,使得接触力不至于过大,体现为柔性需求,为了解决这一矛盾,大量国内外学者开展了柔顺控制研究。

目前柔顺控制分为被动柔顺与主动柔顺两种。被动柔顺是通过设计特殊的柔性机械装置(remote center compliance,RCC)来实现机器人末端与环境柔顺接触[6],具有不改变原有轨迹规划的同时实现柔顺控制的优点, 但被动柔顺装置存在专用型强,适用范围小等问题。主动柔顺是通过设计合理的控制策略来处理好力与位置控制二者的关系以达到一定的柔顺效果。REIBERT等[7]提出了力/位混合控制方法,该方法基于柔顺系数矩阵将机器人任务分解为位置控制和力控制两个正交子空间,直接控制接触力跟随期望值变化,但控制器的结构依赖于机器人与环境的动力学特征和运动学结构,有较大的应用限制[8]。HOGAN、KAZEROONI等[9-10]提出阻抗控制和导纳控制方法,基于虚拟惯量阻尼刚度系统建立了末端位置与接触力之间的动力学关系式。但文中未有研究轨迹规划与柔顺控制内在联系,限制了柔顺控制在实际工程的应用。金玉阳等[11]提出了通过分析接触力误差来动态调整控制模型参数的阻抗控制方法,引入自适应控制策略,提高了系统稳定性。但该方法仍处于仿真阶段,尚不能成熟地运用到实际作业。

针对以上问题,本文在轨迹规划的基础上,构建导纳控制器以感知接触力并计算得到位姿偏差,以此来实时调整轨迹规划的预期轨迹,将二者纳入统一位置控制体系,从而达到实时柔顺的轨迹规划效果。

1 实时柔顺轨迹规划原理

1.1 轨迹规划

轨迹规划可以拆解为几何路径规划与速度规划:速度规划是由一个时间标度参数s(t)表示,其为t∈[0,T]区间内每个时刻对应的值s,既s:[0,T]→[0,1]。另外为使得机器人运动流畅连续,时间标度s(t)需要适当平滑,即二阶导连续,常见的方法有三次多项式法、五次多项式、梯形速度曲线、S型速度曲线[12];几何路径规划由路径参数θ(s)表示,其是将标量参数s(s在路径开始处为0,在路径结束处为1)映射到机器人位形空间Θ中的一点:θ:[0,1]→Θ。随着s从0变化到1,机器人沿着路径移动[13]。几何路径轨迹主要研究笛卡尔空间位姿规划,其包括直线插补运动和圆弧插补运动的位置规划与姿态规划,下面将分别讨论。

1.1.1 位置规划

(1)直线运动。假设直线运动的起点和终点分别为P1(x1,y1,z1)、P2(x2,y2,z2),则各插补点的表达式为:

(1)

式中:s(t)为两阶可微的时间标度参数曲线s:[0,T]→[0,1],以保证直线运动过程连续且平滑。

(2)圆弧运动。由笛卡尔空间任意不共线的3点P1(x1,y1,z1)、P2(x2,y2,z2)、P3(x3,y3,z3)可确定一个圆弧轨迹,求解过程如下:

建立空间圆弧所在平面的新坐标系P1-UVW,从而将三维空间问题变化为二维空间问题,其中以P1为坐标原点,P1P2为U轴,圆弧所在平面的法向量为W轴,再通过U轴与W轴叉乘得到的向量为V轴,如图1所示,具体计算如下:

图1 P1-UVW坐标系示意图图2 P1-UVW坐标系下的圆心位置

(2)

其中两向量叉乘计算如下:

假设P1、P2、P3在新坐标系P1-UVW中表示为A、B、C点,则可以通过向量点积分别求得A、B、C点坐标:

(3)

根据P1P2是圆弧上的弦可知,圆弧圆心一定位于x=bx/2上,如图2所示,因此设圆弧圆心坐标为(bx/2,h,0)。根据平面圆的标准方程可得:

(4)

计算出P1-UVW坐标系下表示的P0圆心坐标后,即可算出点P0圆心在空间基座标系下的位置表示:

P0=[x1,y1,z1]T+(bx/2)*u+h*v

(5)

同时可计算出圆弧半径R:

(6)

以圆心P0为坐标系原点,以P0P1为U轴,圆弧所在平面的法向量为W轴,再通过U轴与W轴叉乘得到的向量为V轴建立新坐标系P0-UVW,如图3所示,则方向余弦计算如下:

图3 P0-UVW坐标系示意图

(7)

令p0=[x0,y0,z0]T表示为空间基座标系{B}下的P0位置,则空间基座标系{B}到局部坐标系{P0}的齐次变换矩阵T为:

(8)

设空间任意一点P在空间基座标下的齐次坐标为[x,y,z,1]T、在局部坐标系P0-UVW下的齐次坐标为[x′,y′,z′,1]T,则存在以下关系:

(9)

(10)

则圆弧运动各插补点表达式为:

(11)

式中:s(t)为时间标度参数,对其进行从0～1的速度规划即可完成圆弧插补。

1.1.2 姿态规划

单位四元数具有计算量小,无万向锁问题等优点,因此广泛用于姿态插值。设四元数的表达式为:

q=[q1,q2,q3,q4]

(12)

旋转矩阵表达式为:

(13)

则存在以下关系式:

(14)

假设轨迹起始姿态与终止姿态分别为旋转矩阵Rs和Re,根据式(14)可求出其对应的单位四元数qs和qe。定义两个单位四元数矢量的夹角为θ,则:

θ=cos(qs*qe)-1

(15)

对于起始姿态到终止姿态的插值可由球面线性插值公式Slerp[14]求出:

(16)

式中:s(t)为时间标度参数,对其进行从0～1的速度规划即可完成姿态插补。

1.2 导纳控制

导纳控制本质是内环位置控制和外环力控制的一种控制策略。根据当前力传感器检测的环境接触力信息以及设定的导纳模型参数,计算位姿附加量,此位姿附加量再去修正预先设定的轨迹位姿,最终将调整后的目标位姿进行逆运动学解算得到机械臂各关节值,并将此值送入位置控制内环,完成最终的位置控制,以实现机器人末端位置对外力的主动柔顺。其中导纳控制模型可等效为一个弹簧阻尼刚度系统,该系统的动力学方程可以表示为:

(17)

由于导纳控制时需要实时根据环境接触力调整预期轨迹,因此可将(17)式写为如下形式:

(18)

进一步通过积分可以得到期望轨迹位姿的附加量Xe:

(19)

(20)

另外可以通过调节式(18)中M、B、K参数来改变导纳系统特性,使之得到最优的控制性能。因此不妨将式(18)进行拉氏变换以获得导纳控制系统的传递函数:

(21)

由式(21)可以看出导纳控制器等价于一个二阶系统,因此可以把式(21)进一步变化为典型二阶系统表达式:

(22)

1.3 实时柔顺轨迹规划

由1.1节轨迹规划部分可以得到以足够光滑的速度曲线从起始位姿到目标位姿的预期轨迹,由1.2节导纳控制部分可以得到由机械臂末端与环境接触产生的接触力/力矩计算得到的位姿修正量,因此系统由位置控制内环和导纳控制力外环两部分组成,将力信号转换为位置信号融入统一位置控制中,从而实现实时柔顺轨迹规划效果,总控制框图如图4所示。

图4 实时柔顺控制框图

具体来说,通过式(18)～式(20),以及根据机械臂末端与环境的接触力/力矩Fe和导纳模型参数(M、B和K)系统,导纳控制外环可产生位姿修正量Xe,与轨迹规划计算的期望机械臂末端位姿Xd比较得到参考末端位姿:

Xr=Xd+Xe

(23)

其中当机械臂末端未与环境发生接触时,末端作用力Fe=0,对应计算的末端位姿修正量Xe=0,根据式(23)得Xr=Xd,此时机械臂的控制状态等效于常规的轨迹规划位置控制;当机械臂末端与环境发生接触时,末端作用力Fe≠0,同时对应的末端位姿修正量Xe≠0,Xr=Xd+Xe,此时机械臂处于导纳控制状态。

2 实验设计与结果

本实验采用末端自带六维力传感器的UR5e六自由度协作机械臂作为实验主体;控制器采用搭载Linux实时操作系统与本文对应算法程序的工控机;控制器接收机械臂上传的各关节位置与力传感器信息,计算出各关节下一伺服周期的位置,并将此关节值发送回机械臂伺服,以达到实时柔顺轨迹规划效果,其中二者信息交互周期为2 ms。整体实验结构如图5所示。选取机械臂各关节角为(0°,-133°,-110°,-27°,90°,0°)作为实验起始状态,其对应的上位机软件坐标系可视化示意图如图6所示,并设定导纳模型各自由度参数分别为M=30,B=173,K=250。

图5 实验设备 图6 机械臂坐标系示意图

2.1 实时柔顺的直线运动实验

末端执行器的运动轨迹为直线,其中直线的起始点和终止点在起始状态的末端工具坐标系下位置分别为[0,0,0]T、[0,-0.3,0]T,姿态保持不变。同时为了验证本文控制策略有效性,在直线运动过程中,人为地在轨迹前进方向的左侧方放置球形障碍物,使之与机械臂末端发生接触碰撞,通过检测接触力大小与轨迹运行状态,评估控制方法表现。

图7和图8为直线运动过程中所受的接触力与力矩曲线。图9和图10为机械臂末端位姿曲线。从图7机械臂末端在x轴与y轴方向的接触力曲线中可以清晰地看出,机械臂在1.9 s时与障碍物开始发生碰撞接触,但由于导纳控制器的作用,机械臂末端轨迹顺应着接触力移动,其中碰撞过程中除x轴与y轴接触力的其他自由度受到的力较小,位姿修正量也很小,x轴与y轴方向的最大接触力绝对值分别为6.4 N和8.84 N,在允许范围内,由此可见在本文所提出的控制方法作用下避免了由轨迹误差过大而造成的关节输出力矩激增问题。8.29 s后机械臂开始与障碍物脱离,接触力与力矩缓慢减小直至为0,机械臂末端重新回到期望直线轨迹上。

图7 直线过程所受接触力 图8 直线过程所受接触力矩

图9 直线过程中的位置 图10 直线过程中的姿态

图11为机械臂直线运动开始时状态和结束时状态。

图11 直线运动开始与结束状态

2.2 实时柔顺的圆弧运动实验

末端执行器的运动轨迹为圆弧,其中圆弧的起始点、中间点和终止点在起始状态的末端工具坐标系下位置分别为[0,0,0]T、[-0.1,-0.1,0]T、[0,-0.2,0]T,姿态以欧拉角形式表示为[0,0,0]T、[0,0,0]T、[0,0,90°]T。与直线实验类似,在圆弧轨迹中间位置处放置球形障碍物,使之与机械臂末端发生碰撞接触,然后检测接触力大小与轨迹运行状态,以评判控制算法有效性。

实验结果如图12～图15所示。由图12可以看出,在2.3 s时刻机械臂末端与障碍物发生接触,此时导纳控制器根据接触力信号来修正机械臂末端轨迹以表现对环境接触力的顺应性,如图14所示。碰撞过程中,接触力最大绝对值为7.4 N,在允许范围内。之后在6.1 s时刻,机械臂末端开始与障碍物脱离,接触力与力矩经振荡后趋于平稳并逐渐减小,同时机械臂末端轨迹开始逐步回到期望圆弧轨迹上。

图12 圆弧过程所受接触力 图13 圆弧过程所受接触力矩

图14 圆弧过程的位置 图15 圆弧过程中的姿态

图16为机械臂圆弧运动开始状态与结束状态。

图16 圆弧运动开始与结束状态

3 结论

本文针对机械臂进行了基于导纳的实时柔顺轨迹规划研究,可以得出以下结论:①将机械臂笛卡尔空间轨迹规划分为了位置规划与姿态轨迹,并通过时间标度参数s(t),s:[0,T]→[0,1]将位置规划结果与姿态规划结果归一化,这样可以通过设计时间标度参数s(t)为二阶导连续的速度曲线,就可以实现机械臂笛卡尔空间路劲规划和速度规划解耦的同时,笛卡尔空间轨迹的位置、速度、加速度连续;②以机械臂最常用的直线轨迹规划和圆弧规划为例阐述了机械臂笛卡尔空间位置规划与姿态轨迹的原理;③为了解决传统笛卡尔空间轨迹离线式规划可能导致的过大环境接触力问题,阐述了导纳控制方法的基本原理,并将此方法与轨迹规划相结合,以实现机械臂末端实时主动顺从外界接触力;④本文最后给出了实时柔顺的直线运动实验和圆弧运动实验,实验结果表明本文提出的研究方法可以保证机械臂实现轨迹运行高精度的同时也能避免因位置误差导致的破坏性接触力,验证了方法的正确性。

本文在导纳控制器参数选取方面仍存在一定不足,比如控制器参数目前仍是人为选取定值而不是自动计算得到,因此如何根据机器人作业对象类型与环境自适应选取更合适参数是下一步研究的重点。