基于无源观测器的机器人笛卡尔空间力/阻抗控制方法

孔繁旭， 吕鹏, 王洪光, 常勇

(1.中国科学院沈阳自动化研究所， 沈阳 110016； 2.中国科学院机器人与智能制造创新研究院， 沈阳 110169)

现阶段大多数的工业机器人都应用于搬运、焊接和喷漆等任务，可以基于运动学的控制思想，采用控制位姿的方法实现。在这些任务中，机器人末端都不与工作环境相接触，位姿控制就可以满足精度和安全要求。但是机器人在完成一些与环境存在力作用的任务时，比如打磨、装配，单纯的位置控制会由于位置误差而引起过大的作用力，从而伤害零件或机器人。机器人在这类运动受限环境中运动时，往往需要配合力控制来使用[1-2]。近年来对实现力控制的不同策略进行了广泛研究[3-6]。力位混合控制是指机械臂在某些自由度上进行力控制，在余下的自由度上进行位置控制[7-9]。位置控制和力控制是解耦的，他们的控制方法可以分开设计，一般来说，位置控制需要比例-微分 (proportional-proportional,PD) 控制，力控制需要比例-积分(proportional-integral, PI)控制，因为位置控制需要响应速度快，而力控制需要误差更小。力位混合控制算法的优点是可以直接控制机器人末端的位置和作用力，而缺点是需要环境的精准模型和控制模式切换时稳定性较差。阻抗控制[10-12]是将机械臂末端等效成理想的质量-弹簧-阻尼系统，建立末端位置或速度与接触力间的动态关系，通过调整目标阻抗参数可以调整这种动态关系。阻抗控制的缺点是由于缺少力的闭环控制无法精确地控制力。为了解决这个问题，文献[13]提出了笛卡尔空间的力/阻抗控制器，该控制器将力控制与阻抗控制进行融合，实现了较高的力控精度的同时还具有柔顺性。由于力控制器与阻抗控制器采用并联结构所以不需要在两种控制算法中进行切换。力控器设计的难点是在力控制过程中末端工具与作业对象脱离接触时可能会使机械臂的移动速度快速增加并造成机械臂与周围环境发生碰撞。提出了基于无源性的笛卡尔空间力/阻抗控制, 通过导入能量罐的概念，使力跟踪控制器、阻抗控制器、能量罐和电机模型构成一个无源系统从而保证系统的稳定性。但是只通过能量罐的能量限制本身并不能阻止能量的突然释放，这可能导致系统某部分不满足无源性的能量突然增加, 文献[14]中通过根据参考功率轨迹调节能量罐和系统之间的交换功率来解决这个问题。在此控制策略中使用笛卡尔空间动力学模型，所以具有计算量大的缺点。文献[15]中提出了无源观测器的概念。无源观测器包括机械臂和除关节空间PD控制器以外的先进控制器。只需保持无源观测器内部系统为无源的，控制系统整体的稳定性便可以保证。

基于此，首先分析笛卡尔空间力/阻抗控制系统的无源性。期望得到可能会破坏系统无源性的控制输入。将文献[15]中的无源观测器导入到笛卡尔空间力/阻抗控制中，尝试解决力控制过程中末端工具与作业对象脱离接触时末端工具可能与周围环境发生激烈碰撞的问题，本文提出的控制算法特点是计算量小并且不需要动力学模型。

1 机器人控制系统的无源性

无源性的概念来自电路理论，主要考虑系统与外界的能量交换。在机械系统中当系统的输入力fin和输出速度vout具有相同的维数时，如果始终满足关系式

(1)

则该系统称为无源的[16]，其中γ0为定数。

n自由度的机械臂动力学方程表示为

(2)

机械臂总能量E包括动能K和重力势能P,其表达式分别为

(3)

g(q)=[∂P(q)/∂qT]T

(4)

(5)

=-γ02

(6)

将(6)式微分可得

(7)

由式(7)可得出机器人是严格输出无源的。

当严格输出无源系统与线性时不变严格正实系统负反馈连接时系统是渐进稳定的[10]。线性时不变严格正实系统可选定为PD控制器，则有

(8)

