사이드 스캔 소나 기반 Pose-graph SLAM

1. 서 론

수중 로봇 공학자들은 수중 실험 데이터를 얻는 데 어려움 을 겪고 있다. 수중 로봇은 물속에서 이동하는 걸 전제로 설계 되어 있으므로 지상에서 움직일 수 없다. 이러한 수중 로봇을 물이 있는 실험장소까지 옮기고 물속에 넣는 과정이 필요하 다. 이때, 수중 시뮬레이터가 제공하는 데이터가 실제 실험데 이터와 같은 수준의 데이터를 제공한다면, 시뮬레이터를 통하 여 얻은 데이터가 실험데이터를 보완할 수 있다.

본 연구에서는 수중 시뮬레이터에서 수중 영상 연구에 사 용할 영상 센서로써, 소나를 선택 했다. 카메라가 영상 획득에 일반적으로 사용되며 Global Positioning System (GPS)가 현재 의 로봇 자세를 얻는 데 널리 사용 되지만, 이러한 센서들은 전 자파를 사용하므로 물속에서 빠르게 감쇠되는 문제가 있다. 반대로, 소나는 수중에서 비교적 잘 전파되는 음파를 사용하 므로 수중 영상 연구에서 자주 사용된다^[5,6].

본 연구에서는 여러 종류의 소나들 중에서 사이드 스캔 소 나를 선택했다. 사이드 스캔 소나는 10 cm × 5 cm의 고해상도 와 70 m의 넓은 감지 범위를 제공하기 때문에 널리 사용되고 있다^[5,6]. 사이드 스캔 소나는 Lambertian 확산 모델^[2]에 의해 근사 될 수 있으므로, 그 모델을 기반으로 시뮬레이션에 적용 될 수 있다. 다만, 사이드 스캔 소나의 빔 파형은 실험을 통해 얻을 수 있으나 이론식을 얻을 수 없기 때문에 근사식을 사용 하여 시뮬레이션을 위한 공식을 표현해야 한다. 또한, 사이드 스캔 소나 영상을 만들기 위해서는 수중 로봇과 해저면 사이 거리인 레인지 정보가 필요한데 이것은 에코사운더로부터 획 득하였다.

본 연구에서는 UWSim에서 사이드 스캔 소나 모듈 개발을 다룬다. UWSim은 Jaume-I Castelln대학의 IRSLab에서 만들 어진 수중 시뮬레이터 이다. MALAB기반의 bathymeter 시뮬 레이터^[7] 와 달리 제안 된 사이드 스캔 소나 모듈은 MATLAB 보다 빠른 컴퓨팅 성능을 제공하는 C ++언어로 개발되었다.

사이드 스캔 소나 이미지 특징점 기반 pose-graph SLAM 연 구가 수행되었다. 항법센서로써, 도플러 속도 로그(DVL)센서 는 선형 속도 측정을 생성하고, 관성 측정 유닛(IMU)은 회전 정보를 제공 한다. 본 연구에서는 이 센서정보들로부터 posegraph SLAM을 위한 로봇 이동 경로 및 사이드 스캔 소나 이미 지들이 생성 되었다. 또한, 사이드 스캔 소나 번들 조정에 대해 A-KAZE^[3] 특징점 매칭 기법을 사용했다. 번들 조정에는 Ceres Solver^[20]를 사용했다. 사이드 스캔 소나 번들 조정의 출력 값인 최적화된 상대적은 로봇 자세는 pose-graph SLAM의 루프 폐쇄 제약 조건에 사용된다. pose-graph 최적화를 위해 incremental Smoothing And Mapping (iSAM)^[4]를 사용했다. 최적화 결과는 추측 항법과 비교되었다.

2. 선행 연구 조사

