로봇의 회전을 나타내는 각도는 Fig. 1에 나타낸 바와 같이 로봇의 Yaw 방향을 θ, Pitch 방향을 ϕ로 정의할 수 있다. 일반적인 도로는 Roll 방향 각도 변화가 크지 않아 로봇의 위치추정에 별다른 영향이 없으므로 로봇의 상태 변수는 정의된 각도 θ, ϕ와 로봇의 3차원 위치를 나타 내는 x, y, z를 포함하여 다음 식(1)과 같이 정의한다.^14-16

Fig. 1 
Angular coordinate system

시간 k에서 k+1로 변화할 때 로봇의 위치를 나타내 는 시스템 모델은 Fig. 2와 같은 관계에 의해 다음 식(2)과 같이 표현된다.^15-17

Fig. 2 
System modeling

여기서 X(k+1)는 시간 k+1에서 로봇의 위치, X(k)는 시간 k에서 로봇의 위치를 나타낸다. F(X(k), u(k))는 상태천이함수이며, v(k)는 평균이 0이고 공분산이 Q(k)인 로봇의 이동과 관련된 가우시안 잡음이다. u(k)는 로봇의 이동거리 d(k)와 방향각 증분 Δθ(k) 및 경사각 증분 Δϕ(k)로 이루어지는 제어입력으로 다음 식(3)과 같다.

상태천이함수 F(X(k), u(k))는 시간 k에서 로봇의 위치 X(k)와 제어입력 u(k)를 이용하여 식(4)로 표현된다.

DGPS와 디지털컴퍼스의 측정값을 로봇의 위치와 연관 시키는 측정 모델은 다음 식(5)과 같이 정의한다.

여기서 ω(k+1)는 평균이 0, 공분산이 R(k+1)인 DGPS와 디지털 컴퍼스의 측정과 관련된 가우시안 잡음 이다. 위치 측정에 사용된 센서인 DGPS와 디지털 컴퍼스는 각각 고유한 측정 메커니즘을 갖고 있으며, 각 센서의 위치정보 발생과정은 로봇의 위치를 제외한 다른 특별한 관계가 존재하지 않는다. 따라서 상태천이함수 H(X(k+1))는 다음 식(6)과 같이 표현된다.

여기서 x_GPS(k+1), y_GPS(k+1), z_GPS(k+1)은 DGPS의 측정값이고 θ_DC(k+1)은 디지털컴퍼스의 측정값이다. ϕ_C(k+1)은 시간 k에서 k+1까지 DGPS 측정값의 변화량으로부터 계산된 로봇의 Pitch 방향 성분을 나타 내며 다음과 같이 계산된다.

여기서 Δx_GPS, Δy_GPS, Δz_GPS은 시간 k에서 k+1까지 DGPS의 x, y, y 방향 변화량을 나타낸다.

2.1절과 같이 정의된 시스템 모델과 측정 모델을 EKF 알고리즘에 적용하여 이동로봇의 위치를 평가한다. EKF 위치평가 알고리즘은 이전 위치를 기반으로 다음 위치를 계산하는 예측 단계와 예측된 값의 신뢰성을 판단하기 위한 유효성 평가 단계, 그리고 위치 갱신 단계로 나누어 진다. 먼저 시간 k에서 로봇의 위치 X(k)와 제어입력 u(k)를 상태천이함수를 이용하여 시간 k+1에서 로봇의 위치 X^k+1k를 다음 식(8)과 같이 예측한다.

이 예측에 수반되는 공분산 행렬 P(k+1|k)은 식(9)와 같이 계산된다.

여기서 ∇F는 상태천이함수 식(4)의 자코비안으로 다음 식(10)과 같다.

다음으로 예측 측정값 Z^k+1은 예측된 위치 X^k+1k를 이용하여 다음 식(11)으로 계산한다.

센서로부터 측정된 실측값 Z(k+1)와 예측된 측정값 Z^k+1의 차로 구성되는 Innovation 행렬 v(k+1)은 다음 식(12)과 같이 계산된다.

이 Innovation 행렬에 대한 공분산 행렬 S(k+1)은 다음 식(13)과 같이 계산한다.

여기서 ∇H는 상태천이함수 식(6)의 자코비안으로 식(14)와 같이 표현된다.

여기서 r은 x, y, z 방향 변화량의 대각선 값으로서 다음 식(18)과 같다.

비 관성센서에 의해 측정된 위치는 여러 가지 오차 요인에 의해 그 신뢰성이 크게 떨어질 수 있다. 따라서 측정된 위치정보의 신뢰성을 평가하는 유효성 평가를 수행하며 이를 위한 Validation Gate G(k+1)을 다음의 식(19)와 같이 계산한다.

