IMU Attitude from Kalman-Filter Sensor Fusion

2023년 SASA 인터랙티브 피직스 자율 주제 프로젝트 보고서.

1. 초록 (Abstract)

자동차나 로봇과 같은 기계장치에는 여러 가지 센서들이 있다. 다양한 센서 정보를 바탕으로 자동차의 위치와 속도를 추정하거나 로봇의 자세를 추정한다. 이때 센서 데이터에 노이즈가 섞여서 보정이 필요하다. 칼만 필터에 대해 공부하고 행렬 연산 기반인 MATLAB으로 칼만 필터를 구현해서 직접 IMU 데이터를 보정해보기 위해 이 프로젝트를 하게 되었다.

아두이노와 6축 IMU 센서 MPU-6050로 센서 데이터를 얻었다. 이 과정에서 서보모터에 센서를 부착시켜 회전시켰다. PLA-DAQ 프로그램을 사용하여 시리얼 통신으로 받는 데이터를 엑셀에 기록하였다. 자이로 데이터와 가속도계 데이터 각각으로만 오일러 각을 계산하는 MATLAB 코드를 작성하고 결과를 분석했다. 그리고 상호 보완 관계인 자이로와 가속도계를 융합하여 칼만 필터로 각도를 추정하는 코드를 작성했다. 분석 결과, 자이로는 오차가 있는 각속도를 적분해서 시간이 지날수록 오차가 쌓이지만 짧은 시간에 대해서는 각의 변화 패턴을 잘 감지했다. 반면에 가속도계는 오차가 누적되진 않지만 오차 값 자체가 너무 컸다. 두 센서를 융합하여 칼만 필터로 보정한 결과는 완벽했다. 실제 센서가 회전한 각과 매우 근접하게 추정된 것을 확인할 수 있었다.

2. 서론 (Introduction)

칼만 필터 (Kalman filter)

칼만 필터는 노이즈가 있는 데이터에서 상태변수를 추정하는 대표적인 상태 추정 알고리즘이다. 실제로 얻는 신호나 데이터에는 노이즈가 존재할 수 밖에 없기 때문에 공학 분야에서 널리 사용된다. 칼만 필터는 선형 시스템에서 최적화된 제어기법이라고 수학적으로 증명된 기법으로 매우 좋은 성능을 갖는다.

칼만 필터는 과거와 현재 값을 기준으로 재귀적 연산을 통해 최적값을 추정하는 재귀 필터이다. 아래는 칼만 필터 알고리즘을 간략하게 나타낸 것이다.

칼만 필터는 크게 예측 과정(⓵, ⓶)과 추정 과정(⓷, ⓸, ⓹)으로 나눌 수 있다. 예측 과정에서는 행렬 $A,\,Q$ 를 사용하여 예측값 $(\hat{x}_k^-,\,P_k^-)$ 을 구하고, 이 예측값과 측정값 $z_k$ 을 입력으로 받아서 추정 과정이 이뤄진다. 추정 과정에서는 행렬 $H,\,R$ 을 사용하여 추정값과 오차공분산 $(\hat{x}_k,\,P_k)$ 를 구한다. 최종 출력값은 $\hat{x}_k$ 이다.

시스템 모델

칼만 필터는 다음과 같은 선형 상태 모델을 대상으로 한다.

x_{k+1} = A x_k + w_k, \qquad z_k = H x_k + v_k

여기서 $x_k$ 는 상태변수이고, $z_k$ 는 측정값, $A$ 는 시스템 행렬, $H$ 는 출력 행렬, $w_k$ 는 시스템 잡음, $v_k$ 는 측정 잡음이다. 이때 모든 잡음은 백색잡음이고, 평균이 $0$ 인 정규분포를 따른다고 가정한다. $Q$ 는 $w_k$ 의 공분산 행렬, $R$ 은 $v_k$ 의 공분산 행렬이다. $Q$ 와 $R$ 은 정확히 잡음의 특성을 반영해서 구성해야 하지만 여러 오차가 복합적으로 작용하기 때문에 결정하는데 한계가 있다. 그래서 실험적으로 적절한 값을 찾아야 한다. 시스템 모델과 관련된 행렬 $A,\,H,\,Q,\,R$ 이 칼만 필터의 성능을 결정한다.

오일러 각 (Euler angles)

오일러 각은 강체가 놓인 방향을 3차원 공간에 나타내기 위해 오일러가 도입한 세 개의 각도이다. 회전하는 세 개의 각도의 순서를 정하여 강체의 방향을 나타낸다. 각도의 순서에 따라 다른데 ZXZ 회전 방식의 오일러 각을 나타내면 회전된 좌표계의 각도 $\phi,\,\theta,\,\psi$ 는 각각 다음과 같이 정의된다.

