机器人入门知识(课件)第10章力控与顺应控制

1、2022-6-7智能与控制工程研究所1第十章第十章 机器人的力控和顺应控制机器人的力控和顺应控制Chapter Force Control and Compliance for Robot Manipulators10.1 引言10.2 通用机器人控制器和控制结构 10.3 通用机器人的动力学10.4 阻抗控制10.5 主动刚度控制10.6 位置和力的混合控制2022-6-7智能与控制工程研究所210.1 引言（引言（Introduction）工业机器人的控制可大致分为三种形式位置控制（Position Control）力控（Force Control）顺应控制（Compliance）2022

2、-6-7智能与控制工程研究所310.1.1 位置控制（位置控制（ Position Control） 位置控制是在预先指定的坐标系上,对机器人末端执行器（end effector）的位置和姿态（方向）的控制。如图10-1所示，末端执行器的位置和姿态是在三维空间描述的，包括三个平移分量和三个旋转分量，它们分别表示末端执行器坐标在参考坐标中的空间位置和方向（姿态）。因此，必须给它指定一个参考坐标，原则上这个参考坐标可以任意设置，但为了规范化和简化计算，通常以end effectorXXYYZZ图10-1 机器人操作手O机器人的基坐标作为参考坐标。机器人的基坐标的设置也不尽相同，如日本的Movema

3、sterEx系列机器人，它们的基坐标都设置在腰关节上，而美国的Stanford机器人和Unimation公司出产的PUM系列机器人则是以肩关节坐标作为机器人的基坐标的。2022-6-7智能与控制工程研究所4 机器人的位置控制主要有直角坐标和关节坐标两种控制方式。直角坐标位置控制：是对机器人末端执行器坐标在参考坐标中的位置和姿态的控制。通常其空间位置主要由腰关节、肩关节和肘关节确定，而姿态（方向）由腕关节的两个或三个自由度确定。通过解逆运动方程，求出对应直角坐标位姿的各关节位移量，然后驱动伺服结构使末端执行器到达指定的目标位置和姿态。关节坐标位置控制：直接输入关节位移给定值，控制伺服机构。 从7

4、0年代初开始，专家们提出了各种各样的位置控制方法和相应的控制算法，其中比较有代表性的有：（1） 解运动位置的控制解运动位置的控制RMPC （Resolved Motion Position Control） 1972年由Richard P. Paul提出机器人关节坐标路径和直角坐标路径两种轨迹控制方法，其代表作为：R P Paul. Modeling, Trajectory Calculation and Serving of a Computer Controlled Arm. Stanford Artificial Intelligence Lab., Stanford Universit

5、y, Stanford, CA. A. I. Memo 177, Sept. 1972R P Paul. Manipulator Cartesian Path Control. IEEE Trans. On Sys. Man, Cybernetics, Vol. SMC-9, Nov.1979, PP.702-7112022-6-7智能与控制工程研究所5（2）解运动速度的控制）解运动速度的控制RMRC （Resolved Motion Rate Control） 1969年由D. E. Whitney提出。代表作是：D E Whitney. Resolved Motion Rate Contr

6、ol of Manipulators and Human, Prostheses IEEE Trans. on Man-Mach. system. Vol. MMS-10, No.2, June 1969, pp.47-53（3）解运动加速度的控制）解运动加速度的控制RMAC ( Resolved Motion Acceleration Control ) 1980年由美籍华人科学家陆养生（J. Y. S. Luh）提出。其代表作为：J Y S Luh, M W Walker, and R P Paul. Resolved Acceleration control of Mechanical

7、Manipulators. IEEE Trans. on Auto. Control , Vol. AC25, No.3, June 1980, pp468-4742022-6-7智能与控制工程研究所6（4）解运动力的控制）解运动力的控制RMFC ( Resolved Motion Force Control ) 1982年由吴清华（Wu C. H.）和R. P. Paul提出。其代表作为：C H Wu and R P Paul. Resolved Motion Force Control of Robot Manipulators. IEEE Trans. on Sys. Man and C

