Modern Robotics单个刚体的动力学

太过爱你忘了你带给我的痛 2022-12-16 03:55 131阅读 0赞

# Modern Robotics单个刚体的动力学 #

![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70_pic_center]

## 基本公式 ##

考虑刚体由质量为 m i m\_i mi的一系列质点 i i i组成，总质量为 m = ∑ i m i \\text m=\\sum\_i\\text m\_i m=∑imi。物体坐标系原点建立在刚体质心处 r i = ( x i , y i , z i ) r\_i=(x\_i,y\_i,z\_i) ri=(xi,yi,zi)，则有 ∑ i m i r i = 0 \\sum\_i\\text m\_ir\_i=0 ∑imiri=0.

设一个刚体的运动旋量为 V b = ( ω b , v b ) \\mathcal V\_b=(\\omega\_b,v\_b) Vb=(ωb,vb),质点 m i \\text m\_i mi的位置坐标为 p i ( t ) p\_i(t) pi(t), 在惯性坐标系\{b\}的最初位置为 r i r\_i ri。注意这些物理量都是在物体坐标系中描述，那么有  
 p ˙ i = v b + ω b × p i p ¨ i = v ˙ b + d d t ω b × p i + ω b × d d t p i = v ˙ b + ω ˙ b × p i + ω b × ( v b + ω b × p i ) \\dot p\_i=v\_b+\\omega\_b\\times p\_i\\\\ \\ddot p\_i=\\dot v\_b+\\frac\{d\}\{dt\}\\omega\_b\\times p\_i+\\omega\_b\\times\\frac\{d\}\{dt\}p\_i\\\\ =\\dot v\_b+\\dot \\omega\_b\\times p\_i+\\omega\_b\\times(v\_b+\\omega \_b\\times p\_i) p˙i=vb\+ωb×pip¨i=v˙b\+dtdωb×pi\+ωb×dtdpi=v˙b\+ω˙b×pi\+ωb×(vb\+ωb×pi)  
把 r i r\_i ri代替 p i p\_i pi，并使用反对称矩阵表示叉乘，那么有  
 p ¨ i = v ˙ b + \[ ω ˙ b \] r i + \[ ω b \] v b + \[ ω b \] 2 r i \\ddot p\_i=\\dot v\_b+\[\\dot \\omega\_b\]r\_i+\[\\omega\_b\]v\_b+\[\\omega\_b\]^2r\_i p¨i=v˙b\+\[ω˙b\]ri\+\[ωb\]vb\+\[ωb\]2ri  
根据牛顿第二定律，对于一个质点有 f i = m i p ¨ i f\_i=m\_i\\ddot p\_i fi=mip¨i,作用在质点 m i m\_i mi的力为  
 f i = m i ( v ˙ b + \[ ω ˙ b \] r i + \[ ω b \] v b + \[ ω b \] 2 r i ) f\_i=\\text m\_i(\\dot v\_b+\[\\dot \\omega\_b\]r\_i+\[\\omega\_b\]v\_b+\[\\omega\_b\]^2r\_i) fi=mi(v˙b\+\[ω˙b\]ri\+\[ωb\]vb\+\[ωb\]2ri)  
这意味着力矩为  
 m i = \[ r i \] f i . m\_i=\[r\_i\]f\_i. mi=\[ri\]fi.  
作用在刚体的总力和总力矩用力旋量 F b \\mathcal F\_b Fb表示为  
 F b = \[ m b f b \] = \[ ∑ i m i ∑ i f i \] \\mathcal F\_b=\\left\[ \\begin\{matrix\} m\_b\\\\f\_b \\end\{matrix\}\\right\]=\\left\[ \\begin\{matrix\} \\sum \_im\_i\\\\\\sum\_if\_i \\end\{matrix\}\\right\] Fb=\[mbfb\]=\[∑imi∑ifi\]  
下面化简 f b , m b f\_b,m\_b fb,mb,注意到 ∑ i m i r i = 0 \\sum \_i\\text m\_ir\_i=0 ∑imiri=0(因此， ∑ i m i \[ r i \] = 0 \\sum\_i\\text m\_i\[r\_i\]=0 ∑imi\[ri\]=0), 对于 a , b ∈ R 3 , \[ a \] = − \[ a \] T , \[ a \] b = − \[ b \] a , \[ a \] \[ b \] = ( \[ b \] \[ a \] ) T a,b\\in \\mathbb R^3,\[a\]=-\[a\]^T,\[a\]b=-\[b\]a,\[a\]\[b\]=(\[b\]\[a\])^T a,b∈R3,\[a\]=−\[a\]T,\[a\]b=−\[b\]a,\[a\]\[b\]=(\[b\]\[a\])T.这些关系在下面化简中会用到。

