机器人建模与控制课件：机器人运动控制

机器人建模与控制

机器人运动控制在第i关节电机的电枢两端施加电枢电压Vi(t)，则有电枢电流iai(t)，因iai(t)在定子磁场中，会产生定子对转子的力矩τmi(t)，该力矩会改变转子转速(电机转速)dθmi(t)/dt，其大小与电流成正比，τmi(t)=

Kmiiai(t)，Kmi是电机的扭矩常数当转子转动时，电枢切割定子磁力线，电枢电路中会出现反电动势Vbi(t)，其大小与转速成正比，Vbi(t)=

Kbidθmi(t)/dt，Kbi是电机的反电动势常数8.1.1

电机及电机驱动器直流电机包括定子和转子，转子的主要部分是可通电的电枢(线圈绕组)记电枢电感为Li

，电枢电阻为Ri

，电枢电路方程：

Lidiai(t)/dt+Riiai(t)=

Vi(t)–Kbidθmi(t)/dt功率模块相当于放大器Vi(t)=

Kciui(t)，

Kci为可调增益，ui(t)为第i关节控制电压功率模块

9mi

(t)Tmi

(t)Vbi

(t)

8.1

独立关节控制

+

+ui

(t)

Kci

Vi

(t)−−+−iai

(t)RiLi8.1.2

减速器及关节模型摩擦当物体与另一物体沿接触面的切线方向运动或有相对运动的趋势时，在两物体的接触面之间有阻碍它们相对运动的作用力，这种力叫摩擦力。接触面之间的这种现象或特性叫“摩擦”摩擦分为静摩擦和动摩擦按动摩擦表面的润滑状态，摩擦可分为干摩擦、边界摩擦和流体(粘性)摩擦干摩擦：摩擦副表面直接接触，没有润滑剂存在时的摩擦

干摩擦力=法向力×干摩擦系数流体(粘性)摩擦：流体润滑状态下的摩擦。当流体为层流状时：粘性摩擦力=物体相对运动速度×粘性摩擦系数边界摩擦相当于干摩擦和流体摩擦之间的一种状态8.1

独立关节控制τi(t)为高速侧对低速侧的作用力的力矩(关节力矩)τai(t)为低速侧对高速侧的反作用力的力矩τi(t)=riτai(t)Bmi是转子轴承的粘性摩擦系数，粘性摩擦力矩=角速度×粘性摩擦系数Jmi为高速侧的转动惯量高速侧力矩平衡：Jmid2

θmi(t)/dt2

=

τmi(t)–τai(t)–Bmidθmi(t)/dt

即Jmid2

θmi(t)/dt2

+Bmidθmi(t)/dt

=

τmi(t)–τi(t)/ri9电机转子通过传动比为ri

:1的齿轮减速器与第i连杆相连电机转角与关节角度的关系：θmi(t)=ri

θi(t)8.1

独立关节控制第i连杆9mi

(t)Tmi

(t)Tai

(t)9i

(t)Ti

(t)ri

:1BmiJmi在带有明暗方格的码盘两侧，安放发光元件和光敏元件随着码盘的旋转，光敏元件输出的波形经整形后变成脉冲根据脉冲计数，可以测量固定在码盘上的转轴的角位移根据A相信号和B相信号间的关系，可以判断旋转方向码盘上有Z相标志信号，每转一圈输出一个脉冲，可用于确定零位根据单位时间内的脉冲计数(高速)或两个相邻脉冲的时间间隔(低速)

，可以测量固定在码盘上的转轴的角速度8.1.3

旋转编码器及关节传递函数模型8.1

独立关节控制9i

(s)

=

Kmi

/

ri

9i

(s)

=

−(Li

s

+

Ri

)

/

ri

Vi

(s)

s[(Li

s

+

Ri

)(Jmi

s

+

Bmi

)

+

Kbi

Kmi

]

Ti

(s)s[(Li

s

+Ri

)(Jmi

s

+Bmi

)+Kbi

Kmi

]因电气时间常数Li/Ri远比1小，假设Li/Ri为零，得22τmi(t)=

Kmiiai(t)Lidiai(t)/dt+Riiai(t)=

Vi(t)–Kbidθmi(t)/dt

