# 坐标系转换之三：欧拉角、四元数、旋转矩阵、方向余弦矩阵、旋转向量、轴角表示

### 欧拉角（Euler angles）与旋转矩阵（Rotation Matrix）

R(α,β,γ)=Rz(α)Ry(β)Rx(γ)=cosαcosβsinαcosβsinβcosαsinβsinγsinαcosγsinαsinβsinγ+cosαcosγcosβsinγcosαsinβcosγ+sinαsinγsinαsinβcosγcosαsinγcosβcosγ

Rz(α)=cosαsinα0sinαcosα0001$R_{z}(\alpha)= \begin{bmatrix} cos\alpha & -sin\alpha &0 \\ sin\alpha & cos\alpha & 0 \\ 0 &0&1 \end{bmatrix}$

Ry(β)=cosβ0sinβ010sinβ0cosβ$R_{y}(\beta)= \begin{bmatrix} cos\beta &0 &sin \beta \\ 0& 1 &0 \\ -sin\beta &0 &cos\beta \end{bmatrix}$

Rx(γ)=1000cosγsinγ0sinγcosγ$R_{x}(\gamma)= \begin{bmatrix} 1 &0 &0 \\ 0& cos\gamma &-sin\gamma \\ 0 &sin\gamma &cos\gamma \end{bmatrix}$

P1=RP2$\textbf P_{1}=R· \textbf P_{2}$

### 旋转向量（Rotation vector）

