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

机器人建模与控制

机器人动力学7.1.1

线速度和角速度的传递⚫点Q以线速度BVQ相对于坐标系{B}运动⚫{B}的原点以线速度AVBoRG相对于坐标系{A}运动⚫{B}以角速度A贝B绕坐标系{A}运动⚫线速度的传递关系为：AVQ

=AVBoRG

+RBVQ+A贝B

×

RBQBABA⚫坐标系

C

以角速度B贝C绕坐标系B运动⚫B

以角速度A贝B绕坐标系

A

运动⚫坐标系

C

绕坐标系{A}运动的角速度为：

A贝C

=A贝B

+RB贝CBA注意：

需要用到角速度A贝B7.1

速度和加速度的传递7.1.2

线加速度的传递⚫线加速度的传递可通过对线速度传递关系式的求导获得⚫特殊情况：坐标系{A}的原点和坐标系{B}的原点重合，有：AVሶQ

=

RBVሶQ+

AnB

×

RBVQ+

AnሶB

×

RBQ

+

AnB

×

RBVQ+

AnB

×

RBQ

=

RBVሶQ+

2AnB

×

RBVQ+

AnሶB

×

RBQ

+

AnB

×

AnB

×

RBQ

BABABABABABABABABAdt⚫同理有：RBVQ

=

RBVሶQ+

AnB

×

RBVQBABABAAVሶQ

=

RBVQ

+

AnሶB

×

RBQ

+

AnB

×

RBQ

BABABA⚫注意到：AV

RBQ

=

RBVQ+

AnB

×

RBQBABA7.1

速度和加速度的传递AVQ

=

RBVQ+

AnB

×

RBQBABA求导7.1.2

线加速度的传递⚫一般情况：如果坐标系{A}的原点和坐标系{B}的原点不重合⚫需加上{B}原点的线加速度AVሶQ

=

AVሶBORG

+

RBVሶQ+

2AnB

×

RBVQ+

AnሶB

×

RBQ

+

AnB

×⚫如果矢量B

Q保持不动⚫即：

BVQ

=

0,

BVሶQ

=

0BABABA7.1

速度和加速度的传递AVሶQ

=

AVሶBORG

+

AnሶB

×

RBQ

+

AnB

×BA

AnB

×

RBQ

BA

AnB

×

RBQ

BAA

ሶC

=

A

ሶB

+

d

ARB

C

A

ሶC

=

A

ሶB

+

RB

ሶC

+

A

B

×

RB

CBABA7.1.3

角加速度的传递⚫类似的，角加速度的传递关系可以通过对角速度传递关系式求导得到

dt

B

RB

C

=RB

ሶC

+A

B

×RB

CBABABA7.1

速度和加速度的传递A

C

=A

B

+RB

CBA求导⚫考虑多个质点连接形成刚体⚫设质点i的质量为mi

，则刚体的总质量m=σimi⚫考虑该刚体的联体坐标系{B}。在惯性坐标系{U}中，

有：UVi

=UVBORG

+UQB

×

RBPi⚫UVi表示质点i在{U}中的速度⚫对速度求导获得其加速度：UVሶi

=UVሶBORG

+UQሶB

×RBPi

+UQB

×UQB

×

RBPi

⚫注意：由于是刚体，

BPሶi

=0⚫作用在质点i上的力：U

fi

=mi

UVሶi

=mi

UVሶBORG

+UQሶB

×RBPi

+UQB

×UQB

×RBPi

BUBUBUBUBU7.2

刚体的惯性张量与欧拉方程=⃞mi

RBPi

×

UVሶBORG

+⃞mi

RBPi

×+⃞mi

RBPi

×UQB

×UQB

×

RBPi

BUBUBUBUU

BN

=⃞iU

BNi

=⃞i

mi

RBPi

×UVሶBORG

+UQሶB

×RBPi

+UQB

×BUBU⚫作用在质点i上的力矩：U

BNi

=RBPi

×U

fi

=mi

RBPi

×BUBU

UQሶB

×

RBPi

BUUVሶBORG

+UQሶB

×RBPi

+UQB

×BU7.2

刚体的惯性张量与欧拉方程注意叉乘不满足乘

法结合律，不能将

后面的括号去掉⚫作用在整个刚体上的总力矩：i

i UQB

×

RBPi

BU UQB

×

RBPi

BUiσimi

RBPi

×UVሶBORG

=0⚫总力矩可简化为：U

CN

=⃞imi

RCPi

×UQሶC

×RCPi

+⃞i

mi

RCPi

×UQC

×UQC

×

RCPi

⚫计算U

CN

在坐标系{C}中的表示：C

CN

=

CN

=

RU

CN=R

⃞mi

RCPi

×UQሶC

×RCPi

+⃞mi

RCPi

×UQC

×UQC

×

RCPi

CUCUCUCUUCUCCUCUCUCUBU⚫如果将联体坐标系{B}的原点选在刚体质心上，则有：