平动动力学  
 f b = ∑ i m i ( v ˙ b + \[ ω ˙ b \] r i + \[ ω b \] v b ) + \[ ω b \] 2 r i = ∑ i m i ( v ˙ b + \[ ω b \] v b ) − ∑ i m i \[ r i \] ω ˙ b + ∑ i m i \[ r i \] \[ ω b \] ω b = m ( v ˙ b + \[ ω b \] v b ) . (8.22) f\_b=\\sum \_i\\text m\_i(\\dot v\_b+\[\\dot\\omega\_b\]r\_i+\[\\omega\_b\]v\_b)+\[\\omega\_b\]^2r\_i\\\\ =\\sum\_i\\text m\_i(\\dot v\_b+\[\\omega\_b\]v\_b)-\\sum\_i\\text m\_i\[r\_i\]\\dot\\omega\_b+\\sum\_i\\text m\_i\[r\_i\]\[\\omega\_b\]\\omega \_b\\\\ =\\text m(\\dot v\_b+\[\\omega\_b\]v\_b).\\tag\{8.22\} fb=i∑mi(v˙b\+\[ω˙b\]ri\+\[ωb\]vb)\+\[ωb\]2ri=i∑mi(v˙b\+\[ωb\]vb)−i∑mi\[ri\]ω˙b\+i∑mi\[ri\]\[ωb\]ωb=m(v˙b\+\[ωb\]vb).(8.22)  
转动动力学  
 m b = ∑ i m i \[ r i \] ( v ˙ b + \[ ω ˙ b \] r i + \[ ω b \] v b + \[ ω b \] T r i ) = ∑ i m i \[ r i \] v ˙ b + ∑ i m i \[ r i \] \[ ω b \] v b + ∑ i m i \[ r i \] ( \[ ω ˙ b \] r i + \[ ω b \] T r i ) = ∑ i m i ( − \[ r i \] 2 ω ˙ b − \[ r i \] T \[ ω b \] T \[ r i \] ω b ) = ( − ∑ i m i \[ r i \] 2 ) ω ˙ b + \[ ω b \] ( − ∑ i m i \[ r i \] 2 ) = I b ω ˙ b + \[ ω b \] I b ω b m\_b=\\sum\_i\\text m\_i\[r\_i\](\\dot v\_b+\[\\dot\\omega\_b\]r\_i+\[\\omega\_b\]v\_b+\[\\omega\_b\]^Tr\_i)\\\\ =\\sum\_i\\text m\_i\[r\_i\]\\dot v\_b+\\sum\_i\\text m\_i\[r\_i\]\[\\omega\_b\]v\_b+\\sum\_i\\text m\_i\[r\_i\](\[\\dot\\omega\_b\]r\_i+\[\\omega\_b\]^Tr\_i)\\\\ =\\sum\_i\\text m\_i(-\[r\_i\]^2\\dot\\omega\_b-\[r\_i\]^T\[\\omega\_b\]^T\[r\_i\]\\omega\_b)\\\\ =\\left (-\\sum\_i\\text m\_i\[r\_i\]^2 \\right)\\dot\\omega\_b+\[\\omega\_b\]\\left(-\\sum\_i\\text m\_i\[r\_i\]^2 \\right)\\\\ =\\mathcal I\_b\\dot\\omega\_b+\[\\omega\_b\]\\mathcal I\_b\\omega\_b mb=i∑mi\[ri\](v˙b\+\[ω˙b\]ri\+\[ωb\]vb\+\[ωb\]Tri)=i∑mi\[ri\]v˙b\+i∑mi\[ri\]\[ωb\]vb\+i∑mi\[ri\](\[ω˙b\]ri\+\[ωb\]Tri)=i∑mi(−\[ri\]2ω˙b−\[ri\]T\[ωb\]T\[ri\]ωb)=(−i∑mi\[ri\]2)ω˙b\+\[ωb\](−i∑mi\[ri\]2)=Ibω˙b\+\[ωb\]Ibωb

