1. 서 론
수중에서의 위치 추정에는 수상에서와는 달리 특징점(Landmark 또는 Feature)이 제한적으로 사용된다. 이러한 이유로 수중에서 위치 추정 시, 특징점을 부분적으로 이용하며 속도와 자세를 사용하는 추측항법(Dead reckoning)이 가장 많이 사용된다(Ko and Kuc, 2015; Ko et al, 2015). 추측항법을 이용한 위치 추정은 자세 정보의 영향을 크게 받기 때문에 자세 추정 방법이 중요한 요소로 작용된다(Ko et al., 2016a; Ko and Jeong, 2016).
자세는 가속도와 자기장을 측정하여 검출가능하며, 자세 추정 방법은 계산된 자세를 사용하여 추정 상태변수를 보정한다. 자기장 값은 참값에 편차가 추가되어 측정된다. 편차가 포함된 자기장 측정값을 사용하여 계산된 yaw값이 보정단계에서 사용된다. 그러므로 좀 더 정확한 자세추정을 위하여 측정된 자기장에서 편차를 보정한 후 yaw각을 계산하여야 한다(Ko et al., 2016c). yaw각의 측정 오차가 적어지면 추정되는 Yaw각의 오차도 줄어들게 되고, 이에 따라 추정되는 Roll과 Pitch값의 정확성은 향상된다. 즉, 편차 추정을 통하여 좋은 성능을 가진 자세 추정이 가능하게 되고, 이는 위치 추정의 성능을 향상시키는데 도움이 된다(Ko et al., 2016b).
사용하는 센서들 중 AHRS(Attitude and heading reference system)의 자기장 센서는 가벼운 무게, 높은 신뢰성, 낮은 전력을 요구한다. 그러나 자기장 센서가 지구자기장을 정확하게 측정하지 못한다(Kim et al, 2006). 원인으로는 Hard iron 효과, Soft iron 효과, 환산계수오차(scale factor error), 측정 축 정렬오차들이 있으며 본 연구에서는 그중에서 가장 큰 원인으로 작용하는 Hard iron 효과에 의해 발생하는 편차를 추정한다.
기존의 편차 추정 방법은 Twostep, Centering, 최소자승법, 확장 칼만 필터(Extended Kalman filter)들이 있다(Troni and Whitcomb, 2013). Twostep 방법은 측정의 가중치 평균을 기반으로 중심 편차 추정을 먼저 계산함으로써 편차를 결정한다(Hashmall and Deutschmann, 1996). 본 연구에서는 기존의 방법 대신 무향 칼만 필터(Unscented Kalman filter, UKF)를 사용하여 편차를 추정하였다.
본 논문의 2장에서는 무향 칼만 필터를 이용한 편차 추정 방법에 대하여 서술한다. 3장에서는 실제 실험 정보를 이용한 편차 추정 결과를 통하여 성능을 검증하고, 4장에서는 본 논문의 결론을 맺는다.
2. 본 론
2.1 추정을 위한 상태천이 모델과 측정모델 유도
본 절은 추정을 위한 도구들 중에서 상태천이 모델과 측정 모델에 대한 공식을 유도한다(Troni and Whitcomb, 2013). m은 센서에 의해 측정된 자기장 벡터이며 m(t) = [mx(t) my(t) mz(t)]T로 표기된다. 본 논문에서는 특별히 혼동의 염려가 없는 경우 시간에 관한 Parameter t를 생략하여 사용한다. b는 자기장 측정값의 편차를 나타내며 b = [bx by bz]T로 표기된다. w는 센서에 의해 측정된 회전 각속도 벡터이며 w = [wx wy wz]T로 표기된다.
식 (1)에서 센서에 의해 측정된 자기장 벡터 값 m(t)은 참값 
에 편차 b가 더해져 나타난다. 이는 센서좌표계에서 사용되는 식이며 식 (2)는 고정좌표계에서 나타내어진 자기장 벡터의 참값 m0을 구하는 과정이다. 여기서 R(t)은 센서좌표계에서 고정좌표계로의 회전행렬이다. 이를 통해 센서에 의해 측정된 자기장 벡터에서 편차를 빼준 결과에 회전행렬을 곱해줌으로써 고정좌표계에서 나타내어진 자기장 벡터의 참값을 구한다.
에 편차 b가 더해져 나타난다. 이는 센서좌표계에서 사용되는 식이며 식 (2)는 고정좌표계에서 나타내어진 자기장 벡터의 참값 m0을 구하는 과정이다. 여기서 R(t)은 센서좌표계에서 고정좌표계로의 회전행렬이다. 이를 통해 센서에 의해 측정된 자기장 벡터에서 편차를 빼준 결과에 회전행렬을 곱해줌으로써 고정좌표계에서 나타내어진 자기장 벡터의 참값을 구한다.식 (2)를 미분하면 식 (3)이 된다.
회전행렬을 미분한 
은 식 (4)처럼 표현된다.
은 식 (4)처럼 표현된다.
여기서 S(w)은 반대칭 행렬(Skew symmetric matrix)이며 측정된 회전 각속도 w요소를 사용하여 구성된 행렬이다.
식 (4)를 사용하면, 식 (3)이 식 (6)으로 변경된다.
편차는 시간에 따라 변하지 않는다고 가정하고, 식 (6)을 이용하면 식 (7)과 같은 상태천이 모델이 구해진다.
본 연구에서는 자기장을 측정하여 자기장 편차를 추정하므로, 측정변수 z는 z = m이 된다. 따라서 측정모델은 식 (8)과 같이 구해진다.
최종적으로 상태천이 모델을 식 (9)로 표현하며 측정모델은 식 (10)으로 표현된다.
2.2 무향 칼만 필터 편차 추정
본 절은 무향 칼만 필터를 통해 편차를 추정하는 방법에 대해 서술한다(Thrun et al, 2006). 무향 칼만 필터를 통해 편차 추정하는 알고리즘에서 추정 상태변수는 x = [mx my mz bx by bz]T로 자기장 벡터와 자기장에 대한 편차로 구성된다. 추정 상태변수에서 m = [mx my mz]T에 대한 초기 값은 AHRS에서 측정된 자기장을 사용한다. w는 로봇의 회전 각속도를 나타내며 w = [wx wy wz]T이다. z는 측정된 자기장 벡터인 z = [mx my mz]T이다. Table 1에 무향 칼만 필터를 이용한 편차 추정 방법을 의사코드로 나타내었다. 무향 칼만 필터는 t시각의 상태변수와 그에 대한 오차공분산을 출력하는 방법으로서, t − 1시각의 상태변수와 그에 대한 오차공분산, t시각의 회전각속도 벡터와 그 시각의 측정값을 이용한다.
1번 줄은 시그마 포인트(Sigma point)의 초기 요소들의 구성을 결정하는 단계이다. 
는 시각 t − 1의 추정 상태변수를 뜻한다. 2번 줄의 
는 추정된 상태변수 μt − 1에 대한 오차 공분산 Σt − 1과 예측과정에 대한 오차 공분산 Mt, 그리고 측정값에 대한 오차 공분산 Qt로 이루어져있다. 회전각속도 w는 평균값이 0이고 공분산이 Mt인 가우시안 확률변수(w ~ N(0, Mt)) 이며, 측정값 z는 평균값이 0이고 공분산이 Qt인 가우시안 확률변수(z ~ N(0, Qt))이다.
는 시각 t − 1의 추정 상태변수를 뜻한다. 2번 줄의 는 추정된 상태변수 μt − 1에 대한 오차 공분산 Σt − 1과 예측과정에 대한 오차 공분산 Mt, 그리고 측정값에 대한 오차 공분산 Qt로 이루어져있다. 회전각속도 w는 평균값이 0이고 공분산이 Mt인 가우시안 확률변수(w ~ N(0, Mt)) 이며, 측정값 z는 평균값이 0이고 공분산이 Qt인 가우시안 확률변수(z ~ N(0, Qt))이다.3번 줄은 시그마 포인트를 생성하는 단계이다. 
의 요소 개수를 n이라고 하면 2n − 1개 시그마 포인트가 생성된다. 1열의 는 시그마 포인트 초기 정보이다. 
는 2번 줄에서 구한 을 Cholesky 분해한 결과이다. γ는 다음 식을 통해 구해진다(Rhudy and Gu, 2013).
의 요소 개수를 n이라고 하면 2n − 1개 시그마 포인트가 생성된다. 1열의 는 시그마 포인트 초기 정보이다. 는 2번 줄에서 구한 을 Cholesky 분해한 결과이다. γ는 다음 식을 통해 구해진다(Rhudy and Gu, 2013).
식 (11)에서 α는 10−4 ∼ 1까지 범위의 수이며 κ는 0에 근접한 수를 사용한다. α, κ는 시그마 포인트의 범위를 조절하는데 사용된다. 3번 줄을 통해 μt − 1를 기준으로 분포된 시그마 포인트가 생성된다.
4~6번 줄까지는 상태에 대한 시그마 포인트들을 구하고 그들의 평균과 평균에 대한 오차공분산을 구하는 과정이다. 4번 줄은 모션모델을 적용하여 상태에 대한 시그마 포인트들을 구하는 과정이다. 여기서 ut는 회전각속도의 벡터를 의미한다. 
는 3번 줄에서 생성된 회전각속도에 대한 시그마 포인트를 뜻한다. 식 (9)인 상태천이 모델을 4번 줄에 적용한다.
는 3번 줄에서 생성된 회전각속도에 대한 시그마 포인트를 뜻한다. 식 (9)인 상태천이 모델을 4번 줄에 적용한다.
식 (13)에서 
는 시그마 포인트에서 추정 상태변수만으로 구성된다. Δt는 t시각과 t − 1시각의 차이를 뜻한다. 식 (14)에서 A(t)는 식 (5)에서 사용한 반대칭 행렬 S(w)로 구성된 행렬이며 S(w)는 회전 각속도 벡터 ut로 구성된다.
는 시그마 포인트에서 추정 상태변수만으로 구성된다. Δt는 t시각과 t − 1시각의 차이를 뜻한다. 식 (14)에서 A(t)는 식 (5)에서 사용한 반대칭 행렬 S(w)로 구성된 행렬이며 S(w)는 회전 각속도 벡터 ut로 구성된다.
5번 줄은 4번 줄에서 예측된 상태에 대한 시그마 포인트들의 평균을 구하는 과정이다. 예측된 상태에 대한 시그마 포인트에 가중치 (
)을 통해 평균을 구한다.
)을 통해 평균을 구한다.
6번 줄은 5번 줄에서 구한 
에 대한 오차공분산을 구한다. 오차 공분산을 구할 때 사용되는 가중치 (
)는 다음과 같다.
에 대한 오차공분산을 구한다. 오차 공분산을 구할 때 사용되는 가중치 ()는 다음과 같다.
식 (17)에서 β는 2에 근접한 값을 사용한다. 식 (18)에서 과 는 i = 1, 2, 3, ···, 2n에 대하여 동일하다. 본 논문에서 추정 상태변수 의 요소는 6개이며 
는 6×6 크기의 행렬이 된다.
과 는 i = 1, 2, 3, ···, 2n에 대하여 동일하다. 본 논문에서 추정 상태변수 의 요소는 6개이며 는 6×6 크기의 행렬이 된다.7~9번 줄은 측정값에 대한 시그마 포인트들을 구하고 그들의 평균과 평균에 대한 오차공분산을 구하는 과정이다. 7번 줄은 측정 모델을 사용하여 측정값에 대한 시그마 포인트를 구하는 단계이다.
식 (19)은 측정모델인 식 (10)을 적용한 결과이다. 여기서 H = [I 0]이므로 예측된 상태에 대한 시그마 포인트 
에서 자기장 벡터만을 사용하게 되어 측정값에 대한 시그마 포인트들이 생성된다. 8번 줄은 측정값에 대한 시그마 포인트들의 평균을 구한다. 9번 줄은 8번 줄의 측정값에 대한 시그마 포인트의 평균에 대한 오차공분산을 구한다. 8,9줄 모두 5,6줄과 동일한 방법으로 구해지며 측정값으로 사용되는 자기장 벡터는 3개이므로 
는 3×3 행렬이다.
에서 자기장 벡터만을 사용하게 되어 측정값에 대한 시그마 포인트들이 생성된다. 8번 줄은 측정값에 대한 시그마 포인트들의 평균을 구한다. 9번 줄은 8번 줄의 측정값에 대한 시그마 포인트의 평균에 대한 오차공분산을 구한다. 8,9줄 모두 5,6줄과 동일한 방법으로 구해지며 측정값으로 사용되는 자기장 벡터는 3개이므로 는 3×3 행렬이다.10번 줄은 추정한 상태변수와 측정값의 시그마 포인트에 대한 오차 공분산을 구하는 과정이다. 는 6×6 행렬이며 는 3×3 행렬이므로 연산의 결과는 
는 6×3 행렬이 된다. 11번 줄은 칼만 이득을 구하는 과정이다.
는 6×6 행렬이며 는 3×3 행렬이므로 연산의 결과는 는 6×3 행렬이 된다. 11번 줄은 칼만 이득을 구하는 과정이다.12번 줄은 시각 t애서의 상태변수 μt을 구하는 과정이다. 예측한 상태변수에 칼만 이득을 사용하여 보정함으로써 최종적으로 시각 t에서의 상태가 추정된다. 이 과정에서 구한 결과 μt는 다음 시각의 추정 알고리즘에서 μt − 1로 사용된다. 13번 줄은 시각 t에서의 Σt를 구하는 과정이다. 오차 공분산 Σt는 다음 시각의 추정 알고리즘에서 Σt − 1로 사용된다.
추가적으로 성능 비교를 위해 추정한 편차를 보정한 자기장 벡터와 편차를 보정하지 않는 자기장 벡터를 사용하여 자세를 추정한다. AHRS센서 측정값을 사용하며, 추정 방법으로는 무향 칼만 필터를 사용한다. 추정 상태변수는 [ϕ, θ, ψ]T 이며 측정 정보는 가속도 a = [ax, ay,az]T와 편차가 보정된 자기장 
를 사용한다. 마지막으로 추정한 자세 정보와 DVL(Doppler velocity log)센서 정보를 사용하여 추측항법을 사용하여 위치를 추정한다.
를 사용한다. 마지막으로 추정한 자세 정보와 DVL(Doppler velocity log)센서 정보를 사용하여 추측항법을 사용하여 위치를 추정한다.3. 실 험
3.1 실험 환경 및 조건
본 실험은 선박해양플랜트연구소에 설치된 수조에서 실행되었다. Fig. 1에 보인바와 같이 ‘ㄹ’모양의 운항경로로 수중로봇을 이동시키며 AHRS와 DVL을 사용하여 측정값을 구하였다. 운항경로는 1.4m×2.2m의 크기를 가진다. 다양한 궤적의 운항으로 실험했지만 그중에서 ‘ㄹ’모양은 작은 공간에서 이동거리가 길며, 직선운동을 하면서 다양한 방향으로 회전이 가능하기 때문에 다른 이동경로보다 큰 변화량을 가진다.
3.2 실험 결과
Fig. 2에서 (a)∼(c)는 시각에 따른 x축, y축, z축 방향의 자기 장 벡터를 표현하고 있으며 파랑색 선은 각 축방향의 자기장 벡터를 뜻하며 빨강색 점선은 추정한 편차성분을 뜻한다. 마지막으로 녹색 선은 편차의 평균을 뜻한다. Fig. 2에서 (d)는 mx와 my를 동시에 표현하였다. 자기장이 왜곡 되지 않았을 경우에는 (0, 0)지점을 기준으로 하는 원형이 그려 져야한다. 하지만, 본 실험에서는 Hard iron 효과와 Soft iron 효과에 의해 자기장이 왜곡되어 중심점이 (0, 0)에서 벗어나고 원이 아닌 타원 모양을 보여준다. 이러한 측정 결과로부터 본 실험에서 사용되는 자기장 측정값에 편차 요소가 존재함을 알 수 있다.
본 실험은 Roll, Pitch 각이 (0, 0)각도에서 크게 벗어나지 않기 때문에 Table 4의 결과 값 중 bmx 와 bmy는 이론상 문제가 없다. 하지만, 자기장 z정보는 일정한 값으로 출력이 되기 때문에 실험에서 측정된 벡터로는 편차를 추정할 수 없다. 그러므로 편차의 z요소를 구하기 위해서는 실험에서 측정된 자기장 벡터와 World magnet model(WMM)에서 구한 자기장 벡터를 비교한다(NOAA, 2016).
Table 5에서 자기장 z축의 편차를 구하기 위해 실험에서 측정한 자기장 z축의 편차와 WMM에서 구한 자기장 z축의 편차를 연산한다. 그 결과 값은 Table 5에 마지막 Bias_z로 기제 되어있다. 마지막으로 실험에서 측정된 자기장 벡터에서 추정한 편차를 제거한 후 자세를 추정한다. 자세 추정 방법 또한 UKF방법을 사용한다.
Table 5
Comparison of world magnetic model (WMM) and the measured magnetic field, and bias in z directional magnetic field
Fig. 4에서 (a)와 (b)는 Roll, Pitch요소를 추정한 결과이다. 본 실험은 실제 바다가 아닌 만들어진 수조에서 실시되며 수중 로봇이 직선으로 운행했기 때문에 Roll과 Pitch의 변화량이 적다. Fig. 4에서 (c)는 Yaw요소를 추정한 결과이다. 수중로봇이 동일한 깊이에서 이동하였으므로 변화량이 가장 큰 요소는 Yaw이며 −180∼180사이의 변화량을 가지고 있다. 그래프 중에서 빨간색 원은 수중 로봇이 ‘ㄹ’모양의 경로로 움직이기 시작한 지점을 뜻한다. Fig. 4에서 (d)는 수중 로봇의 위치를 추정한 결과이다. 추정된 자세와 실험에서 DVL에 의해 측정된 속도를 사용하여 추측항법에 의해 로봇의 위치를 추정하였다. 총 7번의 ‘ㄹ’ 모양의 경로를 왕복 운항한 실험이며 운항의 최종점은 운항의 시작점과 동일하다.
Fig. 5에서 (a)는 편차가 보정된 자기장 벡터를 사용하여 위치를 추정한 결과이며 Fig. 5에서 (b)는 편차가 보정되지 않는 자기장 벡터를 사용하여 위치를 추정한 결과이다. 성능비교 결과는 Table 6과 같다. Table 6은 ‘ㄹ’모양의 경로로 운항하기 시작한 지점과 마지막 운항이 끝나는 지점사이의 거리를 구한다. 거리를 비교함으로써 편차가 보정된 자기장 벡터를 사용한 위치 추정결과가 편차가 보정되지 않은 자기장 벡터를 사용한 위치 추정결과보다 좋은 성능을 보인다.
4. 결 론
AHRS의 자기장 측정값은 Hard iron 효과와 Soft iron 효과로 인한 왜곡에 취약하다는 문제를 가지고 있다. 본 논문에서는 이러한 문제를 해소하기 위해 Hard iron 효과로 인한 편차를 추정하는 방법을 제안하고, 추정된 편차 보정에 따라 항법성능이 향상됨을 보였다. 무향 칼만 필터를 사용하여 자기장 편차를 추정하였다. 편차추정성능을 확인하기 위해서 편차가 보정된 자기장의 벡터를 사용한 자세추정과 편차가 보정되지 않는 자기장 벡터를 사용한 자세추정을 비교하였다. 편차를 보정한 자기장을 사용할 경우, 편차를 보정하지 않는 자기장을 사용할 경우보다 자세추정 성능이 향상됨을 실험을 통하여 확인했다.
자기장 측정 오차는 Hard iron 효과뿐 아니라 Soft iron 효과, 환산계수오차, 측정 축 정렬오차 등에 의해서 발생한다. 따라서 향후 연구에서는 Hard iron 효과 이외의 요소들을 추정하는 방법을 연구할 예정이다.
PDF Links
PubReader
ePub Link
Full text via DOI
Download Citation
Print