수중 시뮬레이터는 수중 실험에 필요한 데이터가 충분하지 않은 경우 수중 엔지니어에게 필수적이다. 기존에 구현된 수 중 시뮬레이터들은 다음과 같다. MATLAB기반 사이드 스캔 소나 시뮬레이터 개발은 Heriot-Watt University OSL^[7]에 의해 이루어졌고, 명칭은 Sonar sidescan simulator와 bathymeter simulator이다. 이 연구에서 만든 사이드 스캔 소나는 기본적으 로 깊이이미지인데, 이것을 실제 사이드 스캔 소나 이미지와 유사하게 만들기 위하여 Rayleigh 노이즈와 스펙클 노이즈를 추가하였다. 또한, Linux Ubuntu환경에서 C ++언어로 구현 된 시뮬레이터는 Jaume-I Castelln대학의 IRSLab에서 UWSim^[1] 이다. UWSim^[1]은 오픈 소스인 Robot Operating System (ROS) 를 기반이다. OSG (Open Scene Graph)를 사용하여, 수중 로봇 을 위한 로봇 동역학 뿐만 아니라 수중 환경을 위한 유체 역학 또한 구현 되어있다. 그러나, UWSim에 사이드 스캔 소나 모 듈이 존재하지 않았으나 본 연구를 통해 구현되었다.

본 연구에서 사이드 스캔 소나 모듈을 구현하는 것이 목적이 므로, 사이드 스캔 소나의 영상 처리에 대한 선행연구들을 조사 했다. 사이드 스캔 소나의 물리적 속성, 데이터 처리, 이미지 합 성 및 강화 연구가 Helferty^[8]에 의하여 수행되었다. Vanessa가 수행 한 사이드 스캔 소나 이미지의 시뮬레이션은 광선 추적 렌더링 기술을 통해 사이드 스캔 소나 이미지를 만들었다^[9]. 반면에, 정확하지만 복잡한 사이드 스캔 소나 이미지 모델이 소개되었다^[10]. Lambertian 확산 모델을 이용한 간략화 된 사 이드 스캔 소나 영상 모델이 제안되었고^[11], 사이드 스캔 소나 빔 프로파일은 3차원에서 2차원으로 계산 비용을 줄이기 위해 평면으로 근사화 되었다.

본 연구에서는, 개발된 사이드 스캔 소나 영상^[21]을 기반으 로 특징점 기반 pose-graph SLAM을 수행하여 수중 로봇의 자 세를 보정하는 것을 최종적인 목적으로 하고 있다. 따라서, 사 이드 스캔 소나의 영상 매칭 기법에 대한 선행 연구를 조사했 고, 다음과 같은 연구들이 있음을 확인했다. 영상 강도의 상관 관계를 이용한 사이드 스캔 소나 영상 매칭 기법이 제안되었 다^[12]. 이 논문에서는 노이즈 제거를 위해 웨이블릿 변환을 사용 했다. 사이드 스캔 소나 이미지 매칭을 위하여 최첨단의 특징점 추출 기법들이 비교 되었다^[13]. 예를 들어 SIFT (Scale Invariant Feature Transform)^[14], SURF (Fasted Robust Features)^[15], Oriented FAST and Rotated BRIEF (ORB)^[16] 및 A-KAZE^[3]를 비교한다. 이 논문에서 사이드 스캔 소나 이미지의 매칭을 A-KAZE^[3]로 시도 했을 때 최종 매칭점들(inliers) 비율은 35% 이상으로 나 와서 다른 특징점 추출 기법들보다 높았다. 또한, A-KAZE의 사이드 스캔 소나 이미지 매칭 계산 시간은 40 ms으로 충분히 빠르다. 따라서 A-KAZE^[3]는 빠르고 정확한 사이드 스캔 소나 이미지 매칭에 적합하다.

마지막으로, 사이드 스캔 소나 기반 수중 로봇의 항법보정에 대하여 다루는 연구들을 조사했다. 본 연구에서 2가지 연구를 참 고문헌으로 삼았다. RauchTungStriebel (RTS) 알고리즘을 사용 하는 사이드 스캔 소나의 Concurrent Mapping and Localization (CML)^[17]는 확장 칼만 필터 데이터를 개선하기 위해 제안되었 다. 그것은 사이드 스캔 소나 이미지의 음향 그림자와 함께 랜 드 마크와 성공적으로 매칭된다. 그러나 N이 필터의 상태 수 이면 O(N³)가 증가하는 계산 비용 문제가 있다. 에코사운더 와 사이드 스캔 소나를 이용한 pose-graph SLAM이 제안되었 다. iSAM^[4]을 통하여 pose-graph를 최적화 했다. 이 연구에서 는 점진적 계산 알고리듬을 사용하기 때문에 계산 비용이 일정 하다는 강점이 있다. 다만, 수중 실험을 수행할 때 정확한 위치 파악을 위해 수중 표식을 사용했다. iSAM^[4]는 pose-graph SLAM 최적화와 시각화를 포함하여 다양한 그래프 최적화 알고리즘 을 제공 하는 오픈 소스 소프트웨어이다.

