摘要 首先推导角速度公式和角加速度公式,并举一个例子说明角速度公式的细节,然后从加速度公式出发推导转动惯量矩阵 J 和姿态动力学方程 Jw’=w×Jw,并解释为什么需要进行一次坐标变换。最后总结从力矩到姿态四元数的完整过程,并对无力矩输入时的进动现象进行仿真。
本体系绕世界系以角速度 ω ⃗ \vec\omega ω 旋转,设世界系下位置向量 r ⃗ \vec r r 的一阶导和二阶导分别为 w r ⃗ ˙ ^w\dot{\vec r} wr˙ 和 w r ⃗ ¨ ^w\ddot{\vec r} wr¨,本体系下位置向量的一阶导和二阶导分别为 b r ⃗ ˙ ^b\dot{\vec r} br˙ 和 b r ⃗ ¨ ^b\ddot{\vec r} br¨,满足
w r ⃗ = R b r ⃗ w r ⃗ ˙ = R b r ⃗ ˙ + ω ⃗ × w r ⃗ w r ⃗ ¨ = R b r ⃗ ¨ + 2 ω ⃗ × R b r ⃗ ˙ + ω ⃗ × ( ω ⃗ × w r ⃗ ) + ω ⃗ ˙ × w r ⃗ (1) \begin{aligned} ^w\vec r =& R\ {^b\vec r} \\ ^w\dot{\vec r} =& R\ {^b\dot{\vec r}}+ \vec\omega\times{^w\vec r} \\ ^w\ddot{\vec r} =& R\ {^b\ddot{\vec r}} +2\vec\omega\times R\ {^b\dot{\vec r}} +\vec\omega\times(\vec\omega\times{^w\vec r}) +\dot{\vec\omega}\times{^w\vec r} \\ \end{aligned}\tag{1} wr=wr˙=wr¨=R brR br˙+ω×wrR br¨+2ω×R br˙+ω×(ω×wr)+ω˙×wr(1)
式中所有的 R R R 都是从本体系到世界系的旋转矩阵,完整应写作 b w R {^w_bR} bwR;所有的 ω \omega ω 都在世界系下表示,完整应写作 w ω {^w\omega} wω:
w r ⃗ = b w R b r ⃗ w r ⃗ ˙ = b w R b r ⃗ ˙ + w ω ⃗ × w r ⃗ w r ⃗ ¨ = b w R b r ⃗ ¨ + 2 w ω ⃗ × b w R b r ⃗ ˙ + w ω ⃗ × ( w ω ⃗ × w r ⃗ ) + w ω ⃗ ˙ × w r ⃗ \begin{aligned} ^w\vec r =& {^w_bR}\ {^b\vec r} \\ ^w\dot{\vec r} =& {^w_bR}\ {^b\dot{\vec r}}+ {^w\vec\omega}\times{^w\vec r} \\ ^w\ddot{\vec r} =& {^w_bR}\ {^b\ddot{\vec r}} +2{^w\vec\omega}\times {^w_bR}\ {^b\dot{\vec r}} +{^w\vec\omega}\times({^w\vec\omega}\times{^w\vec r}) +{^w\dot{\vec\omega}}\times{^w\vec r} \\ \end{aligned} wr=wr˙=wr¨=bwR brbwR br˙+wω×wrbwR br¨+2wω×bwR br˙+wω×(wω×wr)+wω˙×wr
证明:
d d t ( w r ⃗ ) = d d t ( R b r ⃗ ) w r ⃗ ˙ = R ˙ b r ⃗ + R b r ⃗ ˙ = R b r ⃗ ˙ + w ω ⃗ × R b r ⃗ = R b r ⃗ ˙ + w ω ⃗ × w r ⃗ \begin{aligned} \frac{\text d}{\text dt}(^w\vec r) =& \frac{\text d}{\text dt}({R}\ {^b\vec r}) \\ ^w\dot{\vec r} =& \dot R\ {^b\vec r}+R\ {^b\dot{\vec r}} \\ =& R\ {^b\dot{\vec r}}+{^w\vec\omega}\times R\ {^b\vec r} \\ =& R\ {^b\dot{\vec r}}+{^w\vec\omega}\times{^w\vec r} \\ \end{aligned} dtd(wr)=wr˙===dtd(R br)R˙ br+R br˙R br˙+wω×R brR br˙+wω×wr
此处以及后面的 w ω ⃗ ^w\vec\omega wω 都是在世界系下的坐标,下面简记作 ω ⃗ \vec\omega ω。
此处补充一些个人理解。 R ˙ = ω ⃗ × R \dot R=\vec\omega\times R R˙=ω×R 是因为 R R R 可以简单地看作三个向量绕旋转轴 ω ⃗ \vec\omega ω 转动,不涉及坐标变换;而 w r ⃗ ˙ ≠ ω ⃗ × w r ⃗ ^w\dot{\vec r}\neq\vec{\omega} \times{^w\vec r} wr˙=ω×wr 是因为此时向量 r ⃗ \vec r r 不是绕旋转轴 ω ⃗ \vec\omega ω 转动,而是个复合运动, ω ⃗ \vec\omega ω 是本体系(旋转坐标系)相对于世界系的旋转角速度, r ⃗ \vec r r 在本体系下还有 r ⃗ 2 \vec r_2 r2 的角速度,当 r ⃗ \vec r r 在本体系下的角速度为0时两种情况等价,可以理解成有个本体系固定在 r ⃗ \vec r r 上。
对式(1)的第二行再次求导
d d t ( w r ⃗ ˙ ) = d d t ( R b r ⃗ ˙ + ω ⃗ × w r ⃗ ) w r ⃗ ¨ = R ˙ b r ⃗ ˙ + R b r ⃗ ¨ + ω ⃗ ˙ × w r ⃗ + ω ⃗ × w r ⃗ ˙ = R b r ⃗ ¨ + ω ⃗ × R b r ⃗ ˙ + ω ⃗ × ( R b r ⃗ ˙ + ω ⃗ × w r ⃗ ) + ω ⃗ ˙ × w r ⃗ = R b r ⃗ ¨ + 2 ω ⃗ × R b r ⃗ ˙ + ω ⃗ × ( ω ⃗ × w r ⃗ ) + ω ⃗ ˙ × w r ⃗ \begin{aligned} \frac{\text d}{\text dt}(^w\dot{\vec r}) =& \frac{\text d}{\text dt}(R\ {^b\dot{\vec r}}+\vec\omega\times{^w\vec r}) \\ ^w\ddot{\vec r} =& \dot R\ {^b\dot{\vec r}} +R\ {^b\ddot{\vec r}} +\dot{\vec\omega}\times{^w\vec r} +\vec\omega\times{^w\dot{\vec r}} \\ =& R\ {^b\ddot{\vec r}} +\vec\omega\times R\ {^b\dot{\vec r}} +\vec\omega\times(R\ {^b\dot{\vec r}}+\vec\omega\times{^w\vec r}) +\dot{\vec\omega}\times{^w\vec r} \\ =& R\ {^b\ddot{\vec r}} +2\vec\omega\times R\ {^b\dot{\vec r}} +\vec\omega\times(\vec\omega\times{^w\vec r}) +\dot{\vec\omega}\times{^w\vec r} \\ \end{aligned} dtd(wr˙)=wr¨===dtd(R br˙+ω×wr)R˙ br˙+R br¨+ω˙×wr+ω×wr˙R br¨+ω×R br˙+ω×(R br˙+ω×wr)+ω˙×wrR br¨+2ω×R br˙+ω×(ω×wr)+ω˙×wr
当匀速转动时 ω ⃗ ˙ = 0 \dot{\vec\omega}=0 ω˙=0,最后一项可以省略。
如图所示,世界系用 O x w y w z w Ox_wy_wz_w Oxwywzw 表示,本体系用 O x b y b z b Ox_by_bz_b Oxbybzb 表示。一开始时,本体系与世界系重合,向量 r ⃗ \vec r r 与y轴正向重合,然后本体系沿世界系x轴逆时针旋转,角速度为 w ω ⃗ 1 ^w\vec\omega_1 wω1;向量 r ⃗ \vec r r 沿本体系z轴顺时针旋转,角速度为 b ω ⃗ 2 ^b\vec\omega_2 bω2
w ω ⃗ 1 = [ 1 0 0 ] , b ω ⃗ 2 = [ 0 0 − 1 ] ^w\vec\omega_1=\begin{bmatrix} 1 \\ 0 \\ 0 \end{bmatrix},\ ^b\vec\omega_2=\begin{bmatrix} 0 \\ 0 \\ -1 \end{bmatrix} wω1=
100
, bω2=
00−1
注意两个角速度向量分别在世界系和本体系下表示。向量 r ⃗ \vec r r 在本体系下的坐标为
b r ⃗ = [ sin t cos t 0 ] ^b\vec r=\begin{bmatrix} \sin t \\ \cos t \\ 0 \end{bmatrix} br=
sintcost0
本体系到世界系的旋转矩阵 b w R ^w_bR bwR 为
b w R = [ 1 0 0 0 cos t − sin t 0 sin t cos t ] , w b R = [ 1 0 0 0 cos t sin t 0 − sin t cos t ] {^w_bR}=\begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos t & -\sin t \\ 0 & \sin t & \cos t \end{bmatrix},\ ^b_wR=\begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos t & \sin t \\ 0 & -\sin t & \cos t \end{bmatrix} bwR=
1000costsint0−sintcost
, wbR=
1000cost−sint0sintcost
向量 r ⃗ \vec r r 在世界系下的坐标为
w r ⃗ = b w R b r ⃗ = [ 1 0 0 0 cos t − sin t 0 sin t cos t ] [ sin t cos t 0 ] = [ sin t cos 2 t sin t cos t ] {^w\vec r}={^w_bR}\ {^b\vec r}= \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos t & -\sin t \\ 0 & \sin t & \cos t \end{bmatrix} \begin{bmatrix} \sin t \\ \cos t \\ 0 \end{bmatrix} =\begin{bmatrix} \sin t \\ \cos^2 t \\ \sin t\cos t \end{bmatrix} wr=bwR br=
1000costsint0−sintcost
sintcost0
=
sintcos2tsintcost
两边同时求导得到角速度公式
w v ⃗ = w r ⃗ ˙ = b w R b r ⃗ ˙ + w ω ⃗ 1 × w r ⃗ [ cos t − sin 2 t cos 2 t ] = [ 1 0 0 0 cos t − sin t 0 sin t cos t ] [ cos t − sin t 0 ] + [ 1 0 0 ] × [ sin t cos 2 t sin t cos t ] = [ cos t − sin t cos t − sin 2 t ] + [ 0 − sin t cos t cos 2 t ] \begin{aligned} ^w\vec v = {^w\dot{\vec r}} =& {^w_bR}\ {^b\dot{\vec r}} + {^w\vec\omega_1} \times {^w\vec r} \\ \begin{bmatrix} \cos t \\ -\sin 2t \\ \cos 2t \end{bmatrix} =& \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos t & -\sin t \\ 0 & \sin t & \cos t \end{bmatrix} \begin{bmatrix} \cos t \\ -\sin t \\ 0 \end{bmatrix} +\begin{bmatrix} 1 \\ 0 \\ 0 \end{bmatrix} \times\begin{bmatrix} \sin t \\ \cos^2 t \\ \sin t\cos t \end{bmatrix} \\ =& \begin{bmatrix} \cos t \\ -\sin t\cos t \\ -\sin^2t \end{bmatrix} +\begin{bmatrix} 0 \\ -\sin t\cos t \\ \cos^2 t \end{bmatrix} \\ \end{aligned} wv=wr˙=
cost−sin2tcos2t
==bwR br˙+wω1×wr
1000costsint0−sintcost
cost−sint0
+
100
×
sintcos2tsintcost
cost−sintcost−sin2t
+
0−sintcostcos2t
于是可以得到,世界系下的速度向量转换到本体系下并不等于本体系下位置向量的导数。
b v ⃗ = w b R w v ⃗ = [ 1 0 0 0 cos t sin t 0 − sin t cos t ] [ cos t − sin 2 t cos 2 t ] = [ cos t − sin t cos t ] ≠ b r ⃗ ˙ = [ cos t − sin t 0 ] \begin{aligned} & ^b\vec v = {^b_wR}\ {^w\vec v} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos t & \sin t \\ 0 & -\sin t & \cos t \end{bmatrix} \begin{bmatrix} \cos t \\ -\sin 2t \\ \cos 2t \end{bmatrix} =\begin{bmatrix} \cos t \\ -\sin t \\ \cos t \end{bmatrix} \\ \neq& {^b\dot{\vec r}} = \begin{bmatrix} \cos t \\ -\sin t \\ 0 \end{bmatrix} \end{aligned} =bv=wbR wv=
1000cost−sint0sintcost
cost−sin2tcos2t
=
cost−sintcost
br˙=
cost−sint0
M ⃗ = J ω ⃗ ˙ + ω ⃗ × J ω ⃗ \vec M=J\dot{\vec\omega}+\vec\omega\times J\vec\omega M=Jω˙+ω×Jω
参考资料[3]的第2章是我觉得所有资料里讲这个方程讲的最清楚的,网上看什么资料都不如看这本英文书,这篇博客就当是我的笔记了,强烈建议有条件还是看原文。我把原文大致翻译了一下:
刚体姿态动力学详细推导与翻译
角动量定理
d H ⃗ d t = M ⃗ \frac{\text d\vec H}{\text dt}=\vec M dtdH=M
其中 H , M H,M H,M 分别为刚体的角动量和作用在刚体上的力矩。
下面的推导来自文末参考资料[1],也是主流推导方法,主要思路是因为转动惯量矩阵在本体系保持不变,利用向量在本体系与世界系中的关系推导。但[1]不够详细,更详细的推导见参考资料[3]。
刚体角动量是刚体内所有质量微元相对基准点 S S S 的角动量之和
h ⃗ = m r ⃗ × v ⃗ , H = ∭ V r ⃗ × r ⃗ ˙ d m \vec h=m\vec r\times\vec v,\quad H=\iiint\limits_V\vec r\times\dot{\vec r}\text dm h=mr×v,H=V∭r×r˙dm
其中 d m \text dm dm 为刚体的质量微元。
r ⃗ × v ⃗ = r ⃗ × ( ω ⃗ × r ⃗ ) = r 2 ω ⃗ − ( r ⃗ ⋅ ω ⃗ ) r ⃗ = [ ( x 2 + y 2 + z 2 ) w x − ( x w x + y w y + z w z ) x ] e ⃗ x + [ ( x 2 + y 2 + z 2 ) w y − ( x w x + y w y + z w z ) y ] e ⃗ y + [ ( x 2 + y 2 + z 2 ) w z − ( x w x + y w y + z w z ) z ] e ⃗ z = [ ( y 2 + z 2 ) w x − x y w y − x z w z ] e ⃗ x + [ − x y w x + ( x 2 + z 2 ) w y − y z w z ] e ⃗ y + [ − x z w x − y z w y + ( x 2 + y 2 ) w z ] e ⃗ z = [ y 2 + z 2 − x y − x z − x y x 2 + z 2 − y z − x z − y z x 2 + y 2 ] [ w x w y w z ] \begin{aligned} & \vec r\times\vec v = \vec r\times(\vec\omega\times\vec r) = r^2\vec\omega-(\vec r\cdot\vec\omega)\vec r \\ =& [(x^2+y^2+z^2)w_x-(xw_x+yw_y+zw_z)x]\vec e_x \\ +& [(x^2+y^2+z^2)w_y-(xw_x+yw_y+zw_z)y]\vec e_y \\ +& [(x^2+y^2+z^2)w_z-(xw_x+yw_y+zw_z)z]\vec e_z \\ =& [(y^2+z^2)w_x-xyw_y-xzw_z]\vec e_x \\ +& [-xyw_x+(x^2+z^2)w_y-yzw_z]\vec e_y \\ +& [-xzw_x-yzw_y+(x^2+y^2)w_z]\vec e_z \\ =&\begin{bmatrix} y^2+z^2 & -xy & -xz \\ -xy & x^2+z^2 & -yz \\ -xz & -yz & x^2+y^2 \\ \end{bmatrix} \begin{bmatrix} w_x \\ w_y \\ w_z \end{bmatrix} \\ \end{aligned} =++=++=r×v=r×(ω×r)=r2ω−(r⋅ω)r[(x2+y2+z2)wx−(xwx+ywy+zwz)x]ex[(x2+y2+z2)wy−(xwx+ywy+zwz)y]ey[(x2+y2+z2)wz−(xwx+ywy+zwz)z]ez[(y2+z2)wx−xywy−xzwz]ex[−xywx+(x2+z2)wy−yzwz]ey[−xzwx−yzwy+(x2+y2)wz]ez
y2+z2−xy−xz−xyx2+z2−yz−xz−yzx2+y2
wxwywz
于是
H = ∭ V r ⃗ × v ⃗ d m = J ω ⃗ H=\iiint\limits_V\vec r\times\vec v\text dm=J\vec\omega H=V∭r×vdm=Jω
其中 J J J 为刚体的转动惯量矩阵,形式为
J = [ J x − J x y − J x z − J x y J y − J y z − J x z − J y z J z ] J=\begin{bmatrix} J_x & -J_{xy} & -J_{xz} \\ -J_{xy} & J_y & -J_{yz} \\ -J_{xz} & -J_{yz} & J_z \\ \end{bmatrix} J=
Jx−Jxy−Jxz−JxyJy−Jyz−Jxz−JyzJz
其中
J x = ∭ V ( y 2 + z 2 ) d m J_x=\iiint\limits_V(y^2+z^2)\text dm Jx=V∭(y2+z2)dm
其它项类似。可以看出 J J J 是刚体的属性,在与刚体固连的本体系下不随姿态改变。
这里需要注意区分世界系和与刚体固连的本体系,也就是为什么需要进行一次坐标变换。在世界系中,刚体上各点的坐标不是定值,所以刚体的转动惯量矩阵在世界系下不是定值,只有在本体系下才是定值,所以这里需要转换坐标系。
下面推导坐标系变换的最后一步,同时加上坐标系角标来清楚地标明每个变量属于哪个坐标系下的描述。
w M ⃗ = w h ⃗ ˙ w M ⃗ = R b h ⃗ ˙ + w ω ⃗ × w h ⃗ R − 1 w M ⃗ = b h ⃗ ˙ + R − 1 w ω ⃗ × R − 1 w h ⃗ b M ⃗ = b h ⃗ ˙ + b ω ⃗ × b h ⃗ b M ⃗ = J b w ⃗ ˙ + b ω ⃗ × J b ω ⃗ \begin{aligned} {^w\vec M} =& {^w\dot{\vec h}} \\ {^w\vec M} =& R\ {^b\dot{\vec h}}+{^w\vec\omega}\times{^w\vec h} \\ R^{-1}\ {^w\vec M} =& {^b\dot{\vec h}} +R^{-1}\ {^w\vec\omega}\times R^{-1}\ {^w\vec h} \\ {^b\vec M} =& {^b\dot{\vec h}}+{^b\vec\omega}\times{^b\vec h} \\ {^b\vec M} =& J{^b\dot{\vec w}}+{^b\vec\omega}\times J\ {^b\vec\omega} \\ \end{aligned} wM=wM=R−1 wM=bM=bM=wh˙R bh˙+wω×whbh˙+R−1 wω×R−1 whbh˙+bω×bhJbw˙+bω×J bω
根据上面结果可以看出,姿态动力学方程 M ⃗ = J w ⃗ ˙ + w ⃗ × J w ⃗ \vec M=J\dot{\vec w}+\vec w\times J\vec w M=Jw˙+w×Jw 中的所有量,包括角速度向量,都在本体系下描述。
下面的推导来自文末参考资料[2],主要思路是所有变量都在世界系中处理。但我觉得这种方法不对。
推导1中得到转动惯量矩阵 J J J 后, J J J 在本体系下保持不变,但在世界系下是变量,可以记作
J ( t ) = ∑ i ∑ j J i j a ⃗ i ( t ) a ⃗ j ( t ) T J(t)=\sum_i\sum_j J_{ij}\vec a_i(t)\vec a_j(t)^\text T J(t)=i∑j∑Jijai(t)aj(t)T
其中 J i j J_{ij} Jij 是本体系下保持不变的转动惯量矩阵元素, a ⃗ i , a ⃗ j \vec a_i,\vec a_j ai,aj 表示本体系的坐标轴,例如当两个系重合时
a ⃗ 2 a ⃗ 3 T = [ 0 1 0 ] [ 0 0 1 ] = [ 0 0 0 0 0 1 0 0 0 ] \vec a_2\vec a_3^\text T= \begin{bmatrix} 0 \\ 1 \\ 0 \end{bmatrix} \begin{bmatrix} 0 & 0 & 1 \end{bmatrix} =\begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \end{bmatrix} a2a3T=
010
[001]=
000000010
但 a ⃗ i , a ⃗ j \vec a_i,\vec a_j ai,aj 在旋转时是个变量,所以
M = d H d t = d d t ( J ( t ) w ⃗ ( t ) ) = J ˙ ( t ) w ⃗ ( t ) + J ( t ) w ⃗ ˙ ( t ) = ∑ i ∑ j J i j ( a ⃗ ˙ i a ⃗ j T + a ⃗ i a ⃗ ˙ j T ) w ⃗ + J w ˙ = ∑ i ∑ j J i j [ ( w ⃗ × a ⃗ i ) a ⃗ j T w ⃗ + a ⃗ i ( w ⃗ × a ⃗ j ) T w ⃗ ] + J w ˙ = ∑ i ∑ j ( w ⃗ × J i j a ⃗ i ) a ⃗ j T w ⃗ + J w ˙ = w ⃗ × ( ∑ i ∑ j J i j a ⃗ i a ⃗ j T ) w ⃗ + J w ˙ = w ⃗ ( t ) × J ( t ) w ⃗ ( t ) + J ( t ) w ˙ ( t ) \begin{aligned} M =& \frac{\text dH}{\text dt} = \frac{\text d}{\text dt}(J(t)\vec w(t)) \\ =& \dot J(t)\vec w(t) + J(t)\dot{\vec w}(t) \\ =& \sum_i\sum_j J_{ij}(\dot{\vec a}_i\vec a_j^\text T +\vec a_i\dot{\vec a}_j^\text T)\vec w+J\dot w \\ =& \sum_i\sum_j J_{ij}\big[(\vec w\times\vec a_i)\vec a_j^\text T\vec w +\vec a_i(\vec w\times\vec a_j)^\text T\vec w\big]+J\dot w \\ =& \sum_i\sum_j (\vec w\times J_{ij}\vec a_i)\vec a_j^\text T\vec w+J\dot w \\ =& \vec w\times\left(\sum_i\sum_j J_{ij}\vec a_i\vec a_j^\text T\right)\vec w +J\dot w \\ =& \vec w(t)\times J(t)\vec w(t)+J(t)\dot w(t) \end{aligned} M=======dtdH=dtd(J(t)w(t))J˙(t)w(t)+J(t)w˙(t)i∑j∑Jij(a˙iajT+aia˙jT)w+Jw˙i∑j∑Jij[(w×ai)ajTw+ai(w×aj)Tw]+Jw˙i∑j∑(w×Jijai)ajTw+Jw˙w×(i∑j∑JijaiajT)w+Jw˙w(t)×J(t)w(t)+J(t)w˙(t)
其中 ( w ⃗ × a ⃗ j ) T w ⃗ = 0 (\vec w\times\vec a_j)^\text T\vec w=0 (w×aj)Tw=0。
这个例子描述了没有外部力矩输入时角速度却一直在变化的现象,这一现象在参考资料[3]的第13章中有详细描述。
设转动惯量矩阵 J J J、角速度初始值 w ⃗ 0 \vec w_0 w0 分别为
J = [ 2 0 0 0 2 0 0 0 1 ] , w ⃗ 0 = [ 1 0 1 ] J=\begin{bmatrix} 2 & 0 & 0 \\ 0 & 2 & 0 \\ 0 & 0 & 1 \\ \end{bmatrix},\ \vec w_0=\begin{bmatrix} 1 \\ 0 \\ 1 \end{bmatrix} J=
200020001
, w0=
101
当力矩为0时,由姿态动力学方程得
w ⃗ ˙ = − J − 1 ( w ⃗ × J w ⃗ ) = − [ 0.5 0 0 0 0.5 0 0 0 1 ] ( [ w x w y w z ] × [ 2 w x 2 w y w z ] ) = − [ 0.5 0 0 0 0.5 0 0 0 1 ] [ − w y w z w x w z 0 ] [ w ˙ x w ˙ y w ˙ z ] = [ 0.5 w y − 0.5 w x 0 ] \begin{aligned} \dot{\vec w} =& -J^{-1}(\vec w\times J\vec w) \\ =& -\begin{bmatrix} 0.5 & 0 & 0 \\ 0 & 0.5 & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} (\begin{bmatrix} w_x \\ w_y \\ w_z \end{bmatrix} \times\begin{bmatrix} 2w_x \\ 2w_y \\ w_z \end{bmatrix}) \\ =& -\begin{bmatrix} 0.5 & 0 & 0 \\ 0 & 0.5 & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} \begin{bmatrix} -w_yw_z \\ w_xw_z \\ 0 \end{bmatrix} \\ \begin{bmatrix} \dot w_x \\ \dot w_y \\ \dot w_z \end{bmatrix} =& \begin{bmatrix} 0.5w_y \\ -0.5w_x \\ 0 \end{bmatrix} \end{aligned} w˙===
w˙xw˙yw˙z
=−J−1(w×Jw)−
0.50000.50001
(
wxwywz
×
2wx2wywz
)−
0.50000.50001
−wywzwxwz0
0.5wy−0.5wx0
解得
w x ( t ) = cos 0.5 t w y ( t ) = − sin 0.5 t w z ( t ) = 1 \begin{aligned} w_x(t) =& \cos 0.5t \\ w_y(t) =& -\sin 0.5t \\ w_z(t) =& 1 \\ \end{aligned} wx(t)=wy(t)=wz(t)=cos0.5t−sin0.5t1
补充一个在推导过程中遇见的一个公式。当 A A A 是正交矩阵时,
A ( b ⃗ × c ⃗ ) = ( A b ⃗ ) × ( A c ⃗ ) A(\vec b\times\vec c)=(A\vec b)\times(A\vec c) A(b×c)=(Ab)×(Ac)
A A A 不是正交矩阵时暂时没发现什么有意思的性质。前面还有一个公式
A ( A − 1 b ⃗ × b ⃗ ) A(A^{-1}\vec b\times\vec b) A(A−1b×b)
也没找到能合并 A A A 和 A − 1 A^{-1} A−1 的方法。
证明:
( A b ⃗ ) × ( A c ⃗ ) = [ ( b x a ⃗ 1 ) + ( b y a ⃗ 2 ) + ( b z a ⃗ 3 ) ] × [ ( c x a ⃗ 1 ) + ( c y a ⃗ 2 ) + ( c z a ⃗ 3 ) ] = ( b x a ⃗ 1 ) × ( c y a ⃗ 2 ) + ( b x a ⃗ 1 ) × ( c z a ⃗ 3 ) + ( b y a ⃗ 2 ) × ( c x a ⃗ 1 ) + ( b y a ⃗ 2 ) × ( c z a ⃗ 3 ) + ( b z a ⃗ 3 ) × ( c x a ⃗ 1 ) + ( b z a ⃗ 3 ) × ( c y a ⃗ 2 ) = ( b x c y − b y c x ) a ⃗ 3 + ( b z c x − b x c z ) a ⃗ 2 + ( b y c z − b z c y ) a ⃗ 1 = A ( b ⃗ × c ⃗ ) \begin{aligned} (A\vec b)\times(A\vec c) =& [(b_x\vec a_1)+(b_y\vec a_2)+(b_z\vec a_3)]\times [(c_x\vec a_1)+(c_y\vec a_2)+(c_z\vec a_3)] \\ =& (b_x\vec a_1)\times(c_y\vec a_2) +(b_x\vec a_1)\times(c_z\vec a_3) +(b_y\vec a_2)\times(c_x\vec a_1) \\ &+(b_y\vec a_2)\times(c_z\vec a_3) +(b_z\vec a_3)\times(c_x\vec a_1) +(b_z\vec a_3)\times(c_y\vec a_2) \\ =& (b_xc_y-b_yc_x)\vec a_3 +(b_zc_x-b_xc_z)\vec a_2 +(b_yc_z-b_zc_y)\vec a_1 \\ =& A(\vec b\times\vec c) \end{aligned} (Ab)×(Ac)====[(bxa1)+(bya2)+(bza3)]×[(cxa1)+(cya2)+(cza3)](bxa1)×(cya2)+(bxa1)×(cza3)+(bya2)×(cxa1)+(bya2)×(cza3)+(bza3)×(cxa1)+(bza3)×(cya2)(bxcy−bycx)a3+(bzcx−bxcz)a2+(bycz−bzcy)a1A(b×c)
仿真一下前文提到的角速度向量在本体系下绕圈的现象,这一现象使用了本体极锥和空间极锥的概念(不知道是不是这么翻译),在参考资料[3]中有详细介绍。
仿真结果见视频 https://www.bilibili.com/video/BV1Ck4y1F7Ba/。视频中白色线条代表刚体,黄色和橙黄色分别代表本体系的z轴和xy轴,红色线条表示角速度向量,绿色线条表示角动量向量。视频前半段摄像头在世界系下,后半段跟随刚体转动。
基于 simucpp 和 raylib 的部分代码如下,完整代码见 https://gitee.com/xd15zhn/atttitudemotion。
/**************************************************************
// 验证刚体自由转动时的进动现象
simucpp版本:2.1.14
**************************************************************/
int main(void) {
Mat J(3, 3, vecdble{
2, 0, 0, 0, 2, 0, 0, 0, 1});
Mat Jinv = J.inv();
/* 初始化仿真器 */
Simulator sim1;
auto intQ = new MStateSpace(&sim1, BusSize(4, 1), true, "intQ");
auto intW = new MStateSpace(&sim1, BusSize(3, 1), true, "intW");
auto misoQ = new MFcnMISO(&sim1, BusSize(4, 1), "misoQ");
auto misoW = new MFcnMISO(&sim1, BusSize(3, 1), "misoW");
auto muxtau = new Mux(&sim1, BusSize(3, 1), "muxtau");
UInput **intau = new UInput*[3];
for (uint32_t i = 0; i < 3; i++) {
intau[i] = new UInput(&sim1, "inw"+to_string(i));
sim1.connectU(intau[i], muxtau, BusSize(i, 0));
}
sim1.connectM(intQ, misoQ);
sim1.connectM(intW, misoQ);
sim1.connectM(misoQ, intQ);
sim1.connectM(intW, misoW);
sim1.connectM(muxtau, misoW);
sim1.connectM(misoW, intW);
sim1.Set_SimStep(0.01);
intQ->Set_InitialValue(Mat(vecdble{
1, 0, 0, 0}));
intW->Set_InitialValue(Mat(vecdble{
1, 0, 1}));
intau[0]->Set_Function([](double t){
return 0; });
intau[1]->Set_Function([](double t){
return 0; });
intau[2]->Set_Function([](double t){
return 0; });
misoQ->Set_Function([](Mat *u){
// u[0]为姿态四元数(4x1),u[1]为本体系下的角速度向量(3x1)
Mat W(4, 4, vecdble{
// 角速度向量w1对应的四元数乘法左矩阵(4x4)
0, -u[1].at(0, 0), -u[1].at(1, 0), -u[1].at(2, 0),
u[1].at(0, 0), 0, u[1].at(2, 0), -u[1].at(1, 0),
u[1].at(1, 0), -u[1].at(2, 0), 0, u[1].at(0, 0),
u[1].at(2, 0), u[1].at(1, 0), -u[1].at(0, 0), 0,
});
return 0.5*W*u[0];
});
misoW->Set_Function([&J, &Jinv](Mat *u){
Vector3d omega = u[0]; // 本体系下的角速度向量(3x1)
Vector3d tau = Mat(3, 1, vecdble{
// 本体系下的力矩向量(3x1)
u[1].at(0, 0), u[1].at(1, 0), u[1].at(2, 0)
});
Vector3d ans = Jinv*(tau - (omega & (J*omega)));
return Mat(ans);
});
sim1.Initialize();
sim1.Simulate_FirstStep();
Mat curq, curw, curR, omegaI, hmom;
/*初始化场景*/
Camera camera;
SetWindowMonitor(1);
SetConfigFlags(FLAG_MSAA_4X_HINT);
SetConfigFlags(FLAG_FULLSCREEN_MODE);
SetTargetFPS(50);
InitGraph(0, 0, "Universe");
Init_Camera(&camera);
/*场景循环*/
while (!WindowShouldClose()) {
curq = intQ->Get_OutValue(); // 姿态四元数
curR = Quaternion_to_Rotation(curq); // 旋转矩阵
curw = intW->Get_OutValue(); // 本体系下的角速度向量
omegaI = curR * curw; // 世界系下的角速度向量
hmom = curR * (J*curw); // 世界系下的角动量
if (Shift_View())
SetReferenceFrame(Mat33_to_Matrix(curR));
else
SetReferenceFrame(MatrixIdentity());
Update_Camera(&camera);
BeginDrawing();
ClearBackground(BLACK);
BeginMode3D(camera);
DrawGrid(120, 2);
Draw_Body(curR); // 画刚体和固连坐标系
DrawLine3D(Vector3Zero(), Mat_to_Vector3(omegaI), RED); // 画世界系下的角速度向量
DrawLine3D(Vector3Zero(), Mat_to_Vector3(hmom), GREEN); // 画世界系下的角动量向量
EndMode3D();
EndDrawing();
if (IsKeyDown(KEY_Q)) {
sim1.Simulation_Reset();
}
if (Pause()) continue;
for (int i = 0; i < 5; i++)
sim1.Simulate_OneStep();
}
CloseGraph();
return 0;
}
文章浏览阅读325次。插头DP小结_dp插头模型
文章浏览阅读3.5k次。关于测试工作效率低的一些思考和改进方法引子 汇总统计了一下项目组近期测试项目实际工作量与基线工作量的对比,发现一个严重问题。就是工作效率特别低下。下面简单列举一下几个项目预期工作量和实际工作量以及时间耗费严重的地方、项目简要背景。 1、B版本测试。版本预期工作量15人天,实际耗费工作量在30人天。更为严重的是测试人员并没有因为测试周期延长和工作量投入加大而测试的更轻松,反而是测试期..._测试人员不足与改进
文章浏览阅读173次。级联样式表 CSS-难以成熟 (CSS — Difficult to maturation)Unlike software, the CSS specifications are developed by successive versions, which would allow a browser to refer to a particular version. CSS was devel..._级联样式表是哪年产生的
文章浏览阅读1.7k次。目录批处理语句1、批处理语句简介示例一:示例二:存储过程一、什么是存储过程1、存储过程的简介2、存储过程包含的内容3、存储过程的优点4、存储过程的分类系统存储过程:用户定义存储过程5、常用的系统储存过程(1)一般常用的存储过程(2)xp_cmdshell二、创建存储过程1、定义存储过程的语法2、不带参数的存储过程3、带参数..._sql的批处理
上次,我们解除了css的内外边距、鼠标悬停及其练习。现在我们学习css元素练习和定位。
文章浏览阅读2.2k次,点赞4次,收藏6次。本文提出了 WU-UCT, 一种新颖的并行 MCTS 算法, 通过监控未观察样本的数量来解决并行化过程中统计数据过时的问题. 基于新设计的统计数据, 它有正确地修正了UCT节点选择策略, 实现了有效的探索和利用的权衡. _树搜索并行化
文章浏览阅读673次。有没有师兄师姐有meganv2.04以上的版本小弟只有低版本的 需要高版本运行一下有偿!_megan v2.04
文章浏览阅读32次。springboot基于springboot的小型超市库存管理系统。springboot基于SpringBoot的校园失物招领系统。springboot基于springboot的残障人士社交平台。springboot基于springboot的酒店管理系统。springboot基于springboot的电商购物系统。springboot基于微信小程序的Sunmoon口红商城。springboot基于云平台的便民物流速递信息管理系统。springboot基于微服务的固定资产管理系统。
文章浏览阅读3.1k次。还在用PPT、Word和Excel画企业组织结构图吗?对于人力资源的同事来说,画组织结构图是一键非常头疼的事情,尤其是对于一些大公司和人员变动较大的公司来说,需要经常更换组织结构图,每次变动都要耗费大量的时间和精力去重新绘图。其实绘制织结构图很简单,之所以难是因为没有找对工具和方法!今天小编就教你如何用亿图图示轻松绘制一个既美观又专业的组织结构图!下图是一个简单的组织结构图例子,小编就以此为例,详细讲解一下好看清晰、实用的公司组织结构图是怎么画出来的。1、新建组织结构图2、创建组织结构_书本里印刷的结构图是用什么软件做的
文章浏览阅读1.8k次,点赞4次,收藏5次。BLE5.0 扩展蓝牙名称长度_蓝牙广播名称过长
文章浏览阅读3.5k次。centos8安装NVIDIA显卡驱动,docker模式运行机器学习_centos8安装显卡驱动
文章浏览阅读2.4k次。需求变更这件事,每个开发人员都遇到过,每个产品经理也都遇到过。 以前,我们会追求需求不变更,但无论是产品型团队还是项目型团队,需求不变更都是天方夜谈,不可能实现的。即使把需求变更的成本提得很高,流程搞得很复杂,又要填变更单,又要几级经理审批,又要需求评审,依然无法避免。 于是,团队的目标变成了少变更,希望尽量少的变更既能满足业务的需要,又能减少开发团队的反感。但‘少’是个相对的概念_需求优先级反复变化