0 引言
本文以一种新的角度推导刚体姿态运动学，也即角速度和欧拉角速率之间的换算，不同于相似博文的地方在于，本文旨在从原理上给出直观清晰生动的解释。将详细过程记录于此，便于后续学习科研查找需要。 主要参考书目：机器人学导论（第四版）——John J.Craig著，贠超 王伟译
1 符号
符号含义
{
E
}
\{E\}
{E}地面坐标系（惯性坐标系，牛顿运动定律严格成立）
{
B
}
\{B\}
{B}随体坐标系（固连在刚体上，且原点位于质心）
Φ
=
[
ϕ
,
θ
,
ψ
]
T
\Phi=[\phi,\theta,\psi]^T
Φ=[ϕ,θ,ψ]T姿态角，ZYX欧拉角，分别为roll, pitch, yaw
R
X
(
ϕ
)
,
R
Y
(
θ
)
,
R
z
(
ψ
)
R_X(\phi),R_Y(\theta),R_z(\psi)
RX(ϕ),RY(θ),Rz(ψ)随体坐标系绕地面坐标系X/Y/Z轴旋转
ϕ
/
θ
/
ψ
\phi/\theta/\psi
ϕ/θ/ψ角度得到的旋转矩阵
B
E
R
^E_BR
BER旋转矩阵，随体坐标系姿态在地面坐标系下的表达
c
,
s
c,s
c,s
c
c
c表示
c
o
s
cos
cos，
s
s
s表示
s
i
n
sin
sin
ω
b
=
[
ω
b
x
,
ω
b
y
,
ω
b
z
]
T
{\omega_b}=[{\omega_{bx}},{\omega_{by}},{\omega_{bz}}]^T
ωb=[ωbx,ωby,ωbz]T刚体相对地面坐标系转动的角速度在
{
B
}
\{B\}
{B}系下的表达
2 欧拉角与旋转矩阵
这里我们使用ZYX欧拉角来表达姿态，那么有：
B
E
R
=
R
Z
(
ψ
)
R
Y
(
θ
)
R
X
(
ϕ
)
=
[
c
ψ
−
s
ψ
0
s
ψ
c
ψ
0
0
0
1
]
[
c
θ
0
s
θ
0
1
0
−
s
θ
0
c
θ
]
[
1
0
0
0
c
ϕ
−
s
ϕ
0
s
ϕ
c
ϕ
]
=
[
c
ψ
c
θ
s
θ
s
ϕ
c
ψ
−
c
ϕ
s
ψ
s
θ
c
ϕ
c
ψ
+
s
ψ
s
ϕ
s
ψ
c
θ
s
θ
s
ϕ
s
ψ
+
c
ϕ
c
ψ
s
θ
c
ϕ
s
ψ
−
c
ψ
s
ϕ
−
s
θ
c
θ
s
ϕ
c
θ
c
ϕ
]
\begin{equation} \begin{split} ^E_BR&=R_Z(\psi)R_Y(\theta)R_X(\phi)\\ &= \begin{bmatrix} c\psi & -s\psi & 0\\ s\psi & c\psi & 0\\ 0&0&1 \end{bmatrix} \begin{bmatrix} c\theta & 0 & s\theta\\ 0 & 1 & 0\\ -s\theta&0&c\theta \end{bmatrix} \begin{bmatrix} 1 & 0 & 0\\ 0 & c\phi & -s\phi \\ 0 & s\phi &c\phi \end{bmatrix}\\ &=\begin{bmatrix} c\psi c\theta & s\theta s\phi c\psi-c\phi s\psi & s\theta c\phi c\psi + s\psi s\phi\\ s\psi c\theta & s\theta s\phi s\psi+c\phi c\psi & s\theta c\phi s\psi - c\psi s\phi\\ -s\theta & c\theta s\phi & c\theta c\phi \end{bmatrix} \end{split} \end{equation}
BER=RZ(ψ)RY(θ)RX(ϕ)=
cψsψ0−sψcψ0001
cθ0−sθ010sθ0cθ
1000cϕsϕ0−sϕcϕ
=
cψcθsψcθ−sθsθsϕcψ−cϕsψsθsϕsψ+cϕcψcθsϕsθcϕcψ+sψsϕsθcϕsψ−cψsϕcθcϕ
上述旋转矩阵用欧拉角给出，可以理解为：随体坐标系（1）先绕自身的
Z
^
B
\hat Z_B
Z^B轴旋转
ψ
\psi
ψ角度，（2）再绕
Y
^
B
\hat Y_B
Y^B轴旋转
θ
\theta
θ角度，（3）最后绕
X
^
B
\hat X_B
X^B轴旋转
ϕ
\phi
ϕ角度。
欧拉角和固定角的区别为：
欧拉角：刚体绕运动轴旋转的角度（内旋Intrinsic rotations）固定角：刚体绕固定轴旋转的角度（外旋 Extrinsic rotations）
3 机体下的角速度表达与欧拉角的关系
姿态角速率
Φ
˙
\dot \Phi
Φ˙和机体角速度
ω
b
\omega_b
ωb之间的转换关系为：
该公式不便于记忆，但是需要知道如何推导，并且最重要的是理解其原理，关键的时候查找即可。我几乎把高赞和高收藏的博客都看了一遍，但是都没能理解作者的意思，写的也有一定模糊性，后来还是自己琢磨才明白的，于是将自己能够理解的推导过程记录如下。
4 推导
假设当前姿态角为
Φ
=
[
ϕ
,
θ
,
ψ
]
T
\Phi=[\phi,\theta,\psi]^T
Φ=[ϕ,θ,ψ]T，为了使角速度的表达更直观，这里用
ω
b
=
[
ω
b
x
,
ω
b
y
,
ω
b
z
]
T
{\omega_b}=[{\omega_{bx}},{\omega_{by}},{\omega_{bz}}]^T
ωb=[ωbx,ωby,ωbz]T代替上图所示的
ω
b
=
[
p
,
q
,
r
]
T
{\omega_b}=[p,q,r]^T
ωb=[p,q,r]T，那么: 由偏航角速率
ψ
˙
\dot \psi
ψ˙引起的角速度在最终的
{
B
}
\{B\}
{B}系下可以表达为：
[
ω
b
x
ω
b
y
ω
b
z
]
ψ
˙
=
B
z
B
z
,
y
,
x
R
[
0
0
ψ
˙
]
\begin{bmatrix}\omega_{bx}\\ \omega_{by}\\ \omega_{bz}\end{bmatrix}_{\dot\psi}= {^{B_{z,y,x}}_{B_{z}}R}\begin{bmatrix}0\\0\\\dot\psi\end{bmatrix}
ωbxωbyωbz
ψ˙=BzBz,y,xR
00ψ˙
其中，
B
z
B
z
,
y
,
x
R
=
R
X
T
(
ϕ
)
R
Y
T
(
θ
)
B
z
,
y
B
z
,
y
,
x
R
=
R
X
T
(
ϕ
)
B
z
B
z
,
y
R
=
R
Y
T
(
θ
)
\begin{split} ^{B_{z,y,x}}_{B_{z}}R&=R^T_X(\phi)R^T_Y(\theta)\\ {{^{B_{z,y,x}}_{B_{z,y}}}R}&=R^T_X(\phi)\\ {{^{B_{z,y}}_{B_{z}}}R}&=R^T_Y(\theta)\\ \end{split}
BzBz,y,xRBz,yBz,y,xRBzBz,yR=RXT(ϕ)RYT(θ)=RXT(ϕ)=RYT(θ) 为了便于理解，我特地画了一个示意图，如上图所示。 这里假设物体有一个预设的姿态角（本文与其他文章最大的不同）：
第一幅图只有绕绿色
z
z
z轴的
ψ
\psi
ψ运动，为了与后面的情况作区分，这里
{
B
z
}
\{B_z\}
{Bz}用下标
z
_z
z 表示第一步绕机体
z
z
z轴的运动时的随体坐标系，
ω
b
z
=
ψ
˙
\omega_{bz}=\dot \psi
ωbz=ψ˙。第二幅图，在第一幅图的基础上，俯仰角
θ
\theta
θ不为0，虽然引入了
θ
\theta
θ，但是却没有绕
y
y
y轴的运动，也即此时仍然只有
ψ
\psi
ψ变化，请大家想象第二幅图下方的圆柱，在倾斜的情况下，仍然绕“竖直”方向转动，那么显然，在这个时候的随体坐标系
{
B
z
,
y
}
\{B_{z,y}\}
{Bz,y}下，出现了角速度的
x
x
x轴分量，而不仅仅有
z
z
z轴分量，也即角速度（注意橙色的
ϕ
˙
\dot\phi
ϕ˙箭头）在新的坐标系下的表达。同理，第三幅图，引入了
ϕ
\phi
ϕ角，但没有绕
x
x
x轴的运动（
ϕ
˙
=
0
\dot \phi=0
ϕ˙=0），此时的机体角速度就是
ψ
˙
\dot \psi
ψ˙角速度（注意橙色的
ϕ
˙
\dot\phi
ϕ˙箭头）在姿态角
Φ
\Phi
Φ表示的坐标系下的表达。
类似的，如果
θ
˙
\dot \theta
θ˙不为0，则由俯仰角速率
θ
˙
\dot \theta
θ˙引起的角速度在最终的
{
B
}
\{B\}
{B}系下可以表达为：
[
ω
b
x
ω
b
y
ω
b
z
]
θ
˙
=
B
z
,
y
B
z
,
y
,
x
R
[
0
θ
˙
0
]
\begin{bmatrix}\omega_{bx}\\ \omega_{by}\\ \omega_{bz}\end{bmatrix}_{\dot\theta}= {^{B_{z,y,x}}_{B_{z,y}}R}\begin{bmatrix}0\\\dot\theta\\0\end{bmatrix}
ωbxωbyωbz
θ˙=Bz,yBz,y,xR
0θ˙0
由滚转角速率
ϕ
˙
\dot \phi
ϕ˙引起的角速度在最终的
{
B
}
\{B\}
{B}系下就更直接了，就是：
[
ω
b
x
ω
b
y
ω
b
z
]
ϕ
˙
=
[
ϕ
˙
0
0
]
\begin{bmatrix}\omega_{bx}\\ \omega_{by}\\ \omega_{bz}\end{bmatrix}_{\dot\phi}= \begin{bmatrix}\dot\phi\\0\\0\end{bmatrix}
ωbxωbyωbz
ϕ˙=
ϕ˙00
以上三个成分相加：
[
ω
b
x
ω
b
y
ω
b
z
]
=
[
ω
b
x
ω
b
y
ω
b
z
]
ψ
˙
+
[
ω
b
x
ω
b
y
ω
b
z
]
θ
˙
+
[
ω
b
x
ω
b
y
ω
b
z
]
ϕ
˙
=
R
X
T
(
ϕ
)
R
Y
T
(
θ
)
[
0
0
ψ
˙
]
+
R
X
T
(
ϕ
)
[
0
θ
˙
0
]
+
[
ϕ
˙
0
0
]
=
[
c
θ
0
−
s
θ
s
θ
s
ϕ
c
ϕ
c
θ
s
ϕ
s
θ
c
ϕ
−
s
ϕ
c
θ
c
ϕ
]
[
0
0
ψ
˙
]
+
[
1
0
0
0
c
ϕ
s
ϕ
0
−
s
ϕ
c
ϕ
]
[
0
θ
˙
0
]
+
[
1
0
0
0
1
0
0
0
1
]
[
ϕ
˙
0
0
]
=
[
1
0
−
s
θ
0
c
ϕ
c
θ
s
ϕ
0
−
s
ϕ
c
θ
c
ϕ
]
[
ϕ
˙
θ
˙
ψ
˙
]
\begin{split} \begin{bmatrix}\omega_{bx}\\ \omega_{by}\\ \omega_{bz}\end{bmatrix}&= \begin{bmatrix}\omega_{bx}\\ \omega_{by}\\ \omega_{bz}\end{bmatrix}_{\dot\psi}+\begin{bmatrix}\omega_{bx}\\ \omega_{by}\\ \omega_{bz}\end{bmatrix}_{\dot\theta}+\begin{bmatrix}\omega_{bx}\\ \omega_{by}\\ \omega_{bz}\end{bmatrix}_{\dot\phi}\\ &= R^T_X(\phi)R^T_Y(\theta)\begin{bmatrix}0\\0\\\dot\psi\end{bmatrix} +R^T_X(\phi)\begin{bmatrix}0\\\dot \theta\\0\end{bmatrix} +\begin{bmatrix}\dot\phi\\0\\0\end{bmatrix}\\ &= \begin{bmatrix} c\theta & 0 & -s\theta\\ s\theta s\phi & c\phi & c\theta s\phi \\ s\theta c\phi & -s\phi & c\theta c\phi \end{bmatrix} \begin{bmatrix}0\\0\\\dot\psi\end{bmatrix} + \begin{bmatrix} 1 & 0 & 0\\ 0 & c\phi & s\phi\\ 0 & -s\phi & c\phi \end{bmatrix} \begin{bmatrix}0\\\dot \theta\\0\end{bmatrix} +\begin{bmatrix}1&0&0\\0&1&0\\0&0&1\end{bmatrix} \begin{bmatrix}\dot\phi\\0\\0\end{bmatrix}\\ &= \begin{bmatrix} 1& 0& -s\theta \\ 0& c\phi& c\theta s\phi \\ 0&-s\phi& c\theta c\phi \end{bmatrix} \begin{bmatrix} \dot\phi\\\dot\theta\\\dot\psi \end{bmatrix} \end{split}
ωbxωbyωbz
=
ωbxωbyωbz
ψ˙+
ωbxωbyωbz
θ˙+
ωbxωbyωbz
ϕ˙=RXT(ϕ)RYT(θ)
00ψ˙
+RXT(ϕ)
0θ˙0
+
ϕ˙00
=
cθsθsϕsθcϕ0cϕ−sϕ−sθcθsϕcθcϕ
00ψ˙
+
1000cϕ−sϕ0sϕcϕ
0θ˙0
+
100010001
ϕ˙00
=
1000cϕ−sϕ−sθcθsϕcθcϕ
ϕ˙θ˙ψ˙
也即：
[
ω
b
x
ω
b
y
ω
b
z
]
=
[
1
0
−
sin
θ
0
cos
ϕ
cos
θ
sin
ϕ
0
−
sin
ϕ
cos
θ
cos
ϕ
]
[
ϕ
˙
θ
˙
ψ
˙
]
\begin{bmatrix}\omega_{bx}\\ \omega_{by}\\ \omega_{bz}\end{bmatrix}= \begin{bmatrix} 1& 0& -\text{sin}\theta \\ 0& \text{cos}\phi& \text{cos}\theta \text{sin}\phi \\ 0&-\text{sin}\phi& \text{cos}\theta \text{cos}\phi \end{bmatrix} \begin{bmatrix} \dot\phi\\\dot\theta\\\dot\psi \end{bmatrix}
ωbxωbyωbz
=
1000cosϕ−sinϕ−sinθcosθsinϕcosθcosϕ
ϕ˙θ˙ψ˙
上述矩阵的逆，由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))
也即：
[
ϕ
˙
θ
˙
ψ
˙
]
=
[
1
sin
ϕ
tan
θ
cos
ϕ
tan
θ
0
cos
ϕ
−
sin
ϕ
0
sin
ϕ
/
cos
θ
cos
ϕ
/
cos
θ
]
[
ω
b
x
ω
b
y
ω
b
z
]
\begin{bmatrix} \dot\phi\\\dot\theta\\\dot\psi \end{bmatrix}= \begin{bmatrix} 1 & \text{sin}\phi \text{tan}\theta & \text{cos}\phi \text{tan}\theta\\ 0 & \text{cos}\phi & -\text{sin}\phi \\ 0 & \text{sin}\phi/\text{cos}\theta & \text{cos}\phi/\text{cos}\theta \end{bmatrix} \begin{bmatrix}\omega_{bx}\\ \omega_{by}\\ \omega_{bz}\end{bmatrix}
ϕ˙θ˙ψ˙
=
100sinϕtanθcosϕsinϕ/cosθcosϕtanθ−sinϕcosϕ/cosθ
ωbxωbyωbz
参考
刚体姿态运动学（二）旋转的微分形式——角速度、欧拉角速度、四元数导数、旋转矩阵导数 控制笔记 姿态角速度和机体角速度，横摆角速度（Yaw Rate）估算 欧拉角速度和机体角速度