⃞imiBPi

=

07.2

刚体的惯性张量与欧拉方程⚫为强调联体坐标系原点在刚体质心上这一情况，下面用{C}替代{B}i

i=⃞mi

CPi

×R

UQሶC

×RCPi

=⃞mi

CPi

×RUΩሶC

×R

RCPi

i

i=⃞i

−mi

CP

CP

C

UQሶC

=⃞i

−mi

CP

2C仙ሶ

C⚫遵循之前符号规则，这里用仙ሶ

C

=UQሶC

，表示该

(联体)

质心坐标系在惯性坐标系U中的角加速

度i^i^i^CUUCUCCUUCi

i=⃞mi

CPi

×C

UQሶC

×CPi

=⃞−mi

CPi

×CPi

×

C

UQሶC

7.2

刚体的惯性张量与欧拉方程⃞imi

RCPi

×UQሶC

×

RCPiCUCU=⃞mi

R

RCPi

×CUUCR

UQሶC

×

RCPiCUUC⚫CN的第一项RUCiR

⃞mi

RCPi

×UQC

×UQC

×

RCPi

=⃞mi

R

RCPi

×R

UQC

×UQC

×

RCPi

=⃞mi

CPi

×RUQC

×R

UQC

×

RCPi

=⃞mi

CPi

×C

UQC

×RUQC

×R

RCPi

=⃞mi

CPi

×C

UQC

×C

UQC

×

CPi

=⃞−mi

C

UQC

×CPi

×CPi

×C

UQC=⃞−mi

C仙CP

2

C仙Ci^C^CUUCUCCUUCUCCUUCCUUCCUCUUC⚫遵循之前符号规则，这里

用仙C

=UQC，表示该(联

体)质心坐标系在惯性坐

标系U中的角速度7.2

刚体的惯性张量与欧拉方程⚫CN的第二项iiiiiii⚫该式为旋转刚体的欧拉方程⚫欧拉方程描述了作用在刚体上的力矩CN与刚体旋转角速度C仙C和角加速度C仙ሶ

C之间的关系⚫CI称为刚体的惯性张量(inertia

tensor)

，或旋转惯性矩阵(rotational

inertia

matrix)CN

=

⃞−mi

CP

2C仙ሶ

C

+⃞−mi

C仙CP

2C仙Ci∧C^i∧7.2

刚体的惯性张量与欧拉方程CN

=CIC仙ሶ

C

+C仙C

×CIC仙C记CI

=

σi

−mi

CP

2i∧i

i−

⃞mixizi−

⃞miyizi⃞mi⚫可以看出，

CI是一个3×3矩阵⚫如将CPi完整记为

xi,yi,zi

T

，CI矩阵各元素为：⚫记为：CI

=:7.2

刚体的惯性张量与欧拉方程Ixx

−Ixy

−Ixz−

⃞mixiyi−

⃞mixizi−

⃞mixiyi−

⃞miyizi−IxyIyy−Iyz−Ixz−IyzIzz⃞mi⃞mix+

yi2i2y+

zi2i2x+

zi2i2CI

=Ixx

=⃞y2

+z2

p

x,y,z

dVBIyy

=⃞x2

+z2

p

x,y,zdVIzz

=⃞x2

+y2

p

x,y,zdVIxy

=⃞xypx,y,zdVIxz

=⃞xzpx,y,z

dVB⚫

惯性张量是一个对称矩阵

Iyz

=

⃞

yzp

x,y,z

dV⚫惯性张量中的对角元素Ixx

、Iyy和Izz称为惯性矩(mass

moments

of

inertia)⚫考虑质量连续分布的刚体⚫用密度函数p

x,y,z

和微分单元体dV的乘积替代点质量，用积分运算替代求和运算，可得：⚫非对角元素Ixy

、Ixz和Iyz称为惯性积(mass

product

of

inertia)7.2

刚体的惯性张量与欧拉方程Ixx

CI

=:−Ixy−Ixz−IxyIyy−Iyz−Ixz−IyzIzzBBBB⚫例7-1:考虑如图中的质量为m，长度为l，宽度为w，高度为ℎ的长方体连杆。连杆的质量是均匀分布的。建立如图所示的(原点)位于长方体连杆质心的联体坐标系{C}。计算该连杆在{C}下的

惯性张量。解：

该连杆的密度p=。ℎ

l

ℎ

l=⃞

⃞

w

y2

+z2

p

dydz=wp

⃞

⃞

d

+z2y

dzℎ

ℎ=wp

⃞

+z2l

dz=wp

⃞

d

+

=wp

+

=l2

+ℎ2

类似的，可计算得：Iyy

=ℎ2

+w2

,Izz

=w2

+

l2

l(1)

惯性矩：Ixx

=

2l2l2ℎ2ℎ7.2

刚体的惯性张量与欧拉方程−