8、ybernetics, Vol. SMC12，No.3, May/June, 1982 解运动位置的控制RMPC，解运动速度的控制RMRC，解运动加速度的控制RMAC和和解运动力的控制RMFC这四种控制方法是机器人运动控制的经典之作。 现有的通用工业机器人一般只具有位置（姿态，速度）控制能力。如美国的Unimation PUMA系列机器人，CINCINNAT1T3系列机器人，Stanford机器人等，它们的重复定位精度均达到或接近0.1mm。日本三菱公司的MovemasterEX机器人为0.3mm，高精度的Adapt机器人和Delta机器人的重复定位精度达到或接近0.01mm。所有这些都具有关

9、节位置和直角坐标位置的控制，且具有专用的机器人语言（如VAL）或通用的高级语言（如BASIC）编程和示教再现能力。2022-6-7智能与控制工程研究所710.1.2 力控力控 ( Force control ) 力控是对机器人末端执行器输出力或关节力矩的控制。较早提出机器人力控的是Groome，他在1972年将力反馈控制用在方向舵的驾驶系统中。参见下文：R C T Groome. Force Feedback Steering of teleoperator System. Masters Thesis, Massachusetts Institute of Technology (MIT),

10、 Aug. 1972 1974年，Jilani将力传感器安装在一台单轴液压机械手上进行力反馈控制实验。参见下文：M A Jilani. Force Feedback Hydraulic Servo for Advanced Automation Machine. Masters Thesis, MIT, Dept. of Mechanical Engineering, 19742022-6-7智能与控制工程研究所8 真正将力控用于多关节机器人上的是Whitney，他在1977年将力传感器用在多关节机器人上，并用解运动速度的方法（RMRC）推导出力反馈控制的向量表达式。而R.P.Paul（197

11、2）和Silver（1973）则分别用选择自由关节（free joints）的方法实现对机器人力的开环控制。见下文：见RMPC列举的文（1）D Silver. The little Robot System. AIM-73, Cambridge, MIT, Artificial Intelligence Lab., 1973 1976年R. P. Paul 和 B. Shimano进一步完善上述方法，采用腕力传感器实现对机器人力的闭环控制。见下文：R P Paul and B Shimano. Compliance and Control. Proc. Joint Automatic cont

12、rol, Conf. Sam Francisco, IEEE, pp694-699, 19762022-6-7智能与控制工程研究所910.1.3 顺应控制顺应控制 ( Compliance Control ) 顺应控制又叫依从控制或柔顺控制，它是在机器人的操作手受到外部环境约束的情况下，对机器人末端执行器的位置和力的双重控制。顺应控制对机器人在复杂环境中完成任务是很重要的，例如装配，铸件打毛刺，旋转曲柄，开关带铰链的门或盒盖，拧螺钉等。 顺应控制可分为两种方式:被动式 ( Passive Compliance )主动式 ( Active Compliance )2022-6-7智能与控制工程研

13、究所10被动柔顺被动柔顺 ( Passive Compliance ) ) 被动式顺应控制是设计一种柔性机械装置，并把它安装在机械手的腕部，用来提高机械手顺应外部环境的能力，通常称之为柔顺手腕（Compliance Wrist）。这种装置的结构有很多种类型，比较成熟的典型结构是由美国麻省的The Charles Stark Draper Lab. 的D. E. Whitney领导的一个小组研制的一种称之为RCC（Remote Center Compliance）的无源机械装置，它是一种由铰链连杆和弹簧等弹性材料组成的具有良好消振能力和一定柔顺的无源机械装置。该装置有一个特殊的运动学特性，即在它

14、的中心杆上有一个特殊的点，称为柔顺中心（Compliance Center），如图10-2所示。若对柔顺中心施加力，则使中心杆产生平移运动，若把力矩施加到该点上，则产生对该点的旋转运动，该点（柔顺中心）往往被选作为工作坐标的原点。 像RCC这样的被动式柔顺手腕，由于不需要信息处理，而只靠自身的机构调整，所以具有快速响应的能力，而且结构简单，价格低廉。但它只能在诸如插轴入孔这样一些专用场合使用，且柔顺中心的调整也比较困难，不能适应杆件长度的变化，柔顺度固定，无法适应不同作业任务要求，这些都是由于其机械结构和弹性材料决定的，因此其通用性较差。后来也有人设计一种柔顺中心和柔性度可变的RCC装置，称为

