KR100816269B1 - 언센티드 필터를 적용한 강인한 동시 위치 추정 및 지도작성 방법 - Google Patents

Abstract

본 발명은, 로봇이나 차량의 위치를 추정함과 동시에 알지 못하는 주위 환경에 대한 지도를 작성하는 동시 위치 추정 및 지도 작성 방법에 관한 것이다. 본 발명은, 이동체의 위치 추정과 형상 지도 작성을 동시에 수행하는 방법에 있어서, FastSLAM 프레임워크에서 언센티드 칼만 필터(UKF)를 사용하여 형상 지도의 특징점을 업데이트 하는 단계; 센서 정보로부터 추출된 새로운 특징점에 대해서 언센티드 변환(UT)을 이용하여 초기화하는 단계를 포함하는 것을 특징으로 한다.

이러한 본 발명에서는, 로봇 및 차량 위치에 대한 추정에 있어서는 정확한 proposal 분포를 얻기 위해 개량된 UPF를 사용함으로써 선형화 오차를 제거함과 동시에 자코비안 행렬을 사용하지 않을 수 있고, 지도 작성에 있어서는 기존의 EKF 대신 UKF를 사용함과 동시에 새로운 특징점에 대해서도 UT를 이용하여 초기화함으로써 자코비안 행렬을 피하고 비선형 함수의 부정확한 선형 근사화를 모두 극복할 수 있다.

위치 추정, 형상 지도 작성, 로봇, 언센티드 필터, 칼만 필터, 파티클 필터

Description

언센티드 필터를 적용한 강인한 동시 위치 추정 및 지도 작성 방법{Robust Fast Simultaneous Localization and Mapping Method applying Unscented Filter}

도1은 본 발명과 종래기술에 따른 차량 위치 추정 및 지도 작성 결과를 도시함

도2는 본 발명의 UFastSLAM필터와 다른 필터의 비교 실험 그래프

도3은 본 발명의 UFastSLAM필터와 다른 필터의 비교 실험 데이터

도4는 Measurement 노이즈 정도에 따른 본 발명의 UFastSLAM필터와 FastSLAM2.0필터의 강인성 비교 그래프

도5는 동일한 정확도 하에서 본 발명의 UFastSLAM필터와 FastSLAM2.0필터의 파티클 수를 비교함

도6은 본발명의 UFastSLAM필터와 FastSLAM2.0필터에 따른 차량 위치 추정과 지도 작성 결과를 도시함

도7은 본발명의 UFastSLAM필터와 FastSLAM2.0필터에 따른 차량 위치 추정 오차 그래프

본 발명은, 로봇이나 차량의 위치를 추정함과 동시에 알지 못하는 주위 환경에 대한 지도를 작성하는 동시 위치 추정 및 지도 작성 (SLAM: Simultaneous Localization and Mapping)방법에 관한 것이다.

로봇이나 차량의 위치를 추정함과 동시에 알지 못하는 주위 환경에 대한 지도를 작성하는 SLAM에 대하여 많은 연구들이 행해져 왔다.

그래서 최근의 SLAM 필터는 그 기본 원리에 따라 크게 Extended Kalman Filter (EKF)를 이용하는 EKF-SLAM 필터와 'Rao-Blackwellized 파티클 필터'로 분류된다.

EKF-SLAM 필터는 로봇 또는 차량의 위치와 지도 간의 관계 (correlation)를 모두 고려할 수 있다는 장점이 있지만, 모션과 센서의 비선형 모델을 선형화로 근사화시키고 모든 확률 분포를 정규분포 (Gaussian distribution)로 가정하기 때문에, 이로 인한 오차가 항상 존재하게 된다. 게다가 로봇 또는 차량의 위치와 지도간의 관계를 모두 하나의 공분산 행렬에 넣어서 다루기 때문에 실시간 처리를 위한 계산상의 부담도 크다.

이러한 EKF-SLAM 필터의 문제점을 분석하고 해결하기 위하여, 필터 일관성 분석 (consistency analysis)과 여러 개량된 필터에 대한 연구가 이루어져 왔다.