$\phi$ : $z$ 축을 회전축으로 하여 $x$ - $y$ 좌표축이 회전한 각도.
$\theta$ : 회전된 $x$ 축( $x'$ 축)을 회전축으로 하여 $z$ - $y$ 좌표축이 회전한 각도.
$\psi$ : 회전된 $z$ 축( $z'$ 축)을 회전축으로 하여 회전된 $x$ - $y$ 좌표축( $x'$ - $y'$ 좌표축)이 회전한 각도.

그림 2. Euler angles. 출처: Wolfram MathWorld

회전 변환을 나타내기 위한 행렬을 변환 행렬이라 한다. 연속적인 오일러 각의 회전을 통해 관성계에 고정되어 있는 한 점의 body frame 기준 좌표를 구함으로써 회전 변환 행렬을 구할 수 있다. 우선 회전 전의 body frame 좌표는 초기 좌표와 같으므로 $\vec{x}_b^{\,0} = \vec{x}$ 이다. 이제 이동 가능한 프레임 $z$ 축을 각도 $\phi$ 만큼 회전시키면 $z$ 좌표는 변하지 않고 나머지 좌표들은 삼각함수를 이용해 구하면 다음을 얻을 수 있다.

\vec{x}_b^{\,1} = \begin{bmatrix} \cos\phi & \sin\phi & 0 \\ -\sin\phi & \cos\phi & 0 \\ 0 & 0 & 1 \end{bmatrix} \vec{x}_b^{\,0} = R(\phi)\,\vec{x}_b^{\,0}

그리고 두 번째 회전으로 새로운 $x$ 축에 대해 $\theta$ 만큼 회전시키면 같은 방법으로 아래와 같다.

\vec{x}_b^{\,2} = \begin{bmatrix} \cos\theta & 0 & -\sin\theta \\ 0 & 1 & 0 \\ \sin\theta & 0 & \cos\theta \end{bmatrix} \vec{x}_b^{\,1} = R(\theta)\,\vec{x}_b^{\,1}

마지막으로 새로운 $z$ 축에 대해 $\psi$ 만큼 회전시키면 다음과 같고

\vec{x}_b^{\,3} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\psi & \sin\psi \\ 0 & -\sin\psi & \cos\psi \end{bmatrix} \vec{x}_b^{\,2} = R(\psi)\,\vec{x}_b^{\,2}

일반적인 변환행렬 $R(\phi,\theta,\psi)$ 를 얻을 수 있다.

\begin{aligned} \vec{x}_b &= R(\psi)\,R(\theta)\,R(\phi)\,\vec{x} \\ &= \begin{bmatrix} \cos\theta\cos\phi & \cos\theta\sin\phi & -\sin\theta \\ -\cos\psi\sin\phi + \sin\psi\sin\theta\cos\phi & \cos\psi\cos\phi + \sin\psi\sin\theta\sin\phi & \sin\psi\cos\theta \\ \sin\psi\sin\phi + \cos\psi\sin\theta\cos\phi & -\sin\psi\cos\phi + \cos\psi\sin\theta\sin\phi & \cos\psi\cos\theta \end{bmatrix} \vec{x} \\ &= R(\phi,\theta,\psi)\,\vec{x} \end{aligned}

오직 매우 작은 오일러 각에 대해서만 오일러 각의 시간 변화율이 몸체(강체) 기준 회전율과 같다. 예를 들어 yaw, pitch, roll 순서에서 가장 우선인 오일러 yaw 각( $\phi$ )은 최종 몸체의 yaw 축에 관한 것이 아니다. 몸체의 각속도 $\vec{\omega}$ 를 구하기 위해 각 오일러 각의 미세 변화를 고려하고 회전 벡터에 끼치는 영향을 고려하면 다음과 같다.

\begin{aligned} \vec{\omega} &= R(\psi)\,R(\theta)\begin{pmatrix} 0 \\ 0 \\ \dfrac{d\phi}{dt} \end{pmatrix} + R(\psi)\begin{pmatrix} 0 \\ \dfrac{d\theta}{dt} \\ 0 \end{pmatrix} + \begin{pmatrix} \dfrac{d\psi}{dt} \\ 0 \\ 0 \end{pmatrix} \\ &= \begin{bmatrix} 1 & 0 & -\sin\theta \\ 0 & \cos\psi & \sin\psi\cos\theta \\ 0 & -\sin\psi & \cos\psi\cos\theta \end{bmatrix} \begin{bmatrix} \dfrac{d\psi}{dt} \\[4pt] \dfrac{d\theta}{dt} \\[4pt] \dfrac{d\phi}{dt} \end{bmatrix} \end{aligned}

역행렬을 구하여 다음과 같이 오일러 각도의 변화율을 몸체의 각속도로 나타낼 수 있다.

\begin{bmatrix} \dot{\psi} \\ \dot{\theta} \\ \dot{\phi} \end{bmatrix} = \begin{bmatrix} 1 & \sin\psi\tan\theta & \cos\psi\tan\theta \\ 0 & \cos\psi & -\sin\psi \\ 0 & \sin\psi/\cos\theta & \cos\psi/\cos\theta \end{bmatrix} \vec{\omega}

Roll, Pitch, Yaw

배나 비행기, 우주선의 움직임을 설명할 때 보통 이 각들을 Roll, Pitch, Yaw 각이라고 한다. Roll, Pitch, Yaw 각각은 기체의 $x,\,y,\,z$ 축을 기준으로 회전반경을 나타낸다.

그림 3. 오른손 법칙을 따르는 Yaw, Pitch, Roll 축. 출처: Wikipedia

오일러 각의 ZYX 표현 방법이 Roll, Pitch, Yaw 표현 방법과 같다.

\text{Roll Pitch Yaw XYZ } (\gamma,\,\beta,\,\alpha) \;\Longleftrightarrow\; \text{Euler ZYX } (\alpha,\,\beta,\,\gamma)

여기서부터는 Roll, Pitch, Yaw의 표현 방식을 따르고 Roll, Pitch, Yaw 각을 각각 $\phi,\,\theta,\,\psi$ 으로 표기한다.

쿼터니언 (Quaternion)

쿼터니언(사원수)는 수학에서 나오는 허수 $i=\sqrt{-1}$ 와 복소평면의 개념을 3차원으로 확장시키는 과정에서 만들어진 것이다. 허수 축 $j,\,k$ 를 추가하면 3차원 회전을 쉽게 표현할 수 있게 된다. 실수 성분 하나에 허수 성분 3개로 표현하는 것인데 허수 성분 3개는 3차원 좌표로 생각하고, 실수 성분을 회전 각도를 표현하는 의미로 둔다.

공학에서 쿼터니언은 회전을 나타내기 위한 표현법 중 하나이다. 오일러 각이 가장 대표적인 방법이지만 표현 방법이 어렵다. 하나의 축에 대해 어떤 각도로 회전하기만 하면 Roll, Pitch, Yaw와 같은 결과가 나오는 쉬운 방법이 쿼터니언이다.

오일러 각을 쿼터니언으로 변환하는 식은 아래와 같다.

\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} \\[4pt] \sin\frac{\phi}{2}\cos\frac{\theta}{2}\cos\frac{\psi}{2} - \cos\frac{\phi}{2}\sin\frac{\theta}{2}\sin\frac{\psi}{2} \\[4pt] \cos\frac{\phi}{2}\sin\frac{\theta}{2}\cos\frac{\psi}{2} + \sin\frac{\phi}{2}\cos\frac{\theta}{2}\sin\frac{\psi}{2} \\[4pt] \cos\frac{\phi}{2}\cos\frac{\theta}{2}\sin\frac{\psi}{2} - \sin\frac{\phi}{2}\sin\frac{\theta}{2}\cos\frac{\psi}{2} \end{bmatrix}

오일러 각을 쿼터니언으로 변환시켜주는 MATLAB 내장함수 eul2quat를 사용해서 구할 수도 있다. 쿼터니언을 오일러 각으로 변환시켜주는 함수는 quat2eul이다.

자이로-가속도계 센서 MPU-6050

그림 4. MPU-6050 with the pin layout and axes orientation. 출처: ResearchGate

MPU-6050은 Accelerometer(가속도계)와 Gyroscope(자이로)가 모두 내장된 6DOF MEMS 센서로 I2C 통신 프로토콜을 통해서 데이터를 가져올 수 있다. 본 프로젝트에서는 자이로, 가속도계 센서 하나씩만 사용해서 자세 기울기를 추정하면 어떤 문제가 있고, 두 센서를 융합해서 쓰면 어떤 효과가 나타나는지 확인해보았다.

3. 방법 (Methods)

본 프로젝트에서는 Roll, Pitch, Yaw (오일러 각 ZYX) 표현법을 따랐다.

① MPU-6050 Raw Data 얻기

MPU-6050의 가속도계를 통해 $x,\,y,\,z$ 축 중력 가속도 데이터를 얻을 수 있고, 자이로를 통해 각속도를 알 수 있다. 시리얼 통신으로 얻은 값들을 엑셀에 실시간으로 저장하기 위해서 PLA-DAQ 프로그램을 사용하였다. PLA-DAQ 프로그램을 사용하여 엑셀 파일에 데이터를 저장하도록 아두이노 코드를 작성했다. PLA-DAQ를 이용해 얻은 데이터 기록 간격은 0.038초였고, 총 15079개의 데이터를 얻었다. (아두이노 코드에서 데이터를 0.01초 간격으로 받았지만 엑셀로 데이터를 연동받는 매크로에서 0.01초보다 느리게 입력받았다. 그래서 직접 계산했는데 데이터를 입력받은 시간을 측정해서 측정 시간을 데이터 수로 나눈 값이 약 0.038초였다.)

센서를 최대한 같은 진폭과 주기로 회전시키기 위해 서보모터에 센서를 부착했다. 아두이노 코드로는 서보모터를 $-30°$ ~ $30°$ (센서 기준)로 좌우로 회전시키는 코드를 업로드했지만 실제로는 $-25°$ ~ $35°$ 정도로 센서가 회전했다. 센서 기준 $y$ 축 중심으로 회전시켜서 Pitch 각을 변화시켰다.

② 자이로 센서를 이용한 자세 추정

자이로센서로 측정한 각속도를 적분해서는 오일러 각을 구할 수 없다. 오일러 각의 변화율이 아니라 몸체의 각속도를 측정하기 때문이다. 그래서 몸체의 각속도와 오일러 각 변화율 사이의 관계를 이용해서 오일러 각을 구해야 한다.

\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} \vec{\omega}, \qquad \vec{\omega} = \begin{bmatrix} p \\ q \\ r \end{bmatrix}

