path planning and execution for real worldssungeui/mp_tutorial/shim.pdf · 2014-06-22 · execution...
Post on 29-Jul-2020
0 Views
Preview:
TRANSCRIPT
KIDCS C-FRIEND Field Robotic Systems Center
Path Planning and Execution for Real Worlds
심현철
1
KIDCS C-FRIEND Field Robotic Systems Center
경로 및 운동계획법
• 경로 및 운동 계획
로보틱스에서 가장 중요한 분야들 중 하나
복잡도와 구속환경 : 완벽한 계획법을 저해하는 요소
최적의 경로를 가능한 빨리 찾아내는 기술
• 관련 분야
로봇의 조작 기술 및 계획법
Nonholonomy 및 동역학 방정식 고려
위협 감지 및 회피 계획
최적의 경로 탐색
시간에 따라 변하거나 움직이는 장애물 회피
감지 및 예측에 따른 불확실성 파악
2
KIDCS C-FRIEND Field Robotic Systems Center
Path Planning
Visibility Graph
Voronoi Diagram
Roadmap
3
Cell Decomposition
Trapezoidal Decomposition
Octree Decomposition
Potential Field
Rapidly-Exploring
Random Tree
KIDCS C-FRIEND Field Robotic Systems Center
Real Time Path Planning
4
Visibility Graph
Voronoi Diagram
Roadmap Cell Decomposition
Trapezoidal Decomposition
Octree Decomposition
Potential Field
Rapidly-Exploring
Random Tree
경로생성결과를 예측 가능
환경의복잡도에따라계산량증가
실시간 사용이 가능하도록 수정
지정된 구역이 아닌Open Field (실제환경)에사용하기 적합하지 않음
Local Minima에 빠져Solution을 구하지
못할 수 있음
실시간 경로 계획에사용하기 위해 계산 시간을줄이면 동일한 조건에도매번 다른 결과를 도출
개발자(탑승자)로 하여금불안감 조성
KIDCS C-FRIEND Field Robotic Systems Center
One Voronoi Cell Algorithm
5
One Voronoi Cell Algorithm Voronoi diagram의 정의를 이용하여 장애물 사이의
최대 Clearance를 보장
경로상의 충돌하는 한개의 voronoi core 만을 이용하여 계산
실시간 장애물 회피 경로 생성 가능
장애물 간의 간격이 큰 경우 크게 회피 하는 것을방지하지 위해 Voronoi cell의 크기를 제한시켜
최소한의 기동으로 장애물 회피
동일한 장애물 환경에서 동일한 결과 도출
-20
-15
-10
-50
510
15
20
10
15
20
25
30
35
40
45
-20
-15
-10
-50
510
15
20
10
15
20
25
30
35
40
45
-20
-15
-10
-50
510
15
20
10
15
20
25
30
35
40
45
-20
-15
-10
-50
510
15
20
10
15
20
25
30
35
40
45
-20
-15
-10
-50
510
15
20
10
15
20
25
30
35
40
45
-20
-15
-10
-50
510
15
20
10
15
20
25
30
35
40
45
-20
-15
-10
-50
510
15
20
10
15
20
25
30
35
40
45
-20
-15
-10
-50
510
15
20
10
15
20
25
30
35
40
45
-20
-15
-10
-50
510
15
20
10
15
20
25
30
35
40
45
-20
-15
-10
-50
510
15
20
10
15
20
25
30
35
40
45
-20
-15
-10
-50
510
15
20
10
15
20
25
30
35
40
45
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 20
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8
2
Original Path
Re-planned Path
Collision Point
Original Path
OBS Point
Nestest Point
Bisector
Path node
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 20
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8
2
Original Path
Re-planned Path
Collision Point
Original Path
OBS Point
Nestest Point
Bisector
Path node
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 20
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8
2
Original Path
Re-planned Path
Collision Point
Original Path
OBS Point
Nestest Point
Bisector
Path node
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 20
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8
2
Nearest Point
Obs. Point
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 20
0.2
0.4
0.6
0.8
1
1.2
1.4
1.6
1.8
2
Nearest Point
Obs. Point One Voronoi Cell Algorithm ( Xobs, Xc )
n ←0
do
n ← n + 1
Xnear(n) ← GetNearestPoint( Xobs, Xc )
(E(n), Xavoid(n), tavoid(n))←GetVoronoiEdge(Xnear, Xc)
Xobs ← RemovePointSet( Xobs, E(n) )
while ( Xobs ≠ Null )
return ( Xavoid, tavoid )
KIDCS C-FRIEND Field Robotic Systems Center
Path Generation
6
One Voronoi Cell Algorithm 으로부터구한
Avoidance points 후보들로부터 차량이 주행 가능한 경로 생성
Control Point
Avoidance Point
Avoidance Point
OBS.
Return Point
Path Generation
경로선정조건- 차량이 주행 가능한 경로중조건 1. 충돌하지 않는 가장 짧은 경로를 선택조건 2. 충돌하기까지 가장 긴 경로를 선택
- 차량이 주행 가능한 경로가 없는 경우 충돌 전까지 기존 경로를 따라 주행
(경로 생성이 안된 경우를 대비) 충돌 전까지 주행해도 경로 생성이 안되는 경우
후진 하면서 경로 생성
No
Delete from Candidates SET
Path Candidates SET
Shortest Collision Free Path
No
Is Exist?
Nonholonomicconstraint
Longest Path before collision
Path Result
Is Exist?Terminate Sequence
KIDCS C-FRIEND Field Robotic Systems Center
모든 경로는 Path Segment의 Array로 구성
각각의 Path Segment는 3차 방정식으로 이루워짐.
이점 :
1. 3차방정식으로 구성
양끝의 경계조건(위치, 기울기) 만으로 생성 가능
Coefficients를 제한하여 Nonholonomic 조건부합 가능
2. 매개변수를 이용한 표현법 보다 직관적임
3. WPT를 이용한 경로 관리보다 효율적임
- 경로의 연속성으로 제어 및 장애물 검사가 연속적임
- 데이터 저장의 효율성 증대
- 경로 수정 용의
Path Representation
7
Path Representation
𝑷𝒊 = 𝑥𝒊𝐸𝑁𝑈 , 𝑦𝒊
𝐸𝑁𝑈 , ℎ𝒊𝐸𝑁𝑈 , 𝑎𝒊, 𝑏𝒊, 𝑥_𝑒𝑛𝑑𝒊
𝑻
𝒚𝒑𝒂𝒕𝒉 = 𝑎𝒊(𝒙𝒑𝒂𝒕𝒉)𝟑
+𝑏𝒊(𝒙𝒑𝒂𝒕𝒉)𝟐
𝒚𝒊𝒑𝒂𝒕𝒉
𝒙𝒊𝒑𝒂𝒕𝒉
𝒚𝒊+𝟏𝒑𝒂𝒕𝒉
𝒙𝒊+𝟏𝒑𝒂𝒕𝒉
𝑥𝑖𝐸𝑁𝑈 , 𝑦𝑖
𝐸𝑁𝑈
ℎ𝑖𝐸𝑁𝑈
𝑎𝑖 , 𝑏𝑖𝑥_𝑒𝑛𝑑𝑖
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.
KIDCS C-FRIEND Field Robotic Systems Center
Collision Detection
8
Collision Detection
47.5
-16.2
-10.0
0.0
10.0
20.0
30.0
40.0
East
34.2
-34.2
-20.0
-10.0
0.0
10.0
20.0
30.0
park
_wp
t
OB
S
ET
C
car
Co
l Po
int
x_p
re
x_re
v
x_cu
r
x_g
ps
x_p
ark
pre
path
path
path
_orig
in
road
_bo
un
d
park
_bo
un
d
1. 경로위장애물검출- 주어진경로위의장애물검출
3차방정식으로이루어진경로를좌우로벌려충돌감지
이점 : 단시간내에주행가능영역을판별경로를이용하여연속적인판별가능
단점 : 차량위치와경로사이의거리오차가큰경우
2. 주행경로위장애물검출- 차량위치와경로사이의거리오차가큰경우차량의현재속도와 Steering 각을이용해차량의이동궤적을예측이동궤적상의장애물판별
KIDCS C-FRIEND Field Robotic Systems Center
Path Planning Sequence & Simulation Result
9
-80 -70 -60 -50 -40 -30 -20 -10 0 10 2090
95
100
105
110
115
120
Path Planning Sequence Simulation Result Case 1
Local minima
KIDCS C-FRIEND Field Robotic Systems Center
Simulation Result
10
-60 -50 -40 -30 -20 -10 0 10
-55
-50
-45
-40
Simulation Result Case 2
1
2
3
4
5
시뮬레이션을 통한 알고리즘 검증- 실시간으로 경로 계획 가능- 동일한 상황에서 반복적인 결과를 줌- 복잡한 장애물 환경에서 최대한의clearance 를 확보 하며 회피
KIDCS C-FRIEND Field Robotic Systems Center
자율 주행 차량 개발
EureCar
최소한의 센서와 저가의 GPS를 이용한 자율 주행 시스템의 센서 구성
EURECAR TURBO - System Configuration
EureCar Turbo 주요 기능
• Path Tracking - Sensor Fusion & GPSsingle GPS + Vehicle ECU(Speed, Yaw rate) Position, HeadingLaser Scanner Road Boundary Detection, Obstacles Camera Road Detection
• Collision avoidance Voronoi cell 기반의 실시간 장애물 회피 알고리즘 탑재
• 신호등 횡단보도 보행자 인식/ 풀 시퀀스 직각 자동 주차
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
11
KIDCS C-FRIEND Field Robotic Systems Center
Development of Self-driving car
12
KIDCS C-FRIEND Field Robotic Systems Center
차량 개조 작업
Brake & ThrottleControl Actuator
Batteries & InverterGenerator 추가 Actuator Controller
비상 정지 스위치 Low Cost GPS 360º Lidar
13
KIDCS C-FRIEND Field Robotic Systems Center
14
Velodyne HDL-32EHorizontal angle : 360degVertical angle : -30~10degRange : ~70m
KIDCS C-FRIEND Field Robotic Systems Center
System Configurations
Local Trajectory
Re-planning
Driving Status Checkhigh-level decision making
Path tracker
New
Path
Vehicular Status
Current path
Collision Point
Vehicular Status
Path
Vehicle Control
Yaw rate, Wheel Speed
Car Built in Senor, CAN
Navigation
Position, Heading,
Vehicle Info.
Absolute location and heading
Speed, Steering angle
Steering Speed
command
Path Planner
Vehicle Controller
Sensor Interface Situation Awareness
Unified Map
Parking Line
Detection
Rear Down Camera
Ethernet
Front Camera
Ethernet
Crosswalk, Traffic Lights
Pedestrian, Toll Bar
Detection
Actuator
Throttle Pedal
Control Motor
Brake Pedal
Control Motor
Steering Wheel
CAN
Shift Lever
Control Motor
Velodyne HDL-32e
Ethernet
Obstacle DetectionCar, cones
Road Blocks, Toll Bar
Laser Data ProcessingApple Mac mini
C++
Navigation & ControlNI CompactRIO
Path PlannerApple Mac mini
Labview
Vision ProcessingDesktop PC
C++
GPS (Ublox)
USB
Remote On/Off
Emergency Stop
Alarm (Audio/Visual)
Human Interface
Auto/Manual Toggle SW.
15
KIDCS C-FRIEND Field Robotic Systems Center
The experiment result
25.3
-42.4
-40.0
-37.5
-35.0
-32.5
-30.0
-27.5
-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
X[m]
59.5-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
Tempal STOP
GPS/INS
Path_origin
Obstacles
27.0
-41.0
-37.5
-35.0
-32.5
-30.0
-27.5
-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
X[m]
56.2-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
Tempal STOP
GPS/INS
Path_origin
Obstacles
32.4
-49.6
-45.0
-40.0
-35.0
-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
X[m]
67.7-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
Tempal STOP
GPS/INS
Path_origin
Obstacles
33.3
-49.6
-45.0
-40.0
-35.0
-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
X[m]
69.1-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
Tempal STOP
GPS/INS
Path_origin
Obstacles
33.3
-49.5
-45.0
-40.0
-35.0
-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
X[m]
68.1-14.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
Tempal STOP
GPS/INS
Path_origin
Obstacles
28.0
-46.6
-42.5
-40.0
-37.5
-35.0
-32.5
-30.0
-27.5
-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
X[m]
63.3-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
Tempal STOP
GPS/INS
Path_origin
Obstacles
- Obstacle avoidance result – 다수의 Case에 관하여 알고리즘의 반복성 테스트
Case I
Crosswalk Obstacle Vehicle track Original path
Case II Case III
Case VICase IV Case V
16
KIDCS C-FRIEND Field Robotic Systems Center
Parking Lot
Number SignParking Area
(Finish Point)
Start
Crosswalk
Pedestrian Detection
Crossbar
Detour
“Hyundai Challenge” Competition-at-a-glance
Mission Result
Crosswalk detection Success
Traffic light Success
Complex obstaclesSuccess(zero cone)
Pedestrian Detection Failure
DetourSuccess(zero cone)
Passing slow vehicle Success
Pop-up obstacle Success
Crossbar Detection Success
ParkingSuccess(rearward)
Speed [km/h]
Ave. 22.43
Max. 51.75
time 9min 07sec
17
KIDCS C-FRIEND Field Robotic Systems Center
Path Planning Result In Competition
130 140 150 160 170 180 190 200 210
290
300
310
320
330
340
350
360
0 5 10 15 20 25 30 35 40
10
15
20
25
30
35
40
45
먼지Original Path
GPS Pos.
OBS
-80 -60 -40 -20 0 20 40 60 80
240
260
280
300
320
340
360
380
Original Path
GPS Pos.
OBS
Car
Original Path
GPS Pos.
OBS
Original Path
Car GPS Pos.
OBS
-210 -200 -190 -180 -170 -160 -150 -140
20
30
40
50
60
70
80
Original Path
GPS Pos.
OBS(Traffic Light)
X[m]
Y[m]
X[m]
Y[m]
dust
Passing slow vehicle
Complex ObstaclesTraffic Light
Detour
18
- 대회에서의 장애물 회피 결과 – 다양한 미션에 관하여 완결성 있는 결과 도출
KIDCS C-FRIEND Field Robotic Systems Center
Competition Video
19
USRGTube https://www.youtube.com/user/USRGTube
Speed [km/h]
Ave. 22.43
Max. 51.75
time 0:09:07
KIDCS C-FRIEND Field Robotic Systems Center
Conclusion and Future Work
자율 주행 차량 개발 사항• Single GPS와 Odometer를 이용한 주행• 캠퍼스 내의 완전 자율 주행• 장애물 회피 주행• 고속 주행
차후 Application• 무인택시, 교내 셔틀• 다양한 실제 도로 환경에서 자율 주행
top related