사이드 스캔 소나는 Lambertian 확산 모델^[11]을 사용하여 근 사화할 수 있으며 [Fig. 1]에 표시된 좌표계를 기반으로 한다.

[Fig. 1] 

Side Scan Sonar beam to image intensity model

식 (1)은 Lambertian 확산 모델을 음파에 적용한 간단한 모 델이다^[11]. 방정식과 [Fig. 1]에 사용된 변수들은^[11]의 참고 문 헌을 인용했으며 각각을 설명하면 다음과 같다. x는 로봇의 이 동 방향 이다. o는 센서 좌표계의 원점이다. p는 해저면의 반사 면 이다. r은 o와 p 사이의 벡터이다. N은 p의 법선 벡터이다. θ 는 r과 N 사이의 각도이다. α는 y축과 r 사이의 각도이다. ϕ는 사이드 스캔 소나의 소나 파형을 의미한다. I(p)는 소나 영상 의 세기를 의미한다. K는 영상 세기의 최대값인 I_max의 역수 로써 K = 1/I_max이다. d_3D는 AUV와 p 사이의 거리이다. h는 해수면으로부터 AUV의 높이이다. ψ는 AUV의 피치 각도 이 다. 반사 계수 R이 점 p와 소나 주파수에서의 파라미터의 함수 라면, 반사 계수 R은 모래 바닥에서 주파수가 일정하면 상수 로 가정할 수 있다. 시뮬레이션에서 R = 0.5로 설정했다.

반사 계수 R 은 소나 주파수와 반사체의 매질의 함수이며 0 과 1사이 값을 가진다. 시뮬레이터 상에서 소나 주파수와 반사 체의 매질이 상수 값을 가진다 가정하여 R = 0.5로 세팅하였다. 본 논문의 목적은 사이드 스캔 소나 이미지의 A-KAZE 특징점 매칭 기반 pose-graph SLAM이므로 정확한 소나 파라미터를 추 정하기 보다는 R 값은 상수라는 가정을 사용하여 구해지는 영 상에서 특징점 기반의 SLAM을 소개함에 있다. 단순화된 이미 지 모델을 기반으로 소나 이미지를 획득하여 그로부터 특징점 을 추출하여 포즈 보정에 사용할 수 있었다. 반사 계수 R 값이 변화하더라도 A-KAZE 특징점 매칭을 수행하는데 문제가 없 다는 것을 입증하기 위하여 4.2 A-KAZE 특징점 기반 이미지 매 칭의 [Fig. 6]에 R 값 변화에 따른 매칭 결과를 보였다. 또한, o, p, K를 계산하였다. 사이드 스캔 소나의 빔 파형은 실험으로 그래 프를 구할 수 있으나 이론식을 구할 수 없으므로, 시뮬레이션을 위해서는 그래프를 표현할 식을 사용할 필요가 있다. 본 논문에 서는 빔 파형을 사인 함수 한 쌍으로 근사하였다^[18].

사이드 스캔 소나 이론식에서 사이드 스캔 소나로 부터 해 저면 까지 거리 즉, 레인지 정보가 필요하다. 참고문헌^[11]에서 는 이 레인지를 영상 정보로부터 추정하여 사용하였다. 본 연 구에서는 UWSim^[1]에 구현되어 있는 에코사운더를 사용하여 레인지 데이터를 얻을 수 있었고, 그 데이터를 사이드 스캔 소 나 이론식의 레인지 정보로 사용하였다.

위 그림은 사이드 스캔 소나 센서 스펙으로 부터 주파수 540 kHz 일 때 구형파 형태 특성을 보이므로 구형 좌표계로 모델 링 한 것이다. φ는 로봇이 이동 방향의 소나 beam 각도 이고, θ 는 로봇 이동방향에 수직한 소나 beam 각도이다. H 는 수직방 향 거리를 의미한다. 센서 스펙에서 540 kHz일 때 소나 빔 폭 이 0.34도이므로 이것을 1픽셀의 기준으로 보았다. 즉, 각도와 픽셀간 변환식은 다음과 같으며, 사이드 스캔 소나의 한 방향 의 빔 범위 각을 β = 80°로 하면, 양 방향으로 160°가 되므로 한 이미지의 수평방향 최대 픽셀 수를 구할 수 있다.

