【刚体姿态运动学】角速度和欧拉角速率的换算关系的详细推导

0 引言

本文以一种新的角度推导刚体姿态运动学，也即角速度和欧拉角速率之间的换算，不同于相似博文的地方在于，本文旨在从原理上给出直观清晰生动的解释。将详细过程记录于此，便于后续学习科研查找需要。

1 符号

符号	含义
{ E } \{E\} {E}	地面坐标系（惯性坐标系，牛顿运动定律严格成立）
{ B } \{B\} {B}	随体坐标系（固连在刚体上，且原点位于质心）
$\Phi =[\varphi ,\theta ,\psi {]}^T$	姿态角，ZYX欧拉角，分别为roll, pitch, yaw
$R_X(\varphi ),R_Y(\theta ),R_z(\psi )$	随体坐标系绕地面坐标系X/Y/Z轴旋转$\varphi /\theta /\psi$角度得到的旋转矩阵
${}_B^{E}R$	旋转矩阵，随体坐标系姿态在地面坐标系下的表达
$c,s$	$c$表示$cos$，$s$表示$sin$
${\omega }_b=[{\omega }_{bx},{\omega }_{by},{\omega }_{bz}{]}^T$	刚体相对地面坐标系转动的角速度在 { B } \{B\} {B}系下的表达

2 欧拉角与旋转矩阵

这里我们使用ZYX欧拉角来表达姿态，那么有：
$$\begin{array}{c}\begin{array}{rl}{}_B^{E}R& =R_Z(\psi )R_Y(\theta )R_X(\varphi )\\ & =\left[\begin{array}{ccc}c\psi & -s\psi & 0\\ s\psi & c\psi & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}c\theta & 0& s\theta \\ 0& 1& 0\\ -s\theta & 0& c\theta \end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& c\varphi & -s\varphi \\ 0& s\varphi & c\varphi \end{array}\right]\\ & =\left[\begin{array}{ccc}c\psi c\theta & s\theta s\varphi c\psi -c\varphi s\psi & s\theta c\varphi c\psi +s\psi s\varphi \\ s\psi c\theta & s\theta s\varphi s\psi +c\varphi c\psi & s\theta c\varphi s\psi -c\psi s\varphi \\ -s\theta & c\theta s\varphi & c\theta c\varphi \end{array}\right]\end{array}\end{array}$$
上述旋转矩阵用欧拉角给出，可以理解为：随体坐标系（1）先绕自身的${\hat{Z}}_B$轴旋转$\psi$角度，（2）再绕${\hat{Y}}_B$轴旋转$\theta$角度，（3）最后绕${\hat{X}}_B$轴旋转$\varphi$角度。

欧拉角和固定角的区别为：

• 欧拉角：刚体绕运动轴旋转的角度（内旋Intrinsic rotations）
• 固定角：刚体绕固定轴旋转的角度（外旋 Extrinsic rotations）

3 机体下的角速度表达与欧拉角的关系

姿态角速率$\dot{\Phi }$和机体角速度${\omega }_b$之间的转换关系为：

该公式不便于记忆，但是需要知道如何推导，并且最重要的是理解其原理，关键的时候查找即可。我几乎把高赞和高收藏的博客都看了一遍，但是都没能理解作者的意思，写的也有一定模糊性，后来还是自己琢磨才明白的，于是将自己能够理解的推导过程记录如下。

4 推导

假设当前姿态角为$\Phi =[\varphi ,\theta ,\psi {]}^T$，为了使角速度的表达更直观，这里用${\omega }_b=[{\omega }_{bx},{\omega }_{by},{\omega }_{bz}{]}^T$代替上图所示的${\omega }_b=[p,q,r{]}^T$，那么:
由偏航角速率$\dot{\psi }$引起的角速度在最终的 { B } \{B\} {B}系下可以表达为：
$${\left[\begin{array}{c}{\omega }_{bx}\\ {\omega }_{by}\\ {\omega }_{bz}\end{array}\right]}_{\dot{\psi }}={}_{B_z}^{B_{z,y,x}}R\left[\begin{array}{c}0\\ 0\\ \dot{\psi }\end{array}\right]$$
其中，
$$\begin{array}{rl}{}_{B_z}^{B_{z,y,x}}R& =R_X^T(\varphi )R_Y^T(\theta )\\ {}_{B_{z,y}}^{B_{z,y,x}}R& =R_X^T(\varphi )\\ {}_{B_z}^{B_{z,y}}R& =R_Y^T(\theta )\end{array}$$

为了便于理解，我特地画了一个示意图，如上图所示。
这里假设物体有一个预设的姿态角（本文与其他文章最大的不同）：