자이로 측정값에는 오차가 있어서 위 식에 $\vec{\omega}$ 를 대입하고 적분해서 구한 오일러 각에도 오차가 있다. 위 식으로 자이로 각속도를 입력받아서 오일러 각을 반환하는 함수 파일 GyroToEuler.m를 작성했다. 그리고 실제 자이로 센서로 측정한 데이터를 읽어오는 함수 파일 GetGyroData.m을 작성했다. GetGyroData 함수는 미리 myAcclGyroData.xlsx 파일에 저장된 GyX, GyY, GyZ 값을 읽어와서 반환한다. 이 두 함수를 이용해서 센서의 Roll, Pitch 각을 결정하는 코드 TestGyroEuler.m을 작성했다. 초기 오일러 각은 모두 $0$ 이라 가정했다.

③ 가속도 센서를 이용한 자세 추정

가속도 센서로 측정한 가속도에는 중력 가속도와 센서의 속도가 바뀔 때 생기는 가속도 등 다양한 가속도가 포함되어 있다. 이를 수식으로 표현하면 다음과 같다.

\begin{bmatrix} A_x \\ A_y \\ A_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$ 는 중력 가속도, $u,\,v,\,w$ 는 이동 속도를 나타내고, $p,\,q,\,r$ 은 각속도이다. $(A_x,\,A_y,\,A_z)$ 와 $(p,\,q,\,r)$ 은 가속도계와 자이로로 측정할 수 있는 값이고, 오일러 각 $\phi,\,\theta$ 를 구하기 위해 우리가 알아야 할 값은 $(u,\,v,\,w)$ 와 $(\dot{u},\,\dot{v},\,\dot{w})$ 이다. 그런데 이 값은 매우 고가의 센서가 아니면 알 수 없다. 시스템이 정지해 있는 상황이라면 이동 속도와 이동 가속도를 모두 $0$ 으로 생각할 수 있기 때문에 가속하지 않는 상황만 생각하면 계산 가능해진다. 등속으로 직진 운동하는 경우를 생각하면, 일단 $(\dot{u},\,\dot{v},\,\dot{w})$ 는 모두 $0$ 이고, 자세 변화가 없으니 각속도 $(p,\,q,\,r)$ 도 모두 $0$ 이다. 그러면 위 식 우변에서 마지막 항만 남게 되어 다음과 같이 간단한 형태가 된다.