만약, 540 kHz보다 낮은 주파수를 사용할 경우 소나 빔 폭 이 커지게 되므로 최대 픽셀 수가 줄어들게 된다. 식 (2)와 [Fig. 2]로 부터 픽셀을 각도로 변환하고 각도를 x, y 유클리디안 위 치로 변환하는 수식은 다음과 같다.

[Fig. 2] 

Side Scan Sonar transform for image pixel

여기서 u, v는 이미지 픽셀 평면에서의 픽셀 값들을 의미한 다. C_u와 C_υ는 이미지 평면의 중점이다.

사이드 스캔 소나 중앙 부분은 사각 지대이므로, 사이드 스 캔 소나 영상 중앙에도 데이터가 없는 것으로 처리하였다. 이 사각지대 영역을 10픽셀로 잡아서 로봇 이동에 수직한 방향인 470 + 10 = 480 픽셀이 된다.

사이드 스캔 소나로 획득한 두 이미지가 A-KAZE^[3] 특징점 매칭에 의하여 대응점들을 찾게 되면 [Fig. 3]과 같이 대응점들 을 표현 할 수 있다.

[Fig. 3] 

Side Scan Sonar image correspondences

식 (3) 에 의하여 영상 좌표 (u₁, υ₁)과 (u₂, υ₂)는 각각 2차원 상의 좌표 x₁ (x₁, y₁) 와 x₂ (x₂, y₂)로 변환 할 수 있다. 또 한, 에코사운더로부터 이 대응점들에 대한 높이 데이터를 얻 을 수 있다. 높이 데이터를 추가하여 3차원 벡터로 다음과 같 이 표현할 수 있다. P1 = (x₁, y₁, z₁)과 P₂= (x₂, y₂, z₂) 이때, 두 좌표간의 상대적인 회전행렬과 평행이동 벡터를 각각 R, t 라고 하자. 또한, 번들 조정에서의 재투영 오차를 정의하기 위 하여 다음과 같은 벡터들은 정의한다. P1′=(x1′,y1′,z1′) 은 P₂를 P₂의 좌표계로 변환한 점이다. 반대로, P2′=(x2′,y2′,z2′) 은 P₂를 P₂의 좌표계로 변환한 점이다. 정리하면 다 음과 같은 관계로 표현 할 수 있다.

위 수식을 기반으로 두 사이드 스캔 소나 영상의 대응점들 이 N 개 있다고 하면 다음과 같이 2차원 번들 조정식을 표현할 수 있다.

여기서 x1′=(x1′,y1′)이고 x2′=(x2′,y2′)이다.

이 번들 조정 식으로부터 최적화된 상대적 로봇 자세 R^,t^ 를 얻을 수 있고, 이것이 pose-graph SLAM에서의 최적화 제약 조건들로 작용하게 된다.

AUV 에 장착되어 있는 도플러 속도 로그(DVL)센서 에서 선속도를 측정하고 이것을 시간에 대하여 적분하여 선형 이동 량을 얻는다. 유사하게, 관성 측정 유닛(IMU)에서 회전각 정 보를 측정할 수 있다. 이 정보들을 이용하여 추측 항법 경로를 얻을 수 있다.

사이드 스캔 소나 기반 SLAM은 크게 3가지 파트로 나뉜다. 첫번째 파트는 A-KAZE 특징점 기반 사이드 스캔 소나 번들조 정이다. 사이드 스캔 소나 이미지로부터 A-KAZE 특징점 매칭 을 수행하고 그 최종 매칭점들(inliers)를 번들 조정에 넣어서 최적화된 상대적 로봇 자세를 얻는 과정이다. 이 상대적 로봇 자세가 pose-graph의 최적화 제약조건들로 사용된다.

두번째 파트는, 도플러 속도 로그(DVL)센서와 IMU를 이용 한 추측 항법 경로 생성이다. 도플러 속도 로그(DVL)센서와 관성 측정 유닛(IMU)로부터 항법 정보를 얻어서 로봇의 추측 항법 경로를 획득하는 과정이다. 수중 로봇의 기구학을 적용 하였다^[19].

