IMU Angular velocities 값으로 Quaternion 업데이트 방법

이 글은 아래 Attitude from angular rate 문서를 보고 작성되었습니다.

Attitude from angular rate — AHRS 0.3.1 documentation

로봇의 orientation을 Gyroscope의 각속도 측정값을 통해 업데이트할 수 있다. 이를 수행하는 가장 쉬운 방법은 로봇의 회전율(local rotation late)에 관한 미분 방정식으로 계산하는 것이다.

 

시간이 지남에 따라 쿼터니언 형식으로 회전을 누적하는 것은 다음의 미분 방정식을 적분하여 수행된다. $\mathbf{q}$ 는 로봇의 회전율을 뜻한다. 이러한 지속적인 증가를 태도 전파(Attitude Propagation) 라고도 한다.

\begin{equation} \hat{\mathbf{q}}t = \mathbf{q}{t-1} + \int_{t-1}^t\boldsymbol\omega\, dt \end{equation}

프로그램에서 연속적인 적분을 할 수 없으므로 근사적으로 해를 구해야 한다. 가장 간단한 사례로는 IMU에서 $\Delta t$ 순간에 대한 가속도를 얻어 사용한다. $\boldsymbol\omega(t_n)=\begin{bmatrix}\omega_x&\omega_y&\omega_z\end{bmatrix}^T$ IMU에 구한 가속도를 통해 어떻게 Quaternion으로 표현된 Orientation을 업데이트하는지 알아본다.

Quaternion 미분

우선 각속도를 Quaternion으로 표현해 보자. 순간 축을 중심으로 하는 axis $\mathbf{u}=\frac{\boldsymbol\omega}{\|\boldsymbol\omega\|}$와 회전 각도 $\theta=\|\boldsymbol\omega\|\Delta t$ 으로 나타낼 수 있다.

\begin{equation}\begin{split}\begin{array}{rcl}\Delta\mathbf{q} &=& \cos\frac{\theta}{2} + \mathbf{u}\sin\frac{\theta}{2} \\&=& \cos\frac{\|\boldsymbol\omega\|\Delta t}{2} + \frac{\boldsymbol\omega}{\|\boldsymbol\omega\|}\sin\frac{\|\boldsymbol\omega\|\Delta t}{2}\end{array}\end{split}\end{equation}

 

$\mathbf{q}(t+\Delta t)=\Delta\mathbf{qq}(t)$ 인것을 알고있다. Quaternion의 미분을 위해 $\mathbf{q}(t)$ 와 $\mathbf{q}(t+\Delta t)$ 차이를 계산한다.

\begin{equation}\begin{split}\begin{array}{rcl}
\mathbf{q}(t+\Delta t)-\mathbf{q}(t)
&=& \Big(\cos\frac{\|\boldsymbol\omega\|\Delta t}{2} + \frac{\boldsymbol\omega}{\|\boldsymbol\omega\|}\sin\frac{\|\boldsymbol\omega\|\Delta t}{2}\Big)\mathbf{q} - \mathbf{q} \\
&=& \Big(-2\sin^2\frac{\|\boldsymbol\omega\|\Delta t}{4} + \frac{\boldsymbol\omega}{\|\boldsymbol\omega\|}\sin\frac{\|\boldsymbol\omega\|\Delta t}{2}\Big)\mathbf{q}
\end{array}\end{split}\end{equation}

식에서 생략되어 있는 부분을 작성해 보자.

$$ \begin{equation}\begin{aligned}\mathbf{q}(t+\Delta t)-\mathbf{q}(t)&= \mathbf{q}\Big(\cos\frac{\|\boldsymbol\omega\|\Delta t}{2} - 1 + \frac{\boldsymbol\omega}{\|\boldsymbol\omega\|}\sin\frac{\|\boldsymbol\omega\|\Delta t}{2}\Big) \\ \end{aligned}\end{equation} $$

$\mathbf{q}$로 묶은 위 식에 $\cos(x) = 1 - \sin^2(\frac{x}{2})$ 공식을 이용하여 정리하면 똑같이 나온다.

 

이제 Quaternion을 시간에 대해 미분해 보자.