12

−

12

3

12

122ℎ2ℎw

2w22l32l3z

z3l

l3

ℎℎ3l−

−

−

−

32l2ℎ2l2ℎ2222

y3y2

+z2

pdxdydzwhX(3)

该连杆在{C}下的惯性张量： l2

+ℎ2

0

ℎ2

+w2

000 w2

+l2

ℎ

l

w=p

d

dydz

=0类似的，可计算得：Iyz

=

0,

Iyz

=

02w22l22ℎ2l7.2

刚体的惯性张量与欧拉方程w⃞xypdx

dydzw2Ixy

=

2l2l2ℎ2ℎ(2)

惯性积：CI

=00−

2whX解：

该连杆的密度p=。(1)

惯性矩：Ixx

=⃞⃞⃞y2

+z2

pdxdydz=⃞⃞w

y2

+z2

p

dydz=wp

⃞⃞d

+z2ydzℎ

l3

ℎ

l3z

z3l333类似的，可计算得：⚫注意：

惯性张量也可以定义在非质心坐标系中⚫例7-2:考虑如图中的质量为m，长度为l，宽度为w，高度为ℎ的长方体连杆。连杆的质量是均匀

分布的。建立如图所示的(原点)位于长方体连杆一顶点的联体坐标系{B}。计算该连杆在{B}下

的惯性张量。7.2

刚体的惯性张量与欧拉方程=wp

⃞+z2l

dz=wp

⃞d

+0

0=

wp

+

=

l2

+

ℎ2

Iyy

=ℎ2

+w2

,Izz

=w2

+

l2

000ℎ

l

ℎ

l

y300

00Xℎl

wwh3l(2)

惯性积：Ixy

=⃞⃞⃞xypdx

dydz=p

⃞⃞⃞d

dydz=p⃞⃞dydz=p⃞⃞ddz=p

⃞dz=

p

=

wl类似的，可计算得：Iyz

=lℎ,Ixz

=ℎw(3)

该连杆在{B}下的惯性张量：m

m

m l2

+ℎ2

−

wl

−

ℎwBI

=

−

wl

ℎ2

+

w2

−

lℎ−

ℎw

−

lℎ

w2

+

l2

7.2

刚体的惯性张量与欧拉方程000ℎ

l

w2y

ℎ

l

w2y2

ℎ

w2l2000ℎ

l

w

x2y224

43440000

0Xℎl

wwhl⚫

利用牛顿-欧拉法求解动力学方程分两个阶段：➢向外迭代：

从(虚拟)的连杆0开始，依次计算连杆1到N联体坐标系得速度(线速度和角

速度)以及加速度(线加速度和角加速度)

，同时利用连杆i联体坐标系得速度和加速度

计算连杆i质心的加速度，并利用牛顿方程和欧拉方程求取作用在连杆上的力和力矩➢向内迭代：

从连杆N开始，根据力平衡方程和力矩平衡方程，依次计算出连杆N−

1到连

杆1上的力，同时计算出产生这些力和力矩所需的(转动型关节)关节力矩或(平动型关

节)关节力⚫注意：

下面牛顿-欧拉迭代动力学方程的讨论中忽略了摩擦的影响，即假设各关节均无摩擦7.3

牛顿-欧拉迭代动力学方程⚫考虑一般的连杆坐标系{i}及其相邻连杆坐标系{i+1}，已知：i仙i+1

=i仙i

+i+

Reሶi+1i+1

i+1iUi+1

=iUi

+i仙i

×iOi+1⚫

左乘i+

R

：i+1仙i+1

=

i+

Ri仙i

+

eሶi+1i+1

i+1i+1Ui+1

=i+

R

iUi

+i仙i

×iOi+1

i1i1i11i7.3.1

向外迭代：速度和加速度的计算(1)连杆的速度传递:i+1Ui+1{i+1}i+1i+1ii7.3

牛顿-欧拉迭代动力学方程ii+1i+1PCi+1i+1UCi+1iUi{i}7.3.1

向外迭代：速度和加速度的计算(2)连杆的角加速度传递:⚫角加速度传递公式：AnሶC

=AnሶB

+RBnሶC

+AnB

×

RBnC

⚫令A={0}，B={i}，

C={i+1}：仙ሶ

i+1

=仙ሶ

i+Rinሶi+1

+仙i

×Rini+1

⚫ini+1=eሶi+1

i+

Ri+1

i+1⚫注意：

ini+1和

i仙i+1具有不同的物理意义1ii0i0BABA7.3

牛顿-欧拉迭代动力学方程7.3.1

向外迭代：速度和加速度的计算ini+1=eሶi+1

i+

Ri+1

i+1⚫求导得：inሶi+1=i+1

i+

Ri+1

i+1⚫代入仙ሶ

i+1式：仙ሶ

i+1

=仙ሶ

i

+R

i+1

i+

Ri+1

i+1

+仙i