본 논문에서는 이미지 매칭을 위해 사이드 스캔 소나 이미 지 모델을 사용하고^[11], 그로부터 A-KAZE^[3] 이미지 매칭을 수 행하고, Ceres Solver^[20]로 번들 조정을 수행하여, iSAM^[4]에서 제공하는 pose-graph SLAM으로 자세를 보정하였다.

3. 사이드 스캔 소나 pose-graph SLAM

3.1 사이드 스캔 소나 모델링

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

$\[ \begin{array}{l}I\left(p\right)=RK\varphi \hspace{0.17em}\text{cos}\left(\theta \right)\\ \text{cos}\left(\theta \right)=\frac{\overrightarrow{r}\cdot \overrightarrow{N}}{\Vert \overrightarrow{r}\Vert \hspace{0.17em}\Vert \overrightarrow{N}\Vert }\end{array} \]$

(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].

3.2 사이드 스캔 소나와 에코사운더 데이터 융합

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

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

$\[ \begin{array}{l}{\phi }_0={\theta }_0={0.34}^{°}/pixel\\ \frac{2*{80}^{°}}{{0.34}^{°}/pixel}=470pixel\end{array} \]$

(2)

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

$\[ \begin{array}{l}x=Htan\left(\phi \right)=Htan\left(\left(u-c_u\right)*{\phi }_0\right)\\ y=Htan\left(\theta \right)=Htan\left(\left(\upsilon -c_{\upsilon }\right)*{\theta }_0\right)\end{array} \]$

(3)

[Fig. 2]

Side Scan Sonar transform for image pixel

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

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

3.3 사이드 스캔 소나 번들 조정

사이드 스캔 소나로 획득한 두 이미지가 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 라고 하자. 또한, 번들 조정에서의 재투영 오차를 정의하기 위 하여 다음과 같은 벡터들은 정의한다. $$ P_1{}^{\prime }=\left({\text{x}}_1{}^{\prime },{\text{y}}_1{}^{\prime },{\text{z}}_1{}^{\prime }\right)$$ 은 P₂를 P₂의 좌표계로 변환한 점이다. 반대로, $$ P_2{}^{\prime }=\left({\text{x}}_2{}^{\prime },y_2{}^{\prime },z_2{}^{\prime }\right)$$ 은 P₂를 P₂의 좌표계로 변환한 점이다. 정리하면 다 음과 같은 관계로 표현 할 수 있다.

$\[ \begin{array}{l}{P}_1{}^{\prime }={\text{R}}^T\left(P_2-t\right)\\ P_2{}^{\prime }=\text{R}{P}_1+t\end{array} \]$

(4)

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

$\[ \underset{\widehat{R},\widehat{t}}{\text{min}}={\displaystyle \sum _{i=0}^n\left({\Vert x_1-x_1{}^{\prime }\Vert }^2+{\Vert x_2-x_2{}^{\prime }\Vert }^2\right)} \]$

(5)

여기서 $$ x_1{}^{\prime }=\left({\text{x}}_1{}^{\prime },{\text{y}}_1{}^{\prime }\right)$$이고 $$ x_2{}^{\prime }=\left({\text{x}}_2{}^{\prime },{\text{y}}_2{}^{\prime }\right)$$이다.

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

3.5 사이드 스캔 소나 기반 SLAM 알고리즘 개요

사이드 스캔 소나 기반 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

4. 시뮬레이션

4.2 A-KAZE 특징점 기반 이미지 매칭

