PRM-based Trajectory Planningfor Underactuated Vehicles

in 3-D Space

Dongkyu ChoiDongkyu ChoiJinwhan KimJinwhan Kim

CS326a Motion PlanningCS326a Motion Planning Professor J-C. LatombeProfessor J-C. Latombe

Aircraft Dynamics Model

• 6 DOF (3 positions & 3 orientations)• 12 states (x, u, z, u, v, w, , , , p, q, r)• 2 inputs (E, R)

x

y

z

E

R

Equations of Motion in 3-D Space

r

q

p

CS

SC

CS

secsec0

0

tantan1

EGRAVwu EZZwZuZqupvwm )(

GRAVwu XwXuXrvqwum )(

RGRAVv RYYvYpwruvm )(

RArpvyzx RAKKKKKqrIIpI )(

Eqwuzxy EMqMwMuMrpIIqI )(

Rrpvxyz RNrNpNvNpqIIrI )(

w

v

u

CCCSS

SSSSCCCSSSSC

SSCSCSCCSSCC

z

y

x

Stability Adjustment• Twin-piston engined general aviation aircraft

Vcruise = 250 kmLength ≈ 10 m

• Poles of the longitudinal modes

• Poles of the lateral modes

-4.4110 4.3150 j-0.0100 0.0401 j

-4.4110 4.3150 j-0.0310 0.0273 j

uX

-0.1792 2.0316 j-2.5724 0.0017

-1.7120 1.1595 j 0.0000 0.0000

rNK ,*

PRM based Path-Planning• Pseudo-code

(D. Hsu, R. Kindel, J.C. Latombe, and S. Rock , 2002 )

Insert an initial milestone into T(Milestone set)Repeat

Pick a milestone m from T with a probabilityPick a control function u from U with a probabilityPick a time duration td for propagationm’ = Propagate(m,u,td)if a path from m to m’ is collision-free

Add m’ into TIf m’ Endgame region

exit with successif I=N exit with failure

Milestone Scatter PlotR=50° , E=50° , w/o Speed Constr. R=30° , E=20° , w/o Speed Constr.

R=30° , E=10° , w/ Speed Constr.

Homing Guidance and Control

• Assume (t) = 0– This can be accomplished by

controlling ailerons.– This makes it possible to

decouple 6-DOF dynamics into vertical and horizontal dynamics.

• Vertical-mode PD controller

• Horizontal-mode PD

controller

b

x

y

bVDVbVPVE KK

bHDHbHPHR KK

bBearing Angle

Modified PRM based Path-Planning

• Pseudo-code

Insert an initial milestone into T(Milestone set)Repeat

Pick a milestone m from T with a probabilityPick a control function u from U with a probabilityPick a time duration td for propagationm’ = Propagate(m,u,td)if a path from m to m’ is collision-free

Add m’ into TIf m’ Endgame region

exit with success// Apply homing control u// Apply homing control uhh(t) from m’ to the goal(t) from m’ to the goalIf (m’ = Propagate(m, uIf (m’ = Propagate(m, uhh(t),t(t),tdd<t<tlimlim)) )) Endgame region Endgame region

exit with successexit with successif i=N exit with failure

Example: Planned Path

Example: Planned Path(cont’d)

Performance Statistics

Avg Std Avg Std Avg Std Avg Std Avg Std Avg Std

Random 100% 525 128 0.229 0.053 136.42 2.12 100% 1 0 < 0.01 0.000 135.60 0.00

Gaussian 100% 360 103 0.164 0.041 136.21 1.48 100% 1 0 < 0.01 0.000 135.60 0.00

Random 90% 1103 359 0.516 0.163 139.41 2.48 100% 40 11 0.207 0.054 138.97 1.35

Gaussian 43% 6382 4107 2.946 1.886 141.25 2.43 100% 74 29 0.331 0.122 138.27 0.48

Random 100% 964 209 0.746 0.122 100.63 7.53 100% 1 0 < 0.01 0.000 90.00 0.00

Gaussian 100% 449 120 0.404 0.079 97.61 7.44 100% 1 0 < 0.01 0.000 90.00 0.00

Random 56% 6171 2356 4.778 1.566 117.73 18.75 100% 355 71 0.917 0.190 108.53 32.68

Gaussian 32% 9398 6213 7.096 4.037 127.62 14.31 100% 503 167 1.276 0.417 103.10 26.22

ControlDistribution

Basic PRM

2-D Configuration

Space

( Goal Area /Entire Area

= 0.2% )

3-D Configuration

Space

( Goal Volume / Entire Volume

= 0.04% )

Obstacle-Free

5 MovingObstacles

Obstacle-Free

7 MovingObstacles

PRM with Homing Guidance

SuccessRate

# of Milestones

Run_Time(sec)

Time-To-Go(sec)

SuccessRate

# of Milestones

Run_Time(sec)

Time-To-Go(sec)

Note : All the numbers are calculated using the data of best 30 runs from 100 runs.