×Reሶi+1

i+

Ri+1

i+1

=仙ሶ

i

+i+1

i+

Ri+1

i+1

+仙i

×eሶi+1

i+

Ri+1

i+1⚫两边乘上

R：i仙ሶ

i+1

=i仙ሶ

i

+i+1

i+

Ri+1

i+1

+i仙i

×eሶi+1

i+

Ri+1

i+1⚫为得到{i}到{i+1}的递归式⚫两边再乘上i+R：i+1仙ሶ

i+1

=

i+

Ri仙ሶ

i

+

i+1i+1

i+1

+

i+

Ri仙i

×eሶi+1i+1

i+1⚫对于平动型关节，因为eሶi+1=0，

i+1

=0，上式可简化为：i+1仙ሶ

i+1

=

i+

Ri仙ሶ

ii1i1i1i11i1i0i10101ii01ii01i1i7.3

牛顿-欧拉迭代动力学方程7.3.1

向外迭代：速度和加速度的计算(3)连杆的线加速度传递：⚫线加速度传递公式AVሶQ

=AVሶBORG

+RBVሶQ+2AnB

×RBVQ+AnሶB

×RBQ+AnB

×AnB

×

RBQ⚫令A={0}，B={i}，Q为{i+1}原点：Uሶi+1

=Uሶi+RiVሶi+1

+2仙i

×RiVi+1+仙ሶ

i×RiOi+1+仙i

×仙i

×RiOi+1

⚫两边同乘上

R：iUሶi+1

=iUሶi+iVሶi+1+2i仙i×iVi+1+i仙ሶ

i

×iOi+1

+i仙i

×i仙i×iOi+1

⚫两边再乘上i+

R，可得到{i}到{i+1}的递归式：i+1Uሶi+1

=i+

RiUሶi+i仙ሶ

i

×iOi+1

+i仙i

×i仙i×iOi+1+i+

R

iVሶi+1+2i仙i×iVi+1

i1i1i10ii0i0i0i0BABABABA7.3

牛顿-欧拉迭代动力学方程7.3.1

向外迭代：速度和加速度的计算⚫对于平动型关节：i+

RiVi+1

=

i+1Vi+1=

dሶi+1i+1

RiVሶi+1=

i+1Vሶi+1=

i+1i+1

i+1

，i+1仙i+1

=

i+

Ri仙i⚫i+1Uሶi+1式可表示为：i1i1i+1Uሶi+1

=i+

RiUሶi+i仙ሶ

i

×iOi+1

+i仙i

×i仙i

×iOi+1+i+1i+1

i+1

+2i+1仙i+1

×dሶi+1i+1

i+1i1⚫对于转动型关节：iVi+1=0，iVሶi+1=0⚫i+1Uሶi+1式可简化为：i+1Uሶi+1

=i+

RiUሶi+i仙ሶ

i

×iOi+1

+i仙i

×i仙i×iOi+1

i17.3

牛顿-欧拉迭代动力学方程⚫选取

A=0，

B={i}，Q为连杆i质心，表示为Ci⚫因为BVQ

=iVCi

=0，BVQ

=iVሶCi

=0，有：Uሶ

Ci

=Uሶi+仙ሶ

i×RiPCi

+仙i

×仙i

×RiPCi

⚫两边同乘上

R：iUሶ

Ci

=iUሶi+i仙ሶ

i

×iPCi

+i仙i

×i仙i

×iPCi

0ii0i07.3.1

向外迭代：速度和加速度的计算(4)连杆质心的线加速度传递：i

i⚫

为了计算作用在连杆质心上的力，还需

要计算连杆质心的加速度⚫线加速度传递公式：AVሶQ

=AVሶBORG

+RBVሶQ+2AnB

×RBVQ+AnሶB

×RBQ+AnB

×BABABA7.3

牛顿-欧拉迭代动力学方程i+1Ui+1{i+1}i+1i+1

AnB

×

RBQBAiUi{i}i+1PCi+1i+1UCi+1ii+1⚫由前面的计算，我们可以从连杆0开始，

向外迭代计算连杆1至连杆n的质心坐标系的线加速

度、角速度和角加速度⚫接着利用牛顿-欧拉公式，可计算作用在连杆质心上的惯性力和力矩:

Fi=

mUሶ

CiCiNi

=CiICi仙ሶ

i

+

Ci仙i

×

CiICi仙i⚫坐标系{Ci}的原点位于连杆质心，

可以选取坐标系{Ci}的各坐标轴方向与原连杆坐标系{i}方向

相同，则上式中Ci仙ሶ

i

=i仙ሶ

i

，Ci仙i

=i仙i7.3

牛顿-欧拉迭代动力学方程ni+1fi+1{i+1}NinifiFi{i}⚫考虑连杆i，先考虑其力平衡方程

Fi⚫

右图为典型连杆在无重力状态下的受力情况

{i}

