Precise Vehicle Localization Using Gaussian Mixture Map Based on Road Marking

Abstract

It is essential to estimate the vehicle localization for an autonomous safety driving. In particular, since LIDAR provides precise scan data, many studies carried out to estimate the vehicle localization using LIDAR and pre-generated map. The road marking always exists on the road because of provides driving information. Therefore, it is often used for map information. In this paper, we propose to generate the Gaussian mixture map based on road-marking information and localization method using this map. Generally, the probability distributions map stores the single Gaussian distribution for each grid. However, single resolution probability distributions map cannot express complex shapes when grid resolution is large. In addition, when grid resolution is small, map size is bigger and process time is longer. Therefore, it is difficult to apply the road marking. On the other hand, Gaussian mixture distribution can effectively express the road marking by several probability distributions. In this paper, we generate Gaussian mixture map and perform vehicle localization using Gaussian mixture map. Localization performance is analyzed through the experimental result.

1. INTRODUCTION

안전한 자율 주행을 위해서는 정밀한 차량 위치 추정이 반 드시 수행되어야 한다. 이를 위해서는 차선 유지가 필수적이며, 이를 위한 위치 정확도는 95%의 신뢰도로 횡 방향 0.5 m 이내의 정확도(Ansari & Feng 2013)가 요구된다. 또한, 종 방향의 경우 정해진 기준은 존재하지 않지만, 일반적으로 95%의 신뢰도로 1 m 이내의 위치 정확도가 요구된다. 차량 위치 추정에 사용되는 Global Positioning System (GPS)과 같은 Global Navigation Satellite System (GNSS)은 위성이 보내오는 전파를 이용하여 위 치를 결정한다. 하지만, 도심 지역과 같이 건물이 많은 환경에서는 전파가 가로 막혀 수신되는 위성 전파의 개수가 적으며, 건물에 반사되어 전파가 들어와서 생기는 다중 경로 오차로 인해 GPS Real-Time Kinematic (RTK)와 같은 정밀 항법 장비를 이용하여도 위와 같은 조건을 충족하지 못한다.

이 문제를 해결하기 위해, 각종 센서들을 이용하여 지도를 생성하고 센서 데이터와 매칭하여 차량 위치 추정을 수행하는 연구들이 진행되고 있다. 특히, 3D LIDAR는 주변 환경을 스캔하여 정밀한 거리와 반사도 정보를 3D 점 군으로 제공하기 때문에, 많이 이용되고 있다.

그 중에서, 확률 분포 지도(Saarinen et al. 2013, Akai et al. 2017)는 LIDAR점 군을 확률 분포로 표현한 지도이다. 일반적으 로 단일 가우시안 확률 분포 기반의 지도로 생성되며, 점 군을 일정한 크기의 격자로 나누어 격자 별 평균과 공 분산을 도출하고 저장한 지도이다. 점 군의 형상을 확률 분포를 이용하여 표현하기 때문에 지도 용량도 적고 확률 분포가 점 군의 형상과 비슷할 경우 지도 매칭 성능도 준수하다. 또한, 점대 점 매칭인 Iterative Closest Point (ICP) 스캔 매칭(Segal et al. 2009)과 달리 상대 적으로 적은 개수의 확률 분포와 매칭을 수행하기 때문에 처리 속도도 빠르다. 일반적인 매칭 방법으로 Normal Distributions Transform (NDT) 스캔 매칭(Biber & Straßer 2003, Magnusson 2009)이 있다. NDT 스캔 매칭은 점 군과 지도 격자 내의 확률 분 포 간의 확률 밀도 함수를 도출하고 이를 이용하여 생성된 비용 함수를 통해 최적의 해를 도출하는 방법이다. 이를 위해, 비선형 함수 최적화 기법이 사용되며 함수의 최소 값을 찾아 수렴할 때 까지 계속 반복하여 수행하기 때문에, 수렴 속도에 따라 성능 및 처리 속도가 결정된다. 하지만 단일 격자 기반 지도의 경우 격자 크기에 따라 매칭 성능 및 처리 속도가 달라진다. 격자 크기가 클 경우 수렴 속도가 빠르기 때문에 매칭이 빨리 이루어진다. 반면, 복잡한 형태의 물체의 경우 이를 제대로 표현하지 못하기 때문에 매칭 성능이 떨어지는 단점이 있다. 격자 크기가 작을 경우 복잡 한 형상도 표현이 가능하지만, 격자 개수가 많기 때문에 지도 용량이 증가하며 수렴 속도가 느리기 때문에 매칭 속도도 감소한다. 또한, local minima에 빠질 가능성이 높기 때문에 위치 오차가 커질 경우 다시 오차를 보정하기 어려운 단점이 있다.