θmi(t)=ri

θi(t)9i

(s)

−1

/

ri

Ti

(s)s[Jmi

s

+Bmi

+Kbi

Kmi

/Ri

]Ui

(t)=(Kmi

/ri

Ri

)Vi

(t),Di

(t)=Ti

(t)/ri

2

Ji

=

Jmi

,

Bi

=

Bmi

+

Kbi

Kmi

/

Ri229i

(s)

Kmi

/

ri

Ri

Vi

(s)s[Jmi

s

+Bmi

+Kbi

Kmi

/Ri

]Jmid2

θmi(t)/dt2

+Bmidθmi(t)/dt

=

τmi(t)–τi(t)/ri8.1

独立关节控制KbiKmis9mi

(s)

9mi

(s)

9i

(s)

1

Li

s

+Ri

−1Ji

s

+

Bi 1i

sDi

(s)−Jmi

s

+

BmiTmi

(s)s9i

(s)Ui

(s)9i

(s)Ti

(s)iai

(s)Vi

(s)

1i1s==−rr11机器人的每个轴都被作为一个单输入/单输出(SISO)系统来控制由于其他关节的运动而引起的耦合效应被包含在干扰中设计控制算法，保证闭环系统稳定，并较好地跟踪参考轨迹和减小干扰的影响8.1

独立关节控制功率放大器关节i的

参考轨迹传感器控制器关节i干扰输出+ei

(s)

=

2s

+

s(B

+

K

)

/

J

9di

(s)

+

Di

(s)s

+s(Bi

+KDi

)/Ji

+KPi

/Ji

s

+s(Bi

+KDi

)/Ji

+

KPi

/Ji特征方程s2

+s(Bi

+KDi

)/Ji

+KPi

/Ji

=s2

+2匕iOi

s

+Oi2自然频率ωi

，阻尼系数ζi

闭环系统稳定22222222222222iiDiDii21sUi

(t)=(Kmi

Kci

/ri

Ri

)ui

(t)=Ki

ui

(t)9di

(s)ei

(s)8.1.4

考虑阶跃输入的PD控制器设计Ui

(t)=(Kmi

/ri

Ri

)Vi

(t)Vi(t)=

Kciui(t)8.1

独立关节控制偏差ei(t)=θdi(t)–θi(t)Ksi微分系数KDi

=

KiKsi比例系数KPi

=KiKeiDi

(s)−

9i

(s)Di

(s)Ui

(s)

−Ki−KDi

ss(Ji

s

+Bi

)Ji

s

+Bis9i

(s)9di

(s)Ui

(s)9i

(s)ui

(s)ei

(s)KeiK−−−11Pi通常取ζi=1常使得响应是临界阻尼，以产生最快的非振动响应。在这种情况下，

ωi

决定响应的速度8.1

独立关节控制阶跃响应(有干扰di(t)=40)阶跃响应(无干扰di(t)=0)特征方程s3

+s2

(Bi

+KDi

)/Ji

+sKPi

/Ji

+

KIi

/Ji由Routh判据可知，闭环系统稳定的充要条件是KIi

<(Bi

+KDi)KPi/JiKi1s1s(Ji

s

+Bi

)8.1.5

考虑阶跃输入的PID控制器设计8.1

独立关节控制Ksi积分系数KIi

=

KiKfiUi

(s)Di

(s)−KPi

+

Kei

+

KDi

s9di

(s)ei

(s)9di

(s)ei

(s)Di

(s)−

−Ji

s

+Bis9i

(s)Ui

(s)9i

(s)9i

(s)ui

(s)−−−1在控制器中增加积分环节，主要是为了消除稳态误差阶跃响应(有干扰di(t)=40)8.1

独立关节控制8.1.6

考虑二阶可导输入的PID控制器设计若θdi(t)二阶可导，可增加前馈提高控制性能，如下面的前馈+PID控制Ki1sei

(s)=3

2

i

Di

(s)s

+s(Bi

+KDi

)/Ji

+sKPi

/Ji

+KIi

/

Ji在阶跃干扰时，对任意的二次可导参考轨迹，跟踪误差以渐近方式趋于零1s(Ji

s

+Bi

)引入前馈可以使偏差与θdi(t)的变化无关8.1

