上一篇我们讲了姿态的表达方式及其转换，可以说还是比较简单的。接下来面临的问题是，我们不仅想知道刚体的姿态，还想知道姿态是怎么变化的，也就是刚体的角速度。接下来我们研究一下姿态的各种表达的微分形式该如何转化为角速度。角速度是描述刚体姿态变化的速度，由三个方向的分量 w x w_{x} wx​, w y w_{y} wy​ , w z ,w_{z} ,wz​组成。

r ( t ) = R ( t ) r 0 \mathbf{r}(t)=\mathbf{R}(t) \mathbf{r}_{0} r(t)=R(t)r0​

关于t求导： r ˙ = R ˙ r 0 \dot{\mathbf{r}}=\dot{\mathbf{R}} \mathbf{r}_{0} r˙=R˙r0​ 又因： r ˙ = ω × r \dot{\mathbf{r}}=\boldsymbol{\omega} \times \mathbf{r} r˙=ω×r 把叉乘转化为矩阵乘法，得到： R ˙ = Ω R Ω = ( 0 − ω z ω y ω z 0 − ω x − ω y ω x 0 ) \begin{array}{c} \dot{\mathbf{R}}=\mathbf{\Omega R} \\ \boldsymbol{\Omega}=\left(\begin{array}{ccc} 0 & -\omega_{z} & \omega_{y} \\ \omega_{z} & 0 & -\omega_{x} \\ -\omega_{y} & \omega_{x} & 0 \end{array}\right) \end{array} R˙=ΩRΩ=⎝⎛​0ωz​−ωy​​−ωz​0ωx​​ωy​−ωx​0​⎠⎞​​

假设选定RPY欧拉角，当完成了RPY三组旋转后，最后的yaw角速度，其实就等于绕z轴的角速度，因为此时的z轴就是求角速度时的z轴，即： ω z = [ 0 0 d ψ d t ] \omega_{z}=\left[\begin{array}{c} 0 \\ 0 \\ \frac{d \psi}{dt} \end{array}\right] ωz​=⎣⎡​00dtdψ​​⎦⎤​ 假设只完成了两组旋转，在求出pitch的角速度后，需要再进行一次yaw角的旋转，猜得到最后的绕y轴的角速度，即： ω y = R y a w ⋅ [ 0 d θ d t 0 ] \omega_{y}=R_{yaw} \cdot\left[\begin{array}{c} 0 \\ \frac{d \theta}{d t} \\ 0 \end{array}\right] ωy​=Ryaw​⋅⎣⎡​0dtdθ​0​⎦⎤​ 同理可得： ω x = R p i t c h ⋅ R y a w ⋅ [ d ϕ d t 0 0 ] \omega_{x}=R_{pitch} \cdot R_{yaw} \cdot\left[\begin{array}{c} \frac{d \phi}{d t} \\ 0\\ 0 \end{array}\right] ωx​=Rpitch​⋅Ryaw​⋅⎣⎡​dtdϕ​00​⎦⎤​

三个量相加就得到最后的结果 ( w x w y w z ) = ( cos ⁡ θ ∗ cos ⁡ φ − sin ⁡ φ 0 cos ⁡ θ ∗ sin ⁡ φ cos ⁡ φ 0 − sin ⁡ θ 0 1 ) ( d δ / d t d θ / d t d φ / d t ) \left(\begin{array}{l} w x \\ w y \\ w z \end{array}\right)=\left(\begin{array}{ccc} \cos \theta * \cos \varphi & -\sin \varphi & 0 \\ \cos \theta * \sin \varphi & \cos \varphi & 0 \\ -\sin \theta & 0 & 1 \end{array}\right)\left(\begin{array}{c} d \delta / d t \\ d \theta / d t \\ d \varphi / d t \end{array}\right) ⎝⎛​wxwywz​⎠⎞​=⎝⎛​cosθ∗cosφcosθ∗sinφ−sinθ​−sinφcosφ0​001​⎠⎞​⎝⎛​dδ/dtdθ/dtdφ/dt​⎠⎞​ 这里的角速度是相对于固定参考系的，相对于旋转参考系如下： w b = [ c r 0 − s r ⋅ c p 0 1 s p s r 0 c r ⋅ c p ] ⋅ [ d p d t d r d t d y d t ] w^{b}= \left[\begin{array}{ccc} c r & 0 & -s r \cdot c p \\ 0 & 1 & s p \\ s r & 0 & c r \cdot c p \end{array}\right] \cdot\left[\begin{array}{c} \frac{d p}{d t} \\ \frac{d r}{d t} \\ \frac{d y}{d t} \end{array}\right] wb=⎣⎡​cr0sr​010​−sr⋅cpspcr⋅cp​⎦⎤​⋅⎣⎡​dtdp​dtdr​dtdy​​⎦⎤​

