摘要 首先推导角速度公式和角加速度公式,并举一个例子说明角速度公式的细节,然后从加速度公式出发推导转动惯量矩阵 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;
}
文章浏览阅读1.3k次。kube operator简介kube operator是一个kubernetes集群部署及多集群管理工具,提供web ui支持在离线环境部署多个kubernetes集群。KubeOperator 是一个开源项目,通过 Web UI 在 VMware、OpenStack 和物理机上规划、部署和运营生产级别的 Kubernetes 集群。支持内网离线环境、支持 GPU、内置应用商店,已通过 CNCF 的 Kubernetes 软件一致性认证。官网:https://kubeoperator.io/离线包_kubeop
文章浏览阅读1.1w次,点赞10次,收藏81次。今天先介绍一下openmv4 H7的相关资料文章目录简介引脚电路图板子信息尺寸规格功耗温度范围简介openmv4 H7具有:STM32H743VI ARM Cortex M7 处理器,480 MHz ,1MB RAM,2 MB flash. 所有的 I/O 引脚输出 3.3V 并且 5V 耐受。并且还有以下的IO接口:1.全速 USB (12Mbs) 接口,连接到电脑。当插入Open..._openmv4 h7 cam
文章浏览阅读509次。前面我曾经发帖推荐过网上的一些做“图像处理和计算机视觉的”有料博客资源,原帖地址图像处理与机器视觉网络资源收罗——倾心大放送http://blog.csdn.net/baimafujinji/article/details/32332079做机器学习和数据挖掘方面的研究和开发,常会在线搜索一些资源,日积月累便挖出了一堆比较牛的博主,特别说明:做这个方向的
文章浏览阅读1.5w次,点赞14次,收藏92次。0x00 准备工具1、容量8G及以上的U盘2、kali linux镜像下载 - 下载地址:https://www.kali.org/downloads/3、镜像刻录软件Win32 Disk Imager - 下载地址:https://win32-disk-imager.en.lo4d.com/4、硬盘分区助手 - 下载地址:https://www.disktool.cn/5、制作引导工具..._kali系统和win可以共同吗
文章浏览阅读587次,点赞6次,收藏14次。因为媒体的宣传能够带给我们重要的信息资源,新闻发布和评论管理是国家管理机制重要的一环,,面对这一世界性的新动向和新问题,新闻发布如何适应新的时代和新的潮流,开展有效的信息服务工作,完成时代赋予的新使命?在新闻发布和评论管理系统开发之前所做的市场调研及其他的相关的管理系统,都是没有任何费用的,都是通过开发者自己的努力,所有的工作的都是自己亲力亲为,在碰到自己比较难以解决的问题,大多是通过同学和指导老师的帮助进行相关信息的解决,所以对于新闻发布和评论管理系统的开发在经济上是完全可行的,没有任何费用支出的。
文章浏览阅读1.7w次,点赞44次,收藏177次。python中set方法详解以及案例说明超详细_python set
为了通过HTTP方式在线访问HDFS上的文件,您可以利用WebHDFS REST API或者HttpFS Gateway这两种机制实现。1:httpfs是cloudera公司提供的一个hadoop hdfs的一个http接口,通过WebHDFS REST API 可以对hdfs进行读写等访问2:与WebHDFS的区别是不需要客户端可以访问hadoop集群的每一个节点,通过httpfs可以访问放置在防火墙后面的hadoop集群3:httpfs是一个Web应用,部署在内嵌的tomcat中。
文章浏览阅读236次。网站的快速加载是良好用户体验和搜索引擎优化的要素之一,但每当谷歌AdSense的广告代码部署到网站上时,速度都会显着下降,这是因为要下载大量文件才能显示广告,并且其中包含大量无用、被墙的请求及代码。_wordpress 怎么做谷歌优化
文章浏览阅读8.6k次。c# controls.add 控件的使用 10函数内部用下面代码增加控件:Button btn = new Button();btn.Location = new Point( 20, 20);btn.Size = new Size( 60,40);btn.Text = "btn'sText";this.Controls.Add( btn );问题:函数执行完后,bt_this.controls.add
文章浏览阅读329次。原理宿主APP安装在手机中的APP,并且通过该APP加载插件中的Activity插件APP没有安装的apk,通过宿主直接打开其内部Activity标准(协议)宿主APP和插件APP通信的桥梁。宿主APP通过一个空壳Activity(代理Activity)加载插件app中的Activity,实际上插件app中的Activity并没有入栈,也没法入栈,因为插件app没有安装,没有上下文和..._activity跳转 插件
文章浏览阅读1.0k次,点赞9次,收藏13次。输入格式:一行,一个整数,即图形的层数输出格式:如上述图形输入样例:3输出样例:AAAAAABBBAABCBAABBBAAAAAA//打印图形题关键是找规律,一般只需两重循环(行循环、列循环)#include<stdio.h>#include<string.h>int main() { int i, n; char ..._c语言pta怎么使用
文章浏览阅读3.3w次,点赞9次,收藏25次。刚开始学习docker,发现下载镜像非常的慢。如果不经过,docker的镜像下载都来源于国外,因此需要配置国内的镜像源。Docker中国区官方镜像。_docker国内镜像源