控制系统工程案例分析-机器人技术(第三章-机器人动力学)

p 2 = − m 2 gd 1Cos (ϑ1 ) − m 2 gd 2 Cos (ϑ1 + ϑ 2 )
(3.11)
3. 2. 2 拉格朗日算子 ( The Lagrangian ) 拉格朗日算子 L = K – P 可根据式(3.3)、(3.4)、(3.10)和(3.11)求得
L = 1 1 ( m 1 + m 2 ) d 1 2ϑ 1 2 + m 2 d 2 2 (ϑ 1 2 + 2 ϑ 1 2 ϑ 2 2 + ϑ 2 2 ) 2 2
3.2 拉格朗日力学 —— 一个简例 ( Lagrangian Mechanics — A Simple Example )
拉格朗日算子 L 定义为系统的动能 K 与势能 P 的差 L=K–P (3.1)
系统的动能和势能可以用任何能使问题简化的坐标系统来表 示，并不一定要使用笛卡尔坐标。 动力学方程通常表述为
2
把方程(3.16)、(3.20)与(3.21)、(3.22)比较，我们就得到各项系数的值： 等效惯量系数 D11 = [ (m1 + m2) d12 + m2 d22 + 2m2 d1 d2 cos(θ2 ) ] D22 = m2 d22 耦合惯量系数 D12 = m2 d22 + m2 d1 d2 cos(θ2 ) 向心力系数 D111 = 0 D122 = - m2 d1 d2 sin(θ2 ) D211 = m2 d1 d2 sin(θ2 ) D222 = 0 (3.26) (3.27) (3.28) (3.29) (3.25) (3.23) (3.24)
∂L 2 = − m 2 d 1 d 2 Sin (ϑ 2 ) ϑ 1 + ϑ 1ϑ 2 − m 2 gd 2 Sin (ϑ 1 + ϑ 2 ) ∂ϑ 2
(3.18)
(
)
(3.19)
于是关节2的力矩为
T2 = [m 2d
2 2
+ m 2 d 1 d 2 Cos ( ϑ 2 )] ϑ 1 + m 2 d
0 下面给两连杆机械手赋予具体数值，并且对于静止状态（ ϑ 1 = ϑ 2 = ）和 在无重力环境中的机械手求解方程(3.21)和(3.22)。求解在下列两种条件下进 0 行: 关节2处于锁定状态（ ϑ 2 =）；关节2处于自由状态（T2 = 0）。在第一种
条件下, 方程(3.21)和(3.22)简化为
速度的直角坐标分量为
x 2 = d 1 Cos (ϑ 1 )ϑ 1 + d 2 Cos (ϑ 1 + ϑ 2 )( ϑ 1 + ϑ 2 )
(3.7)
y 2 = d 1 Sin (ϑ 1 )ϑ 1 + d 2 Sin (ϑ 1 + ϑ 2 )( ϑ 1 + ϑ 2 )
速度平方的值为
(3.8)
V 2 = d 1 ϑ 1 + d 1 (ϑ1 + 2ϑ1 ϑ 2 + ϑ 2 )
+ m 2 d 1 d 2 Cos ( ϑ 2 )( ϑ 1 2 + ϑ 1ϑ 2 ) + ( m1 + m 2 ) gd 1Cos (ϑ1 ) + m 2 gd 2 Cos (ϑ1 + ϑ 2 )
(3.12)
3. 2. 3 动力学方程 ( The Dynamics Equations )
为了求得动力学方程，我们现在根据式（3.2）对拉格朗日算子进行微分
T 1 = [ D 11
D 2 12 − ]ϑ 1 D 22
(3.36)
现在，取定 d1 = d2 = 1 ，m1 = 2，而对于三个不同的 m2 值，分别求出各个 系数： m2 = 1，表示机械手无负载情况；m2 = 4 ，表示有负载；m2 = 100 ，表 示位于外太空的机械手在无重力环境下的负载（在外太空，没有重力负载，允 许较的工作负载）。根据求得的系数以及方程(3.34)和(3.35)，分别对应关节2的 四种不同的锁定状态 IL 和自由状态 If ，计算关节1的惯量如下表所示（表中 IL 表示锁定状态，If 表示自由状态）。 表3.1 m1 = 2，m2 = 1，d1 = 1，d2 = 1
− 2 m 2 d 1 d 2 Sin (ϑ 2 )ϑ 1ϑ 2 − m 2 d 1 d 2 Sin (ϑ 2 )ϑ 2
2
+ ( m 1 + m 2 ) gd 1 Sin (ϑ 1 ) − m 2 gd 2 Sin (ϑ 1 + ϑ 2 )
（3.16）
用拉格朗日算子对 2和ϑ2 求偏微分，进而得到关节2的力矩方程 ϑ
哥氏力系数 D112 = D121 = - m2 d1 d2 sin(θ2 ) D212 = D221 = 0 重力项 D1 = (m1 + m2) g d1 sin(θ1 ) + m2 g d2 sin(θ1 +θ2 ) D2 = m2 g d2 sin(θ1 +θ2 ) (3.32) (3.33) (3.30) (3.31)
T 1 = D 11 ϑ 1
(3.34) (3.35)
T 2 = D 12 ϑ 1
在第二种条件下，T2 = 0 ，我们可以由方程(3.22)解出 ϑ2 ，再把它代入方 程(3.21)，得到T1
T 2 = D 12 ϑ 1 + D 22 ϑ 2 = 0
于是
ϑ2 = −
D 12 ϑ1 D 22
代入方程(3.21)有
θ2
Cosθ2 1 0 -1 0
D11 3 4 2 4
D12 2 1 0 1
D22 1 1 1 1
IL 3 4 2 4
If 2 3 2 3
0
90
180
270
表3.2 m1 = 2 ，m2 = 4 ，d1 = 1，d2 = 1
θ2
0
90
Cosθ2 1 0 -1 0
D11 18 10 2 10
D12 8 4 0 4
第三章 动力学
Chapter Ⅲ Dynamics
3.1 引言 3.2 拉格朗日力学 3.3 机械手的动力学方程
3.1 引言 ( Introduction )
动力学是机器人控制的基础，本章主要从控制的角度来研究机 械手的动力学问题。机械手通常是一种开链式多关节机构，是一种 复杂的动力学系统，需要采用系统的分析方法来研究它的动态特 性。由于拉格朗日方法能以最简单的形式求得非常复杂的系统的动 力学方程，本章我们运用拉格朗日力学原理来分析机械手的动力学 问题。本章的主要内容如下： 运用拉格朗日力学原理分析和求取两自由度机械手的动力学方程； 介绍六自由度机械手动力学方程的求取方法和步骤； 推导出完整的动力学方程，然后根据有效性分析来简化这些方程。
∂L 2 2 = m 2 d 2 ϑ 1 + m 2 d 2 ϑ 2 + m 2 d 1 d 2 Cos ( ϑ 2 ) ϑ 1 ∂ϑ 2
d ∂L 2 2 = m 2 d 2 ϑ 1 + m 2 d 2 ϑ 2 + m 2 d 1 d 2 Cos (ϑ 2 )ϑ 1 dt ∂ ϑ 2
(3.17)
− m 2 d 1 d 2 Sin ( ϑ 2 ) ϑ 1ϑ 2
2 2 2 2 2 2 2 2
+ 2 d1d 2 Cos (ϑ1 ) Cos (ϑ1 + ϑ 2 )(ϑ12 + ϑ1ϑ 2 )
+ 2 d 1 d 2 Sin (ϑ1 ) Sin (ϑ1 + ϑ 2 )(ϑ1 + ϑ1ϑ 2 )
2
= d 1 ϑ1 + d 1 (ϑ1 + 2ϑ1 ϑ 2 + ϑ 2 ) + 2 d 1 d 2 Cos (ϑ 2 )(ϑ1 + ϑ1ϑ 2 )
2 2
T2 = D12ϑ1 + D 22ϑ 2 + D 211ϑ1 + D 222 ϑ 2 + D 212 ϑ1ϑ 2 + D 221ϑ 2ϑ1 + D 2 （3.22）
2 2
在方程（3.21）和（3.22）中各项系数 D 的含义如下： Dii — 关节 i 的等效惯量（Effective inertia）系数 关节 i 的加速度使关节 i 产生的力矩 D ii ϑ i Dij — 关节 i 与关节 j 之间的耦合惯量（Coupling inertia）系数 关节 i 或关节 j 的加速度分别使关节 j 或 i 产生的力矩 Dijϑi 和 Dijϑ j Dijj —向心力（Centripetal force）系数 关节 j 的速度产生的作用在关节 i 上的向心力 Dijjϑ j Dijk —哥氏力（ Coriolis force）系数 关节 j 和关节 k 的速度作用在关节 i 上的复合向心力 Dijk ϑ jϑ k + Dijk ϑ kϑ j Di — 作用在关节 i 上的重力（Gravity）
Fi =
∂L d ∂L − ∂qi dt ∂ q i
(3.2)
其中，qi是广义坐标变量， i 是速度，而Fi是对应的力或力矩，Fi是 q 力还是力矩，这取决于qi是直线坐标还是角度坐标。这些力、力矩 和坐标分别称为广义力、广义力矩和广义坐标。
y 为了说明问题，我们看一个 具体例子，假定有如图3.1所示的 两连杆的机械手，两个连杆的质 量分别为m1、m2，由连杆的端部 质量代表，两个连杆的长度分别 为d1、d2，机械手直接悬挂在加 速度为g的重力场中，广义坐标 为θ1和θ2。 0 d1 m1 d2
（3.15）
根据式(3.2)，把式(3.14)与(3.15)相减就得到关节1的力矩
T1 = [( m 1 + m 2 ) d 1 2 + m 2 d 2 2 + 2 m 2 d 1 d 2 Cos (ϑ 2 )] ϑ 1 + [ m 2 d 2 2 + m 2 d 1 d 2 Cos (ϑ 2 )]ϑ 2
2 2
ϑ
2
2 + m 2 d 1 d 2 Sin ( ϑ 2 ) ϑ 1 + m 2 gd 2 Sin (ϑ 1 + ϑ 2 )
（3.20）
将式(3.16)和(3.20）重写为如下形式
T1 = D11ϑ1 + D12ϑ 2 + D111ϑ1 + D122 ϑ 2 + D112 ϑ1ϑ 2 + D121ϑ 2ϑ1 + D1 （3.21）
+ [ m 2 d 2 2 + m 2 d 1 d 2 Cos (ϑ 2 )]ϑ 2
− 2 m 2 d 1 d 2 Sin (ϑ 2 )ϑ 1ϑ 2 − m 2 d 1 d 2 Sin (ϑ 2 )ϑ 2
2
（3.14）
∂L = − ( m 1 + m 2 ) gd 1 Sin (ϑ 1 ) − m 2 gd 2 Sin (ϑ 1 + ϑ 2 ) ∂ϑ1
p1 = − m1 gd 1Cos (ϑ1 )
分，以便得到速度
(3.4)
对于质量m2，由图3.1，我们先写出直角坐标位置表达式，然后求微
x2 = d1Sin (ϑ1 ) + d 2 Sin (ϑ1 + ϑ2 )
y2 = − d1Cos (ϑ1 ) − d 2Cos (ϑ1 + ϑ2 )
(3.5) (3.6)
θ2
x
θ1
m2
图3.1 两连杆的机械手
3. 2. 1 动能和势能 ( The Kinetic and Potential Energy )
动能的一般表达式为 K =
1 2 mv ，质量m1的动能可直接写出 2
K
1
1 = m 1 d 1 2ϑ 1 2 2
(3.3)
势能与质量的垂直高度有关，高度用y坐标表示，于是势能可直接写出
D22 4 4 4 4
IL 18 10 2 10
If 2 3 2 3
180 270
表3.3 m1 = 2， m2 = 100，d1 = 1，d2 = 1
θ2
0
Cosθ2 1 0 -1 0
D11 402 202 2 202
D12 200 100 0 100
D22 100 100 100 100
2 2 2 2 2 2 2 2
(3.9)
从而动能为
K
2
=
1 1 m 2 d 1 2ϑ 1 2 + m 2 d 2 2 (ϑ 1 2 + 2 ϑ 1 2 ϑ 2 2 + ϑ 2 2 ) 2 2
+ m 2 d 1 d 2 Cos ( ϑ 2 )( ϑ 1 + ϑ 1ϑ 2 )
2
(3.10)
质量的高度由式(3.3)表示，从而势能就是
∂L Hale Waihona Puke Baidu ( m 1 + m 2 ) d 1 2ϑ 1 2 + m 2 d 2 2ϑ 1 2 + m 2 d 2 2ϑ 2 2 ∂ ϑ1
+ 2 m 2 d 1 d 2 Cos (ϑ 2 )ϑ 1 + m 2 d 1 d 2 Cos (ϑ 2 )ϑ 2
（3.13）
d ∂L = [( m 1 + m 2 ) d 1 2 + m 2 d 2 2 + 2 m 2 d 1 d 2 Cos (ϑ 2 )] ϑ 1 dt ∂ ϑ 1