[칼만필터] Chap 11. 기울기 자세 측정하기

 Chap 11. 기울기 자세 측정하기

항공기나 인공위성의 위치와 자세를 측정하는 항법분야에서는 칼만필터를 가장 많이 사용한다. 

 

 

문제

가속도계(accerometer)와 자이로스코프(gyroscope) 로 수평자세를 찾아내는 문제 

 

헬기의 가속도와 각속도로부터 수평면에 대한 헬기의 앞뒤 및 좌우로 기울어진 각도를 찾아내기

여기서의 수평자세는 오일러각도(Roll, Pitch, Yaw)에서 Roll(\(\phi\)), Pitch(\(\theta\)) 만 알면된다. 

 

헬기의 자세 = 오일러 각도

 

 

가속도계나 자이로는 관성(inertial)좌표계에 대한 측정값을 출력하여 관성항법센서라고 불린다. 

(관성좌표계 - 이동X, 회전X 인 고정되어있는 가상의 좌표계)

 

여러개의 센서출력을 모아서 더 좋은 성능을 끌어내는 기법을 센서융합(sensor fusion)이라고 한다.

센서융합은 각 센서의 특성을 잘 분석해서 상호보완이 되는 것끼리 묶는 것이 중요하다. 단점을 커버할 수 있는 것끼리 묶어야한다. 

 

 

실험데이터

주파수 0.2Hz, 최대 진폭 +- 30도 정현파로 항법센서를 흔드는 실험을 실시

 

롤축 -> 롤-피치축 -> 피치축

 

각 실험마다 1분 수행, 1분 정지, 데이터는 0.01초 간격

 

 

 

11.2 자이로를 이용하여 자세 결정하기 

자이로값을 적분해서 오일러 각을 구할 수 없다. 자이로는 오일러각도의 변화율이 아니라 헬기의 각속도를 측정하는 것이다. 

 

자이로의 측정값을 오일러 각도의 변화율로 바꾼후 적분해야한다. 

 

(=> 즉, local한 헬기에서의 각속도를 자이로센서를 이용해서 얻었는데, 이거를 오일러각(=관성좌표계)으로 변환이 필요한 것이다. )

 

오일러와 각속도의 행렬관계식

 

물체좌표계에 대한 각속도와 오일러각의 회전속도 사이에는 다음과 같은 관계가 있다. 

(ZYX 순서로 변환 / 오일러각은 1번 변환후, 다음 변환이 이루어지는 것이다.)

 

\(R_z(\dot{\psi}) R_y(\dot{\theta}) R_x(\dot{\phi}) \omega  \)

오일러변환식

 

\(\omega =  \begin{bmatrix} \omega_x \\ \omega_y \\ \omega_z \end{bmatrix} \)

각속도

 

재정리하면...

 

\(R_z(\dot{\psi}) R_y(\dot{\theta}) R_x(\dot{\phi}) \omega  =  \begin{bmatrix} 0 \\ 0 \\ \dot{\psi} \end{bmatrix}+ R_z(\dot{\psi}) \begin{bmatrix} 0 \\  \dot{\theta} \\ 0 \end{bmatrix} + R_z(\dot{\psi}) R_y(\dot{\theta})  \begin{bmatrix} \dot{\phi} \\ 0 \\ 0 \end{bmatrix}  \)

 

 

=> 자세하는 나중에 다시 정리하자.

 

\(  \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} \omega_x \\ \omega_y \\ \omega_z \end{bmatrix} \)

 

 

책에서는 각속도를 (p, q, r)로 정의하였다. 일단 이 정의를 그대로 사용한다. 

\(\omega =  \begin{bmatrix} \omega_x \\ \omega_y \\ \omega_z \end{bmatrix} =  \begin{bmatrix} p \\ q \\ r \end{bmatrix}\)

 

 

Python 코드를 살펴보면

 

mat에서 데이터를 읽어온후 샘플 개수만큼 반복시킴.

반복시마다 accel 데이터를 가져오고, 위의 식을 사용하여 accel을 euler로 변환한 후 출력

 

 

자이로원본 데이터(rad 단위인거같은데, rad2deg해주었다)

 

 

원본데이터는 깔끔하다..

 

 

와아우... 장난이 아닌걸... dt로 적분시키는 것만 하면...어마 무지하게 발산하는구나.

오차누적!!

 

오차누적으로 값은 편향되어가지만... 변화되는 패턴은 감지되고있다. 

자이로를 사용하여 얻은 각속도를 적분하여 사용하는 경우 실제값에서 멀어지는 특성이 있다. 

자이로는 자세각 측정보다는 자세각의 동태를 측정하는데 유용하다. 단기간의 측정에서는 정확하지만, 장시간사용시에는 부정확하다.

 

 

 

