Approaches to Probabilistic Localization and Tracking for Autonomous Mobility Robot in Unknown Environment

미지환경에서 무인이동체의 자율주행을 위한 확률기반 위치 인식과 추적 방법

Abstract

This paper presents a comparison result of two simultaneous localization and mapping (SLAM) algorithms for navigation that have been proposed in literature. The performances of Extended Kalman Filter (EKF) SLAM under Gaussian condition, FastSLAM algorithms using Rao-Blackwellised method for particle filtering are compared in terms of accuracy of state estimations for localization of a robot and mapping of its environment. The algorithms were run using the same type of robot on indoor environment. The results show that the Particle filter based FastSLAM has the better performance in terms of accuracy of localization and mapping. The experimental results are discussed and compared.

1. 서론

최근 자율주행 자동자를 비롯한 무인이동체에 대한 이동기술이 많은 화제가 되면서 무인인동체의 주행기법과 환경 인식 방법에 대한 연구방법이 진전을 보이고 있다.

무인 자율주행 기법에 있어서 자기위치추정은 미지의 환경에서 센서기반 검색과 인식을위하여 기반으로 센서 데이터를 기반으로 이루어진다. 센서 데이터는 불확실성이 따르기 때문에 확률에 근거한 방법이 사용된다. 본 연구에서는 그 중에서 주류의 하나를 이루는 베이시안 필터에 의한 자기 위치 추정 방법을 소개한다. 또 다른 방법으로 최적화 방법에 대해서도 제시하였다[1][2].

본 연구에서는 차륜형 무인이동체가 수평면을 주행하는 경우를 가정하여 모델링 하였다. 센서는 로봇에 탑재되어 있다고 가정하여 GPS 등의 외부 센서는 고려하지 않는다. 무인이동체의 위치 \(\begin{align}(x,y,\theta)^{T}\end{align}\)의 3 자유도로 나타낸다. x, y는 지도상의 좌표 값, \(\begin{align}\theta\end{align}\)는 로봇의 방향 정보로 표시하였다.

2. 자기위치추정

자기위치추정은 지도에 주어진 대로 자신이지도의 어디에 있는지를 센서 데이터로부터 추정하는 문제이다. 자기 위치 추정에서의 상태 벡터는 시간 t에서 무인이동체의 위치 \(\begin{align}r_{t} = (x_{t},y_{t},\theta_{t})^{T}\end{align}\)이다.

자기위치추정은 위치추적과 전역 자기위치추정이 있다. 본 연구에서는 위치추적을 설명한다.

2.1 위치 추적

위치추적(position tracking)는 무인이동체 위치의 초기 값을 외부에서 주어 그 초기 값을 시작점으로 이전 시간의 위치에서 현재 시간 위치를 순차적으로 추정한다.

확률적 위치 추적은 주어진 지도 m을 기준하여 외부 센서의 시계열로 표현된 측정 데이터 \(\begin{align}z_{1:t} = z_{1}, z_{2},...,z_{t}\end{align}\) 와 로봇 동작의 시계열 데이터 \(\begin{align}a_{1:t} = a_{1}, a_{2},...,a_{t}\end{align}\)를 취득하였을 때의 로봇 위치 r_t 의 확률밀도 \(\begin{align}p(r_{t}|z_{1:t}, a_{1:t},m)\end{align}\)의 추정 문제로 공식화 할 수 있다. 이것은 베이지안 정리와 마르코프 가정을 사용하여 다음과 같이 변형시킬 수 있다[3][4].

\(\begin{align}\left. \begin{array} { l } { p ( r _ { t } | z _ { 1 : t } , a _ { 1 : t } , m ) = \alpha p ( z _ { t } | r _ { t } , m ) } \\ { \quad \times \int p ( r _ { t } | r _ { t - 1 } , a _ { t } ) p ( r _ { t - 1 } | z _ { 1 : t - 1 } , a _ { 1 : t - 1 } , m ) d r _ { t - 1 } } \end{array} \right.\end{align}\)       (1)