ni+1ni

fi

Ni➢作用于{i}原点的力向量fi表示连杆i

−

1施加在连杆i上的力，力矩向量ni表示连杆i

−

1施

加在连杆i上的力矩➢除了惯性力iFi，连杆i还受到连杆i

−

1施加在连杆i上的ifi，这里左上标表示这两个力表

示在坐标系{i}下➢由于连杆i对连杆i

+

1有一作用力fi+1，所以连杆i

+

1对连杆i有一反作用力−fi+1，同样

的，用i+1fi+1在坐标系{i

+

1}下表示fi+1⚫将所有作用于连杆i上的力向量相加，得到力平衡方程：iFi

=

ifi

−

i+

Ri+1fi+11i7.3.2

向内迭代：力和力矩的计算

fi+17.3

牛顿-欧拉迭代动力学方程{i

+

1}➢由于连杆i对连杆i

+

1有一作用力矩ini+1，所以连杆i

+

1对连杆i有一反作用力矩−

i+1ni+1

，

这里ini+1

=

i+

Ri+1ni+1➢存在ifi和−ifi+1在连杆i质心处产生的力矩⚫将所有力矩向量相加，得到力矩平衡方程：1i➢在坐标系{i}中表示，除了力矩iNi，连杆i还受到连杆i

−

1施加在连杆i上的力矩ini7.3

牛顿-欧拉迭代动力学方程7.3.2

向内迭代：力和力矩的计算⚫下面考虑作用在连杆i质心处的力矩平衡方程

iOi+1

−

iPCi

iNi

=

ini

+−ifi+1−iPCi−

ini+1×ifi

+ni+1fi+1{i

+

1}×NinifiFi{i}7.3.2

向内迭代：力和力矩的计算⚫将力平衡方程与力矩平衡方程整理为连杆i

+

1到连杆i的的迭代形式：➢力平衡方程ifi

=

i+

Ri+1fi+1

+iFi➢力矩平衡方程ini

=

iNi

+

i+

Ri+1ni+1

+iPCi

×ifi

+

iOi+1

−

iPCi

×ifi+1⚫由于ifi

=

ifi+1

+iFi

，有：ini

=

iNi

+

i+

Ri+1ni+1

+

iPCi

×

ifi+1

+iFi

+

iOi+1

−

iPCi

×ifi+1=

iNi

+

i+

Ri+1ni+1

+iPCi

×iFi

+iOi+1

×ifi+1=

iNi

+

i+

Ri+1ni+1

+iPCi

×iFi+iOi+1

×

i+

Ri+1fi+1⚫用上述方程对连杆依次求解，从连杆N开始向内递推直至机器人基座1i1i1i1i1i1i7.3

牛顿-欧拉迭代动力学方程7.3.2

向内迭代：力和力矩的计算⚫对于转动型关节i

，为产生力矩ni

，所需的关节力矩：

Ti

=

in

i⚫

对于平动型关节i，为产生力fi，所需的关节力：

Ti

=

if

iiTiiTi⚫注意：

上面的讨论并没有谈及重力⚫这是因为我们可以考虑惯性系中连杆坐标系{0}以加速度G运动，即

0Uሶ

0

=

G

，这里G与重力

矢量大小相等，方向相反，其产生的效果就与重力作用的效果是一样的7.3

牛顿-欧拉迭代动力学方程7.3.2

向内迭代：力和力矩的计算⚫例7-3

：计算如图所示平面机器人的动力学方程，假设每个连杆的质量都集中在连杆末端。解：连杆质心的位置矢量11PC1

=l1

1

=0

07.3

牛顿-欧拉迭代动力学方程2000C2I2

=000000000C1I1

=000000连杆质心的惯性张量2PC2

=l2

2

=11l200 m21mlll27.3.2

向内迭代：力和力矩的计算无力作用于末端执行器上，即：03f3

=0,3n3

=0机器人基座保持不动00仙0

=0

,0仙ሶ

0

=07.3

牛顿-欧拉迭代动力学方程2Ci+1Si+10

−Si+1

Ci+1

0001Ci+1

−Si+1i+

R

=

Si+1

Ci+11i00

R

=1连杆间的相对转动110Uሶ

0

=g

0

=考虑重力0g000000000 m21mll2➢向外迭代：连杆0到连杆1

(i

=0)

:1仙1

=

R0仙0

+

eሶ1

1

1

=

R

m22m1l101010

0

0

=0

1

eሶ11仙ሶ

1

=R0仙ሶ

0

+R0仙0

×eሶ1

1

1

+11

1=

R0101017.3

牛顿-欧拉迭代动力学方程7.3.2

向内迭代：力和力矩的计算−m1l1eሶ

+m1gS1

m1l1

1

+m1gC11200

×

eሶ11

1

+

10−l1eሶ

+gS1

l1

1

+gC10120仙ሶ

0

×0O1+0仙0