단일 격자 확률 분포 지도의 단점을 해결하기 위한 방법으로 복수 격자 기반 확률 분포 지도가 있다. 기존의 복수 격자 기반 확률 분포 지도의 경우 다중 격자 가우시안 혼합 지도(Wolcott & Eustice 2017)가 있다. 다중 격자 가우시안 혼합 지도는 LIDAR로 스캔한 건물의 높이를 격자 별로 확률 분포로 변환하여 가장 작은 크기의 격자에 저장하고 GMM을 이용하여 크기가 큰 격자에 작은 격자의 확률 분포를 합치는 방식으로 복수 격자 확률 분포 지도를 생성하였다. 이 방법을 이용할 경우, Root mean square error (RMSE)는 횡 방향, 종 방향 각각 10 cm와 13 cm로 매우 정밀하게 추정함을 확인할 수 있다. 하지만, 검색 영역을 설정하고, 격자 크기별로 지도 매칭을 수행하기 때문에, 처리 속도가 매우 느리며 지도 용량도 크다.

본 논문에서는, Gaussian mixture modeling (GMM)을 이용하여 가우시안 혼합 지도를 생성하고 이를 이용하여 도심 지역에서 정밀한 차량 위치 추정 방법에 대해 제안한다. 가우시안 혼합 지도는 여러 개의 격자 크기를 가진 다중 격자 확률 분포 지도와 달리 격자 크기가 정해져 있지 않다. 따라서, 물체의 크기에 따라 다양한 크기의 확률 분포로 표현이 가능하다. 가우시안 혼합 지도 는 추출된 점 군을 물체 별로 나누고, 이를 GMM을 통해 확률 분포로 변환한다. 또한, 물체 별로 여러 개의 확률 분포를 생성할 수 있기 때문에 물체의 형태를 다양한 크기의 확률 분포로 효과적으로 표현할 수 있다.

본 논문의 기여도는 크게 2가지이다. 첫 번째는 GMM을 이용하여 차량 측위에 이용 가능한 노면 표시 기반 확률 분포 지도 생성이다. 기존 단일 가우시안 기반의 확률 분포 지도를 생성할 경우, 복잡한 형상의 노면 표시를 제대로 표현하려면 격자 크기가 작아야 한다. 하지만, 가우시안 혼합 지도는 GMM을 통해 하나의 물체를 여러 확률 분포로 표현 가능하기 때문에, 효과적으로 노면 표시를 표현할 수 있다. 따라서, 빠르고 정밀한 지도 매칭을 통해 정밀한 차량 측위가 가능하다. 두 번째는 가우시안 혼합 지도 기반 차량 측위 기법 제안이다. 가우시안 혼합 지도는 기존 단일 격자 확률 분포 지도와 다르게 정해진 격자 크기가 존재하지 않는다. 따라서, 지도와 매칭 하기 위한 새로운 자료 연관 기법이 필요하다. 본 논문에서는 가우시안 혼합 지도에 적합한 지도 매칭 기법을 제안하고 이를 기반으로 정밀한 차량 측위 결과를 도출하였다.

본 논문의 구성은 다음과 같다. 2장에서 가우시안 혼합 지도 생성 방법에 대해 설명하고, 3장에서 가우시안 혼합 지도 기반 차량 위치 추정 방법에 대해 설명한다. 4장에서 실험 결과를 통해 가우시안 혼합 지도 기반 차량 위치 추정 성능을 분석하고, 5장 결론으로 마무리한다.

2. GAUSSIAN MIXTURE DISTRIBUTIONS MAP GENERATION

