多刚体系统运动学与动力学

如图 1 所示，设有矢量 P，它在坐标系 O0x0y0z0 下的表达式为 P = x0 i 0 + y 0 j0 + z 0 k 0 它在坐标系 O1x1y1z1 下的表达式为
(1)
P = x1 i1 + y1 j1 + z1 k1
因此有
(2) (3) (4) (5) (6)
x0 i 0 + y 0 j 0 + z 0 k 0 = x1 i1 + y1 j1 + z1 k1
，
，
(12)
最终的方向余弦矩阵是这三次方向余弦矩阵的乘积：
(13)
3 齐次坐标
齐次坐标是用 n+1 维坐标来描述 n 维空间中的位置。引入齐次坐标，不仅对 坐标变换的数学表达带来方便，而且也具有坐标值缩放的实际意义。 三维空间任一点 P 在直角坐标系 Oxyz 下的坐标为(x，y，z)T，对应的齐次坐 标为 ( x1 , x 2 , x3 , x 4 ) T ，且：
(16)
x0 x1 1 y 0 = T y1 = 0 z0 z1 0 1 1 0 1 0 0 0 1 0 其中 T 为平移变换矩阵， T = 0 0 1 0 0 0
0 1 0 0
(7)
该矩阵称为方向余弦阵。 假设 P 在坐标系 O0x0y0z0 和 O1x1y1z1 的坐标值分别为 P0 = [x 0
P1 = [x1 y1 z1 ] ，则有：
T 1 P0 = R0 P1
y0
z0 ] 、
T
(8)
用 R ij 表示 i 系向 j 系转换的方向余弦阵，则有：
2 1 2 1 2 3 n n 1 2 R0 = R0 R1 ， R 3 0 = R 0 R 1 R 2 ，…， R 0 = R 0 R 1 ⋅ ⋅ ⋅ R n −1
(9)
2 欧拉角坐标和卡尔丹角坐标
（1）欧拉角坐标 可以将刚体的姿态分解为依次绕连体基 e b 的基矢量 e 3
b
e 2 和 e 3 转过有限
b
b
角度 ϕ 、 θ 和 φ 来实现，这三个角坐标称为欧拉角坐标，并分别称为进动角、章 动角和自转角。
图 2 欧拉角 刚体的姿态可以看成是 ϕ 、 θ 和 φ 三次旋转的叠加，每次旋转的方向余弦矩 阵分别为：
0 0 1 0
a x1 b y1 c z1 1 1
(17)
a b ，[a b c 1] T 为 O1x1y1z1 的原点 O1 在 c 1 O0x0y0z0 下的齐次坐标。若求[x1 y1 z1] T，则： x1 x0 y 1 = T −1 y 0 (18) z1 z0 1 1
(26)
其中， S 1 = sin q 1 ， C1 = cos q1 （下同） 。
则齐次变换矩阵为：
1 0 0 C 1 h A = 0 S 1 0 0
（2）万向节
0 − S1 C1 0
0 0 0 1
(27)
万向节是有两个相对转动自由度 q1 和 q2 的铰。
图 9 万向节 若初始转角为零，则万向节的转动可以认为是分别进行两次旋转变换，两次 旋转变换的方向余弦矩阵分别为：
,
万向节总的旋转变化是这两次变换的叠加，因此总的方向余弦矩阵为：
(28)
(29)
则齐次变换矩阵为：
C2 SS Ah = 1 2 − C1 S 2 0
（3）球铰
0 C1 S1 0
S2 − S1C 2 C1C 2 0
0 0 0 1
(30)
球铰是有三个相对转动自由度 q1、q2 和 q3 的铰。
z0]T,在 O1x1y1z1 下的坐标为[x1 y1 z1] T，坐标系 O1x1y1z1 的原点 O1 在 O0x0y0z0 下
的坐标为[a b c] T。则有下列关系： x0 x1 + a y y + b 0 = 1 z 0 z1 + c 1 1 把式(12)写成矩阵的形式：
次坐标为 ( x1 , x 2 , x3 , x 4 ) T ， ( x5 , x6 , x7 , x8 ) T ，这两个齐次坐标相等的条件是：
x1 x 4 = x5 x8 ,
x 2 x 4 = x 6 x8 ,
x 3 x 4 = x 7 x8
(15)
Oxyz 直角坐标系原点的齐次坐标为[0 0 0 a]T， a 为非零实数。 齐次坐标[1 0 0 0]T、
图 12 圆柱铰 圆柱铰的齐次变换矩阵为：
1 0 0 C 2 h A = 0 S 2 0 0
0 − S2 C2 0
h 0 0 1
(34)
7 多体系统坐标系的建立
建立多体系统坐标系时， 首先从基础到末端由低到高的顺序给每个刚体依次 编号。通常基础坐标系的编号为 0，且置于第一个关节上，第 n 个坐标系与第 n 个刚体一起运动，最后一个坐标系固定于末端上。但是，一个刚体可能与多个刚 体相连接，为便于计算和便于利用铰相对运动的齐次变换矩阵，可以在每个铰接 处，在铰接的两个刚体上分别固定一个坐标系，若某个刚体与 i 个刚体相铰接， 则需要在该刚体上固定 i 个本地坐标系。
7 铰的相对运动学
铰的相对运动可以通过固节于铰的两个部件上的坐标系的相对运动关系来 描述。 两个部件中一个作为相对运动的参考物， 其上的连体基称为该铰的本地基， 相对该基运动的另一个部件的连体基称为该铰的动基。
图 7 铰的相对运动
(1) 旋转铰
旋转铰只有一个自由度 q1。
图 8 旋转铰 若初始角为零，则旋转铰的方向余弦矩阵为：
计 算 多 体 系 统 运 动 学 与 动 力 学(Simplified)
Computational Kinematics and Dynamics of Multi-body Systems
（forengineer@ forengineer@）
1 旋转变换
图1
旋转变换
S2 − S1C 2 C1C 2 0
0 0 0 1
(32)
棱柱铰是一种只有一个相对滑移自由度 q1 的铰。
图 11 棱柱铰 棱柱铰的齐次变换矩阵为：
1 0 Ah = 0 0
0 1 0 0
0 0 1 0
h 0 0 1
(33)
（5）圆柱铰 圆柱铰是一个有相对滑移自由度与一个相对转动自由度的铰， 其自由度分别 记为 q1 和 q2。
R4×4
(20)
如果坐标系 O1x1y1z1 分别绕坐标系 O0x0y0z0 的 x、y 和 z 轴旋转θ，其相应的 旋转变换矩阵分别为： 0 1 0 cos θ R ( x, θ ) = 0 sin θ 0 0 0 − sin θ cos θ 0 0 1 0 0 0 0 0 1 sin θ 0 0 0 cos θ 0 0 1 0 0 0 0 cos θ 0 0 1
5 旋转齐次变换
图5
旋转齐次变换
当两坐标系只有转动没有平动时，已知 P 点在坐标系 O1x1y1z1 下的齐次坐标 为[x1 y1 z1 1] T，坐标系 O1x1y1z1 相对于坐标系 O0x0y0z0 旋转一定角度后，求 P 在
O1x1y1z1 下的坐标。这是旋转齐次变换问题。
将式(8)写成齐次坐标的形式： x0 y 1 0 = R0 3×3方向余弦阵 z0 0 0 0 1
，
，
(10)
最终的方向余弦矩阵是这三次方向余弦矩阵的乘积
(11) （2）卡尔丹角坐标 将刚体的姿态分解为依次绕连体基 e b 的基矢量 e1
b
e 2 和 e 3 转过有限角度
b
b
α 、 β 和 γ 来实现，这三个角坐标称为卡尔丹角坐标。
图 3 卡尔丹角 刚体的姿态可以看成是 α 、 β 和 γ 三次旋转的叠加，每次旋转的方向余弦矩 阵分别为：
a b (24) c 1
A4×4 称为齐次变换矩阵。将齐次变换矩阵 A4×4 写成分快的形式：
A A = 11 A21
T
A12 1
(25)
其中，A11= R10 表示方向余弦阵，它表示前后坐标系之间的旋转变换，A12=[a b c] 表示后一坐标系的原点在前一坐标系下的位置，如果 A12=[0 0 0] T，说明两 坐标系的原点重合。
(3)式两边点乘 i 0 ，得
x0 = x1 i1 ⋅ i 0 + y1 j1 ⋅ i 0 + z1 k1 ⋅ i 0
同理，可得
j 0 = x1 i1 ⋅ j 0 + y1 j1 ⋅ j 0 + z1 k1 ⋅ j 0 z 0 = x1i1 ⋅ k 0 + y1 j1 ⋅ k 0 + z1k1 ⋅ k 0
O0x0y0z0 的坐标轴 x0、y0 和 z0 旋转，可得旋转变换矩阵式(15)，然后沿 x0、y0 和 z0 分别平移 a、b、c，可得平移变换矩阵式(13)。根据变换矩阵相乘的顺序与操
作顺序相反的原则，可得总的变换矩阵为平移和旋转矩阵的乘积： 1 0 0 a 0 0 1 0 b 1 1 R0 3×3方向余弦阵 0 = R0 3×3方向余弦阵 A4×4 = TR = 0 0 1 c 0 0 0 0 1 0 0 0 0 0 0 1
[0 1 0 0 0] T 和[0 0 1 0] T 分别表示了 x、y 和 z 轴的无穷远点。当齐次坐标中最后 一个元素趋近于 0 时， 表示了无穷远点， 它扩大了描述空间， 当这个元素取 1 时， 表示了物理空间的一个点，通常取它为 1。
4 平移齐次变换