×1N1=C1I11仙ሶ

1

+1仙1

×C1I1

1仙1

=1Uሶ

C1

=1仙ሶ

1

×1PC1

+1仙1

×0000000000

0

0

=0

1

11仙1

×1PC1

+1Uሶ

1

=00+

R0010仙0

×0O1+0Uሶ

0

=00

+eሶ101F1=m1

1Uሶ

C1

=式中，C1I1

=gS1gC101Uሶ

1

=R010000l217.3.2

向内迭代：力和力矩的计算➢向外迭代：连杆1到连杆2

(i=1)

:m2

2仙2

=R1仙1

+eሶ22

2

=0

=0

为简化表达式，这里用eሶ12

=eሶ1

+eሶ21202仙ሶ

2

=R1仙ሶ

1

+R1仙1

×eሶ22

2

+22

2

=0122Uሶ

2

=

R

1仙ሶ

1

×

1O2

+

1仙1×

1仙1×

1O2

+

1Uሶ

1

=

l1

1C2

+

l1eሶ

S2

+

gC12121212122m2l1

1S2

−

m2l1eሶ

C2

−

m2l2eሶ

2

+m2gS12

m2l1

1C2

+m2l1eሶ

S2

+m2l2

12

+m2gC12121212−l2eሶ

2

l2

120+l1

1S2

−

l1eሶ

C2

+gS12

l1

1C2

+l1eሶ

S2

+gC1201212127.3

牛顿-欧拉迭代动力学方程l1

1S2

−

l1eሶ

C2

+gS1201200eሶ1

+eሶ2

eሶ122N2=C2I22仙ሶ

2

+2仙2

×C2I22仙2

=2Uሶ

C2

=2仙ሶ

2

×2PC2

+2仙2

×000000000

2仙2

×2PC2

式中，C2I2

=2F2=m22Uሶ

C+2Uሶ

2

=000 m1210=ll217.3.2

向内迭代：力和力矩的计算➢向内迭代：连杆3到连杆2

(i

=

2)

:2f2

=

R3f3

+

2F2

=

m2

l1

1C2

+

m2

l1

eሶ

S2

+

m2

l2

12

+

m2gC122n2

=

2N2

+

R3n3

+

2PC2

×

2F2

+

2O3

×

R3f30=

0m2

l1

l2

1C2

+

m2

l1

l2

eሶ

S2

+

m2

l

12

+

m2

l2gC12221232321232=

0m2

l1

l2

1C2

+

m2

l1

l2

eሶ

S2

+

m2

l

12

+

m2

l2gC122212T2

=

2n

2

2T0

02T7.3

牛顿-欧拉迭代动力学方程=

m2

l1

l2

1C2

+

m2

l1

l2

eሶ

S2

+

m2

l

12

+

m2

l2gC122212m2

l1

1S2

−

m2

l1

eሶ

C2

−

m2

l2

eሶ

2

+

m2gS1212120107.3.2

向内迭代：力和力矩的计算➢向内迭代：连杆2到连杆1

(i

=

1)

:1f1

=

R2f2

+

1F1

=

m12

l1

1

−

m2

l2S2eሶ

2

+

m2

l2C2

12

+

m12gC1

1221T1

=

1n

1

1C2

+

m2

l1

l2

eሶ

−

eሶ

2

S2

+

m2

l

12

+

m2

l2gC12

+

m12

l

1

+

m12

l1gC1122212121T00S2

+

m2

l

12

+

m2

l2gC12

+

m12

l

1

+

m12

l1gC112227.3

牛顿-欧拉迭代动力学方程0为简化表示m12

=

m1

+

m21n1

=

1N1

+

R2n2

+

1PC1

×

1F1

+

1O2

×

R2f22121−m12

l1

eሶ

−

m2

l2C2eሶ

2

−

m2

l2S2

12

+

m12gS11212

eሶ

−

eሶ

2

1212

1

+

12

1

+

12

C2

+

m2

l1

l2=

m2

l1

l2m2

l1

l2=⚫牛顿-欧拉方法是基于动力学方程以及作用在连杆之间约束力和力

矩的分析之上的⚫拉格朗日力学是基于能量项对系统变量及时间微分的动力学方法⚫对于一个机器人来说，这两种方法得到的运动方程是相同的⚫运用拉格朗日力学比运用牛顿力学更繁琐，

但随着系统复杂程度的增加，前者将变得相对简单7.4

机器人动力学方程的拉格朗日方法⚫机器人的动力学方程可表示为：

−

=

新⚫新是非保守力/力矩向量⚫它包括关节力/力矩向量T=[T…T]T、摩擦力/力矩向量B小ሶ

、末端执行1

N器与环境接触而引起的关节负荷力/力矩向量JT

小F小ℒaa小ሶℒaa⚫关节向量巾为广义坐标⚫k小,小ሶ

