소형 자율운항 해양 이동체의 ICP 기반 지도 정합 측위 기법 개발 및 평가에 관한 연구

2.1 H/W 아키텍처

Fig. 1은 본 논문에서 활용한 데이터 획득에 이용되었던 소형 자율운항 해양 이동체의 H/W 구성을 보여주고 있다. 대상 해양 이동체는 위성항법시스템, 관성측정장치, 3D 라이다 등의 센서를 탑재하고, 데이터 처리를 위한 소형 산업용 컴퓨터를 내장하였다. 해당 이동체의 세부 사양은 Table 1과 같다. Fig. 1과 같이 하부 선체는 좌현과 우현에 각각 설치되었고, 쓰러스터 및 서보모터는 좌현 및 우현의 선수부와 선미부에 각각 1개씩 총 4개가 배치되어 추력 및 선수각 제어에 활용되었다. 대상 해양 이동체는 2023년 자율운항보트경진대회 규정에 따라 설계, 제작되었기 때문에, 실제 해상보다는 내수면 또는 소형 수조에서 시험하는 것을 고려하여 설계, 제작되었다. 다만, 모델 프리 측위 기법의 개발을 목표했기 때문에, 해양 이동체의 크기가 검증의 신뢰도를 저해하지 않는다고 판단했다.

Fig. 1

The H/W configuration of the small autonomous marine mobility

Table 1

Specification of the model-scale autonomous marine mobility (You, 2024)

Fig. 2는 상술한 소형 해양 이동체의 시험에 이용된 시험 환경을 보여주고 있다. 소형 해양 이동체는 직사각형 수조 내에서 장애물을 인지, 회피하며 주어진 과업을 달성하기 위하여 효율적이면서, 안전한 이동을 할 수 있도록 H/W 및 S/W가 설계되었다. 직사각형 수조의 네 모서리에 해당하는 경위도는 사전 측정을 통해서 제공되었다. 제안된 측위 기법은 단순한 형상을 갖는 수조 외에도, 항만, 안벽을 포함하여 고정된 해안선의 경위도가 제공될 때도 활용할 수 있다.

Fig. 2

The test environments of small autonomous marine mobility built in the University of Ulsan

2.2 S/W 아키텍처

Fig. 3은 본 논문에서 활용한 데이터 축적에 이용되었던 소형 자율운항 해양 이동체의 S/W 구성을 보여주고 있다. 제안된 측위 기법은 저비용, 저정밀 위성항법시스템, 관성측정장치, 3D 라이다를 포함한 온보드(on-board) 센서를 활용하며, 추정된 위치와 선수각 정보를 출력함으로써 측위 기능을 수행하도록 구성되었다.

Fig. 3

The S/W architecture for small autonomous marine mobility implementing proposed localization

제안된 지도 정합 기반 측위 기법은 총 4개의 세부 모듈로 나누어진다. 첫째, 위성항법시스템 및 관성측정장치 데이터 전처리 세부 모듈은 위성항법시스템 및 관성측정장치로부터 수신한 데이터를 국제횡축메르카토르(UTM) 좌표계로 변환, 전처리한다. 둘째, 3D 라이다 점군 전처리 세부 모듈은 라이다로 스캔한 원시 데이터(raw data)에 잡음 필터를 적용한 후, 2D 점군으로 압축, 경량화하여, 측위 모듈로 전달할 수 있도록 후처리한다. 셋째, ICP 기반 지도 정합 세부 모듈은 후처리된 2D 점군과 추정된 소형 해양 이동체의 위치 및 선수각 정보를 활용하여 지도 정합을 수행, 계측된 위치 및 선수각 정보를 생성한다. 넷째, 확장칼만필터 기반 필터링 모듈은 위성항법시스템, 관성측정장치, ICP 기반 지도 정합 세부 모듈에서 획득한 모든 정보를 융합하여, 최종적으로 소형 해양 이동체의 위치 및 선수각을 추정한다.

3.1 위성항법시스템 및 관성측정장치 전처리 기법

본 논문에서 개발한 소형 자율운항 해양 이동체의 측위 모듈에서는 위성항법시스템으로부터 경위도와 선수각을 획득하며, 관성측정장치로부터는 병진가속도, 각가속도 및 방위각 정보를 획득한다. 다만, 위성항법시스템에서 획득한 경위도는 국토지리정보원에서 제공하는 국토지리정보원 위성기준점서비스(NTRIP)를 이용, 보정함으로써 정밀도를 향상할 수 있었다. 보정된 경위도 좌표를 국제횡축메르카토르에서 정의된 2D 위치 정보로 변환한다. 방위각 정보는 위성항법시스템과 관성측정장치에서 각각 획득할 수 있는데, 안테나가 2개 이상 장착되지 않은 위성항법시스템의 경우 선수각의 신뢰도가 낮으므로, 본 논문에서는 관성측정장치에서 획득한 선수각을 주요 측정값으로 활용하였다. 관성측정장치는 병진가속도, 각가속도, 자기장 측정값을 이용하여 선수각을 산출하므로, 전자기 차폐 및 편향 교정을 거친 후 소형 해양 이동체에 장착하였다. 다만, 관성측정장치 특성상 선수각의 표류(drift)가 빈번하게 일어나기 때문에 추가적인 보정 기법이 필수적으로 요구된다.

3.2 3D 라이다 점군 전처리 기법