이에 따라, Murphy는 위의 SLAM 문제를 효과적으로 해결하기 위해서 EKF-SLAM 필터와는 다른, Rao-Blackwellized 파티클 필터(RBPF)를 개발하였으며, RBPF는 로봇 및 차량의 위치와 지도 간의 관계를 별개로 각각 추정을 하게 된다. 더 나아가서 Montemerlo와 Thrun은 'FastSLAM'이라고 하여 특징점 (feature)을 기반으로 하는 지도에 대해서도 RBPF를 사용할 수 있는 framework을 개발하였다.

RBPF나 FastSLAM 필터의 성능과 관련해서 가장 중요한 질문들은 추정 오차의 정확도를 유지하면서 어떻게 하면 사용되는 파티클의 수를 줄일 수 있겠는가 하는 것이며, 두 필터 모두 파티클 필터를 기반으로 로봇의 위치를 추정하기 때문에, 파티클 필터의 'resampling' 단계가 올바른 위치를 추정하고 있는 파티클을 제거해 버릴 수 있는 위험에 대해 어떻게 해결할 것인가 하는 것이다. 이 두 가지 질문은 항상 고려되어야 하고, 또 더 좋은 성능이라는 것은 이 두 가지 질문에 대해 좀 더 좋은 답변을 해 줄 수 있다는 것을 의미한다.

현재 잘 알려져 있고, 또 널리 쓰이고 있는 RBPF와 FastSLAM 필터에 있어서 주목해야 할 것은 두 필터 모두 여전히 EKF-SLAM과 같이 모션과 센서 모델을 선형 근사화시켜서 사용하고 있다는 사실이다. 이로 말미암아, 불행히도, EKF를 지도 작성용 필터로 사용하는 기존의 RBPF와 FastSLAM 필터는 잠재된 두 가지의 중요한 한계를 가지고 있다. 한 가지는 로봇 및 차량의 공분산 행렬을 계산하기 위해서 자코비안 행렬을 필요로 한다는 점이고, 또 한 가지는 비선형 함수를 단순히 1차 항만을 사용하여 선형 근사화시킨다는 점이다. 이것은 필터를 발산하도록 만들 수 있는 중요한 문제이다. Martinez-Cantin과 Castellanos는 넓은 실외 환경에서 EKF-SLAM 필터의 일관성(consistency)이 쉽게 깨지는 것을 보완하기 위해서 Unscented Kalman Filter (UKF)를 EKF 대신 사용하는 시도를 하였으며 보다 정확하면서도 일관성 있는(consistent) 결과를 얻었다. 또한, Merwe는 파티클 필터에 대하여 UKF를 적용하고 이에 대해 Unscented Particle Filter (UPF)라고 이름을 붙였는데, 파티 클을 뿌리는 'sampling step'에서 훨씬 더 정확한 파티클을 뿌릴 수 있음으로 말미암아 보다 정확한 위치 추정이 가능하였다.

UKF나 UPF와 같은 unscented 필터는 먼저 추정하려는 변수 (state)의 평균과 공분산을 이용하여 여러 개의 시그마 포인트를 정의하고 이 시그마 포인트들을 비선형 함수에 그대로 적용하여 순수한 비선형 변환 결과를 얻은 후에, 이들을 따로 계산된 가중치에 곱함과 동시에 모두 더함으로써, Kalman 게인 등을 구하는데 있어서, 비선형 변환을 보다 정확하게 수행할 수 있도록 한 것이다.

EKF가 Taylor 시리즈의 1차 까지 만을 사용하여 비선형 함수를 선형화하는 반면, 이러한 unscented 필터는 보다 고차의 항까지도 고려할 수 있기 때문에 EKF 보다 정확하고 신뢰할 수 있다. 게다가 EKF에 비해 추가로 필요한 계산량이 거의 없다.

본 발명에서는 RBPF와 FastSLAM 필터에서, 로봇 및 차량의 공분산 행렬을 계산하기 위해서 자코비안 행렬을 필요로 한다는 점과 비선형 함수를 단순히 1차 항만을 사용하여 선형 근사화시킨다는 문제점이 발생한다는 점을 극복한 새로운 SLAM 필터, 즉 언센티드 빠른 동시 위치 추정 및 지도 작성방법(약칭하여, UFastSLAM)을 제공한다.

