[항법] 쿼터니언 Quaternion (2) 쿼터니언 미분과 업데이트 식

이전 글을 반드시 참고할 것

 

https://normal-engineer.tistory.com/187?category=954775 

쿼터니언 quaternion (1) 쿼터니언 개념과 오일러각

---

쿼터니언 업데이트 식

 

navigation 문제에서 오일러각의 단점인 gimball lock을 피하기 위해, 오일러각을 사용하는 대신 쿼터니언을 이용해서 navigation frame에 대한 body frame의 각도를 계속 업데이트한다. 오일러각과 쿼터니언은 서로 바꿔가며 쓸 수 있기 때문이다.

 

따라서 쿼터니언에 대한 업데이트 식을 알아야 수치적으로 적분할 수 있다.

 

일단 쿼터니언의 미분값을 알아야 한다.

 

$q(t)$라는 쿼터니언이 $\Delta t$시간 후에 $q(t+\Delta t)$라는 벡터로 변할 때 쿼터니언의 시간변화율을 구해보자. 지금 이 과정은 navigation에서 사용하기 위한 과정이기 때문에 쿼터니언 $q$는 inertial frame에서 body frame으로 좌표 변환해주는 쿼터니언이다.

inertial frame에 대해서 body frame은 각 축마다 $\vec{\omega} = [P,Q,R]^{\top}$의 각속도로 회전하고 있다.

 

t라는 시간에 $\vec{\omega}$ 각속도로 회전하는 경우 $\Delta t$동안 회전한 각도 $\mu$가 있을 때

$\Delta \mu = \omega \Delta t$로 표현할 수 있다. 여기서 $\omega$는 각속도 벡터의 크기라고 생각하면 된다.

 

$\vec{\omega} = Pi+Qj + Rk$ 는 이미 정해진 것이고 앞에서 $\Delta \mu$와 오일러 축에 대한 표현으로 각속도를 표현하면 아래와 같다.

$\vec{\omega}= \omega(\cos \alpha i + \cos\beta j + \cos\gamma k)=\frac{\Delta \mu}{\Delta t}(\cos \alpha i + \cos\beta j + \cos\gamma k)$

 

두 식은 같으므로 아래의 관계식을 얻을 수 있다.

$\begin{equation} \begin{aligned} &P=\cos \alpha \frac{\Delta \mu}{\Delta t} \\ &Q=\cos \beta \frac{\Delta \mu}{\Delta t} \\ &R=\cos \gamma \frac{\Delta \mu}{\Delta t} \end{aligned} \end{equation}$

 

 

그런데 우리는 $q(t)$라는 쿼터니언이 $\Delta t$라는 짧은 시간동안 회전한 쿼터니언을 알고 싶다. 이 쿼터니언을 알고 있어야 미소 시간에 대한 미소 쿼터니언 변화량을 구해서 쿼터니언의 시간 변화율을 구할 수 있기 때문이다.

 

시간 t일 때 오일러 축을 기준으로 $\Delta \mu$가 회전할 때

 

$\begin{equation} \begin{aligned} &q(\Delta \vec{\mu})_{0}=\cos \left(\frac{\Delta \mu}{2}\right) \approx 1 \\ &q(\Delta \vec{\mu})_{1}=\cos \alpha \sin \frac{\Delta \mu}{2} \approx \cos \alpha \frac{\Delta \mu}{2} \\ &q(\Delta \vec{\mu})_{2}=\cos \beta \sin \frac{\Delta \mu}{2} \approx \cos \beta \frac{\Delta \mu}{2} \\ &q(\Delta \vec{\mu})_{3}=\cos \gamma \sin \frac{\Delta \mu}{2} \approx \cos \gamma \frac{\Delta \mu}{2} \end{aligned} \end{equation}$

 

이 식을 우리가 알고 있는 각속도인 $P,Q,R$에 대해서 표현하고자 한다면 앞서 구했던 각속도와 $\Delta \mu$에 대한 식을 이용한다.

즉, $\begin{equation} q(\Delta \vec{\mu}) \approx 1+\frac{1}{2} (P \Delta t) i+\frac{1}{2} (Q \Delta t) j+\frac{1}{2} (R \Delta t) k \end{equation}$

 

이제 $q(\Delta \vec{ \mu})$를 알고 있으므로 쿼터니언 미분을 구해보자.

 

 

이전 글에서 A에서 B를 거쳐 C라는 frame으로 회전하는 과정일 때 $q_{A}^{C}=q_{A}^{B}q_{B}^{C}$라고 표현했었다.

따라서 시간 $t+\Delta t$에 대한 쿼터니언은 다음과 같이 표현할 수 있다.

 

 $q(t+\Delta t)=q(t)q(\Delta \vec{ \mu})$

 

미분의 정의를 이용해서 식을 써보자.

 

