5.5 机器人的位置和力的双重控制

机器人工作程序：①自然约束发生变化的情况是通过检测发 现的，而检测量并不是受控量；②手部的位置控制是沿着有自 然力的约束方向；③手部的力控制是沿着有自然位置约束的方 向。
在机器人的作业任务中定义一个广义平面——沿此平面的法 线方向有自然位置约束，可以加入人为的力约束，即实施力 控制；而沿此平面的切线方向有自然力约束，可以加入人为 的位置约束，即实施位置控制。为了便于描述这个广义平面， 也可以用一个坐标系{C}来取代这个广义平面。{C}就是 compliance frame。有些文献称之为task frame， 或者constraint frame。它具有以下四个特点：
+
xc
－
d
+
cfd
+
－
c x
q
J
cx
q
J
I-S c xe J-1 qe Kpd
I-S c xe J-1 qe
cfd
S
JT
Kpp+ Kpi∫dt Kfp
S
cfe
JT
cf cTH
+ #43;
Kfi∫dt
图7-13 R－C 力/位置混合控制器
q ARM q
Hf
顺应控制可分为两种方式: • 被动式 ( Passive Compliance ) • 主动式 ( Active Compliance )
被动式顺应
被动式顺应控制是设计一种柔性机械装置，并把它安装在 机械手的腕部，用来提高机械手顺应外部环境的能力，通常称 之为柔顺手腕（Compliance Wrist）。这种装置的结构有很多 种类型，比较成熟的典型结构是由美国麻省研制的一种称之为 RCC（Remote Center Compliance）的无源机械装置，它是一 种由铰链连杆和弹簧等弹性材料组成的具有良好消振能力和一 定柔顺的无源机械装置。该装置有一个特殊的运动学特性，即 在它的中心杆上有一个特殊的点，称为柔顺中心（Compliance Center)。若对柔顺中心施加力，则使中心杆产生平移运动， 若把力矩施加到该点上，则产生对该点的旋转运动，该点（柔 顺中心）往往被选作为工作坐标的原点。
a.为了方便描述作业，把机器人末端的自由度总是分解为 两个正交集合，{C}是直角坐标系。
b.{C}总是处于与某项具体任务有关的位置。
c.视任务不同，{C}可能在环境中固定不动，也可能随手 爪一起运动。
d.{C}有6个自由度。任一时刻的作业均可以分解为沿每一 个自由度的位置控制或者力控制。
2. R-C控制器
f ＝ kδx
（7.6.3）
式中δx ＝[ dx dy dz δx δy δz] T 称为位置偏差向量，其中前三 个分量是位置偏差平移分量，后三个分量是旋转分量；
f = [ fx fy fz mx my mz ]T 是六维力向量；
k ＝ 6×6 维刚度矩阵，矩阵元素 kij ( i, j = 1, 2, 3 … 6 )表示位置偏差 向量与力向量之间的关系，如果将k选定为6×6的对角阵，即
对于需要进行位置控制的自由度，给予大的刚性； 对于需要进行力控制的自由度，给予小的刚性。
Xd －
＋ －
．
Xd
－
＋ －
J-1 J-1
KP
＋
＋－
ARM
X
XE
＋－
．
X
Kv
JT
KE
Kf2
Fs
力传感器 F
Kf1
图 7-10 一种阻抗控制结构图
图7-10中，当阻尼反馈矩阵Kf2＝0时，称为刚度控制。
刚度控制是用刚度矩阵Kp来描述机器人末端作用力与位置误差的关 系，即
由腕力传感器测取Hf，通过坐标变换为C坐标系力向量cf。图7-13中的s
为6×6的对角阵，即 s = diag[ s1 s2 …s6 ]，称为顺应选择矩阵。其 对角线元素为1或0，由它来确定（选择）那些自由度施加力控，那些自 由度施加位置控制。I 是6×6的单位矩阵。所以I－s是选择矩阵s的逆。
c xd －
图7-13中，机器人各关节驱动电机的力矩分别由位置环（上部）和 力控制环（下部）这两个相对独立的控制环共同提供。位置环由PI调节 器整定，而力控制环由带限幅器的PI调节器整定，给定力通过Jacobian 矩阵转换直接加到关节驱动器。关节位置q和速度由光电码盘或测速发电
机提供。用Jacobian矩阵转换为直角坐标变量 c和x c，x 力反馈信号
τ＝ JT k Jδθ
（7.6.9）
令 kP＝ JT k J，则有
τ＝ kPδθ
（7.6.10）
我们将kP称为关节刚度矩阵（Joint Stiffness Matrix），它
表示关节位移偏差与关节力矩之间的关系。如果直角坐标
刚度矩阵k是对角阵，由kP＝ JT k J 可知，关节刚度矩阵kP 是非对角的对称阵。这意味着有关关节的位置误差会影响
（7.6.4）
式中δθ＝θd－θ，为指令关节角位移与实际关节角位移的 差值。设静力和动态力均被补偿，则满足式（7.6.3）作用
力f所需的关节力矩为：
τ＝ JT f
（7.6.5）
f ＝ kδx
（7.6.6）
δx ＝Jδθ
(7.6.7）
τ＝ JT f
（7.6.8）
由式（7.6.6）～（7.6.8）可得：
k ＝ diag [ k11 k22 … k66 ]，即表明力向量与位置偏差向量是去耦的， 这时它们之间的各个分量之间具有一一对应的线性关系。
式 f ＝ kδx 是在直角坐标中描述六维力向量与位置偏差 向量的关系式，因而称k为广义直角坐标刚度矩阵。运用 Jacobian阵J作微分变换，则有
δx ＝ Jδθ
其它关节的指令力矩，即关节刚度是耦合的。正是基于这
个原因，采用直角坐标刚度控制比较方便。
主动刚度控制结构
J为机械手末端执行装置的雅可比矩阵，Kp为定义 于末端笛卡儿坐标系的刚性对角矩阵（与关节刚度 不同，人为定义的对角阵），如果希望在某个方向 上遇到实际约束，那么这个方向的刚性应当降低， 以保证有低的结构应力；反之，在某些不希望碰到 实际约束的方向上，则应加大刚性，这样机械手紧 紧跟随期望轨迹。于是，就能够通过改变刚性来适 应变化的任务要求。
F ( t ) = Kv △ X
（7..5.2）
式中Kv是六维的阻尼系数矩阵，阻尼控制由此得名。通过调整Kv中元素 值，可改变机器人对运动速度的阻尼作用。
阻抗控制本质上还是位置控制，因为其输入量为末端
执行器的位置期望值Xd（对刚度控制而言）和速度的期 望值（对阻抗控制而言）。但由于增加了力反馈控制环，
7.7 位置和力的混合控制
1. C曲面
自然约束：机器人末端与环境或作业对象接触时，环境的几 何特性或作业结构特性对机器人构成的约束。自然约束与机器 人打算作的运动无关。一般将接触表面定义为一个广义曲面， 沿法向定义自然位置约束，沿切向定义自然力约束。
人为约束：用来描述机器人预期的运动或施加的力时，由人 为定义的一组约束条件。由于在一个给定的自由度上不能同时 对力和位置实施控制，认为约束就必须与自然约束相适应。
图7-13是由Raibert和Craig提出的一种力/位置控制方案，即著名的R －C控制器。该控制器不同于刚度控制和阻抗控制，阻抗控制和刚度控制 的输入是位置和速度，其力控隐含在刚度反馈矩阵中，其本质还是属于 位置控制。而R－C控制器的输入变量既有位置、速度，也有力。R－C控 制器是位置/力混合控制的经典之作，以后许多控制方案都是在这一方案 基础上演变或改进的结果。
主动柔顺
通过改变控制器控制方式，增加力反馈等使 机器人与工作对象间按需要的刚度运动的柔顺 方式。（必须控制器参与） 分为阻抗控制，力和位置混合控制
1.阻抗控制 ( Impedance Control )
通过控制力和位置间的动态关系（阻抗），来实 现柔顺功能。通过控制使机械手末端呈现所需要的刚 性和阻尼。这样的动态关系类似于电路中阻抗的概念， 因而称为阻抗控制。
机器人的位置和力的双重控制
• 柔顺控制: 主动柔顺-阻抗控制和主动刚度控制; 被动柔顺
• 力位混合控制
7.6 柔顺控制
柔顺控制又叫依从控制或顺应控制，它是在机器人的操作手 受到外部环境约束的情况下，对机器人末端执行器的位置和力 的双重控制。顺应控制对机器人在复杂环境中完成任务是很重 要的，例如装配，铸件打毛刺，旋转曲柄，开关带铰链的门或 盒盖，拧螺钉等。
F ( t ) = Kp △X
(7.5.1)
式中Kp通常为对角阵，即Kp＝diag[Kp1 Kp2 … Kp6]。刚度控制的输入为 末端执行器在直角坐标中的名义位置，力约束则隐含在刚度矩阵Kp中， 调整Kp中对角线元素值，就可改变机器人的顺应特性。
当阻尼反馈矩阵Kf1＝0时，称为阻尼控制。阻尼控制则是用阻尼矩 阵Kv来描述机器人末端作用力与运动速度的关系，即
使其位置偏差△X 和速度偏差△ 与末X端 执行器与外部
环境的接触力的大小有关，从而实现力的闭环控制。这
里力－位置和力－速度变换是通过刚度反馈矩阵Kf1和阻 尼反馈矩阵Kf2来实现的。
2.主动刚度控制
广义直角坐标刚度与关节坐标刚度
将线性弹簧的虎克定理f ＝ k dx 推广到直角坐标中六维矩阵的形式 有
被动顺应的结构
像RCC这样的被动式柔顺手腕，由于不需要信息处理，而只 靠自身的机构调整，所以具有快速响应的能力，而且结构简单， 价格低廉。但它只能在诸如插轴入孔这样一些专用场合使用，且 柔顺中心的调整也比较困难，不能适应杆件长度的变化，柔顺度 固定，无法适应不同作业任务要求，这些都是由于其机械结构和 弹性材料决定的，因此其通用性较差。