마지막 파트는, 앞선 2개의 파트로부터 pose-graph를 구성 하고 이 그래프를 iSAM^[4]를 통하여 최적화 하는 과정이다. 최 적화 알고리즘은 gauss newton 최적화 알고리즘을 사용했다. 전체 알고리즘 흐름을 [Fig. 4]에 정리하였다.

[Fig. 4] 

(a) A-KAZE feature based SSS BA (b) Odometry by DVL and IMU (c) Pose-graph optimization by iSAM

UWSim 기본 환경에서 해저면을 실제 해저면과 유사하도 록 특징점 검출이 적게 수행되도록 설정했다. 여기에 바위를 추가하고, 수중 파이프 검사 상황을 가정하여 파이프를 추가 로 넣어서 이미지 매칭이 수행되는 것을 확인했다. 사이드 스 캔 소나는 로봇 진행 방향으로 5 cm/pixel 의 해상도를 가지며, 로봇 진행의 횡 방향으로 5 cm/pixel 의 해상도를 가지도록 설 정했다. 이것은 사이드 스캔 소나가 높은 해상도를 가지고 있 으므로 그 해상도를 반영한 것이다^[7]. 현재 영상이 320 × 640 픽셀 이므로 좌우로 32 m 길이를 스캔 할 수 있다.

[Fig. 5 (a)] 및 [Fig. 5 (b)]는 시뮬레이터로 생성된 사이드 스 캔 소나 이미지들 이다. 시뮬레이터에서 세팅한 해저면 좌측 에 추가로 넣은 바위와, 우측에 파이프를 사이드 스캔 소나 이 미지에서도 확인 할 수 있다. 로봇의 위치를 이동하여 서로 다 른 위치에서 사이드 스캔 소나 이미지를 얻었다. 총 4종류의 매칭 기법들을 사용하여 사이드 스캔 소나 이미지 매칭을 시 도했다. [Table 1-4]의 Keypoints 는 각각의 특징점 추출법들에 의하여 추출된 특징점들의 개수이다. 초기 매칭점들(matches) 는 Nearest Neighbor Distance Ratio (NNDR) matching을 수행 한 후 매칭 조건을 만족한 결과이다. 최종 매칭점들(inliers)는 초기 매칭점들(matches) 중에서 호모그래피 모델까지 만족한 경우이고, outliers는 만족하지 못한 경우이다. 이미지간의 호 모그래피 모델을 알 수 없으므로, Random sample consensus (RANSAC)까지 수행하여 호모그래피를 추정하였다. 호모그 래피로 개선된 성능을 나타내기 위하여 [Fig. 5]에 초기 매칭점 들(matches)와 매칭점들(inliers)의 pair 그림을 비교하였다. 결 과는 다음과 같다. SIFT 매칭 결과는 [Fig. 5 (c)]과 [Table 1]에 정리하였다. BRISK 매칭 결과는 [Fig. 5 (d)]과 [Table 2]에 정 리하였다. ORB 매칭 결과는 [Fig. 5 (e)]과 [Table 3]에 정리하 였다. A-KAZE 매칭 결과는 [Fig. 5 (f)]과 [Table 4]에 각각 정 리되어 있다. 매칭 결과를 비교해보면, 최종 매칭점(inliers)비 율은 ORB가 가장 높았으나 최종 매칭점들(inliers) 갯수는 A-KAZE가 가장 많았다. 본 연구에서는 번들 조정으로 자세 보정을 목적으로 하고 있다. 따라서, 최종 매칭점들(inliers) 수 가 가장 많은 A-KAZE를 이미지 매칭에 사용했다.

[Fig. 5] 

(a) The first image was obtained from the starting point (b) The matched image was extracted from the loop closing point (c) SIFT Image matching (d) BRISK Image matching (e) ORB Image matching (f) A-KAZE Image matching

