Smooth Path Planning for Cars
Thierry Fraichard
Dubins/Reeds & Shepp Car Kinematics
y
q = ( x, y , θ )
ϕ
w
⎧ x& sin θ − y& cosθ = 0 Perfect rolling
⎨
Bounded steering angle
⎩ ρ ≥ ρ min
⎛ x& ⎞ ⎛ cosθ ⎞ ⎛ 0 ⎞
⎜ ⎟ ⎜
⎟ ⎜ ⎟
⎜ y& ⎟ = ⎜ sin θ ⎟v + ⎜ 0 ⎟ω
⎜θ& ⎟ ⎜ 0 ⎟ ⎜ 1 ⎟
⎠ ⎝ ⎠
⎝ ⎠ ⎝
ρ
θ
x
y&
θ = tan
⇒ Path ≡ xy - curve
x&
−1
“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)
(iv) (v)
(vi)
(vii)(viii)
(ix)
C|C|C or C|CC or CC|C
CC|CC or C|CC|C
C|CSC|C
C|CSC or CSC|C
CSC
Importance of Curvature Continuity
Theoretically [De Luca et al., 98]
and practically…
Ligier Optima electric car
Computer-controlled
Trajectory tracking [Kanayama, 91]
1 m/s
3 m/s
Single Turn
Sinusoid
Single Turn
Sinusoid
RS
CC
0.35
1.5
1.8
∞
0.01
0.01
0.11
0.16
κ max = 0.2m −1 ( ρ min = 5m); σ max = 0.05m −2
Continuous Curvature Car (CC-Car)
[Scheuer & Fraichard, 96]
q = ( x, y , θ , κ )
• Curvature vs. steering angle:
κ = w −1 tan ϕ
• Perfect rolling constraint
• Bounded curvature constraint:
| ϕ |≤ ϕ max ⇒| κ |≤ κ max
• Steering velocity (curvature derivative):
σ = κ& =
cos 2 ϕ
• Bounded curvature derivative constraint: | σ |≤ σ max
⎛ x& ⎞ ⎛ cosθ ⎞ ⎛ 0 ⎞
⎜ ⎟ ⎜
⎟ ⎜ ⎟
θ
y
sin
&
⎜ ⎟ ⎜
⎟ ⎜ 0⎟
⎜ θ& ⎟ = ⎜ κ ⎟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
κ
RS-path: straight segments
−1
circular arcs of radius κ max
κ
straight segments
−1
CC-path: circular arcs of radius κ max
clothoid arcs of sharpness ± σ max
Steering Method for the CC-Car
Principle: circular arcs in Dubins / Reeds & Shepp steering methods
replaced by Continuous-Curvature Turns:
μ
clothoid arc of sharpness ± σ max
−1
κ max
CC-Turn: circular arc of radius
clothoid arc of sharpness ± σ max
qg
Ω CCT
−1
κ max
rCCT
qs
μ
CC-Turns
δ
μ
C
+l
CCT
qi = ( xi , yi ,θ i , κ max )
qg
q j = ( x j , y j ,θ j ,κ max )
( qs )
qj
q g = ( x g , y g ,θ g ,0)
Ω CCT
Ω CCT
δ − δ min
−1
κ max
rCCT
qi
qs
μ
⎧ xCCT = xi − sin θ i / κ max
=⎨
⎩ yCCT = yi + cosθ i / κ max
rCCT = ( xCCT − xs ) 2 + ( yCCT − y s ) 2
CC - turn deflection : δ = θ g − θ s
2
Minimum deflection : δ min = κ max
/ σ max
CC-Turns
μ
−l
CCCT
( qs )
−μ
+l
CCCT
( qs )
+l
Ω CCT
−l
Ω CCT
rCCT
qs
−r
Ω CCT
−r
CCCT
( qs )
μ
−μ
+r
Ω CCT
+r
CCCT
( qs )
From CC-Turns to CC-Paths
1
CCCT
2
CCCT
Ω1CCT
2
Ω CCT
μ
q1
TST paths
1
CCCT
q2 μ
External μ - tangent always exists
2
q1q2 exists iff Ω1CCT Ω CCT
≥ 2rCCT sin μ
Ω1CCT
μ
q2
μ
q1
2
Internal μ - tangent exists iff Ω1CCT Ω CCT
≥ 2rCCT cos μ
2
q1q2 exists iff Ω1CCT Ω CCT
≥ 2rCCT
2
Ω CCT
2
CCCT
From CC-Turns to CC-Paths
μ
1
CCCT
2
CCCT
q1, 2
Ω1CCT
T|T paths
2
Ω CCT
q1, 2
2
Existence iff Ω1CCT Ω CCT
= 2rCCT cos μ
1
CCCT
TT paths
2
CCCT
μ
Ω1CCT
q1, 2
2
Ω CCT
2
Existence iff Ω1CCT Ω CCT
= 2rCCT
Steering Method for the CC-Car
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)
(iv) (v)
(vi)
(vii)(viii)
(ix)
T|T|T or T|TT or TT|T
TT|TT or T|TT|T
T|TST|T
T|TST or TST|T
TST
⇒ 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
CC-Paths include at least one CC-Turn + minimum length of a CC-Turn: O(κ max)
⇒ CC-Steering does not satisfy the Topological Property [Sekhavat & Laumond, 98]:
B(q1,ε)
∀ε > 0, ∃η > 0, ∀(q , q ) ∈ C ,
2
1
2
q ∈ B (q ,η ) ⇒ Steering (q , q ) ⊂ B (q , ε )
2
1
1
2
1
⇒ Uncomplete planner when CC-Steering used with
general motion planning scheme, i.e. Holonomic Path
Approximation, Ariadne’s Clew, Probabilistic Path Planner
B(q1,η)
Let CC-Steering Satisfy TP (1)
Two new types of CC-Paths are introduced:
L-Type:
Δd
l(α)
κ
α
Δd →0+,α →0+,l(α)→0+
“Elementary Paths”
[Scheuer & Fraichard, 97]
R-Type:
l(d)
d
Δθ
Δθ →0+,d →0+,l(d)→0+
Let CC-Steering Satisfy TP (2)
D
εCC-Path:
B
C
From A to B:
From B to C:
From C to D:
L-Type
Segment
R-Type
A
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)