INSTITUT NATIONAL DE RECHERCHE EN INFORMATIQUE ET EN AUTOMATIQUE Sensor-Based Control Architecture for a Car-Like Vehicle Christian Laugier, Thierry Fraichard, Philippe Garnier, Igor Paromtchik and Alexis Scheuer N 3552 Octobre 1998 ` THEME ISSN 0249-6399 apport de recherche q rqstq rrqr q rqq qq q qq q qstqtq qqq qqqqq qr rqrq rrqq rqq qqrqrrq qq rqqqrqqqqqrq rq rq qrq q qqqq q rqrqqq qqqqrq qq qqqq rqq q qr qqrq qqqqrqqrqq q qqẽqqqr r qqqqrq qqr qq qqqq qq qéqq qqq qẹềểẹễếệìỉ r qqrqqqqrqẹ ềểẹễệệìếệệệếĩì qqqírqrqqííqqq qrqq qqịqq òỏõóọ rr qqqq q ổỗộố ờởỡ ộố ợờùủ Unite de recherche INRIA Rhone-Alpes 655, avenue de lEurope, 38330 MONTBONNOT ST MARTIN (France) Telephone : 04 76 61 52 00 - International: +33 76 61 52 00 Telecopie : 04 76 61 52 52 - International: +33 76 61 52 52 qq q qqq q qq qqq qqq qrqrqrq rrqqrq qqq qq qqqqrqrqqqrq q ỉ qrq qqqr q rqq ị qq qrqqq qq q qqq qq r q q q q ểếẹq q rq q ẽqqqq qqqqq qẽq qrqq qq qrqq q qqrqq qqqrqqq q qqqq qqqq qrqq qqrqqqrqq q q rq q qqqqq q q qqq qqqéqq qq q qqqq qếệ ểẹẹềểẹỉẽqrrq qqq qqqrqq qqqqqqqqq q qq rqq q rq qqqq qq q qq qq ọ õ rqqrqrq qqq qq ẹệ ể ẹểẹ ĩ ểệ ọ ọ óọ ọ ọ q ọ ọ ọ ỏ q q s q ọ õ q q q q ọ ọ ọ s s ìể ẹể ĩ ể ẹệ ể ẹểẹ ĩ ểệ rqq rrí qqíq qqqqqqq q q qq rqqr rqrqqr qq qqqqr rqrq qq qqqqqq rq qqqqq q qq qqqq qqq qq rqír qrqr q ệịẹểẹq q q qq qq q qq qqqỉqqqrqrq q qqqqq qrrqq qqqẽqrq qq q qq qr qq r rqqq qrq q q q qrrqrq q qqrqqqr qéqqqqq q rrq qqrqqrq í r rqỉ qq qqqq qqq q qq qqq qrq rqỉ q qqqq qqqqqq qq q qéqq q qqqqq q qqq q q q qqrqqqqqqq qqqqqqrq qq qqq qịríqqrq qrq qqq q q qq q qqỉ s qrrq q qq qẹềểẹễếệ qqqqrq qqq qq q qqq q rqrq qrqqr qqr qqq q qqqqrqqq q rqqqqq q qqrq qqqrqrqq qqq íqqrqq q qq qqqqrqq qqq q qqqq qq qq ỉ qqr q qqqqể ếẹ qqqrq qqqqq qq qqrq qq qq rqq qqrq qqrqqrqrqqỉ ìể ẹể ĩ ể World Model + Prediction Trajectory Planner Mission Description Mission Monitor PMP generation and update SBMs library Execution Report + Sensor data SBM [+ Nominal Trajectory] Motion Controller SBM execution Skills library Skills instantiation and execution Commands Sensor Data q qq qq qqq qrqq rqq qq qqqrqq qq q rq qq qr q q qrqqqqqrqqỉ rqểẹệ ìĩệệ qqqrq qrqqqqqq qq ẹệ ể ẹểẹ ĩ ểệ qrỉqq qqq qqrr qq q rqqq qq qq qqrqịrqqqr q q q qqỉể qqr éqqíqqq qqq rq qq q qrqq qqqq r qq q qr qịrỉể qqrqq q qqẽqqqr r qqqqrq qqr qq qqqịq qqq éqq q qqqqr rq qẹ q ẹểẹệệ qqqq q qrq ì qrrỉ qqqếẹẹíếệỉ qqqqq r qỉ rqq rqq q qqqqr r qẽq rq ỉ qq q q rqrqqqqẽqqỉ q qq q qqq qqqqqqq qqq q qqrr qq q qqqrq qqq ếĩể ệ qq qqrq qqẹỉ ìq qrq ệếểẹ q qrqq qrq qqq qq rqq qrqqqqq q q qỉ qqqíqq qq q q qq q qq qqq qqrqq q qq q qq rq qrq ìqqq qq qqqíqq rqqq rqq qq qr qq q qqq qqqqẽq qqẽqq qqqíq ìể ẹể ĩ ể qqrq qqqqqq qq qqq q q qq q qq q q qqqqỉrqqrqqqrq q qq q qqq q qq qrqqr qqq qqr q q qqq qq qrqqq q qqq q qq qq qq q q qq ỉ qq ỉ ỉ q qq qqqr ỉ q q qq q qqqqq q qr qq q qr qq qqq qqẽ ỉ qq ỉ qqqq qqqq q qq q q q ỉ qrq qqr q qqq ỉ qqqqrqq qrr q q qq qqqqrq rq q ỉ ểẹểẹẹ ỉ ìể ẹể ĩ ể Parallel Parking Trajectory Following Nominal Trajectory yes Goal reached ? Trajectory tracking obstacle detection no no Obstacle avoidance possible ? Find a parking place Free parking place detected Free-lane description Local map building + FAILURE yes Lane changing FAILURE Reaching an appropriate start location Free-lane reached Free-space parameters Trajectory tracking Generation-execution of appropriate Local Trajectory backward & forward motions no Updating no Success ? local map Obstacle overtaken ? yes Nominal trajectory to reach yes Lane changing SUCCESS yes Nominal trajectory reached ? no SUCCESS q q q q qq qq q qrqqqqq qq rqqệểệẹềểẹ qqqq q qqqq qq qrqr qqr q rq q qỉrq qqíqq qqqqqịí qr qq íẹềểẹễíẹểĩìệể ìì ỉể q qq ọ ỏ qq q qrr qq q qrqqqrq r q qqq qqq qrq qq ị qqq ẹệ ể ẹểẹ ĩ ểệ qqqq qqqqqrq q qq q rrq qr ỉ ỉs ỉ qq ỉ qqq qqqqq qqqqq qq qq qỉ q qqqqq qq qqqrq qq ỉ qqqqqq q qrq dT n d(st) n sT nominal trajectory obstacle st traffic lane s0 traffic lane q q qr qqq qqqqq rq qq qqqqqqq qr q q qqqqq í qrqq qqqq q q qqq qqq qq q qqỉ q qqqq qq q q qq q q qq qỉể qqr qqq q qqq q ẽrỉể ỉ ỉ ìể ẹể ĩ ể qq qqqq q qrr qqẽq qqrq qq ỉ qrrr qq ỉ ỉ qírrr qqqq qr qqrq q rq r qrq qqqqq q qq ỉ qq qqq qrq qrqq qqqqqq qqí ỉ q qqqqq q qqẽ ỉ ỉ ỉ ỉ ỉ ỉ ì qq q qq q qqqqq qq q qq qqíqqq qq qq rqíỉ rqí qqqqq qq q qq qq r q qq q qq qqqrqq rqqq qqỉqq q q qq q qqq r q q qq q q ỉ qq qq qq qq qqqq s qqqr q q q rq qqrqỉể íqq q q q qq qqqr q qrqqqr qq q q qq qq qrq q qq ẹệ ể ẹểẹ ĩ ểệ L1 traffic direction A11 A14 D1 A1 traffic lane L2 A13 D4 B14 B11 B24 D2 B1 A12 B21 D3 parking lane B2 parking place B13 B12 B22 B23 border of the parking lane q q q q r q q q r q q r q q q q q r q q q r q q q q q q q q q q q q q r q q q q r q q q q q q q q q q q q q q r q q q q q q q q q q q q q q r q q q s q q q q r q q q s q q q q q s q r q r q q q q r q q ỉ s q ỉ q q q q q r q q q q q q r q q q q q q q q q q q q í r q q q q q r q q q r q q q q r q q q q q q q r q r q q q q s q q q q q q q q q q q q q q q q q q r q q q q r q q q r ỉ r q q q q q q q q q ỉ ỉ ỉ q q q q ỉ ỉ ỉ q q q r q q r q ỉ q q r r q q q q q q q r r q q q q r q q q q r q ỉ ỉ ỉ ỉ ỉ ỉ ìể ẹể ĩ ể q q q qrqrq qqqq rqqqq q qỉ qqỉ qqq q q qỉ ỉ r ỉ ỉ ỉs ỉ qq q q qq q qỉ ỉs y(t)/y(T) 0.5 0 0.2 0.4 0.6 x(t)/x(T) 0.8 q qqqr qrrỉ ỉqqí qỉ q qqq qqqqr rqqr rqqq qrrỉ ỉ íqq qqq ỉ íq qq q qq qr qỉ qqq q qqq qr qqqrqqqẽqqqr q qqrỉ qq q íỉ q qẽ qqqqqq qqq qqr q qq qqq qq qqq qqqqq qqqqr qr qrrỉ ỉqqỉỉsqq qqqqỉ ỉs qqrq q qrq rq q q qq qqr r q qq qqqqr q qrr ỉ q qq ỉ qq q qrrrqqqqqqqqq qqq qqqqr qq q q q qq qqr qqq q ẹệ ể ẹểẹ ĩ ểệ q q qq q qq qq q q qrr ỉq qq ỉ ỉ qq ỉ qrqqqqr r qq q q qqq r qr r qrrq q q qq ỉ ỉ ỉ ỉ ỉ ỉ qrrq q qíqq qqrq qq qrr qqqqq q rq q qrqỉ q qrrỉ ỉ qq r qq rqqqẽqrq rqỉ ì qr q q qq rrr q q qqrq íqqq q qq q rrq q qrqỉ ỉ ỉ ỉ ỉqq sqq qq q ỉ ỉ qq qq q qq q íqqqrqq qq q q q q qqqí q qq q rq qqr q qq ỉ q qq qrq qq ỉqq qq qrqq qqẽqr qqq qr qq qq qq q q s qqqqrrq q íqqq qq q qqqq q q q qẽqr qqqqq qq s qq ỉ s qqqq qrqqrqqqq q qrqỉ qí qrrỉ ỉqq qrqqq qqqqq qqqrq sqqqrrqq q s ìể ẹể ĩ ể q qqq q qqíq qqqqqq qq q qqqq qq qrqq q rq qqqqq qqq q í q qq qq qqrqrqqqqqqrq rq qỉrịqíqqq q qqẽq q qỉẹrqqqqqqqq qq q qrqqỉẹ qqqq qr qỉsểẹệẹ qqr qqqq q qq qqqrr q qíq qq rqrqr qểẹệẹq qq qrqqqrq qíẹ qq qrqqqqqỉ q rq qqqq qqqqr rqqr rqqr qq q qqq qqqqrqẽq qqrqqrqq qqqq qr qqr qq qqẽq r qqq qq qqỉrq qq q qqr q qr qqqqqr qqqqq qq qqr qqrq q qq q qqẽq qírr q q qq rqrqq q q s qrqqrq qq q qqqrqqẽq qqỉ qrrrqq qqq qr qr sqq q qỉ qq íqqq q q qqq qq qỉ qq qrqqrqq ír r qr q r qqqqqẽq ỉqq qr qqq qq q q qqr qqq q rqqr q rqrqqqr ì rqrq qqqqqqrqrq qqrqí q q qq ỉ qqrq qqq q rq qq q qq q rqq ỉqq q qrqqqq q q qqqq qqr qqqr q ẹệ ể ẹểẹ ĩ ểệ q q qqỉ qrqỉq í q qỉq qqỉ qrq -10 nominal trajectory y [m] 0.4 2.2 -20 2.1 -30 motion direction velocity [m/s] angle [rad] 0.2 1.9 1.8 -0.2 -40 1.7 local trajectory -0.4 -20 -10 x [m] 10 20 10 20 30 40 time [s] 50 60 70 1.6 10 20 30 40 time [s] 50 60 q rr q qỉrỉ qqq ỉqq q rqqí q qqrq qqrqqq qqrq qq qq qqỉ qq qr qq qqr qq qqq qqqq q rqq qq qrq qq qq q qq qr q q q ỉ q q qq q í q rqqrq qqrq qq qq rqqqqrqrq ìqq rq qq qỉể qq qqrq qqrq q qq qqq qrrqq qq q q qqrqrq q q qq qq qrq íqrq q rqqíq qqq qr q q q rqqqq qq q q qqq qqqr 70 ìể ẹể ĩ ể q q ỉ qq q ỉ qqq ỉqr r ỉrq qq 0.6 0.4 0 -0.2 -2 end location 0.4 0.2 velocity [m/s] start location angle [rad] y [m] -4 -10 -8 -6 -4 -0.2 -0.4 -0.6 -0.4 -2 0.2 10 x [m] 20 30 40 time [s] 50 60 -0.8 10 20 30 40 time [s] 50 60 qs rr qqqỉrỉ qqq ỉqq qqqq q q qrqq rqíqq qrqrqq q qqrq q í r q rq q q rq q qq q q q qqrqqqqq qq qqq q q qqq q qqqrqq qqq q q qqrq qrqqqq qqq q qq rqqrqq qqír qq qqq s qq qr qq qq q qr qq qqqq q rrỉ íỉqqq q q qq qq s sqqq qq qq q q íí q qqq qq qq qq q qq q q qq rqq qq qq s qrq qq qrrq q qqqqrqqrq qq q qq s q qqqrrq qqqq r q q qqqrq qq ỉ ì qq ịq qqqr ẹệ ể ẹểẹ ĩ ểệ s rqq rr qqq qq q rrqqq qqqr qq qqqqqq qq qrqqqq qqrqq qqqqq ỉ qq q qqq r qqqr q q qqq rqqq qqqqqqq qr q qq q qrqq q qqq ệ ẹếếể q rqrq qqrq qqr qqq qqrqq q qqr qqqqqqrq qq qqrqq qrrq rqrq rq qqrq qq r qqrq ị s qíqrq q qq ểẹ ếếể q q q q qq qqqqrrr qq qrqíqéq q rq rq qrqqqqq qqqq q qqqq qq q q qq qqqq qqỉrq q qq qq qqrqqq ị ss qrrq qq qqq qq q qq qqrq qqq q qq q qr ễểẹệể ẹểẹ qqqqq q í qqqqqír q qqqq qqrq q qqrqrqqq qq qq s q qqrqqqqq q qqr qqqqqrqqéqqrq qqqq qqrq qq qqrq éqqqr qq qq qqqq q ì qrq qqrq qqqrqqrqỉqq q qrq q qqqq rq r q qqq q qrq ìể ẹể ĩ ể qrq q qííqqqqr qqqr qqq q qqqqq qq ị qqq qqrq qrq qqqrq r q qrr qqrq q qq qq q rq q qq qrqqqqịrq qqqrq qq qqqq rqq q qrqqrq qrqq q qqẽqqqr rq qqqqrq qqr q qqqqq qqq qq q rrqrqỉ qq r rqqrqq qqrqr rq qqrqqqịrqqqqq rq r q rq qrq q qqqq q rq rqqq qíqq rqqqqrq qq qqqq rqqí q qịrqqrq qqqq rqqrqq qqqẽqqqírí r qqqqrq qqr qq qqq ịq qqqéqq qqqqq q qqq qq qq rq q qqỉ r qqrq íqqqrqq q qrqq qrqq q qq qqq qqqrq qq q q qq qqq rqq qqqqrq q qq q qrrq q q q q qr qr q q q q qq qq ẹệ ể ẹểẹ ĩ ểệ r q qq rẹệẹểể ỉ sss qrqrq ểẹ ẹ ẹể ẹẹrqq ỉ s ẽ rq rrqrr qq ẹệệ ịìể ẹ ểệệsỉ s q q q q q qq q qqq qq qq q q q q qq rẽqqq rẽq ẽqỉ qqqrrq q qq ì ểẹ ìq s r q q q rq r ểẹ ễếẹ ẹ ếẹ ểểq s s q ỉ qrq qrrq qqq qqqrq ể ệ ẹ ẹể r qq rqrq ệ ìẹ ể rq r qrq q ểẹ ẹ ẹệệịịìẹ ẹ ễẹ rqqs rỉ s ị qq qírq ể ẹ ẹ ẹể ẹẹrqq q ỉ ị q r qíqq qí ểẹ ẹ ẹệệ ịìẹ ẹ ễẹrqsq ỉ qrq ị qqq qq qr q ẹ ểệ ẹệệịịìể ệ ẹ q ìể ẹể ĩ ể ịs q qq ríq q ểẹ ẹ ẹể ẹẹrqq ỉ ị r r q q r q ểẹ ẹ ẹệệịịìẹẹ ễẹỉ qrq qq qq q q q ể ẹệệ ịìẹ ẹ ễẹq ị í ị q rqqq qq q q q q qqrẽqqq rẽqqqrq ị rq ẹ ẹ ệì q qr q ị r í rq rrq ẹể ẹẹ ỉ s q ị q q q q ểìẹ ỉ s q q rq q ểẹ ỉ ẹ í ẹể ẹẹrqsq q q q q qss qq q q ỉ q q rqq qrq ỉ q ểẹ ẹ ẹể ẹẹ qs ss q ỉ s r q rq r q q ểẹ ẹ ễế ẹệệ ịìẹ ểệqs ỉ qqrq r q qq rq q ểẹ ẹ ẹể ẹẹq s s q ỉ r q rq q qq q r ẽqẽq ỉ q q q q qqrẽqqq rẽq q q rrqrqẽqỉsr ẹệ ể ẹểẹ ĩ ểệ qq qr q ể ể ệ ẹ ẹể ỉ s ss ẹ ểệẹệệịịìể s qq q qq q ểẹ ẹ ẹệệ ịìẹ ẹ ễẹrqq s qqỉ qqrq rrqr ẹể ẹẹ ỉ s s r q rq q q qq q ểẹ ẹệ ễẹ ể qệìễ qqs ịrq qqrqqs q qr q rrqq r ểẹ ẹ ẹể ẹẹrqq s s ỉ r q q ểẹ ẹ ẹể ẹẹq ỉ qq qqq q ẹ ĩ ế ẹệệ ịìẹ ẹ ẹệịrỉ s s s ss Unite de recherche INRIA Lorraine, Technopole de Nancy-Brabois, Campus scientifique, ` NANCY 615 rue du Jardin Botanique, BP 101, 54600 VILLERS LES Unite de recherche INRIA Rennes, Irisa, Campus universitaire de Beaulieu, 35042 RENNES Cedex Unite de recherche INRIA Rhone-Alpes, 655, avenue de lEurope, 38330 MONTBONNOT ST MARTIN Unite de recherche INRIA Rocquencourt, Domaine de Voluceau, Rocquencourt, BP 105, 78153 LE CHESNAY Cedex Unite de recherche INRIA Sophia-Antipolis, 2004 route des Lucioles, BP 93, 06902 SOPHIA-ANTIPOLIS Cedex Editeur INRIA, Domaine de Voluceau, Rocquencourt, BP 105, 78153 LE CHESNAY Cedex (France) ISSN 0249-6399 [...]... ✠ ☞✞ Parallel Parking ☞✞ ✟✞ Trajectory Following Nominal Trajectory yes Goal reached ? Trajectory tracking obstacle detection no no Obstacle avoidance possible ? Find a parking place Free parking place detected Free-lane description Local map building + FAILURE yes Lane changing FAILURE Reaching an appropriate start location Free-lane reached Free-space parameters Trajectory tracking Generation-execution... appropriate start location Free-lane reached Free-space parameters Trajectory tracking Generation-execution of appropriate Local Trajectory backward & forward motions no Updating no Success ? local map Obstacle overtaken ? yes Nominal trajectory to reach yes Lane changing SUCCESS yes Nominal trajectory reached ? no SUCCESS ✟⑧❸❼❊➞❖④③q ❿➭ ♥❧ ♠ q ✛➁➅❿⑤❭④➁⑤❥➝❸➝❸q✴➝♣➅❿⑤❭④➁➠●⑧❸✇❿❼ ✢✙⑤❭✇❿❾ ✛③②➁④⑥⑤❥➃①q✾⑦✹②⑥⑨❊④⑥➤❷➧➩⑨❭➝❸➝⑩⑨◆➟♥⑧❻✇❹❼... de recherche INRIA Lorraine, Technopˆole de Nancy-Brabois, Campus scientifique, ` NANCY 615 rue du Jardin Botanique, BP 101, 54600 VILLERS LES Unit´e de recherche INRIA Rennes, Irisa, Campus universitaire de Beaulieu, 35042 RENNES Cedex Unit´e de recherche INRIA Rhˆone-Alpes, 655, avenue de l’Europe, 38330 MONTBONNOT ST MARTIN Unit´e de recherche INRIA Rocquencourt, Domaine de Voluceau, Rocquencourt,... ➵✹➸❖➳✴➺◆➻✪➼ ❛➾◗➳✴➵➚➪ ❘➺◆➸✍Ñ❳➻⑥➺❥Ö ➛➻⑥Ó ❪➬➱Ñ❃➵➚Ó✹Ñ■➘❹➻⑥➵✚Ú✹➺❥➻✡➾ ❘➾❥➻✪➼ ✚➬⑩Ü❊➵ ❵➵ ❹➬➫Ó✹Ö⑩➵ ✂✁ ☎✄ ✝✆ ✟✞ ☎✄ ✂✠ ➎◆➑ ☛✡ ☞✞ L1 traffic direction A1 1 A1 4 D1 A1 traffic lane L2 A1 3 D4 B14 B11 B24 D2 B1 A1 2 B21 D3 parking lane B2 parking place B13 B12 B22 B23 border of the parking lane ✛⑩⑧ ●❼ ❿➞ ③④ ✑q ❖➐ ❘➭ ❪➄ ⑩⑧ ➁② ❿➞ ❥⑤ ⑥② ⑩⑧ ●⑨ ➜ ✇ ❭⑤ ❚② ② ♠ ✰q ✍➍ ✴q ❊❼ ❸⑧ ❿✇ ❿✇ ❸⑧ ❹✇ ➢❼ ❭⑨ ✟➧ ❷⑤ ❿➅ ❊⑤ ⑥④ ❭⑤ ⑩➝ ❸➝ ✴q ❒➝... MARTIN Unit´e de recherche INRIA Rocquencourt, Domaine de Voluceau, Rocquencourt, BP 105, 78153 LE CHESNAY Cedex Unit´e de recherche INRIA Sophia-Antipolis, 2004 route des Lucioles, BP 93, 06902 SOPHIA-ANTIPOLIS Cedex ´ Editeur INRIA, Domaine de Voluceau, Rocquencourt, BP 105, 78153 LE CHESNAY Cedex (France) ✂✁✄✁✆☎✞✝ ✟✠✟✂✡☛✡☞✡✍✌✏✎✒✑✔✓✕✎ ✖✞✌ ✗✄✓ ISSN 0249-6399 ... ☎♣➧➩⑨❊④✫➧➫➞❿➝⑩➝✟❾❹q✴②➁⑤❥⑧❸➝❸❽♥⑤❊➍❖⑨●➞❹②♥② ♠ ⑧❻❽✔⑦✴⑨❊✇●②⑥④⑥⑨❭➝♣❽➁⑦ ♠ q✾r✙q◗Ù✪➥ ✝ ✝ ☛ ✗ ✙ ✗ ✙ ✠ ☛ ✩✙ ✂ ✌ ✄ ☛ ✝ ✄ ✝ ✙ ✂ ✌ ✶ ✌ ✌ ✝ ✠ ☎ ☛✚✙ ✠ ✓✠ ☛✛✠ ✝ ✠ ✙ ✗ ✶ dT n d(st) n sT nominal trajectory obstacle st traffic lane s0 traffic lane ✛⑧⑩❼●➞❿④⑥q✰➑❹➭ ✠ ✾q ✇❹q❑④⑥⑤❭②③⑧❸⑨❊✇✂⑨❥➧✚❽⑥r✙⑨❪⑨❭② ♠ ➝❸⑨❹⑦✴⑤❥➝♣②➁④⑥⑤❥➃①q✾⑦✴②③⑨❊④⑥⑧❸q✾❽➴➧➩⑨❊④♥⑤◗➡❊⑨❭⑧❻❾❹⑧❸✇❿❼✙⑤❊✇✂⑨●➍❿❽③②➁⑤❭⑦✴➝⑩q❊➥ ❁ ↔ à ✟↔ ❧♥♠ ⑧❸❽✡⑦✹⑨❊✇●②➁④③⑨❊➝✧❽⑥➠●⑧❸➝⑩➝➴⑧❸❽✙⑤❭➅❖➅❹➝⑩⑧❸q✾❾❝②⑥⑨❬q✹➯❹q❑⑦✴➞❹②⑥q❤⑤✝➝❻⑤❭✇❹q➜⑦... ②✙⑤❭✇❿❾✲⑨◆➡❭q✾④⑥②⑥⑤❊➠●⑧❸✇❹❼✍❺❚Ø❳⑦❑Ù✵➝❻⑤❭✇❹q ⑦ ♠ ⑤❭✇❹❼❊⑧❸✇❹❼✙②③⑨❷② ♠ q✑➝⑩q✴➧➩②✴❺✒Ø❳❾❬Ù✫⑦✴⑤❭②⑥⑦ ♠ ⑧❻✇❹❼➢➞❿➅➜➟♥⑧⑩② ♠ ② ♠ q✰✇❹⑨●r✙⑧❸✇❿⑤❭➝♣②⑥④➁⑤◆➃①q❑⑦✹②③⑨●④③➤❊➥ ❁ ✓✹ 0 -10 nominal trajectory y [m] 0.4 2.2 -20 2.1 -30 motion direction velocity [m/s] angle [rad] 0.2 0 2 1.9 1.8 -0.2 -40 1.7 local trajectory ✚ -0.4 -20 -10 0 x [m] 10 20 0 10 20 30 40 time [s] 50 60 70 ☛ 1.6 0 10 20 30 40 time [s] 50 60 ✛⑧❸❼❊➞❿④⑥q✩➎❭➎❊➭ ❬⑨❊②③⑧❸⑨❊✇❝⑤❭✇❖❾... ❽③②➁⑤❭④⑥②❆➝❸⑨❹⑦✴⑤❥②⑥⑧⑩⑨●✇♣❺▼Ø■⑦❑Ù❆➅❖q❑④①➧➩⑨●④⑥r✙⑧❻✇❹❼✮⑤ ➍❿❊⑤ ⑦✪➠●➟✫⑤❊④⑥❾ ➅❿⑤❊④⑥➠●⑧❻✇❹❼ r✙⑨❊②③⑧❸⑨❊✇☛✡ Ø■❾✍Ù✫⑦✹⑨●r❷➅❹➝⑩q✾②③⑧❻✇❹❼❷② ♠ q✰➅❖⑤❭④➁⑤❥➝❸➝⑩q✾➝❒➅❿⑤❭④➁➠●⑧❸✇❹❼✍➥ ❁ ✟✞ 4 0.6 0.4 0 0 -0.2 -2 end location ✚ 0.4 0.2 velocity [m/s] start location angle [rad] y [m] 2 -4 -10 -8 -6 -4 0 2 4 0 0 -0.2 -0.4 -0.6 -0.4 -2 0.2 10 x [m] 20 30 40 time [s] 50 ☛ 60 -0.8 0 10 20 30 40 time [s] 50 60 ✛⑧❸❼❊➞❿④⑥q➢➎✾s❖➭ ➊⑨❭②③⑧❸⑨❊✇✝⑤❊✇❿❾✩⑦✹⑨●✇❊②➁④③⑨❊➝☞⑦✹⑨●r❷r✙⑤❊✇❿❾❿❽☎⑧❸✇✝②... ☞☞☛✍✌✏✎✁✑✓✒✔✎✖✕✏✗✘✕✏✙✛✚✜☛✍✢✤✣✆✥✧✦✩★✛✒✤✎✖✙✆✦✩☛✪✢✔✣✆✥✖✚✫✑✆✢✤✣✆✦✬✕✏✙✛✚✬✌✏✕ ▼✤✦✪✚✁✣✮✭✯✢✥▼✤☛✰✑✆✕✏✎✾▼✘✎✁✭✱✕ ✑✆✙✲✚✁✣✆☛✳✌✏✦ ✤ ▼ ✴✁✑✓✒✶✵ ▲✦▲◆▼❖✢◗✑❘✾❘✾❙ ➎◗➉ ✟➾◆➘❭×❭➬➫➵✹➻✁ ✄✂❒➻⑥➾❥➬➫Ó ❖➾◆➻⑥➪☎ ✝✆✔➾◆➻✪➸✍➬➫➵✹➻✁ ✄✞✔➾◆➻⑥➺❥➶✡Ñ❃Ó ❪➬⑩Ü➜➾◆➸➨➪ Ó ❖➵✹➘❖➵✴➻ ✠ ☞✞ ☞✞ ✟✞ moving obstacles s search graph t trajectory ✛⑧⑩❼●➞❿④③q✰➈❿➭ ➛✇✝q✹➯❿⑤❭r❷➅❹➝❸q❱⑨❥➧✟➡❭q✴➝❸⑨❹⑦✹⑧❸②❃➤✂➅❹➝❻⑤❭✇❿✇❿⑧❸✇❹❼✍➥ ❁ ✷ ② ♠ q➢❾❹➤❹✇❿⑤❭r✙⑧❻⑦✡⑦✴⑨❊✇❿❽⑥②⑥④➁⑤❥⑧❻✇●②⑥❽➛⑨❥➧✫② ♠ q❷➡❭q ♠ ⑧❻⑦✹➝❸q✙⑤❊✇❿❾❬➤❪⑧❸q✴➝❻❾❿❽✑✇❹⑨✝⑦✹⑨❭➝❸➝❸⑧❸❽⑥⑧⑩⑨●✇➊➍✍q✴②❃➟✫q✴q❑✇❬② ... detected Free-lane description Local map building + FAILURE yes Lane changing FAILURE Reaching an appropriate start location Free-lane reached Free-space parameters Trajectory tracking Generation-execution... of appropriate Local Trajectory backward & forward motions no Updating no Success ? local map Obstacle overtaken ? yes Nominal trajectory to reach yes Lane changing SUCCESS yes Nominal trajectory... Parallel Parking Trajectory Following Nominal Trajectory yes Goal reached ? Trajectory tracking obstacle detection no no Obstacle avoidance possible ? Find a parking place Free parking place