Path Planning & Feedback Control

HYU ELE6033 Autonomous Driving Vehicle Control System, Spring 2019 S.-H.
Lee
Lecture 7
Path Planning & Feedback Control
Goals of this lecture:

· Longitudinal Reference Trajectory Planning
· Lateral Reference Trajectory Planning
· State Reference
· Longitudinal Control
· Lateral Control
· Implementation
· An Application Case
HYU ELE6033 ADVC, Spring 2019 S.-H. Lee
Good to recall: Vehicle driving control framework
Speed vd
Reference control
Vehicle Camera
generation d
Lateral Radar
control LiDAR
Path IMU
GPS
planning ..
.
<latexit sha1_base64="OfGX/54f8kR3X3GJBjzV4tiILnI=">AAAC5HicdVHLTtwwFPWktIX0NZQlG4tRJVajDJvSHVI3XU6lTkGajEbXzk3GwnYi22kZonxCV1W7gy/pFv6hf4MTIl4DV7J0dM/x8fW5rJDCuij63wuerT1/8XJ9I3z1+s3bd/3N999tXhqOE57L3BwxsCiFxokTTuJRYRAUk3jIjj83/OEPNFbk+ptbFjhTkGmRCg7Ot+b9KGaYCV2lsrQLiamrx+AWNI7DQoLWQmcx6uQOPe8PomHUFl0Fow4MSFfj+WbvLE5yXirUjkuwdjqKCjerwDjBJdZhXFosgB9DhlMPNSi0s6r9Wk0/+E5C09z4ox1tu3dvVKCsXSrmlcoPbh9yTfMxblq6dH9WCV2UDjW/figtJXU5bXKiiTDInVx6ANwIPyvlCzDAnU8zjDX+5LlS4LOJmairLkbhUIlTrOv7CmwUTY5P8AzkjQVIkWlMVixaSePxhIAxdePB/H+NOFnxaCWNx60g9AsdPVzfKpjsDT8No697g4PdbrPrZJvskF0yIh/JAflCxmRCOPlL/pELchlkwa/gd/DnWhr0ujtb5F4F51eWI/MP</latexit>
<latexit sha1_base64="wcCxrWwadjxSj6kYHm44/1MWxZ4=">AAADCXicdVHNbhMxEHaWv7L8pXDkYhGQOEW7Ban0VlQkQAIplKatlI0i2zubWLW9K9tbCKvtC/AMPAQ31CtPwY0rPAX2ZhVaQkey5tPM58+eb2ghuLFR9KMTXLp85eq1tevhjZu3bt/prt/dN3mpGQxZLnJ9SIkBwRUMLbcCDgsNRFIBB/Rox/cPjkEbnqs9Oy9gLMlU8YwzYl1p0t1LKEy5qjJRmpmAzNbhDpGgCU6ScJekRHvwhr94vuvB67dDn14O3vt0cpIcp7k1YQIqPSMx6faiftQEXgVxC3qojcFkvfMlSXNWSlCWCWLMKI4KO66ItpwJqMOkNFAQdkSmMHJQuS+acdWMX+NHrpLiLNfuKIub6tkbFZHGzCV1TEnszPzb88X/9UalzZ6NK66K0oJii4eyUmCbY+8lTrkGZsXcAcI0d3/FbEY0YdY5HiYKPrBcSuK8SSivq9ZqbkHyT1DX5xngGd7HC/qUiKUEEXyqIF2RaChe4wICpXKpQd28mn9c0WgoXuMvIWwWutUEXoDNpy3YipcL3d/ox0/60buN3vbDdrVr6D56gB6jGG2ibfQKDdAQMXSKfqJf6HfwOfgafAtOF9Sg0965h85F8P0P2/r/Bg==</latexit>
ENVIR Localization
Perception
modeling
<latexit sha1_base64="DVmcIa7Yjhu64i3cuzd+uGQy5Bc=">AAAC5nicdVHLbtQwFPWEVwmvKSzZWIyQuqqSbuiyEkKCDSqImVaajEa2c5Ox6kdk3wBDlF9gh7pC4kfYwjfwNzhpVNoOvZKlo3uOj33P5ZWSHpPkzyi6cfPW7Ttbd+N79x88fDTefjzztnYCpsIq644586CkgSlKVHBcOWCaKzjiJy87/ugjOC+t+YDrChaalUYWUjAMreU4zTiU0jSFqv1KQYHtq7ezN+9plsXa5p1tmYHJL/DL8STZTfqimyAdwIQMdbjcHn3PcitqDQaFYt7P06TCRcMcSqGgjbPaQ8XECSthHqBhGvyi6Wdr6fPQyWlhXTgGad+9eKNh2vu15kGpGa78Va5r/o+b11jsLxppqhrBiLOHilpRtLQLiubSgUC1DoAJJ8NfqVgxxwSGOOPMwCdhtWYhm4zLthlylAhafoG2vayATtHleA3PmTq3YEqWBvINi17SeVwj4Fyfe/Awr5OfNzx6SefxTxCHhaZX17cJZnu7acDv9iYHO8Nqt8hT8ozskJS8IAfkNTkkUyLIKflJfpHf0Sr6Gn2LTs+k0Wi484RcqujHX5MJ8zU=</latexit>
d
→ ẋ \ẋ
( )
Longi. speed control
Maneuver / Trajectory / Reference → d
→ Vehicle motion
Lateral control → δ \δ
Path Planning & Feedback Control VII-1

Good to recall: Occupancy Grid driving environment modeling

{x}
<latexit sha1_base64="1qFjB5JnH8DGOzc2u2Q83UAeJ4E=">AAACuHicdVFNb9QwEPWGAmX5asuRi9UWidMqAaRSTpV64Vgktq20WZWxM0lHtZ3IdqBLlN/AFQ78MP4NTna1/dj2SbaeZt48z3hEpcj5OP43iB6sPXz0eP3J8Omz5y9ebmxuHbuythLHslSlPRXgUJHBsSev8LSyCFooPBEXh13+5DtaR6X56mcVTjUUhnKS4ENonDaXaXu2sROP4h58lSQLsnOwlvc4Otsc/E2zUtYajZcKnJskceWnDVhPUmE7TGuHFcgLKHASqAGNbtr03bb8TYhkPC9tOMbzPnq9ogHt3EyLoNTgz93tXBe8Kzepff5x2pCpao9Gzh/Ka8V9ybvReUYWpVezQEBaCr1yeQ4WpA8fNEwN/pCl1mCyJhXUhgsLMg151PQT2/amAjsFBnZPXoBaWoCiwmC2YtFLOo97BELopYcI81q6XPHoJZ3HlWDYL3S/B5+TvQ8Lsp8sF3r8bpS8H8VfwmZ32Rzr7DXbZm9ZwvbYAfvMjtiYSUbsF/vN/kSfom9REdFcGg0WNa/YDUT2PzlE43U=</latexit>
√
Grid-wise driving environment modeling: e.g.
· Unoccupied: Safe
· Occupied (by other vehicles): Unsafe
· Unoccupied, but not directly movable:
{y}
√
<latexit sha1_base64="Ur8eES+0od+/MJN9ys2dKVzhnzU=">AAACuHicdVFNb9QwEPWGAmX5aAtHLlY/pJ5WCa1UyqkSF45FYttKm1UZO5N0VNuJbAdYovwGrnDgh/FvcLKrhXbbJ9l6mnnzPOMRlSLn4/jPIHqw9vDR4/Unw6fPnr/Y2Nx6eebK2kocy1KV9kKAQ0UGx568wovKImih8Fxcv+/y51/QOirNJz+rcKqhMJSTBB9C47SZpe3l5k48invwVZIsyM7JWt7j9HJr8DvNSllrNF4qcG6SxJWfNmA9SYXtMK0dViCvocBJoAY0umnTd9vyvRDJeF7acIznffT/iga0czMtglKDv3K3c13wrtyk9vnbaUOmqj0aOX8orxX3Je9G5xlZlF7NAgFpKfTK5RVYkD580DA1+FWWWoPJmlRQGy4syDTkUdN3bNubCuwUGNg9eQFqaQGKCoPZikUv6TzuEQihlx4izGvp24pHL+k8/gmG/UKPe/A5OTpckONkudCzN6PkYBR/DJvdZXOss9dsm+2zhB2xE/aBnbIxk4zYD/aT/YreRZ+jIqK5NBosal6xG4jsXzuW43Y=</latexit>
Simply determines the safety of
· increasing / decreasing speed

