2.1.1 오일러 적분을 위한 회전 행렬 정의

본 연구에서 사용되는 오일러 각은 롤($\phi$), 피치($\theta$), 요($\psi$)로 구성되어 있으며, 롤은 차량의 진행 방향($x$축)에 대한 회전 각을 나타내고, 피치는 차량의 진행 방향의 오른쪽($y$축)을, 요는 지면을 향하는 방향($z$축)을 기준으로 한 회전 각을 나타낸다. 3축 자이로 센서로부터 측정되는 각속도 정보를 오일러 각에 대한 프레임 기준으로 나타내기 위해서 회전 행렬(rotation matrix)을 사용하는데, 여기서는 요-피치-롤 순서로 회전시키도록 하였다^[15]. 아래 식은 오일러 각속도($\dot \theta$)와 3축 각속도 센서 신호($ \omega$)와의 관계를 나타낸다.

2.1.2 이산 시간 자세 계산

식 (1)에 나타난 연속 시간에 대한 각도 함수를 이산 시간으로 나타내기 위해 아래와 같이 테일러 급수 전개(Taylor series expansion)를 통해 2차 항까지 근사한다.

식 (1)을 시간에 대해 미분하면 아래와 같은 편미분 식을 얻을 수 있다.

식 (6)으로부터 아래와 같이 $V_{B}(\theta ,\:\omega)$를 정의하고 $\dot\omega$을 근사한다.

식 (1), 식 (6), 식 (7), 식 (8)을 식 (5)에 대입하면 다음과 같이 2차에서 근사된 자세 각을 점화식 형태로 표현할 수 있다.

2.1.3 칼만 필터 상태 변수 정의

식 (9)에서 얻은 자세 각 근사식을 이용하여 각도의 오차와 자이로 바이어스로 구성되는 간접 칼만 필터의 상태 변수를 다음과 같이 정의한다.

여기서 $A$와 $B$는 3x3 행렬로써 각각 시간 $t$에서 주어진 각도의 오차($\delta \theta(t)$)와 각속도 오차($\delta \omega(t)$)가 시간 $t+\triangle t$ 에서의 상태 변수에 미치는 영향에 대한 관계로써 식 (11)에 정리되어 있다. 그리고 $0$과 $I$는 각각 3x3 영행렬과 단위행렬을 의미한다.

식 (10)으로부터 간접 칼만 필터의 상태 변수($z_{k}$)를 아래와 같이 각도의 오차($x_{k}=\delta \theta(t_{k})$)와 자이로 바이어스($b_{k}=\delta \omega(t_{k})$)로 구성되도록 정의한다.

식 (12)에서 정의한 상태 변수를 식 (10)에 적용하면 아래와 같이 칼만 필터 식을 표현할 수 있다.

여기서 $F_{k}$는 상태 변수 전이 행렬(state transition matrix)이고, $w_{k}$는 시스템 노이즈를 나타낸다. 칼만 필터의 출력($y_{k}$)은 아래와 같다.

여기서 $L_{k}$은 상태변수 $z_{k}$로부터 각도의 오차를 나타내기 위한 행렬로써 3x6의 크기를 가진다. $v_{k}$는 센서 측정값에서 발생하는 화이트 노이즈를 의미한다.

2.1.4 상태 변수 업데이트

간접 칼만 필터의 상태 변수는 측정값과 추정값의 오차를 기반으로 업데이트 된다. 3축 가속도 정보를 통해 아래와 같이 오일러 각 측정값을 계산한다.

여기서 $a_{x},\: a_{y},\: a_{z}$는 각각 $x,\: y,\: z$축에서의 가속도를 의미한다. 그리고 요 각 측정값($\psi_{m}$)의 경우에는 가속도 정보로부터 구할 수 없기 때문에 별도의 지자기 센서(magnetometer)를 사용하여야 하는데, 본 연구에서는 롤과 피치 각의 추정에 대해서만 고려하고, 요 각은 생략하였다.

식 (15), 식 (16)에서 계산된 가속도 센서의 각도는 간접 칼만 필터의 상태 변수($z_{k}$)를 업데이트하기 위한 측정값 (measurement, $y_{k+1}$)으로 사용된다. 측정값과 상태 변수 업데이트 식은 각각 아래와 같다.

여기서 $K_{k}$는 칼만 게인으로써 센서로부터 측정된 값($y_{k}$)과 추정된 값의 오차가 새로운 상태 변수에 업데이트되는 양을 결정한다. 칼만 게인은 아래 식과 같이 계산되는데, 여기서 R 값은 측정값의 노이즈를 조절하기 위한 게인이다.