式(8)中：uPD为PD控制器的输出；qd为关节位置期望值；KP为比例系数，KD是微分系数，KP与KD均大于0。该控制器实现了关节空间的位置控制，系统控制框图如图1所示。

图1 关节空间的位置控制Fig.1 Position control in joint space

文中所考虑的力/阻抗控制需要在笛卡尔空间内实现, 所以将图1所示的关节空间控制转换成笛卡尔空间控制，转换结果如图2所示。

图2 笛卡尔空间的位置控制Fig.2 Position control in Cartesian space

FPD是笛卡尔空间PD控制器的输出信号，笛卡尔空间PD控制器表示为

(9)

式(9)中：Xd为笛卡尔空间位置期望值；X为笛卡尔空间位置；Kpx与Kdx分别为比例系数和阻尼系数。

笛卡尔空间与关节空间的力与速度变换关系表示为

τin=JT(q)FPD

(10)

(11)

式中：J(q)是机器人雅克比矩阵；τin为关节空间的输入力矩。

在关节空间下讨论的无源性和稳定性的结论在笛卡尔空间下依然成立，即如图2所示的笛卡尔空间的机器人子系统是无源的，整体的控制系统是渐进稳定的。

2 笛卡尔空间力/阻抗控制和无源性分析

笛卡尔空间自由度n的机械臂动力学模型为

(12)

笛卡尔空间力/阻抗控制器的计算式为

Fin=Fimp+FF

(13)

(14)

(15)

式中：Fimp是阻抗控制器的输出，Kpx与Kdx分别是阻抗控制器的刚度和阻尼系数。FF是力控制器的输出，Fd是机械臂末端对环境的期望力，KFx、IFx和DFx是力控制器的PID参数。这里的阻抗控制器与上文提到的笛卡尔空间PD控制器是相同的。笛卡尔空间力/阻抗控制器由式(14)的阻抗控制器与式(15)的力控制器组成，控制框图如图3所示。

图3 笛卡尔空间力/阻抗控制器Fig.3 Cartesian force/impedance control

图4 力/阻抗控制器的无源性分析Fig.4 Passivity analysis of Cartesian force/impedance control

当式(13)中FF=0时，笛卡尔空间力/阻抗控制变为阻抗控制，由于机器人与外部环境组成的子系统是无源的，并且阻抗控制为线性时不变严格正实的，所以系统整体是稳定的。当式(13)中FF≠0时，系统整体的稳定性无法确定, 因此认为只有FF会破坏系统的无源性和稳定性。在下文中会提出监控系统无源性的方法，当检测到系统接近非无源状态时会减小力控制器的输出FF，从而使系统始终保持为无源的。

3 基于无源性观测器的笛卡尔空间力/阻抗控制

图5 无源性观测器Fig.5 Passivity monitor

储存在无源性观测器的能量表达式为

(16)

式(16)中：Emon与Pmon分别表示进入无源性观测器的能量与功率。

由式(1)的无源性的定义可知，当Emon满足

Emon≥E0

(17)

时，无源性观测器为无源的。其中E0为表示无源性观测器初始能量的定数。

通过上文的分析可知只有力控制输出FF会破坏无源性观测器的无源性。当无源性观测器接近非无源的状态时，可以通过引入整形函数α(Emon)和β(Pmon)减小FF，从而达到恢复无源性的目的。引入整形函数后的力控制输出设计F′F为

(18)

α=

(19)

(20)

式中：Eu与El(Eu=E0>El)是合适的定数。当Emon在El与Eu之间时，说明无源性观测器有从无源转变为非无源的倾向，此时设定0<α<1到达减小FF的效果。当Emon小于El时，说明无源性观测系统已经接近非无源的状态，此时设定α=0到达使FF为零的效果。β起到了当系统有快速从无源转换为非无源的倾向时减小FF的作用。

基于无源性观测器的笛卡尔空间力/阻抗控制就是对笛卡尔空间力/阻抗控制器中的力控制器引入整形函数的控制策略。

通过检测储存在无源性观测器中的能量调节会破坏系统的无源性的力控制输出FF，从而使无源性观测器始终保持为无源的。因为无源性观测器与线性时不变严格正实的PD控制负反馈连接所以系统整体是稳定的。

图6 仿真模型Fig.6 Simulation model