：系统动能⚫u小：系统势能⚫一个机械结构系统的动能和势能的差值称为拉格朗日函数，表示为：7.4

机器人动力学方程的拉格朗日方法ℒ小,小ሶ

=k小,小ሶ

−

u小⚫本章中假设机器人末端执行器与环境不接触，因此末端执行器与环境的接触力/力

矩向量F

=0⚫机器人的动力学方程可进一步表示为：

−

+=T

−

B小ሶ小uaa小kaa小ሶkaa7.4

机器人动力学方程的拉格朗日方法

−

=

新小ℒaa小ሶℒaa式中，B=diag(b,…,b)，1

Nb

为折算到关节i的粘滞摩擦参数ℒ小,小ሶ

=k小,小ሶ

−

u小新=T−

B小−

JTሶ

小Fi⚫i仙i

=R

仙i

=

RT仙i⚫整个操作臂的动能是各个连杆动能之和:

nk=⃞kii00i7.4.1

动能的计算⚫考虑N连杆机器人，其中ki表示连杆i的动能⚫连杆i的动能：=miU

iUCi

+仙RCiIi

RT仙ii0i0iTCTki

=miU

iUCi

+i仙CiIii仙iiTCT7.4

机器人动力学方程的拉格朗日方法i=1⚫运用前面微分运动学部分引入的雅可比矩阵，可由关节变量计算UCi以及仙i：

UCi

=J小ሶ,仙i

=J小ሶ⚫将各连杆的动能相加，并注意J

J

)和