15、VRCC (Variable RCC)，但结构复杂，重量大，且可调范围有限。2022-6-7智能与控制工程研究所11柔顺中心旋转部件平移部件O(a) RCCxF(b) 平移M(c) 旋转图10-2 RCC工作原理2022-6-7智能与控制工程研究所12主动刚度控制主动刚度控制 ( ( Active Stiffness Control ) ) 刚度控制是阻抗控制的一个特例，它是对机器人操作手静态力和位置的双重控制。控制的目的是调整机器人操作手与外部环境接触时的伺服刚度，以满足机器人顺应外部环境的能力。其代表作是：J K Salisbury. Active Stiffness Control of

16、 a Manipulator in Cartesian Coordinates. IEEE Conf. of Decision and Control. Nov. 1980. pp.95-106. Dept. of Computer Science, Stanford University.位置位置/力混和控制（力混和控制（ Hybrid Position/Force Control ） 位置/力混和控制是由Raibert and Craig 在1981年提出的 它的思想是分别将机器人的力控和位置控制在控制器的两个不同通道上实现，这就是著名的RC控制器。其代表作是：M H Raibert an

17、d J J Craig. Hybrid Position/Force control of Manipulators. Trans, of ASME, Journal of DSMC, Vol. 102, June 1981. pp.126-1332022-6-7智能与控制工程研究所13顺应控制（顺应控制（ Compliance control ） 有关顺应控制的理论和方法，是由Mason在1981年提出的。内容包括对外部环境的描述，自然约束和人为约束条件，力控与位置控制等。其代表作是：M T Mason. Compliance and Force control for Computer C

18、ontrolled Manipulators. IEEE Trans. On SMC, Vol. SMC-11, No.6, June.1981. pp.418-432R P Paul and B Shimano. Compliance and Control. American Automatic Control Council , proc. of the 1976 Joint Automatic Control Conference, 1976. pp.694-6992022-6-7智能与控制工程研究所1410.2 通用机器人控制器和控制结构通用机器人控制器和控制结构 （The Stru

19、cture of General Robot）图10-3 通用机器人控制结构解逆运动程Xd d关节位控制PID光电码盘机器人操作手XddibieiX 2022-6-7智能与控制工程研究所15由图10-3可知，通用机器人是一个半闭环控制机构，即关节坐标采用闭环控制方式，由光电码盘提供各关节角位移实际值的反馈信号bi。直角坐标采用开环控制方式，由直角坐标期望值Xd解逆运动方程，获得各关节位移的期望值di，作为各关节控制器的参考输入，它与光电码盘检测的关节角位移bi比较后获得关节角位移的偏差ei，由偏差控制机器人操作手各关节伺服机构（通常采用PID方式），使机械手末端执行器到达预定的位置和姿态。直角

20、坐标位置采用开环控制的主要原因是目前尚无有效准确获取（ 检测 ）末端执行器位置和姿态的手段。但由于目前采用计算机求解逆运动方程的方法比较成熟，所以控制精度还是很高的。如美国Unimation PUMA系列机器人 CINCINNATI-T3系列机器人和Stanford机器人，其直角坐标位置重复定位精度达到0.1mm 。日本三菱公司的RM101和 MovemasterEX机器人重复定位精度为0.3mm，而坐标型高精度机器人Delta和Adapt机器人重复定位精度甚至达到0.01mm 。（注意：重复定位精度不是轨迹控制精度，后者精度要低得多）。应该指出的是目前通用工业机器人位置控制是基于运动学的控制

21、而非动力学控制。只适用于运动速度和加速度较小的应用场所。对于快速运动，负载变化大和要求力控的机器人还必须考虑其动力学行为。2022-6-7智能与控制工程研究所1610.3 通用机器人的动力学（通用机器人的动力学（ Dynamics of General Robots ）正动力学计算正动力学计算 由关节力矩计算关节加速度 ，即 通用机器人的动力学模型为 H ( q ) + C ( q, ) + G ( q ) + JT( q ) Fe = （10.1）式中： H( q ) 惯量矩 q 关节位置 C ( q, ) 向心力和哥氏力的力矩G ( q ) 重力矩 Fe 外力和外力矩(含摩擦力和阻尼作用)