4 仿真实验

使用Matlab/Simulink的Simscape Multibody 工具箱进行模型的搭建，仿真模型如图6所示。双连杆机械臂的长度分别是l1=0.32 m,l2=0.29 m。在杆2的末端固定了一个半径10 mm的小球。关节1与关节2的角度分别用q1和q2表示。板1与板2在Y方向上有4 mm的落差。

实验过程分成3个过程，过程1是双连杆机械臂从初始位置q1=0 rad,q2=0 rad运动到q1=0.785 rad,q2=-2.04 rad的位置，此时采用的控制策略是式(8)所示的关节空间位置控制。过程2是使用笛卡尔空间力/阻抗控制器进行控制。阻抗控制器的运动指令为沿着X方向，以0.05 m/s的速度运动。力控制器的期望力为(Fdx,Fdy,Fdz)=(0, -40,0)N。在力控制器的作用下双连杆机械臂会向Y负方向运动, 直到连杆2的末端小球与板1接触为止，末端小球会给板-40 N的下压力。过程3中所使用的控制器与过程2相同，由于板1与板2之间在Y方向上有高度差，机械臂末端运动到板1与板2的交界处时会产生较大的冲击力。仿真实验中对比了力/阻抗控制器与基于无源性观测器的力/阻抗控制器的机械臂末端的受力曲线。

力/阻抗控制器参数选择为Kpx=1 000 N/m,Kdx=20 N·s/m,KFx=0.1,IFx=5 Hz，DFx=0。无源性观测器的参数选择为，Eu=E0=1 J,El=0 J,Pl=-2.0 W。

图7所示为使用力/阻抗控制器时机械臂末端在Y方向的受力和速度。

图7 笛卡尔空间力/阻抗控制器仿真结果Fig.7 Simulation results of cartesian force/impedance control

从图7(a)中可知过程2中在力控制作用下机械臂末端在0.536 s时与板1接触, 机械臂末端在板1与板2的交界处受到的冲击力的最大值为550 N。图7(b)中可知在板1与板2的交界处机械臂末端Y方向的移动速度vy的最大值为-0.51 m/s。

图8所示为基于无源性观测器的力/阻抗控制结果。图8(a)为机械臂末端在Y方向的受力Fyext，图8(b)为机械臂末端Y方向的移动速度vy，图8(c)为储存在无源性观测器的能量Emon，图8(d)为无源性观测器的能量储存功率Pmon，图8(e)自变量是Emon的整形函数α的曲线，图8(f)为自变量是Pmon的整形函数β的曲线。

图8 基于无源观测器的笛卡尔空间力/ 阻抗控制器仿真结果Fig.8 Simulation results of passivity monitor based cartesian force/impedance control

从图8(a)中可得末端小球在板1与板2的交界处受到的冲击力的最大值为仅为62 N。由图8(b)可知在板1与板2交界处机械臂末端最大移动速度的为-0.053 m/s。由图8(c)可知储存在无源性观测系统的能量Emon在过程2中是递增的，机械臂运动到板1与板2的交界处时Emon减小。由图8(e)可知α=1,说明由于Emon>E0，所以无源观测器内的系统是无源的。由图8(d)可知在板1与板2的交界处无源观测器的功率为-4 W，说明无源观测器内部的系统有快速成为非无源的倾向。无源观测器的功率为-4 W，如图8(f)所示β=0.5，F′F=0.5FF。说明无源性观测系统起到了减小力控制器的输出的作用。

5 结论

(1)首先分析了笛卡尔空间力/阻抗控制器的无源性，得出只有力控制器的输出会破坏系统的无源性的结论。

(2)针对笛卡尔空间力/阻抗控制器设计了无源性观测器。无源观测器的输入和输出分别是阻抗控制器的输出和机械臂末端速度。通过储存于无源性观测器的能量判断系统是否是无源的，在无源性的判断上由于不需要使用机器人动力学模型，所以本算法具有较好的可行性及一定的工程参考性。无源性观测器也可以针对其他的先进力控算法进行设计所以具有一定的泛用性。

(3)仿真结果表明使用无源性观测器的力/阻抗控制器可以解决机器人末端工具与作业对象脱离接触时速度激增的问题提高了系统的安全性。