\begin{bmatrix} A_x \\ A_y \\ A_z \end{bmatrix} = g \begin{bmatrix} \sin\theta \\ -\cos\theta\sin\phi \\ -\cos\theta\cos\phi \end{bmatrix}

위 식을 통해서 Roll 각과 Pitch 각을 다음과 같이 두 가지로 유도할 수 있다.

\phi = \sin^{-1}\!\left(\frac{-A_y}{g\cos\theta}\right), \qquad \theta = \sin^{-1}\!\left(\frac{A_x}{g}\right)

\phi = \tan^{-1}\!\left(\frac{A_y}{A_z}\right), \qquad \theta = \tan^{-1}\!\left(\frac{-A_x}{\sqrt{A_y^2 + A_z^2}}\right)

이 식을 통해 Roll각과 Pitch각을 반환하는 함수 파일 AccelToEuler.m을 작성하였다. 그리고 실제 가속도 센서로 측정한 데이터를 읽어오는 함수 파일 GetAccelData.m을 작성했다. GetAccelData 함수는 미리 myAcclGyroData.xlsx 파일에 저장된 AcX, AcY, AcZ 값을 읽어와서 반환한다. 이 두 함수를 이용해서 센서의 Roll, Pitch 각을 결정하는 코드 TestAccelEuler.m을 작성했다.

④ 센서 융합과 칼만 필터를 이용한 자세 추정

자이로와 가속도계의 센서 융합은 다음과 같이 이루어진다. 가속도계로 결정한 자세가 칼만 필터의 측정값이 된다. 칼만 필터는 이 값을 자이로의 각속도를 적분하여 계산한 자세와 비교하여 오차를 보정한다. 우선 칼만 필터를 구현하기 위해 자이로와 가속도계의 센서 융합에 필요한 시스템 모델을 설정했다. 자이로 각속도와 오일러 각도 변화율 사이의 관계식은 다음과 같다.

\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} \vec{\omega}, \qquad \vec{\omega} = \begin{bmatrix} p \\ q \\ r \end{bmatrix}

시스템 모델이 되려면 상태변수가 $x_{k+1} = A x_k + w_k$ 와 같은 형태를 갖춰야 하는데 $\begin{bmatrix} \dot{\phi} & \dot{\theta} & \dot{\psi} \end{bmatrix}^{\top}$ 을 $\begin{bmatrix} \phi & \theta & \psi \end{bmatrix}^{\top}$ 이 곱해져 있는 형태로 만들 수 없기 때문에 칼만 필터를 쓸 수 없다. 그래서 오일러 각을 상태 변수로 잡으면 안된다. 상태 변수를 쿼터니언으로 잡으면 시스템 모델을 유도할 수 있다. 쿼터니언과 각속도의 관계는 다음과 같이 알려져 있다.

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

이를 이산 시스템으로 바꿔서 다음과 같은 시스템 모델을 얻을 수 있고 시스템 행렬 $A$ 가 정의된다.