위성항법시스템 및 관성측정장치 외 추가적인 계측값으로, 본 연구에서는 3D 라이다에서 계측한 점군을 지도 정합에 사용하였다. 다만, 다채널 3D 라이다로부터 취득한 원시 점군에서 자선 구조물에 해당하는 점군, 관심 영역 외부의 지형 및 지물, 수면 반사 등에 의해 발생하는 점군이 함께 포함될 수 있다. 따라서 지도 정합에 필요한 점군만을 추출하고, 위치 및 선수각 추정에 불필요한 점군을 제거하기 위한 전처리 및 경량화 과정이 필요하다. 본 연구에서 사용한 점군 전처리 과정은 Fig. 4에 표현하였다. 점군 전처리 파라미터는 Table 2에 정리하였으며, Fig. 5는 Table 2의 파라미터를 이용한 점군 전처리 절차를 의사코드(pseudo code) 형태로 나타낸 것이다.

Fig. 4

Point cloud preprocessing results obtained from the 3D LiDAR data

Table 2

Parameters used in LiDAR point cloud preprocessing

Fig. 5

Pseudo code for preprocessing 3D LiDAR point cloud using the parameters defined in Table 2

Fig. 5의 입력 데이터는 다채널 3D 라이다로부터 취득한 3D 원시 점군 데이터이다. 먼저 원시 점군은 Table 2에 제시한 변환 파라미터를 이용하여 라이다 로컬 좌표계에서 선박 좌표계로 변환된다. 이후 선박 좌표계에서 사전에 정의한 영역(Bego) 내부의 점군을 자선 구조물에 해당하는 점군으로 가정하여 제거한다. 다음으로 관심영역(Region of Interest, ROI)으로 정의된 영역(BROI) 외부에 위치한 점군을 제거하고, 지도 정합에 필요한 관심 영역 내부의 점군만을 유지하였다.

ROI 필터링 이후에는 점군의 강도(intensity)를 제거하고, 자유 표면상에서 운항하는 소형 해양 이동체의 위치 및 선수각 추정에 필요한 수평면 정보만을 사용하기 위해 z값을 제거(z=0)하여 점군을 xy 평면으로 투영한다. 투영된 점군은 복셀 격자 필터(voxel grid filter)를 이용하여 다운샘플링 하고, 반경 기반 이상치 제거 필터(radius-based outlier filter)를 이용하여 고립점을 제거하였다. 이후 DBSCAN (Density-Based Spatial Clustering of Applications with Noise) 군집화(clustering)를 수행하고(Ester et al., 1996), 유효 군집(valid cluster)에 포함되지 않는 잡음점(noise point) 및 사전 정의한 임계값(Nmin)보다 점군의 개수가 적은 군집을 제거한다. 최종적으로 잡음 필터링 및 압축된 2D 점군 데이터를 반환함으로써, 3D 라이다 점군 전처리를 완료한다.

본 연구에서 수면 반사에 의해 발생하는 점군은 실제 구조물에서 획득되는 점군과 달리 일시적이고 소규모의 점군으로 나타나는 경우가 많았다. 따라서, 소규모 점군을 제거함으로써 수면 반사로 인한 잡음을 완화하고, 이를 통해 지도 정합에 사용되는 2D 점군의 안정성을 향상시켜 위치 및 선수각 추정에 불필요한 점군의 영향을 저감하고자 하였다.

앞에서 기술한 점군 전처리 과정에 대한 실제 처리 결과는 Fig. 4과 같다. 수조의 네 모서리는 고정되어 있으며, 장애물 부이는 수면에서 바람, 잔물결에 따라 약간의 움직임이 발생하도록 설치되어 있다. 해당 시점에 라이다로부터 획득한 원시 데이터 점군은 Fig. 4(a)와 같다. Fig. 1에서 표기된 것처럼, 16채널 라이다로부터 획득한 점군은 3차원 형상까지 식별할 수 있으며, 강도 정보까지 포함되어 있다. Fig. 4(a)는 전처리되기 전 3D 라이다로부터 획득한 원시 데이터를 보여주고 있으므로, 관심 영역을 초과한 지형, 지물의 점군도 포함되어 있음을 확인할 수 있다. 3D 라이다 점군 전처리를 거친 점군은 Fig. 4(b)와 같다. Fig. 4(a)와 달리, 관심 영역을 벗어난 점군이 제거되었음을 알 수 있다. Figs. 4(a), 4(b)에서는 관심 영역은 10m로 가정하였는데, 이 값은 자율운항 해양 이동체의 최소 요구조건, 운용 환경 등에 따라 조정 가능한 인자이다. 실제 소형 해양 이동체의 시험에 있어서는 Lpp의 4배에 해당하는 값을 사용했다. Figs. 4(a), 4(b) 모두 3차원 정보를 모두 포함하고 있음을 확인할 수 있다. 점군에서 클러스터링을 통해 수면 반사된 데이터를 제거한 결과는 Fig. 4(c)와 같다. Fig. 4(b)와 비교할 때, 수면상에 표시된 복수의 점군이 사라졌음을 확인할 수 있다. 16채널의 라이다 특성상 수면의 잔물결, 또는 반사에 따라 일시적으로 장애물이 있는 것으로 인지하는 오인지가 발생하고, 순간적으로 몇 개의 점군이 나타났다가 사라지는 것이 일반적이다. 여기서는 클러스터링 하는 점군의 개수를, 시행 착오를 통해 7개로 결정하였다.

