Path Planning and Execution for Real Worlds 심현철 1
경로및운동계획법 경로및운동계획 로보틱스에서가장중요한분야들중하나복잡도와구속환경 : 완벽한계획법을저해하는요소최적의경로를가능한빨리찾아내는기술 관련분야 로봇의조작기술및계획법 Nonholonomy 및동역학방정식고려위협감지및회피계획최적의경로탐색시간에따라변하거나움직이는장애물회피감지및예측에따른불확실성파악 2
Path Planning Roadmap Visibility Graph Cell Decomposition Trapezoidal Decomposition Potential Field Voronoi Diagram Octree Decomposition Rapidly-Exploring Random Tree 3
Real Time Path Planning Roadmap Visibility Graph 경로생성결과를예측가능 환경의복잡도에따라계산량증가 실시간사용이가능하도록수정 Voronoi Diagram Cell Decomposition Trapezoidal Decomposition 지정된구역이아닌 Open Field ( 실제환경 ) 에사용하기 Octree Decomposition 적합하지않음 Potential Field Local Minima 에빠져 Solution 을구하지못할수있음 Rapidly-Exploring Random Tree 실시간경로계획에사용하기위해계산시간을줄이면동일한조건에도매번다른결과를도출 개발자 ( 탑승자 ) 로하여금불안감조성 4
One Voronoi Cell Algorithm One Voronoi Cell Algorithm Voronoi diagram 의정의를이용하여장애물사이의최대 Clearance 를보장 경로상의충돌하는한개의 voronoi core 만을이용하여계산실시간장애물회피경로생성가능 2.8.6.4.2 1.8.6.4-20 -15-10 -5 0 5 10 15 20 10 15 Original Path Re-planned Path Collision Point Original Path OBS Nearest Point Point Nestest Obs. Point Point Bisector Path node.2 0.6 0.8 1 1.2 1.4 1.6 1.8 2 20 1.2 1.4 1.6 1.8 2 0 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 0.6 0.8 1 1.2 1.4 1.6 1.8 2 25 30 35 40 45 장애물간의간격이큰경우크게회피하는것을방지하지위해 Voronoi cell 의크기를제한시켜최소한의기동으로장애물회피 Original Path Re-planned Path Collision Point Original Path Original Path Nearest OBS Point Point Re-planned Path Obs. Nestest Point Point Collision Point One Voronoi Cell Algorithm ( X obs, X c ) Bisector Original Path Path node OBS Point Nestest Point n 0 Bisector do Path node n n + 1 동일한장애물환경에서동일한결과도출 X near (n) GetNearestPoint( X obs, X c ) (E(n), X avoid (n), t avoid (n)) GetVoronoiEdge(X near, X c ) X obs RemovePointSet( X obs, E(n) ) while ( X obs Null ) return ( X avoid, t avoid ) 5
Path Generation Path Generation One Voronoi Cell Algorithm 으로부터구한 Avoidance points 후보들로부터차량이주행가능한경로생성 Avoidance Point Return Point Path Candidates SET Nonholonomic constraint No Delete from Candidates SET Control Point OBS. Avoidance Point Is Exist? Shortest Collision Free Path Terminate Sequence 경로선정조건 - 차량이주행가능한경로중조건 1. 충돌하지않는가장짧은경로를선택조건 2. 충돌하기까지가장긴경로를선택 Is Exist? Path Result No Longest Path before collision - 차량이주행가능한경로가없는경우 충돌전까지기존경로를따라주행 ( 경로생성이안된경우를대비 ) 충돌전까지주행해도경로생성이안되는경우후진하면서경로생성 6
Path Representation Path Representation 모든경로는 Path Segment 의 Array 로구성 각각의 Path Segment 는 3 차방정식으로이루워짐. 이점 : 1. 3 차방정식으로구성 양끝의경계조건 ( 위치, 기울기 ) 만으로생성가능 Coefficients 를제한하여 Nonholonomic 조건부합가능 2. 매개변수를이용한표현법보다직관적임 3. WPT 를이용한경로관리보다효율적임 - 경로의연속성으로제어및장애물검사가연속적임 - 데이터저장의효율성증대 - 경로수정용의 y i path P i = x i ENU, y i ENU, h i ENU, a i, b i, x_end i x i ENU, y i ENU y path = a i (x path ) 3 +b i (x path ) 2 h i ENU a i, b i x_end i path y i+1 x i path path x i+1 Path Segment Start Point s Pos. in ENU Frame. Path Segment Start Point s Heading in ENU Frame. Cubic Function Coefficients. Path Segment End Point s x value in Path Frame. T 7
Collision Detection KIDCS C-FRIEND Field Robotic Systems Center 1. 경로위장애물검출 - 주어진경로위의장애물검출 3 차방정식으로이루어진경로를좌우로벌려충돌감지 Collision Detection 이점 : 단시간내에주행가능영역을판별경로를이용하여연속적인판별가능 단점 : 차량위치와경로사이의거리오차가큰경우 2. 주행경로위장애물검출 - 차량위치와경로사이의거리오차가큰경우 East 차량의현재속도와 Steering 각을이용해차량의이동궤적을예측이동궤적상의장애물판별 -34.2-20.0-10.0 0.0 10.0 20.0 30.0 34.2 8
Path Planning Sequence & Simulation Result Path Planning Sequence Simulation Result Case 1 120 115 110 105 100 95 90-80 -70-60 -50-40 -30-20 -10 0 10 20 Local minim 9
Simulation Result Case 2 KIDCS C-FRIEND Field Robotic Systems Center Simulation Result -40-45 1 4-50 -55-60 -50-40 -30-20 -10 0 10 2 5 3 시뮬레이션을통한알고리즘검증 - 실시간으로경로계획가능 - 동일한상황에서반복적인결과를줌 - 복잡한장애물환경에서최대한의 clearance 를확보하며회피 10
EURECAR TURBO - System Configuration 자율주행차량개발 최소한의센서와저가의 GPS 를이용한자율주행시스템의센서구성 EureCar EureCar Turbo 센서및피씨구성 single GPS U-blox EVK-6T ( 자율주행용 ) DGPS Novatel OEMV-2 ( 항법비교용 ) Laser Scanner Velodyne HDL-32E Nav. & Controller NI CompactRio Path Planner PC Apple Mac mini Vision PC Apple Mac mini Obs. Processing PC CompuLab Intense PC EureCar Turbo 주요기능 Path Tracking - Sensor Fusion & GPS single GPS + Vehicle ECU(Speed, Yaw rate) Position, Heading Laser Scanner Road Boundary Detection, Obstacles Camera Road Detection Collision avoidance Voronoi cell 기반의실시간장애물회피알고리즘탑재 신호등횡단보도보행자인식 / 풀시퀀스직각자동주차 11
Development of Self-driving car 12
차량개조작업 Generator 추가 Brake & Throttle Control Actuator Batteries & Inverter Actuator Controller 비상정지스위치 Low Cost GPS 360º Lidar 13
Velodyne HDL-32E Horizontal angle : 360deg Vertical angle : -30~10deg Range : ~70m 14
System Configurations Sensor Interface Situation Awareness Path Planner Actuator Front Camera Ethernet Crosswalk, Traffic Lights Pedestrian, Toll Bar Detection New Path Local Trajectory Re-planning Vehicular Status Current path Collision Point Brake Pedal Control Motor Throttle Pedal Control Motor Rear Down Camera Ethernet Parking Line Detection Driving Status Check high-level decision making Steering Wheel CAN Vehicular Status Path Shift Lever Control Motor Velodyne HDL-32e Ethernet Obstacle Detection Car, cones Road Blocks, Toll Bar Path tracker Human Interface Steering Speed command Auto/Manual Toggle SW. Unified Map Vehicle Controller Remote On/Off Position, Heading, Vehicle Info. Vehicle Control Emergency Stop Alarm (Audio/Visual) GPS (Ublox) USB Yaw rate, Wheel Speed Car Built in Senor, CAN Navigation Absolute location and heading Speed, Steering angle Vision Processing Desktop PC C++ Laser Data Processing Apple Mac mini C++ Path Planner Apple Mac mini Labview Navigation & Control NI CompactRIO 15
The experiment result - Obstacle avoidance result 다수의 Case 에관하여알고리즘의반복성테스트 25.3 27.0 33.3 22.5 20.0 17.5 15.0 12.5 10.0 7.5 5.0 2.5 0.0-2.5-5.0-7.5-10.0-12.5-15.0-17.5-20.0-22.5-25.0-27.5-30.0-32.5-35.0-37.5-40.0 Case I Obstacles Tempal STOP GPS/INS Path_origin 25.0 22.5 20.0 17.5 15.0 12.5 10.0 7.5 5.0 2.5 0.0-2.5-5.0-7.5-10.0-12.5-15.0-17.5-20.0-22.5-25.0-27.5-30.0-32.5-35.0-37.5 Case II Obstacles Tempal STOP GPS/INS Path_origin 30.0 25.0 20.0 15.0 10.0 5.0 0.0-5.0-10.0-15.0-20.0-25.0-30.0-35.0-40.0-45.0 Case III Obstacles Tempal STOP GPS/INS Path_origin -42.4-8.0-5.0 0.0 5.0 10.0 15.0 20.0 25.0 30.0 35.0 40.0 45.0 50.0 55.0 X[m] 59.5-41.0-11.2-5.0 0.0 5.0 10.0 15.0 20.0 25.0 30.0 35.0 40.0 45.0 50.0 X[m] 56.2-49.6-13.9-10.0-5.0 0.0 5.0 10.0 15.0 20.0 25.0 30.0 35.0 40.0 45.0 50.0 55.0 60.0 65.0 69.1 X[m] 32.4 33.3 28.0 30.0 30.0 25.0 25.0 25.0 22.5 20.0 20.0 20.0 17.5 15.0 15.0 10.0 5.0 Obstacles Tempal STOP GPS/INS 15.0 10.0 5.0 Obstacles Tempal STOP GPS/INS 12.5 10.0 7.5 5.0 2.5 Obstacles Tempal STOP GPS/INS 0.0 Path_origin 0.0 Path_origin 0.0-2.5 Path_origin -5.0-5.0-5.0-7.5-10.0-10.0-10.0-12.5-15.0-15.0-15.0-17.5-20.0-20.0-20.0-25.0-25.0-22.5-25.0-30.0-30.0-27.5-30.0-35.0-40.0-45.0 Case IV -35.0-40.0-45.0 Case V -32.5-35.0-37.5-40.0-42.5 Case VI -49.6-49.5-14.9-14.4-10.0-5.0 0.0 5.0 10.0 15.0 20.0 25.0 30.0 35.0 40.0 45.0 50.0 55.0 60.0 65.0 67.7-10.0-5.0 0.0 5.0 10.0 15.0 20.0 25.0 30.0 35.0 40.0 45.0 50.0 55.0 60.0 65.0 68.1 X[m] Crosswalk Obstacle Vehicle track Original path X[m] -46.6-10.9-5.0 0.0 5.0 10.0 15.0 20.0 25.0 30.0 35.0 40.0 45.0 50.0 55.0 60.0 63.3 X[m] 16
Hyundai Challenge Competition-at-a-glance Detour Pedestrian Detection Crosswalk Mission Crosswalk detection Traffic light Complex obstacles Pedestrian Detection Detour Passing slow vehicle Pop-up obstacle Crossbar Detection Parking Result Success Success Success (zero cone) Failure Success (zero cone) Success Success Success Success (rearward) Crossbar Parking Lot Number Sign Start Parking Area (Finish Point) Speed [km/h] Ave. 22.43 Max. 51.75 time 9min 07sec 17
Path Planning Result In Competition - 대회에서의장애물회피결과 다양한미션에관하여완결성있는결과도출 380 360 340 Passing slow vehicle Original Path GPS Pos. OBS Car 360 350 340 Original Path GPS Pos. OBS Detour Y[m] 320 330 300 320 280 310 260 300 240-80 -60-40 -20 0 20 40 60 80 290 130 140 150 160 170 180 190 200 210 80 70 Original Path GPS Car GPS Pos. Pos. OBS(Traffic Light) 45 40 60 35 Y[m] 30 50 Traffic Light 40 30 20-210 -200-190 -180-170 -160-150 -140 25 20 15 10 먼지 dust Original Path GPS Pos. OBS 0 5 10 15 20 25 30 35 40 Complex Obstacles X[m] X[m] 18
Competition Video USRGTube https://www.youtube.com/user/usrgtube Speed [km/h] Ave. 22.43 Max. 51.75 time 0:09:07 19
자율주행차량개발사항 Single GPS 와 Odometer 를이용한주행 캠퍼스내의완전자율주행 장애물회피주행 고속주행 차후 Application 무인택시, 교내셔틀 다양한실제도로환경에서자율주행 Conclusion and Future Work