IMU 센서로부터 달 환경에서 획득 가능한 데이터는 가속 도, 자이로 데이터가 있다. 이를 확장 칼만 필터(Extended Kalman filter, EKF)에 적용하면, 자세 데이터인 오일러 각을 추정할 수 있다. Kalman filter는 과거의 정보와 새로운 측정값을 사용하 여 측정값에 포함된 잡음을 제거하여 최적의 값을 추정하는 알고리즘이다. 이는 선형적인 움직임을 갖는 대상에 재귀적으 로 동작시키는데, IMU의 자이로 데이터와 오일러 각 사이의 시스템 모델이 비선형이기 때문에, 이를 재귀적으로 동작시키 는 EKF를 이용한다. EKF는 간단하게 5단계로 구분하여 설명 할 수 있다. 먼저, 초기값 및 공분산 선정, 추정값과 오차 공분 산 예측, 칼만 이득 계산, 추정값 계산 그리고 오차 공분산 계 산을 마지막으로 5단계를 반복 실행하여 결과를 찾아낸다. 여 기서 공분산이란, 2개의 확률변수의 상관 정도를 나타내는 값 이다. 이 과정은 위의 [Fig. 3]에서 자세히 확인할 수 있다.

[Fig. 3] 

Extended kalman filter

IMU의 시스템 모델을 얻기 위하여 아래의 식 (1)과 같은 오 일러 각의 변화율(ϕ˙,θ˙,ψ˙)과 각속도(p˙,q˙,r˙) 사이의 관계를 이용한다^[4,5].

여기서 w는 시스템 잡음을 의미하며, (ϕ, θ, ψ)은 각각 X, Y, Z 축의 오일러 각을 의미한다. 비선형인 시스템 모델을 선 형화시키기 위하여 아래의 식 (2)와 같은 자코비안(Jacobian) 행렬인 A 행렬식을 구해야 한다^[4].

여기서, f=[f1 f2 f3]T 이며, 시스템 모델을 적용하여 표현 한 최종 자코비안 행렬을 다음과 같다^[4].

A(1,1)=q cosϕ tanθ−r sinϕ tanθA(1,2)=q sinϕ secθ2+r cosϕ secθ2A(1,3)=0A(2,1)=−q sinϕ −r cosϕA(2,2)=0A(2,3)=0A(3,1)=q cosϕ secθ−r sinϕ secθA(3,2)=q sinϕ secθ tanθ+r cosϕ secθ tanθA(3,3)=0

여기서, A_{m, n}은 자코비안 행렬 A의 m 번째 행, n번째 열 의 요소를 의미한다. 또한, Kalman filter에서 보정용 측정값을 위해 선형 가속도 데이터를 이용하며, 이에 대한 측정 모델 식 은 다음과 같다^[4].

여기서, H=[100010], x={ϕθψ} 이다.

이는 선형 시스템이기 때문에 자코비안을 구하지 않고 행 렬 H를 바로 사용한다. 이때, 가속도 값으로는 Yaw 각(ψ)을 계산할 수 없어서 제외한다. 또한, 잡음에 대한 공분산 행렬인 Q와 R은 아래의 식 (4)를 통해 적용한다^[6] .

여기서, (σwx, σwy, σwz) 는 각 축에서 자이로의 공분산을 나 타내며, (σϕ, σθ) 은 Yaw (ψ)을 제외한 Roll (ϕ), Pitch (θ) 에 대한 공분산을 의미한다.