22、JT ( q ) Jacobian阵 关节驱动力矩q.q.q.q .q .q .2022-6-7智能与控制工程研究所17解析法解析法：求（10.1）式的解析解，计算精确，旦计算量大，通用性差。数值法数值法：将（10.1）式改写成H ( q ) = b (10.2)式中： b C ( q, ) + G ( q ) + JT( q ) Fe 偏移力矩步骤是：（）计算b：由于b与 无关，只与q和 有关，可假设 0，由逆动力学计算得到关节力矩b；（）计算H（ q ）只与 有关，与 q 和 无关，可设 G ( q ) = Fe = 0, 令 j0 0 0 1 00T，用逆动力学计算得到关节力矩应为H (

23、 q )的第j列，由于H ( q )为对称阵，因而只要计算上三角阵即可。因而需进行N次逆动力学计算，计算量大。可做如下简化： 由于 j，则第j个关节后所有N-j+1个连杆可合成为单个刚体，称为第j个杆到机械手末端的组合件（合成杆），由力学基本原理可计算出组合件质量Mj，质心Cj，惯量矩Ej，所受合力Fj及合力矩Nj，这样可作为一个臂参加逆动力学递推计算，大大减小计算量。q.q .q .q .q .q .q.q.q.q.q.2022-6-7智能与控制工程研究所18逆动力学计算逆动力学计算根据机械手状态q， ，计算关节驱动力矩，方法如下：（1）拉格朗日法*（2）牛顿欧拉法（递推算法）（3）Kane

24、法* （4）阿贝尔方程法（5）高斯最大约束原理（6）广义达朗贝尔原理 * 最常用，计算量最小控制系统仿真控制系统仿真（七种不同类型的控制器）（1）独立关节的PID控制 （2）分解运动速度控制（3）分解运动加速度控制 （4）计算力矩控制（5）变结构控制（6）自适应控制（7）顺应控制q .2022-6-7智能与控制工程研究所19参考文献参考文献1 黄心汉，PUMA560机器人及其运动学方程，电气自动化，1986（5）：16212 黄心汉，机器人的主动顺应控制，华中学院学报，1987（4）：1471543 Richard P. Paul, Ma Rong and Hong Zhang. The Dy

25、namics of the PUMA Manipulator . Research Report of School of Electrical Engineering, Purdue University, West Lafayette, Indiana 49707 TR-EE84-194 韩朔眺等，机器人PUMA560的动力学方程，机器人, 1987（4）：23275 贺日盢，机器人PUMA560逆运动方程的解析法，西安公路学院自动化系2022-6-7智能与控制工程研究所2010.4 阻抗控制阻抗控制 ( Impedance Control ) 阻抗控制的概念是N. Hogan在1985年

26、提出的*，他利用Norton等效网络概念，把外部环境等效为导纳，而将机器人操作手等效为阻抗，这样机器人的力控制问题便变为阻抗调节问题。阻抗由惯量弹簧阻尼三项组成，期望力为：Fd = K X + B + M (10.3) 式中：XXdX, Xd为名义位置，X为实际位置。它们的差X为位置误差，K、B、M为弹性、阻尼和惯量系数矩阵，一旦K、B和M被确定，则可得到笛卡儿坐标的期望动态响应。利用式（10.3）计算关节力矩，无需求运动学逆解，而只需计算正运动学方程和Jacobian矩阵的逆J1。N Hogan. Impedance Control: An Approach to Manipulation

27、Part-Theory, Part-Implementation, Part-Application. ASME Trans. of DSMC, Vol. 107(1), 1985XX 2022-6-7智能与控制工程研究所21图10-4 阻抗控制结构图J-1KPARMXdXJ-1JTKvKf1KE力传感器FsFKf2XEXdX2022-6-7智能与控制工程研究所22 图10-4中，当阻尼反馈矩阵Kf20时，称为刚度控制。 刚度控制是用刚度矩阵Kp来描述机器人末端作用力与位置误差的关系，即F ( t ) = Kp X (10.4) 式中Kp通常为对角阵，即KpdiagKp1 Kp2 Kp6。刚度

