기울기 자세 측정하기

칼만필터는 어렵지 않아 로 공부하고 있습니다

칼만필터

-> 항공기나 인공위성의 위치와 자세를 측정하는 항법 분야에서 많이 사용
-> 가속도계와(accelerometer)와 자이로스코프(gyroscope)로 수평자세를 찾아냄

물체의 자세

• 물체의 자세는 오일러각도를 통해서 알 수 있음
• 가속도계와 자이로스코프 센서를 사용해서 오일러 각도를 찾아내고, 자세를 결정함

센서

1. 가속도계(Accelerometer)를 이용하여 자세 결정하기

• 가속도 수식에서 롤각($\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 : 이동속도/ 이동가속도($\dot{p},\dot{q},\dot{r}$)/ g:중력 가속도
가속도, 각속도->측정값
g->이미 알고 있는값
이동속도, 이동 가속도는 고가의 항법센서로만 측정가능

평범한 항법센서로 계산하는 방법

1) 시스템이 정지해 있는 조건
$\dot{u}=\dot{v}=\dot{w}=0$
$u=v=w=0$
2) 일정한 속도로 직진하는 조건
$\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=tan^{-1}({f_y/f_x})$
• $\theta=tan^{-1}(f_x/\sqrt{f_y^2+f_z^2})$
• 근사식 가정해도 되는지??
  • 보통 헬기는 대부분의 임무를 제자리 비행이나 일정 속도로 나는 도중에 수행하므로 이런 가정이 잘 맞음
  • 보행로봇에도 적용 가능
  • but, 빠른 속도로 회전하거나 속도 변화가 심하면 오차가 커질 수 있음

2. 자이로를 이용하여 자세 결정하기

1) 자이로 센서의 각속도와 오일러 각속도

• 자이로 센서는 오일러 각도의 변화율($\dot{\phi},\dot{\theta},\dot{\psi}$)이 아니라, 헬기의 각속도(p,q,r)를 측정
  -> 자이로의 측정값을 오일러 각도의 변화율로 바꿔서 적분
• 오일러 각도와 각속도의 관계
  $\begin{bmatrix}\dot{\phi}\\\dot{\theta}\\\dot{\psi} \end{bmatrix}=\begin{bmatrix}1&sin\phi tan\theta &cos\phi tan\theta\\0&cos\phi&-sin\phi \\ 0&sin\phi/cos\theta&cos\phi/cos\theta \end{bmatrix}$ $\begin{bmatrix}p\\q\\r \end{bmatrix}$
  -> $\phi_k=\phi_{k-1}+dt(p+qsin\phi tan\theta+rcos\phi tan\theta$
  -> $\theta_k=\theta_{k-1}+dt(qcos\phi-rsin\phi)$
  -> $\psi_k=\psi_{k-1}+dt(qsin\phi/cos\theta+rcos\phi/cos\theta)$
  ===> 현재자세를 나타냄!!

2) 오일러 각도로 변환 후 적분한 결과

• 자이로의 측정값에는 오차가 섞여있음->적분하여 구한 자세에도 오차포함
• so, 각속도를 적분하여 얻은 자세는 동적 움직임을 잘 포착하지만 점차 오차가 누적되어 실제값에서 멀어짐
• 단기간의 측정에서는 비교적 정확하지만 장시간의 변화에는 부정확해짐

3. 센서 융합

자이로 센서+가속도센서 => 칼만필터 사용
-> 자이로스코프는 빠른 시간 내에 변위값을 측정하기 유리하나 적분오차 발생
-> 가속도 센서는 느리지만 절대각을 구할 수 있음(+정지상태에서만 기울기 값 측정 가능)

4. 센서 융합을 통해 자세 결정하기

1) 자이로스코프와 가속도계의 센서 융합 원리
(그림)
가속도계로 결정한 자세가 칼만필터의 측정값이 되고, 칼만ㄹ필터는 이 값을 자이로의 각속도를 적분하여 계산한 자세와 비교하여 오차를 보정
2) 자이로스코프와 가속도계의 센서 융합에 필요한 시스템 모델

• 상태변수
  $x=\begin{bmatrix}\phi\\ \theta\\ \psi \end{bmatrix}$
  $x_{k+1}=Ax_k+w_k <=> x=\begin{bmatrix}\dot{\phi}\\ \dot{\theta}\\ \dot{\psi} \end{bmatrix}=\begin{bmatrix}&&&\\&.&&\\&&&\end{bmatrix}\begin{bmatrix}\phi\\ \theta\\ \psi \end{bmatrix}$
  --> 불가능

3) 상태변수를 쿼터니언으로