一、习惯约定
讯享网
图片来自 PSINS（高精度捷联惯导算法） PSINS工具箱入门与详解.pptx
二、基本旋转矩阵
绕x轴逆时钟旋转 α \alpha α角度
R x ( α ) = [ 1 0 0 0 cos α sin α 0 − sin α cos α ] R_x(\alpha)=\begin{bmatrix} \ 1 & 0 & 0 \\ 0 & \cos \alpha & \sin \alpha \\ 0 & -\sin \alpha & \cos \alpha\end{bmatrix} Rx(α)=
1000cosα−sinα0sinαcosα
绕y轴逆时钟旋转 α \alpha α角度
R y ( α ) = [ cos α 0 − sin α 0 1 0 sin α 0 cos α ] R_y(\alpha)=\begin{bmatrix} \ \cos \alpha & 0 & -\sin \alpha \\ 0 & 1 & 0 \\ \sin \alpha & 0 & \cos \alpha\end{bmatrix} Ry(α)=
cosα0sinα010−sinα0cosα
绕z轴逆时钟旋转 α \alpha α角度
R z ( α ) = [ cos α sin α 0 − sin α cos α 0 0 0 1 ] R_z(\alpha)=\begin{bmatrix} \cos \alpha & \sin \alpha & 0 \\ -\sin \alpha & \cos \alpha & 0 \\ 0 & 0 & 1 \end{bmatrix} Rz(α)=
cosα−sinα0sinαcosα0001
三、313转序（ZXZ）
ZXZ转序如图所示，转角依次为： ψ \psi ψ、 θ \theta θ、 ϕ \phi ϕ，即绕 z z z轴转 ψ \psi ψ角，在绕 x ′ x' x′轴转 θ \theta θ角，最后绕 z ′ ′ z'' z′′轴转 ϕ \phi ϕ角。
根据基本旋转矩阵，可得出姿态矩阵
C = R z ( ϕ ) R x ( θ ) R z ( ψ ) = [ cos ϕ sin ϕ 0 − sin ϕ cos ϕ 0 0 0 1 ] [ 1 0 0 0 cos θ sin θ 0 − sin θ cos θ ] [ cos ψ sin ψ 0 − sin ψ cos ψ 0 0 0 1 ] = [ cos ϕ cos ψ − sin ϕ cos θ sin ψ cos ϕ sin ψ + sin ϕ cos θ cos ψ sin ϕ sin θ − sin ϕ cos ψ − cos ϕ cos θ sin ψ − sin ϕ sin ψ + cos ϕ cos θ cos ψ cos ϕ sin θ sin θ sin ψ − sin θ cos ψ cos θ ] \begin{align} C &= R_z(\phi)R_x(\theta)R_z(\psi)\\ &= \begin{bmatrix} \cos \phi & \sin \phi & 0 \\ -\sin \phi & \cos \phi & 0 \\ 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} \ 1 & 0 & 0 \\ 0 & \cos \theta & \sin \theta \\ 0 & -\sin \theta & \cos \theta\end{bmatrix} \begin{bmatrix} \cos \psi & \sin \psi & 0 \\ -\sin \psi & \cos \psi & 0 \\ 0 & 0 & 1 \end{bmatrix}\\ &= \begin{bmatrix} \cos\phi\cos\psi-\sin\phi\cos\theta\sin\psi & \cos\phi\sin\psi+\sin\phi\cos\theta\cos\psi & \sin\phi\sin\theta \\ -\sin\phi\cos\psi-\cos\phi\cos\theta\sin\psi & -\sin\phi\sin\psi+\cos\phi\cos\theta\cos\psi & \cos\phi\sin\theta \\ \sin\theta\sin\psi & -\sin\theta\cos\psi & \cos\theta \end{bmatrix} \end{align} C=Rz(ϕ)Rx(θ)Rz(ψ)=
cosϕ−sinϕ0sinϕcosϕ0001
1000cosθ−sinθ0sinθcosθ
cosψ−sinψ0sinψcosψ0001
=
cosϕcosψ−sinϕcosθsinψ−sinϕcosψ−cosϕcosθsinψsinθsinψcosϕsinψ+sinϕcosθcosψ−sinϕsinψ+cosϕcosθcosψ−sinθcosψsinϕsinθcosϕsinθcosθ
{ ψ = − arctan C z x C z y θ = arccos C z z ϕ = arctan C x z C y z \begin{equation} \left\{ \begin{array}{lr} \psi=-\arctan\dfrac{C_{zx}}{C_{zy}} \\ \theta=\arccos{C_{zz}}\\ \phi=\arctan\dfrac{C_{xz}}{C_{yz}} \end{array} \right. \end{equation} ⎩
⎨
⎧ψ=−arctanCzyCzxθ=arccosCzzϕ=arctanCyzCxz
四、312转序（ZXY）
ZXY转序如图所示，转角依次为： ψ \psi ψ、 ϕ \phi ϕ、 θ \theta θ，即绕 z z z轴转 ψ \psi ψ角，在绕 x ′ x' x′轴转 ϕ \phi ϕ角，最后绕 y ′ ′ y'' y′′轴转 θ \theta θ角。
根据基本旋转矩阵，可得出姿态矩阵
C = R y ( θ ) R x ( ϕ ) R z ( ψ ) = [ cos θ 0 − sin θ 0 1 0 sin θ 0 cos θ ] [ 1 0 0 0 cos ϕ sin ϕ 0 − sin ϕ cos ϕ ] [ cos ψ sin ψ 0 − sin ψ cos ψ 0 0 0 1 ] = [ cos θ cos ψ − sin θ sin ϕ sin ψ cos θ sin ψ + sin θ sin ϕ cos ψ − sin θ cos ϕ − cos ϕ sin ψ cos ϕ cos ψ sin ϕ sin θ cos ψ + cos θ sin ϕ sin ψ sin θ sin ψ − cos θ sin ϕ cos ψ cos θ cos ϕ ] \begin{align} C &= R_y(\theta)R_x(\phi)R_z(\psi)\\ &=\begin{bmatrix} \ \cos \theta & 0 & -\sin \theta \\ 0 & 1 & 0 \\ \sin \theta & 0 & \cos \theta\end{bmatrix}\begin{bmatrix} \ 1 & 0 & 0 \\ 0 & \cos \phi & \sin \phi \\ 0 & -\sin \phi & \cos \phi\end{bmatrix} \begin{bmatrix} \cos \psi & \sin \psi & 0 \\ -\sin \psi & \cos \psi & 0 \\ 0 & 0 & 1 \end{bmatrix}\\ &= \begin{bmatrix} \cos\theta\cos\psi-\sin\theta\sin\phi\sin\psi & \cos\theta\sin\psi+\sin\theta\sin\phi\cos\psi & -\sin\theta\cos\phi \\ -\cos\phi\sin\psi & \cos\phi\cos\psi & \sin\phi \\ \sin\theta\cos\psi+\cos\theta\sin\phi\sin\psi & \sin\theta\sin\psi-\cos\theta\sin\phi\cos\psi & \cos\theta\cos\phi \end{bmatrix} \end{align} C=Ry(θ)Rx(ϕ)Rz(ψ)=
cosθ0sinθ010−sinθ0cosθ
1000cosϕ−sinϕ0sinϕcosϕ
cosψ−sinψ0sinψcosψ0001
=
cosθcosψ−sinθsinϕsinψ−cosϕsinψsinθcosψ+cosθsinϕsinψcosθsinψ+sinθsinϕcosψcosϕcosψsinθsinψ−cosθsinϕcosψ−sinθcosϕsinϕcosθcosϕ
{ ψ = − arctan C y x C y y θ = arcsin C y z ϕ = − arctan C x z C z z \begin{equation} \left\{ \begin{array}{lr} \psi=-\arctan\dfrac{C_{yx}}{C_{yy}} \\ \theta=\arcsin{C_{yz}}\\ \phi=-\arctan\dfrac{C_{xz}}{C_{zz}} \end{array} \right. \end{equation} ⎩
⎨
⎧ψ=−arctanCyyCyxθ=arcsinCyzϕ=−arctanCzzCxz
版权声明：本文内容由互联网用户自发贡献，该文观点仅代表作者本人。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容，请联系我们，一经查实，本站将立刻删除。
如需转载请保留出处：https://51itzy.com/kjqy/56169.html