path planning and execution for real worldssungeui/mp_tutorial/shim.pdf · 2014-06-22 · execution...

KIDCS C-FRIEND Field Robotic Systems Center

Path Planning and Execution for Real Worlds

심현철

1


경로 및 운동계획법

• 경로 및 운동 계획

로보틱스에서 가장 중요한 분야들 중 하나

복잡도와 구속환경 : 완벽한 계획법을 저해하는 요소

최적의 경로를 가능한 빨리 찾아내는 기술

• 관련 분야

로봇의 조작 기술 및 계획법

Nonholonomy 및 동역학 방정식 고려

위협 감지 및 회피 계획

최적의 경로 탐색

시간에 따라 변하거나 움직이는 장애물 회피

감지 및 예측에 따른 불확실성 파악

2


Path Planning

Visibility Graph

Voronoi Diagram

Roadmap

3

Cell Decomposition

Trapezoidal Decomposition

Octree Decomposition

Potential Field

Rapidly-Exploring

Random Tree


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을 구하지

못할 수 있음

실시간 경로 계획에사용하기 위해 계산 시간을줄이면 동일한 조건에도매번 다른 결과를 도출

개발자(탑승자)로 하여금불안감 조성


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 )


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


모든 경로는 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.


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 각을이용해차량의이동궤적을예측이동궤적상의장애물판별


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


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 를 확보 하며 회피


자율 주행 차량 개발

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


Development of Self-driving car

12


차량 개조 작업

Brake & ThrottleControl Actuator

Batteries & InverterGenerator 추가 Actuator Controller

비상 정지 스위치 Low Cost GPS 360º Lidar

13


14

Velodyne HDL-32EHorizontal angle : 360degVertical angle : -30~10degRange : ~70m


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


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


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


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

- 대회에서의 장애물 회피 결과 – 다양한 미션에 관하여 완결성 있는 결과 도출


Competition Video

19

USRGTube https://www.youtube.com/user/USRGTube

Speed [km/h]

Ave. 22.43

Max. 51.75

time 0:09:07

https://www.youtube.com/user/USRGTube


Conclusion and Future Work

자율 주행 차량 개발 사항• Single GPS와 Odometer를 이용한 주행• 캠퍼스 내의 완전 자율 주행• 장애물 회피 주행• 고속 주행

차후 Application• 무인택시, 교내 셔틀• 다양한 실제 도로 환경에서 자율 주행

path planning and execution for real worldssungeui/mp_tutorial/shim.pdf · 2014-06-22 · execution...

Documents