이것은 추정에 따른 확룔밀도를 나타내는 점화식(관계식)이며 시간 t-1의 로봇 위치의 확률 밀도를 알면 시간 t의 무인이동체 위치의 확률 밀도가 구해진다. 이 점화식은 후술하는 바와 같이 칼만 필터와 파티클필터 등의 베이지안 필터에 의해 계산된다.

여기서 중요한 것은 상태벡터 r_t 는 직접 관측할 수 없고, 관측 가능한 데이터 z_1:t와 동작 데이터 a_1:t에서 추정할 수 있다.

동작모델(motion model) \(\begin{align}p(r_{t}|r_{t-1}, a_{t })\end{align}\)과 측정모델(measurement model) \(\begin{align}p(z_{t}|r_{t}, m)\end{align}\)을 각각 정의할 수 있고, 측정모델은 로봇의 절대 좌표와 지도가 한번 정의된 것으로 가정하고 관측값 z는 절대 좌표 x와 지도 m에 대해 조건부 독립임을 보여주고 있다.

또한 동작모델에서는 r_t가 r_t-1와 a_t에만 의존하고 있고 마르코프 연쇄에 따른 동작 가정이 성립하는 것으로 보았다. 중요한 것은 절대 좌표의 확률 분포를 추정하기 위해 측정값과 지도의 영향을 생각한다는 점이다.

2.2 동작 모델

동작모델 \(\begin{align}p(r_{t}|r_{t-1}, a_{ t })\end{align}\)는 Fig. 1(a)에 나타낸 것과 같이 시간 t-1에서의 무인이동체 위치가 r_t-1이고, 동작 명령이 a_t일 때, 시간 t에서의 무인이동체의 위치가 r_t 가 될 확률 밀도를 나타낸다. 여기에 불확실성을 가지는 것은 a_t에 의한 동작이며, rt_-1는 고정된 값으로 볼 수 있다. r_t-1도 오차를 가지지만 그것은 식 (1)의 적분에 의해 반영된다.

Fig. 1 Error pattern for motion model and measurement model is the conformation, points outside the circle is the error distribution

현재의 무인이동체 속도를 v_t 각속도를 ω_t일 때의 관계식을 a_t = (v_t, ω_t)^T로 표현하고, 동작 명령에 의한 지령값을 다시 \(\begin{align}\hat{a}_{t}=\left(\hat{v}_{t}, \hat{\omega}_{t}\right)^{T}\end{align}\)로 표현될 수 있다. a-_t 는 실제 값이며 실제로 관측되는 값은 \(\begin{align}\hat{a_{ t }}\end{align}\)이다. 구현에서는 오도메토리 의한 측정값을 \(\begin{align}\hat{a_{ t }}\end{align}\)로 하는 경우도 많다. \(\begin{align}\epsilon_{t}\end{align}\)를 오차라고 가정한다면 \(\begin{align}a_{t} = \hat{a_{t}} + \epsilon_{t}\end{align}\)로 나타낼 수 있다.

또한, 이식을 확률모델에 의한 동작 모델로 표현하면 \(\begin{align}p(r_{t}|r_{t-1}, \hat{a_{ t }})\end{align}\)이 된다. 본 확률식에서 a_t을 사용한 무인이동체 위치 r_t는 식 (2)와 같이 계산할 수 있다[5][6].

\(\begin{align}g ( r _ { t - 1 } , a _ { t } ) = \left( \begin{array} { c } { x _ { t - 1 } } \\ { y _ { t - 1 } } \\ { \theta _ { t - 1 } } \end{array} \right) + \left( \begin{array} { c } { v _ { t } \operatorname { cos } \theta _ { t - 1 } } \\ { v _ { t } \operatorname { sin } \theta _ { t - 1 } } \\ { \omega _ { t } } \end{array} \right) \Delta t\end{align}\)       (2)