G(k+1)은 실측값과 예측값의 차와 측정오차의 비를 나타내는데, G(k+1)이 γ² 이하일 때만 측정된 위치 정보가 위치보정에 사용될 정도로 신뢰성이 있다고 판단 한다. γ는 설계파라메타이며 유효성 평가를 통과하지 못할 경우 최종 위치와 공분산은 추측항법으로 예측된 값이 된다.

예측된 위치와 공분산에 가중치를 주기 위한 칼만 이득 W(k+1)은 다음 식(20)과 같이 계산한다.

최종적으로 계산된 칼만 이득 W(k+1)을 이용하여 다음 식(21), 식(22)과 같이 위치 및 공분산을 갱신한다.

개발된 3차원 위치평가 방법의 성능을 평가하기 위해 Fig. 3과 같은 환경에서 실험을 수행하였다. 주행거리는 400 m정도이고 고도차가 25 m정도인 내리막길과 오르막길이 존재하는 환경이다. 그림의 X 표시는 기준 경로를 계산하기 위한 기준위치로서 로봇이 주행한 이동 경로 내의 모든 위치에서의 실제 좌표는 측정이 어렵기 때문에 사전에 15 곳의 고도정보를 포함한 실제 좌표를 측정한 후 이것을 연결하여 기준경로로 설정하였다.

Fig. 3 
Experimental environment

실험은 리모컨을 이용하여 로봇이 기준경로를 따라 주행시킨 후 기준경로에 대한 DGPS 및 위치평가 결과의 위치오차를 비교 분석하는 방법으로 수행되었다. 로봇은 평균 0.6 m/s의 일정한 속력으로 주행하도록 하였다.

Fig. 4는 실험에 사용된 로봇과 장착된 센서들의 사진으로서 ⓐ는 경사센서, ⓑ는 DGPS, ⓒ는 디지털 컴퍼스를 나타낸다. 로봇 플랫폼은 Adept Technology 사의 Pioneer-3DX이며 그 사양은 Table 1과 같다. 바퀴에는 로봇의 이동정보를 획득할 수 있는 엔코더가 부착되어 있으며, 경사센서는 Level Development 사의 LCH-360-USB, DGPS는 AscenKorea 사의 RCV-3000, 그리고 디지털 컴퍼스는 WithRobot 사의 myAHRS+를 사용하였으며 그 사양은 Table 2와 같다.

Fig. 4 
Photo of the robot and sensors(ⓐ: Inclinometer, ⓑ: DGPS, ⓒ: Digital compass)

Fig. 5는 DGPS 및 개발된 3차원 위치평가 방법의 결과로서 점선은 DGPS 경로, 실선은 3차원 위치평가 결과, X 표시가 포함된 가는 실선은 기준경로를 나타낸다. 그림에서 알 수 있듯이 DGPS는 기준경로와 비교적 큰 차이를 보이며 로봇이 기준경로를 따라 직선적으로 이동 했음에도 불구하고 평탄하지 못한 결과를 보여주고 있다. 3차원 위치평가 방법은 이와 같은 DGPS의 오차를 보정 하고 이동경로를 어느 정도 평탄하게 하여 기준 경로와 가까운 결과를 보여주고 있다.

Fig. 5 
Results of the 3D localization

위치평가의 정량적인 성능을 확인하기 위해 각각의 위치오차를 Fig. 6에 나타냈으며, 점선은 DGPS의 위치오차, 실선은 3차원 위치평가의 위치오차이고 이에 대한 위치 오차 특성은 Table 3에 나타냈다. 오차특성을 분석 해보면 DGPS는 최대 6.4 m, 평균 3.9 m의 위치오차를 보이고 있는 반면에 3차원 위치평가는 최대 2.3 m, 평균 0.9 m의 비교적 낮은 위치오차를 보여주고 있다. 따라서 제안된 3차원 위치평가 방법은 DGPS의 위치오차를 크게 개선 해주며 이동경로를 평탄하게 하여 로봇의 실제 이동 경로에 가까운 위치평가 결과를 보여준다고 판단된다.

Fig. 6 
Position error (RMS error)

Fig. 5의 결과는 3차원 위치오차로서 고도에 대한 위치오차도 이미 포함되어 있지만 제안된 방법의 고도정보에 대한 성능을 확실히 확인하기 위해 주행거리에 따른 고도에 대한 결과만을 따로 Fig. 7에 나타내었다. 그림에서 점선은 DGPS를 이용하여 획득된 고도이며 실선은 3차원 위치평가의 고도이다. DGPS는 실제고도와는 다소 큰 차이를 보이고 있는 반면에 제안된 방법은 이와 같은 오차를 보정하여 기준경로에 가까운 결과를 제공해주고 있다.