• 第一幅图只有绕绿色$z$轴的$\psi$运动，为了与后面的情况作区分，这里 { B z } \{B_z\} {Bz​}用下标 ${}_z$ 表示第一步绕机体$z$轴的运动时的随体坐标系，${\omega }_{bz}=\dot{\psi }$。
• 第二幅图，在第一幅图的基础上，俯仰角$\theta$不为0，虽然引入了$\theta$，但是却没有绕$y$轴的运动，也即此时仍然只有$\psi$变化，请大家想象第二幅图下方的圆柱，在倾斜的情况下，仍然绕“竖直”方向转动，那么显然，在这个时候的随体坐标系 { B z , y } \{B_{z,y}\} {Bz,y​}下，出现了角速度的$x$轴分量，而不仅仅有$z$轴分量，也即角速度（注意橙色的$\dot{\varphi }$箭头）在新的坐标系下的表达。
• 同理，第三幅图，引入了$\varphi$角，但没有绕$x$轴的运动（$\dot{\varphi }=0$），此时的机体角速度就是$\dot{\psi }$角速度（注意橙色的$\dot{\varphi }$箭头）在姿态角$\Phi$表示的坐标系下的表达。

类似的，如果$\dot{\theta }$不为0，则由俯仰角速率$\dot{\theta }$引起的角速度在最终的 { B } \{B\} {B}系下可以表达为：
$${\left[\begin{array}{c}{\omega }_{bx}\\ {\omega }_{by}\\ {\omega }_{bz}\end{array}\right]}_{\dot{\theta }}={}_{B_{z,y}}^{B_{z,y,x}}R\left[\begin{array}{c}0\\ \dot{\theta }\\ 0\end{array}\right]$$

由滚转角速率$\dot{\varphi }$引起的角速度在最终的 { B } \{B\} {B}系下就更直接了，就是：
$${\left[\begin{array}{c}{\omega }_{bx}\\ {\omega }_{by}\\ {\omega }_{bz}\end{array}\right]}_{\dot{\varphi }}=\left[\begin{array}{c}\dot{\varphi }\\ 0\\ 0\end{array}\right]$$

以上三个成分相加：
$$\begin{array}{rl}\left[\begin{array}{c}{\omega }_{bx}\\ {\omega }_{by}\\ {\omega }_{bz}\end{array}\right]& ={\left[\begin{array}{c}{\omega }_{bx}\\ {\omega }_{by}\\ {\omega }_{bz}\end{array}\right]}_{\dot{\psi }}+{\left[\begin{array}{c}{\omega }_{bx}\\ {\omega }_{by}\\ {\omega }_{bz}\end{array}\right]}_{\dot{\theta }}+{\left[\begin{array}{c}{\omega }_{bx}\\ {\omega }_{by}\\ {\omega }_{bz}\end{array}\right]}_{\dot{\varphi }}\\ & =R_X^T(\varphi )R_Y^T(\theta )\left[\begin{array}{c}0\\ 0\\ \dot{\psi }\end{array}\right]+R_X^T(\varphi )\left[\begin{array}{c}0\\ \dot{\theta }\\ 0\end{array}\right]+\left[\begin{array}{c}\dot{\varphi }\\ 0\\ 0\end{array}\right]\\ & =\left[\begin{array}{ccc}c\theta & 0& -s\theta \\ s\theta s\varphi & c\varphi & c\theta s\varphi \\ s\theta c\varphi & -s\varphi & c\theta c\varphi \end{array}\right]\left[\begin{array}{c}0\\ 0\\ \dot{\psi }\end{array}\right]+\left[\begin{array}{ccc}1& 0& 0\\ 0& c\varphi & s\varphi \\ 0& -s\varphi & c\varphi \end{array}\right]\left[\begin{array}{c}0\\ \dot{\theta }\\ 0\end{array}\right]+\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}\dot{\varphi }\\ 0\\ 0\end{array}\right]\\ & =\left[\begin{array}{ccc}1& 0& -s\theta \\ 0& c\varphi & c\theta s\varphi \\ 0& -s\varphi & c\theta c\varphi \end{array}\right]\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]\end{array}$$
也即：
$$\left[\begin{array}{c}{\omega }_{bx}\\ {\omega }_{by}\\ {\omega }_{bz}\end{array}\right]=\left[\begin{array}{ccc}1& 0& -\text{sin}\theta \\ 0& \text{cos}\varphi & \text{cos}\theta \text{sin}\varphi \\ 0& -\text{sin}\varphi & \text{cos}\theta \text{cos}\varphi \end{array}\right]\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]$$
上述矩阵的逆，由matlab代码求出：

syms theta phi real
A=[1,0,-sin(theta);0,cos(phi),cos(theta)*sin(phi);0,-sin(phi),cos(theta)*cos(phi)];
A_inv = simplify(inv(A))

也即：
$$\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{ccc}1& \text{sin}\varphi \text{tan}\theta & \text{cos}\varphi \text{tan}\theta \\ 0& \text{cos}\varphi & -\text{sin}\varphi \\ 0& \text{sin}\varphi /\text{cos}\theta & \text{cos}\varphi /\text{cos}\theta \end{array}\right]\left[\begin{array}{c}{\omega }_{bx}\\ {\omega }_{by}\\ {\omega }_{bz}\end{array}\right]$$

参考

刚体姿态运动学（二）旋转的微分形式——角速度、欧拉角速度、四元数导数、旋转矩阵导数
控制笔记
姿态角速度和机体角速度，横摆角速度（Yaw Rate）估算
欧拉角速度和机体角速度