파이프와 바위 뿐만 아니라 배경으로 사용한 해저면 환경에 서도 매칭이 발생하는 것을 확인 할 수 있었다. A-KAZE 특징 점 추출 후에 NNDR 매칭을 수행하면 초기 매칭점들(matches) 는 364개가 나오는 것을 확인 할 수 있었다. 이때, image 간의 호모그래피를 적용하여 최종 매칭점들(inliers)73개를 얻을 수 있었다. 일련의 과정을 통하여 얻은 최종 매칭점들(inliers)들 을 번들 조정 에 넣어서, 그 출력인 보정된 상대적인 로봇 자세 를 pose-graph 최적화에 사용했다. 해저면 환경에 대하여 반사 계수 R 값을 0.3, 0.5, 0.7로 변화해가며 A-KAZE 특징점 매칭 의 최종 매칭점들(inliers) 개수를 비교해보았다. [Fig. 6]에 결 과를 정리했다. R = 0.3일 때 최종 매칭점들(inliers) 111개, R = 0.5 일 때 최종 매칭점들(inliers) 101개, R = 0.7일 때 최종 매칭 점들(inliers) 107 개 의 결과를 얻을 수 있었다. 즉, R 값 바뀌더 라도 A-KAZE 특징점 매칭에 문제가 없으므로 본 논문의 목적 인 A-KAZE 특징점 매칭 기반 pose-graph SLAM 을 수행하는 데 문제가 없다는 것을 입증할 수 있었다. 본 논문에 사용된 소 나 모듈이 Black-Brown-Yellow colormap^[21]을 사용했다. 그 결 과, R이 매우 작으면 black color가 될 것이고 R이 0.5일 때 brown이고 R이 1에 가까워질수록 yellow color를 가지게 된다. [Fig. 6]에서 R = 0.3일 때 갈색이고 R = 0.7일 때 노란색으로 나 타나는 것은 그 때문이다. 사이드 스캔 소나 모듈은 참조 논문 ^[21]에 상세한 내용이 나와 있고 해당 모듈을 사용하였다.

[Fig. 6] 

In case of R=0.3 is (a), (b), (c). A-KAZE Inliers: 111 In case of R=0.5 is (d), (e), (f). A-KAZE Inliers: 101 In case of R=0.7 is (g), (h), (i). A-KAZE Inliers: 1071

MATLAB 에서 인공적으로 높이 스케일 조절을 하면서 각 높이에 따른 사이드 스캔 소나 번들 조정의 성능을 비교하였 다. 또한, 이미지들의 대응점이 20개라고 할 때, 대응점들의 모 든 높이 값 을 알고 있을 때 와 10개(50%), 4개(20%)의 높이 값 만 알고 있을 경우에 대하여 비교하였다. 이것은 에코사운더 가 모든 대응점의 높이를 측정하지 못할 것이므로 더 현실적 인 성능테스트를 위한 것 이다.

MATLAB 시뮬레이션 환경은 [Fig. 7]과 같은 인공 환경과 서로 다른 위치에서 획득한 사이드 스캔 소나 이미지로 구성 된다.

[Fig. 7] 

a) MATLAB simulation environment with synthetic seabed and Side Scan Sonars. The S1 is assigned for Side Scan Sonar 1 and the S2 is assigned for the Side Scan Sonar 2 with transformed pose (b) Side Scan Sonar Image 1 is obtained from S1 (c) Side Scan Sonar Image 2 is gathered from S2

서로 다른 위치에 있는 사이드 스캔 소나들 간의 상대적 6 자유도 포즈는 [3 m, 5 m, 2 m, 10°, 15°, 10°] 이다. 번들 조정은 특징점 매칭으로 얻어진 대응점 N 개의 대하여 3N + 6의 초기 값과 4N 개의 픽셀 측정 값들로 수행된다. 초기값 측정 잡음과 픽셀 측정 잡음을 정규분포로 모델링하였다.

초기값은 평행이동 3차원 벡터 t₀, 회전이동 3차원 벡터, rph₀ 대응점 N 개의 3차원 좌표값 X₀으로 구성된다. 평행이 동, 회전이동, 3차원 좌표값에 추가되는 정규분포 잡음의 표준 편차는 각각 σ_t = 1m, σ_rph = 15°, σ_X = 1m라고 했다. 또 한 픽셀 측정값들을 u, v라고 하고, 픽셀값에 더해지는 정규분 포 표준편차는 σ_u = 1 pixel, σ_υ = 1pixel일 때 다음과 같다.