지도를 이용하여 위치 추정을 수행하기 위해서는 정밀한 지도 궤적 도출이 필수적이다. 일반적으로, 지도 생성에 사용되는 Mobile Mapping System 장비 (Geiger et al. 2015)의 경우, 고가의 GPS(RTK)/Inertial Navigation System (INS)를 이용하여 지도 궤적을 도출한다. 그러나, 도심 지역과 같이 주변에 건물이 많은 경우, 고가의 장비를 이용하여도 다중 경로로 인한 오차가 발생한다.

Fig. 1은 가우시안 혼합 지도 생성을 위한 도심 지역에서의 지도 궤적이다. Fig. 1에서 보듯이, 모든 구간에 걸쳐 건물이 밀집해 있는 것을 확인할 수 있다. 이는 다중 경로 오차로 인한 지도 궤적의 불일치를 야기하며, 생성된 지도의 신뢰성을 보장할 수 없게 한다. 그러므로, pose graph 최적화 기반 정밀 지도 궤적 생성을 통해 지도 궤적의 불일치 문제를 해결하였다. Pose graph 최적화를 위한 초기 궤적은 GPS(RTK)/INS 장비 (NovAtel RTK/SPAN system)를 이용하였으며, graph 생성을 위한 edge measurement 는 3D LIDAR로 스캔한 점 군을 기반으로 ICP 스캔 매칭을 통해 도출된 궤적 노드 간 상대 위치 및 자세를 이용하였다.

Fig. 1. Map trajectory in urban area.

Fig. 2는 pose graph 최적화 결과이다. 왼쪽은 각각 GPS(RTK)/ INS 및 pose graph 최적화를 통해 도출된 궤적을 기반으로 생성한 노면 표시 지도이며(Kim et al. 2019), 오른 쪽은 각각의 궤적 도출 결과이다. GPS(RTK)/INS 궤적으로 생성된 노면 표시 들은 서로 불일치하는 반면, pose graph 최적화로 도출된 궤적으로 생성된 노면 표시들은 서로 일치하는 것을 확인할 수 있다. 본 논문 에서는, pose graph 최적화로 도출된 궤적을 기반으로 가우시안 혼합 지도를 생성하였으며, 위치 추정 성능 분석을 위한 ground truth로 이용하였다.

Fig. 2. Result of pose graph optimization.

가우시안 혼합 지도 생성을 위해 LIDAR 점 군에서 노면 표시를 추출해야 한다. 노면 표시 추출을 위해 먼저 도로 위의 점 군들만 선별할 필요가 있다. 이를 위해, 높이 필터 (Hata et al. 2014)를 이용하여 도로 상의 점 군만 추출하였다. 높이 필터를 수행하기 위해 먼저 LIDAR 채널 별로 일정 높이 이하의 점 군만 선별하였다. 선별한 점 군 중에서 연속되는 점들의 높이 차를 비교하여 차이가 많이 나는 점 군을 구별하고, 높이가 낮은 점 군만 추출한다. 이를 통해, 노면 위의 동적 물체가 제거되며 노면 점 군만 추출된다.

Fig. 3은 높이 필터를 통해 노면 점 군을 추출한 결과이다. 빨간 점 군이 추출 결과이며, 파란 점 군은 그 외의 점 군이다. Fig. 3에서 보듯이, 주변 동적 물체가 제거되어 노면만 추출됨을 확인할 수 있다.

Fig. 3. Road surface extraction using height filter.

추출된 노면 점 군에서 노면 표시를 추출하기 위해서는 LIDAR 반사도를 이용한 이진화 과정이 필요하다. 이를 위해, Otsu (1979)의 임계 값 추정 방법을 이용하였다. Otsu 임계 값 추정 방법은 이미지 픽셀의 밝기 분포를 이용하여 최적의 임계 값을 찾는 방법 이다. 이를 이용하면, 반사도 레벨이 변화하더라도 최적의 임계 값을 찾는 것이 가능하다.

Fig. 4는 노면 표시 추출 결과를 나타낸 것이다. 아스팔트를 제외한 노면 표시에 해당되는 점들만 추출된 것을 확인할 수 있다. 가우시안 혼합 지도 생성을 위해 추출된 노면 표시 점들은 모두 누적하여 저장된다.

Fig. 4. Result of road-marking extraction.

노면 표시 점 군을 이용하여 가우시안 혼합 지도를 생성하기 위해서는 점 군을 확률 분포로 변환해야 한다. 점 군의 확률 분포 변환은 크게 3단계로 구성되어 있다. 점유 격자 필터를 이용한 outlier points 제거, 점 군들의 군집화, GMM을 이용한 확률 분포 생성이다.