3.3 ICP 기반 지도 정합 기법

ICP 기반 지도 정합을 수행할 때는 3D 라이다에서 계측한 점군과, 전역 특징 지도에서 생성한 점군 사이의 상대적인 위치 및 선수각 차이를 계산할 수 있다. 이 차이는 전역 좌표계에서 정의된 위치 및 선수각의 추가 계측값을 생성하는데 활용된다. ICP 기반 지도 정합 및 계측값 생성 절차는 Fig. 6에 나타내었다.

Fig. 6

Conceptual illustration of the proposed ICP-based map matching process for LiDAR-based localization

전역 특징 지도는 경위도로 주어지나, 지도 정합에 활용하기 위해 점군 형태로 변환하였다. 따라서, 항만, 안벽을 포함하는 해안선 외에도 위치가 고정된 장애물은 모두 전역 특징 지도로 정의할 수 있다. 2.1절에서 설명한 바와 같이 수조의 네 모서리 경위도는 사전에 주어진 값이기 때문에, 이를 실측 정보(ground truth)로 간주하였다. Fig. 6(a)와 같이, 수조의 경계면을 일정 간격으로 분할하여 전역 좌표계에서 정의되는 점군으로 변환하였다. 점군 생성 간격은 0.02m로 설정하였다. 간격이 작을수록 조밀한 점군을 얻을 수 있지만, 점의 개수가 증가하면 ICP 연산량도 증가한다. 따라서, ICP 수행 시 연산 효율성과 정합 정확도 사이의 균형을 고려하여 해당 값을 선택하였다.

전역 좌표계에서 정의된 특징 지도 점군은 다음과 같이 표현할 수 있다.

여기서 $$ {\mathit{m}}_i^G$$는 전역 좌표계에서 정의된 i번째 지도 점군이며, 위첨자 G는 전역 좌표계를 의미한다.

ICP 기반 지도 정합을 수행하기에 앞서, 전역 특징 지도에서 지도 정합에 사용할 점군을 추출했다. Fig. 6(b)와 같이, 먼저 확장칼만필터의 prediction step에서 추정된 위치 및 선수각을 이용하여 전역 좌표계를 기준으로 기술된 전역 특징 지도 점군을 지역 좌표계로 변환하였다. k번째 time step에서 확장칼만필터 prediction을 통해 추정된 위치 및 선수각은 다음과 같다.

여기서 $$ {\widehat{\eta }}_k^{\text{*}}$$는 ICP 기반 지도 정합을 수행하기 직전에 필터가 보유한 최신 위치 및 선수각 추정값이다. 위성항법시스템 및 관성측정장치 계측 갱신이 먼저 수행된 경우에는 해당 갱신 결과를 사용하고, 새로운 위성항법시스템 및 관성측정장치 계측값이 없는 경우에는 prediction step의 결과를 사용한다. x_est,k, y_est,k, ψ_est,k는 각각 k번째 time step에서 예측된 위치와 선수각이며, 이 값을 이용하여 지역 좌표계에서 전역 좌표계로의 변환행렬은 다음과 같이 구성하였다.

전역 특징 지도 점군은 위 변환행렬의 역행렬을 이용해 지역 좌표계로 변환된다.

여기서 $$ {\tilde{\mathit{m}}}_i^L$$은 homogeneous coordinate으로 표현된 점이며, 위첨자 L은 지역 좌표계를 의미한다. 이 과정을 통해 전역좌표계를 기준으로 기술된 특징 지도 점군을 현재 추정 위치와 선수각을 기준으로 하는 지역 좌표계의 점군으로 변환할 수 있다.

이때, 연산량을 줄이면서 현재 라이다 관측과 직접 비교 가능한 영역만을 사용하기 위해, 자선의 위치 기준으로 관심 영역을 벗어난 특징 지도 점군은 제외하였다. 즉, 기준점 주변 일정 반경(rc) 이내에 존재하는 지도 점군만을 추출하였다.이 반경은 3.5m로 설정하였다.

라이다로부터 획득한 점군을 3.2절의 전처리를 거쳐 획득한 점군도 동일하게 기준점 주변 3.5m 이내에 존재하는 점만을 사용하였다.

여기서 $$ M_k^L$$는 지역 좌표계로 변환된 전역 특징 지도 조각이며, $$ S_k^L$$는 동일한 지역 좌표계에서 표현된 라이다 계측 점군이다. 이와 같은 필터링은 ICP 정합에 사용되는 점의 수를 줄여 연산량을 감소시키며, 현재 위치와 관련성이 높은 점군만을 사용하게 하므로 안정적인 위치 및 선수각 추정에 유리하다.