\(\begin{align}\Delta t\end{align}\)는 t - 1 ~ t 사이의 단위 시간이다. 이처럼 미소 변위를 누적하여 위치 추정을 할 수 있지만 이 방법은 오차가 누적되는 문제가 있다. 인간에 비유하면 눈을 감고 걸음으로써 거리와 위치를 추측하는 것과 같은 경우다. 따라서 장거리를 이동하게 되면 누적 오차가 커져서 측정 모델에 의한 위치 수정이 필요하게 된다.

2.3 측정 모델

측정모델 \(\begin{align}p(z_{t}|r_{t}, m)\end{align}\)은 지도 m 상에 무인이동체의 위치가 r_t일 때, 측정 데이터 z_t를 얻을 확률 밀도이다. z_t는 센서 좌표계에서의 값을 획득한다. 그러나 측정 데이터에서 무인이동체 위치를 추정하는 경우 z_t는 정수로, r_t가 변수이므로 \(\begin{align}p(z_{t}|r_{t}, m)\end{align}\)는 r_t의 우도(likelihood)로 불린다.

측정 모델을 엄격하게 표현하면 랜드마크 측정 데이터에 대응하는 결합함수 c_t를 포함한 측정모델 \(\begin{align}p(z_{t}|r_{t}, c_{t}, m)\end{align}\)이다. 데이터 결합(data association)는 그 자체가 큰 기술적 과제로써 본 연구에서는편의상 c_t는 주어진다고 가정하였다. 측정 모델은 센서에 크게 의존하지만, 본 연구에서는 레이저 스캐너 센서를 이용하였다. 레이저 스캐너는 대상물까지의 거리와 방향을 구할 수 있다. Fig. 1(b)에서 랜드마크 m_t의 위치 \(\begin{align}(m_{j,x},m_{j,y})^{T}\end{align}\)가 지도상에 등록되어 있을 때 무인이동체 위치 r_t에서 측정되는 데이터는 다음과 같다.

\(\begin{align}h ( r _ { t } , m _ { j } ) = \left( \begin{array} { c } { \sqrt { ( m _ { j , m } - x _ { t } ) ^ { 2 } + ( m _ { j \cdot y } - y _ { t } ) ^ { 2 } } } \\ { \operatorname { tan } ^ { - 1 } ( \frac { m _ { j \cdot y } - y _ { t } } { m _ { j \cdot x } - x _ { t } } ) - \theta _ { t } } \end{array} \right)\end{align}\)       (3)

m_j의 실측치 z_t에 대한 표현으로 \(\begin{align}z_{ t } = h(r_{ r }, m_{j})\end{align}\)라는 제약 관계가 성립한다. 하지만 랜드마크가 1개인 경우에는 이 관계식에서 고유한 r_t를 구할 수 없다. 또한 자기위치 인식을 위한 명확한 랜드마크가 존재하지 않는 곳도 있을 수 있으므로 관측이 불연속적이 될 수 있다.

이러한 문제는 동작 모델로 보완할 수 있다. Fig. 2는 3 개의 원이 랜드마크, 타원 또는 점군이 로봇 위치의 분포. (a)와 (b)지도는 같지만, 난수를 사용하기 때문에 로봇 궤적은 동일하지 않다[7].

Fig. 2 Simulation of self position estimation. Robot’s trajectory shows real value and estimated value

2.4 칼만 필터에 의한 위치 추적

칼만 필터를 이용하는 방법으로는 무인이동체 위치, 작동모형, 측정모델을 각각 하나의 정규 분포로 나타낸다.

동작모델을 식 (4)와 같이 나타낼 수 있다. g는 비선형 함수이므로 확장칼만필터(EKF)로 처리하기 위해 테일러 전개식으로 선형화 하였다. G_t 는 r_t에 관한 g의 자코비안 행렬, V_t 는 a_t에 관한 g의 자코비안 행렬이다. \(\begin{align}\hat{a_{ t }}\end{align}\)을 동작 지령값 또는 오도메토리 값으로 표현되며 a_t는 정규분포 \(\begin{align}N(\hat{a_{ t }}, M_{ t })\end{align}\)를 따른다.