[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

[Table 1]

SIFT feature matching results

[Table 2]

BRISK feature matching results

[Table 3]

ORB feature matching results

[Table 4]

A-KAZE feature matching results

파이프와 바위 뿐만 아니라 배경으로 사용한 해저면 환경에 서도 매칭이 발생하는 것을 확인 할 수 있었다. 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

4.3 인공 환경에서의 사이드 스캔 소나 번들 조정

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일 때 다음과 같다.

$\[ \begin{array}{l}{t}_0=t_0+\text{N}\left(0,\hspace{0.17em}{\sigma }_{\text{t}}\right)\\ rp{h}_0=rp{h}_0+\text{N}\left(0,\hspace{0.17em}{\sigma }_{\text{rph}}\right)\\ X_0=X_0+\text{N}\left(0,\hspace{0.17em}{\sigma }_{\text{X}}\right)\\ u=u+\text{N}\left(0,\hspace{0.17em}{\sigma }_{\text{u}}\right)\\ v=v+\text{N}\left(0,\hspace{0.17em}{\sigma }_{\text{v}}\right)\end{array} \]$

(6)

해저면의 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 오차를 가지는 것을 볼 수 있다. 즉, 매우 희소한 깊이 정보를 측정할 수 있는 성능이 좋지 못한 에코 사 운더를 사용 하더라도 오차를 가진 포즈 보정을 사용할 수 있 다는 결과를 얻었다.

4.4 iSAM를 사용한 pose-graph 최적화

시뮬레이터에서 루트 폐쇄 알고리즘 수행 확인을 위하여 [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 또한 감소하는 것을 확인 할 수 있었다.

[Table 5]

The comparison of DR and pose-graph SLAM by iSAM

5. 결 론

본 논문에서는, UWSim을 위한 사이드 스캔 소나 모듈^[20]을 사용한 사이드 스캔 소나 기반 pose-graph SLAM에 대하여 다 룬다. Lambertian 확산 모델을 응용한 간략화 된 사이드 스캔 소나 영상 모델^[2]을 사용하였고, 빔 프로파일은 정현파 파형에 기초하여 추정 되었다. 사이드 스캔 소나 영상을 만드는데 레 인지 정보가 필요하므로 이것은 UWSim에 구현되어 있는 에 코사운더를 이용하였다^[18]. Rayleigh 노이즈와 스펙클 노이즈 를 사용했고^[7] 칼라맵을 적용하여 실제 사이드 스캔 소나 이미 지와 유사한 이미지를 시뮬레이터를 통하여 얻을 수 있었다. pose-graph SLAM을 위한 추측 항법 및 사이드 스캔 소나 이미 지를 얻을 수 있었다. 사이드 스캔 소나 이미지에 대하여 특징 점 추출 기법들의 이미지 매칭 성능을 비교했다. 번들 조정을 통한 자세 보정을 목적으로 하므로, 최종 매칭점들(inliers) 수 가 가장 많게 나온 A-KAZE 기법을 선택했다. A-KAZE 이미 지 매칭 결과를 이용하여 Ceres Solver^[20]로 번들 조정을 수행 했다. 사이드 스캔 소나 번들 조정의 출력은 pose-graph SLAM 의 루프 폐쇄 제약 조건으로 사용되었다. 또한, pose-graph 최 적화를 위해 iSAM^[4]를 사용하였고, 최적화 결과가 추측 항법 과 비교 되어 루프 폐쇄를 통한 사이드 스캔 소나 이미지 기반 수중 로봇의 자세 보정이 성공적으로 수행되었다.

Acknowledgments

This work is supported by LIG nex1 (Development of navigation technology based on SSS image information based SLAM). This work is financially supported by Korea Ministry of Land, Infrastructure and Transport(MOLIT) as `U-City Master and Doctor Course Grant Program'

References

• M. Prats, J. Pérez, J.J. Fernández, and P.J. Sanz, “An open source tool for simulation and supervision of underwater intervention missions,” IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 2577-2582, 2012. [https://doi.org/10.1109/IROS.2012.6385788]
• H. Ragheb and E.R. Hancock. “Surface radiance correction for shape from shading,” Pattern Recognition, Vol. 38, No. 10, pp. 1574-1595, 2005. [https://doi.org/10.1016/j.patcog.2005.03.025]
• P.F. Alcantarilla, A. Bartoli, and A.J. Davison. “KAZE features,” European Conference on Computer Vision. Springer, Berlin, Heidelberg, pp. 214-227, 2012. [https://doi.org/10.1007/978-3-642-33783-3_16]
• M. Kaess, A. Ranganathan, and F. Dellaert, “iSAM: Incremental Smoothing and Mapping,” IEEE Transactions on Robotics, Vol. 24, No. 6, pp. 1365-1378, 2008. [https://doi.org/10.1109/TRO.2008.2006706]
• M.F. Fallon, M. Kaess, H. Johannsson, and J.J. Leonard, “Efficient AUV navigation fusing acoustic ranging and sidescan sonar,” IEEE International Conference on Robotics and Automation, Shanghai, pp. 2398-2405, 2011. [https://doi.org/10.1109/ICRA.2011.5980302]
• D. Langer and M. Hebert, “Building qualitative elevation maps from side scan sonar data for autonomous underwater navigation,” Proceedings IEEE International Conference on Robotics and Automation, pp. 2478-2483, 1991.
• Y. Pailhas, Y. Petillot, C. Capus, and K. Brown, “Real-time sidescan simulator and applications,” OCEANS 2009- EUROPE, pp. 1-6, 2009. [https://doi.org/10.1109/OCEANSE.2009.5278111]
• H.P. Johnson and M. Helferty. “The geological interpretation of side‐scan sonar,” Reviews of Geophysics, Vol. 28, No. 4, pp. 357-380, 1990. [https://doi.org/10.1029/RG028i004p00357]
• V.S. Blake, “The simulation of side‐scan sonar images,” Archaeological Prospection, Vol. 2, No. 1, pp. 29-56, 1995. [https://doi.org/10.1002/1099-0763(199503)2:1<29::AID-ARP6140020105>3.0.CO;2-P]
• S. Anstee, “Removal of range-dependent artifacts from sidescan sonar imagery,” DTIC Document, Tech. Rep. 2001.
• E. Coiras, Y. Petillot, and D. M. Lane, “Multiresolution 3-D Reconstruction From Side-Scan Sonar Images,” IEEE Transactions on Image Processing, Vol. 16, No. 2, pp. 382-390, 2007. [https://doi.org/10.1109/TIP.2006.888337]
• N. Neretti, N. Intrator, and Q. Huynh, “Target detection in side-scan sonar images: expert fusion reduces false alarms,” 2002.
• X.-F. Ye, P. Li, J.-G. Zhang, J. Shi, and S.-X. Guo, “A feature-matching method for side-scan sonar images based on nonlinear scale space,” Journal of Marine Science and Technology, Vol. 21, No. 1, pp. 38–47, 2016. [https://doi.org/10.1007/s00773-015-0330-5]
• D. Lowe, “Distinctive image features from scale-invariant keypoints,” International Journal of Computer Vision, Vol. 60, No. 2, pp. 91-110, 2004. [https://doi.org/10.1023/B:VISI.0000029664.99615.94]
• H. Bay, A. Ess, T. Tuytelaars, and L. Van Gool, “Speeded-up robust features (SURF),” Computer Vision and Image Understanding, Vol. 110, No. 3, pp. 346–359, 2008. [https://doi.org/10.1016/j.cviu.2007.09.014]
• E. Rublee, V. Rabaud, K. Konolige, and G. Bradski, “ORB: An efficient alternative to SIFT or SURF,” Proceedings of the IEEE International Conference on Computer Vision, pp. 2564–2571, 2011. [https://doi.org/10.1109/ICCV.2011.6126544]
• I. T. Ruiz, S. De Raucourt, Y. Petillot, and D. M. Lane, “Concurrent mapping and localization using sidescan sonar,” IEEE Journal of Oceanic Engineering, Vol. 29, No. 2, pp. 442–456, 2004. [https://doi.org/10.1109/JOE.2004.829790]
• C. de Jong, G. Lachapelle, S. Skone, and I. Elema, “Multibeam sonar theory of operation,” Delft University Press, Delft, the Netherlands, Tech. Rep. 2002.
• Antonelli, Gianluca, Thor I. Fossen, and Dana R. Yoerger. “Underwater robotics,” Springer handbook of robotics. Springer Berlin Heidelberg, pp. 987-1008, 2008. [https://doi.org/10.1007/978-3-540-30301-5_44]
• S. Agarwal, N. Snavely, S. M. Seitz and R. Szeliski, “Bundle Adjustment in the Large,” Proceedings of the European Conference on Computer Vision, pp. 29-42, 2010. [https://doi.org/10.1007/978-3-642-15552-9_3]
• D. H. Gwon, J. Kim, M. H. Kim, H. G. Park, T. Y. Kim, and A. Kim, “Development of a side scan sonar module for the underwater simulator,” 14^th International Conference on Ubiquitous Robots and Ambient Intelligence (URAI), pp. 662-665, 2017.