탐사 로버가 비전 센서를 이용하여 달 착륙선까지의 거리 를 추정하기 위해서는 먼저, 카메라 앵글 안에 물체 즉, 달 착 륙선이 보여야 한다. 따라서 기본적인 하드웨어로써 로버에 팬 틸트 카메라 제어 시스템이 부착되어 카메라가 달 착륙선 을 추적한다고 가정한다. 하지만 일반적인 모노 카메라는 깊 이 데이터가 없으므로 스테레오 카메라를 이용한다^[7]. 또한, 카메라에 보이는 달 착륙선을 인식하여 달 착륙선까지의 거리 를 추정하기 위하여 3가지의 알고리즘을 제안한다. 먼저, 주변 환경을 무시하고 달 착륙선을 관심 영역(Region Of Interest, ROI)으로 지정하기 위한 HSV 필터링, 다음으로 stereo 카메라 의 좌, 우 영상으로부터 디스패리티를 구하기 위하여 SIFT 특 징점 매칭 알고리즘을 이용한다. 마지막으로 계산된 디스패리 티를 에피폴라 기하학에 적용하여 디스패리티와 거리 사이의 관계를 도출을 통한 탐사 로버와 달 착륙선 사이의 직선거리 를 추정한다.

2.2.2 SIFT 특징점 매칭 알고리즘

2.2.1장의 HSV 필터링을 통하여 관심 영역을 추출하면, 2.2.3장의 관심 영역 내부의 물체까지 거리를 추정할 수 있다.

이를 위해 위의 [Fig. 4]과 같이 스테레오 카메라를 통해 얻은 좌. 우 이미지의 특징점을 비교하여, 같은 특징점을 매칭하는 과정이 필요하다. 특징점 매칭 알고리즘으로는 SIFT (Scale Invariant Feature Transform), ASIFT (Affine-SIFT), SURF (Speeded- Up Robust Feature), Harris corner 등이 있지만, 본 논문에서는 SIFT 알고리즘을 이용한다. SIFT 알고리즘은 성능적인 면에 서 SURF, Harris corner 알고리즘보다 우수하지만, SIFT 알고 리즘의 시점 변화에 민감한 단점을 보완한 ASIFT 알고리즘보 다는 우수하지 못하다^[10-14]. 하지만, ASIFT를 이용할 경우 성 능적으로는 조금 향상되는 반면에, 계산량이 방대하여 속도가 너무 느려진다. 본 논문에서는 스테레오 이미지를 통해서 얻 는 좌, 우 이미지를 서로 매칭하기 때문에 시점 변화가 크지 않 다. 따라서 본 논문에서는 SIFT 알고리즘을 이용하는 것이 적 합하다.

[Fig. 4] 

Example of stereo image feature matching algorithm

2.2.3 에피폴라 기하학을 이용한 거리 추정

에피폴라 기하학은 두 개의 카메라를 이용하는 스테레오 비전 시스템에서 3차원 공간상의 입체 대상을 각 카메라의 2 차원 평면에 사영시켰을 때 나타나는 기하학적 관계를 다루는 것이다^[15].

아래의 [Fig. 5]와 같이, 이를 이용하여 3차원 공간상의 한 점 P, 이미지 평면에 사영된 점 P_l과 P_r 그리고 카메라 원점 O_l과 O_r 사이의 관계를 볼 수 있다. 이때 그림에서 C_l과 C_r은 이미지 평면의 중심좌표이며, f 는 카메라 원점부터 이미지 평면 중심좌표까지의 거리인 초점거리를 의미한다. 마지막으 로 T 는 두 카메라 사이의 거리인 베이스라인을 의미한다. 또 한, 삼각형의 닮음 법칙을 이용한 비례식을 사용하면, 카메라 원점에서 P 까지 수직 거리인 Z_P 를 찾을 수 있다^[16].

[Fig. 5] 

Distance estimation using epipolar geometry

ZP=f×Txl−xr

(5)

이 식에서, xl=XP×fZP, xr=(XP−T)×fZP 이다.

또한 Z_P의 분모에 해당하는 x_l - x_r은 디스패리티이며, 3차 원 공간상의 좌표를 두 이미지 평면에 사영시켰을 때 생기는 좌표의 x축 좌표의 차이를 의미한다.

2.2.4 좌표 변환을 통한 3차원 위치추정

앞선 알고리즘들을 통해 얻은 오일러 각과 직선거리는 구 면좌표계의 (r, θ, ψ)와 같다. 따라서 구면좌표계와 직교좌표 계 (x, y, z)의 관계에 적용하여 로버의 현재 위치를 추정한다.