이제, 이상과 같은 기술적 과제를 달성하기 위한 UFastSLAM 필터를 구체적으로 설명하기로 한다.

1. FastSLAM framework에서 UKF를 사용한 형상 지도 (feature map) 업데이트와 새로운 특징점의 초기화

UKF는 시그마 포인트를 정의하고 이들을 비선형 함수에 그대로 적용함으로써 선형 근사화를 하거나 자코비안을 계산할 필요가 없도록 만든다. 이로 인해서 EKF보다 정확한 추정이 가능하게 하는데, 이러한 시그마 포인트는 무엇을 근거로 하여 만드는가가 중요하다. 센서가 지도를 이루는 특징점(feature)을 다시 보았을 때, 이 특징점을 업데이트하기 위하여, 먼저, 시그마 포인트는 다음의 수학식1과 같이 이전 시간 스텝 (t-1) 에서 가지고 있던 특징점의 위치

와 공분산 행렬

로부터 구해진다. 여기서 [m]은 각 파티클의 인덱스이고, n은 특징점의 dimension인데, 특징점은 2차원으로 표현되기 때문에 n은 2가 된다.

여기서 λ = α²(n+κ)-n이며 본 발명에서 α = 0.005, κ = 1로 이 두 변수는 평균으로부터 시그마 포인트가 얼마나 멀리 떨어져 있는지를 결정하는 스케일 변수이다. 위 수학식1에서 공분산 행렬

의 제곱근을 구하는 것은 Cholesky 인수분해를 이용하여 수행되며, 시그마 포인트를 정의하였으면 다음으로는 각 시그마 포인트들의 가중치를 구한다. 가중치

는 평균을 구하기 위한 가중치이며,

는 공분산을 계산하기 위한 가중치이다. 이 두 가중치는 다음과 같이 구해진다.

이 수학식2에서 사용된 β = 2 는 어떠한 값이든 사용가능하지만, 정규 분포의 경우에 대해서는 2가 최적값이라고 알려져 있다. 앞에서 구한 시그마 포인트들과 가중치로부터 predicted measurement

를 구할 수 있으며, 이때 시그마 포인트들이 비선형 함수인 measurement 모델 h(·)을 거쳐 변환된다. 비선형 변환을 위하여 현재의 로봇 또는 차량의 위치인,

와 앞에서 구한 시그마 포인트들이 사용되며, predicted measurement

는 아래의 수학식3, 4와 같이 가중치가 곱해진 변환된 시그마 포인트들의 합으로 구해진다.

이렇게 구한 predicted measurement

를 통해 EKF에서의 innovation 공분산 (innovation covariance)

와 자코비안에 해당하는

및 Kalman 게인

도 아래의 수학식5,6,7를 통해 구한다.

여기서 R _t 는 measurement 노이즈 공분산 (measurement noise covariance)이며 innovation 공분산를 구하는데 있어서 더해지고 있음을 볼 수 있다. 이것은 특징점에 대해서 이만큼의 불확실성을 더해 준다는 것을 의미한다. EKF와는 달리 자코비안을 계산하지 않고, 비선형 변환을 그대로 인정함으로써 보다 정확하게 특징점의 위치와 그 공분산을 업데이트 하고 있다. 최종적으로 특징점의 위치

와 공분산

는 다음의 수학식8, 9을 통해 업데이트 된다.

여기서 Z _t 는 실제로 센서를 통해 측정된 measurement이다. 수치적으로 보다 안정된 업데이트를 수행하려면 위의 두 식 대신 Cholesky 인수분해를 사용한 업데이트를 수행하면 된다.

새로운 특징점을 보았을 때, 이 특징점을 초기화시켜 지도에 기억시키는 과정에서도 극 좌표계와 Cartesian 좌표계간의 비선형 변환이 존재하며, 공분산에 대한 자코비안도 계산해야 하므로 이 역시도 unscented 변환(UT)을 사용하여 정확한 변환이 이루어지도록 해야 한다.

특징점을 초기화 할 때 입력값으로 가지는 것은 센서가 측정한 실제 측정거리와 센서 원점을 기준으로 하여 특징점까지의 각도, 두 가지이다. 이로부터 시그마 포인트를 구하면 다음 수학식 10과 같다.

