第9章　机器人控制

2020-03-01 204浏览

  • 1.第9章 机器人控制 机器人学简明教程 第9章 机器人控制 9.1 反馈与闭环控制 9.2 二阶系统控制 9.3 控制律的分解 9.4 轨迹跟踪控制 9.5 单关节控制 9.6 机械臂非线性控制
  • 2.第9章 机器人控制 9.1 反馈与闭环控制 一般机械臂的每个关节都安装测量关节角位置的传感器， 同时装有对相邻连杆施加扭矩的驱动器。我们希望机械臂关 节按期望轨迹运动，而驱动器是按照扭矩指令运动的。因此， 必须应用某种控制律计算出适当的驱动指令来实现期望的运 动。而这些期望的扭矩主要是根据关节传感器的反馈计算出 来的。图9－1为机器人控制系统的示意图。机器人控制系统 根据期望关节轨迹和传感器反馈得到的关节位置与速度信息 计算控制力矩，驱动机器人实现期望的运动。如果不使用传 感器反馈信息，且假设已知系统的动力学模型，可以直接根 据期望关节轨迹得到需要的驱动扭矩 τ  M (θd )θd  C (θd , θd )  G (θd ) (9－1)
  • 3.第9章 机器人控制 图9－1 机器人控制系统示意图
  • 4.第9章 机器人控制 如果模型是精确的，而且不存在其他干扰，应用式(9－ 1)控制机械臂即可实现期望的轨迹。式(9－1)这种不使用反 馈信息的控制方法称为开环控制。然而实际系统的模型一般 不会是精确的，而且干扰总是存在的，所以这种开环控制方 案是不实用的。建立高性能控制系统的方法是利用关节角反 馈信息，通过比较期望位置和实际位置之差以及期望速度与 实际速度之差得到伺服误差  E θd  θ     E  θd  θ (9－2) 系统根据伺服误差函数计算驱动器需要的扭矩。这种利用反馈 的控制系统称为闭环控制系统。从图9－1中可以看出，控制系 统的信号流形成一个封闭的环。
  • 5.第9章 机器人控制 控制系统设计的核心问题是保证闭环系统满足特定的性 能要求，而最基本的标准是系统必须保持稳定。稳定系统的 定性解释是，机器人在按照各种期望轨迹运动时系统始终保 持“较小”的伺服误差，即使存在“中度”的干扰。而不稳 定系统的伺服误差随系统运行增大，而不是减小。
  • 6.第9章 机器人控制 图9－1中所有信号线均表示n维矢量（假设机械臂有n个 关节），因此机械臂控制系统是一个多输入多输出（MIMO） 控制系统。本章将每个关节作为一个独立的系统进行控制， 因此需要设计n个独立的单输入单输出（SISO）控制系统。 这也是目前大部分工业机器人所采用的控制系统设计方法。 这种独立关节控制方法是一种近似方法，因为机械臂的动力 学方程的关节变量之间是相互耦合的。 控制系统设计采用的模型可以选择传递函数或微分方程 描述。因为非线性问题只能采用微分方程描述，所以本书采 用微分方程描述系统的模型。
  • 7.第9章 机器人控制 9.2 二阶系统控制 图9－2所示为一个带驱动器的弹簧质量系统，驱动器在 质量块上施加力f，根据牛顿第二定律可以得到系统的动力 学方程 mx  cx  kx  f (9－3) 系统自由振动的微分方程为 mx  cx  kx 0 (9－4) 可以写成标准二阶系统形式  x  2n x  n2 x 0 (9－5)
  • 8.第9章 机器人控制 图9－2 带驱动器的弹簧-质量系统
  • 9.第9章 机器人控制 式中，ωn为固有频率；ζ为阻尼比，其表达式分别为 n  k / m c   2 km (9－6) 假定控制的任务是使质量块固定在x=0处不动，且可以 通过传感器测量质量块的位置和速度。可以采用如下的反 馈控制律 f   k p x  kd x 该控制系统称为位置校正系统。其功能是在存在扰动的情 况下，保持质量块在固定的位置。将反馈控制律式(9－7) 带入到动力学方程式(9－3)，得到系统的闭环动力学方程 如下 (9－8) mx  cx  kx  k p x  kd x
  • 10.第9章 机器人控制 合并相同项得 mx  c ' x  k ' x 0 (9－9) 式中，k′=k+kp；c′=c+kd。由式(9－9)可知，通过设定控制增 益可以使闭环系统呈现任何期望的二阶系统特性。通常选择 控制增益使阻尼比ζ=0.7或者ζ=1（临界阻尼），并使系统得 到期望的闭环刚度。
  • 11.第9章 机器人控制 例9－1 对于图9－2所示的弹簧质量系统，各参数为质 量m=10kg，阻尼系数c=2Ns/m，刚度系数k=20N/m。求使闭 环系统固有频率为ωn=20且阻尼比ζ=0.7或者ζ=1时临界阻尼 系统的位置校正控制律的增益kp和kd。 解：闭环增益 k’=m n2 =4000 N/m 根据式(9－6)，当ζ=1时 c’=2 mk ' =400 Ns/m
  • 12.第9章 机器人控制 因此系统的控制增益分别为 kp = k’- k =4000-20=3980N/m， kd = c’- c =400-2=398 Ns/m。 当 =0.7 时 c’=20.7 mk ' =280 Ns/m， 因此系统的控制增益分别为 kp = k’- k =4000-20=3980N/m，kd = c’- c =280-2=278 Ns/m。
  • 13.第9章 机器人控制 图9－3 闭环系统响应曲线
  • 14.第9章 机器人控制 9.3 控制律的分解 在系统模型已知的条件下，控制律式(9－7)可以分解为 基于模型的控制和伺服控制两部分。这样系统的参数（m， c，k）仅出现在基于模型的控制部分，使得伺服控制部分的 设计变得简单容易。这种控制律分解方法在复杂的非线性系 统控制中非常重要，可以把复杂的非线性系统控制器设计问 题转化为线性系统控制器设计问题。下面以9.2节介绍的弹 簧质量系统为例介绍这种控制器设计方法。
  • 15.第9章 机器人控制 在式(9－7)中，令基于模型的控制为f=αf′+β,则 mx  cx  kx  f '  (9－10) 将f′作为新的系统输入，并选择 则式(9－10)变为   m   cx kx (9－11)  xf ' (9－12) 采用与9.2节相同的方法设计控制律 f '   k p x  k d x (9－13)
  • 16.第9章 机器人控制 则闭环系统动力学方程为  x  k d x  k p x  0 这种控制律分解设计方法使得控制增益选择非常容易，且与系 统参数无关。kp 即为闭环刚度，使闭环系统处于临界阻尼状态 时 kd=2 k p 。
  • 17.第9章 机器人控制 9.4 轨迹跟踪控制 9.2节和9.3节讨论了期望系统维持在零点的控制律设计 问题，下面研究让质量块按期望轨迹运动的控制律设计问 题。给定质量块的期望轨迹xd(t)，并假设轨迹充分光滑，即 xd和xd存在。定义伺服误差e=xd-x，并采用下面的控制律 f '   xd  k p e  kd e (9－15) 代入到式(9－12)得到系统的闭环误差动力学方程 e  k d e  k p e  0 (9－16)
  • 18.第9章 机器人控制 因此，可以通过选择控制增益，使闭环系统式(9－16) 呈现期望的性能。采用控制分解技术设计的轨迹跟踪控制结 构如图9－4所示。下面讨论闭环控制系统的抗干扰能力和存 在干扰条件下的稳态误差。
  • 19.第9章 机器人控制 图9－4 轨迹跟踪控制结构
  • 20.第9章 机器人控制 控制系统的一个重要作用是具有抗干扰能力，即系统在 存在外部干扰的情况下，仍能保持良好的性能。假设弹簧 质量系统存在有界的常值干扰力，闭环系统的误差动力学方 程变为 e  k d e  k p e  f dist (9－17) 根据常微分方程理论，误差动力学方程的解是有界的。当系 统达到稳态时，系统的稳态误差为 e=fdist/kp (9－18) 式(9－18)表明系统在常值干扰的情况下存在稳态误差，其数 值随控制增益kp增大而减小。
  • 21.第9章 机器人控制 为了消除稳态误差，一般采用在控制律中附加积分项。 控制律为 f '   xd  k p e kd e  ki  edt (9－19) 系统的误差方程变为 e  kd e  k p e  ki  edt  f dist (9－20) 计算式(9－20)的导数，得  e  kd e  k p e  ki e  fdist (9－21) 这是一个三阶常微分方程，注意到干扰是常数，因此系统的 稳态误差为 e=0 (9－22)
  • 22.第9章 机器人控制 控制律式(9－19)是工程上广泛使用的所谓比例-积分-微 分控制，即PID控制。式(9－15)是其简化形式，称为PD 控制。 从以上分析可知，在存在常值外部干扰的情况下，PD 控制存在稳态误差，而附加积分项的PID控制可以消除这 种稳态误差。
  • 23.第9章 机器人控制 我们仍然以例9－1的弹簧质量系统为例设计轨迹跟踪控 制律。各参数取为质量m=10kg，阻尼系数c=2Ns/m，刚度系 数k=20N/m，设计闭环系统的固有频率ωn=20，阻尼比ζ=1。 PD控制律的增益kp=400和kd=40，系统的期望轨迹为 xd=sin2πt。图9－5给出了弹簧质量系统的闭环跟踪控制曲 线，其中图9－5(a)是没有外部扰动时系统的跟踪控制曲线， 虚线为期望轨迹，实线为实际轨迹。从图上可以看出两者基 本是重合的，即实现了完美跟踪。
  • 24.第9章 机器人控制 图9－5(b)给出了系统存在常值扰动fdist=300N时系统的跟踪 控制曲线，曲线的含义与图9－5(a)的含义相同，从图上可 以看出系统响应明显向上偏移，即存在静差。图9－5(c)给 出了在前面PD控制基础上附加积分项ki=400，即PID控制 的系统跟踪控制曲线。观察图9－5(c)可以发现，开始阶段 系统存在和PD控制类似的误差，但随着误差积分作用的显 现，经过两个周期以后系统实现了对期望轨迹的完美跟踪。
  • 25.第9章 机器人控制 图9－5 弹簧质量系统跟踪控制曲线
  • 26.第9章 机器人控制 9.5 单关节控制 1.电机模型 假设电机常数为电枢绕组感抗la、电枢绕组电阻ra、转矩常 数km、反电动势常数ke，电路中各物理量表示为电源电压va、 电流ia、反电动势v、电机转矩τm，则电机输出转矩为 电路方程为  m  k m ia (9－23) laia  rai  va  kem (9－24) 忽略电机感抗影响，在理想情况下可以将电机看成一个纯力矩源。
  • 27.第9章 机器人控制 2.机电模型 电机通常通过减速器与负载相连，图9－6所示为通过减 速器与负载相连的电机转子的动力学模型。设传动比为η， 负载转矩为τ，负载转角θ，则负载与电机相应量之间的关系 为    m (9－25)   1 /   m (9－26) 电机转子动力学方程为 m  Imm  cmm  (1/) I c   (9－27) 式中，Im、I分别为转子和负载的转动惯量；cm、c分别为转 子和负载的黏性摩擦系数。采用转子变量表示的动力学方程 为  I    b m   Im  2 m  bm  2 m (9－28)      
  • 28.第9章 机器人控制 图9－6 电机系统模型
  • 29.第9章 机器人控制 采用负载变量表示的动力学方程为    I 2Im  c 2cm  (9－29) I+η2Im称为负载端的有效惯量，当η>>1时,电机转子惯量占有 效惯量的主要部分。因此,可以将有效惯量视为常数,这也是 可以采用独立关节控制的原因之一。 在上面的建模过程中,我们忽略了电机和负载的柔性,但 实际系统都存在一定的柔性。假设系统的柔性振动固有频率 为ωres,为了防止激起未建模的柔性动态（柔性共振）,则设计 闭环控制系统的固有频率应该满足 n  res 2 (9－30)
  • 30.第9章 机器人控制 如果为了加快系统的响应速度而选择很高的增益,就有 激起系统未建模柔性共振的风险。对于带有太阳帆板等柔性 部件的航天器,因为没有空气阻力,柔性共振后果比较严重。 历史上就曾有过因为控制系统引起卫星柔性共振而出现事故 的教训。 单关节控制系统设计中,假设电机为纯力矩源、有效惯 量为常数、机构柔性可以忽略。则可以采用如下的分解控制 策略   I max   2 I m    c   2c max m  (9－31)  '  d  k d e  k p e (9－32)
  • 31.第9章 机器人控制 则闭环系统误差动力学方程为 e  kd e  k p e dist (9－33) 设计闭环系统的固有频率为柔性固有频率的1/2，系统处于临 界阻尼状态，则控制增益为 2 k p  n2  res /4 kd  2 k p  res (9－34) 显然，控制系统设计需要对结构的柔性进行估计。
  • 32.第9章 机器人控制 9.6 机械臂非线性控制 1.反馈线性化控制 前面介绍的单关节控制忽略了机械臂连杆间的耦合,而实 际系统是高度耦合的非线性系统。下面仍然采用控制律分解 技术设计控制律,系统动力学方程为 τ  M (θ )θ  C (θ , θ )  G (θ ) (9－35)
  • 33.第9章 机器人控制 选择分解控制参数(此时为矩阵和矢量) τ  ατ ' β  α  M (θ )  β  C (θ , θ )  G (θ ) 伺服控制律 τ ' θd  K d E  K p E (9－36) (9－37) 其中，伺服误差定义为   E θd  θ      E  θd  θ (9－38)
  • 34.第9章 机器人控制 系统的闭环误差动力学方程为   K E  K p E  0 E d (9－39) 实际应用一般选择控制增益矩阵Kp和Kd为对角矩阵，则此时 矢量方程式(9－39)是解耦的，可以写成与独立关节控制相同 的形式 e  kdi e  k pi e 0 (9－40) 采用控制律分解技术，基于模型的控制部分的反馈补偿 将系统变为单位质量的二阶线性系统，因此该方法也被称为 反馈线性化方法。当然，实际系统不可能具有如此理想的控 制性能，因为模型总是不够精确的。
  • 35.第9章 机器人控制 下面以第6章例6－5采用的两连杆平面机械臂（见图6－ 7）为对象，说明机械臂反馈线性化控制（控制律分解技术） 方法。仿真采用的机械臂的参数如下 大臂质量m1=1kg,小臂质量m2=0.2kg,大臂长度L1=1m,小 臂长度L2=0.6m,大臂质心到臂起点距离Lc1=0.5m,小臂质心到 臂起点距离Lc2=0.3m, 大臂转动惯量I1=0.1kgm2, 小臂转动惯 量I2=0.02kgm2, 重力加速度g=9.8m/s2。
  • 36.第9章 机器人控制 采用式（9－36）和式（9－37）给出的控制律分解和伺 服控制方法，闭环系统的刚度为ωn=10（kp=100），阻尼比 η=0.7（kd=14）。期望机械臂末端画一个以（0.5,0）为圆心， 以0.1为半径的圆，期望机械臂末端运动轨迹选为 rx (t)=0.1cos(2t)+0.5; ry (t)=0.1sin(2t);
  • 37.第9章 机器人控制 因为机械臂控制是在关节空间进行的，所以必须先求出 关节空间的期望关节角及其角速度轨迹。期望关节角轨迹可 以按一定的时间步长采用逆运动学式（3－12）~式（3－14） 计算离散时间点的值。期望角速度轨迹可以采用雅可比矩阵 建立的关节空间速度与笛卡尔空间速度的关系式（5－15） 获得，例5－3已经计算了该机械臂的雅可比矩阵。而笛卡尔 空间速度可以通过对上面给出的期望机械臂末端运动轨迹求 导而得到。图9－7给出了两连杆机械臂反馈线性化跟踪控制 的仿真结果，仿真图表明，即使在初始误差比较大的情况下， 闭环系统也可以实现期望轨迹的有效跟踪。
  • 38.第9章 机器人控制 图9－7 两连杆机械反馈线性化跟踪控制仿真结果
  • 39.第9章 机器人控制 2.独立关节PID控制 实际工业机器人一般采用如下的独立关节PID控制 (9－41) τ θd  K d E  K p E  K i  Edt 式中，增益矩阵Kd、Kp、Ki为正常数对角矩阵，当期望加速 度未知时可以简单地设为零。也就是实际工业机器人一般不 采用基于模型的控制。因此，可以简单地对机械臂的每个关 节独立进行控制，一般每个关节采用一个独立的单片机（或 数字信号处理器DSP）进行控制。因为动力学的重力项估计 相对比较容易，所以也有些工业机器人采用附加重力补偿项 的控制方法 τ θd  K d E  K p E  K i  Edt  Gˆ (θ ) (9－42)
  • 40.第9章 机器人控制 同时也可以将质量矩阵的估计引入到控制系统中，但速度项 和摩擦项一般难以建模估计。独立关节PID控制和带重力补偿 项PID控制的稳定性分析不能采用前面的线性系统分析方法， 可以采用下面介绍的李雅普诺夫方法证明系统的稳定性。 （1）李雅普诺夫直接法。 19世纪俄国数学物理学家李雅普诺夫提出了两种分析非线性系统 稳定性的方法。一种是将非线性系统在平衡点附近线性化，再根 据线性系统特征值分布情况分析系统的稳定性，称为李雅普诺夫 第一方法。另一种是定义李雅普诺夫能量函数，从能量的角度直 接分析非线性系统的稳定性，称为李雅普诺夫第二方法（又称李 雅普诺夫直接法）。李雅普诺夫直接法及其改进方法仍然是目前 非线性控制系统稳定性分析与设计的基本方法。
  • 41.第9章 机器人控制 下面以前面介绍的弹簧质量系统自由振荡平衡点的稳 定性为例说明李雅普诺夫直接法。系统动力学方程为 mx  cx  kx  0 (9－43) 式中，x=0是系统的平衡点。对任意的x，系统的能量函数为 1 2 1 2 V ( x)  mx  kx 2 2 (9－44) 式中，第一项是系统动能；第二项是系统弹性势能。将式(9 －44)对时间求导，并考虑动力学方程式(9－43)，可以得到系 统能量随时间的变化率为   kxx  (mx  kx) x  cx 2  0 V ( x)  mxx (9－45)
  • 42.第9章 机器人控制 . 上式只有x=0时等号成立，否则就是小于零的。因此， 从任何初始状态出发，系统的能量总是随时间下降（耗散） . 的，直到系统达到静止（x=0）状态。当系统静止时加速度 为零，所以根据动力学方程式(9－43)可知  kx=0x=0 (9－46) 因此，系统从任何初始状态出发都将稳定到系统的平衡点。 下面给出一般非线性系统稳定性判定的李雅普诺夫直接法。 给定一般的非线性常微分方程组 X  f ( X ) (9－47) 式中，X是m维矢量；f(X)是任意的非线性函数，假设零点是 系统的平衡点（f(0)=0）。构造广义能量函数（李雅普诺夫 函数）V(X)，满足以下条件：
  • 43.第9章 机器人控制 ①V(X)具有连续一阶偏导数，对任意X≠0都有V(X)>0， 且V(0)=0。 ②V(X)沿任何系统的轨迹对时间的导数满足 V ( X )  0 。 则式(9－47)表示的系统平衡点是稳定的，如果条件②中的 导数除平衡点X=0外均有V ( X )  0，则系统平衡点是渐进稳 定的。
  • 44.第9章 机器人控制 例9－2 . 给定线性系统X=－AX，其中，A为m×m对称 正定矩阵。试用李雅普诺夫直接法证明系统平衡点的稳定性。 解：因为A为对称正定矩阵，所以X=0是系统唯一平衡 点，选取李雅普诺夫函数为 1 T V (X )  X X 2 该函数是正定二次函数，满足前面的条件①，对时间求导得 V ( X )  X T X   X T AX 因为A是对称正定矩阵，上式为非正的，且只有当X = 0 时为零，所以系统的平衡点是渐进稳定的。
  • 45.第9章 机器人控制 例9－3 荡动力学方程 给定具有非线性刚度的弹簧质量系统自由振  x  x  x3 0 ，试用李雅普诺夫直接法证明 系统平衡点的稳定性。 解：x=0是系统唯一平衡点，选择李雅普诺夫函数为 1 2 1 4 V ( x)  x  x 2 4 该函数是正定的，满足前面的条件1，对时间求导得   x3 x  (  V ( x)  xx x  x3 ) x   x 2 该函数是非正的，所以系统将稳定到  x  0 。此 x  0 ，且  时，根据动力学方程知x = 0，因此该系统是渐进稳定的。
  • 46.第9章 机器人控制 （2）带重力补偿PID控制系统的稳定性。 下面采用李雅普诺夫直接法分析工业机器人控制系统的稳定 性。系统动力学方程为 τ  M (θ )θ  C (θ , θ )  G (θ ) (9－48) 采用附加重力补偿项的控制方法 τ  K p E  K d θ  G (θ ) (9－49) 代入到式(9－48)得系统误差动力学方程为 M (θ )θ  C (θ , θ )  K d θ  K p E  0 (9－50)
  • 47.第9章 机器人控制 选择李雅普诺夫函数为 1 T 1 T  V ( X )  θ M (θ )θ  E K p E 2 2 (9－51) 式中，X  [θ T , θT ]T 为系统状态变量，该函数是正定的，满 足前面的条件①，对时间求导得 1 T   V ( X )  θ M ( θ) θ  θ T M ( θ) θ  ET K p θ 2 1 T   θ M (θ )θ  θ T K p E  θ T K d θ  θ T C ( θ, θ )  ET K p θ 2 T  θ K d θ  0 (9－52)
  • 48.第9章 机器人控制 上式推导过程中使用了如下结果： ① ② 1 T  θ M (θ )θ  θT C (θ , θ )  0 2 θT K p E  E T K p θ 第一式是根据机械臂拉格朗日动力学方程的结构得到的结论， 详细的可参考相关文献。第二式中注意到两项都是标量 （数），转置和自身相等，同时Kp是对称矩阵即可得到该 结果。
  • 49.第9章 机器人控制 3.笛卡尔空间控制方法 前面介绍的工业机器人控制采用的都是关节空间的方法， 而实际应用中一般给出的是末端执行器的轨迹，需要采用逆 运动学方法计算。逆运动学计算比较复杂而且一般是多解的。 所以，人们研究了采用基于笛卡尔坐标空间的控制方法。图 9－8给出了其中一种笛卡尔坐标空间控制的实现方法（不需 要位姿测量，只测量关节角）。笛卡尔坐标空间控制的实现 通过系统雅可比矩阵的转置建立笛卡尔空间力和关节空间力 矩的关系，通过系统的正运动学计算出笛卡尔空间坐标值。 笛卡尔空间控制系统的稳定性和性能分析均比较复杂，而且 由于非线性作用，难以保证系统在整个空间上的性能良好。
  • 50.第9章 机器人控制 图9－8 笛卡尔空间控制结构
  • 51.第9章 机器人控制 4.机械臂力控制 前面介绍了机械臂的空间轨迹跟踪控制问题，为了提高 控制的精度和速度，闭环系统一般都具有非常高的刚度。当 考虑机械臂与环境接触的操作任务时，如让机器人抓取一个 生鸡蛋，采用位置控制方法显然是不合适的。因为位置误差 总是存在的，操作的结果经常会是将鸡蛋抓碎或者抓不起来。 完成此类任务的方法就是机械臂的力控制。下面以图9－9 所示简单的弹簧质量系统介绍力控制方法。
  • 52.第9章 机器人控制 图9－9 接触力控制
  • 53.第9章 机器人控制 力控制主要用于机械臂与环境接触问题，接触模型通常 简化为一个弹簧表示接触刚度ke。需要控制的是作用于环境 的力fe，它是施加在弹簧上的作用力。 f e  ke x (9－53) f  mx  ke x (9－54) 系统的动力学方程为 利用式(9－53)，将x及其导数用环境力fe表示，式(9－54)变为 f  mke1  fe  fe (9－55)
  • 54.第9章 机器人控制 采用控制律分解技术令基于模型的控制为f=αf′+β,并选 择   mke1     fe 得到系统控制律 f  mk (  f d  kd e  k p e)  f e 1 e (9－56) 式中，e=fd-fe为期望环境力与实际环境力之差。系统的闭环误 差方程为 e  kd e  k p e  0 (9-57)
  • 55.第9章 机器人控制 同9－4节类似，可以通过选择控制增益系统来使系统具 有期望的性能。一般控制任务是将系统的接触力控制到常数 值，所以系统的控制律简化为 1  f  m (  k d x  k e k p e)  f d (9－58) 图9－10给出了弹簧质量系统力控制误差曲线，仿真中参数选 为质量m=10kg，环境刚度ke=1000N/s，控制律的增益kp=400和 kd=40，期望环境力fd=100N，干扰力fdist=100N。图9－10(b)是 采用PD力伺服控制时，系统的力跟踪误差曲线。从图上可以 看出系统跟踪误差稳定在一个恒定值，即系统存在静差。图9 －10(c)给出了在前面PD控制基础上附加积分项ki=400，即PID 力伺服控制的系统跟踪控制曲线。
  • 56.第9章 机器人控制 图9－10 弹簧质量系统力控制误差曲线
  • 57.第9章 机器人控制 观察图9－10(c) 可以发现，开始阶段系统存在和PD控制类 似的误差，且有一定的超调量，但随着误差积分作用的显现， 经过4s以后系统实现了对期望力轨迹的完美跟踪（误差趋于 0）。图9－10(a)是采用PID力伺服控制时，系统的位移跟 踪误差曲线。可以发现系统位移趋于0.1m，与环境刚度相 乘得到的环境力恰好为期望环境力。
  • 58.第9章 机器人控制 5.机械臂阻抗控制 前面介绍的机械臂力控制方法需要已知环境的刚度，同时还需 要环境力的测量。环境刚度估计是一个比较复杂的问题，同时，力 的传感器测量经常是不方便的，而且难以达到比较高的精度。同位 置测量相比，力测量的代价是比较大的。下面以机器人绘图为例说 明所谓“阻抗控制”的基本思想。图9－11所示极坐标机器人，机 器人可以沿Z轴转动，同时可以沿X轴滑动。通过控制θ和d可以使机 械臂末端按期望的平面轨迹运动（例如直线、圆等）。如果在机械 臂末端夹持一支笔，则可以期望能在工作台的纸上画出相应的曲线。 但是，在实际系统上实现却不是很容易的。因为误差总是存在的， 若笔太高则不能与纸张接触，而笔太低则可能卡住。一种简单的解 决方法是在笔与机械臂末端之间放置一根合适的弹簧，则可以在一 定的误差范围内完成绘制曲线的任务。
  • 59.第9章 机器人控制 图9－11 极坐标机器人
  • 60.第9章 机器人控制 上面的例子不是通过力控制方法来实现笔与纸面的接触 力大小，而是通过弹簧来实现的。该方法是一种被动的力控 制方法。按照这种思想，机器人的力控制可以通过控制闭环 系统自身的刚度来实现。下面仍然以简单的弹簧质量系统 为例介绍机器人的阻抗控制方法。 图9－12所示与环境接触的弹簧质量系统，其中fe为机 器人对环境的作用力。系统的动力学方程为 mx  cx  kx  f  f e (9－59) md  x  cd e  kd e  f e (9－60) 期望动作为
  • 61.第9章 机器人控制 式中，e=xd-x为系统位置误差；md、cd、kd分别表示期望的 质量、阻尼和刚度系数。假设系统的加速度可测，根据式(9 －59)和式(9－60)，可以得到系统的阻抗控制律为 f  (m  md )  x  (c  cd ) x  (k  kd ) x  cd xd  kd xd (9－61)
  • 62.第9章 机器人控制 图9－12 与环境接触的弹簧质量系统
  • 63.第9章 机器人控制 选择合适的期望的质量、阻尼和刚度系数，采用式(9－ 61)的阻抗控制即可使环境力按式(9－60)进行期望的动作。 图9－13给出了弹簧质量系统阻抗控制闭环响应曲线， 仿真中参数选为质量m=10kg，阻尼系数c=20N·s/m，刚度系 数k=100N/m，位置控制设计系统的闭环系统固有频率为 ωn=20，系统的期望轨迹为xd=0。阻抗控制律的期望系统质 量md=10kg，阻尼系数cd=14N·s/m，刚度系数kd=40N/m，环 境刚度ke=10000N/s。
  • 64.第9章 机器人控制 图9－13(a)是分别采用位置伺服控制和阻抗控制时，系 统的位移跟踪曲线，其中细实线表示位置控制，粗实线表示 阻抗控制。从该图上可以看出阻抗控制系统6s左右可以实现 期望轨迹跟踪，比位置伺服控制跟踪速度慢一些。图9－ 13(b)给出了使用位置伺服控制的闭环系统环境力响应曲线， 可以看出最大环境力超过300N。图9－13(c)给出了使用阻抗 控制的闭环系统环境力响应曲线，可以看出最大环境力小于 30N。比较图9－13(b)与图9－13(c)可以发现采用阻抗控制可 以有效减小机械臂对环境的冲击力。为了说明问题，位置伺 服控制设计成欠阻尼系统，因为实际系统总是存在一定的误 差，不可能恰好处于理想状态。
  • 65.第9章 机器人控制 图9－13 弹簧质量系统阻抗控制响应曲线
  • 66.第9章 机器人控制 6.机械臂力/位置混合控制 如图9－14所示3自由度移动关节笛卡尔机械臂，末端手 爪与竖直表面接触。假设关节轴线沿着约束坐标系的三个坐 标轴方向。忽略滑动摩擦，末端执行器与刚度为ke的 表面接触，Yc轴垂直于接触表面。因此，在Yc轴方向需要 力控制，Xc轴和Zc轴方向需要进行位置控制。对于图9－14 所示的笛卡尔机械臂，1轴和3轴采用位置伺服控制，而2轴 则采用力控制。
  • 67.第9章 机器人控制 图9－14 与表面接触的3自由度笛卡尔机械臂
  • 68.第9章 机器人控制 图9－15给出了三自由度笛卡尔机械臂的力/位置混合控 制的一般结构。引入约束矩阵S和S′描述力和位置控制的模 式来控制每个关节。S为对角矩阵，对角线元素为0或1，1表 示采用位置控制，0表示自由不受控制；S′与S的含义类似， 1表示采用力控制，0表示自由不受控制。
  • 69.第9章 机器人控制 图9－15 笛卡尔机械臂力/位置混合控制结构
  • 70.第9章 机器人控制 本混合控制例子的约束矩阵如下 1 0 0  0 0 0  S  0 0 0  , S   0 1 0  0 0 1  0 0 0  同一个方向或者采用位置伺服控制，或者采用力伺服控制， 但只能采取其中之一。即不能同时控制位置和力，所以约束 矩阵满足S+S′=I，其中I是单位矩阵。图9－15给出的力/位置 混合控制的一般结构，可以根据具体操作任务选择约束矩阵， 具有很好的通用性。
  • 71.第9章 机器人控制 下面讨论一般机械臂的力/位置混合控制问题，图9－16 给出了一般机械臂的力/位置混合控制示意图。可以假设任 务是采用通用旋转关节机械臂完成擦黑板动作。在垂直黑板 面的方向（用单位矢量eF表示）需要进行力控制，而在平行 黑板面的方向（用单位矢量eP表示）需要进行位置控制。用 rd和r表示机器人末端手爪的期望位姿和实际位姿，用Fd和F 表示末端手爪对黑板的期望作用力和实际作用力。用Δr表 示机器人末端手爪位姿的误差在eP方向的投影，ΔF表示机 器人末端手爪力的误差在eF方向的投影。则两个误差的投影 可以按以下公式计算 r  [eTP (rd  r )]e p  T  F  [ e F ( Fd  F )]e F  (9－62)
  • 72.第9章 机器人控制 图9－16 一般机械臂力/位置混合控制示意图
  • 73.第9章 机器人控制 位置伺服采用PD控制方法 τ P  K PP θ  K DP θ (9－63) 式中，控制增益矩阵为正定对角矩阵，其上标“P”表示位置 控制。假设机器人的雅可比矩阵是可逆的，则式(9－63)的关 节角度和角速度偏差可以用下式估计 θ  J 1r , θ  J 1r (9－64) 力伺服采用PI控制方法 F P τ F  K τ  K F I  τdt (9－65)
  • 74.第9章 机器人控制 式中，控制增益矩阵亦为正定对角矩阵，上标“F”表示 力控制。利用机器人的雅可比矩阵得到手爪环境力和关节力 矩的关系 τ  J T F （9－66) 注意，式(9－64)利用的是机器人运动学关系计算得到的， 而式(9－66)利用的是机器人静力学关系计算获得的。将位 置控制律式(9－63)和力控制律式(9－65)相加即得到力/位置 混合 τ  τP  τF (9－67) 力/位置混合控制的结构图如图9－17所示。按照该混合控制 策略，可以实现机器人即在垂直黑板面方向用期望手爪力Fd 推压，由同时在平行黑板方向跟踪期望轨迹rd。
  • 75.第9章 机器人控制 图9－17 一般机械臂混合控制示意图
  • 76.第9章 机器人控制 本章重点介绍了机械臂位置控制和力控制方法。采用直 流电机为驱动器、编码器为传感器的位置伺服控制已经相当 成熟，并已经广泛应用于工业机器人系统。而力控制系统由 于结构复杂，同时力传感器技术本身也远没有位置传感器成 熟，所以尚处在研究阶段。