아래의 [Fig. 6]은 구면좌표계와 직교좌표계의 관계를 보여 주며, 다음과 같이 간단히 나타낼 수 있다.

[Fig. 6] 

Relationship of Spherical coordinate system and rectangular coordinate system

[xyz]=[r sinθ cosψr sinθ sinψr cosθ]

(6)

따라서, 2.1장에서 얻은 오일러 각 중 Pitch 각 및 Yaw 각을 의미하는 (θ,ψ)와 2.2장에서 얻은 거리(r)을 이용하여 로버 의 (x, y, z) 위치 좌표를 추정한다.

본 논문에서 제안하는 알고리즘의 성능 확인을 위하여, 먼 저, IMU 센서를 통해 추정된 오일러 각의 정확도를 확인하는 실험을 진행하였다. 실험을 위해서 사용하는 IMU 센서는 E2BOX 사의 EBIMU-9DOFV3 이며, 모바일 로봇은 아래의 [Fig. 7]과 같다. 이를 아래의 [Fig. 8]과 같은 직사각형 복도 (1408 x 880 cm)를 주행하여 IMU의 가속도 및 자이로 데이터 를 획득하였다. 또한, 각도 변화를 명확히 보기 위하여, 코너 부근에서만 90°로 방향 전환하였다.

[Fig. 7] 

Mobile robot for experiments

[Fig. 8] 

Experiment environment for IMU sensor (Rectangle corridor : 1408 x 880 cm^2)

획득한 IMU 데이터를 EKF에 적용하여 각 축에 대한 오일 러 각을 추정한 결과, 약 1.5° 내외의 잡음이 발생하지만, 위의 [Fig. 9]과 같이 Z축(yaw, ψ)에서 정확히 90° 회전하면서 복도 를 주행하는 결과를 확인하였다.

[Fig. 9] 

Result of extended kalman filter (Rectangle corridor : 1408 x 880 cm^2)

2.2장에서 제안하는 알고리즘의 실험을 위하여 사용한 스 테레오 카메라는 ZED사 모델을 사용하였고, 성능은 아래 [Table 1]에서 확인할 수 있다.

먼저, HSV 필터 적용을 위하여 아래의 [Fig. 10]와 같은 기 본 스테레오 이미지를 획득한다. 만약 이를 ROI를 지정하지 않고 SIFT 알고리즘에 적용한다면, 특징점 매칭이 제대로 이 루어지지 않는다.

[Fig. 10] 

Input stereo image (Case of the distance between object and camera is 100 cm)

따라서 ROI 지정을 위하여 H, S, V의 범위를 조절하여 원하 는 색상을 가진 물체를 찾는다. 본 논문에서는 달 착륙선과 같 은 노란색 표면을 가진 물체를 목표로 하며, 필터 적용 결과는 아래의 [Fig. 11]과 같다.

[Fig. 11] 

Result of HSV filtering (Case of the distance between object and camera is 100 cm)

입력 이미지에서 ROI 지정을 위하여 HSV 필터링을 적용한 이미지의 pixel 값을 참조한다. 이때, 이미지에서 흰색인 부분 은 pixel 값이 255이며, 나머지 부분은 0을 가진다. 따라서 가로 방향과 세로 방향, 각각 255 값을 가진 pixel의 양을 아래의 [Fig. 12], [Fig. 13]과 같이 히스토그램으로 나타낸다. 이 중, 최 빈값으로부터 좌우로 임곗값 이상의 빈도를 보이는 값까지 ROI 영역을 선정한다. 본 논문에서 임곗값은 0으로 지정하고, 아래의 [Fig. 12]을 통해 얻은 가로 방향에서 최빈값을 지닌 지점 은 368이며, 관심 영역은 321~388이다. [Fig. 12]를 통해 얻은 세 로 방향에서 최빈값을 지닌 지점은 176이며, 관심 영역은 87~202이다. 또한, 이미지상에서 원점은 왼쪽 위의 꼭짓점이다.

