AHRS Euler angles

Rotate Frame

각도 계산을 μœ„ν•œ λͺ‡κ°€μ§€ 가정을 ν•˜κ² μŠ΅λ‹ˆλ‹€.

  • global(0) frame의 κ΄€μ°°μžκ°€ λ³Ό λ•Œ 0aβƒ—{}^{0}\vec{a}λŠ” 크기와 λ°©ν–₯이 λ°”λ€Œμ§€ μ•ŠλŠ”λ‹€.
  • 0 frameμ—μ„œ νšŒμ „μ„ μ‹œμž‘ν•˜μ—¬ n 번째 κ΄€μΈ‘λœ μ’Œν‘œκ³„λ₯Ό n frameμ΄λΌν•˜κ³ , μΆ©λΆ„νžˆ 짧은 μ‹œκ°„ κ°„κ²©μœΌλ‘œ κ΄€μΈ‘ν•œλ‹€.
  • k frameμ—μ„œ k+1 frame으둜 νšŒμ „ν•  λ•Œ x, y, z μΆ• κΈ°μ€€μœΌλ‘œ νšŒμ „κ°μ„ μΈ‘μ •ν•  수 μžˆλ‹€.
  • μΆ©λΆ„νžˆ 짧은 μ‹œκ°„ κ΄€μΈ‘ν•˜λ―€λ‘œ, μΈ‘μ •λœ νšŒμ „κ°μ€ small angle approximation을 μ μš©ν•΄λ„ 될만큼 μΆ©λΆ„νžˆ μž‘λ‹€.

k frameμ—μ„œ k+1 frame으둜 νšŒμ „ν•  λ•Œ, xμΆ•μœΌλ‘œ dΟ•d\phi, yμΆ•μœΌλ‘œ dΞΈd\theta, zμΆ•μœΌλ‘œ dψd\psi만큼 νšŒμ „ν–ˆλ‹€λ©΄ 각 좕을 κΈ°μ€€μœΌλ‘œ νšŒμ „ μΏΌν„°λ‹ˆμ–Έμ€ μ•„λž˜μ™€ κ°™μŠ΅λ‹ˆλ‹€.

qβ€Ύ(x^,dΟ•)=[sin⁑dΟ•200cos⁑dΟ•2]Tβ‰ˆ[dΟ•2001]T(dΟ•β†’0)qβ€Ύ(y^,dΞΈ)=[0sin⁑dΞΈ20cos⁑dΞΈ2]Tβ‰ˆ[0dΞΈ201]T(dΞΈβ†’0)qβ€Ύ(z^,dψ)=[00sin⁑dψ2cos⁑dψ2]Tβ‰ˆ[00dψ21]T(dΟˆβ†’0)\begin{aligned} \underline{q}(\hat{x},d\phi) & = \begin{bmatrix}\sin{\cfrac{d\phi}{2}} & 0 & 0 & \cos{\cfrac{d\phi}{2}}\end{bmatrix}^T & \approx \begin{bmatrix}\cfrac{d\phi}{2} & 0 & 0 & 1\end{bmatrix}^T & (d\phi \to 0) \\ \underline{q}(\hat{y},d\theta) & = \begin{bmatrix}0 & \sin{\cfrac{d\theta}{2}} & 0 & \cos{\cfrac{d\theta}{2}}\end{bmatrix}^T & \approx \begin{bmatrix}0 & \cfrac{d\theta}{2} & 0 & 1\end{bmatrix}^T & (d\theta \to 0) \\ \underline{q}(\hat{z},d\psi) & = \begin{bmatrix}0 & 0 &\sin{\cfrac{d\psi}{2}} & \cos{\cfrac{d\psi}{2}}\end{bmatrix}^T & \approx \begin{bmatrix}0 & 0 & \cfrac{d\psi}{2} & 1\end{bmatrix}^T & (d\psi \to 0) \end{aligned}