\begin{bmatrix} q_1 \\ q_2 \\ q_3 \\ q_4 \end{bmatrix}_{k+1} = \left( I + \frac{1}{2} \begin{bmatrix} 0 & -p & -q & -r \\ p & 0 & r & -q \\ q & -r & 0 & p \\ r & q & -p & 0 \end{bmatrix} \Delta t \right) \begin{bmatrix} q_1 \\ q_2 \\ q_3 \\ q_4 \end{bmatrix}_{k}

A = I + \frac{1}{2} \begin{bmatrix} 0 & -p & -q & -r \\ p & 0 & r & -q \\ q & -r & 0 & p \\ r & q & -p & 0 \end{bmatrix} \Delta t

오일러 각을 쿼터니언으로 바꾸는 식은 다음과 같고, 이 쿼터니언이 칼만 필터의 측정값 $z_k$ 가 된다.

\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} \\[4pt] \sin\frac{\phi}{2}\cos\frac{\theta}{2}\cos\frac{\psi}{2} - \cos\frac{\phi}{2}\sin\frac{\theta}{2}\sin\frac{\psi}{2} \\[4pt] \cos\frac{\phi}{2}\sin\frac{\theta}{2}\cos\frac{\psi}{2} + \sin\frac{\phi}{2}\cos\frac{\theta}{2}\sin\frac{\psi}{2} \\[4pt] \cos\frac{\phi}{2}\cos\frac{\theta}{2}\sin\frac{\psi}{2} - \sin\frac{\phi}{2}\sin\frac{\theta}{2}\cos\frac{\psi}{2} \end{bmatrix}

모든 상태 변수가 측정되므로 $H$ 는 단위 행렬이 된다.

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

시스템 모델을 모두 구했으니 센서 융합과 칼만 필터로 각을 계산하는 함수 파일 KalmanToEuler.m을 작성했다. 일반적인 칼만 필터 구현 소스코드에다가 쿼터니언 추정값을 오일러 각도로 변환하는 코드가 있다. 쿼터니언은 직관적이지 못하기 때문에 오일러 각으로 바꿔서 출력했다. 초기 상태는 움직이지 않고 정지한 상태이기 때문에 초기 각도를 모두 $0$ 으로 설정했다.

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

잡음의 공분산 행렬 $Q$ 와 $R$ 은 실제 데이터를 분석해보며 성능 변화를 확인해서 여러 시행착오를 거쳐 다음과 같이 설정했다.

Q = \begin{bmatrix} 0.001 & 0 & 0 & 0 \\ 0 & 0.001 & 0 & 0 \\ 0 & 0 & 0.001 & 0 \\ 0 & 0 & 0 & 0.001 \end{bmatrix}, \qquad R = \begin{bmatrix} 10 & 0 & 0 & 0 \\ 0 & 10 & 0 & 0 \\ 0 & 0 & 10 & 0 \\ 0 & 0 & 0 & 10 \end{bmatrix}

그리고 오일러 각을 쿼터니언으로 바꾸는 함수 파일 EulerToQuaternion.m을 작성하고 KalmanToEuler.m를 테스트 해 볼 수 있는 코드 TestKalman.m을 작성했다.

이때까지 작성한 3개의 테스트 코드는 TestGyroEuler.m, TestAccelEuler.m, TestKalman.m으로 이들의 대략적인 Flowchart는 다음과 같다.

4. 결과 (Results)

① 자이로 센서를 이용한 자세 추정

[그림 7]은 자이로 센서로 측정한 각속도 $(p,\,q,\,r)$ 로 구한 오일러 각의 변화율 $(\dot{\phi},\,\dot{\theta})$ 이다. 센서를 $y$ 축 기준으로 회전시켰기 때문에 Roll 각 변화율이 Pitch각 변화율에 비해 매우 작은 것을 볼 수 있다. 완벽히 $y$ 축으로만 회전시키지 못했기 때문에 Roll각 변화율이 항상 $0$ 은 아니었다.

그림 7. 자이로 센서로 측정한 몸체 각속도를 이용해 얻은 오일러 각의 변화율

이를 시간에 대해 적분하여 $\phi,\,\theta$ 를 구하는데 우리가 주목하고 있는 $\theta$ 의 변화만 살펴보았다. 초반에 진동하면서 전체적으로 값이 증가한다. 이는 측정 오차가 시간이 지날수록 누적되어서 측정 오차가 상당히 커지기 때문이다. 그런데 값이 증가하기만 하진 않고, 결국 $180°$ 를 넘어가게 되는 문제가 있다. 이것은 raw data를 매크로를 통해 엑셀로 가져오는 과정에서 잘못된 값이 저장되었거나 300초 이전에 있던 튀는 값으로 인해 급격하게 상승해버린 것이다. 그럼에도 불구하고 뒤쪽에서도 경향만 보면 변화 패턴은 잘 감지하고 있고, 전체적으로 값이 커져가는 것을 확인할 수 있다.

② 가속도 센서를 이용한 자세 추정

[그림 9]는 가속도계로 측정한 가속도 값이다. 여기서 $A_x$ 가 $A_y$ 보다 크다. 이것은 $y$ 축 기준 회전 운동을 했기 때문에 $x$ 축 방향으로 수평면이 올라가거나 내려가게 돼서 중력가속도와 원심력 성분 등이 $x$ 축(Roll 축) 가속도계에 측정되었기 때문이다. 따라서 실제 회전운동에 타당한 결과이다.