即  
 m b = I b ω ˙ b + \[ ω b \] I b ω b (8.23) m\_b=\\mathcal I\_b\\dot\\omega\_b+\[\\omega\_b\]\\mathcal I\_b\\omega\_b \\tag\{8.23\} mb=Ibω˙b\+\[ωb\]Ibωb(8.23)  
这里 I b = − ∑ i m i \[ r i \] 2 ∈ R 3 × 3 \\mathcal I\_b=-\\sum\_i\\text m\_i\[r\_i\]^2\\in\\mathbb R^\{3\\times 3\} Ib=−∑imi\[ri\]2∈R3×3 是旋转惯量矩阵。方程（8.23）称为旋转刚体的欧拉方程。

写出 I b I\_b Ib的每个元素，有  
 I b = \[ ∑ m i ( y i 2 + z i 2 ) − ∑ m i x i y i − ∑ m i x i z i − ∑ m i x i y i ∑ m i ( x i 2 + z i 2 ) − ∑ m i y i z i − ∑ m i x i z i − ∑ m i y i z i ∑ m i ( x i 2 + y i 2 ) \] = \[ I x x I x y I x z I x y I y y I y z I x z I y z I z z \] \\mathcal I\_b =\\left\[\\begin\{matrix\} \\sum \\text m\_i(y\_i^2+z\_i^2)& -\\sum\\text m\_ix\_iy\_i &-\\sum\\text m\_ix\_iz\_i \\\\ -\\sum\\text m\_ix\_iy\_i & \\sum \\text m\_i(x\_i^2+z\_i^2) &-\\sum\\text m\_iy\_iz\_i \\\\ -\\sum\\text m\_ix\_iz\_i & -\\sum \\text m\_iy\_iz\_i &\\sum \\text m\_i(x\_i^2+y\_i^2) \\end\{matrix\} \\right\]\\\\ =\\left\[\\begin\{matrix\} \\mathcal I\_\{xx\}&\\mathcal I\_\{xy\} &\\mathcal I\_\{xz\}\\\\ \\mathcal I\_\{xy\}&\\mathcal I\_\{yy\} &\\mathcal I\_\{yz\}\\\\ \\mathcal I\_\{xz\}&\\mathcal I\_\{yz\} &\\mathcal I\_\{zz\} \\end\{matrix\} \\right\] Ib=⎣⎡∑mi(yi2\+zi2)−∑mixiyi−∑mixizi−∑mixiyi∑mi(xi2\+zi2)−∑miyizi−∑mixizi−∑miyizi∑mi(xi2\+yi2)⎦⎤=⎣⎡IxxIxyIxzIxyIyyIyzIxzIyzIzz⎦⎤  
