【刚体姿态运动学】角速度和欧拉角速率的换算关系的详细推导 |
您所在的位置:网站首页 › 卡尔丹角 › 【刚体姿态运动学】角速度和欧拉角速率的换算关系的详细推导 |
0 引言
本文以一种新的角度推导刚体姿态运动学,也即角速度和欧拉角速率之间的换算,不同于相似博文的地方在于,本文旨在从原理上给出直观清晰生动的解释。将详细过程记录于此,便于后续学习科研查找需要。 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(θ) 类似的,如果 θ ˙ \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))
刚体姿态运动学(二)旋转的微分形式——角速度、欧拉角速度、四元数导数、旋转矩阵导数 控制笔记 姿态角速度和机体角速度,横摆角速度(Yaw Rate)估算 欧拉角速度和机体角速度 |
今日新闻 |
推荐新闻 |
CopyRight 2018-2019 办公设备维修网 版权所有 豫ICP备15022753号-3 |