위 식에서 나타난 $P$는 추정 오차에 대한 공분산 행렬(error covariance matrix)로써 아래와 같이 계산된다.

2.4.1 횡 가속도 차단 실험

횡 가속도 차단 실험은 본 알고리즘이 정지 상태 여부를 구분하여 움직임이 없을 때 가속도 센서로부터 측정된 자세 각을 반영하고, 움직임이 있을 때 가속도 신호를 억제하는지 검증하기 위한 실험이다. 횡 가속도 차단 실험 절차는 다음과 같다. 먼저 AHRS에 전원 인가 후 약 수 초간 대기하여 자세 각을 초기화 시켜주고, 가속도를 수평 방향으로 작용시킨다. 그림. 6은 횡 가속도(점선)와 롤 각(실선)의 결과를 나타낸 그래프이다. 결과를 살펴보면 약 11~14초 구간에서 횡 가속도가 인가되었음에도 롤 각은 횡 가속도의 영향을 받지 않은 것을 확인할 수 있다. 이 결과로부터 본 알고리즘은 횡 가속도 영향을 효과적으로 차단하는 것을 알 수 있다.

2.4.2 3축 로봇 시뮬레이터 실험

AHRS에서 추정한 자세의 정확도를 검증하기 위해 그림. 7의 3축 로봇 시뮬레이터를 이용하여 실험을 수행하였다. 이 실험에서는 AHRS를 로봇 시뮬레이터 위에 고정시키고 약 90초 동안 사인파 형태의 움직임을 인가하였다. 그림. 8은 이 실험에서 나타난 3축 결과 중 피치 각도에 대한 결과와 오차를 나타낸다. 결과를 살펴보면 오차는 약 $+-0.5^{\circ}$수준인 것을 확인할 수 있다. 여기서 측정된 시뮬레이터의 각도는 스텝 모터에 인가되는 각도의 명령을 나타낸다. 따라서 명령과 응답 값 사이에 약간의 시간 지연이 포함되어 있으며 이 점을 고려하면 실제 오차는 이보다 작을 것으로 판단할 수 있다.

2.4.3 차량 주행 실험

개발된 시스템의 성능을 검증하기 위하여 승용차에 시스템을 탑재하고 도로 주행 시험을 수행하였다. 차량 주행 실험에서는 차량의 실제 자세 각을 알 수 없기 때문에 상용 INS(Inertial Navigation System)를 함께 구성하여 결과를 참조하는데 사용하였다. 실험에 사용된 INS는 Advanced Navigation 社에서 제작한 Spacial Dual이다.

그림. 9에서는 차량 주행 실험 중 측정한 Spacial Dual 데이터(Reference)와 [8]^[8]에서 제시된 $R_{k}$ 곡선 모델을 사용한 결과(Original), 그리고 본 연구에서 개발한 $R_{k}$ 곡선 모델을 사용한 결과(Ours)에 대한 롤 및 피치 각도를 비교하였다. 차량 주행 실험은 일반 도로에서 약 12분간 이어졌으며 약 240초에서 255초 구간 동안 차량이 잠시 정차하였다. 그림을 살펴보면 정지 상태 구간에서 Reference 값과 Original 및 Ours 값에 오차가 많이 나타나는데, 그 이유는 간접 칼만 필터 알고리즘이 정지 상태 동안 가속도 센서로부터 측정된 각도에 맞추어 오차가 보정 되었지만 Reference 센서의 경우 각도 보정이 거의 이루어지지 않았거나(Roll), 비교적 느리게 보정(Pitch)되고 있었기 때문이다. 그림. 10을 살펴보면 붉은색으로 표시된 그래프가 식 (15), 식 (16)으로부터 계산된 가속도 센서의 각도를 의미하는데, 본 연구에서 제안한 알고리즘의 경우 정지 상태 구간에서 롤 및 피치 각도가 가속도 센서의 각도로 신속히 수렴하는 것을 확인할 수 있다. $R_{k}$ 곡선 모델의 형태에 따른 각도의 오차 비교는 그림. 10과 표 1에 정리되어 있다. 오차 결과를 살펴보면 우리가 제안한 곡선 모델을 사용할 경우 각도 오차의 평균과 표준편차가 모두 감소하는 것을 확인할 수 있다. 한편, 자이로 센서의 경우 주변 환경의 온도에 따라 바이어스 특성이 변하기 때문에 온도 정보도 함께 고려^[16]해주어야 하지만, 본 연구에서는 온도에 따른 특성은 배제하였다.