여기서 measurement 노이즈 공분산 R _t 을 사용하였는데, FastSLAM이나 RBPF framework에서는 로봇 및 차량의 추정과 특징점의 추정을 별개로 수행하기 때문에, 특징점의 초기 불확실 정도를 R _t 로 사용할 수 있다. 이렇게 구한 시그마 포인트는 극 좌표계 상에 있다. Z _t 가 극 좌표계 상에 있기 때문이다. 따라서 특징점 지도를 그리기 위해 이를 아래의 수학식11와 같이 Cartesian 좌표계로 변환시킨다.

최종적으로 계산된 새로운 특징점의 위치와 공분산은 아래의 수학식 12와 같다. 주목할 점은 기존의 필터와는 달리 어떠한 자코비안이나 선형 근사화식도 사용하지 않는다는 점이다.

2. FastSLAM framework에서 수정된 UPF를 사용하여 파티클을 샘플링하는 기법

FastSLAM이나 RBPF framework에서는 로봇 및 차량의 위치 추정을 위해서 파티클 필터를 사용한다. 그 결과, 일반적인 파티클 필터에서와 같이 proposal 분포를 결정하는 것이 중요한 문제가 된다. 참고로, 파티클 필터는 크게 다음과 같은 세 가지의 단계로 이루어진다.

- 로봇 및 차량의 실제 위치와 공분산을 알지 못하기 때문에 proposal 분포를 정의하고 이에 근거하여 고정된 개수만큼의 파티클을 Gaussian 랜덤으로 샘플링하는 단계

- 센서로부터 얻은 정보를 기반으로 실제 로봇 및 차량의 위치와 공분산을 알고 나서 proposal 분포와 이 실제 분포의 비율로 'importance weight'를 계산하는 단계

- 마지막으로, 앞서 구한 'importance weight'를 바탕으로 높은 weight를 가진 파티클만을 남겨두는 'resampling 단계'

proposal 분포를 정의하는데 있어서 가장 일반적으로 쓰이는 기법은 모션 모델을 사용하는 것이다. 하지만 이 기법을 사용하게 되면 센서로부터 얻은 likelihood가 proposal 분포의 끝부분에 위치했을 경우 또는 센서가 정확해서 likelihood가 매우 날카로운 형태로 분포될 경우에 정확한 파티클들이 제거되고 부정확한 위치로 수렴할 위험이 있다.

이러한 문제점을 극복하기 위해서 여러 연구자들은 현재의 observation을 proposal 분포를 정의하는데 고려하는 기법을 제안하였으며 이러한 방식의 접근을 통해서 실제 로봇 및 차량의 분포와 거의 비슷한 proposal 분포를 만들어 낼 수 있었다. 그러나 이들의 기법에도 여전히 부정확한 선형 근사화와 자코비안 행렬의 계산이 존재한다.

본 발명에서 제안하는 필터는 proposal 분포를 정의하는데에, 선형 근사화를 하지 않음과 동시에 현재의 observation을 사용하기 때문에 보다 정확한 위치 추정이 가능하다.

센서가 제공하는 observation 정보는 항상 주어지는 것이 아니기 때문에, proposal 분포를 정의하고 이로부터 파티클을 샘플링하는 것은 두 가지의 과정으로 나뉘어져서 이루어져야 한다. 한 가지는 prediction 단계이고, 다른 한 가지는 measurement 업데이트 단계이다. 이 두 단계에 대한 설명에 앞서 먼저, 아래의 수학식 13, 14와 같이 augmented 벡터를 정의하도록 한다.

augmented 벡터

는 이전 시간 스텝 (t-1) 에서의 로봇 및 차량의 위치

와 현재 시간 스텝 (t) 에서의 제어 입력

으로 구성되며, augmented 공분산

는 이전 시간 스텝 (t-1) 에서의 로봇 및 차량에 대한 공분산

와 모션 노 이즈 공분산 Q로 구성된다.

로봇 및 차량의 위치는 3 X 1 벡터이고, 제어 입력은 2 X 1 벡터이므로 augmented 공분산은 5 X 5 행렬이 된다. 여기서 augmented 벡터 또는 공분산에 measurement 항을 포함시키지 않은 것은 measurement가 항상 들어오는 것이 아니기 때문이다.

