칼만 필터는 어렵지 않아
이 책은 칼만 필터 자체가 어려운 것이 아니라 설명하는 방식이 불친절해 배우기 어려웠다는 문제의식에서 출발합니다. 그래서 수학적인 유도와 증명을 앞세우는 대신 칼만 필터의 핵심 알고리
book.naver.com
기울기 자세 측정하기
가속도계와 자이로스코프로 수평 자세(roll, pitch)를 찾아내는 간단한 항법 문제를 다룬다.
1. 관성항법 센서
2. 자이로를 이용하여 자세 결정하기
3. 가속도계를 이용하여 자세 결정하기
4. 센서 융합을 통해 자세 결정하기
5. 가속도계를 이용하여 정밀 자세 결정하기
3. 가속도계를 이용하여 자세 결정하기
3.1 가속도 수식에서 롤각($\phi$)과 피치각($\theta$) 추출
가속도계로 측정한 가속도($ f_x, f_y, f_z $)에는 중력 가속도와 속도의 크기나 방향이 바뀔 때 생기는 가속도 등
다양한 종류의 가속도가 포함되어 있다.
이런 특성을 수식으로 표현하면 다음과 같다.
$$ \begin{bmatrix} f_x \\ f_y \\ f_z \end{bmatrix} = \begin{bmatrix} \dot{u} \\ \dot{v} \\ \dot{w} \end{bmatrix} + \begin{bmatrix} 0 & w & -v \\ -w & 0 & u \\ v & -u & 0 \end{bmatrix} \begin{bmatrix} p \\ q\\ r\end{bmatrix} + g \begin{bmatrix} \sin \theta \\ -\cos \theta \sin \phi \\ -\cos \theta \cos \phi \end{bmatrix} $$
$u, v, w$ : 이동 속도, $p, q, r$ : 회전 각속도, $ g$ : 중력 가속도
롤각($\phi$)과 피치각($\theta$)을 제외한 나머지 항의 값을 알면 이 식으로 수평자세를 계산할 수 있다.
가속도($ f_x, f_y, f_z $)와 각속도($p, q, r$)는 측정값.
중력 가속도($g$)는 이미 알고 있는 값.
이동 속도($u, v, w$)와 이동 가속도($\dot{u}, \dot{v}, \dot{w}$)는 고가의 항법 센서가 아니면 측정 할 수 없다.
3.2 시스템이 정지해 있거나 일정한 속도로 직진한다고 가정.
평범한 항법 센서로는 이동 속도와 이동 가속도를 측정할 수 없기 때문에
시스템이 정지해 있거나 일정한 속도로 직진한다고 가정해보자.
시스템 정지
$$ \dot{u} = \dot{v} = \dot{w} = 0 $$
$$ u = v = w = 0 $$
일정한 속도로 직진
$$ \dot{u} = \dot{v} = \dot{w} = 0 $$
$$ p = q = r = 0 $$
두 경우를 가속도 수식에 적용하면, 다음과 같이 간단한 형태가 된다.
$$ \begin{bmatrix} f_x \\ f_y \\ f_z \end{bmatrix} = g \begin{bmatrix} \sin \theta \\ -\cos \theta \sin \phi \\ - \cos \theta \cos \phi \end{bmatrix} $$
이 식에서 롤각과 피치각의 공식을 유도해낼 수 있다.
$$ \phi = \sin^{-1} (\frac{-f_y}{g\cos \theta}) $$
$$ \theta = \sin^{-1} (\frac{f_x}{g})$$
3.3 이렇게 가정해도 괜찮나?
움직이는 속도가 충분히 느리거나 속도의 크기와 방향이 빠르게 변하지 않는다면 위 식으로 수평자세를 구할 수 있다.
보통 헬기는 대부분의 임무를 제자리 비행이나 일정 속도로 나는 도중에 수행하므로 이런 가정이 잘 맞고
보행 로봇도 대부분 비슷한 조건에서 움직이므로 위 식을 수평 자세에 이용해도 무방하다.
하지만 근사식이기 때문에 빠른 속도로 회전하거나 속도 변화가 심한 경우는 오차가 커질 수 있다는 점을 유의해야 한다.
3.4 적용 결과
이 식에 가속도 측정값을 대입하면 롤각과 피치각을 구할 수 있다.
$$ \phi = \sin^{-1} (\frac{-f_y}{g\cos \theta}) $$
$$ \theta = \sin^{-1} (\frac{f_x}{g})$$
오차가 누적되진 않지만 오차가 상당히 크기 때문에 단독으로 사용하기는 힘들다.
(책에 있는 실습 예제의 결과. )
