第4章机器人动力学

D 22
1 1 1 1 4 4 4 4 100 100 100 100
I1
6 4 2 4 18 10 2 10 402 202 2 202
If
2 3 2 3 2 6 2 6 2 102 2 102
11
4.1.2 动力学方程的两种求法
4.1.2 动力学方程的两种求法 牛顿-欧拉动态平衡法 错误！
二连杆系统的动力学方程的一般形式为：
r1 r0 (d1 cos1 )i (d1 sin 1 ) j
(d1 cos1 )i (d1 sin 1 ) j r2 r1 [d 2 cos(1 2 )]i [d 2 sin(1 2 )] j [d1 cos1 d 2 cos(1 2 )]i [d1 sin 1 d 2 sin(1 2 )] j
4.1 刚体动力学
5
4.1.1 刚体的动能与位能 二连杆机械手的动能和位能
y T1 d1 θ1 (x1, y1) g m1 x
T2 d2 θ2 m2 (x2, y2)
图4.2 二连杆机器手（1）
4.1 刚体动力学
6
4.1.1 刚体的动能与位能 二连杆机械手系统的总动能和总位能分别为：
K K1 K 2
2
0 90 180 270 0 90 180 270 0 90 180 270
cos 2
1 0 -1 0 1 0 -1 0 1 0 -1 0
D11
6 4 2 4 18 10 2 10 402 202 2 202
D12
2 1 0 1 8 4 0 4 200 100 0 100
W d K K D P , i 1,2, n i qi q i qi qi dt q
(4.11)
式中的W、K、D、P和qi等的含义与拉格朗日法一样；i为 连杆代号，n为连杆数目。
4.1 刚体动力学
12
牛顿-欧拉动态平衡法 质量 m1和m2的位置矢量 r1和r（见图 4.3）为： 2
机器人学基础
1
4.1 刚体动力学
拉格朗日函数L被定义为系统的动能K和位能P之 差，即:
LKP
(4.1)
系统动力学方程式，即拉格朗日方程如下：
Fi d L L , i 1,2, n dt qi qi
(4.2)
i为相应的速 q 式中，q i 为表示动能和位能的坐标， 度，而Fi为作用在第i个坐标上的力或是力矩。
(4.15)
4.2 机械手动力学方程
16
4.2.1 速度的计算 P点的加速度为：
0
d 0 d 3 d 3 T3 3 3 i a p ( v p ) (T3 r p ) T3 r p q rp dt dt dt j 1 qi
3 3 2T3 3 3 T3 d 3 i r p k q j rp q q j 1 qi dt k 1 j 1 q j q k


3 3 2T3 3 3 T3 3 i k q j rp q q r p q q q j 1 i k 1 j 1 j k


4.2 机械手动力学方程
17
4.2.1 速度的计算 速度的平方
或用矩阵形式表示为：
M 1 0 1 c c x 1 k k x1 F 0 x M 0 x 0 c c x 0 k k x 0 F
第四章 机器人动力学 分析机器人操作的动态数学模型，主要采用 下列两种理论：
动力学基本理论，包括牛顿—欧拉方程。 拉格朗日力学，特别是二阶拉格朗日方程。
对于动力学，有两个相反的问题：
其一是已知机械手各关节的作用力或力矩，求各 关节的位移、速度和加速度，求得运动轨迹。 其二是已知机械手的运动轨迹，即各关节的位移、 速度和加速度，求各关节所需要的驱动力或力矩。
(4.10)
4.1 刚体动力学 8
拉格朗日功能平衡法 比较可得本系统各系数如下：
有效惯量
2 D11 (m1 m2 )d12 m2 d 2 2m2 d1d 2 cos 2 2 D22 m2 d 2
耦合惯量
2 2 D12 m2 d 2 m2 d1d 2 cos 2 m2 (d 2 d1d 2 cos 2 )
重力项
D1 (m1 m2 ) gd1 sin 1 m2 gd2 sin( 1 2 ) D2 m2 gd2 sin( 1 2 )
4.1.2 动力学方程的两种求法
10
拉格朗日功能平衡法 表4.1给出这些系数值及其与位置 2 的关系。
表4.1 负 载
地 面 空 载 地 面 满 载 外 空 间 负 载
y v1 T1 x
d1 θ1 v2 r1 m1 T2 d2 θ2
图4.3 二连杆机械
r2 m2
4.1.2 动力学方程的两种求法
13
2．牛顿-欧拉动态平衡法 可得:
2 T1 [(m1 m2 )d12 m2 d 2 2m2 d1d 2 cos 2 ] 1 2 c [m2 d 2 m2 d1d 2 cos 2 ] 2 1 1 (2m2 d1d 2 sin 2 )1 2
(4.4)
4.1 刚体动力学
7
4.1.2 动力学方程的两种求法 拉格朗日功能平衡法
二连杆机械手系统的拉格朗日函数L为：
LKP