四元数方程为 Q = cos ⁡ θ 2 + μ R sin ⁡ θ 2 \mathrm{Q}=\cos \frac{\theta}{2}+\mu^{R} \sin \frac{\theta}{2} Q=cos2θ​+μRsin2θ​ μ R \mu^{R} μR为旋转轴。求导，得： d Q d t = − θ ˙ 2 sin ⁡ θ 2 + μ R θ ˙ 2 cos ⁡ θ 2 + sin ⁡ θ 2 d μ R d t \frac{d Q}{d t}=-\frac{\dot{\theta}}{2} \sin \frac{\theta}{2}+\mu^{R} \frac{\dot{\theta}}{2} \cos \frac{\theta}{2}+\sin \frac{\theta}{2} \frac{d \mu^{R}}{d t} dtdQ​=−2θ˙​sin2θ​+μR2θ˙​cos2θ​+sin2θ​dtdμR​ 其中，旋转轴对时间的导数为0.两边同时左乘 μ R \mu^{R} μR： θ ˙ 2 μ R ⊗ Q = θ ˙ 2 μ R ⊗ ( cos ⁡ θ 2 + μ R sin ⁡ θ 2 ) = θ ˙ 2 cos ⁡ θ 2 μ R + μ R ⊗ μ R θ ˙ 2 sin ⁡ θ 2 \frac{\dot{\theta}}{2} \mu^{R} \otimes Q=\frac{\dot{\theta}}{2} \mu^{R} \otimes\left(\cos \frac{\theta}{2}+\mu^{R} \sin \frac{\theta}{2}\right)=\frac{\dot{\theta}}{2} \cos \frac{\theta}{2} \mu^{R}+\mu^{R} \otimes \mu^{R} \frac{\dot{\theta}}{2} \sin \frac{\theta}{2} 2θ˙​μR⊗Q=2θ˙​μR⊗(cos2θ​+μRsin2θ​)=2θ˙​cos2θ​μR+μR⊗μR2θ˙​sin2θ​ 由四元数乘法，得： μ R ⊗ μ R = − 1 \mu^{R} \otimes \mu^{R}=-1 μR⊗μR=−1 因此， d Q d t = θ ˙ 2 μ R ⊗ Q = 1 2 ω R b R ⊗ Q \frac{d Q}{d t}=\frac{\dot{\theta}}{2} \mu^{R} \otimes Q=\frac{1}{2} \omega_{R b}^{R} \otimes Q dtdQ​=2θ˙​μR⊗Q=21​ωRbR​⊗Q 其中， ω R b R \omega_{R b}^{R} ωRbR​为在全局坐标系下的角速度。如果要求得刚体在刚体坐标系下的角速度，需要进行转换， ω R b R = Q ⊗ ω R b b ⊗ Q ∗ \omega_{R b}^{R}=Q \otimes \omega_{R b}^{b} \otimes Q^{*} ωRbR​=Q⊗ωRbb​⊗Q∗ d Q d t = 1 2 ω R b R ⊗ Q = 1 2 Q ⊗ ω R b b ⊗ Q ∗ ⊗ Q \frac{d Q}{d t}=\frac{1}{2} \omega_{R b}^{R} \otimes Q=\frac{1}{2} Q \otimes \omega_{R b}^{b} \otimes Q^{*} \otimes Q dtdQ​=21​ωRbR​⊗Q=21​Q⊗ωRbb​⊗Q∗⊗Q d Q d t = 1 2 Q ⊗ ω R b b \frac{d Q}{d t}=\frac{1}{2} Q \otimes \omega_{R b}^{b} dtdQ​=21​Q⊗ωRbb​ 转换为矩阵形式，为： [ q ˙ 0 q ˙ 1 q ˙ 2 q ˙ 3 ] = 1 2 [ 0 − ω x − ω y − ω z ω x 0 ω z − ω y ω y − ω z 0 ω x ω z ω y − ω x 0 ] [ q 0 q 1 q 2 q 3 ] \left[\begin{array}{c} \dot{q}_{0} \\ \dot{q}_{1} \\ \dot{q}_{2} \\ \dot{q}_{3} \end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc} 0 & -\omega_{x} & -\omega_{y} & -\omega_{z} \\ \omega_{x} & 0 & \omega_{z} & -\omega_{y} \\ \omega_{y} & -\omega_{z} & 0 & \omega_{x} \\ \omega_{z} & \omega_{y} & -\omega_{x} & 0 \end{array}\right]\left[\begin{array}{l} q_{0} \\ q_{1} \\ q_{2} \\ q_{3} \end{array}\right] ⎣⎢⎢⎡​q˙​0​q˙​1​q˙​2​q˙​3​​⎦⎥⎥⎤​=21​⎣⎢⎢⎡​0ωx​ωy​ωz​​−ωx​0−ωz​ωy​​−ωy​ωz​0−ωx​​−ωz​−ωy​ωx​0​⎦⎥⎥⎤​⎣⎢⎢⎡​q0​q1​q2​q3​​⎦⎥⎥⎤​ 或为： [ q ˙ 0 q ˙ 1 q ˙ 2 q ˙ 3 ] = 1 2 [ q 0 − q 1 − q 2 − q 3 q 1 q 0 − q 3 q 2 q 2 q 3 q 0 − q 1 q 3 − q 2 q 1 q 0 ] [ 0 ω x ω y ω z ] \left[\begin{array}{c} \dot{q}_{0} \\ \dot{q}_{1} \\ \dot{q}_{2} \\ \dot{q}_{3} \end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc} q_{0} & -q_{1} & -q_{2} & -q_{3} \\ q_{1} & q_{0} & -q_{3} & q_{2} \\ q_{2} & q_{3} & q_{0} & -q_{1} \\ q_{3} & -q_{2} & q_{1} & q_{0} \end{array}\right]\left[\begin{array}{c} 0 \\ \omega_{x} \\ \omega_{y} \\ \omega_{z} \end{array}\right] ⎣⎢⎢⎡​q˙​0​q˙​1​q˙​2​q˙​3​​⎦⎥⎥⎤​=21​⎣⎢⎢⎡​q0​q1​q2​q3​​−q1​q0​q3​−q2​​−q2​−q3​q0​q1​​−q3​q2​−q1​q0​​⎦⎥⎥⎤​⎣⎢⎢⎡​0ωx​ωy​ωz​​⎦⎥⎥⎤​