먼저 prediction 단계에 대하여 설명한다. 이 augmented 벡터와 augmented 공분산을 이용하여 시그마 포인트를 얻게 되는데, 이 때 시그마 포인트의 개수는 augmented 벡터의 dimension이 5이므로 2*5+1=11 개가 되며 아래 수학식 15와 같다.

여기서 L은 5이다. 이렇게 구한 시그마 포인트들

은 비선형 함수인 모션 모델(수학식 16)을 통해서 다음 시간 스텝 (t) 에서의 로봇 및 차량의 위치로 변환된다.

모션 모델을 통해서 현재 시간 스텝에서의 위치로 변환된 시그마 포인트들과 앞에서 설명한 각 시그마 포인트들의 가중치 (수학식 2)로부터 로봇 및 차량의 predicted mean

과 predicted 공분산

을 얻는다.

만약 센서가 환경을 스캔하다가 특징점들을 발견하였다면 data association에 의해서 현재 발견한 각각의 특징점들과 기존에 저장되어 있던 특징점들이 서로 일치하는지의 여부가 검사되고, 현재의 특징점은 기존의 어떠한 특징점에 해당된다는 결과가 제공된다. 이러한 정보가 있을 때는 위의 prediction 단계에서 더 나아가 정확한 로봇 및 차량의 위치 추정을 위해 measurement 업데이트를 수행한다.

measurement 업데이트를 하는데 있어서 제일 먼저 계산해야 할 것은 predicted measurement

를 구하는 것이며 다음 수학식 18과 같다.

여기서 h(·)는 measurement 모델이다. 이렇게 구한 predicted measurement는 measurement 노이즈 공분산 (measurement noise covariance) R_t 와 함께 innovation 공분산

를 구하는데 사용된다(수학식 19).

앞에서 augmented 벡터와 augmented 공분산을 구성할 때 measurement 항을 포함시키지 않았었다. 일반적으로 augmented 벡터와 공분산을 구성할 때 measurement 항까지 포함하여 구성하는 것이 보다 바람직한 접근이며, 만약 measurement가 계속적으로 들어온다면 이것이 가능할 것이다. 하지만 SLAM 문제에서는 이것이 불가능한 경우가 빈번하다. 따라서 augmented 벡터와 공분산에 measurement 항을 포함시키지 않은 것이며, 대신, measurement 노이즈의 영향을 고려하기 위해서 innovation 공분산을 구할 때 R _t 를 더한 것이다.

EKF의 자코비안에 해당하는 cross-covariance

은 다음과 같이 계산되며, Kalman 게인

은 자코비안과 비선형 함수의 선형화 없이 정확하게 계산된다.

결국, 위의 수학식 20과 같이 계산된 innovation 공분산

와 Kalman 게인

를 이용하여 현재 시간 스텝 (t) 에서의 로봇 및 차량의 위치

와 공분산

을 아래의 Kalman 업데이트 (수학식 21)로 추정한다.

그리고 최종적으로, 추정되는 로봇의 위치 분포를 나타내는 각 파티클의 위치는 위의 추정 위치를 평균으로 가지는 정규 분포로부터 Gaussian 랜덤 샘플링을 통해 구한다.

다음 시간 스텝에서의 augmented 벡터가 이전 시간 스텝에서의 추정 위치를 필요로 하기 때문에

는 항상 저장되어야 한다. 만약 여러 개의 특징점들을 동시에 보는 경우에는 특징점의 개수만큼 앞의 update를 반복함으로써 위치 추정과 공분산 전파가 이루어지게 된다.

3. 선택적인 resampling 기법

파티클 필터의 성능에 영향을 미치는 주요한 인자 중의 하나가 resampling 단계이다. resampling은 로봇 및 차량의 proposal 분포와 실제 분포간의 비로 구해지는 importance weight를 기준으로 낮은 weight를 가진 파티클을 높은 weight를 가진 파티클로 대체하는 과정이다. 이러한 resampling 기법으로 본 연구에서는 '선택적인 resampling 기법(selective resampling)'을 사용하며, 이 기법은 파티클 필터에서 효과적임이 검증되어 있다 .

4. 시뮬레이션 결과