해저면의 height scale 를 0부터 0.4 씩 늘려가며 4의 값을 가 질 때까지 10 종류의 해저면에 대하여 Euclidean error 를 구했 다. 이때 각 해저면에 대하여 10회에 걸쳐서 정규분포 잡음을 넣어서 획득한 결과의 평균이 [Fig. 8]의 붉은점이며, 표준편차 를 막대 그래프 꼴로 표현하였다. [Fig. 8]의 결과에서 높이 정 보를 많이 가질수록 오차도 작고 표준편차도 작은 것을 볼 수 있다.

[Fig. 8] 

SSS BA simulation for Seabed height scaling with 3 cases. (a) 20 height value of 20 inliers are known (b) 10 height value of 20 inliers are known (c) 4 height value of 20 inliers are known optimization by iSAM

다만, 한 이미지에서 4개의 높이 값만 알 수 있더라도 20% 오차 정도의 Euclidian 오차를 가지는 것을 볼 수 있다. 즉, 매우 희소한 깊이 정보를 측정할 수 있는 성능이 좋지 못한 에코 사 운더를 사용 하더라도 오차를 가진 포즈 보정을 사용할 수 있 다는 결과를 얻었다.

시뮬레이터에서 루트 폐쇄 알고리즘 수행 확인을 위하여 [Fig. 9 (a)]와 같이 환경을 구성하였다. 정사각형 형태로 파이 프를 배치하였고, 로봇이 주행하는 직선거리 120m 가 되도록 하여 로봇이 주행하는 전체 경로 길이는 480 m 가 되도록 하였 다. 시작위치에서의 사이드 스캔 소나 이미지와 제자리로 돌 아왔을 때의 사이드 스캔 소나 이미지를 매칭하였다.

[Fig. 9] 

The pipes are placed in a square shape. The length of each side is 120 m. Total path length is 480 m. The robot moves clockwise. It shows the DR result with iSAM. The gauss-newton algorithm was used for pose-graph optimization

오차는 원점과의 Euclidean 거리로 정의했다. 예를 들어, 추 측 항법의 오차는 원점과 한 바퀴 돌아왔을 때 추측 항법 위치 사이의 Euclidean 거리이다. SLAM 의 오차는 루프 폐쇄에 의 하여 최적화된 점의 위치와 원점간의 Euclidean 거리이다. 이 것은 pose-graph SLAM 의 루프 폐쇄에 의한 보정을 검증하기 위한 metric이다.

추측 항법과 루프 폐쇄 제약 조건은 iSAM^[4]의 pose-graph로 구성 되었다. [Table 5]에 결과들을 정리 했다. 추측 항법의 오 차는 3.3109 m, SLAM의 오차는 1.3456 m이므로, 추측 항법의 오차에 비하여 pose-graph SLAM 의 오차는 루프 폐쇄에 의해 0.4064배로 감소하는 것을 확인 할 수 있었다. 추가로, 루프 전 체의 560개 노드에 대하여 true 값에 대한 DR과 pose-graph SLAM 의 Root Mean Square Error (RMSE)를 계산하였다. 이 것은 Euclidean 이 원점에서 발생한 루프 폐쇄 보정의 성능을 검증한 것과 달리, RMSE는 루프 전체의 평균적인 오차를 측 정하기 위한 metric이다. 추측 항법은 3.5051 m, pose-graph SLAM은 2.7857 m이었다. 0.7948배이므로 Euclidian error에 비교하여 감소량이 적었다. 이것은, 한 바퀴 회전 후 루프 원점 에서만 포즈 보정을 수행하여 원점에서는 보정이 이루어졌으 나, 그 이외 구간은 보정이 이루어지지 않았기 때문이다. 그럼 에도, 원점에서 수행된 보정에 의하여 RMSE 또한 감소하는 것을 확인 할 수 있었다.