11.3 가속도계를 사용하여 자세결정하기 

이 챕터는 시작하자나오는 수식에 뭔가 압도되는데...

최대한 이해해보자. 

가속도계로 측정한 가속도에는 중력가속도 + 속도의 크기 + 방향이 바뀔때 생기는 가속도

손으로 잡고 이리 저리 흔들어보면... 중력.. 실제 속도... 방향이 바뀔때... 이런 요소들이 다 적용된다고 생각하면 편하겠다. 이렇게 하나의 물리량안에 여러가지 요소들을 파악하는 것도 능력이겠지?

 

관련 자료는 더 찾아봐야하겠고. 

 

\(u,v,w\) 이동속도

\(\dot{u}, \dot{v}, \dot{w}\) 이동가속도

p,q,r 회전 각속도

\(f_x, f_y, f_z\) 가속도계로 측정한 가속도

g 중력가속도

\(\phi \) roll각

\(\theta\) pitch각

 

 

\(\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} \)

 

가속도와 각속도는 측정값, g도 고정상수

이동속도와 이동가속도는 알수가 없다. 알수가 없기 때문에... 꼼수를 부리면..

 

시스템이 정지해있거나, 일정 속도로 직진하고 있다면 위의 식을 계산할 수 있다. 

움직이지 않으면 이동속도, 이동가속도는 모두 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}{gcos\theta}) \)

\(\theta = sin^{-1} (\frac{f_x}{g}) \)

이 식으로 수평자세를 구할 떄는 근사식이 된다. 

속도 변화가 심하면 오차가 커지게된다. ( 나중에 더 괜찮은 식이 나오니 일단 유도되는 대로 테스트해보자)

 

 

아래 그래프는 실험에서 측정한 가속도 값이다. 

처음에 적었던 것처럼.. 

 

롤축 -> 롤-피치 -> 피치축을 각각 1분씩 기동한다.  30도 각도로 정현파진동

손으로 3축 잡아보고 회전시켜보면.. 아래와 같은 좌표계로 한것 같다.       

 

      x

      ^

       |

y<- z    

 

x: pitch, y:roll , z:yaw

 

 

이 설명또한 재미가 있다. 

롤축이 처음에 회전할때... 피치축에서 자갈자갈~~하게 뭔가 나타난다.

어? 난 롤축을 회전시킨건디...

 

롤축방향의 정현파 진동이 있으면 피치축도 영향을 받는다. 

중력가속도와 원심력 성분도 있기 때문에 피치축에서 측정이 된다.

(측정하는 센서가 하나이니 물리측정에서 두개의 축은 완벽하게 분리될 수가 없다)

 

 

 

 

 

이제 위의 가속도값을 바탕으로 위의 근사자세식을 계산해보면 아래와 같이 계산된다. 

 

이상하다. 위의 식대로 해서 계산하면 실제 각도인 30도 정도는 나올줄 알았는데...

대충봐도 8도가 안나온다. 그냥 반영을 잘하고있다 정도의 느낌뿐..

 

오차가 너무 크다!!!!

하지만...

 

가속도계는 오차를 발산시키지않고, 일정 범위안에 있다. 적분하지 않기 대문에 누적오차가 없다. 

위의 근사식은 가속도와 각속도가 충분히 작은 상황에서 쓸수있다. 

 

(쓰지말자)

 

 

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

자이로와 가속도계는 상호보완 관계이다. 

 

자이로는 자세 각도를 잘 감지하지만 시간이 흐르면서 오차 누적

가속도는 자세변화가 정확도가 낮지만 오차가 커지지않고, 반영이 잘된다

 

이러한 자이로의 누적오차 문제를 가속도계로 보정하면서 사용한다 => 센서 퓨전!

 

가속도계로 계산한 자세가 칼만필터의 측정값이 되고, 칼만필터는 측정값을 기반으로 자이로의 각속도를 적분하여 계산한 자세와 비교하여 오차를 보정한다. 

 

칼만필터를 위해서는 시스템모델이 중요하다고 여러번 언급되었다. 

지금 필요한 식은 현재 측정값을 사용해서 미래의 이론값을 계산하는 모델링이 필요하다

(혹은 과거 측정값을 사용해서 현재의 이론값을 계산하는 것이라고 해도 무방하다)

 

\(  \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} \)

 

아까 앞에서 보았던 식이다. 이 식은 자이로 각속도와 오일러 각도사이의 관계식이다.

(p, q, r)이 각속도다. 

 

이 식이 필요한 이유는 칼만필터의 시스템 모델을 만들기 위해서는 

\(x_{k+1} = A x_{k} + w_{k}\)

이러한 형태의 식으로 만들어내고 A를 찾아야한다. 

 

 