Bailey는 여러 개의 서로 다른 SLAM 필터를 비교 평가 할 수 있는 시뮬레이터를 개발하여 웹사이트 상에서 공개하고 있으며, 이 시뮬레이터를 이용하여 FastSLAM 필터의 정확성과 consistency를 분석하는 연구도 수행하였다. 본 발명에서도 동일한 시뮬레이터를 기반으로 한다.

(1) 예비 실험

본 발명의 UFastSLAM 필터와 기존의 FastSLAM2.0 간의 성능 비교에 앞서서 먼저 불완전한 UFastSLAM 필터라 할 수 있지만 중요한 의미를 가지는 두 개의 필터를 소개하고 이 두 개의 필터와 FastSLAM2.0 및 UFastSLAM을 포함하여 총 4개의 필터에 대하여 성능을 비교 검증하기로 한다.

비교 검증을 위한 시뮬레이션 실험은 도1과 같이 100미터 X 20미터의 환경에서 이루어졌으며 사용된 차량의 휠베이스는 0.26미터이다. 차량의 앞부분에는 최대 20미터까지 전방 180도를 스캔할 수 있는 거리 측정 센서가 달려있다. 차량의 제어 외란 (control noise)은 정규 분포로, 속도에 대해 0.1m/s, 스티어링 각도에 대해서 3도가 주어졌다. 센서의 측정 외란 (measurement noise) 역시 정규 분포로, 거리에 대해 0.2m, 각도에 대해서 8도가 주어졌다.

차량의 제어 주기는 40Hz이며 센서는 5Hz로 스캔을 수행한다. Known data association을 가정하였으며 이 실험에 대해서는 4개의 필터 모두 단 3개의 파티클만이 사용되었다.

도1에서 굵은 선(진한 선)이 실제 차량의 이동 경로이며 가는 선(옅은 선)이 추정된 경로를 의미하는데, UFastSLAM 필터가 기존의 FastSLAM2.0 필터보다 월등한 정확도를 보이고 있음을 알 수 있다. 이것은 비선형 변환을 선형 근사화 시키지 않았기 때문이다.

도2는 4가지 필터의 성능을 비교한 그래프이다. 각 필터의 막대의 높이는 10번씩 수행하여 얻은 평균 제곱근 오차 (MSE)의 평균을 의미하며 분산도 함께 나타내었다. 도3의 표는 도2에 대한 자세한 수치값을 나타낸다.

'UKF aided FastSLAM2.0' (도2의 B)은 FastSLAM2.0과 동일한 파티클 필터를 가지고 있으며, 지도상의 특징점의 업데이트에 EKF대신 UKF를 사용하고 특징점의 초기화에 UT를 사용한 필터로써 오직 지도 작성에만 선형화 오차를 제거한 경우이다. 이 필터는 도2와 도3의 표에서 볼 수 있듯이, 동일한 3개의 파티클을 사용하였을 때 FastSLAM2.0 보다 우수한 정확도를 가진다. 하지만, FastSLAM2.0에 비해 감소한 평균 제곱근 오차 (MSE)는 1미터 정도로써 그리 크지는 않은데, 이것은 특징점(feature)의 공분산이 작은 dimension(2 dimension)을 가지기 때문이다.

'modified UPF aided FastSLAM' (도2의 C)은 앞에서 설명한 바와 같이, 로봇 및 차량의 위치를 추정하는 파티클 필터에 unscented 필터를 적용하고, 이에 따른 효과만을 보기 위해서 지도상의 특징점을 업데이트 하거나 초기화하는데에 기존의 EKF를 그대로 사용한 필터이다. 파티클 필터에 있어서 가장 중요한 것은 어떠한 proposal 분포로부터 파티클을 샘플링하는가 하는 것인 만큼, 선형화 오차를 제거한 proposal 분포로부터 샘플링을 수행하는 'modified UPF aided FastSLAM' (도2의 C)은 기존의 FastSLAM2.0보다 약 3.4미터나 더 정확한 성능을 보인다.