· left/right lane changes

Maneuver planning
Given the results of Occupancy grid environment modeling,
considering driver’s intention,
determine safe longitudinal/lateral maneuvering:
· Changing vehicle speeds:

Increasing Velocity (acceleration) / Cruising Velocity / Decreasing Velocity (deceleration)
Headway xd
h = v d Th
· Changing lanes: left lane change, no lane change, right lane change
d
→ v \v
( )
Longi. speed control
Maneuver / Trajectory / Reference → d
→ Vehicle motion
Lateral control → δ \δ

Good to Note: Vehicle motion models
Input to longitudinal motion, v
Can build a CLP actuation model to describe longitudinal motion with inputs v d or ẍd
Input to lateral motion, δ ⇐ ψ̇

Can build a CLP actuation model to describe lateral motion with inputs δ d or Tδ

Good to Recall: Vehicle motion models
• Longitudinal motion model:

Recall longitudinal acceleration model:
   
d 0 1 0
ẋs = As xs + Bs ẍ =   xs +   ẍd (7.1)
0 −1/τ 1/τ
Assume a feedback controller ẍd = Ks (v d − v) is applied and the resulting CLP is

(approximately) represented by
1 1 d
v̇ = − v + v (7.2a)
τv τv
A DT equivalent CLP

Tc Tc d
v(k + 1) = 1− v(k) + v (k) (7.2b)
τv τv

Alternatively, a simple longitudinal motion model
ẋ = v
DT equivalent for a sample period Tc
x(k + 1) = x(k) + Tc v(k) (7.3)
Combining (7.2) and (7.3) leads to

      
x 1 Tc x 0
  =   +   vkd (7.4)
v 0 1 − Tc /τv v Tc /τv
k+1 k
Note: A CLP actuation is required for speed s.t. v d \v → 1

• The lateral vehicle motion model in terms of the state vector

 
  eyL
xv   h i
x =   with xv =  ė y
 and xm = ψ̇
xm
 
eψ
x(k + 1) = Φx(k) + Γu(k) + Γϕ ϕ(k)





      


 Φv Φvm Γv Γvϕ
= x(k) + u(k) +  ϕ(k)

     

Φmv Φm Γm 0 (7.5)

  

Cv 0





 y = Cx =  x
 0 Cm
Note: A CLP actuation is also required for steering s.t. δ d \δ → 1

Remark 7.1 The longitudinal motion model is independent of the lateral motion model.
The lateral motion model on the other hand requires Vx = ẋ = v from the longitudinal model.
Remark 7.2 To date, vehicles in production are not ready to have CLP actuation functions.

Good to note:
• Path planning: a sequence of desired poses

origin lane
target lane
• Trajectory planning
velocity profiles for the seq. of poses: from the original to the target
• Reference
state references for feedback control

Longitudinal Reference Trajectory Planning
Given a result of maneuver planning xd

h , determine a speed reference trajectory:
v d = v + (xdh − xh )/Th (7.6)
where
Headway (time) Th
Headway distance xh = vTh
e.g. A simple case of planning: Headway distance change at 1 [sec] with Th = 3 [sec], vehicle
speed governed by (7.2) with τv = 0.5.

35
30
25
20
15
10
5
0 0.5 1 1.5 2 2.5 3 3.5 4
Speed ref. trajectory v d and vehicle speed v governed by (7.2) with τv = 0.5.

Lateral Reference Trajectory Planning
Given a result of maneuver planning L/N/R-LX, determine a reference trajectory that allows lane
change to the target lane.
Utilize the result of maneuver planning: L/N/R-LX
An arbitrary position on the original lane → Target lane center

 
  eyL 
xv  ė 
y 
Need to utilize the results of on-road localization: x =  =
 
xm  eψ 
 
ψ̇

Target Lane Curve:
· A target lane curve (TLC) is a set point lane curve passing through the target lane center.
· A TLC is described by
fctlc (x) := fc (x) + etlc
y (7.7)
where
fc (x) = c0 + c1 x + c2 x2 + c3 x3 is the original lane curve detected by the camera,

etlc
y , the TLC offset, is the directional distance from the vehicle’s on-road position to the
target lane center.
· If fctlc (x) not available from the camera-vision sensors, it can be simply computed from an
assumed value of etlc y .

Velocity Reference Trajectory:
• Utilizing the TLC in (7.7), we develop a lateral velocity reference trajectory consists of
acceleration and deceleration reference trajectories:
– The acceleration trajectory is designed in terms of a lane change operation time tlx
defined up to a desired profile switching time t∗lx such that tlx ∈ [0, t∗lx ].
An example of t∗lx is a desired lane crossing time.
– The deceleration trajectory, on the other hand, is designed in terms of the TLC offset
etlc tlc
y (·) = fc (·) − fc (·) that diminishes as the ego vehicle moves onto the target lane
center.

• Velocity reference trajectory