(θx,θy,θz)$(\theta· x^{'},\theta ·y^{'},\theta ·z^{'})$

(xsin(θ2),ysin(θ2),zsin(θ2))$( x^{'}·sin(\frac{\theta}{2}), y^{'}·sin(\frac{\theta}{2}),z^{'}·sin(\frac{\theta}{2}))$

matlab中，可以使用angle2rod函数来从欧拉角转化为旋转向量。

### 四元数（Quaternion）

(cos(θ2),xsin(θ2),ysin(θ2),zsin(θ2))$(cos (\frac{\theta}{2}),x^{'}sin(\frac{\theta}{2}), y^{'}sin(\frac{\theta}{2}),z^{'}sin(\frac{\theta}{2}))$

q=qiqjqkqr=qii+qjj+qkk+qr$\mathbf {q} = \begin{bmatrix} q_{i}\\q_{j}\\q_{k}\\q_{r} \end{bmatrix}=q_{i}\mathbf {i} +q_{j}\mathbf {j} +q_{k}\mathbf {k} +q_{r}$,(这里把角度放在了后面)

q2i+q2j+q2k+q2r=1$q_{i}^{2}+q_{j}^{2}+q_{k}^{2}+q_{r}^{2}=1$

P2=qP1q1$\mathbf P_{2}=q \mathbf P_{1} q^{-1}$

q1=qiqjqkqr$q^{-1}= \begin{bmatrix} -q_{i}\\-q_{j}\\-q_{k}\\q_{r} \end{bmatrix}$

### 四元数与欧拉角的转换

qiqjqkqr=sinϕ2cosθ2cosψ2cosϕ2sinθ2sinψ2=cosϕ2sinθ2cosψ2+sinϕ2cosθ2sinψ2=cosϕ2cosθ2sinψ2sinϕ2sinθ2cosψ2=cosϕ2cosθ2cosψ2+sinϕ2sinθ2sinψ2{\displaystyle {\begin{aligned}q_{i}&=\sin {\frac {\phi }{2}}\cos {\frac {\theta }{2}}\cos {\frac {\psi }{2}}-\cos {\frac {\phi }{2}}\sin {\frac {\theta }{2}}\sin {\frac {\psi }{2}}\\q_{j}&=\cos {\frac {\phi }{2}}\sin {\frac {\theta }{2}}\cos {\frac {\psi }{2}}+\sin {\frac {\phi }{2}}\cos {\frac {\theta }{2}}\sin {\frac {\psi }{2}}\\q_{k}&=\cos {\frac {\phi }{2}}\cos {\frac {\theta }{2}}\sin {\frac {\psi }{2}}-\sin {\frac {\phi }{2}}\sin {\frac {\theta }{2}}\cos {\frac {\psi }{2}}\\q_{r}&=\cos {\frac {\phi }{2}}\cos {\frac {\theta }{2}}\cos {\frac {\psi }{2}}+\sin {\frac {\phi }{2}}\sin {\frac {\theta }{2}}\sin {\frac {\psi }{2}}\end{aligned}}}

rollpitchyaw=atan2(2(qrqi+qjqk),12(q2i+q2j))=arcsin(2(qrqjqkqi))=atan2(2(qrqk+qiqj),12(q2j+q2k))\begin{aligned} {\text{roll}}&=\operatorname {atan2} \left(2\left(q_{r}q_{i}+q_{j}q_{k}\right),1-2\left(q_{i}^{2}+q_{j}^{2}\right)\right)\\{\text{pitch}}&=\arcsin \left(2\left(q_{r}q_{j}-q_{k}q_{i}\right)\right)\\{\text{yaw}}&=\operatorname {atan2} \left(2\left(q_{r}q_{k}+q_{i}q_{j}\right),1-2\left(q_{j}^{2}+q_{k}^{2}\right)\right) \end{aligned}

q=cos(ψ/2)00sin(ψ/2)cos(θ/2)0sin(θ/2)0cos(ϕ/2)sin(ϕ/2)00=cos(ϕ/2)cos(θ/2)cos(ψ/2)+sin(ϕ/2)sin(θ/2)sin(ψ/2)sin(ϕ/2)cos(θ/2)cos(ψ/2)cos(ϕ/2)sin(θ/2)sin(ψ/2)cos(ϕ/2)sin(θ/2)cos(ψ/2)+sin(ϕ/2)cos(θ/2)sin(ψ/2)cos(ϕ/2)cos(θ/2)sin(ψ/2)sin(ϕ/2)sin(θ/2)cos(ψ/2){\displaystyle {\begin{aligned}\mathbf {q} &={\begin{bmatrix}\cos(\psi /2)\\0\\0\\\sin(\psi /2)\\\end{bmatrix}}{\begin{bmatrix}\cos(\theta /2)\\0\\\sin(\theta /2)\\0\\\end{bmatrix}}{\begin{bmatrix}\cos(\phi /2)\\\sin(\phi /2)\\0\\0\\\end{bmatrix}}\\&={\begin{bmatrix}\cos(\phi /2)\cos(\theta /2)\cos(\psi /2)+\sin(\phi /2)\sin(\theta /2)\sin(\psi /2)\\\sin(\phi /2)\cos(\theta /2)\cos(\psi /2)-\cos(\phi /2)\sin(\theta /2)\sin(\psi /2)\\\cos(\phi /2)\sin(\theta /2)\cos(\psi /2)+\sin(\phi /2)\cos(\theta /2)\sin(\psi /2)\\\cos(\phi /2)\cos(\theta /2)\sin(\psi /2)-\sin(\phi /2)\sin(\theta /2)\cos(\psi /2)\\\end{bmatrix}}\\\end{aligned}}}

ϕθψ=atan2(2(q0q1+q2q3),12(q21+q22))asin(2(q0q2q3q1))atan2(2(q0q3+q1q2),12(q22+q23))${\displaystyle {\begin{bmatrix}\phi \\\theta \\\psi \end{bmatrix}}={\begin{bmatrix}{\mbox{atan2}}(2(q_{0}q_{1}+q_{2}q_{3}),1-2(q_{1}^{2}+q_{2}^{2}))\\{\mbox{asin}}(2(q_{0}q_{2}-q_{3}q_{1}))\\{\mbox{atan2}}(2(q_{0}q_{3}+q_{1}q_{2}),1-2(q_{2}^{2}+q_{3}^{2}))\end{bmatrix}}}$

matlab中(Aerospace Toolbox)，
quatmultiply，quatinv，quatconj分别对应四元数的乘法、逆、和共轭。
angle2quat和quat2angle进行欧拉角与四元数的互转。
dcm2quat和quat2dcm进行旋转矩阵与四元数的互转。
rod2quat和quat2rod进行旋转向量与四元数的互转。

Reference:

1. matlab中各种表示之间的相互转换：http://cn.mathworks.com/help/aeroblks/axes-transformations.html
2. 维基百科：坐标转换的各种向量介绍与转换：https://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions
3. Conversion between quaternions and Euler angles：https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles

• 广告
• 抄袭
• 版权
• 政治
• 色情
• 无意义
• 其他

120