마지막으로 앞의 'modified UPF aided FastSLAM' (도2의 C)과 'UKF aided FastSLAM2.0' (도2의 B)을 하나의 framework에 결합시킨 본 발명의 UFastSLAM 필터 (도2의 D)는 다른 어떤 필터보다 우수한 성능을 보이며, 특히 기존의 FastSLAM2.0에 비해서는 무려 약 3.2 배의 정확한 성능을 가진다.

(2) 본 발명의 UFastSLAM과 기존의 FastSLAM2.0 간의 성능 비교

여기서는 본 발명의 UFastSLAM의 효용성과 효과를 검증하기 위해 3가지의 시 뮬레이션 실험을 수행하고 그 결과를 고찰한다.

첫 번째 실험은 measurement 불확실 정도 (uncertainty) 에 따른 두 필터의 강인성을 검증하기 위한 실험으로써 두 필터 모두 10개의 파티클이 사용되었으며 다양한 불확실 정도가 적용되었다. 실험이 이루어진 환경은 20미터 X 25미터로, 각각의 필터에 대해 5번씩 수행하여 평균 제곱근 오차 (MSE)와 분산을 얻었다. 도4에서 확인할 수 있듯이, UFastSLAM과 FastSLAM2.0 필터 모두 measurement의 불확실 정도가 높아짐에 따라 정확도가 떨어졌지만, MSE의 증가율에 있어서 FastSLAM2.0 필터는 UFastSLAM 필터의 거의 두 배에 달한다. 이것은 곧 UFastSLAM이 FastSLAM2.0에 비해서 measurement 노이즈에 강인하다는 것을 의미한다.

두 번째 실험은 50개의 파티클을 사용한 FastSLAM2.0과 동일한 정확도를 얻기 위해서 UFastSLAM이 필요로 하는 파티클의 개수를 확인하기 위한 실험으로써, 실험은 100미터 X 100미터의 큰 환경에서 차량이 약 500미터를 주행하는 것으로 수행된다. 도5의 표에 따르면 본 발명의 UFastSLAM 필터가 50개의 파티클을 사용한 기존의 FastSLAM2.0 필터와 비슷한 정확도를 얻기 위해서는 5분의 1에 해당하는 10개의 파티클 정도가 필요하다. 이것은 계산량의 측면에서 5분의 1의 계산량 만으로도 충분하다는 것을 의미하며, 계산량이 큰 문제가 되고 있는 SLAM 문제에서 상당히 고무적인 결과이다.

마지막으로, 동일한 10개의 파티클을 사용하여 넓은 환경 (두 번째 실험과 동일한 환경)에서 실험을 수행하되, 두 필터의 모든 시간에서의 추정 오차를 비교하기로 한다. 이 때 사용된 measurement 불확실 정도는 거리에 대해서 0.3미터, 각 도에 대해서 14도로 비교적 큰 값이 사용되었다. 도6은 UFastSLAM과 FastSLAM2.0 필터에 의해서 최종적으로 얻어진 로봇 및 차량의 추정 경로와 작성된 지도를 보여준다(굵은 선(진한 선)이 실제 주행 경로이고 가는 선(옅은 선)이 추정 경로). 차량의 추정 경로(가는 선)를 비교해 볼 때, UFastSLAM이 특히 각도 불확실성을 대해 훨씬 효과적으로 해결하고 있음을 확인할 수 있으며 이것은 선형 근사 오차를 제거함으로써 가능하다. 도7의 (a)는 x 위치 오차를, (b)는 y 위치 오차를 각각 보여주고 있으며 UFastSLAM (실선)이 모든 시간에 걸쳐서 보다 정확함을 알 수 있다. 도6의 'A'는 UFastSLAM에서 파티클이 정확한 위치로 수렴하고 있음을 보여주는데, 이로 인해서 도7의 (a), (b)에서와 같이 약 100초 까지 매우 정확한 위치추정을 수행하고 있다. 또한 도6의 'B'는 차량이 처음 보았던 특징점을 다시 봄으로써 루프를 닫는 (loop closing) 순간이며, 이 때 오차가 급격히 감소하는 것을 확인할 수 있다 (도7의 (b)에서 약 580초에 발생).

