308 로봇공학회논문지제 3 권제 4 호 (2008.2) Rao-Blackwellized 파티클필터를이용한이동로봇의위치및환경인식결과도출 Resul Represenaion o Rao-Blackwellized aricle Filer or Mobile Robo SLAM 곽노산, 이범희 2, YOKOI Kazuhio 3 Nosan Kwak, Beom-Hee Lee 2, Kazuhio Yokoi 3 Absrac Recenly, simulaneous localizaion and mapping (SLAM) approaches employing Rao- Blackwellized paricle iler (RBF) have shown good resuls. However, no research is conduced o analyze he resul represenaion o SLAM using RBF (RBF-SLAM) when paricle diversiy is preserved. Aer inishing he paricle ilering, he resuls such as a map and a pah are sored in he separae paricles. Thus, we propose several resul represenaions and provide he analysis o he represenaions. For he analysis, esimaion errors and heir variances, and consisency o RBF-SLAM are deal in his sudy. According o he simulaion resuls, combining daa o each paricle provides he beer resul wih high probabiliy han using jus daa o a paricle such as he highes weighed paricle represenaion. Rao-Blackwellized aricle Filer, SLAM, Mobile Robo, aricle Diversiy. 서론 최근, 파티클필터 (aricle iler) 는로봇분야에서다양하게이용되고있다. 그중에서, 로봇의위치 (pose) 및환경맵 (map) 을동시에추정하는 SLAM 문제를해결하기위해많은연구가진행되어오고있다. SLAM 문제는로봇의위치와로봇주변환경을동시에추정해야하는어려운문제이다. 로봇의위치와환경을확률적으로추정하고자, 최근 Rao-Blackwellized 파티클필터 (RBF) [,2] 가확장칼만필터 (Exended Kalman Filer, EKF) 보다주로사용되고있다. 그이유는 RBF가가진두개의큰장점때문인데 [3], 첫째, 기존의 EKF를이용한 SLAM (EKF-SLAM) 의복잡도는특징점 (eaure) 의수의제곱에비례하는데, RBF를이용한 SLAM (RBF- 본연구는일본학술진흥회 (JSS) 의 Gran-in-or JSS osdocoral Fellowship or Foreign Researchers, 서울대학교 BK2, 서울대학교지능로봇연구실의 KOSEF, NRL rogram (no. R0A-2008-000-20004-0) 을통한연구비지원으로수행되었으며, [0] 의연구내용을확장한것이다 일본산업총합기술연구원 (AIST) JSS osdocoral Fellow 2 서울대학교전기. 컴퓨터공학부교수 3 일본산업총합기술연구원 (AIST) 자율행동제어 Group Leader SLAM) 은복잡도가특징점개수의선형으로증가한다. 두번째장점은, 강인한다중측정데이터연관 (daa associaion) 인데, 이것은 EKF-SLAM이하나의추정치만을저장하는것과달리, 파티클각각다른추정치를저장함으로써, 추정치간의보완이가능하기때문이다. 하지만, RBF-SLAM의단점은파티클의다양성손실로인한추정성능저하이다. RBF-SLAM에서는파티클들이재귀적알고리즘에의해로봇의위치및환경을동시에추정하는데, 알고리즘의마지막단계인재추출과정에서파티클의복사혹은제거가발생한다. 이때, 제거된파티클이가지고있던정보는모두사라지고, 결국특정파티클이파티클집합을장악하게된다. 이것이파티클의다양성손실문제인데, 파티클의다양성을잃음으로써, RBF-SLAM의장점인강인한측정데이터연관을수행할수없게된다 [4]. 이것은파티클개수가초기개수와같지만, 내부적으로파티클의추정정보를살펴봤을때, 실제적으로는극소수의파티클정보가다른파티클에복사가된것을확인할수있다. 우리는이파티클손실문제를해결하고자기존의재추출방법을사용하지않고, 파티클의가중치를이용한
Rao-Blackwellized 파티클필터를이용한이동로봇의위치및환경인식결과도출 309 보정방법 (Compensaion Technique) 을제안하였다 [5]. 이방법은파티클의정보보정을통해, 파티클의다양성을유지하였고, SLAM의추정오차를줄일수있었다. 그렇지만, SLAM 과정을마쳤을때각각의파티클은다양한정보를가지고있게되는데, SLAM의수행결과를보여주기위한방법이요구된다. 기존의대부분의연구자들은 SLAM 수행결과로써, 가중치가가장큰파티클의정보, 즉로봇의경로및환경맵으로나타내었다 [2,6]. 하지만, 실제적으로위와같은결과도출방법은항상최상의결과를도출할수없다. 더욱이많은연구자들은 RBF-SLAM의결과도출문제를간과하였다 [3,4,7]. 따라서, 본논문에서는 RBF-SLAM의결과를도출하기위한여러방법들을제안하는데, 그것은첫째, 최대가중치를이용하는방법, 둘째, 파티클분포의기하정보를이용하는파티클트리 (aricle Tree) 의루트 (roo) 파티클을이용하는방법, 셋째, 파티클집합에속한모든파티클정보의평균을이용하는방법, 마지막으로, 파티클분포의기하정보를이용하고, 그분포의경계를파티클트리의끝노드 (lea node) 의정보를이용하는방법이다. 제안된여러결과도출방법의성능평가기준으로, RBF-SLAM의일관성 (consisency), 추정오차, 오차의분산을도입한다. 2장에서는 RBF-SLAM의개요와파티클다양성에대해서기술한다. 3장에서는결과도출방법, 4장에서컴퓨터모의실험을수행하고, 그결과를나타내고, 결과에대한검토를기술한다. 마지막으로 5장에서는본연구의의미와적용대상에대해서언급한다. 그림 에 RBF-SLAM의재귀적네단계, 즉추출, 특징점정보갱신, 파티클가중치계산, 재추출과정을두개의파티클에대해서묘사하였다. 추출과정은각각의파티클이로봇의위치를추정하기위해추정확률분포로부터임의점을뽑아내는것이다. 두번째단계는센서의측정데이터를이용하여각각의파티클이가지고있는특징점의정보를갱신하는과정이다. 세번째단계로, 파티클의성능을평가하기위해시간 에서 k번째 [ k] 파티클의가중치 w 를다음과같이실제확률분포 (arge disribuion) 과추정확률분포 (proposal disribuion) 의비로구한다 : [ ] arge disribuion w k = proposal disribuion 그림 (c) 에서는파티클두개의가중치를구하는것도식적으로보여주며, 여기서각파티클의가중치는정규화된값이다. 마지막단계인재추출과정에서는파티클의가중치를이용하여, 다음재귀단계에서이용될파티클을다시뽑는다. 이마지막과정은재생산과정이라고도불린다. 이때, 제거된파티클들이가지고있던로봇경로정보및환경맵은파티클과같이사라지게된다. Landmark iler # o paricle m 2. RBF-SLAM 및파티클다양성 x mp x mp x Landmark iler #2 o paricle m 2. RBF-SLAM의개요 SLAM문제는로봇이로봇의위치와환경맵을동시에추정해야하는어려움이있다. 하지만, RBF-SLAM은 Rao-Blackwellizaion [,2] 을이용하여, 연관관계 c: 를안다고가정했을때, () 과같이 SLAM 사후확률 (oserior) 을로봇위치추정확률부분과환경맵추정확률부분으로인수분해할수있다 [7]. N F px (, M z, u, c ) = px ( z, u, c ) pm ( x, z, c ) : : : : : : : : n : : : n= 여기서, x :, z :, u: 은초기부터현재시간 까지의각각로봇경로, 센서측정값, 로봇제어입력을나타낸다. 그리고, m n 은 n번째특징점 (eaure) 을나타내고, N F 개의특징점들이환경맵 M을구성한다. () x roposal Disribuion Imporance Weigh : mp x (a) 추출 0. 0.8 Targe Disribuion mp x Landmark iler # o paricle n Landmark iler #2 o paricle n (b) 특징점정보갱신 [ Temporary se a ime ] Resampling wih weighs Y is replicaed hree imes [ Resampled se a ime ] (c) 파티클가중치계산 (d) 재추출그림. RBF-SLAM의재귀적과정 is rejeced
30 로봇공학회논문지제 3 권제 4 호 (2008.2) 2.2 파티클다양성 RBF-SLAM의장점인다중측정데이터연관은파티클의다양성에기인한다. 하지만, RBF-SLAM의마지막단계인재추출과정에서, 제거되는파티클들이발생하고, 장기적으로는소수의파티클들이파티클집합을차지하여다양성을잃게되는문제가발생한다. 파티클다양성이낮아졌을때, 소수의파티클에비교적큰오차가발생하면, RBF-SLAM은더이상추정을제대로수행할수가없게된다. 파티클의다양성이낮아지는직접적인원인은재추출과정에서가중치가낮은파티클들은바로제거되고, 가중치가높은파티클들은복사되기때문이다. 따라서, 우리는 RBF-SLAM에일반적으로쓰이는재추출방법들을비교검토하였지만 [5], 파티클의다양성을장기적으로유지할수없었다. 일례로, 여러재추출알고리즘에의한파티클의다양성을측정하기위해, 초기의파티클집합으로부터제거되지않고남아있는파티클의개수 (disinc paricles) 를측정한결과가그림 2에나타나있다. 그림 2에서, 비교된재추출방법은부분재추출 (arial Resampling, R), 체계적추출 (Sysemaic Resampling, SR) 과 SR을개선시킨잉여체계적추출 (Residual Sysemaic Resampling, RSR) 이다. 그림에는두곳의로봇경로가교차하는 loop-closure도같이표시되어있다. 모든경우에서파티클의다양성이급격하게감소함을알수있다. 첫번째 loop-closure에서, 초기 00개의파티클중에서실제적으로파티클집합에남아있는개수는단한개였고, 나머지는이파티클의복사본이었다. 따라서, 과거에우리는파티클의다양성을유지하고자재추출방법을대신하는보정방법을제안하였고, 이로인한 RBF-SLAM의성능을향상시켰다 [5]. 00 지금까지는파티클다양성의감소로인한문제에대해소개하였다. 하지만, 파티클다양성이유지되었을때, 생각해볼수있는문제가있다. 즉, 파티클들이다양한정보를저장하고있는데, 과연어떻게 RBF-SLAM의결과를표시할것인가? 우리가제안한보정방법 [5] 을사용하여, 파티클다양성을유지하며 RBF-SLAM 수행을마쳤을때, 각각의파티클의가진정보가얼마나다른지표 에분산데이터를나타내었다. 표. 파티클데이터의분산값 로봇위치분산 환경맵분산 ( 평균 ) x (meer) y (meer) x (meer) y (meer) 분산 0.038 0.067 0.223 0.5 최소값 7.469 6.38 0.009 0.002 최대값 8.545 8.38 0.782 0.242 표 에따르면, 로봇위치에대한분산은비교적적으나환경맵분산은매우큼을알수있다. 일례로, 위결과를도출한모의실험에서특정특징점의환경맵분산은약 0.8m 였다. 따라서, 그림 3과같이우리는 RBF- SLAM의결과를표시하기위한방법이필요하게된다. Resul Represenaion 그림 3. 다양한파티클데이터로부터 SLAM 결과도출 Number o Disinc aricles (%) 80 60 40 20 RSR R SR Loop-closures 대부분의연구자들은가장높은가중치를가지는파티클의정보를이용하여결과를도출하였고, 간혹데이터의평균을사용하여결과를도출하였다. 하지만, 파티클의다양성이유지될때, 결과도출문제에대한분석이이루어지지않았다. 따라서, 본논문에서는여러도출방법을제안하고, 각각에대한추정오차, 오차의분산, 필터의일관성을분석하는것이주요연구동기이다. 0 0 500 000 500 Time (sep) 그림 2. 재추출방법에따른파티클다양성변화 3. RBF-SLAM 의결과도출 3. 결과도출에대한연구배경 3.2 RBF-SLAM 결과도출방법본논문에서는그림 4와같이네가지의결과도출방법을제안한다. 첫째, 최대가중치를가지는파티클의정보이용, 둘째, 파티클집합에속한모든파티클정보의평균을이용, 셋째, 파티클트리의루트 (roo) 파티클을이용, 마지막으로, 파티클트리의끝노드 (lea node) 의
Rao-Blackwellized 파티클필터를이용한이동로봇의위치및환경인식결과도출 3 정보를이용하는것이다. pah = N map = N N [ k] x: k = N k = μ [ k] : N F 3.2.3 파티클트리의루트기존 RBF-SLAM의재추출과정에서파티클의가중치만을이용하여파티클의복사및제거가이루어지는데, 파티클의분포정보를이용하여가중치만을이용하는방법을보정할수있다. 따라서, 가중치가가장큰파티클의정보를이용하지않고, 파티클트리의중심이되는파티클의정보를이용한다. 파티클트리의중심점을구하기위해, 여기서는트리는만들지않고, 기하적중심을찾기위해다음과같이파티클간의거리가최소로되는점을택한다 : 그림 4. RBF-SLAM의결과도출방법및절차 3.2. 최대가중치파티클데이터 RBF-SLAM의수행을끝마친후, 최대가중치를가지는파티클의정보를이용한다. 이것은많은연구자들에의해, 당연히가장좋은결과를줄것이라는가정하에많이사용되었다. 먼저, 최대가중치를갖는파티클의인덱스 (index) k max 는다음과같이쉽게구할수있다 : [] i [ j] k = arg min ( x x ), i =,..., N (3) roo i N j= [] i k = arg max w, i =,..., N (2) max 여기서, w [] i i 는 RBF-SLAM 이끝난후, 시간 에서 전체파티클개수 N 중의 i번째파티클의가중치를나타낸다. 따라서, RBF-SLAM의결과도출인로봇경로 (pah) 와환경맵 (map) 은다음과같이이루어진다 : (a) 파티클분포및경계 (, 4, 5 가끝노드 ) pah = x map = μ [ kmax ] : [ kmax ] : N F 여기서, [ kmax ] : [ k ] μ : N x, max F 는초기부터시간 까지의각 각로봇경로및환경맵을나타낸다. 3.2.2 파티클의평균데이터각각의파티클이가지고있는정보의평균을취하는방법또한의미있는것이다. 파티클정보의평균을구하는것도다음과같이간단히구할수있다 : (b) 균형 2차원파티클트리그림 5. 파티클기하분포와균형파티클트리의형성 3.2.4 파티클트리의끝노드의평균파티클의분포를고려한평균데이터를이용하기위해파티클트리를만들수있다. 이때, 파티클트리는
32 로봇공학회논문지제 3 권제 4 호 (2008.2) 파티클간의기하정보를이용하기위해균형 k차원트리 (balanced kd-ree) [9] 를구성한다. 균형 k차원트리형성후, 트리의끝노드 (lea node) 만을취하면, 파티클분포의대략적인경계를구할수있다. 예를들면, 그림 5(a) 와같이 6개의파티클 (2,3), 2(5,4), 3(9,6), 4(4,7), 5(8,), 6(7,2) 가분포하고있을때, 균형 2차원트리는그림 5(b) 와같이형성된다. 형성된파티클트리에서제일끝단에위치하는파티클인, 4, 5를취하면대략적인파티클의기하분포의경계를구할수있다. 이때, 끝노드의평균값을 RBF-SLAM의결과로사용한다. 이방법은파티클분포가크게몇개의집단을형성할때, 3.2.2의단순평균값과는달리분포의경계를취하기때문에기하적인평균을구할수있는장점이있다. Average NEES 4 3.5 3 2.5 2.5 0.5 0 0 500 000 500 Time (sep) 그림 7. 일관성검사를위한평균 NEES 4. 결과도출방법의모의실험 4. 모의실험설정 3장에서제안한방법의성능비교를위해그림 6과같은모의실험환경을구성하였다. 모의실험에서는 00개의파티클을사용하였고, 로봇의이동잡음및센서의측정잡음은 (0.5m/s, 5 /s), (0.3m, 3 ) 로설정하였다. 또한, 제어입력주기는 25ms, 센서측정주기는 200ms 였다. 사용된컴퓨터는.86GHz, 2GB 램의노트북이었다. RBF-SLAM은확률기반이므로, 매번임의의결과가얻어지므로, 결과의검증을위해 50번의 Mone Carlo 실험을수행하였다. 그림 6. 모의실험환경 그림 8. 결과도출방법의 RMS 오차비교 4.2 RBF-SLAM 일관성먼저, 파티클의다양성을유지시키기위해재추출방법대신보정방법을사용하였는데, 그효과를알아보기위해일관성검사를하였다. RBF-SLAM의일관성 (consisency) 를측정하기위해일반적으로 Normalized Esimaion Error Squared (NEES) 를본연구에서도적용하였다. 그결과가그림 7에나타나있다. 필터의일관성검사에대한내용은참고문헌 [8] 을참고하고, 그림 7에서가로축은시간의흐름을나타내기위한 RBF-SLAM의재귀적네단계의반복회수이고, 세로축의두개의평행선은필터가일관성이있다고가정했을때의 NEES 상한과하한이다. 일반적인재추출방법을적용하였을때는, NEES가 SLAM 초기에급격히증가하여파티클들이불확실성을무시 (opimisic or over-coniden) 해지는현상이나타난다. 하지만, 그림 7에서는 NEES가일관적인필터의상한선을넘지않고있다. 비록, 보정방법을사용하였을때, RBF-SLAM이매순간일관성을보이진않지만, 파티클들이불확실성 (pessimisic) 을유지하며추정을수행하는것을알수있다.
Rao-Blackwellized 파티클필터를이용한이동로봇의위치및환경인식결과도출 33 표 2. 각결과도출방법의오차의표준편차결과도출방법로봇위치로봇방향환경맵 최대가중치 0.70 0.079 0.58 파티클평균 0.0329 0.0027 0.0346 트리루트 0.59 0.000 0.424 끝노드평균 0.0344 0.0037 0.0353 4.3 RBF-SLAM 오차및오차분산 SLAM의기본평가기준은얼마나정확한로봇경로와환경맵을작성이다. 이를위해일반적으로사용되는것이 Roo Mean Square (RMS) 오차이다. 그림 8에각방법의오차를표시하였다. 이결과는모의실험을독립적으로 (Mone Carlo Run) 으로 50회실행하여얻은결과이다. 결과를보면, 가장일반적으로사용되는가중치가가장큰 (highes) 파티클의데이터를이용했을때, 로봇의 2차원위치와환경맵의오차가상대적으로평균을이용했을때보다많이큼을알수있다. 또한, 파티클트리의루트를이용하는방법도다른방법과비교했을때, 이점이없었다. 반면에, 파티클의평균을사용했을때, 모든경우에서가장적은오차를보여준다. 파티클트리의끝노드를이용한경우도평균을이용한방법과유사한성능을보이지만, 파티클트리를만들기위한추가적인비용이필요하여, 전체평균방법이낫다. 모의실험회수에대해서, 각각의방법에대한오차의표준편차를표 2에나타내었다. 파티클데이터의평균을이용한경우, 표준편차또한가장적었다. 즉, 파티클의평균데이터를이용하여하나의로봇경로와환경맵을추출했을때, 높은확률로오차가적은결과를얻었다. 특정번째의모의실험이끝난후각방법에의한결과를그림 9에나타내었다. 결론적으로, 같은데이터를어떤방법으로도출하느냐에따라서로다른결과를보여준다. 5. 결론 본연구는파티클다양성이유지될때, RBF-SLAM 을결과를도출하기위한방법들을제안하고, 성능을비교하였다. 이것은많은연구자들이파티클필터를사용하지만그결과를도출하기위한방법에대한고찰을하지않고, 최대가중치를가지는파티클의데이터를결과로서사용해왔다. 하지만, 본연구결과에따르면, 전체파티클의정보를모두이용하는파티클평균방법 이가장적은추정오차를보였다. 더욱이이방법은다른방법에비해적은표준편차를가졌다. 즉, 여러회동안추정성능이좋다는것을의미한다. 본연구결과 Y (m) 40 35 30 25 20 5 0 5 landmark environmen pah o robo pah o highes weigh pah o roo pah o mean map o mean map o highes weigh map o roo 0 0 5 0 5 20 25 30 35 40 X (m) 그림 9. 각방법으로로봇의경로와환경맵도출 를보면, 여러파티클의정보를이용하여결과를도출하는경우가단일파티클의정보를이용하는경우보다그결과가좋음을확인하였다. [] K. Murphy, Bayesian map learning in dynamic environmens, Advanced in Neural Inormaion rocessing Sysems, MIT ress, 999. [2] A. Douce, N. de Freias, K. Murphy, e al., Rao- Blackwellized paricle ilering or dynamic Bayesian neworks, in roc. o Conerence on Uncerainy in Ariicial Inelligence, pp. 76-83, 2000. [3] M. Monemerlo, FasSLAM: A acored soluion o he simulaneous localizaion and mapping problem wih unknown daa associaion, h.d. Disseraion, Carnegie Mellon Universiy, 2003. [4] T. Bailey, J. Nieo, and E. Nebo, Consisency o he FasSLAM algorihm, in roc. o IEEE In l Con. on Roboics and Auomaion, pp. 424-429, 2006. [5] N. Kwak, G. W. Kim, and B. H. Lee, A new compensaion echnique based on analysis o resampling process in FasSLAM, Roboica, vol. 26, no.2, pp. 205-27, March 2008. [6] G. Grisei, C. Sachniss, and W. Burgard, Improved echnique or grid mapping wih Rao-Blackwellized paricle ilers, IEEE Transacions on Roboics, vol. 23, no., 2007. [7] S. Thrun, W. Burgard, and D. Fox, robabilisic roboics, Cambridge: MIT ress, 2005. [8] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Esimaion wih Applicaions o Tracking and Navigaion, John Wiley and Sons, 200.
34 로봇공학회논문지제 3 권제 4 호 (2008.2) [9] kd-ree(shor or k-dimensional ree), Wikipedia, hp:// en.wikipedia.org/wiki/kd-ree [0] N. Kwak, B. H. Lee, and K. Yokoi, Resul represenaion o Rao-Blackwellized paricle ilering or SLAM, In l Con. on Conrol, Auomaion and Sysems pp. 698-703, 2008. 곽노산 200 아주대학교기계공학 ( 공학사 ) 2008 서울대학교전기. 컴퓨터공학 ( 공학박사 ) 2008~ 현재 AIST 자율행동제어 JSS osdocoral Fellow 관심분야 : SLAM, aricle diversiy and cooperaion, Humanoid robo exploraion 이범희 978 서울대학교전자공학 ( 공학사 ) 980 서울대학교전자공학 ( 공학석사 ) 985 Universiy o Michigan. 컴퓨터, 정보, 제어공학 ( 공학박사 ) 985~987 urdue Universiy 조교수 996~ 현재서울대학교전기. 컴퓨터공학부교수 2004~ 현재 IEEE Fellow, Roboics and Auomaion Sociey 관심분야 : 다개체로봇제어, moion planning Kazuhio YOKOI 984 Nagoya Insiue o Technology, Mechanical Eng. (B.E.) 986 Tokyo Insiue o Technology, Mechanical Eng. Science (M.E.) 994 Tokyo Insiue o Technology, Mechnical Eng. Science (h.d.) 2003~ 현재 Join Japanese-French Roboics Lab. Co-direcor 2004-현재 AIST 자율행동제어 Group Leader 2005~ 현재 Universiy o Tsukuba, 교수관심분야 : Humanoid robos, Inelligen robo sysems