张家港专业做网站,网站设计与建设ppt,腾讯建站官网,代理网页打不开0 引言
本文以一种新的角度推导刚体姿态运动学#xff0c;也即角速度和欧拉角速率之间的换算#xff0c;不同于相似博文的地方在于#xff0c;本文旨在从原理上给出直观清晰生动的解释。将详细过程记录于此#xff0c;便于后续学习科研查找需要。
1 符号
符号含义 { E }…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_BRR_Z(\psi)R_Y(\theta)R_X(\phi)\\ \begin{bmatrix} c\psi -s\psi 0\\ s\psi c\psi 0\\ 001 \end{bmatrix} \begin{bmatrix} c\theta 0 s\theta\\ 0 1 0\\ -s\theta0c\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\psic\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} BERRZ(ψ)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}}RR^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,yRRXT(ϕ)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 \phi0 ϕ˙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}100\\010\\001\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估算 欧拉角速度和机体角速度