Fig. 4와 같이 노면 표시를 추출할 때 outlier를 제거하였음에도 불구하고, 소량의 outlier들이 추출되는 경우가 있다. 이를 제거하기 위해, 본 논문에서는 점유 격자 필터 (Konrad et al. 2010)를 이용하였다. 점유 격자 필터는 점 군을 일정 크기의 격자로 나누어 격자 별 점유 확률을 계산하여 확률이 낮은 격자 안의 점 들을 제거하는 방법이다. 일반적으로, outlier points는 추출되는 개수가 적기 때문에 점유 확률을 계산하면 확률이 적게 나온다. 따라서, 점유 격자 필터를 통해 제거가 가능하다.

Fig. 5는 점유 격자 필터를 이용한 outlier 제거 결과이다. Outlier points의 경우, 점유 격자로 생성시 점유 확률이 낮기 때문에 제거된 것을 확인할 수 있다. 점유 격자 필터를 통해 outlier 제거 후, 점 군을 군집화를 통해 물체 별로 분류한다. GMM을 이용한 가우시안 혼합 확률 분포 변환은 물체의 형상을 효과적으로 표현하기 위해 넓은 범위에서 수행된다. 또한, 최대 우도 추정 방법을 이용하여 확률 분포를 추정하기 때문에, 모든 점 군을 한 번에 변환할 경우 처리 시간이 기하급수적으로 늘어난다. 따라서, 군집화를 통해 점 군을 물체 별로 분류하고 GMM을 수행하여 지도 생성에 필요한 처리 시간을 단축할 수 있다.

Fig. 5. Result of outlier removal using Occupancy grid filter.

Fig. 6은 군집화 결과이다. K-Nearest Neighbor (K-NN) 알고리즘을 이용하여 군집화를 수행하였으며, 점 군들이 각각의 물체 별로 분류된 것을 확인할 수 있다. 물체 별로 분류된 점 군들은 GMM을 통해 확률 분포로 변환된다. GMM을 위한 방법으로 기대값 최대화 알고리즘 (Xuan et al. 2001)이 있다.

Fig. 6. Result of object clustering.

Fig. 7은 EM 알고리즘 기반 GMM의 의사코드이다. 군집화 과정으로 분류된 점 군 및 확률 분포의 개수가 입력으로 들어가며, 기댓값 단계와 최대화 단계를 반복하여 확률 분포의 파라미터가 도출된다. Fig. 7에서 보듯이, 기댓값 단계로 우도를 추정하며, 최 대화 단계를 통해 우도가 최대가 되는 확률 분포의 파라미터를 결정하게 된다.

Fig. 7. Pseudocode of EM algorithm for GMM.

하지만, GMM을 수행하기 위해서는 추정할 확률 분포의 개수를 정할 필요가 있다. 이를 위해, 확률 분포 및 그 안의 점 군의 밀도를 계산하여 비율을 도출한 후 일정 임계 값 이상이 될 때까지 확률 분포의 개수를 늘리는 방법을 고안하였다.

Fig. 8은 확률 분포 개수 증가 과정의 예시이다. 왼쪽 그림과 같이, 확률 분포 개수가 1개일 때는 전체 점 군의 분포만큼 확률 분포가 생성된다. 이 때, 확률 분포 및 점 군의 밀도를 계산하여 비율을 도출할 수 있다. 이에 관한 식은 Eq. (1)과 같다.

Fig. 8. Example of increase the number of probability distributions.

\(\begin{aligned} \left[\begin{array}{ll} e_{1} & e_{2} \end{array}\right] &=2 \cdot\left[\begin{array}{ll} v_{1} & v_{2} \end{array}\right] \\ \rho_{P D} &=n^{2} /\left(e_{1} \cdot e_{2}\right) \\ \rho_{P C} &=N /\left(e_{1} \cdot e_{2}\right) \\ r &=\rho_{P C} / \rho_{P D} \end{aligned}\)       (1)