가속도 값을 이용해 Roll각과 Pitch각을 구하는 식은 두 가지 방법이 있었는데, 여기서는 아래의 식을 이용하였다.

\phi = \sin^{-1}\!\left(\frac{-A_y}{g\cos\theta}\right), \qquad \theta = \sin^{-1}\!\left(\frac{A_x}{g}\right)

Pitch 각 그래프에서도 같은 양상이 나타난다. 자이로로 구했을 때와 다르게 오차가 누적되지는 않지만 진폭의 오차가 너무 크다.

③ 센서 융합과 칼만 필터를 이용한 자세 추정

[그림 11]을 보면 실제 상황과 동일하게 $-25°$ ~ $35°$ 의 범위 내에서 그래프가 진동한다. 자이로의 누적 오차와 심하게 튀는 값에 의한 오차가 모두 사라졌다. 그리고 가속도계 데이터로만 계산했을 때보다 오차가 훨씬 적다. 칼만 필터의 성능이 매우 좋음을 확인할 수 있었다. 빨간 선 그래프는 [그림 10]의 그래프이다. 실제 테스트 해봤을 때의 상황에 매우 근접한 결과를 확인할 수 있었다.

그림 13. 노이즈가 제거되고 자세 추정이 잘 이루어진 것을 볼 수 있다.

5. 결론 (Conclusion)

자이로와 가속도계의 특징을 분석한 결과를 정리하면, 두 센서가 상호 보완 관계인 것을 알 수 있다. 기울기를 자이로로 구하면 자세 변화를 잘 감지하지만 시간이 지남에 따라 오차가 누적되어 문제였다. 반면 가속도계로 구하면 시간에 따라 오차가 커지지 않고 일정 범위로 제한되는 장점이 있었다. 즉 짧게는 자이로로 결정한 자세가 좋지만, 길게 보면 가속도계로 결정한 자세가 낫다. 그래서 자이로의 누적 오차 문제를 가속도계로 보정하기 위해 센서 융합을 하는 것이고 이 과정에서 칼만 필터를 썼다. 가속도계로 계산한 쿼터니언이 칼만 필터의 측정값이고, 칼만 필터는 이 값을 자이로 각속도를 적분하여 계산한 자세와 비교하여 오차를 보정했다.

칼만 필터를 이용한 센서 융합 결과 Pitch각의 보정이 매우 잘 이루어졌다. 결과는 실제 상황과 매우 근접하게 나왔다. 칼만 필터가 측정 노이즈와 오차가 심한 데이터를 처리해서 원하는 정보인 실제 각도에 대한 신호만 골라낸 것이다. 기본적으로 행렬 연산을 하는 MATLAB을 이용하여 칼만 필터 구현을 간단하게 할 수 있었고, 자이로-가속도 센서의 자세 결정을 위한 칼만 필터의 놀라운 성능을 확인할 수 있었다.

별첨 (Appendixes)

참고문헌 (References)

Wikipedia contributors. (2023, November 1). Euler angles. Wikipedia. https://en.wikipedia.org/wiki/Euler_angles
Wikipedia contributors. (2023, November 17). Quaternions and spatial rotation. Wikipedia. https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
Wikipedia contributors. (2023, October 2). Conversion between quaternions and Euler angles. Wikipedia. https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
MIT OpenCourseWare. Kinematics of moving frames. https://ocw.mit.edu/courses/2-154-maneuvering-and-control-of-surface-and-underwater-vehicles-13-49-fall-2004/bc67e15b31b4f30aceabef2a66a6229d_lec1.pdf
MPU-6050 product specification. https://invensense.tdk.com/wp-content/uploads/2015/02/MPU-6000-Datasheet1.pdf
How to use a three-axis accelerometer for tilt sensing, DFRobot. https://wiki.dfrobot.com/How_to_Use_a_Three-Axis_Accelerometer_for_Tilt_Sensing
김성필. 『칼만 필터는 어렵지 않아』, 한빛아카데미, 2019.
Understanding Kalman filters, MATLAB. https://kr.mathworks.com/videos/series/understanding-kalman-filters.html

프로그램 코드 (Program code): MATLAB & Arduino

GyroToEuler.m

function [phi, theta, psi] = GyroToEuler(p, q, r, dt)
% 자이로 각속도로 계산한 phi, theta를 반환하는 함수
%
persistent prevPhi prevTheta prevPsi
if isempty(prevPhi)
  prevPhi   = 0;
  prevTheta = 0;
  prevPsi   = 0;
end
sinPhi   = sin(prevPhi);    cosPhi   = cos(prevPhi);
cosTheta = cos(prevTheta);  tanTheta = tan(prevTheta);
phi   = prevPhi   + dt*( p + q*sinPhi*tanTheta + r*cosPhi*tanTheta );
theta = prevTheta + dt*(     q*cosPhi          - r*sinPhi );
psi   = prevPsi   + dt*(     q*sinPhi/cosTheta + r*cosPhi/cosTheta );
prevPhi   = phi;
prevTheta = theta;
prevPsi   = psi;

GetGyroData.m