\(\begin{align}\left. \begin{array} { l }{ r _ { t } = g ( r _ { t - 1 } , a _ { t } ) } \\ { \approx g ( \overline { r } _ { t - 1 } , \hat { a } _ { t } ) + G _ { t } ( r _ { t - 1 } - \overline { r } _ { t - 1 } ) + V _ { t } ( a _ { t } - \hat { a } _ { t } ) } \end{array} \right.\end{align}\)       (4)

측정 모델을 식 (5)와 같이 나타낼 수 있다. 여기서 h가 비선형 함수이므로 테일러 전개로 선형화하였다. H_t는 r_t관한 h의 자코비안 행렬이며 \(\begin{align}\beta_{ t }\end{align}\)는 오차로써 정규 분포 N(0, Q_t)를 따른다.

\(\begin{align}\left. \begin{array} { l }{ z _ { t } = h ( r _ { t } , m _ { j } ) + \beta _ { t } } \\ { \approx h ( \tilde { r } _ { t } , m _ { j } ) + H _ { t } ( r _ { t } - \tilde { r } _ { t } ) + \beta _ { t } } \end{array} \right.\end{align}\)       (5)

무인이동체의 위치 분포를 \(\begin{align}N(\overline{ r }_{t} , \sum_ { t })\end{align}\)하고 했을 때 확장 칼만 필터에 의해 다음과 같이 계산할 수 있다[8][9].

(1) 동작 모델에 의한 r_t의 예측

\(\begin{align}\left. \begin{array} { c } { \tilde { r } _ { t } = g ( \overline { r } _ { t - 1 } , \hat { a } _ { t } ) } \end{array} \right.\end{align}\)       (6)

\(\begin{align}\left. \begin{array} { c } { \tilde { \sum } _ { t } = G _ { t } \sum _ { t - 1 } G _ { t } ^ { T } + V _ { t } M _ { t } V _ { t } ^ { T } } \end{array} \right.\end{align}\)       (7)

(2) 측정 모델에 의한 \(r_{ t }\)의 갱신

\(\begin{align}K _ { t } = \tilde { \Sigma } _ { t } H _ { t } ^ { T } ( H _ { t } \tilde { \Sigma } _ { t } H _ { t } ^ { T } + Q _ { t } ) ^ { - 1 }\end{align}\)       (8)

\(\begin{align}\overline { r } _ { t } = \tilde { r } _ { t } + K _ { t } ( z _ { t } - h ( \tilde { r } _ { t } , m _ { j } ) )\end{align}\)       (9)

\(\begin{align}\sum _ { t } = ( I - K _ { t } H _ { t } ) \tilde { \Sigma } _ { t }\end{align}\)       (10)

칼만필터의 솔루션은 작동모델의 값 \(\begin{align}\tilde { r } _ { t }\end{align}\)와 실측치 z_t과 측정모델의 값 \(\begin{align}h(\tilde { r } _ { t },m_{ j })\end{align}\)의 차이 값에 가중치 값 K_t을 곱한 선형화 된 형태이다. 이때, K_t는 칼만게인라고 부르며 동작모델과 측정모델의 정확도에 따라 가중치를 달리하여 최적값을 얻을 수 있다. 또한 칼만 게인은 z_t를 센서 좌표계에서 지도 좌표계로 변환하는 기능도 수행하고 있다. 이와 같이 칼만필터를 이용하여 여러 데이터를 융합하여 최적의 솔루션을 얻을 수 있다.

2.5 파티클 필터에 의한 위치 추적

파티클 필터에 의한 방법은 다수의 파티클을 이용하여 무인이동체 위치를 확률분포로 나타낸 것이다. i번째 입자 \(\begin{align}p_{ t }^{(i)}\end{align}\)는 로봇의 위치 r⁽ⁱ⁾_t = (x⁽ⁱ⁾_t, y⁽ⁱ⁾_t, 𝜃⁽ⁱ⁾_i)와 가중치 \(\begin{align}w_{t}^{ (i)}\end{align}\)를 갖는다. 위치 추적 절차는 다음과 같다[10].