νšŒμ „κ°μ΄ 큰 경우 νšŒμ „ μˆœμ„œμ— 따라 κ²°κ³Όκ°€ λ‹¬λΌμ§€μ§€λ§Œ μΆ©λΆ„νžˆ μž‘μ€ 경우 νšŒμ „ μˆœμ„œμ— λ”°λ₯Έ κ²°κ³Όκ°€ 크게 λ‹€λ₯΄μ§€ μ•ŠμŠ΅λ‹ˆλ‹€.

q(a⃗1,θ1)q(a⃗2,θ2)≠q(a⃗2,θ2)q(a⃗1,θ1)q(\vec{a}_{1},\theta_{1}) q(\vec{a}_{2},\theta_{2}) \neq q(\vec{a}_{2},\theta_{2}) q(\vec{a}_{1},\theta_{1})
q(aβƒ—1,dΞΈ1)q(aβƒ—2,dΞΈ2)β‰ˆq(aβƒ—2,dΞΈ2)q(aβƒ—1,dΞΈ1)dΞΈβ†’0q(\vec{a}_{1},d\theta_{1}) q(\vec{a}_{2},d\theta_{2}) \approx q(\vec{a}_{2},d\theta_{2}) q(\vec{a}_{1},d\theta_{1}) \quad d\theta \to 0

k frameμ—μ„œ k+1 frame으둜 νšŒμ „ν•  λ•Œ, μΆ©λΆ„νžˆ μž‘μ€ 각도 변화라고 κ°€μ •ν–ˆμœΌλ―€λ‘œ μž„μ˜λ‘œ Z-Y-X 순으둜 νšŒμ „ν–ˆλ‹€κ³  κ°€μ •ν•˜μ—¬ qβ€Ύk+1k\underline{q}_{k+1}^{k}λ₯Ό κ΅¬ν•˜λ©΄ μ•„λž˜μ™€ κ°™μŠ΅λ‹ˆλ‹€.

qk+1kβ‰ˆq(z^,dψ)q(y^,dΞΈ)q(x^,dΟ•)qβ€Ύk+1kβ‰ˆ[dΟ•2dΞΈ2dψ21]T\begin{matrix} q_{k+1}^{k} & \approx & q(\hat{z},d\psi) q(\hat{y},d\theta) q(\hat{x},d\phi) \\ \underline{q}_{k+1}^{k} & \approx & \begin{bmatrix} \cfrac{d\phi}{2} & \cfrac{d\theta}{2} & \cfrac{d\psi}{2} & 1 \end{bmatrix}^T \end{matrix}

각 ꡬ간별 νšŒμ „κ° 츑정을 톡해 0 frameμ—μ„œ n frame이 λ˜λŠ” νšŒμ „ μΏΌν„°λ‹ˆμ–Έμ„ ꡬ할 수 μžˆμŠ΅λ‹ˆλ‹€.

qn0=q10q21β‹―qnnβˆ’1q_{n}^{0} = q_{1}^{0} q_{2}^{1} \cdots q_{n}^{n-1}
qβ€Ύn0=[qxqyqzqw]T\underline{q}_{n}^{0} = \begin{bmatrix} q_x & q_y & q_z & q_w \end{bmatrix}^T
caution

small angle approximation에 μ˜ν•΄ μ‹œκ°„μ΄ 흐름에 따라 λˆ„μ  μ˜€μ°¨κ°€ μ¦κ°€ν•©λ‹ˆλ‹€.

0a=qn0naqΛ‰n00aβ€Ύ=R(qΛ‰n0)L(qn0)naβ€Ύ=[1βˆ’2(qy2+qz2)2(qxqyβˆ’qzqw)2(qxqz+qyqw)02(qxqy+qzqw)1βˆ’2(qx2+qz2)2(qyqzβˆ’qxqw)02(qxqzβˆ’qyqw)2(qyqz+qxqw)1βˆ’2(qx2+qy2)00001]naβ€Ύ\begin{aligned} {}^{0}a & = q_{n}^{0} {}^{n}a \bar{q}_{n}^{0} \\ {}^{0}\underline{a} & = R(\bar{q}_{n}^{0}) L(q_{n}^{0}) {}^{n}\underline{a} \\ & = \begin{bmatrix} 1-2({q_y}^2+{q_z}^2) & 2(q_x q_y - q_z q_w) & 2(q_x q_z + q_y q_w) & 0 \\ 2(q_x q_y + q_z q_w) & 1-2({q_x}^2+{q_z}^2) & 2(q_y q_z - q_x q_w) & 0 \\ 2(q_x q_z - q_y q_w) & 2(q_y q_z + q_x q_w) & 1-2({q_x}^2+{q_y}^2) & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} {}^{n}\underline{a} \end{aligned}