전처리 및 좌표 변환을 거친 후, 지역 좌표계를 기준으로 기술된 전역 특징 지도 조각과 라이다 점군을 이용하여 초기 ICP 정합을 수행하였다. ICP는 두 점군 사이의 대응점을 반복적으로 탐색하고, 대응점 간 정합 오차를 최소화하는 강체 변환을 계산하는 대표적인 point cloud registration 방법이다(Besl and Mckay, 1992). 여기서는 수조 경계와 같이 평면 또는 선형 경계 구조가 지배적인 환경 특성을 고려하여 plane-to-plane ICP 알고리즘을 사용하였다. Plane-to-plane ICP는 두 점군의 대응점 사이의 유클리드 거리를 최소화하는 point-to-point ICP와 달리, 점군 주변의 국소 평면 또는 표면 방향성을 고려하여 평면 간 거리, 즉 plane-to-plane distance를 최소화하는 방식으로 동작한다(Segal et al., 2009; Lucks et al., 2021). 점 간 거리뿐 아니라 평면의 방향성을 함께 고려하므로, 점군이 벽과 같은 평면 구조를 가지는 경우 적합하며, 기하학적 왜곡이나 잡음, 이상치에 대해서도 상대적으로 강건한 정합 결과를 얻을 수 있다.

ICP 정합 결과는 기준 점군에서 측정 점군으로의 변환 행렬로 표현된다. 여기서 기준 점군은 전역 특징 지도에서 생성한 점군이고, 측정 점군은 라이다로 계측한 점군이다. 두 점군은 모두 지역 좌표계에서 정의된 상태로 ICP 정합 연산을 수행하므로, ICP에서 계산된 병진 변위 및 각도 변위 역시 지역 좌표계에서 정의된 값이다. ICP 정합 결과는 다음과 같이 나타낼 수 있다.

여기서 t_x,k.t_y,k,ψ_rel,k은 각각 ICP 정합을 통해 얻어진 상대 병진 변위와 상대 회전각이다. 이 변환은 기준 점군인 지도 점군이 라이다 점군과 일치하도록 이동 및 회전하는 관계를 의미한다.

Plane-to-plane ICP 정합 문제는 다음과 같이 표현할 수 있다.

여기서 $$ {\mathit{m}}_{c\left(j\right),k}^L$$는 라이다 점군 $$ {\mathit{s}}_{j,k}^L$$에 대응되는 지도 점군이며, d_pl은 대응점 주변의 국소 평면 또는 표면 방향성을 고려한 정합 오차를 의미한다.

라이다 점군에는 수조 경계 외의 불필요한 반사점이나 잡음이 포함될 수 있기 때문에, 2회에 걸쳐 ICP 정합을 수행하였다. 먼저 Fig. 6(c)와 같이 1차 ICP 정합을 통해 라이다에서 계측한 점군과 전역 특징 지도 점군 사이의 초기 정합을 수행하였다. 이후 Fig. 6(d)와 같이 전역 특징 지도 점군을 기준으로 일정 거리 내에 존재하는 라이다 점군만 남기고, 그 외의 점군은 이상치로 판단하여 제거하였다. 이를 수식으로 표현하면 다음과 같다.

여기서 $$ {{}^S\mathit{T}}_{ICP,k}^{\left(1\right)}$$는 1차 정합 결과이며, d_th는 이상치 제거를 위한 거리 임계값이다. 이 과정을 통해 잡음 또는 불필요한 점들을 제거한 후, 최종적으로 남은 라이다 점군 $$ S_{k,in}^L$$을 이용하여 2차 ICP 정합을 수행하였다. 이를 통해 보다 높은 정확도를 갖는 최종 정합 결과를 얻을 수 있다.

최종 ICP 정합 결과로부터, 두 점군 사이의 상대 위치, 상대 회전 정보, 그리고 평균제곱근오차를 계산하였다. 평균제곱근오차는 다음과 같이 정의된다.

여기서 N_in은 2차 ICP 정합에 사용된 inlier 점군의 개수이다.

최종 정합된 ICP 결과에서 얻어진 상대 위치 및 회전 정보는 측위 필터에서 추가적인 계측값으로 활용하였다.앞서 설명한 것처럼, ICP 결과는 기준 점군에서 측정 점군으로의 변환 행렬을 제공한다. 따라서 ICP에서 계산된 병진 변위 t_x,k.t_y,k는 지도 점군이 라이다 점군에 정합되기 위해 이동해야 하는 방향을 나타낸다. 반면, 위치 계측값은 라이다 계측 결과를 전역 특징 지도에 맞추어 보정한 위치로 정의해야 하므로, 지역 좌표계에서의 ICP 기반 위치 보정량은 ICP 병진 변위의 반대 방향으로 계산하였다.

이를 지역 좌표계에서의 변위 벡터로 표현하면 다음과 같다.

지역 좌표계에서 계산된 위치 보정량은 확장칼만필터 prediction 결과로부터 구성한 변환행렬 T_trans,k를 이용하여 지구고정좌표계로 변환하였다.

따라서 ICP 기반 위치 계측값은 다음과 같이 정의된다.

선수각 계측값도 같은 방식으로 계산하였다. ICP 정합을 통해 얻은 상대 회전각 ψ_rel,k는 지도 점군이 라이다 점군에 정합되기 위해 필요한 회전량이므로, 자선의 선수각 계측값은 확장칼만필터 prediction으로부터 추정된 선수각에서 해당 상대 회전각을 차감하여 계산하였다.

여기서 상대 회전각 ψ_rel,k는 $$ {{}^L\mathit{T}}_{ICP,k}$$의 회전행렬 성분으로부터 계산하였다.

최종적으로 ICP 기반 지도 정합으로부터 얻어지는 계측벡터는 다음과 같이 구성된다.