2.5.1 동작 모델에 의한 r_t 예측

각 파티클을 \(\begin{align}\hat{ a_{ t } }\end{align}\)만 진행한다. 동작의 오차분포 N(0, M_t)에 따라 난수 \(\begin{align}\hat{ a_{ t } }\end{align}\)를 발생시켜 오차로써 \(\begin{align}\hat{ a_{ t } }\end{align}\)에 추가한 무인이동체의 위치는 다음과 같다.

\(\begin{align}r _ { t } ^ { ( i ) } = g ( r _ { t - 1 } ^ { ( i ) } , \hat { a } _ { t } + \epsilon _ { t } ^ { ( i ) } )\end{align}\)       (11)

2.5.2 측정 모델에 의한 가중치 계산

각 파티클에서 보이는 랜드마크의 위치 및 센서 데이터와의 일치도에서 우도를 계산하고 가중치를 계산한다. 이때 가중치 합계가 1이 되도록 정규화한다.

\(\begin{align}w _ { t } ^ { ( i ) } \propto p ( z _ { t } | r _ { t } ^ { ( i ) } , m _ { j } )\end{align}\)       (12)

2.5.3 리샘플링

각 \(\begin{align}p_{ t }^{(i)}\end{align}\)에 대해 \(\begin{align}w_{t}^{ (i)}\end{align}\)에 비례한 개수만큼 복제를 만들고 시간 t+1의 파티클을 표현한다.

파티클 필터에 의한 자기위치추정은 각 파티클이 무인이동체 위치 정보에 대한 시뮬레이션 결과를 제시하였다. 동작 모델에 따라 입자가 이동하고 그 위치에서 보여야 하는 랜드마크가 예측 및 실측 값이 일치하면 가중치가 커진다. 가중치가 큰 입자는 리샘플링에 의해 많은 복제가 만들어져 주변의 밀도가 높아지게 되는데 이러한 입자의 밀도로 무인이동체 위치의 분포를 나타낼 수 있다.

파티클 필터의 장점은 정규 분포에 한정하지 않고 다양한 확률 분포를 다룰 수 있다는 것이다. 하자만 단점으로는 상태 벡터의 차원이 커지면 필요한 입자 수가 증가하여 많은 계산 시간을 필요로 한다. Fig. 3은 EKF에서 평균과 공분산으로 표시된 파이클로 변환된 샘플링 값을 보여주고 있다.

Fig. 3 Distribution of particles expressed as mean and covariance of EKF

Fig. 4는 1회 주행 후 다시 랜드 마크 L1, L2, L3를 관측함으로써 무인이동체 위치의 불확실성에 따른 위치 정보를 각각 제시하였고, 기준선 대비 EK의 위치정밀도는 60%, EKF는 78%, Particle Filter는 84%의 정밀도는 보였다. 동일한 조건에서 1회 주행 후 다시 랜드 마크 관측에 따른 FastSLAM은 궤적 단위별 필터링 하기 때문에 불확실성이 적은 궤적을 보여주고 있다.

Fig. 4 FastSLAM simulation and comparison of EK, EKF, PF

3. 결론

본 논문에서는 무인이동체의 확률적인 자기위치 추정방법으로 베이지안 필터 접근법을 제시하였다. 확장칼만필터(Extended Kalman Filter, EKF)를 기반으로 한 고전적인 솔루션이 될 수 있는 FastSlam이라는 대안 알고리즘에 대한 방법과 장점을 기술하였다.

베이지안 접근법에 대한 자율주행 무인이동체의 네비게이션 방법을 이용함으로써 환경정보에 대한 데이터 융합을 통일적으로 실시 할 수 있고 주행 중인 무인이동체의 위치 등의 불확실성을 나타낼 수 있다. 또한 최적화 접근도 유용한 방법이며 필터 응용을 구분하거나 병행하는 방법은 향후 추가적으로 진행되야 할 연구분야이기도 한다.