Drone's DIYer

아두이노(Arduino) 환경에서 전형적인 MPU6050 센서 입력에 사용되는 칼만 필터(Kalman filter)의 예제입니다.

http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/#comment-57783

MPU6050은 가속도와 자이로센서가 1개의 센서에 모두 포함하고 있는 6DOF(Degrees of Freedom) 센서로, I2C(Inter Integrated Circuit) 통신 프로토콜을 통해서 데이터를 추출 할 수 있습니다. 위 모듈은 MPU6050을 포함하며 사용하기 쉽게 만든 모듈입니다. 

MPU6050의 간단한 스펙입니다.

가속도계(accelerometer)는 움직임이 있을때 일반적으로 많은 잡음(noise)을 포함합니다. 자이로(gyroscope)의 문제는 시간이 누적되면 DC 성분(혹은 이를 bias라 표현)이 누적된다는 것입니다. 결론적으로 자이로는 짧은 시간에 믿을 수 있지만, 반면 가속도계는 긴 시간에 더 신뢰성이 있다는 것입니다.

그렇기 때문에 상보 필터(complimentary filter, 혹은 보상 필터)의 적용이 쉽습니다. 기본적으로 가속도계에 대해서 디지털 low pass filter(LPF), 자이로에 대해서는 디지털 high pass filter(HPF)를 적용하는 것입니다. 그러나 칼만 필터(Kalman filter) 만큼 정확하지는 않다는 것입니다. 근래에 아두이노를 이용하여 처리가 가능하지만 칼만 필터는 아두이노가 주로 제공하는 MCU에서는 무겁다는 단점이 있습니다.

k 시간에 시스템의 상태는 다음과 같습니다.

여기서 는 상태 행렬로 다음과 같습니다.

는 각도 의 시간에 따라 누적된 양으로 bias라고 하며 자이로에서 측정된 값에서 이 bias를 빼는 것에 의하여 현재의 각도를 얻을 수 있습니다.

그러므로 이전 상태 로부터의 전이 행렬 는 다음과 같이 쓸 수 있습니다.

는 제어 입력으로 k 시점에서 자이로의 측정값으로 [˚/s]의 단위를 가지며 이 로 상태방정식을 다음과 같이 다시 쓸 수 있습니다.

여기서 는 제어 입력 행렬로 다음과 같이 나타낼 수 있습니다.

는 프로세스 잡음으로, 주어진 k 시간에서 '0'의 평균과 의 공분산(covariance)을 갖는 가우시안 분포(Gaussian distribution)(혹은 정규분포)를 갖습니다.

는 프로세스 잡음 공분산 행렬이고, 이 경우에 가속도계와 bias의 상태 추정치 공분산 행렬이 됩니다. 우리는 bias와 가속도계의 추정이 서로 독립적임을 가정합니다.

위 식에서 처럼  공분산 행렬은 현재 시간 k에 의존합니다. 따라서 가속도계 분산 와 bias의 분산 은 미소 시간 에 곱하여집니다.

프로세스 잡음은 시간이 길어질수록 커지게 됨을 의미합니다. 이는 상태의 마지막 업데이트이기 때문입니다. 예를 들어 자이로는 드리프트(drift) 할 수 있습니다. 우리는 칼만 필터의 동작을 위해서 이들 상수들을 알아야 합니다. 만일 여러분이 큰 값을 설정한다면 상태 추정에 잡음이 커진다는 것을 기억해야 합니다. 예를 들어, 추정 각도가 드리프트 하기 시작하면 여러분은 의 값을 증가시켜야 합니다. 반면에 추정이 여러분이 생각하는 것보다 너무 많이 느려진다면 반응이 빠르도록 의 값을 감소시켜 볼 수 있습니다.

각종 센서들이 측정한 상태측정치들은 각 센서들 고유의 오차 및 잡음이 포함되기 때문에 비행제어기에서 바로 사용할 수 없습니다. 센서융합기는 자이로 센서, 가속도 센서 및 지자기 센서가 측정한 드론의 회전운동 상태측정치와 GPS 수신기 및 기압 센서가 측정한 드론의 병진운동 상태측정치들을 적절히 융합하여 각종 오차 및 잡음이 최소화 된 상태추정치를 계산합니다.

자이로 센서(Gyroscope)

관성측정장치(IMU) 내부에 있는 3축 자이로 센서를 이용해 드론 기체좌표 x, y, z 세 축이 지구관성좌표에 대하여 회전하는 각속도를 측정한 후 고정좌표로 변환된 값$\left(W_{{\mathrm{x.gyro,}}^{}}{W}_{{\mathrm{x.gyro,}}^{}}{W}_{{\mathrm{x.gyro}}^{}}\right)$을 계산합니다. 자이로 센서 측정치는 선형 미분방정식을 이용해 오일러 각도 $\left({\Phi }_{{\mathrm{gyro,}}^{}}{ϴ}_{{\mathrm{gyro,}}^{}}{\psi }_{{\mathrm{gyro}}^{}}\right)$로 변환될 수 있다. 자이로 센서 측정치는 저주파수 대역에서 바이어스(bias) 오차를 포함하기 때문에(즉, 드론이 정지해 있을 때에도 자이로 센서의 측정치가 '0'이 되지 않음) x, y, z 세 축에 대한 자이로 센서의 바이어스 오차가 제거되어야 합니다.

가속도 센서(Accelerometer)