28、控制的输入为末端执行器在直角坐标中的名义位置，力约束则隐含在刚度矩阵Kp中，调整Kp中对角线元素值，就可改变机器人的顺应特性。 阻尼控制则是用阻尼矩阵Kv来描述机器人末端作用力与运动速度的关系，即F ( t ) = Kv （10.5） 式中Kv是六维的阻尼系数矩阵，阻尼控制由此得名。通过调整Kv中元素值，可改变机器人对运动速度的阻尼作用。X2022-6-7智能与控制工程研究所23 阻抗控制本质上还是位置控制，因为其输入量为末端执行器的位置期望值Xd（对刚度控制而言）和速度的期望值 （对阻抗控制而言）。但由于增加了力反馈控制环，使其位置偏差X和速度偏差 与末端执行器与外部环境的接触力的大小有关，

29、从而实现力的闭环控制。这里力位置和力速度变换是通过刚度反馈矩阵Kf1和阻尼反馈矩阵Kf2来实现的。这样系统的闭环刚度可求出* 当 Kf20时 Kcp ( I Kp Kf1 )1 Kp （10.6）Kf1 Kcp1 Kp1 （10.7） 当 Kf10时Kcv ( I Kv Kf2 )1 Kv （10.8） Kf2 Kcv1 Kv1 （10.9）黄心汉，机器人的主动顺应控制，华中工学院学报， 1987-15(4): 147-154dXX2022-6-7智能与控制工程研究所2410.5 主动刚度控制主动刚度控制 （Active Stiffness Control ）10.5.1 广义直角坐标刚度与关

30、节坐标刚度广义直角坐标刚度与关节坐标刚度Generalized Cartesian Coordinate Stiffness and Joint France Stiffness 将线性弹簧的虎克定理f k dx 推广到直角坐标中六维矩阵的形式有f kx （10.10） 式中x dx dy dz x y z T 称为位置偏差向量，其中前三个分量是位置偏差平移分量，后三个分量是旋转分量；f = fx fy fz mx my mz T是六维力向量； k 66 维刚度矩阵，矩阵元素 kij ( i, j = 1, 2, 3 6 )表示位置偏差向量与力向量之间的关系，如果将k选定为66的对角阵，即 k

31、 diag k11 k22 k66 ，即表明力向量与位置偏差向量是去耦的，这时它们之间的各个分量之间具有一一对应的线性关系。2022-6-7智能与控制工程研究所25 对角刚度矩阵所依附的直角坐标原点称为刚度中心（Stiffness Center），显然刚度中心具有这样的性质，即如果在这一点施加力，只会引起沿力方向上的平移运动，如果对通过该点的坐标轴施加力矩，只会产生绕该轴的旋转运动。这与被动柔顺手腕RCC的柔顺中心的运动特性一致。但由于刚度矩阵所依附的坐标可以任意设置，故刚度中心位置也可任意改变，这是被动柔顺手腕无法实现的。 式 f kx 是在直角坐标中描述六维力向量与位置偏差向量的关系式，因

32、而称k为广义直角坐标刚度矩阵。运用Jacobian阵J作微分变换，则有x J （10.11） 式中d，为指令关节角位移与实际关节角位移的差值。设静力和动态力均被补偿，则满足式（10.11）作用力f所需的关节力矩为： JT f （10.12）2022-6-7智能与控制工程研究所26f kx （10.13） x J （10.14） JT f （10.15） 由式（10.13）（10.15）可得： JT k J （10.16） 令 k JT k J，则有 k （10.17） 我们将k称为关节刚度矩阵（Joint Stiffness Matrix），它表示关节位移偏差与关节力矩之间的关系。如果直角坐标

33、刚度矩阵k是对角阵，由k JT k J 可知，关节刚度矩阵k是非对角的对称阵。这意味着有关关节的位置误差会影响其它关节的指令力矩，即关节刚度是耦合的。正是基于这个原因，采用直角坐标刚度控制比较方便。2022-6-7智能与控制工程研究所2710.5.2 主动刚度控制结构主动刚度控制结构 ( The Structure of Active Stiffness Control ) 图10-5是J. K. Salisbury * 提出的主动刚度控制的结构图。J K Salisbury. Active Stiffness Control of a Manipulator in Cartesian Coo

