자이로,가속도 센서융합(기울기 자세 측정하기)

칼만필터를 이용한 네번째 예제는 '가속도계와 자이로를 이용해 헬기의 수평 자세를 알아내기'이다. 헬기의 가속도($m/s^2$)와 각속도($rad/s$)로 부터, 수평면에 대한 헬기의 앞뒤 및 좌우로 기울어진 각도를 알아낸다. 이 예제에서는 헬기의 수평 자세를 위주로 해석해야하기 때문에 세 개의 각도(Euler angle) Roll($\phi$), Pitch($\theta$), Yaw($\psi$) 중 Roll($\phi$)과 Pitch($\theta$) 정보 만을 이용한다.

센서의 융합을 위해서는 상호 보완을 가장 먼저 고려해야 한다. 즉 각 센서의 단점을 서로 상호보완하는 관계가 되어야 한다는 것이다. 최종적으로 출력값은 각 센서의 장점만을 이용하게 되는 것이다. 이 예제에서는 다음과 같은 방법으로 테스트를 진행한다.

주파수 0.2Hz, 최대 진폭 $\pm$ 30$\degree$인 정현파로 항법 센서를 진동시킨다. 테스트 절차는 다음과 같다.
1) Roll축 기동(1분)
2) 1분 정지
3) Roll-Pitch축 기동(1분)
4) 1분 정지
5) Pitch축 기동(1분)

자이로 센서를 이용하여 자세 결정하기

자이로센서를 이용해 측정한 각속도는 오일러 각도의 변화율($\dot{\phi},\dot{\theta},\dot{\psi}$)이 아닌, 헬기의 각속도($p,q,r$)를 측정하기 때문에 이를 변환하는 행렬을 적용 후, 적분하여 오일러 각도를 구할 수 있게 된다. 오일러와 각속도의 관계는 1.1과 같다.

$\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}$ 1.1

자이로 센서를 이용하여 오일러 각도 계산

function [phi, theta, psi] = EulerGyro(p, q, r, dt)
persistent prevPhi prevTheta prevPsi

if isempty(prevPhi)
    prevPhi = 0;
    prevTheta = 0;
    prevPsi = 0;
end

rotationMat = [1, sin(prevPhi)*tan(prevTheta), cos(prevPhi)*tan(prevTheta)
               0, cos(prevPhi), -sin(prevPhi) 
               0, sin(prevPhi)/cos(prevTheta), cos(prevPhi)/cos(prevTheta)];

angle = dt * rotationMat * [p, q, r]';

phi = prevPhi + angle(1);
theta = prevTheta + angle(2);
psi = prevPsi + angle(3);

prevPhi = phi;
prevTheta = theta;
prevPsi = psi;

코드에 대해 잠깐 설명하자면 이전 각도값을 저장하는 변수인 prevPhi, prevTheta, prevPsi를 선언하였다. 자이로 센서는 이전 값을 기준으로 얼마나 회전했는가를 측정하는 센서이다. 즉, 상대적인 각속도를 측정하기 때문에 (0, 0, 0)라는 상태에서 절대적으로 얼마나 회전했는가를 파악하기 위해선 이전 값을 토대로 누적 합산하여 구할 수 있다. 이를 토대로 구해진 결과는 다음 사진과 같다.

50초에서 250초에는 $\pm{30}\degree$의 진폭으로 진동하는 움직임을 잘 나타내고 있다. 하지만 300초 이후부터는 roll축의 기동이 없음에도 불구하고 값이 점점 발산하는 것을 확인할 수 있는데 이는 오차가 누적되고 있다는 것이다. pitch 축의 경우 이상적으로 0$\degree$에서 $\pm{30}\degree$의 진폭으로 진동하는 움직임을 나타내야 하지만 Roll축 기동의 영향을 받아 오차가 누적되어 값이 발산되는 것을 확인할 수 있다. 자이로 센서의 경우 오차누적으로 인해 값은 편향(biased)되지만, 진동에 대한 변화를 잘 감지하고 있다.

가속도 센서를 이용하여 오일러 각도 계산

function [phi, theta] = EulerAccel(ax, ay, az)

phi = atan(ay / az);
theta = atan(ax / sqrt(ay^2 + az^2));

$\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}$ 2.1

가속도계로 측정한 가속도($f_x,f_y,f_z$)에는 중력 가속도와 속도의 크기나 방향이 바뀔 때 생기는 가속도 등 다양한 종류의 가속도가 포함되어 있는 상태이다. 여기서 $u, v, w$는 이동속도를 의미하고, $p, q, r$은 회전 각속도, $g$는 중력 가속도를 나타낸다. 이 가속도 센서를 이용하는 목적은 결국 식 2.1의 가장 우변에 존재하는 roll angle($\phi$)와 pitch angle($\theta$)를 구하는 것이다. 하지만 이동속도인 ($u, v, w$)와 ($\dot{u},\dot{v},\dot{w}$)는 알 수 없는 값이므로 roll angle($\phi$)와 pitch angle($\theta$)을 구할 수 없다. 하지만 다음과 같은 전제조건을 적용하여 가속도 센서를 통해 식 4와 같이 오일러 각도를 구할 수 있다.

<전제조건>
1. 정지 상태일 경우 이동 가속도 ($\dot{u},\dot{v},\dot{w}$)와 이동 속도 ($u, v, w$)는 모두 0이다.
2. 일정한 속도로 움직이는 경우 이동 가속도가 0이고, 자세의 변화가 거의 없으므로 각속도 ($p, q, r$)도 0이다.
3. 따라서 3.1과 같이 표현된다.

$\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}$ 3.1

3.1을 토대로 구한 오일러 각도는 4.1 , 4.2와 같다.