자이로 센서의 스코프의 오차를 제거하기 위해 가속도 센서가 이용됩니다. 관성측정장치(IMU) 내부에 있는 가속도 센서를 이용해 드론 기체좌표 x, y, z 세 축의 지구관성좌표에 대한 가속도를 측정한 후 고정좌표로 변환된 값 (fx,acc, fy,acc, fz,acc)을 계산합니다. 가속도센서 측정치 역시 오일러 각도의 '롤(Φacc)'과 '피치(ϴacc)'로 변환될 수 있으며, 이 값들은 자이로 센서의 측정치를 이용해 계산한 '롤(Φgyro)'과 '피치(ϴgyro)'에 포함된 바이어스 오차를 제거하는데 이용됩니다. 하지만, 가속도 센서는 '요(yaw)'를 측정할 수 없기 때문에 자이로 센서를 이용해 측정한 '요(ψgyro)'에 포함된 바이어스 오차를 제거할 수 없습니다.

지자기 센서(Magetometer or Compass)

지자기 센서를 이용해 드론 기체좌표 x, y, z 세 축의 자북점에 대한 방향을 측정합니다. 이 값을 이용해 기체좌표의 NED 좌표에 대한 '요' 값을 계산할 수 있으며, 센서융합기는 지자기 센서로 측정한 '요(ψmag)'를 이용해 자이로 센서 측정치 '요(ψgyro)'에 포함된 바이어스 오차를 제거합니다. 고정날개 드론의 경우 몸체 전면에 피토관(Pitot Tube)을 부착해 좀 더 정확한 '요' 값을 측정할 수 있으나, 드론의 경우 몸체가 회전하면서 날아가기 때문에 피토관을 이용하기가 어렵다는 것입니다.

GPS 수신기 

GPS 수신기는 GPS 위성들로부터 수신한 신호를 이용해 NED 좌표 상에서 드론의 병진운동상태, 즉 위도(Pn.GPS), 경도(Pe.GPS), 고도(hMSL.GPS), 위도상의 속도(Vn.GPS), 경도상의 속도(Ve.GPS) 및 고도상의 속도(Vd.GPS)를 계산합니다. 여기서 첨자 MSL은 해수면(MSL: Mean Sea Level)을 의미합니다.

기압 센서(Barometer)

GPS 수신기를 통해 수신한 위치 좌표에는 항상 5~10m의 오차가 존재합니다. 민수용 GPS 수신기는 L1 주파수밴드(1.5GHz)의 C/A(Coarse-Acquisition) 코드 혹은 L2 주파수밴드(1.2GHz)의 C/A 코드 둘 중의 하나만을 수신할 수 있습니다. 하지만 군사용 GPS 수신기는 L1 C/A와 L2 C/A를 동시에 수신할 수 있어 Diversity로 인한 이득을 얻을 수 있으며, 추가로 암호화 신호(Encrypted Signal) P(Y)를 수신할 수 있어 GPS 신호가 지구의 이온층을 통과할 때 교란되는 것을 보정할 수 있습니다. 이를 이온층 보정(Ionospheric Correction라 일컫습니다. 5~10m의 GPS 고도 오차는 주로 저공비행을 하는 드론의 지상시설물들과의 충돌 위험을 야기시킵니다. 따라서 별도의 기압 센서를 이용하여 고도(hALP.baro)를 측정하기도 한다는 것입니다. 여기서, 첨자 ALP는 기압(Air-Level Pressor)를 의미하며 드론의 이륙시 기압과 현재 비행고도에서의 기압을 비교해 이륙 지점으로부터의 현재 고도를 계산합니다.

센서융합기는 회전운동상태(ΦE, ϴE, ψE)만을 추정하거나, 회전운동 상태와 병진운동상태(Plon.E, Plat.E, hE)를 동시에 추정할 수 있습니다. 회전운동상태 만을 추정하는 센서융합기를 AHRS(Attitude & Heading Reference System)라고 부르고, 회전운동상태와 병진운동상태를 동시에 추정하는 센서융합기를 관성항법기(INS; Inertial Navigation System)라고 부릅니다.

AHRS를 이용한 센서융합기

AHRS 센서융합기는 보상필터(Complimentary Filter; 상보필터)를 이용하거나 확장 칼만 필터(EKF: Extended Kalman Filter)를 이용합니다. AHRS 보상필터는 고주파 대역 특성이 좋은 자이로 센서의 상태측정치(Φgyro, ϴgyro)를 고주파 대역 필터로 추출하고, 저주파 대역 특성이 좋은 가속도 센서의 상태측정치(Φacc, ϴacc)를 저주파 대역 필터로 추출한 후 합쳐서 자이로 센서의 바이어스 오차가 최소화된 상태추정치(ΦE, ϴE)를 비행제어기(FC)로 전달해 줍니다.

AHRS 확장칼만필더(AHRS-EKF)는 드론의 비행역학(Flight Dynamics)을 이용해 각종 오차를 제거하는 방법입니다. 자이로 센서를 이용한 측정치(Φgyro, ϴgyro, ψgyro), 가속도 센서를 이용한 측정치(Φacc, ϴacc), 지자기 센서를 이용한 측정치(ψmag) 및 GPS 수신기를 이용한 측정치(Vn.GPS, Ve.GPS, Vd.GPS)를 이용해 실시간으로 드론의 회전운동역학(Rotational Dynamics)을 확장 칼만 필터를 이용해 모델링하면서 자이로 센서 및 가속도 센서의 바이어스 오차가 최소화된 상태추정치(ΦE, ϴE, ψE)를 계산해 비행제어기로 전달해 줍니다. GPS 수신기로 측정한 병진운동상태 측정치(Pn.GPS, Pe.GPS) 및 기압 센서로 측정한 고도측정치(hALP.baro)는 융합과정을 거치지 않고 그대로 비행제어기로 전달된다.