$\begin{align}\dot{q}&=\underset{\Delta t \rightarrow 0}{\lim} \frac{q(t+\Delta t)-q(t)}{\Delta t}=\underset{\Delta t \rightarrow 0}{\lim} \frac{q(t) [q(\Delta \vec{\mu})-1]}{\Delta t}\\&=(q_{0}+q_{1}i+q_{2}j+q_{3}k)*\frac{1}{2}(Pi+Qj+Rk)\end{align}$

 

이와 같이 $\dot{q}_{I}^{B}=\frac{1}{2}q_{I}^{B}\omega_{B/I}^{B}$로 나타낼 수 있다.

(지금까지 썼던 $\omega$가 inertial frame(I)에서 본 body frame(B)의 각속도를 body frame 기준으로 표현한 것이므로 $\omega_{B/I}^{B}$로 표기한다. 쿼터니언 $q_{I}^{B}$는 Inertial에서 body로 바꿔주는 회전을 나타낸다.)

 

많은 과정을 거쳐서 Inertial frame에서 body frame으로 가는 쿼터니언에 대한 미분을 구했다. 이 규칙은 다른 frame으로 바뀌더라도 동일하다.

 

그런데 사실 navigation에서 쿼터니언은 주로 navigation frame과 body frame 사이의 변환을 용이하게 하기 위해 둘 사이의 변환을 구하는 데에 사용된다.

즉 $q_{L}^{B}$가 $q_{I}^{B}$보다 중요한 것이다. (L은 navigation frame을 의미한다)

 

따라서 $\dot{q}_{L}^{B}$를 구해야한다.

$\dot{q}_{I}^{B}=\frac{1}{2}q_{I}^{B}\omega_{B/I}^{B}$에서 $I$ 대신 $L$로 바꾸면 되는 문제이지만 보통은 $\omega_{B/L}^{B}$를 직접적으로 구하지 않으므로 다른 방식으로도 생각해보자.

 

$\begin{equation} \begin{gathered} q_{L}^{B}=q_{L}^{I} q_{I}^{B} \\ \dot{q}_{L}^{B}=\dot{q}_{L}^{I} q_{I}^{B}+q_{L}^{I} \dot{q}_{I}^{B} \\ \dot{q}_{L}^{\prime}=\left(\dot{q}_{I}^{L}\right)^{*}=\frac{1}{2}\left(q_{I}^{L} \omega_{L / I}^{L}\right)^{*}=-\frac{1}{2} \omega_{L / I}^{L} q_{L}^{I} \\ \dot{q}_{L}^{B}=-\frac{1}{2} \omega_{L / I}^{L} q_{L}^{l} q_{I}^{B}+\frac{1}{2} q_{L}^{\prime} q_{l}^{B} \omega_{B / I}^{B} \\ \dot{q}_{L}^{B}=-\frac{1}{2} \omega_{L / I}^{L} q_{L}^{B}+\frac{1}{2} q_{L}^{B} \omega_{B /I}^{B} \end{gathered} \end{equation}$

 

위의 과정을 거쳐 $\omega_{L/I}^{L}$과 $\omega_{B/I}^{B}$에 대한 쿼터니언 업데이트 식을 구할 수 있다. 두 각속도는 navigation 과정에서 구할 수 있는 것이므로, 이 값들로 업데이트 식을 표현한 것이라 이해해도 좋다.

 

사실 $\omega_{B/L}^{B}$도 $\omega_{B/I}^{B}-C_{L}^{B}\omega_{L/I}^{L}$로 구할 수 있어서 바로

$\dot{q}_{L}^{B}=\frac{1}{2}q_{L}^{B}\omega_{B/L}^{B}$를 써도 되긴 하지만

위의 식이 더 편리하다.

 

---

NOTE

 

이 노트는 

$\dot{q}_{L}^{B}=\frac{1}{2}q_{L}^{B}\omega_{B/L}^{B}$와 $\dot{q}_{L}^{B}=-\frac{1}{2} \omega_{L / I}^{L} q_{L}^{B}+\frac{1}{2} q_{L}^{B} \omega_{B /I}^{B}$가 같다는 것을 밝히기 위한 설명이다.

 

$\dot{q}_{L}^{B}=\frac{1}{2}q_{L}^{B}\omega_{B/L}^{B}=\frac{1}{2}q_{L}^{B}(\omega_{B/I}^{B}-\omega_{B/L}^{B})$

$\omega_{B/L}^{B}=(q_{L}^{B})^{*}\omega_{L/I}^{L}q_{L}^{B}$ 이 식을 위에 대입한다.

$\Rightarrow \dot{q}_{L}^{B}=\frac{1}{2}q_{L}^{B}\omega_{B/I}^{B}-\frac{1}{2}\omega_{L/I}^{L}q_{L}^{B}$