평균제곱근오차 RMSEk는 추가적인 계측값을 이용하여 계측 갱신을 수행할 때, 계측 갱신 수행 여부 판단 및 계측 잡음 공분산 행렬을 설정하는 기준값으로 사용하였다. 즉, RMSEk가 큰 경우에는 두 점군 사이의 정합 품질이 낮은 것으로 판단하고, 해당 time step의 ICP 기반 계측값을 계측 갱신에 사용하지 않는다. 반대로 RMSEk가 충분히 작은 경우에는 ICP 기반 계측값을 확장칼만필터의 계측 갱신에 사용한다. 계측 잡음 공분산 행렬의 설정 방법과 계측 갱신 절차는 3.4절에서 설명한다.

이와 같은 추가 계측값을 사용함으로써, 라이다 점군을 이용한 지도 정합 기반 측위 보정이 가능하다. 또한 저비용, 저정밀 측위 장치로 얻어진 위치 및 선수각 정보를 낮은 메모리 자원을 이용해 보정할 수 있다. 특히, 관심 영역 기반 필터링과 2회에 걸쳐 수행된 ICP 정합을 통해 점군에 포함된 잡음과 불필요한 반사점의 영향을 줄이면서도 안정적이고 효율적인 정합 결과를 얻을 수 있었다.

3.4 확장칼만필터 기반 필터링 기법

소형 해양 이동체의 측위를 위해 확장칼만필터를 적용하였다. 소형 해양 이동체의 유체동역학 모델과 비선형성을 반영할 수 있는 필터링 기법을 도입할 수 있으나, 본 논문의 목적은 상세한 유체동역학 모델이 없거나 시스템 식별법을 통해 유체동역학 모델을 구성하기 어려운 소형 해양 이동체에서도 적용 가능한 측위 기법을 개발하는 것이다. 따라서 저비용, 저정밀 장치와 제한된 연산 자원에서도 효율적으로 사용할 수 있도록 단순 운동 모델과 다중 계측값을 결합하는 확장칼만필터 구조를 사용하였다.

제안된 측위 기법을 위한 확장칼만필터는 위성항법시스템 및 관성측정장치로부터 획득한 계측값과 3.3절에서 설명한 ICP 기반 지도 정합 계측값을 순차적으로 융합한다. 위성항법시스템, 관성측정장치, 3D 라이다는 서로 다른 샘플링 주기와 통신 지연을 가질 수 있으므로, 필터는 10Hz의 고정 샘플링 주기로 설정하되, 각 샘플링 주기에서 새로운 계측값이 수신되는 경우에만 해당 계측 갱신을 수행하도록 구성하였다. 상세한 계산 절차는 Fig. 7의 의사코드에 정리하였다.

Fig. 7

Pseudo code of EKF-based filtering using GNSS/IMU and ICP-based map matching measurements

상태벡터는 전역 좌표계에서 정의된 2차원 위치, 선수각, 종방향 속력으로 구성하였다.

여기서 x_k,y_k는 전역 좌표계에서의 위치, ψ_k는 선수각, U_k는 종방향 속력이다.

대상 해양 이동체의 유체동역학 모델이 주어지지 않았으므로, 다음과 같은 단순 운동모델을 공정모델로 사용하였다.

여기서 γ_k는 선수각속도이며, 관성측정장치로부터 획득된 계측값을 사용하였다.

이 연속시간 시스템 모델을 고정된 샘플링 시간(Δt)을 가정하여, 순방향 오일러 방법을 이용하여 이산화하면 다음과 같다.

공정모델은 소형 해양 이동체의 상세 유체동역학을 직접 반영하지 않는 단순 운동모델이므로, 실제 운동에서 발생하는 모델링 동역학, 선수각속도 입력 오차, 속력 변화, 시간 동기화 오차 등을 공정 잡음으로 고려하였다. 이에 따라 공정 잡음 공분산 행렬은 다음과 같이 대각행렬 형태로 설정하였다.

여기서 Δt는 필터의 샘플링 시간이다. 위 공정 잡음 공분산 행렬은 센서의 계측 잡음을 의미하는 것이 아니라, 단순 공정모델이 설명하지 못하는 운동 불확실성을 반영하기 위한 경험적 튜닝 파라미터이다. 또한 각 상태 성분 사이의 공정 잡음 상관성은 고려하지 않고 독립적인 것으로 가정하였다.

위성항법시스템 및 관성측정장치로부터 획득한 계측벡터는 다음과 같이 정의하였다.

여기서 위치는 및 종방향 속력은 위성항법시스템으로부터 획득한 계측값을 사용하였으며, 선수각은 관성측정장치로부터 획득한 계측값을 사용하였다. 위 계측벡터는 상태벡터와 동일한 순서로 구성되므로, 관측행렬을 다음과 같이 정의하였다.

ICP 기반 지도 정합으로부터 얻은 계측벡터는 3.3절에서 정의한 바와 같이 위치 및 선수각으로 구성된다.

ICP 기반 계측값은 종방향 속력에 대한 직접 계측값을 제공하지 않으므로, ICP 기반 계측값에 대한 관측 행렬은 다음과 같이 정의하였다.

위성항법시스템 및 관성측정장치로부터 획득한 계측 잡음 공분산 행렬은 다음과 같이 정의하였다.

ICP 기반 계측 잡음 공분산 행렬은 2차 ICP 정합 후 얻어진 최종 평균제곱근오차(RMSEk)를 이용하여 가변적으로 설정하였다.