独立关节控制

s/

J

Ksis

J2i

/Ki

+s(Bi

/Ki

+Ksi

)Ji

s2

+Bi

sKPi

+

+

KDi

sDi

(s)Ui

(s)

−Kei

+

Di

(s)−Ji

s

+Bis9i

(s)9di

(s)9di

(s)9i

(s)ui

(s)ei

(s)ei

(s)9i

(s)−−−1例：a1和a2分别为两连杆的长度l1和l2分别为两连杆质心到各自关节轴之间的距离机器人的势能为

u

=

m1gl1s1

+

m2

g(a1s1

+

l2s12

)

+

mr

2ga1s1机器人的动能为1

2

2

2

2

2

2k

=2

(m1l1

+m2

a1

+2m2

a1l2

c2

+m2

l2

+mr

2a1

+J1

+J2

+n1

Jr1

+Jr

2)912

1

2

2

2+(m2

a1l2

c2

+m2l2

+J2

+n2

Jr

2)9192

+2

(m2

l2

+J2

+n2

Jr

2)92利用拉格朗日方法可建立机器人动力学模型2C

C1118.2

计算转矩前馈控制CC221m

Jrm

Jmmr1JJr1r22220011221关节1处的动力学模型

Uc1

−ba1

+2n1bm1

+

91=(m1l

+m2

(a

+2a1l2

c2

+l)+mr

2a

+J1

+J2

+n12

Jr1

+Jr

2)91

−2m2

a1l2s29192

+(m2

(a1l2

c2

+l)+J2

+n2

Jr

2)92

−m2

a1l2

s2922

+m1gl1c1

+m2

g(a1c1

+l2

c12

)+mr

2ga1c1按照独立关节控制的思路，此模型等价为=

Kc1Uc1

−Tc1式中Jc1

=

Ja1

+2n1Jm1

Ja1

=m1l

+m2

(a

+2a1l2

c2

+l)+mr

2a

+J1

+J2

+Jr

2

Jm1

=

Jr1Tc1

=2m2

a1l2

(c2

−c2

)91

+(m2

(a1l2

c2

+l)+J2

+n2

Jr

2)92−2m2

a1l2

s29192

−m2

a1l2s2922

+m1gl1c1

+m2

g(a1c1

+l2c12

)+mr

2ga1c1

c2

是c2变化范围的中值Bci

=bai

+ni2bmi

+

K

=

niCTi

kuici

Rmi221222121222122212128.2

计算转矩前馈控制类似可得关节2处的模型由前述模型可见，即使是采取独立关节控制，也还是需要掌握机器人整体动力学模型的信息，因为某些独立关节模型的参数可能会涉及其它关节同时，也可以看到干扰转矩非常复杂，它们与关节的角度、角速度、角加速度甚至关节角度变化范围等等都有关系。对干扰力矩进行估计，并利用估计结果补偿干扰力矩，即形成有利于抑制复杂干扰力矩的计算转矩前馈控制策略如：对干扰力矩1的估计式为1

=2m2a1l2

(cd

2

−c2

)9d

1

+(m2

(a1l2cd

2

+l

)+J2

+n2Jr

2)9d

2−2m2a1l2sd

29d19d

2

−m2a1l2sd

29d22

+m1gl1cd1

+m2g(a1cd1

+l2cd12)+mr

2ga1cd1

式中

cd1

=cos9d1,sd

2

=sin9d

2

,cd

2

=cos9d

2

,cd12

=cos(9d1

+9d1)对于关节1，在带前馈的PID控制中引入计算转矩前馈，可得到如下控制方案：Uc1(s)

=

kP1

+

kI1

91(s)

−

kD1o1(s)

+

Jc

1

s29

c

1

1(s)cT八22cT八cT

s

)Kc1

Kc1

)

Kc18.2

计算转矩前馈控制8.3.1

电机电流控制引入电流反馈会削弱关节的抗干扰能力，考虑到独立关节控制对各关节的抗干扰能力要求甚高，所以在独立关节控制中不使用电流反馈在独立关节控制中，将各关节电机视为运动部件，重点关注转速ωi

，由转速积分为θi