function [p, q, r] = GetGyroData()
% 엑셀 파일에 저장되어 있는
% 자이로 각속도 값을 읽어서 반환하는 함수
persistent GyX GyY GyZ
persistent k firstRun
if isempty(firstRun)
    data = readcell('myAcclGyroData.xlsx');
    GyX = cell2mat(data(2:size(data, 1), 4));
    GyY = cell2mat(data(2:size(data, 1), 5));
    GyZ = cell2mat(data(2:size(data, 1), 6));
    disp('Gyro Data file loaded.');
    k = 1;
    firstRun = 1;
end
p = GyX(k);
q = GyY(k);
r = GyZ(k);
k = k + 1;

TestGyroEuler.m

clear all; clc;
Nsamples = 15079;
EulerSaved = zeros(Nsamples, 3);
GySaved = zeros(Nsamples, 3);
dt = 0.038;
for k=1:Nsamples
    [p, q, r] = GetGyroData();
    [phi, theta, psi] = GyroToEuler(p, q, r, dt);
    EulerSaved(k, :) = [phi, theta, psi];
    GySaved(k, :) = [p, q, r];
end
PhiSaved = EulerSaved(:, 1) * 180/pi;
ThetaSaved = EulerSaved(:, 2) * 180/pi;
PsiSaved = EulerSaved(:, 3) * 180/pi;
t = 0:dt:(Nsamples-1)*dt;
figure;
subplot(2, 1, 1);
plot(t, GySaved(:, 1), 'r', 'LineWidth', 1);
set(gca, 'LineWidth', 1);
title('\bfRoll rate obtained from gyro sensor');
xlabel('Time [sec]');
ylabel('\phi rate [deg/s]');
subplot(2, 1, 2);
plot(t, GySaved(:, 2), 'r', 'LineWidth', 1);
set(gca, 'LineWidth', 1);
title('\bfPitch rate obtained from gyro sensor');
xlabel('Time [sec]');
ylabel('\theta rate [deg/s]');
figure;
plot(t, PhiSaved, 'b', 'LineWidth', 1);
set(gca, 'LineWidth', 1);
title('\bfRoll angle of time (Gyro sensor)');
xlabel('Time [sec]');
ylabel('\phi [deg]');
figure;
plot(t, ThetaSaved, 'b', 'LineWidth', 1);
set(gca, 'LineWidth', 1);
title('\bfPitch angle of time (Gyro sensor)');
xlabel('Time [sec]');
ylabel('\theta [deg]');
% figure;
% plot(t, PsiSaved);
% xlabel('Time [sec]');
% ylabel('Yaw angle [deg]');

AccelToEuler.m

function [phi, theta] = AccelToEuler(Ax, Ay, Az)
% 가속도 센서로 측정한 가속도 값으로
% 롤각, 피치각을 반환하는 함수
% 결과가 더 정확하게 나오는 식
% phi = atan(Ay/Az);
% theta = atan(Ax/sqrt(Ay^2 + Az^2));
g = 9.81;
theta = asin(Ax/g);
phi = asin(-Ay/(g*cos(theta)));

GetAccelData.m

function [Ax, Ay, Az] = GetAccelData()
% 엑셀 파일에 저장되어 있는
% 가속도계의 가속도 값을 읽어서 반환하는 함수
persistent ax ay az
persistent k firstRun
if isempty(firstRun)
    data = readcell('myAcclGyroData.xlsx');
    Accel = cell2mat(data(2:size(data, 1), 1:3));
    ax = Accel(:, 1);
    ay = Accel(:, 2);
    az = Accel(:, 3);
    disp('Accelerometer Data file loaded.');
    k = 1;
    firstRun = 1;
end
Ax = ax(k);
Ay = ay(k);
Az = az(k);
k = k + 1;

TestAccelEuler.m

clear all; clc;
Nsamples = 15079;
EulerSaved = zeros(Nsamples, 2);
AxSaved = zeros(Nsamples, 1);
AySaved = zeros(Nsamples, 1);
dt = 0.038;
for k=1:Nsamples
    [Ax, Ay, Az] = GetAccelData();
    [phi, theta] = AccelToEuler(Ax, Ay, Az);
    EulerSaved(k, :) = [phi, theta];
    AxSaved(k, 1) = Ax;
    AySaved(k, 1) = Ay;
end
PhiSaved = EulerSaved(:, 1) * 180/pi;
ThetaSaved = EulerSaved(:, 2) * 180/pi;
t = 0:dt:(Nsamples-1)*dt;
figure;
subplot(2, 1, 1);
plot(t, AxSaved, 'r', 'LineWidth', 1);
set(gca, 'LineWidth', 1);
title('\bfx-axis acceleration obtained from acceleration sensor');
xlabel('Time [sec]');
ylabel('A_x [m/s^2]');
subplot(2, 1, 2);
plot(t, AySaved, 'r', 'LineWidth', 1);
set(gca, 'LineWidth', 1);
title('\bfy-axis acceleration obtained from acceleration sensor');
xlabel('Time [sec]');
ylabel('A_y [m/s^2]');
% figure;
% plot(t, PhiSaved, 'b', 'LineWidth', 1);
% set(gca, 'LineWidth', 1);
% title('\bfRoll angle of time (Accelerometer)');
% xlabel('Time [sec]');
% ylabel('\phi [deg]');
figure;
plot(t, ThetaSaved, 'b', 'LineWidth', 1);
set(gca, 'LineWidth', 1);
title('\bfPitch angle of time (Accelerometer)');
xlabel('Time [sec]');
ylabel('\theta [deg]');