上面求和形式可以写成积分形式，物体体积为 B \\mathcal B B, 体积微元 d V dV dV, 质点 m i \\text m\_i mi以密度函数替换 ρ ( x , y , z ) \\rho (x,y,z) ρ(x,y,z),  
 \{ I x x = ∫ B ( y 2 + z 2 ) ρ ( x , y , z ) d V I y y = ∫ B ( x 2 + z 2 ) ρ ( x , y , z ) d V I z z = ∫ B ( x 2 + y 2 ) ρ ( x , y , z ) d V I z z = ∫ B ( x 2 + y 2 ) ρ ( x , y , z ) d V I x y = ∫ B x y ρ ( x , y , z ) d V I x z = ∫ B x z ρ ( x , y , z ) d V I y z = ∫ B y z ρ ( x , y , z ) d V (8.24) \\begin\{cases\} \\mathcal I\_\{xx\}=\\int\_\\mathcal B(y^2+z^2)\\rho(x,y,z)dV\\\\ \\mathcal I\_\{yy\}=\\int\_\\mathcal B(x^2+z^2)\\rho(x,y,z)dV\\\\ \\mathcal I\_\{zz\}=\\int\_\\mathcal B(x^2+y^2)\\rho(x,y,z)dV\\\\ \\mathcal I\_\{zz\}=\\int\_\\mathcal B(x^2+y^2)\\rho(x,y,z)dV\\\\ \\mathcal I\_\{xy\}=\\int\_\\mathcal Bxy\\rho(x,y,z)dV\\\\ \\mathcal I\_\{xz\}=\\int\_\\mathcal Bxz\\rho(x,y,z)dV\\\\ \\mathcal I\_\{yz\}=\\int\_\\mathcal Byz\\rho(x,y,z)dV \\end\{cases\} \\tag\{8.24\} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧Ixx=∫B(y2\+z2)ρ(x,y,z)dVIyy=∫B(x2\+z2)ρ(x,y,z)dVIzz=∫B(x2\+y2)ρ(x,y,z)dVIzz=∫B(x2\+y2)ρ(x,y,z)dVIxy=∫Bxyρ(x,y,z)dVIxz=∫Bxzρ(x,y,z)dVIyz=∫Byzρ(x,y,z)dV(8.24)  
给定一个惯量矩阵 I b \\mathcal I\_b Ib, 惯量主轴由 I b \\mathcal I\_b Ib的特征向量和特征值给定。设特征向量为 v 1 , v 2 , v 3 v\_1,v\_2,v\_3 v1,v2,v3,特征值为 λ 1 , λ 2 , λ 3 \\lambda\_1, \\lambda\_2, \\lambda\_3 λ1,λ2,λ3. 那么惯量主轴的方向为特征向量 v 1 , v 2 , v 3 v\_1,v\_2,v\_3 v1,v2,v3的方向，在这些轴的转动惯量值为主惯量矩，分别是 λ 1 , λ 2 , λ 3 > 0 \\lambda\_1, \\lambda\_2 , \\lambda\_3>0 λ1,λ2,λ3>0 .

假设在坐标系\{b\}中，一个惯量矩阵表示为 I b \\mathcal I\_b Ib, 坐标系\{c\}可由旋转矩阵 R b c R\_\{bc\} Rbc描述，那么该惯量矩阵在坐标系\{c\}中表示为 I c \\mathcal I\_c Ic。 由于旋转刚体的动能与选择的坐标系无关，因此有  
 1 2 ω c T I c ω c = 1 2 ω b T I b ω b = 1 2 ω c T ( R b c T I b R b c ) ω c . \\frac\{1\}\{2\}\\omega\_c^T\\mathcal I\_c\\omega\_c=\\frac\{1\}\{2\}\\omega\_b^T\\mathcal I\_b\\omega\_b\\\\ =\\frac\{1\}\{2\}\\omega\_c^T(R\_\{bc\}^T\\mathcal I\_bR\_\{bc\})\\omega\_c. 21ωcTIcωc=21ωbTIbωb=21ωcT(RbcTIbRbc)ωc.  
也就是说  
 I c = R b c T I b R b c . (8.26) \\mathcal I\_c=R\_\{bc\}^TI\_bR\_\{bc\}. \\tag\{8.26\} Ic=RbcTIbRbc.(8.26)  
如果\{b\}的坐标轴与惯量主轴不平行，我们可以在经过旋转变换 R b c R\_\{bc\} Rbc的坐标系\{c\}中表示该惯量矩阵, 从而可以把惯量矩阵对角化。 R b c R\_\{bc\} Rbc 的列就是 I b \\mathcal I\_b Ib的向量。

有时惯量矩阵不在质心的坐标系表示更方便，例如在关节处的坐标系表示。那么可以通过斯坦纳定理（平行轴定理）转换。

定理：惯量矩阵 I q \\mathcal I\_q Iq定义在坐标系\{q\}中，各轴与坐标系\{b\}平行，q为坐标系\{b\}中坐标为 q = ( q x , q y , q z ) q=(q\_x,q\_y,q\_z) q=(qx,qy,qz)的一点，定义在质心\{b\}的坐标系的惯量矩阵为 I b \\mathcal I\_b Ib,那么  
 I q = I b + m ( q T q I − q q T ) , (8.27) \\mathcal I\_q=\\mathcal I\_b+\\text m(q^TqI-qq^T),\\tag\{8.27\} Iq=Ib\+m(qTqI−qqT),(8.27)  
这里 I I I是 3 × 3 3\\times 3 3×3的单位矩阵， m \\text m m为刚体的质量。

## 力旋量 ##

上面的式(8.22)和式(8.23)可以写为  
 \[ m b f b \] = \[ I b 0 0 m I \] \[ ω ˙ b v ˙ b \] + \[ \[ ω b \] 0 0 \[ ω b \] \] \[ I b 0 0 m I \] \[ ω b v b \] (8.29) \\left\[ \\begin\{matrix\} m\_b \\\\ f\_b \\end\{matrix\}\\right\] =\\left\[ \\begin\{matrix\} \\mathcal I\_b & 0 \\\\ 0 &\\text mI \\end\{matrix\}\\right\] \\left\[ \\begin\{matrix\} \\dot \\omega\_b \\\\ \\dot v\_b \\end\{matrix\}\\right\] +\\left\[ \\begin\{matrix\} \[\\omega\_b \]& 0\\\\ 0 & \[\\omega\_b\] \\end\{matrix\}\\right\] \\left\[ \\begin\{matrix\} \\mathcal I\_b & 0 \\\\ 0 &\\text mI \\end\{matrix\}\\right\] \\left\[ \\begin\{matrix\} \\omega\_b \\\\ v\_b \\end\{matrix\}\\right\]\\tag\{8.29\} \[mbfb\]=\[Ib00mI\]\[ω˙bv˙b\]\+\[\[ωb\]00\[ωb\]\]\[Ib00mI\]\[ωbvb\](8.29)

这里 I I I是 3 × 3 3\\times 3 3×3的单位阵。另外 \[ v \] v = v × v = 0 , \[ v \] T = − \[ v \] \[v\]v=v\\times v=0,\[v\]^T=-\[v\] \[v\]v=v×v=0,\[v\]T=−\[v\]，那么式（8.29）可以写为如下等效形式  
 \[ m b f b \] = \[ I b 0 0 m I \] \[ ω ˙ b v ˙ b \] + \[ \[ ω b \] \[ v b \] 0 \[ ω b \] \] \[ I b 0 0 m I \] \[ ω b v b \] = \[ I b 0 0 m I \] \[ ω ˙ b v ˙ b \] − \[ \[ ω b \] 0 \[ v b \] \[ ω b \] \] T \[ I b 0 0 m I \] \[ ω b v b \] (8.30) \\left\[ \\begin\{matrix\} m\_b \\\\ f\_b \\end\{matrix\}\\right\] =\\left\[ \\begin\{matrix\} \\mathcal I\_b & 0 \\\\ 0 &\\text mI \\end\{matrix\}\\right\] \\left\[ \\begin\{matrix\} \\dot \\omega\_b \\\\ \\dot v\_b \\end\{matrix\}\\right\] +\\left\[ \\begin\{matrix\} \[\\omega\_b \]& \[v\_b\]\\\\ 0 & \[\\omega\_b\] \\end\{matrix\}\\right\] \\left\[ \\begin\{matrix\} \\mathcal I\_b & 0 \\\\ 0 &\\text mI \\end\{matrix\}\\right\] \\left\[ \\begin\{matrix\} \\omega\_b \\\\ v\_b \\end\{matrix\}\\right\]\\\\ =\\left\[ \\begin\{matrix\} \\mathcal I\_b & 0 \\\\ 0 &\\text mI \\end\{matrix\}\\right\] \\left\[ \\begin\{matrix\} \\dot \\omega\_b \\\\ \\dot v\_b \\end\{matrix\}\\right\] -\\left\[ \\begin\{matrix\} \[\\omega\_b \]& 0\\\\ \[v\_b\] & \[\\omega\_b\] \\end\{matrix\}\\right\]^T \\left\[ \\begin\{matrix\} \\mathcal I\_b & 0 \\\\ 0 &\\text mI \\end\{matrix\}\\right\] \\left\[ \\begin\{matrix\} \\omega\_b \\\\ v\_b \\end\{matrix\}\\right\]\\tag\{8.30\} \[mbfb\]=\[Ib00mI\]\[ω˙bv˙b\]\+\[\[ωb\]0\[vb\]\[ωb\]\]\[Ib00mI\]\[ωbvb\]=\[Ib00mI\]\[ω˙bv˙b\]−\[\[ωb\]\[vb\]0\[ωb\]\]T\[Ib00mI\]\[ωbvb\](8.30)  
这样方程中的每一项可以用一个6维的空间量表示：

(1)向量 ( ω b , v b ) ， ( m b , f b ) (\\omega\_b,v\_b)，(m\_b,f\_b) (ωb,vb)，(mb,fb)分别用物体旋量 V b \\mathcal V\_b Vb和力旋量 F b \\mathcal F\_b Fb表示,  
 V b = \[ ω b v b \] , F b = \[ m b f b \] (8.31) \\mathcal V\_b=\\left\[\\begin \{matrix\} \\omega\_b\\\\ v\_b \\end\{matrix\}\\right\], \\mathcal F\_b=\\left\[\\begin \{matrix\} m\_b\\\\ f\_b \\end\{matrix\}\\right\]\\tag\{8.31\} Vb=\[ωbvb\],Fb=\[mbfb\](8.31)  
(2)空间惯量矩阵 G b ∈ R 6 × 6 \\mathcal G\_b\\in \\mathbb R^\{6\\times 6\} Gb∈R6×6定义为  
 G b = \[ I b 0 0 m I \] (8.32) \\mathcal G\_b= \\left\[\\begin\{matrix\} \\mathcal I\_b &0\\\\ 0 &\\text mI \\end\{matrix\}\\right\]\\tag\{8.32\} Gb=\[Ib00mI\](8.32)  
这时候动能用空间惯量矩阵可以表示为  
 E = 1 2 ω b T I b ω b + 1 2 m v b T v b = 1 2 V b T G b V b . (8.33) E=\\frac\{1\}\{2\}\\omega\_b^T\\mathcal I\_b\\omega\_b+\\frac\{1\}\{2\}\\text mv\_b^Tv\_b=\\frac\{1\}\{2\}\\mathcal V\_b^T\\mathcal G\_b\\mathcal V\_b.\\tag\{8.33\} E=21ωbTIbωb\+21mvbTvb=21VbTGbVb.(8.33)  
(3)空间动量 P b ∈ R 6 × 6 \\mathcal P\_b\\in\\mathbb R^\{6\\times 6\} Pb∈R6×6定义为  
 P b = \[ I b m v b \] = \[ I b 0 0 m I \] \[ ω b v b \] = G b V b . (8.34) \\mathcal P\_b=\\left\[\\begin\{matrix\} \\mathcal I\_b\\\\ \\text mv\_b \\end\{matrix\}\\right\] =\\left\[\\begin\{matrix\} \\mathcal I\_b&0\\\\ 0 & \\text mI \\end\{matrix\}\\right\] \\left\[\\begin\{matrix\} \\omega\_b\\\\ v\_b \\end\{matrix\}\\right\] =\\mathcal G\_b\\mathcal V\_b.\\tag\{8.34\} Pb=\[Ibmvb\]=\[Ib00mI\]\[ωbvb\]=GbVb.(8.34)

观察发现式（8.30）右边第二项是 P b \\mathcal P\_b Pb左乘下面的矩阵构成

− \[ \[ ω b \] 0 \[ v b \] \[ ω b \] \] T . (8.35) -\\left\[\\begin\{matrix\} \[\\omega\_b\]&0\\\\ \[v\_b\]&\[\\omega\_b\] \\end\{matrix\}\\right\]^T.\\tag\{8.35\} −\[\[ωb\]\[vb\]0\[ωb\]\]T.(8.35)

注意，两个矢量的叉乘可以用反对称矩阵表示

\[ ω 1 × ω 2 \] = \[ ω 1 \] \[ ω 2 \] − \[ ω 2 \] \[ ω 1 \] . (8.36) \[\\omega\_1\\times\\omega\_2\]=\[\\omega\_1\]\[\\omega\_2\]-\[\\omega\_2\]\[\\omega\_1\].\\tag\{8.36\} \[ω1×ω2\]=\[ω1\]\[ω2\]−\[ω2\]\[ω1\].(8.36)

式（8.35）中的矩阵可以看成是6维旋量叉乘运算的一般化。给定两个旋量 V 1 = ( ω 1 , v 1 ) , V 2 = ( ω 2 , v 2 ) \\mathcal V\_1=(\\omega\_1,v\_1),\\mathcal V\_2=(\\omega\_2,v\_2) V1=(ω1,v1),V2=(ω2,v2), 进行

\[ V 1 \] \[ V 2 \] − \[ V 2 \] \[ V 1 \] = \[ \[ ω 1 \] v 1 0 0 \] \[ \[ ω 2 \] v 2 0 0 \] − \[ \[ ω 2 \] v 2 0 0 \] \[ \[ ω 1 \] v 1 0 0 \] = \[ \[ ω 1 \] \[ ω 2 \] − \[ ω 2 \] \[ ω 1 \] \[ ω 1 \] v 2 − \[ ω 2 \] v 1 0 0 \] = \[ \[ ω ′ \] \[ v ′ \] 0 0 \] , \[\\mathcal V\_1\]\[\\mathcal V\_2\]-\[\\mathcal V\_2\]\[\\mathcal V\_1\]= \\left\[\\begin\{matrix\} \[\\omega\_1\]&v\_1\\\\ 0& 0 \\end\{matrix\}\\right\] \\left\[\\begin\{matrix\} \[\\omega\_2\]&v\_2\\\\ 0& 0 \\end\{matrix\}\\right\]- \\left\[\\begin\{matrix\} \[\\omega\_2\]&v\_2\\\\ 0& 0 \\end\{matrix\}\\right\] \\left\[\\begin\{matrix\} \[\\omega\_1\]&v\_1\\\\ 0& 0 \\end\{matrix\}\\right\]\\\\ =\\left\[\\begin\{matrix\} \[\\omega\_1\]\[\\omega\_2\]-\[\\omega\_2\]\[\\omega\_1\]&\[\\omega\_1\]v\_2-\[\\omega\_2\]v\_1\\\\ 0& 0 \\end\{matrix\}\\right\]\\\\ =\\left\[\\begin\{matrix\} \[\\omega '\]&\[v'\]\\\\ 0& 0 \\end\{matrix\}\\right\], \[V1\]\[V2\]−\[V2\]\[V1\]=\[\[ω1\]0v10\]\[\[ω2\]0v20\]−\[\[ω2\]0v20\]\[\[ω1\]0v10\]=\[\[ω1\]\[ω2\]−\[ω2\]\[ω1\]0\[ω1\]v2−\[ω2\]v10\]=\[\[ω′\]0\[v′\]0\],  
还可以用向量写成更简洁的形式  
 \[ ω ′ v ′ \] = \[ \[ ω 1 \] 0 \[ v 1 \] \[ ω 1 \] \] \[ ω 2 v 2 \] \\left\[\\begin\{matrix\} \\omega'\\\\ v' \\end\{matrix\}\\right\]= \\left\[\\begin\{matrix\} \[\\omega\_1\]&0\\\\ \[v\_1\]&\[\\omega\_1\] \\end\{matrix\}\\right\] \\left\[\\begin\{matrix\} \\omega\_2\\\\ v\_2 \\end\{matrix\}\\right\] \[ω′v′\]=\[\[ω1\]\[v1\]0\[ω1\]\]\[ω2v2\]  
一般地两个旋量的叉乘称为**李括号**.

**定义**：给定两个旋量 V 1 = ( ω 1 , v 1 ) , V 2 = ( ω 2 , v 2 ) \\mathcal V\_1=(\\omega\_1,v\_1),\\mathcal V\_2=(\\omega\_2,v\_2) V1=(ω1,v1),V2=(ω2,v2),  V 1 \\mathcal V\_1 V1和 V 2 \\mathcal V\_2 V2的李括号写成 \[ ad V 1 \] V 2 \[\\text\{ad\}\_\{\\mathcal V\_1\} \]\\mathcal V\_2 \[adV1\]V2 或者 ad V 1 ( V 2 ) \\text\{ad\} \_\{\\mathcal V\_1\}(\\mathcal V\_2) adV1(V2), 定义为  
 \[ \[ ω 1 \] 0 \[ v 1 \] \[ ω 1 \] \] \[ ω 2 v 2 \] = \[ ad V 1 \] V 1 = ad V 1 ( V 2 ) ∈ R 6 , (8.37) \\left\[\\begin\{matrix\} \[\\omega\_1\]&0\\\\ \[v\_1\]&\[\\omega\_1\] \\end\{matrix\}\\right\] \\left\[\\begin\{matrix\} \\omega\_2\\\\ v\_2 \\end\{matrix\}\\right\]= \[\\text\{ad\}\_\{\\mathcal V\_1\}\]\\mathcal V\_1=\\text\{ad\}\_\{\\mathcal V\_1\}(\\mathcal V\_2)\\in\\mathbb R^6,\\tag\{8.37\} \[\[ω1\]\[v1\]0\[ω1\]\]\[ω2v2\]=\[adV1\]V1=adV1(V2)∈R6,(8.37)  
其中  
 \[ ad V \] = \[ \[ ω \] 0 \[ v \] \[ ω \] \] ∈ R 6 × 6 \[\\text\{ad\}\_\\mathcal V\]=\\left\[\\begin\{matrix\} \[\\omega\]&0\\\\ \[v\]&\[\\omega\] \\end\{matrix\}\\right\]\\in\\mathbb R^\{6\\times 6\} \[adV\]=\[\[ω\]\[v\]0\[ω\]\]∈R6×6  
**定义**：给一个运动旋量 V = ( ω , v ) \\mathcal V =(\\omega,v) V=(ω,v)和力旋量 F = ( m , f ) \\mathcal F=(m,f) F=(m,f), 定义映射  
 ad V T ( F ) = \[ ad V \] T ( F ) = \[ \[ ω \] 0 \[ v \] \[ ω \] \] T \[ m f \] = \[ − \[ ω \] m − \[ v \] f − \[ ω \] f \] . (8.39) \\text\{ad\}^T\_\{\\mathcal V\}(\\mathcal F)=\[\\text\{ad\}\_\{\\mathcal V\}\]^T(\\mathcal F)=\\left\[\\begin\{matrix\} \[\\omega\]&0\\\\ \[v\]&\[\\omega\] \\end\{matrix\}\\right\]^T \\left\[\\begin\{matrix\} m\\\\ f \\end\{matrix\}\\right\]= \\left\[\\begin\{matrix\} -\[\\omega\]m-\[v\]f\\\\ -\[\\omega\]f \\end\{matrix\}\\right\].\\tag\{8.39\} adVT(F)=\[adV\]T(F)=\[\[ω\]\[v\]0\[ω\]\]T\[mf\]=\[−\[ω\]m−\[v\]f−\[ω\]f\].(8.39)  
使用上述符号和定义，单个刚体的动力学方程可以写成  
 F b = G b V ˙ − ad V b T ( P b ) = G b V ˙ − \[ ad V b \] T G b V b (8.40) \\mathcal F\_b=\\mathcal G\_b\\dot\{\\mathcal V\}-\\text\{ad\}^T\_\{\\mathcal V\_b\}(\\mathcal P\_b)\\\\ =\\mathcal G\_b\\dot\{\\mathcal V\}-\[\\text\{ad\}\_\{\\mathcal V\_b\}\]^T\\mathcal G\_b\\mathcal V\_b \\tag\{8.40\} Fb=GbV˙−adVbT(Pb)=GbV˙−\[adVb\]TGbVb(8.40)

## 在其他坐标系的动力学 ##

式（8.40）依赖于使用的质心坐标系\{b\}。在另一个坐标系\{a\}中的动力学方程，可以通过动能不依赖于坐标系的性质推导出来  
 1 2 = V a T G a V a = 1 2 V b T G b V b = 1 2 ( \[ A d T b a \] V a ) T G b \[ A d T b a \] V a \\frac\{1\}\{2\}=\\mathcal \{V\_a^TG\_aV\_a\}=\\frac\{1\}\{2\}\\mathcal \{V\_b^TG\_bV\_b\}\\\\ =\\frac\{1\}\{2\}(\[Ad\_\{T\_\{ba\}\}\]\\mathcal V\_a)^T\\mathcal \{G\_b\}\[Ad\_\{T\_\{ba\}\}\]\\mathcal V\_a 21=VaTGaVa=21VbTGbVb=21(\[AdTba\]Va)TGb\[AdTba\]Va  
其中Ad是伴随表示. 也就是说\{a\}坐标系的空间惯量矩阵 G a \\mathcal G\_a Ga与 G b \\mathcal G\_b Gb的关系可以用下式表示  
 G a = \[ A d T b a \] T G b \[ A d T b a \] . (8.42) \\mathcal G\_a=\[Ad\_\{T\_\{ba\}\}\]^T\\mathcal G\_b\[Ad\_\{T\_\{ba\}\}\].\\tag\{8.42\} Ga=\[AdTba\]TGb\[AdTba\].(8.42)  
使用空间惯量矩阵 G a \\mathcal G\_a Ga,在\{b\}坐标系的对应的运动方程(8.40)在\{a\}坐标系可等效地写成  
 F a = G a V ˙ a − \[ ad V a \] T G a V a , (8.43) \\mathcal F\_a=\\mathcal \{G\_a\\dot V\_a\}-\[\\text\{ad\}\_\{\\mathcal V\_a \}\]^T\\mathcal\{G\_aV\_a\} ,\\tag\{8.43\} Fa=GaV˙a−\[adVa\]TGaVa,(8.43)  
其中 F a , V a \\mathcal\{F\_a,V\_a\} Fa,Va是在坐标系\{a\}中描述的力旋量和运动旋量.

参考文献

\[1\] Kenvin M. Lynch , Frank C. Park, Modern Robotics Mechanics,Planning , and Control. May 3, 2017

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0dhbGF4eV9Sb2JvdA_size_16_color_FFFFFF_t_70_pic_center]: /images/20221123/07d7643edaae4fc29e9d9c983a8f3d6f.png