进而形成机器人运动完成要求的作业在独立关节控制中各关节电机控制的目标是让转角跟踪期望曲线与独立关节控制不同，集中控制是将各关节电机视为出力部件，重点关注转矩Tei

，由关节电机的转矩驱动机器人运动完成要求的作业。由于电机转矩与电流成正比，在集中控制中各关节电机控制的目标是让电机电流跟踪期望电流。这种电流跟踪控制显然需要使用电流反馈，因此在集中控制中是离不开电流反馈的8.3

集中控制「9d1(t)]给定轨迹|9d

2(t)

||

|

L9dn

(t)」8.3.2

集中控制的被控对象模型电机功率模块减速器考虑电动转动关节型机械臂机器人机构

轨迹规划

8.3

集中控制正运动学「u1

(t)]控制电压|u2

(t)

|「91

(t)]被控轨迹|92

(t)

|「t1

(t)]关节力矩|t2

(t)

|任务机器人结构环境|

||

|||Lun

(t)」||||||L

(t)」n9|

|||||Ltn

(t)」(被控对象)(执行器)|

||

|笛卡尔空间轨迹对n关节的转动关节型机械臂，设旋转关节无摩擦「91

(t)]

「T1

(t)]||||||||L

(t)」LTn

(t)」由理论力学可导出机器人机构的动力学方程M(9(t))9(t)+C(9(t),9(t))9(t)+G(9(t))=T(t)或简记为M(9)9+C(9,9)9+G(9)=

TM(9)是一个依赖于9的n

n

正定矩阵C(9,9)是一个依赖于9和9的n

n

矩阵G(9)是一个依赖于9的n1向量n9令：关节角向量9(t)

=

|92

(t)

|，关节力矩向量T(t)

=

|T2

(t)

|||

||8.3

集中控制有电机执行机构动力学：J9+B9=

v

−t机器人机构动力学：M(9)9+C(9,9)9+G(9)=t忽略摩擦：B

=0，并联合两个动力学方程，得(M(9)+J)9+C(9,9)9+G(9)=v多变量模型：W(9)9+C(9,9)9+G(9)=v

W(9)=M(9)+J正定]「r12

K1u1

]||||

,

v

=

|

|

2rnBn

」||L2rnKnun

」|Ui

(s)Di

(s)−将电机执行单元的动力学与机器人机构的动力学进行联合Di

(t)=ti

(t)/ri2

Ji9i

+Bi9i

=Ki

ui

−ti

ri

2「r12

J1令J

=

|8.3

集中控制]「r12

B1|||

,

B

=

|1Ji

s

+Bi

2rnJn

」|Ki1ss9i

(s)9i

(s)ui

(s)L|L闭环系统方程

W9+

C9

=

KP

e

−

KD9「e

]「e

]「−9

]取其状态

|L9」|

，得状态方程

|L9」|

−1KP

e

−W

−1(KD

+

C)9用李雅普诺夫函数法分析该自治系统的稳定性8.3.3

重力补偿PD控制被控对象模型]「KD1|

||

,

KD

=

|KPn

」||L「KP1|KP

=

||L8.3

集中控制e

=9d

−9,9d

为定常向量W(9)9+C(9,9)9+G(9)=v]||KDn

」|v

=KP

e

−KD9+G(9)控制律定理：设自治系统x

=

f

(x)有平衡状态w=0，如果存在定义于n

上的、具有连续一阶偏导数的标量函数V(x)，满足以下条件：1)V(x)正定2)V(x)

半负定3)在该系统任意的从非零状态出发的状态轨迹上，V(x)

不恒为零4)当x

→，有

V(x)→那么该自治系统在w=0是大范围渐近稳定的对于标量函数V:n

→:x

V(x)，若对于任意非零x有V(x)>0且V(0)=0，则称V(x)正定；若对于任意非零x有V(x)≥0且V(0)=0，则称V(x)半正定V(x)负定当且仅当–V(x)正定；V(x)半负定当且仅当–V(x)半正定8.3

集中控制引理：若A=n人n

是反对称矩阵，则Vx=n

，有xT

Ax

=0证：由题设知A

+

AT

=

0，则xT

(A

+A)xT

=x

Ax

+x

