본 연구에서 사용한 첫 번째 알고리즘은 외부가속도는 보정하지 않고 자이로 센서의 바이어스만 보정한 칼만필터이다.⁸ 이 칼만필터는 가속도 센서에서 계산된 자세(Roll: ϕ, Pitch: θ)와 자이로 센서로부터 계산된 자세 회전속도(ϕ˙, θ˙)를 융합하여 정확한 자세값을 계산한다. 첫 번째 알고리즘에서 사용한 칼만필터는 다음의 상태방정식으로부터 유도된다.

여기서,

위 상태방정식에서 βϕ˙, βθ˙는 각각 ϕ˙, θ˙의 바이어스이다. 자이로 센서에서 측정된 회전속도로부터 자세를 계산할 때, 바이어스에 의해 발생하는 적분오차를 제거해주기 위해 상태변수에 바이어스를 추가하였다.

위 상태방정식을 사용한 칼만필터의 구조는 Fig. 1과 같다. Fig. 1에서 Time Update는 예측부이며 Measurement Update는 측정보상부이다. 우선, 예측부에서 x~k은 상태 추정벡터이며 식(1)의 상태벡터와 같다. P~k은 예측된 에러 공분산 벡터이며 A, B 행렬은 식(1)의 A, B행렬과 같다. u^k는 입력값이며 여기서는 자이로 센서로부터 계산된 자세 회전속도이다. Q는 공정잡음 공분산 행렬이며 제어 입력 노이즈(또는 측정 노이즈 공분산 행렬) R_v에 의해 발생한다고 가정하면 식(2)와 같다.

Fig. 1 
Overview of normal kalman filter

Fig. 1의 예측부에서 입력으로 사용되는 자세 회전속도는 글로벌 좌표에서 표현되는 값이다. 그러나 자이로 센서에서 측정되는 회전속도값은 센서 좌표에서 측정되는 값이므로 좌표변환이 필요하다. 자이로 센서값을 자세 회전속도 값으로 변환하기 위해 식(5)의 좌표 변환 행렬 RSG을 사용한다.^7,18

식(3)의 RSG 행렬은 센서값을 센서좌표계에서 글로벌좌표계로 변환해주는 행렬이며 이 행렬의 오일러 회전 순서는 X-Y-Z이다. 위 행렬에서 γ는 오일러각 yaw이며 본 연구에서는 yaw각을 고려하지 않으므로 γ는 0이다.

다음으로 Fig. 1의 측정보상부에서 K^k, x⁀k, H는 각각 칼만 이득(Gain), 추정 벡터, 관측 행렬이며 관측행렬은 식(1)의 관측행렬과 같다. R_v는 측정 노이즈 공분산 행렬이며 z^k 값의 분산을 사용한다. z^k는 관측벡터이며 가속도 센서로부터 계산된 Roll, Pitch 값을 사용한다. 가속도 센서로부터 Roll, Pitch는 각각 다음과 같이 계산한다.¹⁹

위 식에서 ax, ay, az는 각각 가속도 센서의 x, y, z축 방향으로 측정된 가속도값이다.

본 연구에서 비교하는 두 번째 알고리즘은 외부가속도를 레지듀얼을 이용하여 보정하는 칼만필터^10,11이다. 이 칼만필터에 적용된 외부가속도 보정 알고리즘은 외부가속도가 작용하는 방향을 추정하여 해당 방향 측정값의 노이즈 공분산 R_v를 증가시켜 외부가속도의 영향을 감소시키는 방식이다. 본 연구에서 사용한 두 번째 알고리즘의 구조는 2.1절에서 설명된 칼만필터 구조에 외부가속도를 보정하는 알고리즘을 추가한 구조이다. 레지듀얼을 이용한 외부가속도 보정방법은 칼만필터의 레지듀얼 r_a, 즉 측정보상부(Fig. 1 참조)의 (zk-Hx~k) 값으로부터 외부가속도를 예측하여 보정한다. 이를 위하여 먼저 레지듀얼의 공분산 벡터 U_k를 다음과 같이 근사화한다.

위 공분산 벡터 U_k의 고유벡터와 고유값을 각각 μ_i,k, λ_k라 하면 μ_k는 다음과 같이 정의된다.

