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

最后，我们将求和名义标志的 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 ⎝
⎛ ∂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）求动力学方程 。 先求导数
i
Pi =
linki
∫ dP = − ∫
g T Ti i rdm = − g T Ti
i
linki
linki
∫
i
rdm = − g T Ti mi i ri = −mi g T Ti i r （19）
其中， mi 为连杆 i 的质量； ri 为连杆 i 相对于其前端坐标系的重心位置 。 由于驱动装置的重力作用 Pai 一般很小，可以忽略不计，所以，机械手系统的总势能为
j =1 j =1 k =1
4.3.3 操作手动力学方程 现在我们将对任意操作手推导用一组 A 变换描述的动力学方程 。 我们将分五步来进行这一推 导 。 首先计算任一杆件中任意一点的速度，其次计算动能 。 然 后研究势能，构成拉格朗日函数 和对它微分以得到动力学方程 。 4.3.3.1 操作手上一点速度 给出相对于杆 i 描述的一点 r ，它在基座坐标中的位置为
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
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
⎞ ⎟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
另外，连杆 i 的驱动装置对动能也提供一个重要部分，我们用与关节速度有关的驱动装置惯 量来表示：
K ai =
则
1 & i2 （13） I ai q 2
Ka =
1 n & i2 （14） I ai q ∑ 2 i =1
对于移动关节 Ia 变成一等效质量 。 将求迹和求和运算调换一下，并加上驱动装置的动能部分，最后得到系统的总动能是
⎞ ⎟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 所以下式成立：
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
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 ⎝ ⎠
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 ⎝
⎤ mi x i ⎥ ⎥ mi y i ⎥ （12） ⎥ ⎥ mi z i ⎥ ⎥ mi ⎦
具有 n 个自由度的操作手的总动能是
K = ∑ Ki =
i =1
⎛ i i ∂Ti ⎞ ∂Ti T 1 n ⎜ & jq & k ⎟ （13） q J Trace ∑ ∑∑ i ⎜ j =1 k =1 ∂q ⎟ 2 i =1 ∂q 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
n i i ⎛ ∂ 2Ti ⎞ ∂Ti T ⎟q &&k + I ap q &&p + ∑∑∑ Trace⎜ J ⎜ ∂q ∂q i ∂q ⎟ i = p k =1 m =1 p ⎝ k m ⎠
将式（25）对时间微分得
n i ⎛ ∂Ti ∂TiT d ∂L ⎜ Trace J = ⎜ ∂q i ∂q & p ∑∑ dt ∂q i = p k =1 p ⎝ k
⎞ ⎟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
在公式（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
n j j
n n n
n ⎞ ∂T j j ⎟q &k q & m −∑ m j g T r j （30） ⎟ ∂ q j i = i ⎠
这些方程与求和的次序无关，因此可以将式（30）写成：
&& j + I ai q &&i + ∑∑ Dijk q & jq & k + Di （31） Fi = ∑ Dij q
0
i
r = Ti i r （1）
⎡ i ∂T ⎤ i & j ⎥ r （2） = ⎢∑ i q q ∂ j = 1 ⎢ ⎥ j ⎣ ⎦
2
则它的速度是
d 0r dt
速度的平方是
⎛ d 0r ⎞ 0 0 &⋅ r & （3） ⎜ ⎟ = r ⎜ dt ⎟ ⎠ ⎝
或者用矩阵形式
⎛ d 0r ⎞ 0 0 T &⋅ r & （4） ⎜ ⎟ = Trace r ⎜ dt ⎟ ⎠ ⎝
⎞ ⎟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 ⎠
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
公式（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
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 的物体势能是
则杆 i 的动能是
⎞ ⎟ dm ⎟ ⎠ ⎞ ⎟ ⎟ ⎠
（6）
( rdm r )
i i T
∂ Ti T & jq &k q ∂q k
Ki =
⎛ i i ∂Ti 1 ⎜ ∑∑ = dK Trace i ∫ ⎜ j =1 k =1 ∂q j 2 linki ⎝
⎞ ⎛ i i T ⎞ ∂TiT ⎟ （7） ⎜ ∫ r r dm ⎟ & & q q k j ⎜ ⎟ ∂q ⎟ k ⎝ linki ⎠ ⎠
⎞ ⎟q && + I ap q &&p ⎟ k ⎠
n ⎞ ∂Ti i ⎟q &k q & j − ∑ mi g T ri （29） ⎟ q ∂ i p = p ⎠
n i i ⎛ ∂ 2Ti ∂T T Ji i + ∑∑∑ Trace⎜ ⎜ ∂q ∂q ∂q p i = p j =1 k =1 ⎝ k j
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
P = mgh （16）
如果重力加速度用向量 g 表示，而物体质心的位置用向量 r 表示，则式（16）变为
P = −mgr （17）
连杆 i 上 r 处的质点 dm，其势能为
i
dPi = − dmg T 0 r = − g T Ti i rdm （18）
式中 g
T
= [ g x , g y , g z ,1] 。