Eq. (1)은 확률 분포와 점 군 간의 밀도를 계산한 것이다. 먼저, 밀도를 계산하기 위해서는 점 군의 분포를 균일하게 할 필요가 있다. 이를 위해, GMM을 수행하기 전에 격자 필터를 이용하여 일정한 간격으로 점 군이 분포할 수 있게 하였다. v는 추정된 확률 분포의 고유 벡터이며, e는 확률 분포의 길이이다. v를 이용하여 구한 e₁과 e₂를 곱하면 확률 분포의 넓이를 구할 수 있다. 이를 격자 크기의 역수인 n의 제곱을 곱하면 확률 분포의 밀도인 ρ_PD 가 도출된다. 마찬가지로, 확률 분포 안의 점 군의 개수인 n을 곱 하면 점 군의 밀도인 ρ_PC가 도출된다. 두 밀도 사이의 비율 r을 계산하여 임계 값 이하이면 확률 분포의 개수를 늘리고 조건을 만족할 때까지 반복한다. 모든 확률 분포가 조건을 만족하면, GMM 을 종료한다.

GMM 수행이 완료되면, Fig. 9와 같이 점 군이 확률 분포로 변환된다. Fig. 9에서 보듯이, 노면 표시 점 군이 확률 분포로 변환된 것을 확인할 수 있다. 이와 같이 GMM을 통해 복잡한 형상도 효율적으로 확률 분포로 표현이 가능하다.

Fig. 9. Result of probability distributions transform.

Table 1은 가우시안 혼합 지도 예시이다. 표에서 보듯이, 지도 에는 확률 분포의 평균 μ_x, μ_y와 공분산 요소인 σ_x2, σ_xy, σ_y2가 저장 된다. 위와 같이, 지도에 저장되는 정보가 적기 때문에, 지도 생성에 필요한 용량이 매우 적다. 또한, 확률 분포의 평균은 위도와 경도 기반의 절대 위치 좌표로 저장하였으며, 공분산은 ENU 좌표 계로 저장하여 어떤 차량에서도 이용 가능하도록 하였다. 가우시안 혼합 지도는 격자 크기가 정해져 있지 않고 물체의 크기에 따 라 확률 분포의 크기가 결정된다. 따라서, 크기가 큰 물체들도 나눌 필요 없이 하나의 확률 분포로 표현할 수 있기 때문에, 지도 용량이 다른 지도에 비해 매우 적다.

Table 1. Example of Gaussian mixture map.

Index	Latitude (\(\mu_x\))	Longitude (\(\mu_y\))	\(\sigma_{x^2}\)	\(\sigma_{xy}\)	\(\sigma_{y^2}\)
1	37.123521433	127.053851023	1.22	0.43	0.12
\(\text { : }\)	\(\text { : }\)	\(\text { : }\)	\(\text { : }\)	\(\text { : }\)	\(\text { : }\)
N	37.143205021	127.050295821	1.78	0.84	0.33

 

Table 2는 km 당 지도 용량을 비교한 것이다. Table 2에서 보듯이, 가우시안 혼합 지도의 용량이 가장 작은 것을 확인할 수 있다. 지도 용량이 작으면 작을수록 보다 넓은 범위를 하나의 지도 안에 담을 수 있다. 따라서, 넓은 지역을 이동하는 차량 특성 상, 적합한 지도라 볼 수 있다. 또한, 가우시안 혼합 지도는 지도 용량 이 작음에도 불구하고, 차량 측위를 위한 충분한 정보를 가지고 있다. Table 2의 지도를 예로 들자면, 다중 격자 가우시안 혼합 지 도 (Wolcott & Eustice 2017)은 수직 구조물의 높이를 가우시안 혼합 분포로 저장한 지도이다. 따라서, 주변에 건물이 적으면 측위 성능이 낮아지기 때문에, 가용성이 낮다. 또한, Table 2의 지도 중에서 용량이 가장 크다. 2진 격자 지도 (Kim et al. 2019)의 경우, 이진화 과정을 통해 격자 안에 2진 정보만 포함되어 있다. 따라서, 일반적인 격자 지도 보다 용량이 작다. 하지만, 정밀한 위치 추정을 위해서는 격자 크기가 작아야 하기 때문에, 기본적인 지도 용량이 다른 지도에 비해 상대적으로 크다. 확장선 지도 (Im et al. 2018)은 노면 표시와 수직 구조물을 직선으로 저장한 지도 이다. 직선의 꼭지점만 저장하기 때문에 지도 용량이 낮다. 하지만, 직선 특성상 길이만 표현하기 때문에 물체의 폭에 따른 오차가 발생하게 된다. 확률 분포 지도는 평균과 공분산을 이용하여 물체의 길이뿐만 아니라 폭도 표현할 수 있다. 따라서, 물체의 형 상과 확률 분포가 비슷할 경우 측위 성능이 가장 좋아질 수 있다. 하지만, 단일 격자 기반 확률 분포 지도는 격자 크기에 따라 단일 가우시안 확률 분포로 물체의 형상을 표현한다. 그러므로, 물체의 형상이 복잡할 경우 격자 크기가 크면 제대로 표현이 불가하 며, 격자 크기가 작아질수록 지도 용량이 기하급수적으로 증가한다. 반면, 가우시안 혼합 지도는 GMM을 이용하여 하나의 물체를 여러 개의 확률 분포로 표현이 가능하며, 정해진 격자 크기가 없기 때문에 다양한 크기의 확률 분포를 가질 수 있다. 따라서, 노면 표시와 같이 복잡한 형상도 효과적으로 표현이 가능하여 측위 성능을 높일 수 있다.

