5、机器人动力学解析

例:假设R-P机器人的实际参数为：
m1 10kg, r1 1m, r : 1 ~ 2m, 负载变化范围： 1 ~ 5kg, 最大速度 1rad / s, r 1m / s, 最大加速度 1rad / s , r 1m / s
2 2
r
r1
Y
m2

X
m1
计算3种情况下的关节1的驱动力： 1 ）手臂伸在最外端，在垂直和水平位置静止状态下； 2 ）手臂伸在最外端，以最大速度从垂直位置运动到水平位置； 3 ）手臂伸在最外端，静止，但以最大径向加速度启动 （垂直和水平两种状态）
机器人的总动能为 Ek Ek1 Ek 2
2 2 2 1 1 1 m1r12 m2 r m2 r 2 2 2 2
3、机器人的势能
质量为m，高度为h的质点的势能定义为E p mgh
E p1 m1 gr 1 sin 连杆1和2的势能为 E p 2 m2 gr sin
机器人是一个具有多输入和多输出的复杂的动力学系统， 存在严重的非线性，需要非常系统的方法来处理。 动力学的原问题：给定力/力矩，求解机器人的运动； 是非线性的微分方程组，求解困难。 动力学的逆问题：已知机器人的运动，计算相应的力/力矩， 即实现预定运动所需施加的力矩；不求解 非线性方程组，求解简单。
关节2是移动关节，所以f 2是作用力

该R-P机器人的动力学方程为：
f1 m r m2r 2m2r r g cos(m1r1 m2r )
2 11 2
f 2 m2 r m2 r2 m2 g sin

该方程 表示关 节上的 作用力 与各连 杆运动 之间的 关系
m
ydm
m z V zdV m zdm
4、伪惯性矩阵定义为
x 2 xy xy y 2 T J V rr dm V xz yz y x y z 1 ,
2 2


Y X
说明哥氏力有影响，但与重力比，影响较小
3）情况
1 D1 D11 196 cos 30 1 30 ~ 226kgm2 / s 2
重力负载变化极大，在垂直状态是零，在水平时是最大（196）对机器人控制 影响很大，在实际中采用平衡的方法或前馈补偿的方法。
2 2 2 2
Z
{A}
I zz V ( y 2 x 2 )dV m ( y 2 x 2 )dm dm dV
h Y l X w
2、惯性积（混合矩）定义为：
I xy V xydV m xydm I yz V yzdV

Lagrange动力学方法的基本步骤：
1、计算各连杆的质心的位置和速度；
2、计算机器人的总动能； 3、计算机器人的总势能； 4、构造Lagrange函数L； 5、推导动力学方程。
d Ek Ek E p fi dt q qi qi
i
d L L fi dt q qi
《机器人学》
第五章、机器人动力学 战强
北京航空航天大学机器人研究所
第五章、机器人动力学
机器人动力学是研究机器人的运动和作用力之间的关系。
机器人动力学的用途：
机器人的最优控制；优化性能指标和动态性能、调整伺服增益； 设计机器人：算出实现预定运动所需的力/力矩；
机器人的仿真：根据连杆质量、负载、传动特征的动态性能仿真。
惯性张量跟坐标系的选取有关，如果选取的坐标系使各惯性积 为零，则此坐标系下的惯性张量是对角型的，此坐标系的各轴 叫惯性主轴，质量矩叫主惯性矩。
刚体质量和分布的一阶矩阵定义为：
m V dV m x V xdV m xdm m y V ydV
1 ）情况 1 D1 ( m1r1 m2 r )g cos 20 * 9.8 cos 水平 0， 1 196kgm 2 / s 2； 垂直时 90kgm 2 / s 2， 1 0
2）情况
1 D1 D112 r D121 r 196 cos 20 1 20 ~ 216kgm / s
4、Lagrange动力学方程的一般形式
f1 m r m2 r 2m2 r r g cos(m1r1 m2 r )
f 2 m2 r m2 r2 m2 g sin
f1 D11 D12 r D111 D122 r D112 r D121 r D1 f 2 D21 D22 r D211 D222 r D212 r D221 r D2
Dii : 关节i的有效惯量；Dii qi 是关节i的加速度在关节i上产生的作用力矩 Dij (i j ) : 关节j对i的耦合惯量；Dij q j 是关节j的加速度在关节i上的作用力矩 Dijj q j : 关节j的速度在关节i上产生的向心力 Dijk q j qk , Dikj qk q j 是作用在关节i上的哥氏力 Di : 作用在关节i上的重力
惯性力项 对照可得：
2 11