1 2 1 m d 2 ( 2 2 2 ) (m1 m2 )d12 1 2 2 1 1 2 2 2 2
2 m2 d1d 2 cos 2 ( 1 2 ) 1 1 2 ) (m1 m2 ) gd1 cos1 m2 gd2 cos(
dK 3 1 2 v p dm 2
T 3 3 T T 1 3 T 3 3 3 i q k dm Trace rp ( rp ) q 2 q qk j 1 k 1 i
图4.1 一般物体的动能与位能
4.1 刚体动力学
3
4.1.1 刚体的动能与位能
x 0, x1 为广义坐标
d K K D P W 1 dt x x x x x1 1 1 1
其中，左式第一项为动能随速度（或角速度） 和时间的变化；第二项为动能随位置（或角度） 的变化；第三项为能耗随速度变化；第四项为 位能随位置的变化。右式为实际外加力或力矩。 表示为一般形式：
m gd sin( ) c2 2 2 2 1 2
(4.13)
4.1.2 动力学方程的两种求法
14
4.2 机械手动力学方程 分析由一组A变换描述的任何 机械手，求出其动力学方程。 推导过程分五步进行：
计算任一连杆上任一点的速度； 计算各连杆的动能和机械手的总 动能； 计算各连杆的位能和机械手的总 位能； 建立机械手系统的拉格朗日函数； 对拉格朗日函数求导，以得到动 力学方程式。
i i T i i i T Ti Trace rr q j 1 k 1 q k k
T k q k q
(4.16)
4.2 机械手动力学方程
19
4.2.2 动能和位能的计算 动能的计算 令连杆3上任一质点P的质量为dm，则其动能 为：
向心加速度系数
D111 0 D122 m2 d1 d 2 sin 2 D211 m2 d1 d 2 sin 2 D222 0
4.1.2 动力学方程的两种求法 9
拉格朗日功能平衡法
哥氏加速度系数
D112 D121 m2 d1d 2 sin 2 D212 D221 0
1 c1 x 1 dx1 F M 1 g M1 x
4.1 刚体动力学
4
4.1.1 刚体的动能与位能
x0 0, x0 和x1均为广义坐标，有下式：
1 c( x 1 x 0 ) k ( x1 x0 ) M1 g F M1 x 0 c( x 1 x 0 ) k ( x1 x0 ) M 0 g F M0 x
式中，Trace表示矩阵的迹。对于n阶方程来说， 其迹即为它的主对角线上各元素之和。
4.2 机械手动力学方程 18
4.2.1 速度的计算 任一机械手上一点的速度平方为：
T 2 i i T T dr 2 i i i i j r k r v Trace q q q dt k 1 q k j 1 j
1 2 1 m d 2 ( ) 2 m d d cos ( 2 (m1 m2 )d12 1 2 2 1 2 2 1 2 2 1 1 2) 2 2
(4.3)
PP 1 P 2
(m1 m2 ) gd1 cos1 m2 gd2 cos(1 2 )
(4.5)
求得力矩的动力学方程式：
T1 D11 T D 2 21 D111 D12 1 D22 2 D211 2 D112 D122 1 2 D222 2 D212 D1 D121 1 2 D221 2 1 D2
2 [(m m ) gd sin m d g sin( )] (4.12) (m2 d1d 2 sin 2 ) 2 1 2 1 1 2 2 1 2
2 m d 2 2 T2 (m2 d 2 m2 d1d 2 cos 2 ) 1 2 2 2 m2 d1d 2 sin 21
O1
O3 连杆2 O2 连杆1
0
3
rp P 连杆4 O4
连杆3 rp
O
图4.4 四连杆机械手
第四章 机器人动力学
15
4.2.1 速度的计算 连杆3上点P的速度为：
0
vp
d 0 d 3r ( r p ) (T3 3 r p ) T 3 p dt dt
对于连杆i上任一点的速度为：
dr v dt i Ti i j r q j 1 q j
第四章 机器人动力学
2
4.1.1 刚体的动能与位能
K P D
1 1 2 12 M 0 x 0 M1 x 2 2
1 k ( x1 x 0 ) 2 M 1 gx1 M 0 gx 0 2 1 1 x 0 )2 c( x 2
百度文库错误！
F F
x0 x1
k M0
c
W Fx1 Fx 0
( 0 v p ) 2 ( 0 v p ) ( 0 v p ) Trace[(0 v p ) ( 0 v p )T ]
3 3 T3 T3 3 T 3 j ( rp ) k Trace q q ( rp ) k 1 q k j 1 q j T 3 3 T3 3 3 T T3 jq k Trace ( r p )( r p ) q q k j 1 k 1 q j