ATTT

x

=0注意到x

A

xTT

=(x

Ax)TT

=x

AxT因此x

AxT

=0「0

−3例：对于反对称矩阵A

=

3

0

|L−7

−17]1

|

，有0」|0

1

|

|x2

|

−1

0」||Lx3

」|=−3x1x2

+7x1x3

+3x2

x1

+x2

x3

−7x3x1

−x3x2

=08.3

集中控制−37]「x1

]|||「0|x2

x3

]|

3xT

Ax

=[x1|L−7|「e

]「e

]「−9]原点

|L9」|

=

0

是自治系统

|L9」|

−1KP

e

−W

−1(KD

+

C)9

的平衡态构造正定的李雅普诺夫函数1T

1T2

2

PT

1

T

T2

PT

1

T

TP

D

2

P1

T

T=9(W

−2C)9−9K

9注意到W

−2C

=(M

+J)−2C

=M

−2C

是反对称的，9T

(W

−2C)9=0因此V(e,9)=−9T

KD9半负定=9(K

e

−K

9−C9)+9W9−9K

e8.3

集中控制V(e,9)=9W9+9W9−9

K

eV(e,9)=9W9+

e

K

e2

D从任意非零初态出发的解不会有V(e(t),9(t))=0此点可用反证法证明：设从某非零初态出发的解有V(e(t),9(t))=0即−9T

(t)KD9(t)=0，这意味着9(t)=0，进而9(t)=0由W(9)9+C(9,9)9=KP

e

−KD9，知e(t)

=

0「e(t)]「e(0)]L9(t)」L9(0)」「e

]

1

1显然，当

|L9」|

→

时有

V(e,9)

=

2

9T

W(9)9+

2

e

KTP

e

→

综上，系统大范围渐近稳定，在任何初态下9(t)

都收敛于9d于是解||=0，从而初态||=

0

，与非零初态假设相矛盾8.3

集中控制8.3.4

逆动力学控制如下质量-弹簧-阻尼系统，作用在质量块上的外力f是输入，质量块的位置是输出，该二阶线性系统的动力学模型mx

+bx

+kx

=f可见，在控制反馈作用下，通过设定控制增益kp

和kv

可以使闭环系统呈现期望的二阶阻尼特性(比如临界阻尼ζ

=1)。假设可检测质量块的位置和速度，则设计控制律：两式联立，得：

mx

+(b

+kv

)x

+(k

+kp

)x

=0上图表征了在反馈控制作用下，系统实

现了位置校正f

=−kp

x

−kv

xK8.3

集中控制s2

+2匕On

s

+

nO2基于模型的控制s2

+kv

s

+kp根据期望的控制性能确定kp和kv同样，设计控制率x

+kv

x

+kp

x

=0先利用反馈将模型化为单位质量模型，单位质量模型设计kp和kv分解控制下的闭环控制系统称为α-β分解运动控制简化为质量模型

=mmx

+bx

+kx

=fmx

+bx

+kx

=f

'

+

代入上式，得1分解成模型控制+位置校正两部分8.3

集中控制f

'

=−kv

x

−kp

x

=bx

+kx位置校正部分再对取基于模型的控制f

=mx

+bx

+

kx(x)=qx3也将模型化为单位质量模型非线性的基于模型的控

制律(α-β分解运动控

制)，用来“抵消”系

统的非线性，并简化为单位质量系统后，设计简单的线性伺服控制律➢如图质量-弹簧-阻尼系统中，非线性弹簧k(x)=qx3系统模型为mx

+bx

+qx3

=f基于模型的控制律改为f

=mf

+bx

+

qx38.3

集中控制x

=

f

bx

+

kx对每个双积分系统，可实施“前馈+PD控制”即aq

=9d

+F0

(9d

−9)+F1

(9d

−9)外环控制律F0和F1是对角矩阵内环/外环控制架构逆动力学控制的缺点：精确的W、C

和G

函数难以获得C(9,9)9+

G(9)s2

+Ksi

s

s9i

(s)

1sKsiW(9)9+C(9,9)9+G(9)=vv

=W(9)aq

+C(9,9)9+

G(9)9W(9)9+C(9,9)9+G(9)=v

+98.3