2
2




2
2


向心力项
2
哥式力项
重力项
f1 m r m2r 2m2r r g cos(m1r1 m2r )
f 2 m2 r m2 r2 m2 g sin
D11 m1r12 m2 r 2 ; D12 0; D111 0; D122 0; D112 m2 r ; D121 m2 r ; D1 g cos (m1r1 m2 r ) D21 0; D22 m2 ; D211 m2 r ; D222 0; D212 0; D221 0; D2 m2 g sin
x2 r cos 质心 m2 的位置是 y r sin 2
r C
速度是
x r cos r sin 2 y r sin r cos 2
2 2 2 2
2 速度的模方是 v2 x2 y 2 r r 2
i
5.2 惯性矩阵、惯性积和惯性张量
在R-P机器人的例子中假设各连杆的质量集中在一点，实际上各 连杆的质量是均匀分布的，对于这种情况存在几个特殊的公式。
1、图示均质刚体，绕X、Y、Z轴的惯性矩阵定义为：
I xx V ( y 2 z 2 )dV m ( y 2 z 2 )dm I yy V ( x z )dV m ( x z )dm
m yzdm
I zx V zxdV m zxdm
3、对于给定的坐标系{A}，惯性张量定义为
I xx A I I xy I xz I xy I yy I yz I xz I yz I zz
相对于某一坐标系的 质量分布的二阶矩阵， 表示物体的质量分布

D2 2 m2
D1 1 m1r12 m2r 2
有效惯量对于移动关节是质量，对于转动关节是惯性矩
机器人的有效惯性量和耦合惯性量，随机器人的形态变化而 变化，跟负载、机器人是自由状态/锁死状态有关，变换范围 大，对机器人的控制影响巨大。对于一个机器人的控制而言， 需要计算出各个有效惯量、耦合惯量与机器人位置形态之间 的关系。
动力学方法很多，如Lagrange、Newton-Euler、Gauss、Kane、 Screw、Roberson-Wittenburg。
5.1 Lagrange动力学方法
Lagrange法：能以最简单的形式求得非常复杂的系统动力学方程， 而且具有显式结构。 Lagrange函数L定义：任何机械系统的动能 Ek 和势能E p 之差
x1 r1 sin 速度是 y1 r1 cos
m2
r1
Y

X
m1
笛卡儿
r1 C
速度的模方是 v12 x1 y 1 r12
2
2
2
Cartesian(Latin)[ka:’ti:zjən] Descartes[dei’ka:t]: 法国哲学家、 数学家、物 理学家，1596-1650，将笛 卡尔坐标体系公式化而被 认为是解析几何之父。
L Ek E p
动能和势能可以用任意选取的坐标系来表示，不局限于笛卡儿坐标 假设机器人的广义坐标为 qi , i 1,2,, n
d L L 则该机械系统的动力学方程为： f i dt q qi
i
（5-1）
qi 是直线坐标，f i 是力； 广义力 qi 是角度坐标，f i 是力矩
2
2 1 1

2




2
2




2

2


f1 D11 D12 r D111 D122 r D112 r D121 r D1 f 2 D21 D22 r D211 D222 r D212 r D221 r D2
2、机器人的动能
质量为m，速度为v的质点的动能定义为 Ek 1 2 mv 2
2 1 1 2 2 E mv mr 连杆1和2的动能 k1 2 1 1 2 1 1 为 2 2 质量m1 , m2的动能 1 1 2 2 Ek 2 m2v2 m2 [r r ] 2 2
d L L fi dt q qi
i
广义速度 将 L Ek E p 代入到（5-1）式中：
d Ek Ek d E p E p fi ( )( ) dt q qi dt q qi