여기서 $$ {\mathit{R}}_{ICP,0}$$는 기준 ICP 계측 잡음 공분산 행렬이며, w_ICP(RMSE_k)는 ICP 정합 품질에 따라 계측 잡음 공분산을 조정하기 위한 가중치이다. 설계한 가중치는 Fig. 8과 같다. RMSE_k가 작을수록 두 점군 사이의 정합 품질이 높은 것으로 판단하여 작은 가중치를 부여하고, RMSE_k가 커질수록 ICP 기반 계측값의 신뢰도가 낮은 것으로 판단하여 큰 가중치를 부여하였다. 또한, RMSE_k가 사전에 설정한 임계값 RMSE_th보다 큰 경우에는 두 점군 사이의 정합 품질이 충분하지 않은 것으로 판단하여 ICP 기반 계측값을 갱신에 사용하지 않았다.

Fig. 8

Measurement noise matrix’s weight distribution according to RMSE of ICP-based map matching results

계측값에 포함된 이상치의 영향을 줄이기 위하여, 계측값 모두에 대하여 추정한 공분산에 대한 3 시그마 바운드(3 sigma bound)를 검증 게이트(validation gate)로 사용하였다. 계측 잔차를 y_a,k, 잔차 공분산을 S_a,k라고 할 때, 각 계측 성분은 다음 조건을 만족할 때 유효한 계측값으로 판단하였다.

여기서 $$ {\mathit{y}}_{a,k}^{\left(i\right)}$$는 계측 잔차의 i번째 성분이며, $$ {\mathit{S}}_{a,i}^{\left(i,i\right)}$$는 잔차 공분산 행렬의 i번째 대각 성분이다. 아래첨자 a는 G 또는 ICP를 의미한다. 따라서 RMSEk 조건과 3 시그마 검증 게이트를 모두 만족하는 경우에만 ICP 기반 지도 정합 결과를 확장칼만필터의 계측 갱신에 사용하였다. 이를 통해 정합 품질이 낮거나 이상치가 포함된 ICP 기반 계측값이 최종 위치 및 선수각 추정값에 과도하게 반영되는 것을 방지하였다.

4.1 평가 조건 및 성능 지표

제안된 측위 기법의 성능을 검토하기 위해 Fig. 1에서 설명한 소형 자율운항 해양 이동체의 시험 데이터를 이용하였다. 시험 데이터는 2023년 8월 7일부터 2023년 8월 11일까지 경상남도 창원시 로봇랜드에서 진행된 2023 자율운항보트경진대회(KABOAT) 중 취득한 데이터이며, 자율운항 해양 이동체가 직사각형 수조 내에서 한쪽 외벽에서 반대쪽 외벽까지 이동한 구간을 대상으로 하였다. 해당 시험은 소형 자율운항 해양 이동체의 측위, 인지, 판단/계획, 조종/제어 모듈의 정상 작동 여부를 확인하기 위한 목적으로 수행되었다.

대상 해양 이동체는 저비용, 저정밀 위성항법시스템과 관성측정장치를 탑재하고 있으며, 시험 당시 공간적 제약으로 인해 고정밀 위성항법시스템이나 별도의 외부 측위 장치를 참값으로 설치할 수 없었다. 따라서 위치 및 선수각 추정 결과를 직접적인 실측 정보와 비교하는 방식의 절대 오차 평가는 수행하기 어려웠다. 이러한 제약을 고려하여, 측위 성능은 정성 평가와 간접적 정량 평가로 나누어 검토하였다.

비교 대상은 다음 세 가지 방법으로 설정하였다. 첫 번째 방법은 위성항법시스템과 관성측정장치 계측값만을 이용하여 확장칼만필터를 적용한 경우로써, Figs. 9-14에서 “EKF”로 표시하였다. 두 번째 방법은 확장칼만필터에 ICP 기반 지도 정합 결과를 선수각 보정에만 사용한 경우로써, “EKF+ICP(heading)”로 표시하였다. 세 번째 방법은 3.3절에서 설명한 ICP 기반 지도 정합을 통해 생성한 위치 및 선수각 계측값을 모두 확장칼만필터의 계측 갱신에 사용한 경우로써, “EKF+ICP(pos+heading)”로 표시하였다. 세 번째 방법이 본 논문에서 제안한 측위 기법에 해당한다.

Fig. 9

Time series of estimated position and heading angle implementing proposed localization technique

Fig. 10

Time series of IoU between the transformed global feature map and LiDAR point cloud

Fig. 11

Estimated trajectories and selected snapshot time at trajectory at 30.7 sec

Fig. 12

Snapshot of map matching results at 30.7 sec

Fig. 13

Estimated trajectories and selected snapshot time at 64.8 sec

Fig. 14

Snapshot of map matching results at 64.8 sec

정성 평가 지표로는 위성항법시스템 및 관성측정장치 계측값과 각 방법으로 추정된 상태 사이의 차이를 사용하였다. 이 값은 고정밀 참값에 대한 절대 오차는 아니지만, 저비용 센서에서 획득한 계측값과 각 필터링 결과 사이의 차이를 보여주므로, 각 방법이 센서 계측값에 포함된 편향 또는 표류를 어떻게 보정하는지 분석하는 데 활용할 수 있다.

평가용 잔차는 다음과 같이 정의하였다.

