操作手拉格朗日动力学方程

I zz = ∫ ( x 2 + y 2 )dm ； I yz = I zy = ∫ yzdm ；
my = ∫ ydm,
∫x ∫y ∫z
2
dm = −
1 1 1 ( y 2 + z 2 )dm + ∫ ( x 2 + z 2 )dm + ∫ ( x 2 + y 2 )dm ∫ （9） 2 2 2 = (− I xx + I yy + I zz ) 2 1 1 1 ( y 2 + z 2 )dm − ∫ ( x 2 + z 2 )dm + ∫ ( x 2 + y 2 )dm ∫ （10） 2 2 2 = (+ I xx − I yy + I zz ) 2 1 1 1 ( y 2 + z 2 )dm + ∫ ( x 2 + z 2 )dm − ∫ ( x 2 + y 2 )dm ∫ （11） 2 2 2 = (+ I xx + I yy − I zz ) 2
最后，我们将求和名义标志的 p 和 i 调换成 i 和 j，则动力学方程为：
⎛ ∂T j ∂T jT ⎜ Fi = ∑∑ Trace J ⎜ ∂q k j ∂qi j =i k =1 ⎝
n j
⎞ ⎟q && + I ai q &&i ⎟ k ⎠
⎛ ∂ 2T j ∂T jT ⎜ + ∑∑∑ Trace J ⎜ ∂q k ∂q m j ∂qi j =i m =1 k =1 ⎝
⎞ ⎟q & q & ⎟ j k ⎠
n ⎞ ∂Ti i ⎟q & jq & k + ∑ mi g T ri （27） ⎟ q ∂ i p = p ⎠
⎛ ∂Ti ∂ 2TiT 1 n i i ⎜ Trace J + ∑∑∑ ⎜ ∂q i ∂q ∂q 2 i = p j =1 k =1 k p ⎝ j
n i i ⎛ ∂ 2Ti ∂T T ∂L Ji i = ∑∑∑ Trace⎜ ⎜ ∂q ∂q ∂q p i = p j =1 k =1 ∂q k ⎝ j p
2
dm = +
2
dm = +
于是可以把 Ji 表示为：
⎡ − I ixx ⎢ ⎢ ⎢ Ji = ⎢ ⎢ ⎢ ⎢ ⎣
n
+ I iyy + I izz 2 I ixy I ixz mi x i
I ixy I ixx − I iyy + I izz 2 I iyz mi y i
I ixz I iyz I ixx + I iyy − I izz 2 mi z i
P = ∑ Pi = −∑ mi g T Ti i ri （20）
i =1 i =1
n
n
4.3.3.4 拉格朗日函数与动力学方程 根据拉格朗日函数 L = K − P ，则
⎛ ∂Ti ∂Ti T 1 n i i ⎜ L = ∑ ∑∑ Trace J ⎜ ∂q i ∂q 2 i =1 j =1 k =1 k ⎝ j
∫ x ydm ∫ x zdm ∫ xdm ⎤ ⎥ ⎥ ∫ y dm ∫ y zdm ∫ ydm⎥ （8） ⎥ ∫ y zdm ∫ z dm ∫ zdm ⎥ ⎥ ∫ ydm ∫ zdm ∫ dm ⎥ ⎥ ⎦
i i i i i i
2
linki i
i
linki i
linki
linki
i
i
2
i
linki
linki
j =1 j =1 k =1
4.3.3 操作手动力学方程 现在我们将对任意操作手推导用一组 A 变换描述的动力学方程 。 我们将分五步来进行这一推 导 。 首先计算任一杆件中任意一点的速度，其次计算动能 。 然 后研究势能，构成拉格朗日函数 和对它微分以得到动力学方程 。 4.3.3.1 操作手上一点速度 给出相对于杆 i 描述的一点 r ，它在基座坐标中的位置为
i
i
linki
linki
linki
根据理论力学可知，物体的转动惯量 、 矢量积以及一阶矩量为：
I xx = ∫ ( y 2 + z 2 )dm, I xy = I yx = ∫ xydm, mx = ∫ xdm,
且
I yy = ∫ ( x 2 + z 2 )dm, I xz = I zx = ∫ xzdm, mz = ∫ zdm
T
⎛ ∂T ∂T T Trace⎜ i J i i ⎜ ∂q ∂q k ⎝ j
则
T ⎞ T ⎛ ⎛ ⎞ ⎟ = Trace⎜ ∂Ti J iT ∂Ti ⎟ = Trace⎜ ∂Ti J i ∂Ti ⎜ ∂q ⎜ ∂q ⎟ ∂q j ∂q k ⎟ ⎝ k ⎝ j ⎠ ⎠
T
⎞ ⎟ （23） ⎟ ⎠
n i ⎛ ∂Ti ∂TiT ∂L ⎜ Trace J = ⎜ ∂q i ∂q & p ∑∑ ∂q i =1 k =1 p ⎝ k
K=
⎛ ∂Ti ∂TiT 1 n i i ⎜ Trace J ∑ ∑∑ ⎜ ∂q i ∂q 2 i =1 j =1 k =1 k ⎝ j
⎞ 1 n ⎟q & jq & k + ∑ I ai q & i2 （15） ⎟ 2 i =1 ⎠
4.3.3.3 操作机械手的势能 在重力场 g 中，在某一参考零位上面 h 高处，质量为 m 的物体势能是
⎞ ⎟q & ⎟ k ⎠ ⎞ ⎟q & +I q & ⎟ j ap p ⎠ p = 1,2,3,L, n
⎛ ∂T ∂T T 1 n i + ∑∑ Trace⎜ i J i i ⎜ ∂q 2 i =1 j =1 ∂q p ⎝ j
（22）
根据式（12）知，Ji 为对称阵，即 J i = J i 所以下式成立：
⎞ ⎟q & q & ⎟ k m ⎠
n i i ⎛ ∂T ∂ 2Ti T ⎞ ⎟q & q & （26） + ∑∑∑ Trace⎜ i J i ⎜ ∂q ⎟ k m ∂ ∂ q q i = p k =1 m =1 k p m ⎝ ⎠
再求 ∂L ∂q p 项：
⎛ ∂ 2Ti ∂TiT ∂L 1 n i i ⎜ J = ∑∑∑ Trace ⎜ ∂q ∂q i ∂q ∂q p 2 i = p j =1 k =1 k ⎝ j p
公式（7）中的积分称为伪惯量矩阵 J i 并给出为
Ji =
linki
∫
i
r i r T dm
⎡ i x 2 dm ∫ ⎢ linki ⎢ i x i ydm ⎢∫ = ⎢linkii i ⎢ ∫ x zdm ⎢ linki ⎢ i xdm ∫ ⎢ ⎣ linki
linki
linki i linki
在公式（27）第二项中将求和名义标示 j 和 k 调换，则有
n ⎞ ∂Ti i ⎟q & jq & k + ∑ mi g T ri （28） ⎟ q ∂ = i p p ⎠
将公式（26）中求和名义标志由 m 换成 j，则有
n i ⎛ ∂Ti ∂TiT d ∂L ∂L ⎜ − Trace J = ⎜ ∂q i ∂q & p ∂q p ∑∑ dt ∂q i = p k =1 p ⎝ k
⎛ ∂T ∂T T ∂L 1 n i = ∑∑ Trace⎜ i J i i ⎜ ∂q & p 2 i =1 k =1 ∂q k ∂q ⎝ p
n ⎞ 1 n ⎟q & jq & k + ∑ I ai q & i2 + ∑ mi g T Ti i ri （21） ⎟ 2 i =1 i =1 ⎠
利用式（21）求动力学方程 。 先求导数
⎞ ⎟q & + I ap q & p （24） ⎟ k ⎠
当 p>i 时，后面连杆的变量 qp 对前面各连杆不产生影响，即 ∂Ti ∂q p = 0 ，于是得到
n i ⎛ ∂T ∂T T ∂L = ∑∑ Trace⎜ i J i i ⎜ ∂q & p i = p k =1 ∂q ∂q p ⎝ k
⎞ ⎟q & + I ap q & p （25） ⎟ k ⎠
P = mgh （16）
如果重力加速度用向量 g 表示，而物体质心的位置用向量 r 表示，则式（16）变为
P = −mgr （17）
连杆 i 上 r 处的质点 dm，其势能为
i
dPi = − dmg T 0 r = − g T Ti i rdm （18）
式中 gT= [ g x ,g y , g z ,1] 。
i
4.3.3.2 动能 位于杆 i 上 r 处质量为 dm 的质点的动能是
⎛ i i ∂ Ti i i T ∂ Ti T 1 & jq &k q rr dK i = Trace ⎜ ∑ ∑ ⎜ j =1 k =1 ∂q ∂ q 2 j k ⎝ ⎛ i i ∂ Ti 1 = Trace ⎜ ∑ ∑ ⎜ j =1 k =1 ∂q 2 j ⎝
2
(
)
将公式（2 代入公式（4）得
T 2 i ⎡ i ∂T ⎛ ∂Ti i ⎞ ⎤ ⎛ d 0r ⎞ i i & j r∑ ⎜ &k r ⎟ ⎜ ⎜ dt ⎟ ⎟ = Trace ⎢∑ ∂q q ⎜ ∂q q ⎟ ⎥ ⎢ = 1 = 1 j k ⎝ ⎠ j ⎝ k ⎠ ⎥ ⎣ ⎦ （5）
⎛ i i ∂T ⎞ ∂T T & jq &k ⎟ = Trace⎜ ∑∑ i i r i r T i q ⎜ j =1 k =1 ∂q ⎟ ∂q k j ⎝ ⎠