\begin{equation}\begin{split}\begin{array}{rcl}
\dot{\mathbf{q}}
&=& \underset{\Delta t\to 0}{\mathrm{lim}} \frac{\mathbf{q}(t+\Delta t)-\mathbf{q}(t)}{\Delta t} \\
&=& \underset{\Delta t\to 0}{\mathrm{lim}} \frac{1}{\Delta t}\Big(-2\sin^2\frac{\|\boldsymbol\omega\|\Delta t}{4} + \frac{\boldsymbol\omega}{\|\boldsymbol\omega\|}\sin\frac{\|\boldsymbol\omega\|\Delta t}{2}\Big)\mathbf{q} \\
&=& \frac{\boldsymbol\omega}{\|\boldsymbol\omega\|} \underset{\Delta t\to 0}{\mathrm{lim}} \frac{1}{\Delta t}\sin\big(\frac{\|\boldsymbol\omega\|\Delta t}{2}\big) \mathbf{q} \\
&=& \frac{\boldsymbol\omega}{\|\boldsymbol\omega\|} \frac{d}{dt} \sin\big(\frac{\|\boldsymbol\omega\|}{2}t\big) \Big || _{t=0} \; \mathbf{q} \\
&=& \frac{1}{2}\boldsymbol\omega \mathbf{q} \\
&=& \frac{1}{2}
\begin{bmatrix}
-\omega_x q_x -\omega_y q_y - \omega_z q_z\\
\omega_x q_w + \omega_z q_y - \omega_y q_z\\
\omega_y q_w - \omega_z q_x + \omega_x q_z\\
\omega_z q_w + \omega_y q_x - \omega_x q_y
\end{bmatrix}
\end{array}\end{split}\end{equation}

Omega 연산자를 아래와 같이 정의하고 미분에 동등한 행렬식을 구한다.

\begin{equation}\begin{split}\boldsymbol\Omega(\boldsymbol\omega) =
\begin{bmatrix}0 & -\boldsymbol\omega^T \\ \boldsymbol\omega & -\lfloor\boldsymbol\omega\rfloor_\times\end{bmatrix} =
\begin{bmatrix}
0 & -\omega_x & -\omega_y & -\omega_z \\
\omega_x & 0 & \omega_z & -\omega_y \\
\omega_y & -\omega_z & 0 & \omega_x \\
\omega_z & \omega_y & -\omega_x & 0
\end{bmatrix}\end{split}\end{equation}

\begin{equation}\dot{\mathbf{q}} = \frac{1}{2}\boldsymbol\Omega(\boldsymbol\omega)\mathbf{q}\end{equation}

Quaternion의 곱으로 표현

Vector 또는 Matrix의 지수 제곱 시리즈를 생각해 보자.

$$ \begin{equation}e^\mathbf{X} = \sum_{k=0}^\infty \frac{1}{k!} \mathbf{X}^k\end{equation} $$

$\mathbf{v}=\mathbf{u}\theta$ rotation vector를 아래와 같은 exponential series로 표현할 수 있다.

$$ \begin{equation}e^\mathbf{v} = e^{\mathbf{u}\theta} = \Big(1 - \frac{\theta^2}{2!} + \frac{\theta^4}{4!} + \cdots\Big)+ \Big(\mathbf{u}\theta - \frac{\mathbf{u}\theta^3}{3!} + \frac{\mathbf{u}\theta^5}{5!} + \cdots\Big)\end{equation} $$

Quaternion은 아래와 같이 정의 가능하다.

$$ \begin{equation}\begin{split}\mathbf{q} = e^\mathbf{v} =\begin{bmatrix}\cos\frac{\theta}{2} \\\mathbf{u}\sin\frac{\theta}{2}\end{bmatrix}\end{split}\end{equation} $$

$\mathbf{w}$의 각속도로 $\Delta t$ 시간 만큼 회전했다고 하면 아래 Quaternion의 곱으로 표현 가능하다.

$$ \begin{equation}\mathbf{q}_{t+1} = e^{\frac{1}{2}\boldsymbol\Omega(\boldsymbol\omega)\Delta t}\mathbf{q}_t\end{equation} $$

Euler-Rodrigues 회전 공식과 지수 맵을 이용하여 closed-form solution을 찾을 수 있다.

$$ \begin{equation}\mathbf{q}_{t+1} = \Bigg[ \cos\Big(\frac{\|\boldsymbol\omega\|\Delta t}{2}\Big)\mathbf{I}_4 + \frac{1}{\|\boldsymbol\omega\|}\sin\Big(\frac{\|\boldsymbol\omega\|\Delta t}{2}\Big)\boldsymbol\Omega(\boldsymbol\omega) \Bigg]\mathbf{q}_t\end{equation} $$