R都依赖于Φ，就得到机器人的总动能：i0O(iOiPi⚫对称矩阵M小称为惯性矩阵：M小=⃞mi

J

J+J

RCiIi

RTJOii0i0OiPiPiN

T

Ti=1⚫因为机器人的总动能非负，且仅在巾ሶ

=0时总动能为零，所以惯性矩阵还是一个正定矩阵k小,小ሶ

=⃞i=1ki

小,小ሶ

=2

小ሶTM

小小ሶN

17.4

机器人动力学方程的拉格朗日方法ki

=miU

iUCi

+仙RCiIi

RT仙ii0i0iTCT⚫操作臂的总势能：nu=⃞

uii=1⚫将各连杆势能相加，并注意

0PCi依赖于Φ，就得到机器人的总势能：7.4.2

势能的计算⚫0g表示世界坐标系中的重力加速度向量⚫例如，如果以y轴为竖直向上方向，则0g=0,−g,0T⚫PCi是连杆质心的位置矢量⚫连杆i的势能：u小=⃞ui

小NN7.4

机器人动力学方程的拉格朗日方法=−

⃞mi

g0T

0PCiui

=−mi

g0T0PCii=1i=1Npሶ1

T

m11

pሶ2

m21k

=

小ሶTM

小

小ሶ

=

i1⋮

⋮

pሶN

mN1⚫mij

=mij

小是矩阵M小

第i行第j列元素⚫利用展开后的总动能表达式，可计算得到：ii7.4.3

完整的拉格朗日动力学方程⚫由前述推得的机器人的总动能方程和总势能方程可得到完整的机器人动力学方程m11m21⋮mj1⋮N1m12m22⋮mj2⋮N2⋯m1i

⋯m1N⋯m2i

⋯m2N⋱⋮⋱⋮⋯

mji

⋯

mjN⋱⋮⋱⋮⋯mNi

⋯

mNN⋯m1j

⋯m1N⋯m2j

⋯m2N⋱⋮⋱⋮⋯mij

⋯

miN⋱⋮⋱

⋮⋯mNj

⋯

mNN⋯m1j

⋯m1N⋯m2j

⋯m2N⋱⋮⋱⋮⋯mij

⋯

miN⋱⋮⋱

⋮⋯mNj

⋯

mNN7.4

机器人动力学方程的拉格朗日方法

−

+=T

−

B小ሶ小uaa小kaa小ሶkaa①

=

pሶ1

T

pሶ2

⋮

pሶj

⋮

pሶNpሶ1pሶ2⋮pሶj⋮pሶNm11m21⋮mi1⋮N1m12m22⋮mi2⋮N2m12m22⋮mi2⋮N2pሶ1

pሶ2

⋮

pሶj

⋮

pሶN00⋮1⋮000⋮1⋮0+

12mmmmmT⋯m1j

⋯m1N⋯m2j

⋯m2N⋱⋮⋱⋮⋯mij

⋯

miN⋱⋮⋱

⋮⋯mNj

⋯

mNN=⃞1

⃞=1

pሶkpሶj

+

⃞

1

mijpሷjkN7.4

机器人动力学方程的拉格朗日方法d

mi1

mijdt

miNmi2mi2mi2ddt

−

+=T

−

B小ሶ小uaa小kaa小ሶkaamij

=⃞=1

pሶkkN00

⋮

1

⋮

0pሶ1

pሶ2

⋮

pሶj

⋮

pሶNmi1

T

mi2

⋮

mij⋮miNmi1

mi2

⋮

mij

⋮

miNpሶ1pሶ2⋮pሶj⋮pሶNpሶ1pሶ2⋮pሶj⋮pሶNpሷ1pሷ2⋮pሷj⋮pሷNd

ak

ddtaϕሶi=

dt⚫M小是对称矩阵，有：m11m21⋮m12m22⋮=

⋮

mN1⋮

mN2mi2mi1+=TTTam11apiam21api⋮amj1api⋮amN1apiam12apiam22api⋮amj2api⋮amN2api⋯⋯⋱⋯⋱⋯am1kapiam2kapi⋮amjkapi⋮amNkapi⋯⋯⋱⋯⋱⋯am1Napiam2Napi⋮amjNapi⋮amNNapi=⃞1

⃞=1

pሶkpሶj③=−

⃞1

mj

0gT

=gi

小

kN⃞=1

k

pሶk

⃞=1

k

pሶk⋮⃞=1

pሶk⋮⃞=1

k

pሶkkNkNkNkN7.4

机器人动力学方程的拉格朗日方法

−

+=T

−

B小ሶ小uaa小kaa小ሶkaapሶ1pሶ2⋮pሶj⋮pሶNpሶ1pሶ2⋮pሶj⋮pሶNpሶ1pሶ2⋮pሶk⋮pሶN

=

1=

2

②

TT⚫则有：⃞1

⃞=1

−

pሶkpሶ,

=⃞1

⃞=1

+−

pሶkpሶ,

=⃞1

⃞=1

Ck,ipሶkpሶ,1

ami,

amik

am,kkNkNkN

=⃞=1

⃞=1

pሶkpሶ,

+

⃞1

mi,pሷ,

=

⃞

1

⃞

=1

pሶkpሶ,kNkN,N7.4

机器人动力学方程的拉格朗日方法Ck,i

=

2

apk

+

ap,

−

api

称为

(第一类)

Christoffel符号⃞1

⃞=1

pሶkpሶ,

=⃞1

⃞=1kNkN

−

+=T

−

B小ሶ小uaa小kaa小ሶkaa⃞

1

mi,pሷ,+⃞

1

⃞

=1kNapi

=

−

⃞,=1

m,

0gT

api

=

gi

小

pሶkpሶ,+gi

小=Ti−

bipሶi,i=1,2,⋯,Nami,

1

am,kapk

2

apiami,

amikapk

ap,

au

N

a0PCj⚫可证：pሶkpሶ,+−Ck,i

=

2

apk

+

ap,

−

api⚫因为M小是对称矩阵，有：⚫可以发现：C,ki

=

2

ap,

+

apk

−

apim,k=

mk,C,ki=

Ck,i⚫利用(第一类)

Christoffel符号，

拉格朗日动力学方程可写成更简洁的形式：N

N

Nmi,pሷ,

+−

pሶkpሶ,

+gi

小=Ti

−

bipሶi,i=1,2,⋯,N⃞1

mi,pሷ,+⃞1

⃞=1

Ck,ipሶkpሶ,+giΦ=Ti−

bipሶi,i=1,2,⋯,NkN7.4

机器人动力学方程的拉格朗日方法1

amik

ami,

amk,1

ami,

amik

am,km12m22⋮mi2⋮N2⋯⋯⋱⋯⋱⋯m1,

⋯

m2,

⋯

⋮

⋱mi,⋯

⋮⋱mN,⋯⃞kCk11pሶk

⃞kCk12pሶk⋮⃞kCk1ipሶk⋮⃞kCk1Npሶk⃞kCk21pሶk

⃞kCk22pሶk⋮⃞kCk2ipሶk⋮⃞kCk2Npሶk⋯⃞Ck,1pሶkk⋱⋮⋯⃞Ck,ipሶkk⋯⋯⋱⋯⋱⋯⃞kCkN1pሶk

⃞kCkN2pሶk⋮⃞kCkNipሶk⋮⃞kCkNNpሶkpሶ1

pሶ2

⋮

pሶ,

⋮

pሶNg1

g2⋮gi⋮

N=T1T2⋮Ti⋮TN⃞

1

mi,pሷ,+⃞

1

⃞

=1

Ck,ipሶkpሶ,+

gi将i=1,2,⋯,N所有等式写成如下的矩阵形式:kNpሷ1

pሷ2

⋮

pሷ,

⋮

pሷN+10⋮0⋮00b2⋮0⋮0⋯⋯⋱⋯⋱⋯0⋯0⋯⋮⋱bi

⋯⋮⋱0⋯0

0

⋮

0⋮

bNpሶ1

pሶ2

⋮

pሶi⋮

pሶN+⚫矩阵C的第(i,j)项元素被定义为:Ci,

=⃞Ck,ipሶk7.4

机器人动力学方程的拉格朗日方法M小小ሷ

+C