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

Smooth Path Planning for Cars Thierry Fraichard

Dubins/Reeds & Shepp Car Kinematics = θ y ϕ ( , , ) q x y w θ − θ = ⎧ & & sin cos 0 x y Perfect rolling ⎨ ρ ≥ ρ ⎩ Bounded steering angle min θ ρ ⎞ ⎞ ⎞ ⎛ ⎛ ⎛ & cos 0 x ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ = θ + ω θ ⎜ & ⎟ ⎜ ⎟ ⎜ ⎟ sin 0 y v ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ θ & ⎝ ⎠ ⎝ ⎠ ⎝ ⎠ 0 1 x & y − θ = ⇒ ≡ tan 1 Path - curve xy & x

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

Importance of Curvature Continuity Theoretically [De Luca et al., 98] and practically… Ligier Optima electric car Computer-controlled Trajectory tracking [Kanayama, 91] 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 − − κ = ρ = σ = 1 2 0 . 2 ( 5 ) ; 0 . 05 m m m max min max

Continuous Curvature Car (CC-Car) [Scheuer & Fraichard, 96] q = θ κ ( , , , ) x y − κ = w ϕ • Curvature vs. steering angle: 1 tan • Perfect rolling constraint ϕ ≤ ϕ ⇒ κ ≤ κ | | | | • Bounded curvature constraint: max max ϕ & σ = κ & = • Steering velocity (curvature derivative): ϕ 2 cos σ ≤ σ | | • Bounded curvature derivative constraint: max θ ⎛ ⎞ ⎛ ⎞ ⎛ ⎞ & cos 0 x ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ θ ⎜ & ⎟ ⎜ ⎟ ⎜ ⎟ sin 0 y = + σ v ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ & θ κ 0 ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ κ ⎝ & ⎠ ⎝ ⎠ ⎝ ⎠ 0 1

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 − κ 1 Circular arcs of radius max ± σ Clothoids of sharpness max Unfortunately, they are irregular [Boissonnat et al., 94] : Infinite number of clothoids at segment endpoints!

Steering Method for the CC-Car Principle: extending Reeds & Shepp’s paths into Continuous-Curvature paths κ κ straight segments − κ 1 CC-path: circular arcs of radius RS-path: straight segments max ± σ − clothoid arcs of sharpness κ 1 circular arcs of radius max max

Steering Method for the CC-Car Principle: circular arcs in Dubins / Reeds & Shepp steering methods q μ replaced by Continuous-Curvature Turns : g ± σ clothoid arc of sharpness max − κ 1 CC-Turn: circular arc of radius max ± σ clothoid arc of sharpness Ω max CCT κ − 1 r max CCT μ q s

CC-Turns δ = θ κ ( , , , ) q x y μ q max i i i i g = θ κ ( , , , ) q x y + j j j j max l ( ) C CCT q = θ s ( , , , 0 ) q x y g g g g q = − θ κ ⎧ j sin / x x Ω = ⎨ CCT i i max = + θ κ CCT ⎩ cos / y y CCT i i max = − + − 2 2 Ω ( ) ( ) r x x y y δ − δ CCT CCT s CCT s CCT min δ = θ − θ CC - turn deflection : g s − κ 1 r δ = κ σ 2 Minimum deflection : / max CCT min max max q i μ q s

CC-Turns μ + − l ( ) C CCT q l ( ) C CCT q s s − μ Ω Ω − + l l CCT CCT r CCT μ − μ q s Ω + Ω − r r CCT CCT − r ( ) + C CCT q r ( ) C CCT q s s

From CC-Turns to CC-Paths 1 C 2 C CCT CCT Ω Ω 1 2 CCT CCT μ μ q q TST paths 1 2 μ External - tangent always exists 1 C CCT Ω Ω ≥ μ 1 2 exists iff 2 sin q q r 1 2 CCT CCT CCT Ω 1 CCT μ μ q 2 C 2 CCT q 1 μ Ω Ω ≥ μ 1 2 Internal - tangent exists iff 2 cos r Ω 2 CCT CCT CCT Ω Ω ≥ CCT 1 2 exists iff 2 q q r 1 2 CCT CCT CCT

From CC-Turns to CC-Paths μ 2 1 C C CCT CCT q 1 , 2 Ω Ω 1 2 T|T paths CCT CCT q 1 , 2 Ω Ω = μ 1 2 Existence iff 2 cos r CCT CCT CCT 1 2 C C μ CCT CCT TT paths Ω Ω 2 1 q CCT CCT 1 , 2 Ω Ω = 1 2 Existence iff 2 r CCT CCT CCT

Steering Method for the CC-Car Dubins case: 2 families of paths [Dubins, 57] TST or TTT (i)(ii) Reeds & Shepp case: 9 families of paths [Reeds & Shepp, 90; Souères & Boissonnat, 98] T|T|T or T|TT or TT|T (i)(ii)(iii) TT|TT or T|TT|T (iv) (v) T|TST|T (vi) T|TST or TST|T (vii)(viii) TST (ix) ⇒ 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? − κ ( 1 ) O CC-Paths include at least one CC-Turn + minimum length of a CC-Turn: max ⇒ CC-Steering does not satisfy the Topological Property [Sekhavat & Laumond, 98] : ( 1 ε , ) ∀ ε > ∃ η > ∀ ∈ B q 0 , 0 , ( , ) 2 , q q C 1 2 ∈ η ⇒ ⊂ ε ( , ) ( , ) ( , ) q B q Steering q q B q 2 1 1 2 1 ⇒ Uncomplete planner when CC-Steering used with ( 1 η , ) B q general motion planning scheme, i.e. Holonomic Path Approximation, Ariadne’s Clew, Probabilistic Path Planner

Let CC-Steering Satisfy TP (1) Two new types of CC-Paths are introduced: ( α ) l L-Type: κ α Δ d Δ → + α → + α → + 0 , 0 , ( ) 0 d l “Elementary Paths” R-Type: [Scheuer & Fraichard, 97] ( d ) l Δ θ d Δ θ → + → + → + 0 , 0 , ( ) 0 d l d

Let CC-Steering Satisfy TP (2) D ε CC-Path: B C From A to B: L-Type From B to C: Segment A 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)