위의 식에서는 자이로의 각속도(p,q,r) 만이 아니라 오일러각도\(\phi,\theta,\psi\)도 알아야지만, 오일러 각도변화율(\(\dot{\phi}, \dot{\theta}, \dot{\psi}\)를 계산할 수 있는 식이다. 순수하게 각속도와 오일러각도를 식에서 찢어낼 수가 없다. 

 

그래서 결론은.. 이 식을 그대로 사용할 수 없다는 말이다. 위의 식은 변수들이 종속적이라 안되고, 변수들이 독립적인 식이 되어야 한다. (선형대수에서 이러한 구절이 나온다)

 

 

 

선배님들이 다 해결해놓으셨는데, 바로 쿼터니언을 사용해서 해결이 가능하다. 

오일러각도와 다르게 쿼터니언을 사용하면 이러한 변수독립이 가능하다. 

그리고 오일러각도의 경우 singular의 위험이 있어서 실제 로봇에서도 사용하지 않는다)

 

회전에 대해서는 쿼터니언을 사용한다.(이것도 나중에 한번 길게 다뤄봐야겠다..)

\( x =  \begin{bmatrix} q_1 \\  q_2 \\ q_3 \\ q_4 \end{bmatrix} \)

 

쿼터니언과 각속도의 관계식

\(\begin{bmatrix} \dot{q}_1 \\ \dot{q}_2 \\ \dot{q}_3 \\ \dot{q}_4 \end{bmatrix} = \frac{1}{2} \begin{bmatrix} 0 & -p & -q & -r \\ p  & 0 & r & -q \\ q & -r & 0 &p \\ r & q & -p & 0 \end{bmatrix}  \begin{bmatrix} q_1 \\  q_2 \\ q_3 \\ q_4 \end{bmatrix} \)

 

위의 식이면 시스템모델이 된다. 

쿼터니언 자세를 입력하면 자세의 변화율이 계산된다. 

 

이 식에서 discrete 시스템으로 바꿔서 실제 코드에서 적용해야한다. 

 

\(\begin{bmatrix} \dot{q}_1 \\ \dot{q}_2 \\ \dot{q}_3 \\ \dot{q}_4 \end{bmatrix}_{k+1} = \begin{pmatrix} I + \Delta t \frac{1}{2} \begin{bmatrix} 0 & -p & -q & -r \\ p  & 0 & r & -q \\ q & -r & 0 &p \\ r & q & -p & 0 \end{bmatrix} \end{pmatrix} \begin{bmatrix} q_1 \\  q_2 \\ q_3 \\ q_4 \end{bmatrix}_{k} \)

 

 

쿼터니언을 사용하니까 측정값도 당연히 쿼터니언으로 변경해서 입력해줘야한다. 

 

https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles

 

위에서 부터 \(q_1, q_2, q_3, q_4\)에 대응된다. 

 

출력행렬 H의 경우 모두 출력하게 해줘야한다. 쿼터니언 중에서 안쓰이는 변수는 없으니까 

\( H = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\  0 & 0 & 1 & 0 \\  0 & 0 & 0 & 1 \end{bmatrix} \)

 

 

 

결과만 놓고보자. 

 

일단 Yaw축은 메인은 아니니까 제쳐둔다. 

 

처음에 설정했던 +-30도씩 움직였다는 실험예상치와 아주 비슷하게 출력되고있다. 

아주 신기하다. 이렇게 결과값만 놓고보면 무슨 마법을 부린 것 같다. 

 

 

 

다시 요약해보자. 

처음 실험데이터는 가속도 및 각속도였다. 

이것을 적분해서 출력해보니 아주 못쓸 데이터가 나왔다. 

가속도만을 가지고 테스트했는데, 뭔가 변화는 잘 있는데, 값은 실제치에 못미친다. 

두개를 융합해서 칼만필터에서 출력하게 했더니 꽤 잘 들어맞는 결과가 나왔다. 

 

이정도면... 칼만필터가 충분히 매력적인 필터이고, 이책에서 보여주고싶었던 극적인 효과가 잘 드러나 보인다. 실제로 나도 타자를 치면서도 얼마나 괜찮을까 생각했는데, 상상외로 잘 들어맞아서 놀랐다. 

 

아마 수많은 대학원생들을 구했다는 칼만필터의 이야기가 허사가 아닌 건 확실하다. 

다만, 추가적으로 느끼는 점은 모델링과 변수 설정등에 대한 것들이 이해도와 경험이 많이 필요할 것으로 생각된다. 차차 배워나가도록 하자. 

 

 

알아야하는 것

물리량에 대한 이해(쿼터니언)

각변수들간의 관계식(오일러각도 <-> 쿼터니언 / 오일러와 각속도의 행렬관계식)

numpy의 atan, atan2

11.5 챕터의 정밀자세 결정하기위한 식이 arctan을 쓴 이유?