여기서 $$ {\mathit{e}}_{G,k}^{\left(m\right)}$$는 m번째 비교 방법에 대한 평가용 잔차이며, m ∈ {M₁, M₂, M₃}이다. x_G,k, y_G,k은 위성항법시스템으로부터 획득한 위치 계측값이고, ψ_I,k는 관성측정장치로부터 획득한 선수각 계측값이다. $$ {\widehat{\mathit{x}}}_{k\mid k}^{\left(m\right)}$$는 m번째 방법으로 계산된 최종 상태 추정값이다. 단, 이 잔차는 고정밀 참값에 대한 오차가 아니라, 저비용 센서 계측값과 추정값 사이의 차이이므로 정성 평가 지표로 활용하였다.

간접적 정량 평가 지표로는 교집합 대비 합집합(Intersection over Union, IoU)을 사용하였다. IoU는 두 집합 간의 겹침 정도를 정량적으로 나타내는 지표로, 본 논문에서는 전역 특징 지도 점군과 3D 라이다 계측 점군 사이의 공간적 일치도를 평가하는 데 사용하였다. 제안된 측위 기법을 통해 위치 및 선수각이 정확하게 추정되었다면, 추정 자세를 기준으로 변환된 전역 특징 지도 점군과 라이다 계측 점군이 같은 지역 좌표계에서 더 많이 겹쳐야 한다. 따라서 IoU가 클수록 전역 특징 지도와 라이다 계측 결과 사이의 정합성이 높으며, 이는 위치 및 선수각 추정 정확도가 상대적으로 높다는 것을 간접적으로 의미한다.

IoU 계산을 위해 먼저 각 비교 방법의 추정 자세를 이용하여 전역 특징 지도 점군을 지역 좌표계로 변환하였다. 이는 3.3절에서 설명한 ICP 기반 지도 정합 과정과 동일한 좌표 변환 절차를 따른다. 이후 변환된 전역 특징 지도 점군과 3D 라이다 계측 점군을 동일한 2D 평면 격자에 투영하였다. 격자 해상도는 0.2m로 설정하였으며, 각 격자 내부에 하나 이상의 점군이 존재하면 해당 격자를 점유된 셀로 정의하였다.

세 가지 비교 방법(m)에 대해, 전역 특징 지도 점군으로부터 생성한 점유 격자 집합을 $$ A_k^{\left(m\right)}$$, 라이다 계측 점군으로부터 생성한 점유 격자 집합을 B_k라고 하면, IoU는 다음과 같이 계산된다.

여기서 $$ A_k^{\left(m\right)}\cap B_k$$는 전역 특징 지도 점군과 라이다 계측 점군이 공통으로 점유하는 격자 집합이며, $$ A_k^{\left(m\right)}\cup B_k$$는 두 점군 중 하나라도 점유한 전체 격자 집합이다. 따라서 $$ {IoU}_k^{\left(m\right)}$$가 1에 가까울수록 두 점군의 공간적 일치도가 높고, 0에 가까울수록 두 점군의 불일치가 크다는 것을 의미한다.

정리하면, 본 논문에서는 고정밀 참값을 직접 확보하기 어려운 실험 조건을 고려하여, 위성항법시스템 및 관성측정장치 계측값 대비 추정값의 차이를 정성 평가 지표로 사용하고, 전역 특징 지도 점군과 라이다 계측 점군 사이의 IoU를 간접적 정량 평가 지표로 사용하였다. 이를 통해 제안된 ICP 기반 지도 정합 측위 기법이 위치 및 선수각 추정 결과에 미치는 영향을 분석하였다.

4.2 위치 및 선수각 추정 결과 분석

제안된 측위 기법의 정성 평가를 위해 앞서 기술한 세 가지 방법의 위치 및 선수각 추정 결과를 비교하였다. Fig. 9는 각 방법으로 추정한 위치 및 선수각 시계열을 보여준다. Fig. 9(a)의 x 좌표에서는 위성항법시스템 계측값과 세 가지 추정 결과 사이의 차이가 상대적으로 작게 나타났다. 반면, Fig. 9(b)의 y 좌표는 ICP 지도 정합을 위치 및 선수각 보정에 모두 적용한 경우가 다른 두 방법과 뚜렷하게 구분되는 차이를 보였다. 특히 29초에서 32초 사이의 확대 영역을 보면, EKF 및 EKF+ICP(heading)은 위성항법시스템 계측값과 유사한 경향을 보이는 반면, EKF+ICP(pos+heading)은 다른 위치로 보정되는 것을 확인할 수 있다. 이는 제안된 방법에서 위치 추정이 불안정해졌다는 의미가 아니라, 3.3절에서 설명한 ICP 기반 지도 정합 계측값이 확장칼만필터의 계측 갱신에 반영되면서 위성항법시스템 계측값에 포함된 위치 편향이 보정된 결과로 해석할 수 있다. 즉, ICP 기반 지도 정합을 선수각 보정에만 사용하는 경우에는 위치 상태에 대한 보정이 수행되지 않으므로, 위성항법시스템 계측값에 포함된 위치 편향을 충분히 보상하지 못한다. 반면, 위치 및 선수각을 모두 ICP 기반 계측값으로 보정하는 경우에는 전역 특징 지도와 라이다 점군의 정합 결과가 위치 상태에도 반영된다.