Table 2. Comparison of map size.

Map size	Multi-res Gaussian mixture map	binary grid map(10 cm)	Extended line map	Proposed map
MB/km	44.3	0.901	0.134	0.083

 

3. GAUSSIAN MIXTURE DISTRIBUTIONS MAP BASED PRECISE VEHICLE LOCALIZATION

이번 장에서는 생성된 가우시안 혼합 지도를 이용한 차량 위치 추정 방법에 대해 설명한다. 가우시안 혼합 지도는 2장에서 설명한 대로, 정해진 격자 없이 물체의 크기에 따라 확률 분포의 크기가 결정된다. 따라서, 다양한 크기의 확률 분포가 존재하기 때문에, 기존의 단일 격자 확률 분포가 격자 크기에 따라 지도 매칭 속도와 성능이 결정된 것과 달리, 빠르면서 정밀한 지도 매칭이 가능하다. 또한, 복수 격자 확률 분포 지도의 경우 격자 크기별로 확률 분포와 점 군을 매칭하여 정밀한 지도 매칭이 가능하지만, 격자 크기별로 매칭을 수행하기 때문에, 처리 속도가 느리다. 반면, 가우시안 혼합 지도는 지도 매칭을 한 번만 수행하기 때문에 빠른 속도로 처리가 가능하다.

3D LIDAR에서 추출한 노면 표시의 점 군과 가우시안 혼합 지도를 매칭하여 차량 위치 추정을 위한 측정치를 획득하게 된다. 그러나, 현재 시점의 점 군만 이용하게 되면 지도 매칭을 위한 정보가 충분하지 않을 경우가 많다. 왜냐하면, 노면 표시는 도로 위 에 존재하기 때문에, 주변에 차량과 같은 장애물이 많을 경우 추 출되는 노면 표시가 적기 때문이다. 또한, 3D LIDAR는 수직 레이어 간의 간격이 넓기 때문에 종 방향 추정을 위한 정보가 상대적으로 부족하다. 그러므로, 슬라이딩 윈도우 방식으로 이전 시점 의 점 군을 누적 (Kim et al. 2016)하여 부족한 종 방향 정보를 보충하였다.

Fig. 10은 점 군 누적 결과이다. 점 군 누적을 위한 시점 별 위치와 방위각은 GPS/DR을 이용하여 도출하였다. Fig. 10에서 보듯 이, 점 군이 누적되어 부족한 종 방향 정보가 보충됨을 확인할 수 있다.

Fig. 10. Process of point cloud accumulation.

Fig. 11은 점 대 확률 분포 스캔 매칭 과정을 의사 코드로 나타낸 것이다. 점 대 확률 분포 스캔 매칭은 기존 NDT 스캔 매칭 (Biber & Straßer 2003, Magnusson 2009)과 마찬가지로 점과 확 률 분포 간의 스코어 및 자코비안을 구하고 비선형 최적화 기법을 통해 최적의 파라미터를 도출한다. 여기서 Δx 는 지도와 점 군 간의 스캔 매칭을 통해 오차가 보정된 결과이다. 그러나, 격자 내의 확률 분포와 바로 매칭이 가능한 NDT 스캔 매칭과 달리 가우시안 혼합 지도에는 격자 크기가 정해져 있지 않기 때문에, 점과 확률 분포 간 매칭을 위한 자료 연관 과정이 필요하다.