roll($\phi$) = $\tan^{-1}(\frac{f_y}{f_z})$ 4.1
pitch($\theta$) = $\tan^{-1}(\frac{\sqrt{f_x^2+f_z^2}}{f_z})$ 4.2

가속도계의 오차는 발산하지 않고 일정한 범위 안에 머무르는 장점을 갖고 있다. 자세를 구하는 과정에서 적분을 사용하지 않기 때문에 오차가 누적되지 않기 때문이다. 하지만 안정성이 높은 대신 정밀도는 떨어진다는 단점이 존재한다.

센서 융합을 통해 자세 결정

센서 융합은 각 센서의 단점을 서로 상호 보완하는 것이다. 자이로 센서의 누적 오차 문제를 가속도 센서로 보정하는 것이다. 따라서 센서 융합 시스템의 다이어그램을 나타내자면 아래 그림과 같다.

위의 그림을 시스템 모델을 설정해보자.

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

Quaternion으로 상태 변수를 먼저 잡는다. 이에 대한 설명은 추후 포스트에서 상세히 정리하겠다. 그리고 quaternion과 각속도의 관계는 6.1와 같다.

$\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}$ 6.1

이를 이용하여 다음과 같은 6.2의 시스템 모델을 얻을 수 있다.

$\begin{bmatrix}q_1\\q_2\\q_3\\q_4\end{bmatrix}_{k+1}=(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})\begin{bmatrix}q_1\\q_2\\q_3\\q_4\end{bmatrix}_k$ 6.2

$A=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}$

시스템 모델의 상태 변수는 quaternion이다. 따라서 측정값인 $z_k$를 quaternion으로 변환하여 사용한다. 오일러 각도를 quaternion으로 바꾸는 공식은 6.3과 같다.

$\begin{bmatrix}q_1\\q_2\\q_3\\q_4\end{bmatrix}=\begin{bmatrix}\cos{\frac{\phi}{2}}\cos{\frac{\theta}{2}}\cos{\frac{\psi}{2}}+\sin{\frac{\phi}{2}}\sin{\frac{\theta}{2}}\sin{\frac{\psi}{2}}\\\sin{\frac{\phi}{2}}\sin{\frac{\theta}{2}}\sin{\frac{\psi}{2}}-\cos{\frac{\phi}{2}}\cos{\frac{\theta}{2}}\cos{\frac{\psi}{2}}\\\cos{\frac{\phi}{2}}\cos{\frac{\theta}{2}}\cos{\frac{\psi}{2}}+\sin{\frac{\phi}{2}}\sin{\frac{\theta}{2}}\sin{\frac{\psi}{2}}\\\sin{\frac{\phi}{2}}\sin{\frac{\theta}{2}}\sin{\frac{\psi}{2}}-\cos{\frac{\phi}{2}}\cos{\frac{\theta}{2}}\cos{\frac{\psi}{2}}\end{bmatrix}$ 6.3

따라서 행렬 $H$는 단위행렬이 된다.

$H=\begin{bmatrix}1&0&0&0\\0&1&0&0\\0&0&1&0\\0&0&0&1\end{bmatrix}$

또한, 잡음의 공분산 행렬 $Q, R$은 시스템의 신호 특성과 관련이 있는 값이므로 이에 대한 값은 실험적으로 구하는 것이 보통이다. 이 예제에서는 설정한 초깃값과 $Q, R$은 다음과 같다.

$Q=\begin{bmatrix}0.0001&0&0&0\\0&0.0001&0&0\\0&0&0.0001&0\\0&0&0&0.0001\end{bmatrix}, R=\begin{bmatrix}10&0&0&0\\0&10&0&0\\0&0&10&0\\0&0&0&10\end{bmatrix}$

$\hat{x}_0=\begin{bmatrix}q_1\\q_2\\q_3\\q_4\end{bmatrix}=\begin{bmatrix}1\\0\\0\\0\end{bmatrix}, P^-_0=\begin{bmatrix}1&0&0&0\\0&1&0&0\\0&0&1&0\\0&0&0&1\end{bmatrix}$

$\hat{x}_0$의 초깃값은 오일러 각도가 모두 0이라는 의미이다.

코드

function [phi, theta, psi] = EulerKalman(A, z)

persistent H Q R
persistent x_hat P
persistent firstRun

if isempty(firstRun)
    H = eye(4);
   
    % Initial Noise
    Q = 0.0001 * eye(4);
    R = 10 * eye(4); 

    % Initial Value
    x_hat = [1, 0, 0, 0]';
    P = 1.0 * eye(4);

    firstRun = 1;
end

xp = A * x_hat;
Pp = A*P*A' + Q;

K = Pp*H'*inv(H*Pp*H' + R);

x_hat = xp + K*(z - H*xp);
P = Pp - K*H*Pp;

% Quaternion to Euler
phi = atan2(2*(x_hat(3)*x_hat(4) + x_hat(1)*x_hat(2)), 1 - 2*(x_hat(2)^2 + x_hat(3)^2));
theta = -asin(2*(x_hat(2)*x_hat(4) - x_hat(1)*x_hat(3)));
psi = atan2(2*(x_hat(2)*x_hat(3) + x_hat(1)*x_hat(4)), 1 - 2*(x_hat(3)^2+x_hat(4)^2));

결과

자이로 센서와 가속도 센서의 융합을 통해 자이로의 누적 오차가 제거 된것을 확인할 수 있다. 결론적으로 글의 서두에서 말했듯이 센서의 융합을 위해서는 상호 보완을 가장 먼저 고려해야 한다. 즉 각 센서의 단점을 서로 상호보완하는 관계가 되어야 한다는 것이다. 최종적으로 출력값은 각 센서의 장점만을 이용하게 되는 것이다.

참고

칼만 필터는 어렵지 않아 - 김성필 저, 한빛아카데미