机器人的运动控制

2.4.1 运动控制

对于机器人手臂的运动来说，人们通常关注末端的运动，而末端运动乃是由各个关节的运动合成实现的。因而必须考虑手臂末端的位置、姿态与各个关节位移之间的关系。此外，手臂运动，不仅仅涉及末端从某个位置向另外一个位置的移动，有时也希望它能沿着特定的空间路径进行移动。为此，不仅要考虑手臂末端的位置，而且还必须顾及它的速度和加速度。若再进一步从控制的观点来看，机器人手臂是一个复杂的多变量非线性系统，各关节之间存在耦合，为了完成高精度运动，必须对相互的影响进行补偿。 1. 关节伺服和作业坐标伺服

现在来研究n个自由度的手臂，设关节位移以n维向量q(q1,q2,,qn)Tn 表示，qi是第i个关节的位移，刚性臂的关节位移和末端位置、姿态之间的关系以下式给出：

rfr(q) （1）

rm是某作业坐标系表示的m维末端向量，当它表示三维空间内的位置姿态

时，m=6。如式（1）所示，对刚性臂来说，由于各关节的位移完全决定了手臂末端的位置姿态，故如欲控制手臂运动，只要控制各关节的运动即可。 设刚性臂的运动方程式如下所示：

h(q,q)qg(q) （2） M(q)q)n为表示离心力和哥氏力的向式中，M(q)nn为手臂的惯性矩阵；h(q,q量，nn为粘性摩擦系数矩阵；g(q)n为表示重力项的向量；

(1,,nT)n为关节驱动力向量。

机器人手臂的驱动装置是一个为了跟踪目标值对手臂当前运动状态进行反

馈构成的伺服系统。无论何种伺服系统结构，控制装置的功能都是检测各关节的

，将它们作为反馈信号，最后直接或间接地决定各关节的驱当前位置q及速度q动力。

图1给出了控制系统的构成示意图。来自示教、数值数据或外传感器的信号等构成了作业指令，控制系统根据这些指令，在目标轨迹生成部分产生伺服系统需要的目标值。伺服系统的构成方法因目标值的选取方法的不同而异，大体上可以分为关节伺服和作业坐标伺服两种。当目标值为速度、加速度量纲时，分别称之为速度控制或加速度控制，关于这些将在本节2.和3.中加以叙述。

1

图1 刚性臂控制系统的构成

1） 关节伺服控制

讨论以各关节位移的形式给定手臂运动目标值的情况。

令关节的目标值为qd(qd1,qd2,,qdn)Tn。图2给出了关节伺服的构成。若目标值是以关节位移的形式给出的，那么如图2所示，各个关节可以构成伺服系统，因此问题就变得十分简单。目标值qd可以根据末端目标值rd由式（1）的反函数，即逆运动学（inverse kinematics）的计算得出

qdfr1(rd) （3）

图2 关节伺服构成举例

如果是工业机器人经常采用的示教方法，那么示教者实际上都是一面看着手臂末端，一面进行示教的，所以不必进行式（3）的计算，qd是直接给出的。如果想让手臂静止于某个点，只要对qd取定值即可，当欲使手臂从某个点向另一个点逐渐移动，或者使之沿某一轨迹运动时，则必须按时间的变化使qd发生变化。

为了简单起见，假设驱动器的动态特性忽略不计，各个关节的驱动力i可以

2

直接给出。这时，最简单的一种伺服系统如下所示：

i （4） ikpi(qdiqi)kviqkpi是比例增益，kvi是速度反馈增益。对于全部关节，可以将式（4）归纳表

示为

 （5） Kp(qdq)Kvq式中，Kpdiag(kpi)nn；Kvdiag(kvi)nn。这种关节伺服系统把每一个关节作为简单的单输入、单输出系统来处理，所以其结构简单，现在的工业

机器部分都由这种关节伺服系统来控制。但是，从式（2）中可知，从手臂的动态特性来看，严格地说，每个关节都不是单输入、单输出系统，惯性项和速度项在关节彼此之间存在动态耦合。在式（5）所表示的关节伺服中，这些耦合均被视为外部干扰来进行处理，为了减少外部干扰的影响，在保持稳定性范围内应该尽量将增益kpi、kvi设置得大一些。但无论怎样加大增益，由于重力项的影响，手臂在静止状态下，各个关节仍会产生稳态误差，即将式（5）代入式（6）

q0，将产生下式所示的稳态误差e： 中，若qeqdqKp1g(q) （6）

有时为了使稳态误差为零，可在式（5）中再加上积分项，构成

Ki(qdq)dt （7） Kp(qdq)Kvq式中，Kinn为积分环节的增益矩阵，和Kp、Kv一样，它是一个对角矩阵。 传统上，上述伺服系统是用模拟电路构成的。近年来，由于微处理器和信号处理器等高性能、低价格的计算器件的普及，将伺服系统的一部分或全部改成数字电路的所谓软件伺服已经很普遍了。与模拟电路的情况相比，软件伺服能进行更精细的控制。例如，不再让各个关节的增益kpi、kvi固定不变，而是让其按照手臂不同姿态时所期望的响应特性而变化，用下式代替式（7），通过对重力项的计算，直接实现重力项的补偿

g(q) （8） Kp(qdq)Kvq 后续的内容中，都是在软件伺服假设的前提下展开讨论的。如后面所述，软

件伺服系统方式还能有比式（7）和式（8）更高级的控制方法，但是即使用式（7）和式（8）的简单的控制方法，闭环系统的平衡点qd也能达到渐进稳定，即经过无限长的时间，q能收敛于qd。即在多数场合，式（7）和式（8）的控制方法已经足够了。

3

2）作业坐标伺服控制

关节伺服控制的结构简单，对软件伺服来说，计算量少，采样时间较短，所以是工业机器人经常采用的方法，这一点已经在前面有所论述。但在自由空间内对手臂进行控制时，在很多场合都希望直接给出手臂末端位置、姿态运动的显式表达。例如，让手臂从某个点沿直线运动至另一个点就是这种情况。

在这种情况下，很自然会取末端姿态向量r的目标值rd作为手臂运动的目标值。一旦得到rd，利用上述式（3）变换为qd，当然也能应用关节伺服方式。但是，为此不但需要事前求解末端目标值rd，而且往往要在运动中对其加以在线修正，于是必须实时计算式（3）的逆运动学方程式。此外，因为在关节伺服系统中各个关节是受控的，它们的实际响应结果导致的末端位置、姿态的响应比较难以预测，而且为了得到期望的末端响应，对各关节伺服系统的增益调节也十分困难。

因此，现在我们来研究不将rd变为qd，而把rd本身作为目标值来构成伺服系统。由于在很多情况下，末端位置、姿态rd是用固定于空间内的某一个作业坐标系来描述的，所以把以rd作为目标值的伺服系统称为作业坐标伺服。 下面举一最简单的作业坐标伺服的例子。为此，首先将式（1）的两边对时间进行微分，由此可得下式：

rfrJ(q)q （9） qqTfrqTmn，称之为雅可比矩阵，雅可比矩阵为q的函数。式中，J(q)q和q通常如式（1）所示，为非线性关系。与此相反，由式（9）可知，r为r和q线性关系。式中J(q)是q的函数。

根据式（9）和虚功原理，可得下式：

J(q)Tf （10）

式中，J(q)T表示J(q)的转置，当m=6时，f(fx,fy,fz,m,m,m)T6，是组合向量，包括作业坐标系所描述的三维平移力向量和以欧拉角等描述的r的姿

态所对应的三维旋转力向量，式（10）表示与手臂末端的力和旋转力等效的各关节驱动力的关系式。若取欧拉角(,,)作为r的姿态分量，则m,m,m为绕欧拉角各自旋转轴的力矩，这从直观上非常难以理解。所以，在机器人学中，雅可比矩阵经常不是根据式（9），而是根据速度的关系直接按照下式来定义：

 （11） s(vT,T)TJs(q)q4

在式（11）中，末端速度向量s的姿态分量不是姿态分量的时间微分描述，而是用角速度向量3来表示。不过，在s中，v3是末端的平移速度，和r的位置分量的时间微分一致。式（11）的矩阵Js(q)也称为雅可比矩阵，它表示

之间的关系。虽然它不是从式（9）原本的数学意末端速度向量S与关节速度q义出发的，但是在机器人学中通常称之为雅可比矩阵。

若采用式（11）所定义的雅可比矩阵，对应于式（10）右边的f就成为

f,fxyT,fz,mx,my,mz，f的旋转力分量就变成绕三维空间内某些轴旋转的力矩

向量，这样从直觉就很容易理解。

有了上面一些预备知识，可以用下式给出一个作业坐标伺服的例子：

g(q) （12） J(q)T[Kp(rdr)]Kvq此时对应的控制系统示于图3中，再考虑附加积分环节，即如下式所示：

 （13） J(q)T[Kp(rdr)Ki(rdr)dt]Kvq

图3 作业坐标伺服举例

如果将末端位置、姿态的误差向量rdr分解成位置和姿态分量，用

TTT[eT,e]e[,,]表示，各个分量可以用，来epppoopdddd表示。p3是末端位置向量，pd是目标值，(,,)是欧拉角或横摇角、纵摇角、偏转角，(d,d,d)是其目标值。由式（10）可知，与式（12）、式（13）右边第一项中的Kp有关的项产生的使r与rd一致的潜在的力

fKp(rdr)可视为是施加在末端上的。式（12）、式（13）中手臂末端的当前位置、姿态r可根据当前的关节位移q，由式（1）的正运动学（direct kinematics）计算求得。

5

为了从直观上便于理解，可以认为式（12）、式（13）的方法就是要把末端拉向目标值的方向。另外它还有一个特点，就是不含逆运动学计算。与式（7）、式（8）一样，式（12）和式（13）表明闭环系统的平衡点rd是渐进稳定的。

3）姿态的误差表示

在式（12）或式（13）中，可以用式（11）中的雅可比矩阵Js(q)代替式（9）中的

雅可比矩阵。但此时s的姿态分量无对应的位置量纲来表示（的积分值没有物理意

义），故必须留意末端的误差，即姿态分量rdr的表示方法。现令末端的姿态误差由

基准作业坐标系的姿态矩阵0Rh33给出，即

0Rh[n,o,a] （14）

式中，n,o,a3表示姿态矩阵中的列向量，它们是基准坐标系表示的末端坐标系中x轴，y轴，z轴方向的单位向量。姿态目标值也可以用姿态矩阵的形式来表示，即

0Rhd[nd,od,ad] （15）

在式（12）或式（13）中，如果用雅可比矩阵Js(q)，则rd出的e0代替e0：

r的姿态向量可以用下式给

1e0[nndoodaad] （16）

2从而得到与式（12）对应的式子如下：

TTJs(q)T[Kp[eT,epo]]Kvqg(q) （17）

同样，用式（16）所定义的e0，式（13）可以变形为

e0ksin （18）

式中，k是从0R转向0Rhd的等效旋转轴方向的单位向量（图4）；表示绕此轴的旋转角。e0即是指向k方向的、大小为sin的向量。若用e0表示姿态的误差，虽然姿态的误差角超过2后e0的模反而会变小，当时变为0，会产生错误的结果，但是，如果假设姿态误差不太大，如在22的范围内，那就没什么问题了。

6

图4 等效旋转轴

若用欧拉角（或横摇角、纵摇角、偏转角）表示姿态，则式（10）中f对应的姿态分量在直观上就变得难以理解了，而且在表现奇异点方面也会出现问题。用式（16）定义的e0虽然在直观上容易理解，在表现奇异点方面也没有问题，但是只有在姿态误差小的条件下才有效。因此，这里最后介绍采用四元数（quaternion）的姿态误差的表示法。四元法作为欧拉参数（Euler parameters）为人们所熟知。设从基准姿态向某一个别的姿态0R1变化的等效旋转轴为u，绕

ˆ4用下式定义： 该轴的旋转角为，则四元数qˆ[,T]T q （19）

22在式（19）中要注意，等效旋转轴的向量u无论是从基准坐标系，还是从用

010。这时，旋转矩阵R1表示的坐标系，它的表达都是相同的，即0RuRuuR110cos，usinˆ具有如下式所示的关系： 与四元数q0R1(221)I2(T[]) （20）

式中，[]为与向量的外积等效的变形对称矩阵；I为33单位矩阵。式（20）也可反过来应用，即给出四元数，求解与之对应的旋转矩阵。

本书将与当前手臂末端姿态0Rh及其目标姿态0Rhd所对应的四元数分别定义

TTTTˆhd[hd,hdˆh[h,h]。于是，与从末端姿态0Rh到目标姿态0Rhd的]和q为qˆ[等效旋转相对应的四元数q]T可以利用下式求出：

7

Thhdhhd （21）

hhdhdhhd （22）

式中，这时的等效旋转轴用0Rh或0Rhd表示的坐标系来描述。因此，若考虑用基准坐标系描述该等效旋转轴向量，设为ksin下式给出：

2(0Rh0Rhd)，则可用

hhdhdh  （23）

要注意的是，与仅第三项的外积符号不同。这里使用式（23）给出的代替前面的e0，虽然特性上它们同样是非线性的，但即使姿态的误差角超过

2，仍呈单调增加。

2. 速度控制

在1.中就关节伺服和作业坐标伺服的有关内容作了说明，手臂的目标值是以位置量纲给 出的。但是，有时手臂作业不用末端的位置和姿态来指定，而改成命令它从当前的位置向某一个方向移动，例如手臂末端从当前位置垂直向上运动，或者只绕规定轴旋转变化姿态，这相当于使用操纵杆操纵遥控机械手的情况。对于这种类型的运动指令，虽然也允许用位置量纲的目标值给出，但必须沿着末端目标值运动的方向时时刻刻改变目标值。在关节伺服的场合还必须对每个末端目标值根据式（3）进行一次逆运动学计算，以求得关节目标值，显然为此将花费很多计算时间。对于这种运动指令，人们很自然地想到把末端速度作为目标值给出。

或s与关节速度q之间具有如式（9）或式（11）所示的线性关系。设rd或sd末端速度r为末端速度的目标值。假设手臂无冗余性，也不存在奇异状态，于是m=n，式（9）或式（11）

d或sd的关节速度qd(qd1,,qdn)Tn可由下式的雅可比矩阵为正则矩阵，这时实现r求出：

dJ(q)1rd （24） q或

dJs(q)1sd （25） q如果手臂具有冗余性，即n>m时，或者手臂处于奇异状态下，不存在雅可

比矩阵的逆矩阵，那么就无法直接应用式（24）或式（25）。在实际的计算中，与其按式（24）或式（25）直接求解雅可比矩阵的逆矩阵，不如把式（24）或式（25）看作是雅可比矩阵，写出系数矩阵的联立代数方程，然后用消去法去求解

d，从计算量的角度来看后者会更有利些。 r我们可以把式（24）或式（25）视为把末端运动分解成必要的关节运动，故

8

称之为分解速度控制（RMRC：Resolved Motion Rate Control）。式（24）、式（25）的目标是速度，与其说是这些式子本身在实施控制，倒不如将其视为以速度量纲进行逆运动学计算更妥当。和式（3）一样，应该把它们的作用看成是把末端空间的目标值变换为关节空间的目标值。如果在各个关节处具备跟踪目标速度

di的速度伺服系统，那么只要把式（24）或式（25）所求得的qd的各个元素作为各个关节q伺服系统的目标值即可。因此，这种情况可以说是利用关节伺服进行的速度控制。图5给出了此时控制系统的构成。

图5 关节伺服的速度控制举例

另外，式（24）和式（25）中的各个关节伺服系统也适用于本节1.中所涉及的把关节位

d(t)给出，而且关节目标值的初移qdi作为目标值的情况。设末端速度的目标值用时间函数r始值为rd(t0)，则在时刻t的目标值rd(t)为

d(t)J(q(t))1rd(t) （26） qd(v)dv （27） qd(t)qd(t0)qt0t用式（26）和式（27）计算qd(t)，相当于式（3）的逆运动学计算的数值计算，这个方法对用式（3）的无解析解的手臂尤其有效。不过，若反复用式（26）、

式（27）进行计算，存在着与目标位置之间的位置积累误差增大的问题。为了解决这一问题，只要在式（26）中加上位置反馈即可，如下式所示：

dKp(rdr)] （28） dJ(q)1[rq对含雅可比矩阵Js的式（25）也可以采用同样的办法，不过此时应该将对应于s的姿态分量的末端姿态误差改换成式（16）中的e0，或者用式（23）的

表示，如下式所示：

TTdJs(q)1[sdKp[eTq,epo]] （29）

d的，因此将qd作在式（28）或式（29）中，是从末端误差rdr的角度来求解q为各个关节速度伺服系统的目标值进行控制的方法，无法明确地区分是属于关节

9

伺服还是属于作业坐标伺服。

3．加速度控制

前面就将目标值设定为位置或速度的场合的手臂伺服系统的构成做了介绍。但是在关节

d，实际的响应情况仍然被各个关节伺伺服的情况下，即使给出正确的目标值qd、q服系统的性能所左右。通常的做法是在保证稳定的情况下调大增益，减小与目标值的偏差，然而手臂的运动速度越快、加速度越大，则离心力、哥式力、惯性力和关节轴间耦合的影响也就越大，误差也越严重。即使保证作业坐标伺服的式（12）和式（13）具有目标值的渐进稳定性，也无法保证过渡特性的好坏，而且随着手臂姿态的不同，响应特性还可能会发生变化。这些问题之所以产生，是因为在迄今所考虑的控制策略中，并未涉及手臂动态特性的式（2）所致。因此，在本小节中所叙述的方法是将目标值再追加加速度，即考虑手臂的动态特性，以显式的形式给出过渡特性的要求。

d、qd，即包括关节变量加速度，这时考虑采用如首先，设目标值为qd、q下的控制方法：

dKv(qdq)Kp(qdq)]h(q,q)qg(q)（30） M(q)[q)、M(q)、h(q,q这是关节伺服加速度控制，图6表示伺服系统的结构。在式（30）中，、g(q)均和式（2）中的意义相同，式（30）相当于进行逆运动力学（inverse

adjdynamics）计算，以求出能实现由qdKv(qdq)Kp(qdq)所给出的q)等的值可以正确计算。关节加速度的关节驱动力。为简便起见，先假设M(q)、h(q,q把式（30）代入式（2）的左边整理后，得

KveKpe]0 （31） M(q)[e式中，eqdq。因为M(q)是正定对称矩阵，故两边乘上M(q)1后得到

KveKpe0 （32） e

图6 用关节伺服的加速度控制

10

适当选择位置增益Kp、速度增益Kv，可以使e渐进收敛于0，使q与qd达到一致。瞬态响应特性可由Kp、Kv来确定。例如，设KvkvI、Kp位矩阵），若kv2n、kpkpI（I是nn的单

2n，则式（32）的响应是角频率为n、阻尼系数为的

二阶系统响应。控制手臂运动时，一般不希望出现超调量，所以通常取1。这样在加速度控制中之所以瞬态特性也能被满足，是因为通过离心力、哥氏力、重力等的补偿，使非线

性手臂的动态特性被线性化，同时考虑了惯性项使系统解耦的结果。因此，可以将式（30）的加速度控制视为本节4.中打算加以说明的动态控制（dynamic control）的一种方式。

式（30）所示的加速度控制由于能够给定瞬态特性，所以这种控制策略非常有效，不过也应该指出它的问题是计算量非常大。为了缩短采样时间，可将式（30）的部分计算省略，采用下面简化的公式：

[qdKv(dq)MqKp(qdq)Kiq(dqdt) ] （33）

nn)、q、g(q)的计算，M式（33）中省略了h(q,q被M(q)取代，简化

的对角元素最好选择M(q)的对角元素的代表M为仅由对角元素构成的常数矩阵。

值。此外，为了消除稳态误差，在式（33）中重新补上了积分项。各种工业机器人减速比的数值比较大，故驱动器惯性的影响也会比较大，所以M(q)的对角元素比非对角元素大得多，而轴间耦合的影响相对变小。因而在很多情况下，采用

式（33）的近似控制策略能够满足计算的要求。

d、r下面讨论目标值为rd、rd的情况，此时目标值包括了末端位置、姿态的

加速度。为此，首先应该求出末端加速度和关节加速度之间的关系。将式（9）

的两边对时间进行微分，即可得

(q)qJ （34） J(q)qr(q)表示d[J(q)]dt。 式中，Jr 为了跟踪目标轨迹rd，把根据rd所修正的末端加速度adj以下式给出：

dr)Kp(rdr) （35） radjrdKv(r是当前的末端速度，它由传感器测得的关节速度q式中，Kv、Kp是适当的增益矩阵；r用式（9）求得。从式（34）可以求得实现给定的末端加速度radj的关节加速度为

adjJ(q)1[qradjJ(q)q] （36）

11

再由下式求出实现该关节加速度的关节驱动力：

adjh(q,q)qg(q) （37） M(q)q将式（35）~式（37）归纳为一个式子，得到

M(q)J(q)1[rdKv(rdr)Kp(rdr)J(q)q]h(q,q)qg(q)（38）

这就是作业坐标伺服的加速度控制，它与式（30）给出的关节伺服加速度控制是相对应的。我们假设正确地完成了式（37）的逆动力学计算，即M(q)、

)、、g(q)的计算结果正确，把式（37）代入式（2）中后可得下式： h(q,qqadj （39） qadj一致。把式（39）代入式（36）中后得即实际响应与给定加速度qrradj，再把它代

入式（35）中后可得

rKverKper0 （40） e式中，errdr。式（40）是与关节伺服的式（32）相互相对应的。式（35）~

式（37）给出的作业坐标伺服，是为了在末端产生期望加速度而求解出各个关节的加速度，因而称其为分解加速度控制（resolved acceleration control）。它可以视为是由上述分解运动控制向加速度的扩展。但必须注意的是，分解速度控制只是为了把末端目标速度向关节速度变换，而分解加速度控制则要考虑当前值和目标值之间的误差，并将之视为伺服系统的一部分。另外，在分解速度控制中，实际上无法保证分解的各个关节目标速度正确地被实现，但在分解加速度控制中，若正确掌握了手臂的动态特性模型，则如式（39）所示，指令加速度将得到正确的实现。图7表示了分解加速度控制的总体结构。

12

图7 分解加速度控制

d，pd、Rhd给出，于在式（36）中，若用Js(q)代替J(q)，则目标值以sd，s是可用下式代替式（35）和式（36）：

TTdKv(sds)Kp[eTadjJ(q)1[sqp,eo]Js(q)q] （41）

这就是原来由Luh等提出的分解加速度控制的方案。另外，也可以将姿态分量的误差置换为式（23）定义的，以代替e0。

如前所述，若姿态误差用e0来表示，则在产生特大姿态误差时增益会等效下降，瞬态特性恶化。用虽然可以改善瞬态特性，但误差是非线性的这一条并无改变。

但是无论在什么情况下（用e0时除了，理论上是保证姿态逐渐的特殊情况以外）

收敛于目标姿态。另外，如有必要，也可以用四元数让姿态误差的响应特性成为线性关系。

4. 轨迹控制

本小节中，将首先讨论目标轨迹的给定方法，然后介绍手臂高精度跟踪目标轨迹的方法，即动态控制的方法。

1） 轨迹控制方式

如果想让机器人手臂末端沿目标轨迹运动，有两种给定轨迹的方式：一种是所谓示教机器人中采用的示教再现方式；另一种是把目标轨迹用数值形式给出的数值控制方式。

所谓示教再现方式是在执行作业前，让手臂末端沿着实际目标值进行移动，同时将相应的数据和作业速度等其他信息一起存入机器人中，在执行时将所示教的动作再现，于是手臂末端就沿目标轨迹运动。示教时手臂运动的方式也分为两种：一种是直接示教方式（操作者直接用手握住机器人手臂末端使其做动作）；另一种是远距离示教方式（用示教盒的按钮或

13

开关发出运动指令）。

在示教再现方式中，记忆再现轨迹的方式通常有点位控制（PTP control: Point-To-Point control）和连续路径控制（CP control：Continuous Path control）两种（图8）。

图8 PTP控制和CP控制

（1）PTP控制 例如，点焊等作业，人们关注在示教点上对末端的位置和姿态定位的问题。至于向该点运动的路径和速度等则不是主要的问题。这种不考虑路径，而是一个接一个地在示教点处反复进行定位的控制就是PTP控制。

（2）CP控制 例如，弧焊、喷漆等作业，必须控制机器人以示教的速度沿着示教的路径进行运动。这样的控制称为CP控制。按示教的方法不同CP控制又分为两种：其一是示教时让机器人沿着实际的路径运动，并每隔一个微小的间距就大量记忆该路径上的位置，再现时把所记忆的点一个接一个地作为伺服系统的目标值给出，以达到路径跟踪的目的；另一个是和PTP控制一样，示教时只记忆路径上的主要点，再现时则在这些主要点之间用直线或圆弧来插补，计算出每个微小间距的路径上的点，再把它们输出给伺服系统（图9）。后者和前者相比，需要记忆的点数较少，路径修正也比较容易，因而系统具有灵活性，但必须对其进行插补修正。

（a）圆弧插补 （b）直线插补

图9 带插补的CP控制

所谓数值控制方式，它和数控机床一样，是把目标轨迹以数值、数据的形式给出。所谓数据，是把作业对象的CAD数据、在实施控制中所得到的来自传感器的测量数据等各种数据经过变换后给出的。可见，数值控制方式比单纯的再现示教轨迹的示教再现方式更具有一般性、通用性、灵活性。然而，把目标值以数值的形式给出可能会导致计算时间过长，出现机器人装配误差、每台设备本身的分散误差带来新的问题等麻烦。

2）目标轨迹生成

14

在PTP控制和CP控制中，轨迹是由示教直接给出的，而在数值控制方式中，目标轨迹，即输出给伺服系统的目标值的时间函数必须以数值的形式给出。随着机器人手臂面临的作业不同，作业空间内的末端轨迹不一定非要从起点至终点在整个区间内都要预先给定，有时仅给定起点和终点，有时仅给定起点、终点及路径所经过的若干中间点即可。在这种情况下，必须人为设定未给定区间内适当的轨迹。下面我们就来研究，当手臂末端由某个位置（包括姿态，下同）r0历经某一时间tf移向另一个位置rf时，如何确定r0和rf之间轨迹

的问题。实际上，这个问题也适用于CP控制中的插补。

在求目标轨迹时应该注意的是，为了生成实际可行的光滑轨迹，至少应保证位置和速度的连续性条件。另外，为了不使末端产生不必要的振动，还希望能够保证加速度的连续性条件。关于目标轨迹生成的方法，目前已有许多种方案，本节只介绍利用时间多项式给定轨迹的方法。这个方法进一步可分为用关节变量描述轨迹和用末端位置变量描述轨迹两种，它们分别对应于关节伺服和作业坐标伺服。

首先就关节变量的方法加以讨论。设对应于r0和rf的关节变量q0和qf已经给出。若只给出r0、rf，则可以求解逆运动学方程，预先求出q0、qf。任意选一个关节变量qi记作，令初始时刻0的值为0，终点时刻tf的值为f，即

(0)0，(tf)f （42）

再将另外两个时刻的的速度和加速度作为边界条件以下式表示：

(t) （43） (0)，ff0(t) （44） (0)，ff0虽然满足这些条件的光滑的函数很多，但考虑到简化计算和形式简单，本节

选择时间t的多项式。能满足任意给出的式（42）~式（44）的边界条件的多项式，其最低次数是应该为5，所以设

(t)a0a1ta2t2a3t3a4t4a5t5 （45）

经过计算，满足式（42）~式（44）的待定系统a0a5结果如下：

a00 （46）  （47） a11a210 （48） 2a3

112)t(3)t2] （49） [20f200(8f0f0ff32tf15

a4a5116)t(32)t2] （50） [30030f(14f0f0ff42tf16)t()t2] （51） [1212(6f0f0fff2t5f0、、、满足如图10所示的关系，即 、特殊是，若00fff0f0tf2) （52） (0f时，a50，于是(t)为4次多项式。

图10 起点时刻和终点时刻的边界条件

若把这个4次多项式和直线插补结合起来应用，就可以比较容易地给出各种轨迹。例如，下面讨论一个情况，即从起点0的静止状态开始，经加速、等速、减速，最后到点f停止。如图11所示，先适当选择决定加减时间的参数，然后确定中间的辅助点02、f1。在这里是这样确定的，首先把01、f2取为

010、f2f，然后让02和f1处于01和f2相连的直线上。接着用折线

001f2f把0、02之间和f1、f之间的各个端点连接起来，并用4

次多项式的关系使其加速度为0。再将02、于是0t2f1之间以直线相连接。为加速区间；2ttf2为等速区间；tf2ttf为减速区间。

16

图11 从起点到终点的轨迹

再讨论用多项式连接0和f的情况。若只考虑位置、速度的边界条件（式（42）、式（43）），而不考虑表示加速度连续性的式（44），那么目标轨迹可以不

满足式（52）、用式（45）给出，而用t的3次多项式给出，特别当0、f、f0时，(t)变为2次多项式。即有时3次多项式和2次多项式也可以代替上述的4次多项式。

上面介绍了关节变量的方法。不过若用这种方法确定r0和rf之间的轨迹，就可能难以预测末端将沿着什么样的轨迹进行运动。另外，有些场合要求给定末端轨迹本身，例如作业希望末端沿直线移动。在这种情况下，就可以选择由末端位置变量来确定r0和rf之间轨迹的方法。有许多种方法可以用来描述末端移动位置和姿态的6个变量，一旦选定某种方法，确定了一组变量后，剩下的就完全可以用与关节变量同样的处理方法来确定各个变量的目标时间轨迹。即在6个变量中任选一个，令其为，并假定一个如式（45）所示的时间多项式，然后求出满足边界条件的系数即可。这样就得到沿直线路径运动的目标时间轨迹，而该直线连接六维欧几里德空间内表示末端位置的两个点r0和rf。

表示末端移动位置的3个变量，常常用末端坐标系原点相对于基准直角坐标系的坐标来表示。在上述目标轨迹决定法的例子中，末端坐标系的原点位置做直线运动。关于末端的姿态，若取描述姿态的三个变量为欧拉角或横摇角、纵摇角、偏转角，则直线运动也与这些变量有关。既然末端姿态的运动是在基准直角坐标系内所表示的，却要由围绕3个斜交的轴旋转来合成，这样显然造成了直观上理解的困难。为了改进这个问题，可以将对应于r0的末端坐标系姿态矩阵设为

O0，将对应于rf的末端坐标系的姿态矩阵设为Of，于是能求出从O0转向Of的等效旋转轴kf和转角f，若用式（45）来描述姿态目标轨迹绕kf轴由0旋转至

17

f的目标轨迹，那么从基准坐标系来观察该轨迹，就得到一条围绕唯一的、方

向固定的旋转轴kf旋转的轨迹，这样从直观上就变得容易理解了。

3）动态控制

在示教再现方式中，目标轨迹通常是以各个关节变量的形式来记录的，因而手臂的控制依赖由每个关节伺服回路组成的关节伺服就足够了，其构成也很简单。但在这种情况下，离心力、哥氏力、重力等引起的各个关节之间的耦合是作为外部干扰来处理的，往往无法满足高速、高精度控制的要求。如在本节2.中所

d(t)、rd(t)和加速度述，当目标轨迹以时间函数qd(t)或rd(t)给出时，速度qd(t)、qrd(t)也可以通过解析法求得。在这种场合采用涉及手臂动态特性的高级

控制策略更为可取。

所谓动态控制法（dynamic control）或计算转矩法（computed torque method）就是一种考虑机器人手臂动态特性，并能实现给定目标轨迹的控制方法。下面将介绍它的两级控制方式，即针对非线性系统的线性补偿和针对线性系统的伺服补偿方法。设机械手的运动方程式表示为

h(q,q)qG(q) （53） M(q)q要注意的是，现在可以把[qTT]T作为这个系统的状态变量，将uq作为新的输入，并按下,q述关系进行非线性状态的反馈补偿：

ˆ(q,q)M(q)uq （54） h式中，

ˆ(q,q)h(q,q)qG(q) （55） huq （56） q这是一个对于关节变量来说的线性解耦系统。也就是说，式（54）完成了线性补偿。如

于是可得

d(t)以果式（54）中不含有建模误差，而且在系统中未混入外部干扰，假设将目标加速度quq给出，则它能完全实现q(t)qd(t)的目标轨迹。但是，由于建模误差和外部干

扰是无法避免的，所以一个基本的方法就是通过设置对式（56）线性系统的伺服

补偿器来减少上述因素的影响。图12所示即为该系统的方框图。

18

图12 关节变量线性化和伺服补偿的两级控制

例如，考虑设置如下式所示的补偿值：

dKv(qdq)Kp(qdq) （57） uqq若将误差e定义为

eq （58） dq由式（57）和式（58）可得

Kpe0 （59） Kvee若取KvKp为对角元素是正值的对角矩阵，即对每个关节设置了PD动作反馈回路，

则e的各个元素均收敛于0。那么，即使有建模误差和外部干扰，也能够在某种程度上减少

其影响

以上是基于有关关节变量的线性化的方法。但是，有些场合并非要求关节变量，而是希望针对机械手作业直接相关的变量，如末端位置、姿态等加以解耦和伺服补偿。所以，下面讨论对n维输出变量r线性化的方法，此时r由下式给出：

rR(q) （60）

若对式（60）进行微分，可得

J(q)q （61） r式中，J(q)是以J(q)Rq给出的雅可比矩阵。假设在q的适当范围内雅可比矩阵

TJ(q)是正则矩阵，若将ur作为新的输入，并按下式关系进行非线性状态反馈补偿：

(q,q(q)q)M(q)J1(q)[Jur] （62） h则得到

ur （63） 即得到对输出r的线性解耦系统，于是和式（56）的情况一样，若对线性系统式（63）设置

适当的伺服补偿器，则可以得到如图13所示的控制系统。这样的伺服补偿器有很多种设计方法，如选用下式：

19

dr)Kp(rdr) （） urrdKv(r则当erdr时，就与式（59）相同。值得指出的是，它的控制原则与本节

3.中所述的

分解加速度控制的形式相同。选用这种两级控制方式时必须考虑的因素是，由于式（54）或式（62）的计算非常复杂，所以必须使用数字计算机进行，因而就产生了如何缩短采样周期的问题。另外，还要尽可能减少原数学建模的建模误差和外部干扰的影响，即必须设计所谓鲁棒伺服补偿器。前者可以考虑选择牛顿-欧拉计算公式的逆运动学问题计算来解决。后者就要参考用2个自由度伺服系统的研究方法或灵敏度函数、互补灵敏度函数的研究方法来解决。

图13 输出的线性化和伺服补偿的两级控制

5.解耦控制

在本小节中，将介绍非线性系统的解耦控制理论，并把它用于机械手，然后介绍同时进行动态特性线性化、输出变量间的解耦和任意极点配置的方法。这个理论给出了在本节4.中所介绍的两级控制中线性补偿的理论根据，所以是很有意义的。

现在我们来研究n输入、n输出的非线性时变系统，该系统如下式所示：

x(t)a(x,t)B(x,t)u(t) （65）

y(t)c(x,t)D(x,t)u(t) （66）

式中，x为ns维状态向量；y为n维输出向量；u为n维输入向量。另外，a(x,t)、

B(x,t)、c(x,t)、D(x,t)是已知向量或矩阵。在该系统中，通过实施如下所示的适当的非线性时变状态反馈控制律：

,t) u ()G(x u(t)e(x,t t （67）

(t)输出y的各个元素不但可以相互解耦，而且具有任意指定的极点。式（67）中u为新的n维输入向量。

c(x,t)和y的第i个元素为ci(x,t)和yi，假设D(x,t)的第i行向量为di(x,t)，

20

另外，设yi对于时间的j阶导函数为yi(j)。这时，若反复就时间对yi进行微分，并用式（65）和式（66）整理，直至u的系数不为0为止，则对适当的正整数vi，可得如下关系：

（68） 1,vi, 1yi(j)ci[j](x,t) (j0,yi(vi)ci[vi](x,t)di[vi](x,t)u （69） 式中，yi(0)yi，ci[0](x,t)ci(x,t)，di[0](x,t)di(x,t)

ci[j](x,t)(j1)(j1)yiTyia(x,t) （70） txdi[j](x,t)Tyi(j1)B(x,t) （71）

x另外，vi(i1,2,,n)是直至u开始出现为止的微分次数，并假定在被研究对象系统

中，无论x，t的值如何，vi都是一定的。再设

[v1]c1(x,t)[v2]c(x,t) （72） c*(x,t)2[vn]c(x,t)nd1[v1](x,t)[v2]d(x,t) （73） D*(x,t)2[vn]d(x,t)n(x,t)[c(x,t)]col[ikyi(k)] （74）

*(k)ikik0k0vi1vi1diag[1,2,,n] （75）

式中，col[]为列向量；diag[]为对角矩阵；ik、i为任意常数。这时假如D(x,t)是

*正则矩阵，在式（67）中若规定

**e(x,t)D*1(x,t)c(x,t)(x,t) （76）

G(x,t)D*1(x,t) （77）

则系统对于输出y就是一个解耦系统。实际上，如果在式（65）和式（66）中使

21

用式（67）、式（76）和式（77），即可得到下式：

i (i1,yi(vi)i(vi1)y(vi1)i0yiiu2,n, （78）

在此，适当地选择ik、i，就可在解耦后的各个单输入、单输出系统中给出任意的极点配置和增益。

下面我们将上述理论应用于机械手。将手臂运动方程式（53）表示为

ˆ(q,qh) （79） M(q)qˆ(q,q)为式（55）所给出的函数。另外，如果将任意的n维作业向量r作为输出，式中，h它满足下列关系：

yrR(q) （80） rJ(q)q （81） y再有，若把驱动力看成是输入，在这种情况下，取[q变成如下形式：

TT]T作为状态向量，则式（79）,qq0x1 （82） 1ˆ)M(q)h(q,qM(q)对于式（80）、式（82），vii(2,1,)n，则有

D*J(q)M1(q) （83）

我们把rankJn的手臂姿态（用q表示的整个手臂的位置、姿态）称为奇异姿态（奇异点），对于除奇异姿态以外的手臂任意姿态，式（83）中的D为正则矩阵，并且能够实现解耦。根据式（67）、式（76）和式（77），若设Ji(q)为J(q)的第i行向量，则可以求得如下式所示的状态反馈律：

*ˆ(q,qii0yiiui] （84） )M(q)J1(q)col[Ji(q)qi1yh根据式（84）可以求得如下式所示的具有线性二阶系统特性的解耦系统：

ii1yii0yiiui (i1,y2,n, （85）

式中，i1、i0、i为可由设计者确定的常数。此外，上面表示解耦控制反馈律

ˆ(q,q)这一点上，与动态控制的式（84），在补偿表示离心力、哥氏力、重力等的h的作用是相同的，进而可以认为在图13中具有如下式所示的伺服控制器：

ii0yiiui] （86） uycol[i1y22

此外，在不能忽视驱动器动态特性的情况下，也能够应用解耦控制理论。例如，在考虑直流伺服电机自感的情况下，可得到具有三阶系统特性的解耦系统，而非如式（85）所示的二阶系统。 6.冗余手臂的控制

人类的手臂具有7个自由度，如果仅仅是用手抓住物体并使其固定，那么6个自由度就足够了，因而有1个冗余自由度。但正是由于有了这种冗余性（redundancy），才增加了手臂的柔顺性和通用性。机器人手臂也同样，当它的自由度大于作业所需要的自由度时，手臂具有冗余性。例如，如图14所示，若用平面运动的3个自由度手臂，使其末端位置与某目标值重合时，作为手臂整体可以取无限多的状态。这是因为存在1个冗余自由度所致。积极地利用这一点，可以实现许多功能，例如一面使手臂末端跟踪目标位置，一面使其又不与障碍物相碰；使末端能伸到狭小的孔穴深处；使各个关节的驱动速度与加速度尽量一致；使手臂保持便于操作末端执行器的姿态等。

图14 在平面上运动的3个自由度的机器人

人们针对冗余机器人手臂的控制规律提出了许多方案，举例来说它们有基于以速度级、加速度级、转矩级的公式化方法，另外还有局部优化方法（分别对每个瞬时求出该时刻的最优解）和全局优化的方法（涉及整个控制空间优化）。以下我们来说明基于速度级的瞬态优化法。该方法首先用雅可比矩阵描述手臂的关节速度与末端速度之间的关系，再用雅可比矩阵的伪逆矩阵（pseudo inverse matrix）以线性联立代数方程组的任意解的形式来表现手臂的冗余性。然后，根据冗余性的应用目的来决定该任意参数向量。

假设用rR(q)来描述所给出的冗余手臂的n维关节变量q与m(n)维末端变量r

之间的关系。若将它进行时间微分，则得到

J(q)q （87） r后，我们来求满足式（87）的关节速度q的式中，J(q)为雅可比矩阵。在给出末端速度r及q的单通解。首先做一些准备工作。为了使求出的解不受广义坐标q及r的取法或表示r位的形式的影响，选择

rMrTMrr)12，以及q(rMqTMqq)12，作为评价r及q(q大

小的范数。式中，Mr及Mq为任意选择的正定对称矩阵。

23

当末端能够在三维空间中采取任意姿态时，为了表示姿态，一般采用欧拉角等三个角度的变量组。但是，这种情况下，手臂机构尽管能够绕任意轴旋转，但式（87）中的雅可比矩阵J不是全秩矩阵，可能会存在无法描述旋转姿态的情况。人们将这种情况称为描述上的奇异点。避免出现该问题的一个方法是，改用向量v表示末端速度，该向量用角速度向量代

中的旋转速度分量，这时，式（87）可以改写成 替r （88） vJv(v)q作为范数则改成

vMv(vTMvv)12，v

的关系除了表示上的奇异点以外，由于利用适与rTTrvMvTrv就

，因此系数矩阵Mv与Mr的关系利用Mr当的正则矩阵Trv表示为vTvrr能给出。

TTMr及Mq为正定对称矩阵，因此存在满足MrTrTr及MqTqTq的正则矩阵Tr及

及q进行归一化（无量纲）处理： Tq。利用这些关系式和下式将rˆˆTqq （） Trr，qr则式（87）变成

ˆˆˆˆTJT1 （90） Jq，Jrrqˆˆˆ)12rˆˆ(rTr和q满足rr的通解可由下式给出：

Mrˆq及qˆ的伪逆矩阵Jˆ，。因而，利用J式（90）Mqˆ(t) （91） ˆˆrˆ(IJˆJˆ)kJqnˆ(t)为任意的n维向量，表示手臂的冗余性。另外，将式（91）式中，In为n阶单元矩阵；k，可表示为 及r改用原来的qJr(InJJ)k(t) （92） q式中，

ˆTT1(TJT1)T （93） JTq1Jrqrqrˆ(t)。在J为全秩(rankJm)时，式（93）中的J可简化为 k(t)Tq1k1T1T1JMqJ(JMqJ) （94）

式（91）或式（92）是利用冗余性的基本式。要注意的是，它们是在冗余自由度条件下分解速度控制方式的一般形式。

现举例说明这些基本式子的应用。在图14中，假设时刻t=0，手臂处于实线位置q0处，

0保持不变）的条件下，想要改变手臂姿态时，只要将从下式求在不改变末端位置（即r24

作为各个关节的目标值即可： 得的q(InJJ)k(t) （95） q在式（95）中，适当选择k(t)，就能够使手臂运动至图14中的虚线或短划线的位形。 再举一个例子，说明既让末端跟踪目标轨迹rd(t)，又借助于冗余性回避奇异点的具体应用。例如，在图14中，当手臂的q2和q3取0或180时，无法使末端执行器沿连接第2关节与第3关节的直线方向移动。这样的位形是手臂奇异姿态。有一个指标可以表示接近

ˆ(q)由下式表示的非负函数： 手臂奇异位形的程度，叫做可操作度。这是利用Jˆ(q)JˆT(q))12 （96） w(detJˆˆˆ计算的体积成正比，则是在关节速度q1的范围内 它的数量与以范数r而范数rˆ所构成的集合（称为可操作性椭圆体）全部能够实现的末端速度r。

w也表示为

1TwdetTr[detJ(q)MqJ(q)]12。可操作度w的值在奇异点为0，在手臂的其他姿态下

为正。所以，若利用手臂的冗余性进行控制，尽可能保持较大的w值，就有利于回避奇异

ˆˆ，取k(t)为： r点。因此，在式（92）中，设rd1k(t)Mq(t)kp，(t)w （97） q由下式给出： 式中，kp为适当的正常数。于是可操作度的变化速度w(wwT1TJ(q)rdT(InJJ)Mq)qkp （98） Tq进一步，利用式（93），则式（98）右边第二项成为

111TT(InJJ)MqkpTMq(IJJ)TMq(IJJ)(Mq)kp0 （99）

d的同时，因而可知，在实现手臂末端目标速度r利用冗余性，可以让手臂的位形沿着使w值

增大的方向运动。

对于图14中的手臂，将采用上述方法的结果示于图15中。另外，如果取

q[q1q2q3]T、r[xy]T。设l1l21、l30.3、TqI3、TrI2，末端目

标轨迹的要求是，在1S内从初始位形q0[203020]T沿着与Y轴平行方向向下运

动至y0.1。图15（a）是未利用冗余性的情况，图15（b）是利用冗余性的情况。显然，

25

后者可以回避奇异状态。

（a）未利用冗余性的情况（kq

0） （b）利用冗余性的情况（kq20）

图15 回避奇异点的仿真

26