Euler angles

xμΆ•, yμΆ•, zμΆ• 각 좕을 κΈ°μ€€μœΌλ‘œν•˜λŠ” νšŒμ „ 행렬은 μ•„λž˜μ™€ κ°™μŠ΅λ‹ˆλ‹€.

rot(X,Ο•)=[10000cosβ‘Ο•βˆ’sin⁑ϕ00sin⁑ϕcos⁑ϕ00001]rot(Y,ΞΈ)=[cos⁑θ0sin⁑θ00100βˆ’sin⁑θ0cos⁑θ00001]rot(Z,ψ)=[cosβ‘Οˆβˆ’sin⁑ψ00sin⁑ψcos⁑ψ0000100001]\begin{aligned} rot(X,\phi) & = & \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & \cos{\phi} & -\sin{\phi} & 0 \\ 0 & \sin{\phi} & \cos{\phi} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ rot(Y,\theta) & = & \begin{bmatrix} \cos{\theta} & 0 & \sin{\theta} & 0 \\ 0 & 1 & 0 & 0 \\ -\sin{\theta} & 0 & \cos{\theta} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ rot(Z,\psi) & = & \begin{bmatrix} \cos{\psi} & -\sin{\psi} & 0 & 0 \\ \sin{\psi} & \cos{\psi} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{aligned}

Euler anglesμ—λŠ” proper Euler anglesκ³Ό Tait-Bryan angles 두 그룹이 μžˆμŠ΅λ‹ˆλ‹€. 각 그룹은 6개의 νšŒμ „ μˆœμ„œλ₯Ό κ°–μŠ΅λ‹ˆλ‹€.

AHRS ꡬ성을 μœ„ν•΄ Z-Y-X μˆœμ„œλ₯Ό μ‚¬μš©ν•˜λŠ” Tait-Bryan angle을 μ‚¬μš©ν•˜κ² μŠ΅λ‹ˆλ‹€.

일반적으둜 Z좕을 Yaw, Y좕을 Pitch, X좕을 Roll이라고 λΆ€λ¦…λ‹ˆλ‹€. λ°©ν–₯은 μ•„λž˜ κ·Έλ¦Όκ³Ό 같이 μ„€μ •ν•˜κ² μŠ΅λ‹ˆλ‹€.

Z-Y-X Euler angle에 μ˜ν•œ νšŒμ „ν–‰λ ¬μ€ μ•„λž˜μ™€ κ°™μŠ΅λ‹ˆλ‹€.

