刚体姿态运动学（二）旋转的微分形式——角速度、欧拉角速度、四元数导数、旋转矩阵导数

刚体姿态运动学（二）姿态的微分形式——角速度、欧拉角导数、四元数导数、旋转矩阵导数

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

旋转矩阵导数转角速度

$$\mathbf{r}(t)=\mathbf{R}(t){\mathbf{r}}_0$$

关于t求导：
$$\dot{\mathbf{r}}=\dot{\mathbf{R}}{\mathbf{r}}_0$$
又因：
$$\dot{\mathbf{r}}=\mathbf{\omega }\times \mathbf{r}$$
把叉乘转化为矩阵乘法，得到：
$$\begin{array}{c}\dot{\mathbf{R}}=\mathbf{\Omega }\mathbf{R}\\ \mathbf{\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}$$

欧拉角速度转角速度

假设选定RPY欧拉角，当完成了RPY三组旋转后，最后的yaw角速度，其实就等于绕z轴的角速度，因为此时的z轴就是求角速度时的z轴，即：
$${\omega }_z=\left[\begin{array}{c}0\\ 0\\ \frac{d\psi }{dt}\end{array}\right]$$
假设只完成了两组旋转，在求出pitch的角速度后，需要再进行一次yaw角的旋转，猜得到最后的绕y轴的角速度，即：
$${\omega }_y=R_{yaw}\cdot \left[\begin{array}{c}0\\ \frac{d\theta }{dt}\\ 0\end{array}\right]$$
同理可得：
$${\omega }_x=R_{pitch}\cdot R_{yaw}\cdot \left[\begin{array}{c}\frac{d\varphi }{dt}\\ 0\\ 0\end{array}\right]$$

三个量相加就得到最后的结果
$$\left(\begin{array}{l}wx\\ wy\\ wz\end{array}\right)=\left(\begin{array}{ccc}\cos \theta \ast \cos \phi & -\sin \phi & 0\\ \cos \theta \ast \sin \phi & \cos \phi & 0\\ -\sin \theta & 0& 1\end{array}\right)\left(\begin{array}{c}d\delta /dt\\ d\theta /dt\\ d\phi /dt\end{array}\right)$$
这里的角速度是相对于固定参考系的，相对于旋转参考系如下：
$$w^b=\left[\begin{array}{ccc}cr& 0& -sr\cdot cp\\ 0& 1& sp\\ sr& 0& cr\cdot cp\end{array}\right]\cdot \left[\begin{array}{c}\frac{dp}{dt}\\ \frac{dr}{dt}\\ \frac{dy}{dt}\end{array}\right]$$

四元数转角速度

四元数方程为
$$\mathrm{Q}=\cos \frac{\theta }{2}+{\mu }^R\sin \frac{\theta }{2}$$
${\mu }^R$为旋转轴。求导，得：
$$\frac{dQ}{dt}=-\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}{dt}$$
其中，旋转轴对时间的导数为0.两边同时左乘${\mu }^R$：
$$\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}$$
由四元数乘法，得：
$${\mu }^R\otimes {\mu }^R=-1$$
因此，
$$\frac{dQ}{dt}=\frac{\dot{\theta }}{2}{\mu }^R\otimes Q=\frac{1}{2}{\omega }_{Rb}^R\otimes Q$$
其中，${\omega }_{Rb}^R$为在全局坐标系下的角速度。如果要求得刚体在刚体坐标系下的角速度，需要进行转换，
$${\omega }_{Rb}^R=Q\otimes {\omega }_{Rb}^b\otimes Q^{\ast }$$
$$\frac{dQ}{dt}=\frac{1}{2}{\omega }_{Rb}^R\otimes Q=\frac{1}{2}Q\otimes {\omega }_{Rb}^b\otimes Q^{\ast }\otimes Q$$
$$\frac{dQ}{dt}=\frac{1}{2}Q\otimes {\omega }_{Rb}^b$$
转换为矩阵形式，为：
$$\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]$$
或为：
$$\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]$$