34、rdinates. Proc. of 19th IEEE Conf. on Dec. and contr. 1980, pp. 95-1060图10-5 刚度控制结构图CK J T+K(s+a)/(s+b) Kf / s+ARMG+ K D+000cbssfc+V0sign ( )+2022-6-7智能与控制工程研究所28 图10-5中，关节角度， 关节角速度，fs由腕力传感器提供的末端执行器与外部环境接触力向量 。运用 Jacobian阵可将fs转换成关节平衡力矩s，即 s = JT fs （10.18） 系统外环为位置环，由偏差计算修正力矩K，然后叠加一偏置力矩b，取这两项之和作为关节的指

35、令力矩c，即c Kb （10.19） 式中，b JT fb，是外加力矩，它由任务确定fb，再经J阵转换为b。如果外加力 fb0（b0），则称为零力控制。 对于刚度控制，将c直接加到关节伺服电机，用力开环控制便可实现。该系统为提高系统对力信号的响应性能，加入了力反馈伺服环（内环），采用腕力传感器检测实际作用力fs，用Jacobian矩阵JT变换为关节力矩s，与指令力矩c比较后获得关节力矩误差 ccs，使校正网络C获得修正力矩信号，从而提高机器人对外力作用的响应性能，使末端执行器输出力更接近期望值。在机械手与环境接触前，末端执行器（手爪）与工件的重力可作为偏移量，在计算实际作用力时可将该偏移量减去

36、，从而消除手爪和工件重力的影响。2022-6-7智能与控制工程研究所29 在力反馈回路中，加入超前滞后网络，以提高系统的稳定性，与超前滞后网络并联一积分环节 Kf / s，以减小力的稳态误差，提高系统精度, 积分环节前加入一带死区的限幅器，死区可减小极限环，限幅器则对大误差信号减小积分器的有效增益。 速度反馈回路，提供阻尼力矩KD，其中K是阻尼系数矩阵。D为关节瞬时惯量，加入关节瞬时惯量D的目的是为了保持惯量变化时系统的稳定性。关节阻尼系数矩阵为K JT Kv J (见10.5.1节式（10.16）) （10.20） 考虑重力负载G和摩擦力矩Vo sign（），这样加到关节i的控制力矩为 i

37、=ci Cici + Ki Di+ Vo sign（）+ Gi （10.21）2022-6-7智能与控制工程研究所3010.6 位置和力的混合控制位置和力的混合控制（Hybrid Position/Force Control for Robot Manipulators ）10.6.1 C曲面曲面 （C-surface ） 在环境约束情况下，对机器人进行位置和力的混合控制，通常要先建立一个控制曲面，即所谓C曲面*，在C曲面的切线方向进行位置控制而沿C曲面的法线方向进行力控。M T Mason. Compliance and Force Control for Computer Controll

38、ed Manipulators. IEEE Trans. on SMC-11, 1981(6): 418-432 C曲面定义为*: C = ( q | f2 F (q) f1 ) (10.22)J P Merlet. C-surface Applied to the Design of Hybrid Position/Force Controller. IEEE Conf. of Robotics & Automation,1987(2) 式（10.22）中，当f10, f20时（即零力 作用）的C集叫做C曲面，它的一面为非接 触面，另一面为约束面。如图10-6所示。Nq0图10-6 C曲面2022-6-7智能与控制工程研究所31 在静态情况下，C曲面可看作一几何问题。如果只有接触力，且机器人定位精度已知，在结构化环境下，我们就能确定机器人以什么样的组合形态会导致它与环境相接触。定义N（q0）为C曲面 q0点的法线方向，则在接触点q0处沿N轴方向的运动会导致作用力增大，反之作用力减少。因此沿法线N的运动可控制作用力的大小。以混合控制的观点，找到了C曲面的法线N，即给出了力控制的方向。而位置控制则不能越过C曲面，它只能沿C曲面接触面的切线方向运动。因此沿C曲面的法线方向进行力控制，沿切线方向进行位置控制，便形成了位置/力混合控