Fig. 11. Pseudocode of point-to-probability distribution scan matching.

자료 연관은 Fig. 12의 과정을 통해 수행된다. 먼저, 포인트와 확률 분포 사이의 유클리디언 거리를 도출한다. 유클리디언 거리는 두 점 사이의 실제 거리로서, 이를 통해 점과 확률 분포 평균 사이의 거리를 알 수 있다. 유클리디언 거리를 비교하여 가장 거 리가 가까운 확률 분포를 찾는다. 그러나, 실제 거리가 가깝다 하더라도, 확률 분포의 크기는 각자 다르기 때문에 실제 점과 일치 하는 확률 분포가 아닐 가능성이 있다. 따라서, 포인트와 유클리디언 거리로 찾은 확률 분포 간의 마할라노비스 거리인 dMH를 구하였다. 마할라노비스 거리는 두 점 사이의 거리를 표준 편차의 비율로 나타낸 것이다. 따라서, 이를 통해 한 점이 확률 분포 안에 포함되는지 확인할 수 있다. 하지만, 측위 오차로 인해 실제 포인트와 확률 분포가 매칭되더라도, 점이 확률 분포 밖에 존재하는 경우가 많기 때문에 확률 분포 공 분산에 관심 영역만큼 ΣROI를 더하여 d_MH를 도출한다. 그 결과, d_MH가 임계 값 이하이면 최종적으로 매칭되는 확률 분포를 찾게 된다.

Fig. 12. Pseudocode of point-to-probability distribution data association.

자료 연관 기법을 통해 각각의 점과 대응되는 확률 분포를 찾으면 점과 확률 분포 간의 스코어 및 자코비언의 도출이 가능하다.

\(\begin{aligned} m &=M_{i d x_{\text {math }}}\{\mu, \Sigma\} \\ p &=\hat{P}_{i} \\ q &=(p-\mu) \\ \frac{\partial q}{\partial x} &=\left[\begin{array}{lll} \frac{\partial q_{x}}{\partial x_{t_{x}}} & \frac{\partial q_{x}}{\partial x_{t_{y}}} & \frac{\partial q_{x}}{\partial x_{\theta}} \\ \frac{\partial q_{y}}{\partial x_{t_{x}}} & \frac{\partial q_{y}}{\partial x_{t_{y}}} & \frac{\partial q_{y}}{\partial x_{\theta}} \end{array}\right]=\left[\begin{array}{lll} 1 & 0 & -p_{x} \sin \theta-p_{y} \cos \theta \\ 0 & 1 & p_{x} \cos \theta-p_{y} \sin \theta \end{array}\right] \\ S(p, m) &=\exp \left(-\frac{1}{2} q^{T} \Sigma^{-1} q\right) \\ J(p, m) &=q^{T} \Sigma^{-1} \frac{\partial q}{\partial x} \exp \left(-\frac{1}{2} q^{T} \Sigma^{-1} q\right) \end{aligned}\)       (2)

Eq. (2)는 점과 확률 분포 간의 스코어 및 자코비언 도출 식이다(Biber & Straßer 2003). 여기서, m과 p는 자료 연관 기법을 통해 매칭된 지도의 확률 분포와 LIDAR의 점이다. 이를 이용하여 매칭된 점과 확률 분포 간의 스코어인 S(\(\hat{p}\), m)이 도출되며, 스코어를 편미분하여 자코비언 J(\(\hat{p}\), m)이 도출되는 것을 확인할 수 있다.

\(\begin{array}{l} S(x)=\left[\begin{array}{llll} S_{1} & S_{2} & \mathrm{~L} & S_{N} \end{array}\right]^{T} \\ J(x)=\left[\begin{array}{lll} J_{1} & J_{2} & \mathrm{~L} & J_{N} \end{array}\right]^{T} \\ D(x)=\operatorname{diag}\left(J(x)^{T} \cdot J(x)\right) \\ \delta(x)=-\left(J(x)^{T} J(x)+\lambda D(x)\right)^{-1} J(x)^{T} S(x) \end{array}\)       (3)