Fig. 7 
Results of altitude estimation

Fig. 8은 고도위치오차를 나타내는데, 점선은 DGPS의 위치 오차, 실선은 3차원 위치평가의 오차이며 이에 대한 위치오차 특성을 Table 4에 나타내었다. DGPS는 최대 5.01 m, 평균 2.53 m의 비교적 큰 오차를 보이는 반면에 3차원 위치평가의 오차는 최대 0.95 m, 평균 0.18 m로써 고도에 대한 오차를 크게 개선해 주고 있다.

Fig. 8 
Error for altitude estimation

제안된 방법의 오차 수렴성 및 안정성을 확인하기 위해 주행거리에 대한 오차 공분산을 Fig. 9에 나타냈다. Figs. 9(a)는 x 방향, 9(b)는 y 방향, 9(c)는 z 방향, 9(d)는 θ방향, 그리고 Fig. 5(e)는 ϕ 방향으로의 오차공분산을 나타내며, 각 방향의 초기 오차공분산은 로봇의 이동특성 및 Table 2에 나타낸 바와 같은 센서들의 특성을 기준으로 설정 하였다. 결과를 분석해보면 x, y, z, θ 방향의 오차 공분산은 초기에 설정된 오차로부터 이동거리 10 m 내에서 빠르게 감소하여 점차 일정한 값으로 수렴하고 있지만 ϕ 방향 오차공분산은 오차의 수렴속도 측면 에서는 다른 방향에 비해 느리게 수렴하고 있다. 이것은 다른 상태벡터와는 달리 ϕ는 식(7)과 같이 여러 상태 벡터에 연관이 되어 있어 이들 상태 벡터의 수렴성에 영향을 받기 때문이다. 그러나 수렴하는 값의 크기 측면에서 보면 오히려 ϕ 방향의 오차공분산이 다른 방향에 비해 더 낮은 값을 보이고 있으며, 안정성 측면에서도 문제가 없다고 판단된다. 따라서 제안된 방법은 안정적인 성능을 보이고 있는 것으로 사료된다.

Fig. 9 
Error covariance

Fig. 10은 제안된 방법과 기존의 2차원 위치평가를 비교한 것이다. 이 실험은 동일한 데이터를 이용하여 2차원 및 3차원으로 위치평가를 하는 방법으로 수행 하였다. 2차원 위치평가는 고도 정보가 없으므로 경위도 평면 위치평가 결과만 비교하였으며 2차원 위치평가 방법은 참고문헌 11과 동일한 방법을 사용하였으며 두 방법의 계산시간은 큰 차가 없었다. 그림 에서 실선은 3차원 위치평가, 점선은 2차원 위치평가, X 표시가 포함된 검은 색 실선은 기준경로를 나타낸다. 실험 결과 Fig. 10과 같이 2차원과 3차원 위치평가의 차이가 거의 없게 보이기 때문에 확실한 비교를 위해 각각의 위치 오차의 통계학적 특성을 Fig. 11에 나타냈다. 전체 적으로 위치 오차의 차이가 작긴 하지만 이동거리가 길어 질수록 3차원 위치평가 결과가 2차원 결과보다 양호함을 알 수 있다.

Fig. 10 
Comparison of the 2D and 3D localization results

Fig. 11 
Comparison of the 2D and 3D localization error

두 방법의 위치오차가 주행거리가 길어짐에 따라 차이가 발생하고 있는 것은 고도정보를 고려하였는지에 따른 것이다. 예를 들어 로봇이 경사면을 따라 이동한 거리를 d, 경사면의 경사각도를 ϕ라고 하였을 때, 3차원 위치평가에서는 로봇의 수평 이동거리는 dcosϕ가 된다. 그러나 2차원 평가방법에서는 수평이동거리가 d이고 2차원과 3차원 위치평가의 오차는 d(1−cosϕ)가 되므로 경사가 심한 환경일수록 주행 거리가 길어질수록 오차가 더 커진다. 따라서 급격한 경사가 존재하는 환경에서 장시간 임무를 수행하는 경우에 오차는 더욱 커지게 되고 정확한 임무수행에 치명적이다. 결론적으로 고도정보가 필요한 경우에는 2차원 방법은 고도정보를 주지 못하므로 3차원 방법이 필연적이며, 고도정보가 중요하지 아니한 경우에도 급경사가 존재하는 환경이나 장거리 주행이 필요한 경우에는 2차원 방법은 오차가 크게 발생할 수 있으므로 3차원 방법이 더욱 효과적이라고 사료된다.