위에서 계산된 λ_k, μ_k로부터 적응 추정알고리즘은 다음과 같이 도출된다.

식(7)에서 계산된 R_acc를 측정보상부(Fig. 1 참조)의 R_v에 더하여 외부가속도를 보정한다.

본 연구에서 비교하는 세 번째 알고리즘은 모델기반 외부가속도 보정 칼만필터¹²이다. 모델기반 외부가속도 보정 칼만필터는 식(8)처럼 가속도 센서에 작용하는 외부가속도를 가속도 센서 모델에 추가하여 외부가속도를 추정하고 자세 계산 시 제거해주는 방식이다.

식(8)에서 y_A는 가속도 센서의 측정값이며 ^Sg, ^Sa는 각각 센서에서 측정된 중력가속도와 외부가속도, n_A는 측정 노이즈이다. 본 연구에서 사용한 모델기반 외부가속도 보정 칼만필터의 구조는 Fig. 2 와 같다.

Fig. 2 
Overview of external acceleration model-based Kalman filter

여기서 ZSk-은 z축 회전행렬이며 Φk-1, Pk-, Q는 각각 변환행렬, 예측에러 공분산 행렬, 공정잡음 공분산 백터이다. K_k, H, M_k는 각각 칼만 이득, 추정 벡터, 측정잡음 공분산 벡터이며 z_k는 측정벡터이다. y_A, g, ak+는 각각 가속도 센서값, 중력가속도, 외부가속도이다. 본 알고리즘에서 외부 가속도를 보정하는 방법은 다음과 같다. 우선, 가속도 센서에서 측정된 가속도 값 y_A에서 칼만필터로부터 추정된 중력가속도 gSZk+를 제거해주어 외부가속도 ak+를 추정한다. 그리고 추정된 그 외부가속도값을 가속도 센서의 측정값에서 제거해주어 외부가속도를 보정한다. 외부가속도 모델 칼만필터의 각 변수들에 대한 좀 더 자세한 정보는 참조문헌¹²을 참고한다.

본 절에서는 2절에서 설명한 세가지 칼만필터 기반 자세추정 방법의 특성과 성능을 실험을 통하여 조사한다. 먼저, 실험을 위하여 테스트 베드를 Fig. 3와 같이 제작하였다. 실험 방법은 IMU 센서를 테스트 베드에 부착하여 원하는 입력에 따라 Roll(ϕ), Pitch(θ) 방향으로 회전시키다. 이때 IMU 센서로부터 추정된 Roll, Pitch 회전각도와 엔코더로부터 측정된 Roll, Pitch 회전각도를 비교하여 IMU 기반 자세 추정 알고리즘들의 추정 에러를 계산한다. 테스트 베드에서 상대적으로 낮은, 중간, 높은 외부 가속도 효과를 발생시키기 위하여 세가지 서로 다른 회전 주파수(0.3, 3, 6 Hz)를 입력으로 사용하여 Roll, Pitch를 회전시키며 실험하였다. 추가적으로 Roll과 Pitch 방향으로 크기가 변하는 직각파 입력을 사용하여 각 알고리즘의 자세 추정 에러를 비교하였다.

Fig. 3 
Attitude test equipment

IMU 센서를 Roll과 Pitch 방향으로 0.3, 3, 6 Hz 사인파 및 크기가 변하는 직각파를 입력으로 사용하여 회전시켰을 때, 엔코더에서 측정한 실제 회전각을 Fig. 4에서 보여준다. 이 때 Algorithms 1-3의 추정에러를 Figs. 5부터 8에 요약하였다. Figs. 5부터 8은 4개의 서로 다른 외부가속도 입력에 대해 각각의 Roll과 Pitch 방향 추정 에러를 외부가속도의 함수로 보여준다. Figs. 5부터 8에 표시된 에러 기울기(Gradient)와 y절편 (y-Intercept)은 Curve Fitting을 통해 결정되었다. 이 기울기를 통해 각가속도와 추정 에러가 비례관계에 있음을 확인할 수 있으며 y절편을 통해 추정 에러의 편차를 알 수 있다. 그리고 각 그림에 표시된 Sigma 값은 실험 데이터값의 표준편차이다.

