[쿼드콥터] 상보필터 사용

필터의 필요성

 우리가 쿼드콥터의 자세유지를 위해 필요한 것은 각속도도 아니고 가속도도 아닌 각도(오일러각)가 필요하다. 그렇다면 각도를 어떻게 구해야할까?? 그것은 센서끼리 융합을 하여서 얻어오는 것인데 두가지 방식이 있다. 바로 아래와 같다.

• ARS(Attitude Reference System) : 자이로 가속도 센서를 이용한 롤. 피치각 추정하는 시스템

• AHRS(Attitude Heading Reference System) : 자이로 가속도 자기컴파스 센서를 이용하여서 롤, 피치, 요각을 추정하는 시스템

 나는 여기서 ARS 를 이용하였고 자이로 가속도값으로 각도를 구하였다. 그럼 어떻게 자이로 가속도값으로 각도를 알 수 있을까?? 그 이유는 자이로, 가속도 센서는 각도에 대한 정보를 간접적으로 가지고 있기 때문이다. 하지만 각각의 센서들의 바이어스와 잡음들의 요인 때문에 정확한 값을 읽어오기가 힘들다. 왜냐하면 각도값을 얻기 위해서는 각속도 값을 적분해줘야 하는데 우리가 필요하지 않는 오차까지 적분이 되어버려서 드리프트 현상같은게 일어나서 정확성이 떨어지게 되기 때문이다. 그러므로 정확한 각도를 얻어오기 위해서 필터는 꼭 필요한 셈이다. 

상보 필터

 각도를 얻기 위한 필터는 크게 상보 필터와 칼만 필터가 있다. 처음에는 칼만 필터를 구현하려고 하였지만 공부하다가 도저히 이해가 안 되고 구현도 되질 않아서 상보필터를 이용하였고 구현하였다. 상보필터로 값을 추정하는 것을 관측기라고도 한다. 상보필터도 백색 잡음과 바이어스 잡음을 없애기위해서 필요한 필터라고 보면된다. 들어가기에 앞서서 자이로 센서와 가속도 센서의 특징을 짚고 넘어가자면 가속도 센서 같은 경우는 저주파수 영역에서 즉 잘 움직이지 않는 상태에서 센서의 값의 신뢰도가 높고 자이로 같은경우는 고주파수 영역 즉 많이 흔들리거나 움직이는 상태에서 센서 값의 신뢰도가 높다. 그렇기에 가속도 센서에는 저주파통과필터(LPF : Low Pass Filter)를 달아주고 자이로 센서에는 고주파 센서(HPF : High Pass Filter)를 달아주어서 합치는 과정이 상보필터라고 봐도 무방할 거 같다. 

 상보필터의 블록선도는 위의 그림과 같다. 여기서 추가하자면 가속도계의 출력을 Roll 이나 Pitch로 바꿔서 입력 해줘야 한다. 그거 같은 경우는 중력가속도를 이용하여서 간단하게 구할 수 있다. 원래 라면 좌표변환 행렬을 사용해서 3차원 변환으로 하여서 변환시켜줘서 오일러 각을 알아야 하지만 나같은 경우는 각 Roll, Pitch 를 독립적으로 생각해서 1차원 변환으로 가속도 센서의 값을 변환시켜 주었다. (글을 적다 느끼는 거지만 정말 하다가 하다가 못해서 구현을 쉬운길로만 간 느낌이다 ;;) Roll, Pitch 의 1차원 변환 같은 경우는 중력가속도를 이용하기 때문에 쉽게 구할수 있다. 

 여기서 중요한 것은 Roll 각과 Pitch 각은 자세가 유지된다고 가정하고 매우 작은 값이라고 생각하였다. 이렇게 가정한 이유는 각도가 작을 때는  로 가정할 수 있기 때문이다. 그래서 Roll 과 Pitch 가 같은 경우는 아래와 같이 구했다. 

즉 쉽게 설명하자면 가속도 센서로 받아온 값이 기준값을 만들어 주고 자이로 센서로 받은값을 적분해줘서 각도값을 얻는다고 보면 될 것이다. 

 이 상보필터의 식을 구현하기 위해서는 상태 방정식과 측정 방정식을 이용한 관측기를 알아야하는데 자이로 가속도에 대해서는 아래와 같이 나온다. 

 즉 이 부분을 구현하게 되는 것이 상보 필터를 구현하는 셈이 되는 것이다. 여기서 이득 K1, K2 같은경우는 이미 다른 사람이 구해놓은 값을 사용하였다. 그래서 K1 = 0.2621 K2 = 0.0523 을 설정하여 설계하였다. 

 아마 이 글을 끝까지 읽어 주신 분이 있어도 아마 이해가 잘 안 될 수도 있을거 같다. 나도 글을 쓰면서 이미 이해했다고 생각한 부분에 혼란이 생겨서 두서없이 적은 것 같은데 그런분을 위해 혹시 몰라서 이 상보필터를 구현한 코드도 첨부하겠다.

 ※ 이 코드는 이미 센서에서 값을 읽어왔다고 생각하고 짠거기 때문에 유의해서 보시길 바랍니다. 

filter.c

filter.h