vacc (tlc ) if tlc < t∗lc ,
(
ėdy = (7.8)
vdec (etlc
y ) otherwise
with a non-empty set defined by
S = {etlc
y | |v dec (·) − νetlc
y | < , |e tlc
y | < e∗
y}
for some ν > 0, > 0, and e∗y > 0.
The set S defines the region of settling to the target lane center in which near linear decay
of lateral error (in the phase plane) occurs with vdec (·) ≈ νetlc tlc ∗
y for |ey | < ey
s.t. near exponential decay is assured in time domain.

• The profiles vacc (·) and vdec (·) need to exhibit a continuous geometric derivative at the
switching instant with
vacc (t∗lc ) = vdec (etlc ∗
y (tlc )) (7.9a)
and
dvacc (t∗lc ) dvdec (t∗lc )
= . (7.9b)
dey dey

• The curve vaccc (·) for acceleration initiates lane change and is given in terms of time.
The set of acceleration trajectories is defined as
Sa = {(ey , ėdy ) | ėdy = vacc (tlc ), tlc < t∗lc }.
• The curve for deceleration vdec (·), on the other hand, is given in terms of lateral offset from
the target lane center at which its value disappears.
The set of deceleration trajectories is defined as
Sd = {(ey , ėdy ) | ėdy = vdec (ey (t)), t > t∗lc }
with S ⊂ Sd .
• The two curves meet with a continuous geometric derivative at the switching instant, as
shown in (7.9).

A simple design example of an acceleration profile that satisfies the zero derivative condition at
tlc = t∗lc as well as at the beginning of lane change (tlc = 0) is a sine-shaped reference
velocity profile
ew

y tlc 1 tlc
vacc (tlc ) := ∗ − sin 2π (7.10)
tlc t∗lc 2π t∗lc
where ew ∗
y denotes a directional distance to complete lane change and tlc satisfies
s
2ew
y
t∗lc >
v̇acc,max
for a pre-specified limit of lateral acceleration, v̇acc,max .

0.9
0.8
0.7
0.6
0.5
0.4
0.3
0.2
0.1
0
1 0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0
Figure 7.1: Lateral velocity profile for acceleration

A simple design example of a deceleration profile to the target lane center is

!
ewy p π etlc
y
vdec (etlc
y ) := sin (7.11)
t∗lc 2 etlc
y (t∗)
lc
with p ≥ 1.

0.9
0.8
0.7
0.6
0.5
0.4
0.3
0.2
0.1
0
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Figure 7.2: Lateral velocity profile for deceleration

· Find that the profiles satisfy the conditions in (7.9).

· Also find that the curve exhibits zero derivative with respect to ey at the inflection point
where etlc tlc ∗
y = ey (tlc ) and approximately linear profile near the target lane center with
p ≥ 1.

0.9
0.8
0.7
0.6
0.5
0.4
0.3
0.2
0.1
0
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1
Figure 7.3: Lateral velocity profile: ėd tlc

y as a fcn. of ey

0.9
0.8
0.7
0.6
0.5
0.4
0.3
0.2
1 0.9 0.8 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0
Figure 7.4: Settling from ey = 0.2, ėy = 0.3 with p = 1; The profile in S yields exp. decay

Longitudinal state reference:
Recall
     
d 0 1 0 v
ẋs = As xs + Bs ẍ =   xs +   ẍd with xs =  .
0 −1/τ 1/τ v̇
A state reference for slow acceleration driving
v d = xdh /Th , v̇ d = 0
Feedback control
   
d
d d d v v
ẍ = Ks (v − v) or ẍ = Ks   − 

0 v̇
leads to an approx. CLP
Tc Tc d
vk+1 = 1− vk + v
τv τv

Lateral state reference:
Utilizing the velocity reference trajectory (7.8)
tlc < t∗lc

(
vacc (tlc ),
ėdy =
vdec (ey (t)), t > t∗lc
Utilizing (7.7), we compute

fˆc (x) = fctlc (x) − etlc
y .
Utilizing ėd d
y , we compute eyL from
ˆ
fc (L) + ρ(tlc )
 if tlc ≤ t∗lc
edyL = ė d tlc
y (ey ) ∗
(7.12a)
tlc ∗
fc (L) − ρc (tlc )

w
tlc otherwise
ey
with Z tlc
ρ(tlc ) = ėdy (tlc )dt, ρc (tlc ) = ew
y − ρ(tlc ). (7.12b)
0

Further, from the TLC.    

edψ c
  =  1 . (7.13)
ψ̇ d 2c2 Vx
Now, we can define a state reference for lane keeping and changing:
h iT
xd = edyL ėdy edψ ψ̇ d .
 
edψ
Remark 7.3 Applying   = 0 is plausible as far as we are concerned with highway driving
ψ̇ d
with small lane curve windings.

State reference for lane keeping:
Utilize the linear approximation in S < Sd . . .
State reference from the lane curve:

    
edyL 1 L L2 3
L c0
    
 ėd  −ν 0 0 0  c 
 y    1
xd =   =    (7.14)
 ed   0 1 0 0  c2 
 ψ   
ψ̇ d 0 0 2Vx 0 c3
with ν > 0 for small e∗y > 0

Good to note:
Lateral lateral velocity profile
−νey

d
Sa −−→ Sd −−→ ėy = ėy,max sat in S (7.15)
ėy,max
with ν > 0 for some e∗y > 0 where a damped controller is applied.

Feedback control
• The longitudinal motion model in (7.2) is independent of vehicle’s lateral motion.

• The lateral motion model in (7.5) is on the other hand affected by ẋ (vehicle speed).
• Fully utilizing this observation, we want to introduce a decentralized control framework

with such a unidirectional connection being considered:
√
We are to design a longitudinal controller without considering lateral motion, and then
design a lateral controller by considering longitudinal motion (speed).

Longitudinal Speed Control:
Given a state reference xd

s,
 d    
d v vd ẋd
xs =   =   =  
v̇ 0 0
we can apply a state feedback control law

h i
ẍd = kv d
ka (xs − xs ).
If longitudinal speed variation is not large, one may introduce
ẍd = kv (ẋd − v)
that does not require v̇ , longitudinal acceleration.

Theorem 1 Suppose that a set point speed command
ẋd = ẋ + (xdh − xh )/Th (7.16)

h i
for a given headway distance xd
h and control law a control law ẍ
d
= kv ka (xds − xs ) with
kv > 0 and ka > −1, are applied to control the vehicle longitudinal motion described by (7.1).
Then, the resulting longitudinal motion exhibits asymptotic zero speed error.
Proof: Exercise

Lateral Steering Control:
• By using the profile in (7.8), we desire to have
(ey , ėdy ) ∈ Sa → (ey , ėdy ) ∈ Sd → (ey , ėdy ) ∈ S .
• In S , it is desirable to exhibit smoothly decaying lateral error to the target lane center.
The set S defines near linear decay of lateral error (in the phase plane) vdec (·) ≈ −νetlc
y
for |etlc
y | < e∗y , which forces exponential decay of ey to occur in the time domain.
• It is thus desirable to determine control to attain both xd − x → 0 and ey decaying along

the hyperplane ėy + νey = 0 for (ey , ėd y ) ∈ S .

• State feedback lateral control
State feedback control:

u = K(xd − x)
State reference:
h iT
xd = edyL ėdy edψ ψ̇ d
Simpler state references

h iT
d
x = edyL ėdy edψ 0
h iT
d
x = edyL ėdy 0 0

• State estimator based Controller

State estimator:
x̂(k) = x̄(k) + L(y(k) − ȳ(k))
(
x̄(k + 1) = Φx̂(k) + Γu(k)

Control:
u(k) = K(xd (k) − x̂(k))
The controller dynamics u(k) = Gc (z)y(k) where
x̄(k + 1) = (Φ − ΓK)(I − LC)x̄(k) + (Φ − ΓK)Ly(k)
(
Gc (z) :
u(k) = K(xd (k) − x̂(k)) = F xd (k) − K(I − LC)x̄(k) − KLy(k)
Exercise: Find the resulting closed-loop system and its stability condition.

Good to Recall: Linear quadratic control
Given a discrete-time system

(
x(k + 1) = Φx(k) + Γu(k) + w(k)
z(k) = Cx x(k) + Du u(k)
Design control u = −Kx such that the cost

∞
X ∞
X
T T T T

J= x (k)Qx x(k) + u (k)Ru u(k) = x (k) Qx + K Ru K x(k)
k=0 k=0
is minimized.
Applying the state feedback control u = −Kx leads to the closed-loop

x(k + 1) = (Φ − ΓK)x(k) + w(k)
(
z(k) = (Cx − Du K)x(k)
with (Cx − Du K)T (Cx − Du K) = Qx + K T Ru K .

P∞
Good to Recall: optimal state estimator design with min J = 0 E[z T (k)z(k)]
Using the duality between KF and LQ control:




 Φ − ΦLC : (Φ − ΓK)T

ΦL : KT


Φ − ΦLC ∼ (Φ − ΓK)T = ΦT − K T ΓT ⇒


 Φ : ΦT

ΓT

 C :
find the Lyapunov equation
(Φ − ΓK)T X(Φ − ΓK) − X + Qx + K T Ru K = 0

and the optimal gain
K = (ΓT XΓ + Ru )−1 ΓT XΦ
where X > 0 satisfies the discrete-time algebraic Riccati equation (DARE)
X = ΦT XΦ − ΦT XΓ(ΓT XΓ + Ru )−1 ΓT XΦ + Qx

Implementation
• Longitudinal and Lateral controllers determine v d and δ d

• Vehicle motion is determined by v and ψ̇
• Closed loop actuation functions: v d \v and δ d \δ
Powertrain
ẍ 1
=
ẍd τs + 1
a feedback control input ẍd is applied and the resulting CLP is (approximately)
represented by
v 1
=
vd τv s + 1
vd ẍd 1 v
Controller
<latexit sha1_base64="LNAFoXBS6EhmEcGG/fd8Kq/hF+8=">AAACt3icdVHLThsxFHUG2kL64LVkY5FW6iqaGSJCVkViwxKkBpCSNLI9dxIL2zO1PbTpaH6BLbRSP6CfxN/UnkRpKeVIto7OPT729aW54MaG4X0jWFl99vzF2nrz5avXbzY2t7bPTVZoBn2WiUxfUmJAcAV9y62Ay1wDkVTABb069vWLa9CGZ+qjneUwkmSieMoZsV66/pQ0x5utsN07PIg7Bzhsh2E3iiNP4m5nv4Mjp3i0Pvz67vHjdLzV+DlMMlZIUJYJYswgCnM7Kom2nAmomsPCQE7YFZnAwFFFJJhRWT+2wu+ckuA0024pi2v17xMlkcbMJHVOSezU/Fvz4v9qg8Kmh6OSq7ywoNj8orQQ2GbYd44TroFZMXOEMM3dWzGbEk2Ydf/THCr4wjIpiUrKIeWV22DCVcktSP4NquqhA7wDHHuiTolYRhDBJwqSRxG1xWc8YaBULjOo61fzr48yaovP+GNYDLQGnpNuZ0F60XKg53E72m/HZ2Hr6C2aYw3toj30HkWoi47QCTpFfcTQFN2gW3QX9IJxkAbTuTVoLM7soAcIPv8GhPfkoA==</latexit> <latexit sha1_base64="GeM3XkFCt2z5mSQWylaZqzNbJgU=">AAACvnicdVHLbhMxFHWGVwmvFpZsLAISq2hmGjXtqpHYsCwSaStlQmV77qRW/RjZnjbpaD6jiyIBaz6Jv8GeRIFSeiRbR+ceH9v30lJw6+L4Vye6d//Bw0cbj7tPnj57/mJz6+Wh1ZVhMGZaaHNMiQXBFYwddwKOSwNEUgFH9OxDqB+dg7Fcq89uUcJUkpniBWfEeWmS5bl29bz5kndPNntxf293Jx3s4Lgfx8MkTQJJh4PtAU68EtDb/3kd8PXgZKvzLcs1qyQoxwSxdpLEpZvWxDjOBDTdrLJQEnZGZjDxVBEJdlq3b27wO6/kuNDGL+Vwq/59oibS2oWk3imJO7X/1oL4v9qkcsXutOaqrBwotryoqAR2GocG4JwbYE4sPCHMcP9WzE6JIcz5NnUzBRdMS0lUXmeUN36DGVc1dyD5JTTNTQcEB3h2R50SsY4ggs8U5LciWkvIuMNAqVxnUP9fw+e3MlpLyPhjWA20BV6S4WBF9pL1QA/TfrLdTz/FvdFbtMQGeo3eoPcoQUM0Qh/RARojhjS6Qt/Rj2gUFZGM9NIadVZnXqEbiOa/AZQ85+c=</latexit>
<latexit sha1_base64="XqK/GEH+Nsin9Q+Noa2aKXsoJno=">AAACtXicdVFNb9QwEPWmQMvy1ZYjF4sFidMqaZG2PVGJC8cWsW2l3agaO5OtVX9EtlNYovwDriCB1B/Qn9R/g5OsFsrSJ9l6mnnzPONhhRTOx/FNL1q7d//B+sbD/qPHT54+29zaPnamtBzH3EhjTxk4lELj2Asv8bSwCIpJPGEX75v8ySVaJ4z+5OcFpgpmWuSCgw+hj5f9s81BPIxb0FWSLMjg3fXPBr8Oz7Z6V9PM8FKh9lyCc5MkLnxagfWCS6z709JhAfwCZjgJVINCl1ZtqzV9HSIZzY0NR3vaRv+uqEA5N1csKBX4c/dvrgn+Lzcpfb6XVkIXpUfNu4fyUlJvaDM3zYRF7uU8EOBWhF4pPwcL3Iff6U81fuZGKdBZNWWiDhfOhK6ERyW+Yl3fVmCjwMDuyDOQSwuQYqYxW7FoJY3HHQLG1NKDhXmt+LLi0Uoajz+CbqH7LWhHRm8XZD9ZLvR4Z5jsDneO4sHBK9Jhg7wgL8kbkpAROSAfyCEZE05y8o18Jz+iUZRGWZR30qi3qHlObiEyvwHiEeOI</latexit>
<latexit sha1_base64="xX+uptANPDk9Ur8wRqcC5I6o1Gw=">AAACyXicdVFNbxMxEHWWrxK+0sKNi0VA4hTtlkqlp1bqBYlLkUhbKRtFY+8kteqPre2FJqs98StAQuJa/hH/Bu9uFCihT7L1NPPmeTzDcimcj+NfnejW7Tt3723c7z54+Ojxk97m1rEzheU45EYae8rAoRQah154iae5RVBM4gk7P6zzJ5/QOmH0Rz/PcaxgpsVUcPAhNOk9SxX4M6vKQ6O9NVKirbqTXj8exA3oOkmWpL9/9bXGt6PJZud7mhleKNSeS3BulMS5H5dgveASq25aOMyBn8MMR4FqUOjGZdN+RV+FSEanxoajPW2if1eUoJybKxaUdbPu31wd/F9uVPjp23EpdF541Lx9aFpI6g2tZ0EzYZF7OQ8EuBWhV8rPwAL3YWLdVONnbpQCnZUpE1W4cCZ0KTwqscCquq7AWoGB3ZBnIFcWIMVMY7Zm0UhqjxsEjKmVBwv/teJyzaOR1B5/BO1C9xrQluzuLMleslro8fYgeTPY/hD3D16SFhvkOXlBXpOE7JID8o4ckSHhZEF+kCvyM3ofXUSX0aKVRp1lzVNyDdGX39uZ6/g=</latexit>
<latexit sha1_base64="lVJuROWaGYURSHpoJxzxppbt3vs=">AAACy3icdVFNT9wwEPWmtKXbr6WVeuFiQStRVVoltBLlhtRLL5VAYgFps1qNncliYTuR7UCXkGN/Ri+90mP/SI/8G5xktUC3PMnW08ybZ88My6WwLgyvOsGDpYePHi8/6T599vzFy97KqwObFYbjgGcyM0cMLEqhceCEk3iUGwTFJB6yky91/vAUjRWZ3nfTHEcKJlqkgoPzoXHvTZwa4GVUlXYjdlBQ+yF6X3XHvfWwHzagiySakfWdtb+XivwZ7o5XOj/jJOOFQu24BGuHUZi7UQnGCS6x6saFxRz4CUxw6KkGhXZUNg1U9J2PJDTNjD/a0SZ6u6IEZe1UMa9U4I7tv7k6+L/csHDp51EpdF441Lx9KC0kdRmtp0ETYZA7OfUEuBH+r5Qfg5+I8zPrxhrPeKYU6KSMmaj8hROhS+FQiXOsqrsKrBXo2T15BnJuAVJMNCYLFo2k9rhHwJiaezDfrxHfFzwaSe1xI2gXut2AtmTr04xsR/OFHmz2o4/9zT2/2bekxTJZJWtkg0Rki+yQr2SXDAgnF+QXuSS/g2+BDc6Di1YadGY1r8kdBD+uAXH/6lg=</latexit>
s(⌧ s + 1)
√
Either v d or ẍd is applied to the vehicle with limitations being considered.

Steering
δ a
=
Tδ s(τeps s + 1)
a feedback control input Tδ is applied and the resulting CLP is (approximately)
represented by
δ 1
=
δd τδ s + 1
d 1
T
Controller
<latexit sha1_base64="LQNfRM+Vu90aeBJcXPgnSUm41bc=">AAACunicdVHbbhMxEHW2XEq4teWRF4uAxFO0W5BKJSQq9YXHIpGmUjaqxt5JasX2ruxZIKz2I3hsER/AJ/E3eHejQAk9kq2jmTPHMx5RaOUpjn/1oq1bt+/c3b7Xv//g4aPHO7t7pz4vncSRzHXuzgR41MriiBRpPCscghEax2Jx3OTHn9B5lduPtCxwamBu1UxJoBAapxlqgv75ziAexi34JklWZPDu52WDq5Pz3d6PNMtladCS1OD9JIkLmlbgSEmNdT8tPRYgFzDHSaAWDPpp1fZb8xchkvFZ7sKxxNvo3xUVGO+XRgSlAbrw/+aa4P9yk5Jmb6aVskVJaGX30KzUnHLeDM8z5VCSXgYC0qnQK5cX4EBS+KJ+avGzzI0Bm1WpUHW4cK5spQiN+op1fV2BjQIDuyEvQK8tQKu5xWzDopU0HjcIhDBrDxHmderLhkcraTz+CLqFHrbgHTl4vSKHyXqhp/vD5NVw/0M8OHrOOmyzp+wZe8kSdsCO2Ht2wkZMsgX7xq7Y9+htJCIVLTpp1FvVPGHXENFvIcDlqg==</latexit>
<latexit sha1_base64="tzl4IyFOGbc2UK9PUKs2Qqn0u5M=">AAACvHicdVHLbhMxFHWGVwmvFpZsLAISq2hmGjXtplRiw7JIpC3KhOrac5NasT0j21NIR/MVrED0A/gk/gZ7EgVK6ZFsHZ17fOzry0oprIvjX53o1u07d+9t3O8+ePjo8ZPNradHtqgMxxEvZGFOGFiUQuPICSfxpDQIikk8ZvO3oX58jsaKQn9wixInCmZaTAUH56WPWY7Swae8e7rZi/t7uzvpYIfG/TgeJmkSSDocbA9o4pWA3puf3wK+H55udS6zvOCVQu24BGvHSVy6SQ3GCS6x6WaVxRL4HGY49lSDQjup2xc39JVXcjotjF/a0Vb9+0QNytqFYt6pwJ3Zf2tB/F9tXLnp7qQWuqwcar68aFpJ6goa2qe5MMidXHgC3Aj/VsrPwAB3/pO6mcbPvFAKdF5nTDR+w5nQtXCoxAU2zVUHBgd6dkOdgVxHgBQzjfm1iNYSMm4wMKbWGcz3a8SXaxmtJWT8MawG2oIuyXCwInvJeqBHaT/Z7qfv497BS7LEBnlOXpDXJCFDckDekUMyIpwo8pX8IJfRfpRH80gtrVFndeYZuYLo/DfIi+bC</latexit>
<latexit sha1_base64="HQxnlQ0FWThjOCAVxmSiPd/j8dA=">AAACvHicdVHNbhMxEHa2/JTw15YDBy4WAYlTtLuNmvYClXrhWKSmLcpG0dg7Sa3Y3pXtbRtW+xRc6XPwEhy58Ai8AUe8myhQSkey9embbz7PeFguhXVh+KMVrN25e+/++oP2w0ePnzzd2Nw6tllhOA54JjNzysCiFBoHTjiJp7lBUEziCZsd1PmTczRWZPrIzXMcKZhqMREcnKc+Ho2TFKWD9nijE3b3dnfi3g4Nu2HYj+KoBnG/t92jkWfq6Lz79e3r9+cHPw/Hm62rJM14oVA7LsHaYRTmblSCcYJLrNpJYTEHPoMpDj3UoNCOyqbjir72TEonmfFHO9qwf1eUoKydK+aVCtyZ/TdXk//LDQs32R2VQueFQ80XD00KSV1G6/FpKgxyJ+ceADfC90r5GRjgzn9SO9F4wTOlQKdlwkTlL5wKXQqHSnzCqrquwFqBHt2SZyBXFiDFVGN6w6KR1B63CBhTKw/m5zXi8oZHI6k9/giWC22CLkC/twR70Wqhx3E32u7GH8LO/iuyiHXygrwkb0hE+mSfvCeHZEA4UeQz+UKugrdBGswCtZAGrWXNM3ItgvPfVnHnBQ==</latexit>
<latexit sha1_base64="xX+uptANPDk9Ur8wRqcC5I6o1Gw=">AAACyXicdVFNbxMxEHWWrxK+0sKNi0VA4hTtlkqlp1bqBYlLkUhbKRtFY+8kteqPre2FJqs98StAQuJa/hH/Bu9uFCihT7L1NPPmeTzDcimcj+NfnejW7Tt3723c7z54+Ojxk97m1rEzheU45EYae8rAoRQah154iae5RVBM4gk7P6zzJ5/QOmH0Rz/PcaxgpsVUcPAhNOk9SxX4M6vKQ6O9NVKirbqTXj8exA3oOkmWpL9/9bXGt6PJZud7mhleKNSeS3BulMS5H5dgveASq25aOMyBn8MMR4FqUOjGZdN+RV+FSEanxoajPW2if1eUoJybKxaUdbPu31wd/F9uVPjp23EpdF541Lx9aFpI6g2tZ0EzYZF7OQ8EuBWhV8rPwAL3YWLdVONnbpQCnZUpE1W4cCZ0KTwqscCquq7AWoGB3ZBnIFcWIMVMY7Zm0UhqjxsEjKmVBwv/teJyzaOR1B5/BO1C9xrQluzuLMleslro8fYgeTPY/hD3D16SFhvkOXlBXpOE7JID8o4ckSHhZEF+kCvyM3ofXUSX0aKVRp1lzVNyDdGX39uZ6/g=</latexit>
<latexit sha1_base64="nHQTy9fOg3H/4loXJ//H8engWV4=">AAAC2nicdVFNb9QwEPWGr7J8beHIxWqpVIS0SgpS6a0SF45FYttKm9XKdiZbq7YT2ZOWxcqFG+qVK7+AExIn+BUc+Tc4yWqhLB3J1tO8N8/jGV4q6TCOf/Wia9dv3Ly1drt/5+69+w8G6w8PXVFZASNRqMIec+ZASQMjlKjguLTANFdwxE9fNfzRGVgnC/MW5yVMNJsZmUvBMKSmg600t0z4pPZuO0VWTX2qGZ5Y7R3auqbuWfK07k8Hm/EwboOugmQBNvc3fn7R5Nv4YLre+5xmhag0GBSKOTdO4hInnlmUQkHdTysHJROnbAbjAA3T4Ca+/U9Nt0Imo3lhwzFI2+zfFZ5p5+aaB2XTrPuXa5L/48YV5i8nXpqyQjCieyivFMWCNsOhmbQgUM0DYMLK0CsVJywMCMMI+6mBc1FozUzmUy7rcMFMGi8RtHwPdX1ZAY0CArqC50wtLZiSMwPZikUraTyuEHCulx48/NfKdyseraTx+CPoFrrXBu3A7osF2EuWCz3cGSbPhztvwmafkC7WyGOyQbZJQnbJPnlNDsiICHJBvpLv5EeURh+ij9FFJ416i5pH5FJEn34DhX7xew==</latexit>
s(⌧str s + 1)
√
Either δ d or Tδ is applied to the vehicle with limitations being considered.
• Sensing: Radar, Camera, . . .

• Interfacing: CAN, Ethernet, . . .

• Sample-time synchronization
• Functional flow
<latexit sha1_base64="bYj7XiqG+CRgEMDCKFTp1hIdBsY=">AAACxnicdVFNbxMxEHWWrxK+EjhysQhInKLdFqn01EpceuBQJNJWSqJo7J2kVm3vyp5tmq5W4m+A1CuIn8S/wbuJAiX0SbaeZt48j2dErpWnOP7Viu7cvXf/wdbD9qPHT54+63SfH/uscBIHMtOZOxXgUSuLA1Kk8TR3CEZoPBHnH+r8yQU6rzL7mRY5jg3MrJoqCRRCk053RHhJ5UewyGXhLrCadHpxP27AN0myIr39n19rfDuadFvXozSThUFLUoP3wyTOaVyCIyU1Vu1R4TEHeQ4zHAZqwaAfl03vFX8TIimfZi4cS7yJ/l1RgvF+YURQGqAz/2+uDv4vNyxo+n5cKpsXhFYuH5oWmlPG60HwVDmUpBeBgHQq9MrlGTiQFMbVHlmcy8wYsGk5EqoKF86ULRWhUVdYVTcVWCswsFvyAvTaArSaWUw3LBpJ7XGLQAiz9hDhv05dbng0ktrjj6DdLHSvAV+S3XcrspesF3q83U92+tuf4t7Ba7bEFnvJXrG3LGG77IAdsiM2YJLN2TX7zn5Eh5GNimi+lEatVc0LdgPRl9+Zkeqt</latexit>
Lane curve State referene
<latexit sha1_base64="MxI5jF+067Nt3VnGWsQOgxki/MQ=">AAACynicdVFNbxMxEHWWrxIKpCBOXCwCEqdot0UqOVGJCwcORZC2KBtFtnc2tWp7V/Zs22Dtjb/BgSv8Ig78APgT4N1EgRL6JFtPM2+exzO8VNJhHH/rRFeuXrt+Y+Nm99bm7Tt3e1v3DlxRWQEjUajCHnHmQEkDI5So4Ki0wDRXcMhPXjb5w1OwThbmHc5LmGg2MzKXgmEITXsPUoRz9G+RIVALOVgwUE97/XgQt6DrJFmS/otfP4Y/v7/f3J9udT6lWSEqDQaFYs6Nk7jEiWcWpVBQd9PKQcnECZvBOFDDNLiJb/uv6ZMQyWhe2HAM0jb6d4Vn2rm55kGpGR67f3NN8H+5cYX584mXpqwQjFg8lFeKYkGbYdBMWhCo5oEwYWXolYpjZpnAMLJuauBMFFozk/mUyzpcMJPGSwQtP0BdX1RAo4DALslzplYWTMmZgWzNopU0HpcIONcrDx7+a+X5mkcraTz+CLrtQoct6ILsPluSYbJa6MH2INkZbL+J+3uPyQIb5CF5RJ6ShOySPfKK7JMREcSTz+QL+Rq9jmw0j/xCGnWWNffJBUQffwNDwuyK</latexit>
Controller
<latexit sha1_base64="xX+uptANPDk9Ur8wRqcC5I6o1Gw=">AAACyXicdVFNbxMxEHWWrxK+0sKNi0VA4hTtlkqlp1bqBYlLkUhbKRtFY+8kteqPre2FJqs98StAQuJa/hH/Bu9uFCihT7L1NPPmeTzDcimcj+NfnejW7Tt3723c7z54+Ojxk97m1rEzheU45EYae8rAoRQah154iae5RVBM4gk7P6zzJ5/QOmH0Rz/PcaxgpsVUcPAhNOk9SxX4M6vKQ6O9NVKirbqTXj8exA3oOkmWpL9/9bXGt6PJZud7mhleKNSeS3BulMS5H5dgveASq25aOMyBn8MMR4FqUOjGZdN+RV+FSEanxoajPW2if1eUoJybKxaUdbPu31wd/F9uVPjp23EpdF541Lx9aFpI6g2tZ0EzYZF7OQ8EuBWhV8rPwAL3YWLdVONnbpQCnZUpE1W4cCZ0KTwqscCquq7AWoGB3ZBnIFcWIMVMY7Zm0UhqjxsEjKmVBwv/teJyzaOR1B5/BO1C9xrQluzuLMleslro8fYgeTPY/hD3D16SFhvkOXlBXpOE7JID8o4ckSHhZEF+kCvyM3ofXUSX0aKVRp1lzVNyDdGX39uZ6/g=</latexit> <latexit sha1_base64="4a9oMPTtEeb+3OZoLiS3XJ/MjIA=">AAACwXicdVFNbxMxEHWWrxK+WjhysQhInKLdglR6aqVeOBaJpJWyoRp7J4lV27uyZ2nT1f6PgsSB3vqT+Dd4N1GghD7J1tPMm+fxjCi08hTHvzrRnbv37j/YeNh99PjJ02ebW8+HPi+dxIHMde6OBXjUyuKAFGk8LhyCERqPxOlBkz/6is6r3H6meYFjA1OrJkoChdCXlPCcqiHOlNRYn2z24n7cgq+TZEl6e9ffGnw/PNnq/EizXJYGLUkN3o+SuKBxBY5aw25aeixAnsIUR4FaMOjHVdt2zd+ESMYnuQvHEm+jf1dUYLyfGxGUBmjm/801wf/lRiVNPowrZYuS0MrFQ5NSc8p5MwOeKYeS9DwQkE6FXrmcgQNJYVLd1OKZzI0Bm1WpUHW4cKpspQiNusC6vqnARoGB3ZIXoFcWoNXUYrZm0Uoaj1sEQpiVhwj/dep8zaOVNB5/BN12obst+ILsvF+S3WS10OF2P3nX3/4U9/ZfswU22Ev2ir1lCdth++wjO2QDJpljl+wnu4oOIhUVkVtIo86y5gW7gaj6DSsB6Vk=</latexit>
Vehicle Sensing
<latexit sha1_base64="GZLVhcsuKMP3HNjLpK8GkAK7REs=">AAACxnicdVFNbxMxEHWWrxIKJHDkYjUgcYp2C1LJiUq99FhE0xZlo8j2Traj2t6V7SWkq5X4I73Cb+LAD4A/Ad7dKFBCn2TraebN83iG5xKtC8NvneDW7Tt3723d7z7Yfvjoca//5MRmhREwFpnMzBlnFiRqGDt0Es5yA0xxCaf84qDOn34EYzHTx26Zw1SxVOMcBXM+NOv1Y8XcuVHle9AWdVp1Z71BOAwb0E0Srcjg7a8fo5/fP2wfzfqdqzjJRKFAOyGZtZMozN20ZMahkFB148JCzsQFS2HiqWYK7LRseq/oCx9J6Dwz/mhHm+jfFSVT1i4V98q6U/tvrg7+Lzcp3PzNtESdFw60aB+aF5K6jNaDoAkaEE4uPWHCoO+VinNmmHB+XN1Yw0JkSjGdlDHHyl+Qoi7RgcJLqKrrCqgV4NkNec7k2oJJTDUkGxaNpPa4QcC5Wntw/1+DnzY8Gknt8UfQLnTUgLZk7/WKjKL1Qk92h9Gr4e67cLD/nLTYIs/IDnlJIrJH9skhOSJjIsiCXJEv5GtwGOigCBatNOisap6Sawg+/wZwA+qa</latexit>
<latexit sha1_base64="nxWItojed+9hgjcRzJ1TPfj65E4=">AAACyXicdVFNbxMxEHWWrxIKpMCNi0VA4hTtFqSSE5W4IPUSJNIWZaNo7J1srdrexfZCktWe+B1Ivbb/iAM/AP4EeDdRoIQ+ydbTzJvn8QzLpbAuDL+1gmvXb9y8tXW7fWf77r37nZ0HhzYrDMchz2RmjhlYlELj0Akn8Tg3CIpJPGKnb+r80Sc0VmT6vZvnOFaQajEVHJwPTTqPYoczVw7AndBcgtZCp9Wk0w17YQO6SaIV6b7+9aP/8/uH7cFkp/U1TjJeKNSOS7B2FIW5G5dgnOASq3ZcWMyBn0KKI081KLTjsmm/os98JKHTzPijHW2if1eUoKydK+aVyvdp/83Vwf/lRoWbvhqXQueFQ82XD00LSV1G61nQRBjkTs49AW6E75XyEzDAnZ9YO9b4mWdKgU7KmInKX5gKXQqHSiywqi4rsFagZ1fkGci1BUiRakw2LBpJ7XGFgDG19mD+v0bMNjwaSe3xR9BuFtpvQJdk7+WK9KP1Qg93e9GL3u67sLv/lCyxRR6TJ+Q5icge2SdvyYAMCScLckbOyUVwEHwMZsFiKQ1aq5qH5BKCL78BIkDsFw==</latexit>
Path planning <latexit sha1_base64="ZEBQs2weEuwa5gBBLnJ/YS0UO/E=">AAACyHicdVHLbhMxFHWGVwmPpnTJxiIgsYpmClLbVSt1gxCLIpG2UiaKbM9NatWPkX2HdjqaDX8BEuq2/ST+Bs8kCpTQI9k6OvfcY/ua50p6jONfneje/QcPH6097j55+uz5em/jxZG3hRMwFFZZd8KZByUNDFGigpPcAdNcwTE/O2jqx1/BeWnNFyxzGGs2M3IqBcMgTXqbKcIFVp+sYEpetmI96fXjQdyCrpJkQfp7N98b/DicbHR+ppkVhQaDQjHvR0mc47hiDqVQUHfTwkPOxBmbwShQwzT4cdXevqZvgpLRqXVhGaSt+ndHxbT3pebBqRme+n9rjfi/2qjA6c64kiYvEIyYHzQtFEVLm1HQTDoQqMpAmHAy3JWKU+aYwDCwbmrgXFitmcmqlMs6bDCTppIIWl5CXd92QOOAwO6oc6aWEWHUMwPZSkRraTLuMHCulxk8vNfJi5WM1tJk/DF02w/dbUHnZPv9guwmyw892hok7wZbn+P+/msyxxp5SV6RtyQh22SffCCHZEgEKckVuSY30ccoj86jcm6NOoueTXIL0bffmUvr5Q==</latexit>
Localization
eyL , eψ measurable
ψ̇ , Vx measurable
xdh and ėdy from Path planning for a given Maneuver planning (xdh , L/N/R-LX)
xds and xd from State reference
x̂s and x̂ from Localization
fc (x) = c0 + c1 x + c2 x2 + c3 x3 from Lane curve

Simulation
path gen. parameters mpc
parameter
online mpc Monitor plot
SFC
Limit Plant Sensing

Path planning y=Cx MPC
Reference
Manual Switch
Lateral control simulation: Path planning, Reference, Lateral Control (SFC, MPC), Sensing
· Sensing: Lane curve detection f (x) with curve coefficients ci ’s

· Path planning: Maneuver xdh , L/N/R-LX, Velocity reference trajectory ėdy , State reference xd
· Controller: SFC, MPC, ...

e.g. Lane keeping 5
-5
0 5 10 15 20 25 30 35
-1
-2
0 5 10 15 20 25 30 35
-5
-10
0 5 10 15 20 25 30 35
-5
0 5 10 15 20 25 30 35
0.5
-0.5
0 5 10 15 20 25 30 35
Computational test: SFC

e.g. Lane keeping

5
-5
0 5 10 15 20 25 30 35
-1
-2
0 5 10 15 20 25 30 35
-5
-10
0 5 10 15 20 25 30 35
-5
0 5 10 15 20 25 30 35
0.5
-0.5
0 5 10 15 20 25 30 35
Computational test: MPC

Good to Use: Lane curve generation
e.g.
A clothoid lane curve

fc (x) = c0 + c1 x + c2 x2 + c3 x3
with
   
c0 (k) A0
c1 (k) = A1  sin 2π v(k)k
   
    vmax kp
c2 (k) A2
and
c2 (k) − c2 (k − 1)
c3 (k) =
3v(k)Tc
where kp > 0 are chosen for the period vmax kp of a sinusoid, and A0 , A1 , and A2 are to be
chosen at your preference.

k+100
e.g kp = 500, A0 = 10 × 10−3 , A1 = π/180, A2 = 1 × 10−2 , v(k) = 1100 vmax .
20
18
16
14
12
10
0
10 8 6 4 2 0 -2 -4 -6 -8 -10
at t = 1.4[s]

0.01
0.005
-0.005
-0.01
0 1 2 3 4 5 6 7 8 9 10
0.02
0.01
-0.01
-0.02
0 1 2 3 4 5 6 7 8 9 10
0.01
0.005
-0.005
-0.01
0 1 2 3 4 5 6 7 8 9 10
10 -4
5
-5
0 1 2 3 4 5 6 7 8 9 10

Real Vehicle Tests
• an SUV
• the dynamic motion model with nominal coefficients
• a Mobileye camera for lane curve detection
• the multirate decentralized KF
• maneuver planning followed by the velocity reference trajectory ėdy
• LQ feedback control with an update period Tc = 10 [ms]
• steering via torque overlay Tδ
• a dSPACE µAutobox
• test on the KATRI track

• Test scenario
We presume Cαf = Cαr = 6.783 × 104 [N/rad], the nominal parameters in the
dynamic model are at a vehicle speed Vx = 70 [Km/h]
a22 = −10.9527, a23 = 212.9682, a024 = −35.8988,

a042 = 2.5012, a43 = −48.6348, a44 = −15.9056,
b021 = 106.4841, b41 = 90.4999
with a fixed look-ahead distance L = 20 [m].
The track test scenario is overtaking: the ego vehicle makes a right lane change while
passing an object vehicle at 50 [Km/h]. In the scenario, the ego vehicle speed increases
approximately from 50 [Km/h] to v d = 80 [Km/h] for overtaking the object vehicle.
The result of maneuver planning ‘R-LX’ is given to initiate right lane change.
Velocity reference trajectory design with t∗lx
= 4 [sec], an assumption of lane width
ew
y = 3.5, an acceleration limit v̇acc,max = 1 [m/s2 ], and the design parameter p = 1.

-1
-2
25 30 35 40 45
0.05
-0.05
25 30 35 40 45
10 -5
10
0
-320
-5
25 30 35 40 45 -322
10 -6
1 -324
0.5
-326
0
-328
-0.5
-1 -330
25 30 35 40 45 -1450 -1400 -1350 -1300 -1250 -1200
(a) (b)
Track test: (a) lane curve; (b) GPS

-2
-4
25 30 35 40 45
-2
-4
25 30 35 40 45
25 30 35 40 45
LXC trigger and etlc

y

-2
-4
25 30 35 40 45
0.5
0 70
-0.5
25 30 35 40 45 65
0.06
60
0.04
0.02
55
25 30 35 40 45
0
-0.02
25 30 35 40 45 10
0.05 5
0
0
-5
-0.05 -10
25 30 35 40 45 25 30 35 40 45
(c) (d)
Track test: (c) states; (d) speed and steering wheel

Good to note:
• Lane curve switching (from the origin to the target lane) occurs during lane change
• Lane curve switching more likely causes jerk, unless lane curve estimation fˆ(x) is applied:
fˆ(x) = ĉ0 + ĉ1 x + ĉ2 x2 + ĉ3 x3 in {xyz}
• State estimation x̂ in lane changing: x̂1 = êtlc

yL

Let us wrap up the lecture...
Speed vd
Reference control
Vehicle Camera
generation d
Lateral Radar
control LiDAR
Path IMU
GPS
planning ..
.
<latexit sha1_base64="OfGX/54f8kR3X3GJBjzV4tiILnI=">AAAC5HicdVHLTtwwFPWktIX0NZQlG4tRJVajDJvSHVI3XU6lTkGajEbXzk3GwnYi22kZonxCV1W7gy/pFv6hf4MTIl4DV7J0dM/x8fW5rJDCuij63wuerT1/8XJ9I3z1+s3bd/3N999tXhqOE57L3BwxsCiFxokTTuJRYRAUk3jIjj83/OEPNFbk+ptbFjhTkGmRCg7Ot+b9KGaYCV2lsrQLiamrx+AWNI7DQoLWQmcx6uQOPe8PomHUFl0Fow4MSFfj+WbvLE5yXirUjkuwdjqKCjerwDjBJdZhXFosgB9DhlMPNSi0s6r9Wk0/+E5C09z4ox1tu3dvVKCsXSrmlcoPbh9yTfMxblq6dH9WCV2UDjW/figtJXU5bXKiiTDInVx6ANwIPyvlCzDAnU8zjDX+5LlS4LOJmairLkbhUIlTrOv7CmwUTY5P8AzkjQVIkWlMVixaSePxhIAxdePB/H+NOFnxaCWNx60g9AsdPVzfKpjsDT8No697g4PdbrPrZJvskF0yIh/JAflCxmRCOPlL/pELchlkwa/gd/DnWhr0ujtb5F4F51eWI/MP</latexit>
<latexit sha1_base64="wcCxrWwadjxSj6kYHm44/1MWxZ4=">AAADCXicdVHNbhMxEHaWv7L8pXDkYhGQOEW7Ban0VlQkQAIplKatlI0i2zubWLW9K9tbCKvtC/AMPAQ31CtPwY0rPAX2ZhVaQkey5tPM58+eb2ghuLFR9KMTXLp85eq1tevhjZu3bt/prt/dN3mpGQxZLnJ9SIkBwRUMLbcCDgsNRFIBB/Rox/cPjkEbnqs9Oy9gLMlU8YwzYl1p0t1LKEy5qjJRmpmAzNbhDpGgCU6ScJekRHvwhr94vuvB67dDn14O3vt0cpIcp7k1YQIqPSMx6faiftQEXgVxC3qojcFkvfMlSXNWSlCWCWLMKI4KO66ItpwJqMOkNFAQdkSmMHJQuS+acdWMX+NHrpLiLNfuKIub6tkbFZHGzCV1TEnszPzb88X/9UalzZ6NK66K0oJii4eyUmCbY+8lTrkGZsXcAcI0d3/FbEY0YdY5HiYKPrBcSuK8SSivq9ZqbkHyT1DX5xngGd7HC/qUiKUEEXyqIF2RaChe4wICpXKpQd28mn9c0WgoXuMvIWwWutUEXoDNpy3YipcL3d/ox0/60buN3vbDdrVr6D56gB6jGG2ibfQKDdAQMXSKfqJf6HfwOfgafAtOF9Sg0965h85F8P0P2/r/Bg==</latexit>
ENVIR Localization
Perception
modeling
<latexit sha1_base64="DVmcIa7Yjhu64i3cuzd+uGQy5Bc=">AAAC5nicdVHLbtQwFPWEVwmvKSzZWIyQuqqSbuiyEkKCDSqImVaajEa2c5Ox6kdk3wBDlF9gh7pC4kfYwjfwNzhpVNoOvZKlo3uOj33P5ZWSHpPkzyi6cfPW7Ttbd+N79x88fDTefjzztnYCpsIq644586CkgSlKVHBcOWCaKzjiJy87/ugjOC+t+YDrChaalUYWUjAMreU4zTiU0jSFqv1KQYHtq7ezN+9plsXa5p1tmYHJL/DL8STZTfqimyAdwIQMdbjcHn3PcitqDQaFYt7P06TCRcMcSqGgjbPaQ8XECSthHqBhGvyi6Wdr6fPQyWlhXTgGad+9eKNh2vu15kGpGa78Va5r/o+b11jsLxppqhrBiLOHilpRtLQLiubSgUC1DoAJJ8NfqVgxxwSGOOPMwCdhtWYhm4zLthlylAhafoG2vayATtHleA3PmTq3YEqWBvINi17SeVwj4Fyfe/Awr5OfNzx6SefxTxCHhaZX17cJZnu7acDv9iYHO8Nqt8hT8ozskJS8IAfkNTkkUyLIKflJfpHf0Sr6Gn2LTs+k0Wi484RcqujHX5MJ8zU=</latexit>
The vehicle motion control framework, we’ve been using . . .

Good to Recall:
• Vehicle motion models: Longitudinal & Lateral

• Sensors’ principle: TOF, TOA, Doppler, Vision
• Actuation models
• Multirate decentralized Kalman filter
• Multirate sensor data fusion
• Maneuver planning → Velocity reference trajectory → State reference → Feedback
control
• Implementation

Path Planning & Feedback Control

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Path Planning & Feedback Control

Uploaded by

Copyright:

Available Formats

HYU ELE6033 Autonomous Driving Vehicle Control System, Spring 2019 S.-H.

Goals of this lecture:

Good to recall: Vehicle driving control framework

Path Planning & Feedback Control VII-1

Good to recall: Occupancy Grid driving environment modeling

Simply determines the safety of

· increasing / decreasing speed

Path Planning & Feedback Control VII-2

Given the results of Occupancy grid environment modeling,

considering driver’s intention,

determine safe longitudinal/lateral maneuvering:

· Changing vehicle speeds:

Path Planning & Feedback Control VII-3

Good to Note: Vehicle motion models

Input to longitudinal motion, v

Input to lateral motion, δ ⇐ ψ̇

Path Planning & Feedback Control VII-4

Good to Recall: Vehicle motion models

• Longitudinal motion model:

Assume a feedback controller ẍd = Ks (v d − v) is applied and the resulting CLP is

Path Planning & Feedback Control VII-5

Alternatively, a simple longitudinal motion model

DT equivalent for a sample period Tc

x(k + 1) = x(k) + Tc v(k) (7.3)

Combining (7.2) and (7.3) leads to

Note: A CLP actuation is required for speed s.t. v d \v → 1

Path Planning & Feedback Control VII-6

• The lateral vehicle motion model in terms of the state vector

x(k + 1) = Φx(k) + Γu(k) + Γϕ ϕ(k)

Note: A CLP actuation is also required for steering s.t. δ d \δ → 1

Path Planning & Feedback Control VII-7

Path Planning & Feedback Control VII-8

• Path planning: a sequence of desired poses

Path Planning & Feedback Control VII-9

Longitudinal Reference Trajectory Planning

Given a result of maneuver planning xd

v d = v + (xdh − xh )/Th (7.6)

Headway distance xh = vTh

Path Planning & Feedback Control VII-10

Path Planning & Feedback Control VII-11

Lateral Reference Trajectory Planning

Utilize the result of maneuver planning: L/N/R-LX

An arbitrary position on the original lane → Target lane center

Path Planning & Feedback Control VII-12

Target Lane Curve:

fc (x) = c0 + c1 x + c2 x2 + c3 x3 is the original lane curve detected by the camera,

Path Planning & Feedback Control VII-13

Velocity Reference Trajectory:

Path Planning & Feedback Control VII-14

• Velocity reference trajectory

with a non-empty set defined by

for some ν > 0,  > 0, and e∗y > 0.

Path Planning & Feedback Control VII-15

Path Planning & Feedback Control VII-16

Sa = {(ey , ėdy ) | ėdy = vacc (tlc ), tlc < t∗lc }.

The set of deceleration trajectories is defined as

Sd = {(ey , ėdy ) | ėdy = vdec (ey (t)), t > t∗lc }

Path Planning & Feedback Control VII-17

for a pre-specified limit of lateral acceleration, v̇acc,max .

Path Planning & Feedback Control VII-18

Figure 7.1: Lateral velocity profile for acceleration

Path Planning & Feedback Control VII-19

A simple design example of a deceleration profile to the target lane center is

for some ν > 0, > 0, and e∗y > 0.

Utilize the linear approximation in S < Sd . . .

(ey , ėdy ) ∈ Sa → (ey , ėdy ) ∈ Sd → (ey , ėdy ) ∈ S .