Fig. 4 
Actual orientation angles measured by encoders

Fig. 5 
Experimental results: orientation errors as a function of external acceleration (0.3 Hz)

Fig. 6 
Experimental results: orientation errors as a function of external acceleration (3 Hz)

Fig. 7 
Experimental results: orientation errors as a function of external acceleration with a sinusoidal input of 6 Hz

Fig. 8 
Experimental results: orientation errors as a function of external acceleration (step input)

우선 센서를 비교적 낮은 가속도인 0.3 Hz의 주파수를 갖는 사인파 입력으로 회전시킨 경우, Fig. 5, Roll 방향에서는 Curve Fitting의 기울기가 Algorithm 3이 Algorithm 1, 2보다 2배 컸다. Curve Fitting의 y절편과 실험값의 표준편차는 Algorithm 2가 가장 작았다.

Pitch 방향에서는, 기울기, y절편, 표준편차 모두 Algorithm 2가 가장 작았으며 Algorithm 3이 가장 컸다. Algorithm 3은 Algorithm 2보다 기울기, y절편, 표준편차가 각각 580%, 3200%, 210% 컸다.

다음으로 3 Hz 주파수의 사인파 궤적으로 회전시킨 결과, Fig. 6를 보면, Roll 방향에서는 기울기가 Algorithm 2의 경우 Algorithm 1과 3에서 보다 0.001 컸다. y절편은 Algorithm 1의 경우 0.04로 가장 작았으며 Algorithm 3의 절편이 0.171로 가장 컸다. 표준편차는 Algorithm 2에서 0.125로 가장 작았으며 Algorithm 3의 표준편차가 0.19로 가장 컸다. Pitch 방향의 경우는 Algorithm 3의 기울기가 Algorithm 1과 2의 경우보다 0.0001 작았으며 y절편의 경우는 Algorithm 2가 0.04로 가장 작고 Algorithm 3이 0.09로 가장 컸다. 표준편차는 Algorithm 2에서 0.062로 가장 작았다.

다음으로 6 Hz 주파수의 사인파 궤적으로 회전하였을 때의 결과, Fig. 7을 보면, Roll 방향에서는 기울기, y절편, 표준편차 모두 Algorithm 2가 가장 작았으며 Algorithm 1, 3보다 각각 10%, 80%, 35% 작았다. Pitch 방향에서는 기울기가 Algorithm 3에서 0.00025로 가장 작았으며 Algorithm 1의 경우 0.0003으로 가장 컸다. y절편은 Algorithm 1에서 0.027로 가장 작았으며 Algorithm 3에서 0.099로 가장 컸다. 표준편차의 경우는 Algorithm 3이 0.052로 가장 작았다.

다음으로 직각파 궤적으로 회전시켰을 때의 실험 결과, Fig. 8를 보면, Roll 방향에서는 기울기, y절편, 표준편차 모두 Algorithm 2가 가장 작았다. 기울기와 표준편차는 Algorithm 3이 가장 컸으며 Algorithm 2와의 차이는 각각 1712%, 292%이다. y절편은 Algorithm 1이 가장 컸으며 Algorithm 2와의 차이는 4642%이다. Pitch 방향에서는 기울기가 Algorithm 3의 0.00018이 가장 작았으며 Algorithm 2의 0.000312가 가장 컸다. y절편은 Algorithm 3의 -0.000143이 가장 작았으며 표준편차는 Algorithm 2의 0.078이 가장 작았다.

네 가지 서로 다른 외부가속도 시나리오를 적용한 실험 결과로부터 Curve Fitting의 기울기는 Roll 방향에서 1번, Pitch 방향에서는 2번을 제외하고 Algorithm 2가 가장 작았다. 참고로 직각파 형태의 외부가속도 시나리오를 적용한 경우 Pitch 방향에서 Algorithm 3의 기울기가 Algorithm 2보다 작았지만 표준편차가 4배 이상 차이가나 이 결과를 제외하였다. y절편은 Roll, Pitch 방향에서 각각 1번씩을 제외하고는 Algorithm 2가 가장 작았다. 표준편차도 Roll, Pitch 방향에서 각각 1번씩을 제외하고는 Algorithm 2가 가장 작았다.