Fig. 9(c)는 선수각 추정 결과를 보여준다. EKF+ICP(heading)과 EKF+ICP(pos+heading)은 서로 유사한 선수각 추정 결과를 나타내며, 이는 두 방법 모두 ICP 기반 지도 정합 결과를 선수각 보정에 사용했기 때문이다. 반면, 관성측정장치 계측값과 EKF만 적용한 결과에서는 일부 구간에서 시간에 따라 변하는 선수각 편향이 확인된다. 특히 31초에서 40초, 45초에서 52초 구간에서 차이가 비교적 크게 나타났다. 이러한 결과는 ICP 기반 지도 정합 결과가 관성측정장치의 선수각 표류 또는 시간에 따라 변화하는 편향을 보정하는 데 활용될 수 있음을 보여준다.

다만, Fig. 9만으로는 어느 방법이 실제 전역 특징 지도와 더 잘 정합되는지를 직접적으로 판단하기 어렵다. 따라서 다음 절에서는 전역 특징 지도 점군과 라이다 계측 점군 사이의 교집합 대비 합집합을 이용하여 각 방법의 지도 정합 성능을 간접적으로 평가하였다.

4.3 IoU 및 지도 정합 결과 기반 간접 정량 평가

제안된 측위 기법의 간접적 정량 평가를 위해 4.1절에서 정의한 교집합 대비 합집합을 계산하였다. Fig. 10은 세 가지 비교 방법에 대한 IoU 시계열을 보여준다. EKF만 이용한 경우 IoU의 평균값은 0.11, ICP 정합을 선수각 보정에만 이용한 경우 IoU의 평균값은 0.05, ICP 정합을 위치 및 선수각 보정에 모두 이용한 경우 IoU의 평균값은 0.49로 나타났다. 따라서 ICP 기반 지도 정합 결과를 위치 및 선수각 보정에 모두 적용했을 때, 전역 특징 지도 점군과 라이다 계측 점군 사이의 공간적 일치도가 가장 높게 나타났다.

Fig. 10에서 측위 모듈의 동작 초기에는 모든 방법이 0.45 이하의 낮은 IoU를 보인다. 그러나 약 12초 이후부터 EKF+ICP(pos+heading)의 IoU가 뚜렷하게 증가하고, 이후에도 교란에 따른 변동은 있으나 다른 두 방법보다 높은 수준을 유지한다. 이는 해당 시점 이후부터 전역 특징 지도 점군과 라이다 계측 점군 사이의 정합성이 향상되었음을 의미한다. 반면, EKF+ICP(heading)은 일부 구간에서 EKF만 적용한 경우보다 더 낮은 IoU를 보인다. Fig. 9(c)에서 확인한 것처럼 선수각 보정 효과는 나타났지만, 위치 편향이 보정되지 않았기 때문에 전역 특징 지도 점군과 라이다 계측 점군이 평행 이동된 상태로 남아 IoU가 낮게 계산된 것으로 해석할 수 있다.

이러한 IoU 결과를 구체적으로 확인하기 위해, Figs. 11–14에 특정 시점에서의 궤적과 지도 정합 결과를 도시하였다. Fig. 11과 Fig. 13은 각각 30.7초와 64.8초 부근에서의 자선 위치 추정 결과를 보여주며, Fig. 12와 Fig. 14는 해당 시점에서 전역 특징 지도 점군과 라이다 계측 점군의 정합 상태를 나타낸다.

Fig. 12는 30.7초에서의 정합 결과를 보여준다. Fig. 12(a)의 EKF만 적용한 경우에는 라이다 계측 점군과 수조 경계가 충분히 겹치지 않는다. Fig. 12(b)의 EKF+ICP(heading)은 선수각은 보정되었으나 위치가 보정되지 않았기 때문에, 라이다 계측 점군과 수조 경계가 서로 평행 이동된 형태로 나타난다. 반면, Fig. 12(c)의 EKF+ICP(pos+heading)은 위치와 선수각이 함께 보정되어 라이다 계측 점군이 전역 특징 지도에서 생성된 수조 경계와 더 잘 겹치는 것을 확인할 수 있다.

Fig. 14는 64.8초에서의 정합 결과를 보여준다. Fig. 14(a)의 EKF만 적용한 경우와 Fig. 14(b)의 EKF+ICP(heading)은 수조 모서리 부근에서 라이다 계측 점군과 전역 특징 지도 점군 사이에 위치 차이가 남아 있다. 반면, Fig. 14(c)의 EKF+ICP(pos+ heading)은 수조의 모서리 형상과 라이다 계측 점군이 비교적 잘 일치한다. 이는 ICP 기반 지도 정합에서 얻어진 위치 및 선수각 계측값이 확장칼만필터의 계측 갱신에 함께 반영되면서, 위성항법시스템 계측값에 포함된 위치 편향과 관성측정장치의 선수각 편향이 동시에 보정되었음을 의미한다.

결과적으로, ICP 기반 지도 정합을 선수각 보정에만 사용하는 경우에는 선수각 추정 개선 효과는 얻을 수 있으나, 위치 편향이 남아 전역 특징 지도와 라이다 점군의 공간적 일치도가 낮아질 수 있다. 반면, ICP 기반 지도 정합 결과를 위치 및 선수각 보정에 모두 사용하는 경우에는 두 점군 사이의 정합성이 향상되며, 이는 Fig. 10의 IoU 증가와 Figs. 12, 14의 snapshot 결과를 통해 확인할 수 있다.