控制系统工程案例分析-机器人技术(第六章-力的坐标变换)

6.3 坐标系之间力的变换
(Transformation of Forces Between Coordinate Frames)
虚功原理：所谓虚功原理是指假定有一个力向量F作用于一个物体，它引 起一个微小的假想位移，称之为虚位移D，如果物体实际上并未移动，它 在这个物体上所作的功称为虚功，且虚功为零。 即
Z
T6
E
H
O
图6.2 变换图 于是 p = –2i + 0j – 10k，从而 i j k f × p = 0 0 100 = 0i – 200j + 0k –2 0 –10 f × p + m = 0i – 200j + 1000k 最后，由式(6.15)和(6.16)，可得
T6m T6f
= 0i – 200j + 1000k = 0i + 0j + 100k
⎡ fx ⎤ ⎢f ⎥ ⎢ y⎥ ⎢ fz ⎥ F =⎢ ⎥ ⎢mx ⎥ ⎢m y ⎥ ⎢ ⎥ ⎢ mz ⎥ ⎣ ⎦
用一个六维力向量表示为 F = [ 10 0 –150 0 –100 0 ] T
(6.1)
例如一个力矢量 f = 10i + 0j – 150k 和一个力矩矢量 m = 0i –100j + 0k，可
(8.12)
由上式可求得
cf cf x y
cf z cm x cm y cm z
=
nx ny nz ox oy oz ax ay az (p×n)x (p×n)y (p×n)z (p×o)x (p×o)y (p×o)z (p×a)x (p×a)y (p×a)z
0 0 0 nx ox ax
0 0 0 ny oy ay
W
W Z T6 G Y E X
m=
Fg T W F Fg
T W
W
Fg
(6.28)
图6.5 利用腕力传感器测定物体的质量
6.7 本章小结 (Summary)
本章得到两个重要结果 1. 坐标系之间力和力矩的变换关系式
cm x= y=
n · (( f × p ) + m ) o · (( f × p ) + m ) a · (( f × p ) + m ) (6.15)
本章介绍静态力和力距的表示方法，以及它们在坐标系 之间的变换和等效关节力矩的计算方法。以及通过关节力矩 和利用腕力传感器确定机械手负载物体质量等问题。
6.2 力和力距的表示 (The Representation of Forces and Moments)
力和力距都是矢量，要相对于某个确定的坐标系来进行描述。矢量 f 表 示力，矢量 m 表示力矩。力与力矩合在一起用矢量F表示，称为力向量F。
cm cm cf
z=
x y
=c·f =o·f =a·f (6.16)
cf
cf z
2. 建立了T6坐标系中的力和力矩与机械手关节力矩之间的关系
第六章 力的坐标变换
Chapter Ⅵ Forces Transformation
6.1 引言 6.2 力和力距的表示 6.3 坐标系之间的力变换 6.4 等效关节力矩 6.5 通过关节力矩判断负载质量 6.6 利用腕力传感器判断负载质量 6.7 本章小结
6.1 引言 (Introduction)
δw = FT D = 0
其中δw 表示虚功，D 表示虚位移的微分运动矢量 D = [ dx dy dz δx δy δz ]T F为力矢量 F = [ fx fy fz mx my mz ]T
(6.2)
(6.3)
(6.4)
如果虚位移是由作用在物体上的另一个力向量造成的，它对物体的外 部作用效果相同。假定这个虚位移用坐标系C 来描述，那么就会得到相同 的虚功：
【例6.2】 设斯坦福手的状态如下 0 1 T6 = 0 0
1 0 0 0
0 20 0 6.0 –1 0 0 1
相应各个关节坐标值如表6.1所示。 表6.1 机械手状态 坐标 值 00 900 20in 00 900 900 正弦 0 1 0 1 1 余弦 1 0 1 0 0
θ1 θ2
Βιβλιοθήκη Baidud3
θ4 θ5 θ6
cf cf
cf = z
式中的n，o，a 和p在第五章5.5节中定义过，它们是微分坐标变换 式的列向量，力矩的变换形式与微分平移一样，而力的变换形式与微分 旋转一样。
【例6.1】 一个机械手及其末端执行器的位置为 ZT6E，要把一个螺杆插入表示为OH的孔 眼，如图6.1所示。 Z T6 E = O H 末端执行器的坐标为 1 0 0 0 0 1 0 0 0 0 1 0 2 0 10 1
τ TT m= T τ τ
(6.27)
一旦负载质量确定，就要重新计算动力学方程以便补偿负载质 量的影响。
6.6 利用腕力传感器判断负载质量
(Mass Determination by Wrist Force Sensor)
如果有一个腕力传感器，它相对于T6的变换矩阵为T6W。如果由腕力传感器 测得六维力向量WF（含三个力分量和三个力矩分量），那么未知负载的质量就 可以据此确定。图6.5画出了这个变换图，按照这个图，我们通过变换表达式 YE–1W就能得到坐标系G与W之间的变换关系。经过如同利用关节力矩确定负载 质量一样的方法，我们求出1公斤负载在坐标系W中的等价力 WFg。在机械手提 起未知负载之后，我们观测到作用在腕力传感器上的六维力向量WF。通过归一 化得到负载的质量。
δw = T6FT
T6D
= τT Q
(6.17)
式中，τ是广义关节力的一个列向量，它由各个关节的力或力矩分量 构成，对于旋转关节，它是力矩 τi ；对于滑动关节，它是关节力fi 。Q是 关节虚位移的一个列向量，对于旋转关节为δθi，对于滑动关节为δdi。
以斯坦福机械手为例，关节虚位移所做的虚功为
⎡ δθ 1 ⎤ ⎢ δθ ⎥ ⎢ 2⎥ ⎢δd ⎥ τ 6 ]⎢ 3 ⎥ ⎢ δθ 4 ⎥ ⎢ δθ 5 ⎥ ⎥ ⎢ ⎢ δθ 6 ⎥ ⎦ ⎣
δ w = [τ 1
τ2
f3
τ4
τ5
(6.18)
式中，τi为关节力矩，f3为作用于滑动关节3的关节力。由虚功原理可知
T6FT T6D
=τT Q
(6.19) (6.20)
由微分变换关系式得到 上式与虚位移Q无关，从而 即
T6FT J
Q =τT Q = τT
T6FT J
τ= JT T6F
(6.21)
式(6.21)是一个十分重要的关系式，它表明，给机械 手末端坐标T6施加一个作用力和力矩，可以得到机械手各 关节的等效力或力矩，使机械手与外部作用保持平衡。如 果机械手可以自由地按照作用力和力矩的方向运动，那么 由式(6.21)确定的关节力或力矩就会使得给定的力和力矩 获得作用效果。需要指出的是，式(6.21)适用于任意自由 度的机械手。
0 0 100 0 –200 1000
=
–1000 2000 0 –200 0 1000
6.5 通过关节力矩判断负载质量
(Mass Determination of Load by Joint Torques)
如果机械手要移动一个未知负载，假定是最坏情况的负载，先设定合 适的系统增益以防欠阻尼响应，然后控制机械手运动使它以恒速提起负 载。一旦所有关节都运动起来，按第七章式(7.20)确定误差力矩和力。于 是，这些误差力矩和力就会与负载质量发生关系。设机械手的位置相对基 坐标由变换Z确定。而未知负载被握在由T6E描述的末端执行器的质心处。 负载的位置X就是 X = Z T6 E Z G T6 Y E X (6.22)
mx my mz fx fy fz
(6.14)
由式（6.14）和第五章的式（5.39）可知，力和力矩在坐标系之间 的变换形式与微分平移和微分旋转的变换形式相同，则有
cm cm cm x= y= z= x= y=
n · (( f × p ) + m) o ·(( f × p ) + m) a ·(( f × p ) + m) n·f o·f a·f (6.16) (6.15)
图6.3 利用关节力矩变换图判定负载质量
位于末端执行器的1公斤负载在坐标系G中所施加的力为
GF =
[ 0 0 –g 0 1 0 0 0 0 1 0 0 0 0 1 0
Xp Xp
0 0]
(6.23)
其中
x
G =
y Xp z
(6.24)
1
则G坐标系就与基坐标系定向一致是一个平移变换矩阵。我们定义一个变换Y把G与X 联系起来，则Y是一个旋转变换矩阵 GY=X 则 Y = G–1 X
当施加由例6.1得到的力和力矩
T6F
=[0
0
100
0
–200
1000]T
试计算必要的关节力矩和力。 解：关节力矩和力由式(6.21)求出
τi τi τi τi τi
f3
20 –6 0 0 0 20 0 1 0 = 0 0 0 0 0 0 0 0 0
0 1 0 0 1 0
0 –1 0 0 0 0 1 0 0 0 0 –1
CD
δx δy δz
(6.7)
即 从而得到
=JD
(6.8)
FTD = CFT J D
(6.6)
上式适用于任何虚位移D，于是可以得到 FT = CFT J 转置后得到 即 fx fy fz mx my mz nx ox ax 0 ny oy ay 0 nz oz az 0 = (p×n)x (p×o)x (p×a)x nx (p×n)y (p×o)y (p×a)y ny (p×n)z (p×o)z (p×a)z nz 0 0 0 0 0 0 ox ax oy ay oz az cfx cfy cfz cm x cm y cm z F = JT CF (6.10) (6.11)
即
T6F
= [ 0 0 100 0 –200 1000 ] T
6.4 等效关节力矩 (Equivalent Joint Torques)
这一节要解决的问题是，在作用于坐标系T6的力和力矩与等价关节力 及关节力矩之间建立一定的关系。还是利用虚功原理，使作用于坐标系T6 的力和力矩所完成的虚功与她们在关节上完成的虚功相等，即
T6
y 200 x z 1000 100 y
H
E =
在孔眼坐标系H中，机械手要施加的力 是Hf = 0i + 0j + 100k, 力矩是 Hm= 0i + 0j + 1000k 试求坐标系T6中的等价力和力矩。
z x 1000 100
图6.1 坐标系之间的力变换
解：由图6.2所示的变换图，我们可以得到微分坐标变换E–1 1 0 E–1 = 0 0 0 1 0 0 0 –2 0 0 1 -10 0 1
δw = FT D = CFT CD
即 FT D = CFT CD 由于 nx Cd ox y Cd ax z Cδ = 0 x Cδ 0 y Cδ 0 z
x Cd
(6.5) (6.6) dx dy dz
ny nz (p×n)x (p×n)y (p×n)z oy oz (p×o)x (p×o)y (p×o)z ay az (p×a)x (p×a)y (p×a)z 0 0 nx ny nz 0 0 ox oy oz 0 0 ax ay az
xn xn x y xo xo x xa xa xa x
(6.25)
y
y
Y =
xn
z
xo z
z
0
0
0
0 0 0 1
(6.26)
由图6.3求得 GF与 T6F的微分变换式 YE–1。然后根据式(6.15)和 (6.16)，把YE–1作为微分坐标变换，就得到1公斤负载作用于T6的力， 然后再由式(6.21)得到等价关节力矩τ。最后，根据式(7.20)得出等价 的稳态误差力矩T，把 T 相对于τ进行归一化，通过求取τ与 T 的内 积，我们就得到了负载质量m的表达式
其雅可比矩阵为 20.0 –6.0 ∂T6 = 0.0 0.0 ∂qi 0.0 –1.0 0.0 0.0 20.0 1.0 0.0 0.0 0.0 1.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 1.0 0.0 0.0 0.0 0.0 1.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 1.0
0 0 0 nz oz az
fx fy fz mx my mz
(6.13)
将上式的上三行和下三行互换有
cm cm cm cf cf x y z
x y
cf z
nx ny ox oy ax ay = 0 0 0 0 0 0
nz (p×n)x (p×n)y (p×n)z oz (p×o)x (p×o)y (p×o)z az (p×a)x (p×a)y (p×a)z 0 nx ny nz 0 ox oy oz 0 ax ay az