[Fig. 12] 

Histogram of width direction (Left image of stereo) : The mode is 368 and Range of ROI is from 321 to 388

[Fig. 13] 

Histogram of height direction (Left image of stereo) : The mode is 176 and Range of ROI is from 87 to 202

이를 적용하고 SIFT 알고리즘을 적용한 결과는 위의 [Fig. 14]과 같고, 앞선 [Fig. 10]과는 다르게 관심 물체 이외의 지점 에서 생기는 특징점을 제거하기 위하여 ROI의 외부는 흰색으 로 변경하였다.

[Fig. 14] 

Result of SIFT algorithm (Case of the distance between object and camera is 100 cm)

SIFT 알고리즘 적용 후, 거리 추정을 위하여 매칭된 점들을 에피폴라 기하학에 적용하였다. 하지만, SIFT 특징점 매칭에 서 제대로 된 매칭이 이루어지지 않은 잡음이 있으므로 디스 패리티 계산에 있어서 평균값이 아닌 중간값을 이용하고, 거 리 계산 후에도 잡음이 발생할 것을 대비하여 이동 평균 필터 를 적용하여 최종 거리를 산출하였다. 이때 평균 크기는 10개 데이터로 지정하였으며, 이를 통해 실제 거리를 30~100cm까 지 추정한 결과 아래 [Table 2]와 같다.

마지막으로, 오일러 각과 거리를 이용하여 3차원 위치 정보 를 추정하기 위하여 위의 [Fig. 15]와 같은 실험 환경을 구성하 였다. 로버는 원점에서 출발하여 1번에서 5번까지 순서대로 이동한 다음, 다시 1번으로 돌아오는 과정을 거친다. 이때 팬 틸트 모듈 위에 카메라와 IMU 센서를 탑재하고, 팬 틸트 모듈 을 제어함으로써 카메라를 통해 달 착륙선 모형을 이미지의 중앙에 위치시킨 뒤, 마지막으로 스테레오 영상 및 IMU 데이 터를 획득한다. 실험 데이터는 아래의 [Table 3]에서 확인할 수 있다. 이때 오일러 각은 Y축을 기준으로 늘어나기 때문에, 구 면좌표계 형식으로 바꾸기 위하여 (90°-오일러 각)으로 계산 하여 사용한다.

[Fig. 15] 

Experiment environment for localization

[Table 3]에서 얻은 데이터를 직교좌표계에서의 값으로 변 환하여 모바일 로봇의 위치를 추정한 결과는 위의 [Table 4]와 [Fig. 16]에서 확인할 수 있다.

[Fig. 16] 

Result of localization(Upper image : Final result of localization / Bottom left image : X-Y View of Result for all positions / Bottom right image : X-Z View of Result for only 2-4 positions)

여기서 reference는 실제 위치이며, conventional algorithm은 IMU와 엔코더를 통해 dead-reckoning을 수행하여 위치를 추 정한 결과이다. IMU를 통해 모바일 로봇의 오일러 각을 추정 하고 엔코더를 통해 얻은 이동 거리와 함께 모바일 로봇의 현 재 위치 추정하였다. 처음에는 reference 데이터와 비교했을 때 큰 오차가 발생하지 않지만, 시간이 지남에 따라 누적오차 가 발생하여 최종적으로는 정확하지 못한 결과를 추정하였다. 반면 본 논문에서 제안한 알고리즘은 기존의 알고리즘에 비해 시간이 지남에도 오차가 발산하지 않고, 안정적인 결과를 가 지는 것을 확인할 수 있다. 각 지점에서 기존의 알고리즘과 제 안하는 알고리즘을 통해 추정된 X, Y, Z값들의 최대 오차율과 평균 오차율을 계산한 결과는 위의 [Table 5]에서 확인할 수 있 으며, 본 논문에서 제안한 알고리즘이 상당히 정확한 결과를 가지는 것임을 확인할 수 있다.