mühendislidergisi Dicle Üniversitesi ühendisli Faültesi Cilt: 4,, 3-9 P *1, Firet ARI 2 1) 2) Özet problem olara bilinmetedir. Litera Joint Probabilistic Data Association, JPDA) Smultaneous Localization and apping, SLA), stati ma JPDA a, SLA JPDA ile birlite estirici olara Unscented Kalman Filter, UKF) II (nearest neighbor (NN)-EKF) ve ousuz (NN-UKF) ile JPDA Anahtar Kelimeler:, ousuz Kalman, veri * 77
Data Association for Simultaneous Localization and apping in Clutter Environment Etended abstract Robot or vehicle has been tried to build up a map and simultaneously localize its position within an unnown environment or to update its position and map which was called problem in the early of 199. Smith et al. (199) called his problem simultaneous localization and mapping (SLA). They shows SLA is general problem that while mapping the environments measurement noises statistically dependent on before their values and monotonically growing with map building, so robot/vehicle incorrectly localize their position and obtain environment mapping. There are several theoretical and applicable studies available in the literature. There are some specific problems available with this method. For eample, minimization of observation and measurement noises, increasing the number of obect in the building environment and robot/vehicle remembering the occurrence of its position which is nown as data association(correspondence) in the Recent studies considering these problems and have tried to propose a solution with using different statistical methods based on Bayes theorem. While some of these studies trying to minimize the effects of observation noises on the mapping and position errors, some of them have developed algorithms to reduce the processing times cause of increasing the number of obects related environments causing problems in real-time applications (ontemerlo,. et al 21, Kim C. et al 28). However, some studies have focused on the data association problem, which is nown as remembering the occurrence of the robot/vehicle, in other words trying to solve the problem of uncertainty of the obect location (Neira J. and H. Wong et al 21). These studies can be listed as Kalman- based estimators, sequential monte carlo approaches, nown as particle filters and their derivatives, and epectation maimization based estimators. When observation noise statistically increases over time, measurement data can be obtained in comple and leads to formation of measurement uncertainty. SLA method taes advantage of the hallmars of an autonomous robot/vehicle location information while the surrounding the obects. If the landmars are obtained the correct information, position of autonomous robot/vehicle is used to obtain the correct measurement. In some cases, the number of landmars and to be close each other leads to the interference which landmar is arrival of the measurement landmars. In this situations, declining the performance of estimators, leading to increase mean square error of mapping and positions of autonomous robot/vehicle, so SLA problem causing the results in a number of uncertainties with improper obtaining building of angle. In this case, there is a need for data association. Successful data association is provided by observed measurement results from itself correctly association. In the SLA problem, the estimator is able to forecast the new landmars, recognize the false alarms (incorrect measurements) and follow the measurements correctly. The most basic algorithm for data association is nearest neighbor method. This method uses ahalanobis distance during processing. ahalanobis distance calculates the distance between measured and predicted observation. Algorithm accepts predicted target position closest to the measured position as valid measurement. According to observation measurement creating the acceptance region for net renewal of landmars, acceptance region is referred to as gate. However, the measurement may not be associated with the nearest landmar, in NN filter not interested in this situation. Therefore, updated state vector may lead to divergence. It is also observed in dynamic environments, are not performing well (Re H. Wong et al 21). If number of landmars is very high and landmars to be close to each other in noisy observations, NN algorithms do not give better results addressed in the previous studies. Because of this, predictive value of actual measurement from the nearest measurement is nown as a reference to valid measurement in the gate. NN algorithm shortens the processing time and not used all the measurements to reach conclusions quicly, so the situation away from the optimal structure. Probabilistic data association (PDA) algorithm calculates the probability of being the target measurement all measurement in the gate. PDA calculates a combined value of innovation, as it tries to solve stochastic problem of uncertainty. Using the relational lielihood of hypothesis provides a unified innovation, and with it creates a unified valid gate. 78
During the operation, algorithm accepts the independent of target and not interfering neighboring landmars. However, in SLA problem landmars are correlated with each other, mae mutual interference cannot be ignored. Therefore, it is not suitable multiple obect or target tracing and dynamic situations, or algorithm should be run for each target. This is a retreat for PDA and emphasizes oint PDA (JPDA) for multiple target tracing. JPDA has been developed for following all targets in a loop. There are some studies available using JPDA for solving data association problem of SLA in the literature. (Re H. Wong et al 21) JPDA for solving of wireless sensor networs in SLA problem and 3-scan JPDA algorithm was used. They said that on the basis of noisy sensor information and possible false repercussions that the signal has white noise and this leads to uncertainty in the position and angle of incoming signals. In other JPDA based approach proposed by Zhou et al. Their proposed method is measurement-oriented, using Depth-First-Search (DFS) algorithm for generation of hypothesis. These studies have been generally used to solve the problem of data association in SLA problem. However, performance of the algorithms use the filters is not covered. A number of comparisons were only made during the uncertainties. Proposed study taing into account previous studies have focused on. two new approaches on a more appropriate for SLA problem. First, there is an alternative to be presented the problem of data association problem of SLA in high noisy and uncertainty in featurebased environment. Developed algorithm for the problem of SLA application is proposed for the first time used related environment and scenario. Another improvement is the used filter. Etended Kalman filter (EKF) is generally preferred in previous studies (Re H. Wong et al 21, ontemerlo,. et al 22). In this study, unscented Kalman filter (UKF) is considered to be more successful in minimizing observation noise problem in SLA. UKF tries to estimate posteriori probability distribution of state by selecting a certain number of sigma points on probability distribution with a non-linear function. This method allows filter less time to mae accuracy and processing speed than FastSLA based particle filters. JPDA based UKF is for the first time used for SLA problem in this study. The correct filter used with solution of the uncertainty of situation is thought to achieve the desired result is more favorable. Keywords: Simultaneous Localization and apping, unscented Kalman Filter, data association, oint probabilistic data association. 79
tahmini çevre bilgisini ullanara hem onumunu (199) Simultaneous localization and mapping, SLA) bir problem olara bilinmetedir. SLA modelinde istatistii olara luren ilendirme problemi olara bilinmetedir. harita ve pozisyon hatalar 21, ontemerlo. ve Thrun S. 22, ontemerlo,. Ve ar., 21 ), ni azaltmaya yöneli (ontemerlo,. ve ar 21, Kim C. ve ar. 28) nesne yeri belirsi blemini çözmeye H. Wong ve ar. 21) bir ç estir stiriciler olara SLA bilgilerinin de a e SLA ro umlarda açmata, böylece SLA ol. Bu duyulur. abilir. SLA probleminde ise algoritma yeni nesn ( Re H. Wong ve ar. 21). yle ilgili en Nearest Neighbor- (ahalanobis P.C. 1936). Algoritma tahmin edilen hedef pozisyonuna en NN ) 8
gelmeyebilir, faat süzgeç bu durumla ilgilenmez. Bu yüzden durum vetörü performans H. Wong ve ar. 21). Birbirine yüse gürültülü durumlarda NN ön ve ar. 21, ontemerlo,. ve ar 22). rden NN Probabilistic Data Association, PDA hesaplar. PDA hesaplar, bunun ile belirsizli problemini hedeflerle Öte yandan SLA probleminde rinin orelasyonu, bu hedeflerin (Re H. Wong ve ar. 21) izleme ve dinami durumlar için PDA PDA SLA PDA (Joint Probabilistic Data Association, JPDA). PDA a bir döngüde taip edilebilmesi için JPDA Literatürde SLA JPDA mevcuttur (Re H. Wong ve ar. 21). Re H. Wong ve ar ar. (21) SLA tarama JPDA. söylemetedirler. Re H. Wong ve ar. (21) bir JPDA Zhou B. and Bose N (1993) Zhou B. and Bose N (1993) Fast- JPDA yöntemi ile birlite Depth-first- search (DFS) yöntemini me (ultiple Hypothesis Tracing, HT) hipotezleri burada SLA ç malar göz önünde bulundurulara SLA SLA probleminin atif SLA JPDA için il defa önerilmetedir. JPDA SLA gözlemle (Re H. Wong ve ar. 21, ontemerlo,. ve ar 21 Bailey T. 21). (Unscented Kalman filter, UKF) SLA edilmesinde daha 81
. UKF süzgeci deterministi bir model durumunun SLA prob (212), Kim C. ve ar. (28), ve Bailey T. (22) larda bu süzgecin daha ni JPDA SLA. Ç ve.,. b veri hipotezleri, IV. bsuz alman süzgeci, V. b deneysel sonuçlar son olara VI. b. SLA için e stoasti modelin, :, :,,, : 1, :, Pz Z, U P m Z U P z m P m Z U : 1 : (1) urada m, Z ve U : : bütün vermetedir. P z, m z, z 1, z 2,..., z m verir ve z h(, m) (2) olara modellenir. Burada N(, Q ) z h( ) (3) / 1 olara verilir. iü, vzz z h( / 1) (4) S h( ) [ h( )] T Q / 1 v / 1 (5) olara verilir. h ( / 1 ) v Q sahiptir. (Zhou B. and Bose N, 1993) ahalanobis mesafesi (ya da resi- NIS), T 1 v S v n (6) (Re H. Wong ve ar. 21). 2 NIS N boyutu ile Chi-Kare ( ) n NN test etme için Bütün NN SLA robotun durumdan, PDA 82
JPDA algo. X { 1, 2,..., n } m olara abul edilir ve Z { z1, z2,..., z } olara olara z m, ölçüm m z z z (7) m m o (Y.Bar-Shalom ve ar. 29). Her bir z mz m m1 (8) burada m hedef t m P{ ( ) Z } am( ) (9) 1 m m1 (1), m( ) am( ) z ve hedef [ ( )] (13) 1 1 1 a m.... 1 2 N 1 2 N a1 a1 a z 1 1 1 2 a 2 a2 a N 2 z2 a a a 1 2 N a z (14), i tüm ölçümlerin e P Z P Z Z 1 { ( ) } { ( ) ( ), } 1 ( ( ), 1 ) { ( )} pz Z P c (15) Buradai normalizasyon sabiti c ( ). B 1 pz ( ( ), Z ) 1 pz [ m( ) m( ), Z ] m (16) Hedef m, Gauss burada m=1,2, ; =,1,,N 1 ölçüm hedeften gelmetedir (11) am( ) ölçüm hedeften gelmemetedir pz Z 1 [ m( ) m( ), ] N( z ; m zm, Sm) e m( ) 1 1 A e m( ) (17) burada ( ) ( ) ( m ) (12) m m1 N( z ; z, S ) m m m 1 ( z 1 1/2 m Sm z ( ) 1 ( m ) T ) 2 G (2 Sm) e P ( ) ( (18) 83
m A ( A) gösterilir. Burada P G PF ( m ) e (24) m he z m m! ve S m burada ve A 1/2 A S( ) (25) m olara m c =-m o9) olara verilir, N m! A A 1 P{ ( )} e ( PD) t(1 PD) t (26)! m! t1 P{ ( )} (19) ö P{ ( ) ( ), ( )} P{ ( ), ( )} A ifade edilir. Burada ( ) fonsiyonu m A-m gibi Re H. Wong ve ar. 21). (17) ve ( ) am( ) 1 1,..., N (2) m1 ve ( ) olay m( ) ilgili olayda m N m( ) am( ), m1,..., (21) 1 ( ) [1 m( )] (22) m1 N 1 { ( ), ( )} ( D) t(1 D) t F( ) t1 P P P P m (23) ifade edilir. Burada P F (Y.Bar-Shalom ve ar. 29): 1 m ( ( ), ) ( ; m m, m) m1 pz Z A Nz z S (27) P{ ( ) Z } m N c t1 m1 N( z ; z, S ) ( P ) (1 P ) m m m m 1 D t D t SLA (28) PDA JPDA Bu PDA r. JPDA
sonuçlar verebilmetedir (Pafiliz, 24, Y.Bar- Shalom ve ar. 29). JPDA. fazla n üst üste binere (Pafiliz 24). 1 2 1 1 1 z1 1 1 1 z2 1 1 z3 { az } (29) 1 1 z4 1 z5 1 1 z Burada ; 1 ve 2 hedefleri biliniren, parazit erilir. Bu matrise dayanara JPDA - - - 6 Temel olara bu abullerde sü elemandan sadece bir eleman sorumlu olur. Gelenesel olara geçerlili matrisi içerisinde algo Re H. Wong ve ar. 21) arma SLA nesne uygun problemi. a DFS (Y.Bar-Shalom ve T. Fortman 1988) Bu geremetedir. Bu hipotezle için B. Zhou, and N. Bose 1993)e JPDA - UKF Güncelleme ve (Y. Bar-Shalom ve ar. 29) m ( ) 1 i i1 () i (3) 85
Burada m ( ) inci hedef için geçerli 1 durum tahmini, () i i nci geçe UKF güncellemesi ve i göstermetedir (Y. Bar-Shalom ve ar. 1995, 29). Ko P P 1 m ( ) T [ P () i ( () i )( () i ) ] i i1 (31) 1, P 1 z 1 ve onun inovasyon S UKF e A da Bu bölümde, daha önceden Tim Bailey (22) malar ilmetedir; il uygulama SLA in gürültülü olara bilinmete, iinci uygulama ise yine a Noise Ratio-SNR) ba P D (P F ) SNR P F P D daha faz Re H. Wong ve ar. 21) dur 3* /18 ol 2.5 Q 2 [5*( /18)] 2.5 R 2 [5*( /18)] (32) (33) las 8*.25 saniye olara b e. 2 1 olara beyaz Senaryo I: m,- bitmetedir. JPDA-UKF süzgecinin birbirine metre 4 2-2 -4-6 -8-1 -12 UKF-BOVI suzgeci parazit yanisiz ortam estirim sonuclari nesne isaretleri gerce rota rota dugumleri arac gerce arac tahmin edilen tahmin edilen rota lazer cigisi tahmin edilen nesne arac ovaryans elipsi nesne ovaryans elipsi 5 1 15 2 metre JPDA - Senaryo I NN-EKF, NN-UKF, FastSLA II ve JPDA - 86
tei gibi Aci (Rad/Sn.) etre.2.15.1.5 Robot Basli Acisi Hatasi nn-ef nn-uf fastslam pda-uf 5 1 15 2 25 3 35 4 45 5 25 2 15 1 5 Robot X-Y oordinati onum Hatasi nn-ef nn-uf fastslam pda-uf 5 1 15 2 25 3 35 4 45 5 ortalama a bilgiler vermetedir. bu hata areleri etisiyle esi sonucu SLA hata. estiriminden itibaren harita boyunca bilgiler (teori olara nesne nin var gerelidir -Y a yla daral JPDA- eyiz. estirim Std.sapma-X (m) Std.sapma-X (m) Std.sapma-X (m) 6 4 2 5 1 15 2 25 3 35 4 45 5 6 4 2 5 1 15 2 25 3 35 4 45 5 6 4 2 5 1 15 2 25 3 35 4 45 5 6 Std.sapma-X (m) 4 2 5 1 15 2 25 3 35 4 45 5 ((a)- 87
Std.sapma-Y (m) Std.sapma-Y (m) Std.sapma-Y (m) Std.sapma-Y (m) 6 4 2 5 1 15 2 25 3 35 4 45 5 6 4 2 5 1 15 2 25 3 35 4 45 5 6 4 2 5 1 15 2 25 3 35 4 45 5 6 4 2 5 1 15 2 25 3 35 4 45 5 ((b)- az - ni ve aabinde ya sonucunu vermetedir. gibi JPDA- lmetedir. JPDA- varyans minimizasyonu ile paralel olara en uygun sonucu JPDA- NN-EKF.311 3.3582 NN-UKF.226 1.9287 FastSLA II.23 2.3915 JPDA-UKF.1747 1.793 Senaryo II CD senaryo I -11m) JPDA-UKF. Senaryo II etre 6 4 2-2 -4-6 -8-1 -12 gerce rota rota dugumu arac gerce arac tahmin edilen tahmin edilen rota lazer cizgisi tahmin edilen parazit yani tahmin edilen nesne isareti parazit yani elipsi nesne ovaryans elipsi 5 1 15 2 arac ovaryans elipsi etre JPDA - SLA ortamda JPDA- 88
verilmetedir. Aci (Rad/Sn..2.15.1.5 nn-ef nn-uf fastslam pda-uf Robot Basli Acisi Hatasi 1 2 3 4 5 2 15 1 5 nn-ef nn-uf fastslam pda-uf Robot X-Y oordinati onum Hatasi 1 2 3 4 5 Std.sapma-X (m) Std.sapma-X (m) Std.sapma-X (m) Std.sapma-X (m) 6 4 2 5 1 15 2 25 3 35 4 45 5 6 4 2 5 1 15 2 25 3 35 4 45 5 6 4 2 5 1 15 2 25 3 35 4 45 5 6 4 2 5 1 15 2 25 3 35 4 45 5 - -EKF, b) NN-UKF, c) FastSLA II, ve d) JPDA-UKF Std.sapma-Y (m) Std.sapma-Y (m) Std.sapma-Y (m) Std.sapma-Y (m) 6 4 2 5 1 15 2 25 3 35 4 45 5 6 4 2 5 1 15 2 25 3 35 4 45 5 6 4 2 5 1 15 2 25 3 35 4 45 5 6 4 2 5 1 15 2 25 3 35 4 45 5 - -EKF, b) NN-UKF, c) FastSLA II, ve d) JPDA-UKF. NN-EKF.3217 3.463 NN-UKF.245 1.952 FastSLA II.2623 2.275 JPDA-UKF.186 1.8634 senaryo I 89
. senaryo I JPDA-UKF sebebi JPDA- erisindei bu ise olumsuz etilenm SNR a SNR daha senaryo I JPDA- - senaryo II edebilme abiliyetini JPDA- nin Sonuçlar SLA probleminin bilinen ii yararlanbu metotlara alternatif olara SLA JPDA JPDA- JPDA- belirsiz JPDA-UKF adar minimize JPDA duruma g SLA JPDA- 9
E A UKF süzgeci için JPDA JPDA Y. Bar-Shalom ( Hedef durumunu Unscented Transform (UT) (S. J. Julier and J. K. Uhlmann, 24 ) z ) tahmin edebilmetedir. i z i, ) olara abul edilirse; nota matrisi, 1 1 [ 1 1 (),..., 1 1 (2 n)] ve buradan, 1 1 [ 1 1, X 1 1 P, 1 1 X 1 1 P ] 1 1 P ( n P 1 1 ( ) 1 1 (34) Burada P 1 1 Cholesy fatorizasyonunu verir. X 1 1 ise n* n 1 1 e 1 1 [ ( ()),..., ( (2 ))], 1 1 [ f 1 1 f 1 1 n e l S. J. Julier and J. K. Uhlmann, 24) 2n 1 n ( 1 n w ( 1 n ) (35) burada ovaryans matrisi elenere bulunur, 2n 1 n ( ) 1 1 n T 1( n ) 1 P Q w ( ( n) ) ( ( ) ) (36) Burada Q 1 1 [ 1, X 1 P, 1 X 1 P ] 1 P 1 ( n ) P 1 elde edilir ve [ h( ()),..., h( (2 n ))] 1 1 1 (37) g matrisi, 2n 1 n 1 n z w ( n) (38) 2n n 1 1 n T 1 n z 1 S R w ( ( n) z ) ( ( ) ) olara verilir (Y.Bar-Shalom 29). (39) i z() i z 1 91
() i W ( z () i z ) P P W S W 1 1 T 1 ( ) (4) o S. J. Julier and J. K. Uhlmann, 24 2n n 1 1 n T 1 1 n 1 S W w ( ( n) ). ( ( ) ) ( ) Kaynalar (41) Bailey T., (22). obile robot localization and mapping in etensive outdoor environments, Ph.D. dissertation, Univ. Sydney, Sydney, NSW, Australia. Bailey T., (23), Constrained Initialization for Bearing-Only SLA, IEEE International Conference on Robotics and Automation. Bosse., Leonard J., and Teller S., Leonard J.,, Eds., (22). Large-scale CL using a networ of multiple local maps, in Worshop Notes of the ICRA Worshop on Concurrent apping and Localization for Autonomousobile Robots (W4),Washington, D.C. Guivant J. and Nebot E., (21). Optimization of the simultaneous localization and map-building algorithm and real-time implementation, IEEE Trans. Robot. Automat., vol. 17, no. 3, pp. 242 257. Kim C., Sathivel, R. ; Chung, W.K., (28) Unscented FastSLA: A Robust and Efficient Solution to the SLA Problem. IEEE Transaction on Robotics, pp 88 82. Kuipers B. J. ve Byun Y.-T., (1991), A robot eploration and mapping strategy based on a semantic hierarchy of spatial representations, Journal of Robotics and Autonomous Systems, 8: 47-63. ahalanobis, Prasanta Chandra (1936). On the generalised distance in statistics. Proceedings of the National Institute of Sciences of India 2 (1): 49 55. Retrieved 212-5-3. ontemerlo,. Thrun, S. Koller, D. ve Wegbreit, B (22), FastSLA: A factored solution to the simultaneous localization and mapping problem, Proceedings of the AAAI National Conference on Artificial Intelligence, 593 598. ontemerlo. and Thrun S., (23). Simultaneous localization and mapping with unnown data association using fastsla, in Proc. IEEE Int. Conf. Robot. Automat., Taipei, Taiwan, pp. 1985 1991. ontemerlo., Thrun S., Koller D., and Wegbreit B., (23). FastSLA2.: An improved particle and mapping that provably converges, in Proc. 18th Int. Joint Conf. Artif. Intell., Acapulco, eico. (21), Data association in stochastic mapping using the oint compatibility test, IEEE Trans. Robot. Automat., 17( 6), 89 897. Norgaard., Poulsen N., ve Ravn O., (2), New Developments in State Estimation for Nonlinear Systems, Automatica, 36(11):1627 1638. (24), Development of A Probabilistic Tracing Algorithm for aneuvering Targets in Cluttered Environment, Ph.D. thesis, Anara University, 27 p., R. Smith,. Self, and P. Cheeseman. Estimating uncertain spatial relationships in robotics. In I. J. Co and G. T. Wilfong, editors, Autonomous Robot Vehicles, pages 167{193. Springer, 199. Re H. Wong, Jizhong Xiao and Samleo L. Joseph, A Robust Data Association for Simultaneous Localization and apping in Dynamic Environments, Proceedings of the 21 IEEE International Conference on Information and Automation June 2-23, Harbin, China S. J. Julier and J. K. Uhlmann, (24), Unscented Filtering and nonlinear vol. 92, no. 3, pp. 41 422, ar. Thrun S., Fo D., and Burgard W., (1998). A probabilistic approach to concurrent mapping and localization for mobile robots, ach. Learn., vol.31, pp. 29 53. Thrun S.,(22). Robotic mapping: A survey. illenium. The organ Kaufmann Series in Laemeyer (Editor), Bernhard Nebel (Editor). ISBN ISBN-1: 155868117. Williams S. B., Newman P., Dissanayae G., and Durrant-Whyte H., (2) Autonomous underwater simultaneous localization and map building, in Proc. IEEE Int. Conf. Robot. Automat., San Francisco, CA, vol. 2, pp. 1792 1798. 92
Williams S., Dissanayae G., and Durrant-Whyte H. F., (21). Towards terrain-aided navigation for underwater robotics, Adv. Robot., vol. 15, no. 5, pp. 533 55. Y. Bar-Shalom and T. Fortmann, Tracing and Data Association, Academic Press, 1988, Y. Bar-Shalom and X. Rong Li, ultitarget- ultisensor Tracing: Principles and Techniques. Storrs, CT: YBS Publishing, 1995, Y. Bar-Shalom, F. Daum, and J. Huang, The probabilistic data association filter, IEEE Control Syst. ag., vol. 29, no. 6, pp. 82 1, Dec.29. Zhou B. and Bose N.( 1993), ultitarget Tracing in clutter:fast Algorithm for Data Association, IEEE Transaction on Aerospace and Electronics, 93