0aβ€Ύ=rot(Z,ψ)rot(Y,ΞΈ)rot(X,Ο•)naβ€Ύ=Rn0=[cos⁑θcos⁑ψR12R130cos⁑θsin⁑ψR22R230βˆ’sin⁑θsin⁑ϕcos⁑θcos⁑ϕcos⁑θ00001]naβ€Ύ\begin{aligned} {}^{0}\underline{a} & = rot(Z,\psi) rot(Y,\theta) rot(X,\phi) {}^{n}\underline{a} = R_{n}^{0} \\ & = \begin{bmatrix} \cos{\theta}\cos{\psi} & R_{12} & R_{13} & 0 \\ \cos{\theta}\sin{\psi} & R_{22} & R_{23} & 0 \\ -\sin{\theta} & \sin{\phi}\cos{\theta} & \cos{\phi}\cos{\theta} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} {}^{n}\underline{a} \end{aligned}
R12=sin⁑ϕsin⁑θcosβ‘Οˆβˆ’cos⁑ϕsin⁑ψR13=cos⁑ϕsin⁑θcos⁑ψ+sin⁑ϕsin⁑ψR22=sin⁑ϕsin⁑θsin⁑ψ+cos⁑ϕcos⁑ψR23=cos⁑ϕsin⁑θsinβ‘Οˆβˆ’sin⁑ϕcos⁑ψ\begin{aligned} R_{12} & = \sin{\phi} \sin{\theta} \cos{\psi} - \cos{\phi} \sin{\psi} \\ R_{13} & = \cos{\phi} \sin{\theta} \cos{\psi} + \sin{\phi} \sin{\psi} \\ R_{22} & = \sin{\phi} \sin{\theta} \sin{\psi} + \cos{\phi} \cos{\psi} \\ R_{23} & = \cos{\phi} \sin{\theta} \sin{\psi} - \sin{\phi} \cos{\psi} \end{aligned}

Quaternions and Euler angles

νšŒμ „κ°μ˜ 츑정을 톡해 μΏΌν„°λ‹ˆμ–Έ νšŒμ „ ν–‰λ ¬μ˜ 경우 값을 μ•Œ 수 μžˆλŠ” ν–‰λ ¬μž…λ‹ˆλ‹€. μΏΌν„°λ‹ˆμ–Έ νšŒμ „ ν–‰λ ¬κ³Ό Euler angle(Z-Y-X) νšŒμ „ 행렬을 λΉ„κ΅ν•˜λ©΄ AHRS λ₯Ό 톡해 κ΅¬ν•˜κ³ μž ν•˜λŠ” Attitude와 Heading을 ꡬ할 수 μžˆμŠ΅λ‹ˆλ‹€.

[1βˆ’2(qy2+qz2)2(qxqyβˆ’qzqw)2(qxqz+qyqw)02(qxqy+qzqw)1βˆ’2(qx2+qz2)2(qyqzβˆ’qxqw)02(qxqzβˆ’qyqw)2(qyqz+qxqw)1βˆ’2(qx2+qy2)00001]=[cos⁑θcos⁑ψR12R130cos⁑θsin⁑ψR22R230βˆ’sin⁑θsin⁑ϕcos⁑θcos⁑ϕcos⁑θ00001]\begin{aligned} \begin{bmatrix} 1-2({q_y}^2+{q_z}^2) & 2(q_x q_y - q_z q_w) & 2(q_x q_z + q_y q_w) & 0 \\ 2(q_x q_y + q_z q_w) & 1-2({q_x}^2+{q_z}^2) & 2(q_y q_z - q_x q_w) & 0 \\ 2(q_x q_z - q_y q_w) & 2(q_y q_z + q_x q_w) & 1-2({q_x}^2+{q_y}^2) & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \\ = \begin{bmatrix} \cos{\theta}\cos{\psi} & R_{12} & R_{13} & 0 \\ \cos{\theta}\sin{\psi} & R_{22} & R_{23} & 0 \\ -\sin{\theta} & \sin{\phi}\cos{\theta} & \cos{\phi}\cos{\theta} & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} \end{aligned}
[Ο•ΞΈΟˆ]=[atan2(2(qyqz+qxqw),1βˆ’2(qx2+qy2))asin(2(qyqwβˆ’qxqz))atan2(2(qxqy+qzqw),1βˆ’2(qy2+qz2))]\begin{aligned} \begin{bmatrix} \phi \\ \theta \\ \psi \end{bmatrix} = \begin{bmatrix} atan2\left( 2\left( q_y q_z + q_x q_w \right) , 1-2\left( q_x^2 + q_y^2 \right) \right) \\ asin\left( 2\left( q_y q_w - q_x q_z \right) \right) \\ atan2\left( 2\left( q_x q_y + q_z q_w \right) , 1-2\left( q_y^2 + q_z^2 \right) \right) \end{bmatrix} \end{aligned}
Last updated on