A Aɺ A A ɺ ɺ ɺ P = AVP = B R BP + B R BP + APBORG ⇒ AVP = AVBORG + B R BVP + A Ω B × A P
刚体的线加速度（1）
坐标系{B}和{A}原点重合，P点的线加速度为：
A A A A VP = B R BVP + A Ω B × A P = B R BVP + A Ω B × B R BP ⇒ A ɺ ɺ ɺ ɺ ɺ ɺ V = AR BV + AR BV + A Ω × A R BP + A Ω × A R BP + AR BP P B P B P B B B
列出平衡方程
牛顿-欧拉迭代动力学方程（5）
整理得到：
通过计算力或力矩在Z方向的分量即可求得关节力矩： 对于转动关节
对于移动关节
牛顿-欧拉迭代动力学方程（6）
题目：一连杆动力学方程
首先，确定牛顿-欧拉公式中各 参量的值。连杆质心位置矢量 为：
1
g
Pc = [a / 2,0,0]T
0 0 0 ma C I1 = 0 1 0 12 0 0 1
ɺ N = Iω + ω× Iω
c c
牛顿-欧拉迭代动力学方程（2）
连杆i+1角速度和角加速度
i +1
ɺ ˆ ωi +1 = i +i1R iωi + θ i +1 i +1Z i +1
i +1
ɺ ɺ ˆ ɺ ɺ ωi +1 = i +i1R iωi + i +i1R iωi + θɺi +1 i +1Z i +1
(
B
A A A A ɺ ɺ A = A Ω B × B R BVP + B R BVP + A Ω B × B R BP + A Ω B × A Ω B × B R BP + B R BVP ⇒ A ɺ ɺ ɺ V = AR B V + A Ω × A R B P + A Ω × A Ω × A R B P + 2 A Ω × A R B V P B P B B B B B B B P
i +1
ɺ i +1×i +1PCi +1 + i +1ωi +1 ×( i +1ωi +1×i +1PCi +1 )+ i +1vi +1 ɺ ɺ vCi +1 = ω
i +1 0
ɺ ω0 = 0 ω0 = 0
教材P137页式（6-34） （6-36）有误
牛顿-欧拉迭代动力学方程（4）
得到连杆质心线加速度和角加速度 后，运用牛顿-欧拉公式便可求出作 用在连杆质心上的惯性力和力矩。即
题目：RP连杆动力学方程
0
X1 Z2 Y1 X2
ω0 = 0,0 v0 = 0
0 1 0 0 1 0 ɺ T θ
当P点相对{B}静止时，P点的线加速度简化为：
A A ɺ ɺ ɺ A VP = AVBORG + A Ω B × B R BP + A Ω B × A Ω B × B R BP
P132页，式(6-12)有误。
刚体的角加速度
坐标系{B}相对{A}转动 ，坐标系{C}相对{B}，则A
A A Ω c = A Ω B + B R B ΩC
A Aɺ A A Aɺ A A ɺ ɺ P = AVP = B R BP + B R BP⇒ R )T A P = B R BVP + A Ω B × A P
坐标系{B}和{A}原点不重合，其在另一个坐标系{A}中的描述为 AP=A R BP+AP B BORG。那么P点的速度为：
2
在坐标系{1}中的惯性张量为：
其余：
ɺ ɺ f 2 = n2 = 0; ω0 = ω0 = 0; v0 = [− g ,0,0]
0
T
题目：一连杆动力学方程（2）
旋转变换阵为：
cθ 0 1 R = sθ 0 − sθ cθ 0 0 0 1
应用方程(6-45)-(6-53)对连杆1向外迭代法求解如下：
C为
坐标系{C}相对{A} 的角加速度为：
A Aɺ A ɺ ɺ ɺ Ωc = A Ω B + B R B ΩC + B R B ΩC ⇒ A ɺ ɺ ɺ Ω = A Ω + A Ω × A R B Ω + AR B Ω c B B B C B
C
刚体的质心、惯性张量
物体的质心定义为：
c= 1 A ∫ PρdV mV
ɺ ˆ ˆ ɺ ɺ = i +i1R iωi − θ i +1 i +1Z i +1×i +i1R iωi + θɺi +1 i +1Z i +1 ɺ ɺ ˆ ˆ ɺ = i +1R iω + i +1R iω × θ i +1Z + θɺ i +1Z
i i i i i +1 i +1 i +1 i +1
上式称为欧拉-拉格朗日方程。
拉格朗日动力学方程（2）
对于操作臂第i个连杆的动能可表达为：
整个系统的动能为各个连杆动能之和： 速度的函数 对于操作臂第i个连杆的势能可表达为：
它是关节位置和
整个系统的动能为各个连杆势能之和： 函数
它是关节位置的
拉格朗日动力学方程（3）
操作臂的拉格朗日函数可表达为：
操作臂运动方程为：
A
ɺ AR T Ω B ×= R B
A B
( )
0 = Ωz − Ω y
− Ωz 0 Ωx
Ωy − Ωx 0
于是有：AVP = A Ω B × A P 坐标系{B}相对于{A}的角速度等于，B到A的旋转矩阵的微分 乘以B到A的旋转矩阵的转置
刚体的线速度
坐标系{B}和{A}原点重合，假定固定矢量BP相对于坐标系 {B}是时变的，其在另一个坐标系{A}中的描述为AP=ABR BP. 如果坐标系{B}相对于坐标系{A}是旋转的，即 ABR是时变 的。那么P点的速度为：
第六章 动力学
动力学是机器人控制的基础，本章将考虑由 驱动器施加的力矩使操作臂运动。 解决问题： 已知关节的位置、速度和加速度，求出驱动关节 运动所需的力矩矢量； 方法： 牛顿-欧拉法 拉格朗日法
刚体的角速度
坐标系{B}和{A}原点重合，假定固定矢量BP相对于坐标系 {B}是不变的，其在另一个坐标系{A}中的描述为AP=ABR BP. 如果坐标系{B}相对于坐标系{A}是旋转的，即 ABR是时变 A ɺ ɺ ɺ A ɺ A 的。那么P点的速度为：P = AVP = BAR BP⇒ AVP = BAR( B R) −1 A P⇒ AVP = BAR( B R)T A P 定义角速度矢量 A Ω B = [Ω x Ω y Ω z ]T
式中m是物体的质量，ρ是密度 坐标系{A}中物体的惯性张量为：
平等轴定理
坐标系{C}是以刚体的质心为原点，坐标系{A}与{C}平行，那么 惯性张量AI和CI存在在下关系：
A
式中：m为物体质量，Pc = [xc 1 0 0 中的位置。E3 = 0 1 0 0 0 1
I = C I + m( PcT Pc E3 − Pc PcT )
(
B
)
)
相对加速度
转动加速度
向心加速度
科氏加速度
牵引加速度
刚体的线加速度（2）
坐标系{B}和{A}原点不重合，P点的线加速度为：
A A A A ɺ ɺ ɺ ɺ A VP = AVBORG + B R BVP + A Ω B × B R BP + A Ω B × A Ω B × B R BP + 2 A Ω B × B R BVP
yc
zc ] 表示刚体质心在坐标系{ A}
T
一般是通过测量装置来得到连杆的惯性张量。
长方体的惯性张量
牛顿-欧拉迭代动力学方程（1）
问题：已知关节的位置、速度和加速度，如何计算出驱动关节运 动所需要的力矩 牛顿方程描述了移动物体的力与加速度的关系：
ɺ F = mvC
欧拉方程描述了转动物体的力矩与角速度、角加速度的关系：
题目：一连杆动力学方程（3）
应用方程(6-45)-(6-53)对连杆1向外迭代法求解如下：
ɺ ɺ − gcθ − aθ 2 / 2 − m1 gcθ − m1aθ 2 / 2 ɺ 1 ɺ F1 = m1 aθɺ / 2 + gsθ = m1aθɺ / 2 + m1 gsθ 0 0 0 0 0 0 2 2 ma 0 + ma 1 C 1 1 C 1 ɺ N1 = I1 ω1 + ω1× I1 ω1 = 0 1 0 12 12 ɺ 0 0 1 θɺ 0 ma = 0 12 ɺɺ θ
1
ɺˆ ɺ ɺ ɺ ɺ ˆ ω1 =θ 1Z1 = [0,0, θ ]T ,1ω1 =θɺ1Z1 = [0,0, θɺ]T
cθ sθ 0 − g − gcθ 1 ɺ v1 = − sθ cθ 0 0 = gsθ 0 0 1 0 0 ɺ ɺ ɺ 0 − θɺ 0 a / 2 0 − θ 0 0 − θ 0 a / 2 − gcθ ɺ ɺ ɺ 1 ɺ vc = θɺ 0 0 0 + θ 0 0 θ 0 0 0 + gsθ 0 0 0 0 0 0 0 0 0 0 0 0 ɺ2 ɺ2 0 − aθ / 2 − gcθ − gcθ − aθ / 2 ɺ ɺ = aθɺ / 2 + 0 + gsθ = aθɺ / 2 + gsθ 0 0 0 0