Eq. (3)은 각각의 점으로 도출한 스코어와 자코비언을 이용하여 비용 함수 S(x)와 자코비언 함수 J(x)를 생성하고 LevenbergMarquadt 비선형 최적화 방법 (Ulaş & Temeltaş 2013)을 통해 δ(x)를 도출한 결과이다. Levenberg-Marquadt 비선형 최적화 방법은 일반적인 NDT 스캔 매칭에 사용되는 가우스-뉴턴 법보다 수렴이 빠르며, 보다 안정적으로 최적의 해를 찾는다. 따라서, 반복 횟수가 적더라도 해를 찾을 수 있기 때문에 처리 속도가 빨라 진다. 그러므로, Levenberg-Marquadt 최적화 기법을 통해 점 대 확률 분포 스캔 매칭을 수행하였다. 지도와 점 군 간의 스캔 매칭 이 완료되면 칼만 필터 기반의 항법 필터를 통해 최종 위치와 방위각을 도출한다. 이 때, 스캔 매칭을 통해 도출된 위치와 방위각의 보정 값은 측정치 업데이트를 위한 측정치로 이용된다.

4. EXPERIMENTAL RESULTS

실험은 Fig. 1과 같이 도심 구간에서 실시하였으며, 실험 시간은 도로 상에 차량이 많이 존재하는 퇴근 시간에 수행하였다. 또한, 실험 구간의 길이는 대략 3 km이다. Fig. 1에서 보듯이, 모든 구간에 건물이 존재하여 대부분의 구간이 GPS 음영 지역이다. 또 한, 퇴근 시간에 실험을 수행하여 많은 구간에서 주위 차량으로 인해 대부분의 노면 표시가 가려진다. 따라서, 노면 표시 기반 지도를 이용하여 성능을 내기 가장 어려운 구간이라 볼 수 있다.

Figs. 13과 14는 가우시안 혼합 지도를 이용한 차량 위치 추정 결과로서, 각각 위치 및 방위각 오차를 나타낸 것이다. Table 3에서 보듯이, 횡 방향 및 종 방향의 RMSE는 각각 0.04, 0.19 m이다. 또 한, 99%의 신뢰도로 횡 방향은 0.1 m, 종 방향은 0.65 m의 위치 추정 성능을 가지기 때문에, 자율 주행을 위한 위치 추정 성능을 만족함을 확인할 수 있다. 방위각의 경우, GPS/DR의 오차인 0.51도에 비해 0.32도로 지도 매칭을 통해 오차가 보정됨을 확인하였다.

Fig. 13. Position error.

Fig. 14. Heading error.

Table 3. Experimental result.

Position error(m)				Heading error (degree)
RMS		99% COnfidence level		RMS
Lateral	Longitudinal	Lateral	Longitudinal
0.04	0.19	0.10	0.65	0.32

 

5. CONCLUSIONS

본 논문에서는, 3D LIDAR를 이용하여 가우시안 혼합 지도를 생성하고 생성된 지도를 기반으로 차량 위치 추정 방법에 대 해 제안하였다. 가우시안 혼합 지도는 매우 작은 용량으로 생성되었으며, 격자 크기가 정해져 있지 않기 때문에, 작은 크기의 확률 분포와 크기가 큰 확률 분포가 같은 지도에 포함되어 있다. 따라서, 단일 격자나 복수 격자 확률 분포 지도보다 점 대 확률 분포 스캔 매칭시 빠르게 수렴하며, 정밀한 위치 추정이 가능하다. 가우시안 혼합 지도 기반 차량 위치 추정 결과, 횡 방향과 종 방향 의 RMS 오차는 각각 0.04 m와 0.19 m이다. 또한, 방위각의 RMS 오차는 0.32도이다. 이 결과는 자율 주행을 위한 위치 추정 조건을 충족한다. 따라서, 가우시안 혼합 지도를 이용하면 정밀한 차량 위치 추정이 가능함을 확인하였다. 다음 연구에서는 노면 표시뿐만 아니라 도심에 존재하는 수직 구조물을 이용하여 확률 분포 지도를 생성하고 이를 이용한 차량 측위 연구를 진행할 예정이다.