이상에서 살펴 본 바와 같이, 본 발명에서는 기존의 FastSLAM2.0보다 파티클 수를 줄일 수 있고, measurement 노이즈에 강인한, SLAM 문제의 해법으로써 효과적인 UFastSLAM 필터를 제안하였다. 제안한 필터의 주요한 이점은 더 이상 RBPF framework에서 자코비안 행렬과 비선형 함수의 부정확한 선형 근사를 사용할 필요가 없다는 것이다.

본 발명에서는, 로봇 및 차량 위치에 대한 추정에 있어서는 정확한 proposal 분포를 얻기 위해 개량된 UPF를 사용함으로써 선형화 오차를 제거함과 동시에 자코 비안 행렬을 사용하지 않을 수 있고, 지도 작성에 있어서는 기존의 EKF 대신 UKF를 사용함과 동시에 새로운 특징점에 대해서도 UT를 이용하여 초기화함으로써 자코비안 행렬을 피하고 비선형 함수의 부정확한 선형 근사화를 모두 극복하였다.

이로 인해서, 기존의 FastSLAM2.0에 비해 정확도를 향상 시켰고, 동일한 정확도를 얻기 위한 파티클의 개수를 줄였으며, measurement 노이즈에 대해서도 강인하게 되었다.

Claims (8)

1. 이동체의 위치 추정과 형상 지도 작성을 동시에 수행하는 방법에 있어서,

   FastSLAM 프레임워크에서 언센티드 칼만 필터(UKF)를 사용하여 하기의 수학식1, 5, 6, 7로 형상 지도의 특징점을 업데이트 하는 단계를 포함하는 것을 특징으로 하는 이동체의 위치 추정과 형상 지도 작성을 동시에 수행하는 방법.

   
2. 이동체의 위치 추정과 형상 지도 작성을 동시에 수행하는 방법에 있어서,

   FastSLAM 프레임워크에서 센서 정보로부터 추출된 새로운 특징점에 대해서 언센티드 변환(UT)을 이용하여 하기의 수학식10, 12에 의해 초기화하는 단계를 포함하는 것을 특징으로 하는 이동체의 위치 추정과 형상 지도 작성을 동시에 수행하는 방법.

   
3. 이동체의 위치 추정과 형상 지도 작성을 동시에 수행하는 방법에 있어서, FastSLAM 프레임워크에서 파티클을 샘플링하기 위하여,

   이동체의 아규먼트 벡터와 공분산을 하기의 수학식 13, 14에 의해 정의하는 단계를 포함하는 것을 특징으로 하는 이동체의 위치 추정과 형상 지도 작성을 동시에 수행하는 방법.

   
4. 제3항에 있어서,

   상기 정의된 아규먼트 벡터와 공분산을 이용하여 이동체의 위치를 하기의 수학식 15, 17에 의해 추정(prediction)하는 단계를 더 포함하는 것을 특징으로 하는 이동체의 위치 추정과 형상 지도 작성을 동시에 수행하는 방법.

   

   
5. 제1항에 있어서,

   이동체의 위치 추정을 위한 파티클 샘플링을 위하여,

   이동체의 아규먼트 벡터와 공분산을 정의하는 단계;

   상기 정의된 아규먼트 벡터와 공분산을 이용하여 이동체의 위치를 추정(prediction)하는 단계와, 센서가 환경을 스캔하다가 발견한 특징점이 기존의 어떤 특정점에 해당하는지에 대한 정보가 있을 때에는 측정치(measurement)를 이용하여 상기 이동체의 위치를 업데이트하는 단계로 언센티드 파티클 필터(UPF)를 FastSLAM 프레임워크에 적용하는 단계를 더 포함하는 것을 특징으로 하는 이동체의 위치 추정과 형상 지도 작성을 동시에 수행하는 방법.
6. 이동체의 위치 추정과 형상 지도 작성을 동시에 수행하는 방법에 있어서,

   FastSLAM 프레임워크에서 센서가 환경을 스캔하다가 발견한 특징점이 기존의 어떤 특징점에 해당하는지에 대한 정보가 있을 때에는 측정치(measurement)를 이용하여 위치를 하기의 수학식 18, 19, 20, 21, 22에 의해 업데이트하는 단계를 포함하는 것을 특징으로 하는 이동체의 위치 추정과 형상 지도 작성을 동시에 수행하는 방법.

   

   

   

   

   
7. 삭제
8. 삭제

Images