Smooth Path Planning for Cars Thierry Fraichard Dubins/Reeds & - - PowerPoint PPT Presentation

Smooth Path Planning for Cars

Thierry Fraichard

Dubins/Reeds & Shepp Car Kinematics

w ϕ θ ρ y x Perfect rolling Bounded steering angle ω θ θ θ ⎟ ⎟ ⎟ ⎠ ⎞ ⎜ ⎜ ⎜ ⎝ ⎛ + ⎟ ⎟ ⎟ ⎠ ⎞ ⎜ ⎜ ⎜ ⎝ ⎛ = ⎟ ⎟ ⎟ ⎠ ⎞ ⎜ ⎜ ⎜ ⎝ ⎛ 1 sin cos v y x & & & ⎩ ⎨ ⎧ ≥ = − =

min

cos sin ) , , ( ρ ρ θ θ θ y x y x q & & curve

• Path

tan 1 xy x y ≡ ⇒ =

−

& & θ

“Dubins / Reeds & Shepp” Paths

Shortest paths: circular arcs + segments Dubins car: 2 families of paths [Dubins, 57] (i)(ii) CSC or CCC Reeds & Shepp car: 9 families of paths [Reeds & Shepp, 90; Souères & Boissonnat, 98] (i)(ii)(iii) C|C|C or C|CC or CC|C (iv) (v) CC|CC or C|CC|C (vi) C|CSC|C (vii)(viii) C|CSC or CSC|C (ix) CSC

Importance of Curvature Continuity

RS CC Single Turn 0.35 0.01 1 m/s Sinusoid 1.5 0.01 Single Turn 1.8 0.11 3 m/s Sinusoid ∞ 0.16

2 max min 1 max

05 . ; ) 5 ( 2 . Theoretically [De Luca et al., 98] and practically… Ligier Optima electric car Computer-controlled Trajectory tracking [Kanayama, 91]

− −

= = = m m m σ ρ κ

Continuous Curvature Car (CC-Car)

[Scheuer & Fraichard, 96]

σ κ θ θ κ θ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ ⎞ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ ⎛ + ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ ⎞ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ ⎛ = ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ ⎞ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ ⎛ 1 sin cos v y x & & & & ) , , , ( κ θ y x q =

• Curvature vs. steering angle:
• Perfect rolling constraint
• Bounded curvature constraint:
• Steering velocity (curvature derivative):
• Bounded curvature derivative constraint:

max max

| | | | κ κ ϕ ϕ ≤ ⇒ ≤ ϕ κ tan

1 −

= w ϕ ϕ κ σ

2

cos & & = =

max

| | σ σ ≤

Properties of the CC-Car

• Controllability:

The CC-car is small-time controllable [Scheuer & Laugier, 98]

• Existence of a path:

A feasible collision-free path exists if and only if a collision-free path exists [Laumond et al., 98]

• Optimal paths:

No definitive results yet Results for the forward CC-car [Scheuer, 95]: Straight segments Circular arcs of radius Clothoids of sharpness Unfortunately, they are irregular [Boissonnat et al., 94]: Infinite number of clothoids at segment endpoints!

1 max −

κ

max

σ ±

Steering Method for the CC-Car

Principle: extending Reeds & Shepp’s paths into Continuous-Curvature paths κ κ RS-path: straight segments circular arcs of radius straight segments CC-path: circular arcs of radius clothoid arcs of sharpness

1 max −

κ

max

σ ±

1 max −

κ

s

q

g

q

1 max −

κ μ

CCT

Ω μ

CCT

r Principle: circular arcs in Dubins / Reeds & Shepp steering methods replaced by Continuous-Curvature Turns: clothoid arc of sharpness CC-Turn: circular arc of radius clothoid arc of sharpness

Steering Method for the CC-Car

1 max −

κ

max

σ ±

max

σ ±

max 2 max min 2 2 max max max max

/ : deflection Minimum : deflection turn

• CC

) ( ) ( / cos / sin ) , , , ( ) , , , ( ) , , , ( σ κ δ θ θ δ κ θ κ θ θ κ θ κ θ = − = − + − = ⎩ ⎨ ⎧ + = − = = Ω = = =

s g s CCT s CCT CCT i i CCT i i CCT CCT g g g g j j j j i i i i

y y x x r y y x x y x q y x q y x q

s

q

i

q

j

q

g

q δ

1 max −

κ μ

CCT

Ω ) (

s l CCT q

C

+ min

δ δ − μ

CCT

r

CC-Turns

CC-Turns

s

q

l CCT +

Ω ) (

s l CCT q

C

+

μ

CCT

r μ − ) (

s l CCT q

C

−

) (

s r CCT q

C

−

) (

s r CCT q

C

+

μ μ −

r CCT −

Ω

l CCT −

Ω

r CCT +

Ω

From CC-Turns to CC-Paths

μ

1 CCT

C μ

1 CCT

Ω

2 CCT

Ω

1 CCT

Ω

2 CCT

Ω

2 CCT

C

1 CCT

C

2 CCT

C μ μ μ μ sin 2 iff exists exists always tangent

• External

2 1 2 1 CCT CCT CCT

r q q ≥ Ω Ω

CCT CCT CCT CCT CCT CCT

r q q r 2 iff exists cos 2 iff exists tangent

• Internal

2 1 2 1 2 1

≥ Ω Ω ≥ Ω Ω μ μ

1

q

1

q

2

q

2

q TST paths

μ

1 CCT

C

1 CCT

Ω

2 CCT

Ω

1 CCT

Ω

2 CCT

Ω

1 CCT

C

2 CCT

C

2 CCT

C μ

2 , 1

q

2 , 1

q

2 , 1

q

CCT CCT CCT

r 2 iff Existence

2 1

= TT paths T|T paths Ω Ω μ cos 2 iff Existence

2 1 CCT CCT CCT

r = Ω Ω

From CC-Turns to CC-Paths

Dubins case: 2 families of paths [Dubins, 57] (i)(ii) TST or TTT Reeds & Shepp case: 9 families of paths [Reeds & Shepp, 90; Souères & Boissonnat, 98] (i)(ii)(iii) T|T|T or T|TT or TT|T (iv) (v) TT|TT or T|TT|T (vi) T|TST|T (vii)(viii) T|TST or TST|T (ix) TST

⇒ Steering method for the CC-Car

Steering Method for the CC-Car

Steering Method for the CC-Car

CC-Paths vs. Dubins / Reeds & Shepp paths:

• Running time: same order of magnitude (+33%)
• Length: same order of magnitude (+10%)
• Better tracking precision

Looks like the “perfect” steering method but…

What Is Missing To CC-Steering?

CC-Paths include at least one CC-Turn + minimum length of a CC-Turn: ⇒ CC-Steering does not satisfy the Topological Property [Sekhavat & Laumond, 98]: ⇒ Uncomplete planner when CC-Steering used with general motion planning scheme, i.e. Holonomic Path Approximation, Ariadne’s Clew, Probabilistic Path Planner ) (

1 max

κ

−

O ) , ( ) , ( ) , ( , ) , ( , ,

1 2 1 1 2 2 2 1

ε η η ε q B q q Steering q B q C q q ⊂ ⇒ ∈ ∈ ∀ > ∃ > ∀

) , ( 1ε q B ) , ( 1η q B

θ Δ ) (d l d d Δ α ) (α l

Let CC-Steering Satisfy TP (1)

Two new types of CC-Paths are introduced: L-Type: R-Type: “Elementary Paths” [Scheuer & Fraichard, 97] κ

+ + +

→ → → Δ ) ( , , α α l d

+ + +

→ → → Δ ) ( , , d l d θ

Let CC-Steering Satisfy TP (2)

A B D C εCC-Path: From A to B: L-Type From B to C: Segment From C to D: R-Type CC-Steering with εCC-Path satisfies TP When CC-Steering is used with a general motion planning scheme, i.e. Holonomic Path Approximation, Ariadne’s Clew, Probabilistic Path Planner ⇒ Complete path planner

Conclusion

CC-Steering: a steering method for cars

• Computes “Good” paths (near-optimal in length, smooth)
• Efficient
• Completeness when used with general motion planning schemes

Probabilistic Path Planner (Dubins) Ariadne’s Clew (Reeds & Shepp)