集中控制n个独立的双积分系统ei

(s)

K反馈线性化内环控制律

−

−+

+

+9=a

q1s−aqi

(s)

9i

(s)

−9d9dW

(9)1e0e9di

(s)

9d+++FFavqei由e

=9d

−9，有闭环系统方程：e

=−F0e

−F1e

+(n−6a)令h

=

|

|

，有闭环状态方程：h

=

||h

+||(n−6a)=

Fh

+||(n−

6a)首先设计F0

和F1

使得F

的特征值具有负实部，然后设计6a

=S(e)应对n−−W(9)9+C(9,9)9+G(9)=vC八(9,9)9+G八(9)W(9)9+C(9,9)9+G(9)=W八(9)aq

+C八(9,9)9+G八(9)W(9)9=W(9)aq

−(W(9)−W八(9))aq

−(C(9,9)−C八(9,9))9−(G(9)−G八(9))=W(9)aq

−W(9)aq

−C(9,9)9−G(9)9=aq

−W−1(9)(W(9)aq

+C(9,9)9+G(9))=aq

−n(9,9,aq

)

外环控制律aq

=9d

+F0

(9d

−9)+F1(9d

−9)+6aS(e)+

+

+6a0e+x+「e]「0

I

]「0]「0]Le」L−F0

−F1」

LI」LI」内环控制律v

=W八(9)aq

+C八(9,9)9+G八(9)8.3

集中控制8.3.5

鲁棒控制9

d9dn(9,9,aq

)表示不确定性W(9)9+C(9,9)9+G(9)=vvx++1ex99W八(9)9dx+FFaq「m1l

+m2

(l

+l

2

+2l1lc

2

cos92

)+I1

+I2

m2

(l

2

+l1lc2

cos92

)+I2

]「91

]L

m2

(l

2

+

l1lc

2

cos92

)

+

I2

m2

l

2

+

I2

」L92

」+

1

]c2c2c2c212c12例：2转动关节的平面机器人，连杆i

的长度、质量和转动惯量(绕质心)分别是li

、mi

和Ii

，lci

表示前一个关节与连杆i

的质心之间的距离「(m1lc1

+m2

l1

)g

cos91

+m2

lc2g

cos(91

+92

)]

「1

]L

m2

lc

2g

cos(91

+92

)

」

L2

」其中，g是重力加速度L

0

」L92

21c221m

l

l

sin9

9||||l||=

||lc1

918.3.6

自适应控制8.3

集中控制多输入多输出复杂非线性系统W(9)9+C(9,9)9+G(9)=

动力学模型：

2+1

29c2ll21「−O2

sin9292

−O2

sin9291

−O2

sin9292

]「91

]「O5

cos91

+

O6

cos(91

+

92

)]+

O2

sin9291

0

9|LO6

cos(91

+

92

)

」|

92O1

=m1l

+m2

(l

+l

2

)+I1

+I2

+r12

J1

O2

=

m2

l1lc

2O3

=m2

l

2

+I2

O4

=2r2J2O5

=m1lc1g

+m2l1g

O6

=m2

lc2gc2c212c12L|L|n〉l

Y(9,9,9)l

O

W(9)9+C(9,9)9+G(9)=Y(9,9,9)O存在函数以及维参数向量，使例：2转动关节的平面机器人W(9)9+C(9,9)9+G(9)]「91

]「(m1lc1

+m2l1

)gcos91

+m2lc2g

cos(91

+92

)]」L92

」

L

m2

lc

2g

cos(91

+

92

)

」+

m2

l1lc0O3

+O2

cos92

]「91

]O3

+O4

」|

92

L|m2

(l

2

+l1lc2

cos92

)+I2

]「91

]m2l

2

+

I2

+

2r2J2

」L92

」c2c2「m1l

+m2

(l

+l

2

+2l1lc2

cos92

)+I1

+I2

+r12J1c212c12lc28.3

集中控制|||+||「O1

+2O2

cos92

=

|L

O3

+O2

cos92=

|L

m2

(l

2

+l1lc

2

cos92

)+

I2c2W(9)9+C(9,9)9+G(9)=vlc11

92

sin9291「

|

L|||

T21Tll21211221322222221221