$\mathbf{I}_4$는 4x4 Identity Matrix를 의미한다. 여기서 멈추고 쿼터니언을 업데이트할 수 있지만 선형 시스템에서는 사용할 수 없다. 이를 위해서는 쿼터니언 업데이트 방정식을 선형화해야 한다.

Quaternion 선형화

$\mathbf{q}(t+\Delta t)$에 대한 n차 테일러급수를 통해 선형화한다.

$$ \begin{equation}\mathbf{q}_{t+1} = \mathbf{q}_t + \dot{\mathbf{q}}_t\Delta t +\frac{1}{2!}\ddot{\mathbf{q}}_t\Delta t^2 + \cdots\end{equation} $$

\begin{equation}\begin{split}\begin{array}{rcl}
\mathbf{q}_{t+1} &=& \Bigg[\mathbf{I}_4 + \frac{1}{2}\boldsymbol\Omega(\boldsymbol\omega)\Delta t +
\frac{1}{2!}\Big(\frac{1}{2}\boldsymbol\Omega(\boldsymbol\omega)\Delta t\Big)^2 +
\frac{1}{3!}\Big(\frac{1}{2}\boldsymbol\Omega(\boldsymbol\omega)\Delta t\Big)^3 + \cdots\Bigg]\mathbf{q}_t \\
&& \qquad{} + \frac{1}{4}\dot{\boldsymbol\Omega}(\boldsymbol\omega)\Delta t^2\mathbf{q}_t
+ \Big[\frac{1}{12}\dot{\boldsymbol\Omega}(\boldsymbol\omega)\boldsymbol\Omega(\boldsymbol\omega)
+ \frac{1}{24}\boldsymbol\Omega(\boldsymbol\omega)\dot{\boldsymbol\Omega}(\boldsymbol\omega)
+ \frac{1}{12}\ddot{\boldsymbol\Omega}(\boldsymbol\omega)\Big]\Delta t^3\mathbf{q}_t
+ \cdots
\end{array}\end{split}\end{equation}

각속도가 일정하다고 가정할때 $\dot{\boldsymbol\omega}=0$ 아래와 같이 식을 줄일 수 있다.

\begin{equation} \mathbf{q}_{t+1} = \Bigg[\mathbf{I}_4 + \frac{1}{2}\boldsymbol\Omega(\boldsymbol\omega)\Delta t +\frac{1}{2!}\Big(\frac{1}{2}\boldsymbol\Omega(\boldsymbol\omega)\Delta t\Big)^2 +\frac{1}{3!}\Big(\frac{1}{2}\boldsymbol\Omega(\boldsymbol\omega)\Delta t\Big)^3 + \cdots\Bigg]\mathbf{q}_t\end{equation}

\begin{equation}e^{\frac{\Delta t}{2}\boldsymbol\Omega(\boldsymbol\omega)} =\sum_{k=0}^\infty \frac{1}{k!} \Big(\frac{\Delta t}{2}\boldsymbol\Omega(\boldsymbol\omega)\Big)^k\end{equation}

근사치의 오류는 차수가 높을수록 빠르게 사라진다. $\Delta t \to 0$ 차수가 높을수록 센서 신호가 편향되지 않고 잡음이 없다고 가정할 때 근사치가 더 좋아져야 하지만 계산 요구량이 크다는 단점이 있다.

 

1차 테일러급수 형태로 만들어보자.

$$ \begin{equation}\mathbf{q}_{t+1} = \Bigg[\mathbf{I}_4 + \frac{1}{2}\boldsymbol\Omega(\boldsymbol\omega)\Delta t\Bigg]\mathbf{q}_t\end{equation} $$

이렇게 잘리는 두 가지 이유는 다음과 같다.

• 임베디드 시스템과 같은 간단한 아키텍처에서 과도한 계산 부담을 피하기 위함
• 센서 신호가 적절하게 필터링 및 수정되지 않으면 추정이 좋은 결과로 수렴되지 않음

쿼터니언을 유효한 방향을 나타내기 위해 정규화한다.

$$ \begin{equation}\mathbf{q}{t+1} \gets \frac{\mathbf{q}{t+1}}{\|\mathbf{q}_{t+1}\|} \end{equation} $$