KalmanToEuler.m

function [phi, theta, psi] = KalmanToEuler(A, z)
% 칼만 필터를 적용하여 쿼터니언 추정값을 구하고
% 오일러 각으로 바꿔서 반환하는 함수
persistent H Q R
persistent x P firstRun
if isempty(firstRun)
    H = eye(4);
%     Q = [ 0.5850^2      0       0       0;
%           0        0.3988^2     0       0;
%           0             0    0.4982^2   0;
%           0             0       0     0.3703^2 ];
%
%     R = [ 0.0189^2      0       0       0;
%           0        0.1665^2     0       0;
%           0             0    0.0052^2   0;
%           0             0       0     0.0012^2 ];
    Q = 2.7*eye(4);
    R = 40*eye(4);
    x = [1 0 0 0]';
    P = 1*eye(4);
    firstRun = 1;
end
xp = A*x;
Pp = A*P*A' + Q;
K = Pp*H'/(H*Pp*H' + R);
x = xp + K*(z - H*xp);
P = Pp - K*H*Pp;
e = quat2eul(x', 'XYZ');
phi = e(1);
theta = e(2);
psi = e(3);

EulerToQuaternion.m

function z = EulerToQuaternion(phi, theta, psi)
% 오일러 각을 쿼터니언으로 바꿔주는 함수
%
sinPhi = sin(phi/2); cosPhi = cos(phi/2);
sinTheta = sin(theta/2); cosTheta = cos(theta/2);
sinPsi = sin(psi/2); cosPsi = cos(psi/2);
z = [ cosPhi*cosTheta*cosPsi + sinPhi*sinTheta*sinPsi;
      sinPhi*cosTheta*cosPsi - cosPhi*sinTheta*sinPsi;
      cosPhi*sinTheta*cosPsi + sinPhi*cosTheta*sinPsi;
      cosPhi*cosTheta*sinPsi - sinPhi*sinTheta*cosPsi
    ];

TestKalman.m

clear all; clc;
Nsamples = 15079;
EulerSaved = zeros(Nsamples, 3);
EulerSaved2 = zeros(Nsamples, 2);
dt = 0.038;
for k=1:Nsamples
    [p, q, r] = GetGyroData();
    A = eye(4) + dt*1/2*[ 0 -p -q -r; p 0 r -q; q -r 0 p; r q -p 0];
    [Ax, Ay, Az] = GetAccelData();
    [phi, theta] = AccelToEuler(Ax, Ay, Az);
    EulerSaved2(k, :) = [phi, theta];
    z = EulerToQuaternion(phi, theta, 0);
    [Phi, Theta, Psi] = KalmanToEuler(A, z);
    EulerSaved(k, :) = [Phi, Theta, Psi];
end
PhiSaved = EulerSaved(:, 1) * 180/pi;
ThetaSaved = EulerSaved(:, 2) * 180/pi;
PsiSaved = EulerSaved(:, 3) * 180/pi;
PhiSaved2 = EulerSaved2(:, 1) * 180/pi;
ThetaSaved2 = EulerSaved2(:, 2) * 180/pi;
t = 0:dt:(Nsamples-1)*dt;
% figure;
% plot(t, PhiSaved, 'b', 'LineWidth', 1);
% set(gca, 'LineWidth', 1);
% title('\bfTest Kalman')
% xlabel('Time [sec]');
% ylabel('\phi [deg]');
% axis auto
figure;
plot(t, ThetaSaved, 'b-', 'LineWidth', 1);
hold on;
plot(t, ThetaSaved2, 'r', 'LineWidth', 0.3);
legend('Kalman Filter', 'Accelerometer');
set(gca, 'LineWidth', 1);
title('\bfPitch angle of time (Sensor fusion using Kalman Filter)');
xlabel('Time [sec]');
ylabel('\theta [deg]');
axis auto
%axis([260 300 -40 50]);

MPU6050_rawData.ino

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
Adafruit_MPU6050 mpu;
void setup(void) {
  Serial.begin(9600);
  if(!mpu.begin()) {
    while(1) {
      delay(10);
    }
  }
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);
  delay(100);
  Serial.println("CLEARDATA");
  Serial.println("AcX,AcY,AcZ,GyX,GyY,GyZ");
}
void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  Serial.print("Data,");
  Serial.print(a.acceleration.x); Serial.print(",");
  Serial.print(a.acceleration.y); Serial.print(",");
  Serial.print(a.acceleration.z); Serial.print(",");
  Serial.print(g.gyro.x); Serial.print(",");
  Serial.print(g.gyro.y); Serial.print(",");
  Serial.println(g.gyro.z);
  delay(10);
}

TestServo.ino

#include <Servo.h>
Servo servo;
void setup() {
  servo.attach(6);
  servo.write(0);
  delay(2000);
}
void loop() {
  for(int i=0;i<=60;i++) {
    servo.write(i);
    delay(20);
  }
  for(int i=60;i>=0;i--) {
    servo.write(i);
    delay(20);
  }
}