z 1
, c b,1 =
z 9
z 10 z 11 z 12 z 2
,
η 1 = [z 5 z 6 ] ′ ,
p − 2 (u; η 2 ) :
c a,2 =
z 9 z 10 z 11 + π
−z 12
z 3
, c b,2 =
x g y g θ g + π
− 1 l tan δ g z 4
,
η 2 = [z 7 z 8 ] ′ ,
where the freevariables are
z i
,i = 1, . . . 12
, and they form the ve torz ∈ Z
with
Z := [− ˙κ M , ˙κ M ] 4 × R 4 + × R 2 × [0, 2π) × [−κ M , κ M ]
whi h is atwelve-dimensional sear h spa e.
Case
h = 2
with{p − 1 (u; η 1 ), p + 2 (u; η 2 )}
(one ba kward movement plus a aforward one): similarly to the previous ase, all the parameters an be set as
follows
p − 1 (u; η 1 ) :
c a,1 =
x s y s
θ s + π
− 1 l tan δ s z 1
, c b,1 =
z 9 z 10
z 11 z 12 z 2
,
η 1 = [z 5 z 6 ] ′ ,
p + 2 (u; η 2 ) :
c a,2 =
z 9 z 10 z 11 + π
−z 12
z 3
, c b,2 =
x g y g θ g 1 l tan δ g
z 4
,
η 2 = [z 7 z 8 ] ′ .
When
h > 2
, the spline parameters an be set up similarly as in thepre-sented ases.Table2.1reportsthedimension andstru tureofthesear hspa e
Z
as a fun tion ofh
. In parti ular, when the parking is done withh
splines,the dimension ofthe sear h spa eis
8h − 4
: every added splinein reases of 8the dimension of
Z
.Remark The proposed approximation s heme repla es ea h path
Γ i
ofse-quen e
{Γ 1 , Γ 2 , . . . , Γ h }
with only oneη 3
-spline to avoid ex essive in reasingof the dimension of the sear h spa e
Z
. Yet, it would be possible within thesameproposedframeworkto improvetheapproximationbyusingtwoor more
η 3
-splinesfor ea hΓ i
.2.1.3 Setting up the multi-optimization
In this se tion the multi-optimization of problem 1 is dealt with the
sub-stitution of the innite-dimensional spa e
F h
with the nite-dimensional pa-rameter spa eZ
introdu ed in the previous se tion. This orresponds to do the sear hing for multi-optimization on the sequen es of simpliedη 3
-splines{p 1 (u; η 1 ), p 2 (u; η 2 ), . . . , p h (u; η h )}
instead of the sequen es ofG 3
-pathsin-trodu ed in subse tion 2.1.2.
Thethreeindexestobeminimizedusingthestandardweightedsummethod
[34℄ are( f. problem 1):the maximum value ofthe urvature modulus onthe
h
splines,themaximum valueofthe absolutevalueofthe urvaturederivative(with respe t to the ar length) on the
h
splines, and the total length of theh
splines.Theseindexesarerespe tively denotedbyκ max
,˙κ max
, ands tot
anddependontheparameterve tor
z ∈ Z
.They anbedeterminedasfollows(theh
dim(Z) Z
1 4
[− ˙κ M , ˙κ M ] 2 × R 2 +
2 12
[− ˙κ M , ˙κ M ] 4 × R 4 + × R 2 × [0, 2π) × [−κ M , κ M ]
3 20
[− ˙κ M , ˙κ M ] 6 × R 6 + × R 4 × [0, 2π) 2 × [−κ M , κ M ] 2
.
.
.
.
.
.
.
.
.
h 8h − 4 [− ˙κ M , ˙κ M ] 2h × R 2h + × R 2(h−1) × [0, 2π) h−1 × [−κ M , κ M ] h−1
Table2.1: Dimensionand stru tureof the sear hspa e
Z
.dependen ies on
z
are omitted for simpli ity andp i (u; η i ) ≡ [p x,i (u) p y,i (u)] ′
,i = 1, . . . , h
, f.(2.5)):κ max .
= max
i=1,...h κ max,i ,
(2.7)where (
i = 1, . . . , h
)κ max,i .
= max
u∈[0,1] |κ i (u)| ,
and
κ i (u) = ˙p x,i (u)¨ p y,i (u) − ¨ p x,i (u) ˙p y,i (u) ( ˙p 2 x,i (u) + ˙p 2 y,i (u)) 3 2 ,
isthe s alar urvatureof spline
p i (u; η i )
;˙κ max .
= max
i=1,...h ˙κ max,i ,
(2.8)where (
i = 1, . . . , h
)˙κ max,i .
= max
u∈[0,1]
dκ i ds (u)
,
and
dκ i
ds (u) = ˙p x,i
...p y,i −
...p x,i ˙p y,i
( ˙p 2 x,i + ˙p 2 y,i ) 2 − 3 ( ˙p x,i p ¨ y,i − ¨ p x,i ˙p y,i )( ˙p x,i p ¨ x,i + ˙p y,i p ¨ y,i ) ( ˙p 2 x,i + ˙p 2 y,i ) 3 ,
is the derivative of the urvature of spline
p i (u; η i )
with respe t to the arlength (for brevity the dependen y on
u
is omitted in the right side of theabove relation);
s tot .
=
h
X
i
s tot,i ,
(2.9)where
s tot,i .
= Z 1
0
[ ˙p 2 x,i (ξ) + ˙p 2 y,i (ξ)] 1/2 dξ .
The onstraint of obsta le avoidan e is dealt with the on ept of o upan y
span of the vehi lealong apath planning:
Denition 4 The o upan y span of the vehi le along the spline sequen e
{p 1 , p 2 , . . . , p h }
is the set dened asS .
=
n
[
i=1
S i ,
where
S i .
= {p ∈ P : p ∈ A(q) , q 1 = p x,i (u), q 2 = p y,i (u), q 3 = arg( ˙p x,i (u) + j ˙p y,i (u)) , u ∈ [0, 1]} .
Note that the o upan y span depends on
z ∈ Z
, i.e.S ≡ S(z)
. Denethe obsta le region
O
as the union of all the obsta les, i.e.O .
= ∪ n i=1 B i
andthe vehi le avoids all the obsta les along a path planning if and only if the
interse tionof
S(z)
andO
is the emptyset ( f. onstraint (2.11) below).Nowthenonlinear onstrainedmultiobje tiveoptimizationproblemforthe
geometri planning of autonomousparking an bestated asfollows:
Problem 2 (Multi-optimization of a sequen e of
η 3
-splines for thesmoothparkingproblem)Giventhenumber
h
ofpaths, onsidertheparam-eterspa e
Z
thatdenesthesequen es{p + 1 , p − 2 , . . . , p h }
(or{p − 1 , p + 2 , . . . , p h }
)a ordingtothe interpolatings heme exposed in se tion 2.1.3.Then,theposed
problem is(
λ 1 , λ 2 , λ 3 ≥ 0
andλ 1 + λ 2 + λ 3 = 1
):min z∈Z λ 1 κ max (z) + λ 2 ˙κ max (z) + λ 3 s tot (z) ,
(2.10)subje t to
S(z) ∩ O = ∅ ,
(2.11)κ max (z) ≤ κ M ,
(2.12)˙κ max (z) ≤ ˙κ M .
(2.13)The oe ients
λ 1
,λ 2
, andλ 3
of the omposite index to be minimizedin(2.10) anbefreely hoseninordertoweightthesmoothnessoftheresulting
maneuver paths(whi hisrelatedto lowvalues ofboth
κ max
and˙κ max
)versusthe minimization of
s tot
, the total lengthof the parkingpaths.Remark Note that the possible onstraint of avoiding steering at vehi le's
standstilldoesnot appearin the onstraints (2.12)-(2.13)be auseitisplainly
enfor edbyproperassignmentofthegeometri interpolating onditionsonthe
η 3
-splines.Obsta le avoidan e onstraint (2.11) an be equivalently redu ed to an
equality onstraint by omputing the maximal ollision area of the vehi le
alongthe splinesequen e:
m a
= max .
i=1,...,h
m a
i ,
(2.14)m a
i .
= max
u∈[0,1] {
area(A(q) ∩ O) : q 1 = p x,i (u), q 2 = p y,i (u), q 3 = arg( ˙p x,i (u) + j ˙p y,i (u))} .
Constraint (2.11) istherefore equivalent to
m a
(z) = 0 ,
andin su hawayproblem2be omesa onstrainedminimizationproblemfor
whi hastandardpenaltymethod[35℄ an takeintoa ountallthe onstraints
so asto redu e the whole multi-optimization to the minimization of justone
index.Inareal-time s enarioforautonomousparking, fastlo alminimization
algorithms an be then implemented tosolve problem2provided thatthe
fol-lowing data isreadilyavailable:(1)the number
h
of splines;(2)the maneuversequen e to prefer
{p + 1 , p − 2 , . . . , p h }
or{p − 1 , p + 2 , . . . , p h }
; (3) an initialesti-mate of the parameter ve tor
z
. Reasonably, this data an be determined by usinglook-uptablesthat anbe onstru tedo-linebyextensiveoptimizationssu h asthosebasedon methodsof sto hasti global multi-obje tive
optimiza-tion [36℄.
2.1.4 Simulation results
Example 1:Firstly,an exampleof garage parking maneuver in a onstrained
environment is onsidered for a standard ompa t ar with wheelbase and
maximum steering angle of the front wheels
l = 2.3
m andδ M = 0.464
rad.Hen e,themaximum urvatureofthe arpathsis
κ M = 1 l tan δ M = 0.218
m−1
.Theallowed maximum absolutevalue ofthe urvaturederivative with respe t
to the ar length is hosen as
˙κ M = 2.50
m−2
. The origin of the Cartesianplane
P
is hosen to be inside the parking lot that the ar hasto rea h. Thearhasstart onguration
q s = [x s y s θ s δ s ] ′ = [7 − 6 3π/4 0] ′ ,
andthenalgoal onguration,whi h orrespondstoafront arparking mode
(i.e. the ar anonly rea h the goal onguration with aforward nalmotion
be auseof the surroundingobsta les ( f. gure2.6), is
q g = [x g y g θ g δ g ] = [0.7 0 π 0] ′ .
The multi-optimizations for solving this parking problem are set up with
weights
λ 1 = 0.5
,λ 2 = 0.2
, andλ 3 = 0.3
. All the possible spline sequen esto be onsidered up to three splines are the following (the arguments of the
η 3
-splinesareomittedfor ompa tness):h = 1 : {p + 1 }, {p − 1 };
h = 2 : {p + 1 , p − 2 }, {p − 1 , p + 2 };
h = 3 : {p + 1 , p − 2 , p + 3 }, {p − 1 , p + 2 , p − 3 }.
The sequen es
{p − 1 }
,{p + 1 , p − 2 }
,{p − 1 , p + 2 , p − 3 }
have to be dis arded due tothe fa t that the last spline has to be overed with a ar's forward
move-ment(front arparking).Hen ethetopologi allypossiblesequen esare:
{p + 1 }
,{p − 1 , p + 2 }
,{p + 1 , p − 2 , p + 3 }
.Parkingwith{p + 1 }
isnotfeasiblebe ausethemulti-optimization (2.10) fails to satisfy all the required onstraints (2.11)-(2.13).
Instead,both sequen es
{p − 1 , p + 2 }
and{p + 1 , p − 2 , p + 3 }
leadto feasible parkingmaneuvers.
Forthe two splines maneuver the multi-optimizationof
{p − 1 , p + 2 }
leadstoa Pareto optimal solution
¯
z ∈ Z = [−2.5, 2.5] 4 × R 4 + × R 2 × [0, 2π) × [−0.218, 0.218] ,
for whi h
κ max (¯ z) = 0.143
m−1
,˙κ max (¯ z) = 0.260
m−2
,s tot (¯ z) = 22.8
m. Thissolution is depi ted with graphi simulation in gure 2.6. Plots of urvature
and urvaturederivative arereported in gure2.7.
For the three splines maneuver the multi-optimization of
{p + 1 , p − 2 , p + 3 }
leads to solution
¯
z ∈ Z = [−2.5, 2.5] 6 × R 6 + × R 4 × [0, 2π) 2 × [−0.218, 0.218] 2 ,
for whi h
κ max (¯ z) = 0.168
m− 1
,˙κ max (¯ z) = 0.704
m− 2
,s tot (¯ z) = 25
m. Thissolution is depi ted gure 2.8, while urvature and urvature derivative are
reported in gure2.9.
Example 2:Asse ondexample,aparallelparking maneuver ina onstrained
environment is onsidered with the same data for the dynami model andfor
the onstraints, given for the pre edent example. The ar has start and nal
ongurations
q s = [x s y s θ s δ s ] ′ = [−2.5 2.5 π 0] ′ ,
−5 0 5 10 15
−10
−5 0 5
[m]
[m ]
PSfragrepla ements
p − 1
p + 2
Figure2.6:Optimalparkingwithtwo-splinemaneuver
{p − 1 , p + 2 }
inexample1.0 5 10 15 20
−0.1
−0.05 0 0.05 0.1 0.15
[m]
[1/m ]
0 5 10 15 20
−0.2
−0.1 0 0.1 0.2 0.3
[m]
[1/(m 2 )]
Figure2.7: Plots of urvatureand urvature derivativeasfun tionsof the ar
length alongthe entire optimal splinemaneuver
{p − 1 , p + 2 }
in example 1.−4 −2 0 2 4 6 8 10 12
−8
−6
−4
−2 0 2 4 6 8
[m]
[m ]
PSfragrepla ements
p + 1
p − 2
p + 3
Figure 2.8: Optimal parking with three-spline maneuver
{p + 1 , p − 2 , p + 3 }
inex-ample 1.
0 5 10 15 20
−0.1
−0.08
−0.06
−0.04
−0.02 0 0.02 0.04 0.06 0.08 0.1
[m]
[1/m ]
0 5 10 15 20
−0.08
−0.06
−0.04
−0.02 0 0.02 0.04 0.06 0.08
[m]
[1/(m*s)]
Figure2.9: Plots of urvatureand urvature derivativeasfun tionsof the ar
length alongthe entire optimal splinemaneuver
{p + 1 , p − 2 , p + 3 }
in example 1.−10 −8 −6 −4 −2 0 2 4 6
−4
−2 0 2 4 6
[m]
[m ]
PSfragrepla ements
p + 1
p − 2
p + 3
Figure2.10: Optimal parkingwith three-spline maneuver
{p + 1 , p − 2 , p + 3 }
inex-ample 2.
0 5 10 15
−0.15
−0.1
−0.05 0 0.05 0.1 0.15
[m]
[1/m ]
0 5 10 15
−0.4
−0.2 0 0.2 0.4 0.6 0.8
[m]
[1/(m 2 )]
Figure2.11:Plotsof urvatureand urvaturederivativeasfun tionsofthear
length alongthe entire optimal splinemaneuver
{p + 1 , p − 2 , p + 3 }
in example 2.and
q g = [x g y g θ g δ g ] = [0 0 π 0] ′ ,
respe tively. Setting
λ 1 = 0.3
,λ 2 = 0.2
, andλ 3 = 0.5
, sequen e{p + 1 , p − 2 , p + 3 }
isthe rstone results tobe feasible.Theoptimal solution
¯
z ∈ Z = [−2.5, 2.5] 6 × R 6 + × R 4 × [0, 2π) 2 × [−0.218, 0.218] 2 .
forthesequen e
{p + 1 , p − 2 , p + 3 }
givestheresults:κ max (¯ z) = 0.165
m−1
,˙κ max (¯ z) = 0.551
m−2
,s tot (¯ z) = 17.9
m.Thissolutionisdepi ted with graphi simulationin gure 2.10. Plots of urvature and urvature derivative arereported in
g-ure 2.11.
2.2 Path generation for a tru k and trailer vehi le
Inthis se tionamethodforthe smoothpath generationof atru kandtrailer
vehi le is presented. The advantages and potentialities in a hieving full or
partialautonomyintheguidan eofautomatedvehi lesareastrongmotivation
to improve urrent te hnologies and methodologies. Fo using on the motion
automation of arti ulated vehi les, the present work addresses the need to
generate high quality drive paths for an automated tru k and trailer vehi le.
This need an arisein a variety of appli ations (e.g. in industry, agri ulture,
mining, et . [37,38℄).
Considering the usual kinemati model of a tru k and trailer vehi le, this
se tionpresentsanewtraje torygenerationmethodin whi hthe feedforward
(i.e. open-loop) ontrol an steer the vehi le from an initial onguration to
a nalone, while permitting freeshaping ofthe trailer path onne ting these
ongurations.Withthismethod,thefeedforward ontrols,i.e.thetru k
velo -ityandthesteeringangleofthefrontwheels,aresmooth
C 1
-signals,theinitialand nal ongurations arearbitrary and the onne ting path ismodeled by
using anew urve primitive,the
η 4
-spline.The problem of nonholonomi traje tory generation for an
n
-trailerve-hi le (i.e. an arti ulated vehi le onsisting of a tru k towing
n
trailers) wasonsidered and solved in [39℄ byusing three distin t lasses of ontrol inputs:
sinusoids,pie e-wise onstants,andpolynomials.Thismethodrelieson,by
o-ordinatetransformations,the onversionofthe
n
-trailersystemintoaGoursatnormal form and then into the orresponding hained form [40℄ for whi h the
ontrollabilityproblem(i.e.theproblemofsteeringbetween system
ongura-tions)issolved byfeedforward ontrol.Then, byreversingthetransformations
the a tual system inputs are obtained; however in this reversing singularities
mayappearsothatthe desired ontrol isnotguaranteed tobeobtained in all
planning ases. Moreover, the method does not a ount for any exibility in
dire t shaping or modeling the Cartesianpaths ofthe trailersand the tru k.
Thisse tionproposes apath generationmethodologyfor the smooth
feed-forward ontrol ofthe tru k andtrailer vehi le withinthe frameworkof
path-velo ity de omposition [3℄. A result presented in the following subse tions
(proposition 4) shows that the path generated by the vehi le trailer is a
G 4
-path [32,33℄ (i.e. a path whi h has fourth-order geometri ontinuity) if and
only if, ex luding kinemati singularities, the velo ity and the steering
fun -tions ofthe tru kare
C 1
-fun tions.Fourth-order geometri ontinuity a ounts for the ontinuity along the
urveofthepathitself,theunittangentve tor,the urvature,andtherstand
se ond order urvature derivatives with respe t to the ar length. Therefore,
whenpursuing thesmoothfeedforward ontrolofthe arti ulated vehi le,path
planning an be pertinently done with
G 4
-paths. This naturally leads to thepolynomial
G 4
-interpolatingproblem onthe Cartesianplane.These tionpresentsa ompletesolutiontothisinterpolatingproblem.The
solution is the
η 4
-spline whi h is a ninth-order polynomial urve interpolat-ing Cartesian points with asso iated arbitraryG 4
-data (unit tangent ve tor,urvature, rstandse ondderivativesof urvature).The
η 4
-splinegeneralizes theη 2
-splineandη 3
-splinepreviouslypresentedinthepre edent se tions.Theη 4
-splineis a urve primitive that depends on setof 8 parameters, whi h an befreely hosen tomodify the path shape without hanging theinterpolationonditions at the path endpoints.
2.2.1 Smooth feedforward ontrol of the tru k and trailer
ve-hi le
Consideratru kantrailervehi lewiththetrailersupposedtobejoinedtothe
tru k at the midpoint ofits rear axle. See gure 2.12 where a s hemati plan
view of the arti ulated vehi le on a Cartesian frame
{x, y}
is depi ted. WePSfragrepla ements
x 1 y 1
x y
θ 1
θ 0 v d 1
d 0 δ
Figure2.12: S hemati of atru kand trailer vehi le.
indi atewith ouple
(x 1 , y 1 )
the oordinatesoftheaxlemidpointofthetrailer andwithθ 1
itsorientationanglewithrespe ttothex
axis.Thetru ka tuatesthe motion by the velo ity
v
of the rear wheels and by the steering angleδ
of the front wheels. The distan e between the front axle and the rear axle of
thetru kis
d 0
,whereasthe distan ebetween thetrailer axleandtherearaxleof the tru k is
d 1
. Withthe usual modeling assumptions of rigid bodyof the tru k andthe trailerand of no-slippage of the wheels,the following nonlinearkinemati modelof the arti ulated vehi le an bededu ed
˙x 1 = v cos(θ 0 − θ 1 ) cos θ 1
˙y 1 = v cos(θ 0 − θ 1 ) sin θ 1
˙θ 0 = d v
0 tan δ
˙θ 1 = d v 1 sin(θ 0 − θ 1 ) .
(2.15)
We saw in the pre edent se tions that in this ontext it is onvenient to use
the extended state of model(2.15), or onguration of the arti ulated vehi le,
whi h isdened asthe state plusthe inputs and their derivatives:
(x 1 , y 1 , θ 0 , θ 1 , v, ˙v, δ, ˙δ) .
(2.16)The following denition willbeused alongthis se tion:
Denition 5 (
G k
- urve,k ≥ 2
) A urvep(u)
, withu ∈ [u 0 , u 1 ]
, hask
-th order geometri ontinuity, and we say
p(u)
is aG k
- urve, ifp(u)
is aG k−1
- urve,du d k k p (u) ∈ P C([u 0 , u 1 ])
, and the(k − 2)
-th order derivative ofthe urvature withrespe t to the ar length is ontinuous along the urve, i.e.
d k−2
ds k−2 κ(u) ∈ C 0 ([u 0 , u 1 ])
.The
G k
- ontinuity of urves an be naturally extendedto Cartesianpaths as follows:Denition 6 (
G k
-paths) Agivenset ofpointsof aCartesianplaneisaG k
-path if there existsa parametri
G k
- urve whose image is the given path.We stated abovethat, in orderto obtainasmooth vehi lemotion,inputs
v(t)
and
δ(t)
must beC 1
-fun tions. Su h a ontinuity of these vehi le inputs is linked tothe fourth-ordergeometri ontinuityofthe trailerpath asstated bythe following proposition.
Proposition 4 Assignany
t f > 0
. For model (2.15), onsider smooth inputsv(t), δ(t) ∈ C 1 ([0, t f ])
,withv(t) 6= 0
,|δ(t)| < π 2
andinitial onditionssu hthat|θ 0 (t) − θ 1 (t)| < π 2
,∀t ∈ [0, t f ]
. Then the path generated by model (2.15), i.e."
x 1
y 1
#
([0, t f ])
, isaG 4
-path.Conversely,givenaG 4
-pathΓ
thereexistsmoothinputs
v(t), δ(t) ∈ C 1 ([0, t f ])
withv(t) 6= 0
,|δ(t)| < π 2
,∀t ∈ [0, t f ]
and initialonditions for whi h
|θ 0 (t) − θ 1 (t)| < π 2
,∀t ∈ [0, t f ]
and the path generated bysystem (2.15) oin ides with the given
Γ
, i.e."
x 1 y 1
#
([0, t f ]) ≡ Γ
.Proof. Letusdemonstratethe rstpartoftheproposition. Thesolutionofthe
dierential equations (2.15) leads to traje tory
[x 1 (t) y 1 (t)] ′
,t ∈ [0, t f ]
whi hisaregularCartesian urve.Indeed,itsderivative
[ ˙x 1 (t) ˙y 1 (t)] T
nevervanishesover
[0, t f ]
be ausev(t) 6= 0
and|θ 0 (t) − θ 1 (t)| < π 2
,∀t ∈ [0, t f ]
.The unittangent ve torof urve
[x 1 (t) y 1 (t)] ′
an beexpressed asτ (t) = [ ˙x 1 (t) ˙y 1 (t)] ′
p ˙x 2 1 (t) + ˙y 2 1 (t) = sgn(v(t))
"
cos θ 1 (t) sin θ 1 (t)
#
.
(2.17)Hen e, the unit tangent ve tor
τ
is ontinuous over the trailer urve be auseθ 1 (t)
is ontinuous in[0, t f ]
.As known from the theoryof planar urves [41℄, the s alar urvature
κ
isgiven by the derivative of the tangent angle
θ 1
with respe tto the ar lengths
, wheres = R t
0 ( ˙x 2 1 (ξ) + ˙y 2 1 (ξ)) 1 2 dξ
. It an beexpressed asfollowsκ = dθ 1
ds = dθ 1 dt
1
ds dt
= ˙θ 1 1 ( ˙x 2 1 + ˙y 1 2 ) 1 2
= v d 1
sin(θ 0 − θ 1 ) 1
|v| cos(θ 0 − θ 1 )
= sgn(v) 1
d 1 tan(θ 0 − θ 1 ) .
(2.18)For the ontinuity of the state variables
θ 0
andθ 1
, urvatureκ
is ontinuousin
[0, t f ]
too.The derivative of the s alar urvatureκ
isgivenbydκ
ds = 1
d 1 cos 3 (θ 0 − θ 1 )
1
d 0 tan δ − 1
d 1 sin(θ 0 − θ 1 )
.
(2.19)The urvaturederivative
dκ
ds
isthen ontinuousalong the urve be auseθ 0
,θ 1
and
δ
are ontinuous in[0, t f ]
. Finally, the se ond derivative of the urvaturean beexpressed asfollows
d 2 κ
ds 2 = ˙δ
|v|d 0 d 1 cos 2 δ cos 4 (θ 0 − θ 1 ) − sgn(v)
1
d 0 tan δ − d 1 1 sin(θ 0 − θ 1 ) d 2 1 cos 3 (θ 0 − θ 1 )
+sgn(v) 3 h
1
d 0 tan δ − d 1 1 sin(θ 0 − θ 1 ) i 2
sin(θ 0 − θ 1 ) d 1 cos 5 (θ 0 − θ 1 ) .
(2.20)
Again, from the ontinuity of the state variables
θ 0
andθ 1
and from thehy-pothesis
v, δ ∈ C 1 ([0, t f ])
, the se ondderivative of the urvaturewith respe tto the ar lengthis ontinuousin
[0, t f ]
. Thisshowsthat urve[x 1 (t) y 1 (t)] ′
isa
G 4
- urve,hen ethe image"
x 1
y 1
#
([0, t f ])
isaG 4
-path.In order to prove the onverse part of the proposition, onsider the
G 4
- urve
p(s)
,wheres
isthear lengthonΓ
andp([0, s f ]) ≡ Γ
withs f
beingthetotal ar length of
Γ
. We hoosethe following initial onditions
"
x 1 (0) y 1 (0)
#
= p(0)
θ 0 (0) = arg dp ds (0) + arctan(d 1 κ(0)) θ 1 (0) = arg dp ds (0) ,
(2.21)
where
dp
ds (s)
andκ(s)
are the unit tangent ve tor and the urvature ofp(s)
respe tively.
Also onsider any
v 1 (t) ∈ C 1 ([0, t f ])
su hthatv 1 (t) > 0
,∀t ∈ [0, t f ]
andZ t f
0
v 1 (ξ)dξ = s f .
Then dene the ontrol inputs as
v(t) = v 1 (t) 1 + d 2 1 κ 2 (s) 1 2 s= R t
0 v 1 (ξ)dξ
(2.22)
and
δ(t) = arctan
"
d 0 κ
(1 + d 2 1 κ 2 ) 1 2 + d 0 d 1 dκ ds
(1 + d 2 1 κ 2 ) 3 2
# s= R t
0 v 1 (ξ)dξ
.
(2.23)Obviously,
v(t) 6= 0
,∀t ∈ [0, t f ]
andv(t) ∈ C 1 ([0, t f ])
be ausev 1 ∈ C 1 ([0, t f ])
and
κ ∈ C 1 ([0, s f ])
. Moreover,|δ(t)| < π 2
,∀t ∈ [0, t f ]
andδ(t) ∈ C 1 ([0, t f ])
be ause
κ ∈ C 2 ([0, s f ])
(indeedp(s)
is aG 4
- urve).Expli it solutions ofsystem(2.15) an begiven for
θ 0
andθ 1
asfollows:θ 0 (t) = θ 0 (0) + Z t
0
v(r)
d 0 tan δ(r) dr ,
(2.24)θ 1 (t) = θ 0 (t) − arctan [d 1 κ(s)]| s= R t
0 v 1 (ξ)dξ .
(2.25)Straightforwardly, solution(2.24) satisesthethird equationofsystem(2.15).
By expli it derivation of solution (2.25) and some omputations the fourth
equation ofsystem(2.15) isalso veried and
˙θ 1 (t) = v 1 (t)κ(s)| s= R t
0 v 1 (ξ)dξ , t ∈ [0, t f ] .
(2.26)From (2.25) evidently the inequality
|θ 0 (t) − θ 1 (t)| < π 2
,∀t ∈ [0, t f ]
follows.The lastpoint to prove is
"
x 1 (t) y 1 (t)
#
= p(s)| s= R 0 t v 1 (ξ) dξ , t ∈ [0, t f ] .
(2.27)First notethat
θ 1 (t) = arg dp ds s= R t
0 v 1 (ξ) dξ
,
(2.28)and re allthat
κ = d
ds (arg τ ) ,
(2.29)be ause
θ 1 (0) = arg dp ds (0)
( f. onditions (2.21)) and the derivatives of both sidesof (2.28) oin ide( f. (2.29) and(2.26)):d
dt arg dp ds s= R t
0 v 1 (ξ) dξ
= d
ds arg dp ds s= R t
0 v 1 (ξ) dξ
· ds dt
= κ(s)| s= R t
0 v 1 (ξ) dξ · v 1 (t) = ˙θ 1 (t) .
Inturn,identity(2.27)holdsbe ause
[x 1 (0) y 1 (0)] ′ = p(0)
( f. onditions(2.21))andderivativesofthesidesof(2.27) areequal toea hother. Indeed,byvirtue
of (2.22) and(2.25)
v 1 (t) = v(t) cos(θ 0 (t) − θ 1 (t)) ,
sothat
d dt p(s)
s= R t
0 v 1 (ξ)dξ
= dp ds s= R t
0 v 1 (ξ) dξ
· ds dt
=
"
cos arg dp ds sin arg dp ds
# s= R t
0 v 1 (ξ)dξ
· v 1 (t)
=
"
cos θ 1 (t) sin θ 1 (t)
#
v(t) cos(θ 0 (t) − θ 1 (t)) =
"
˙x 1 (t)
˙y 1 (t)
# ,
the last equality being derived from the rst two equations of system (2.15).
The provided proof of proposition 4 is fully onstru tive. Indeed, it
pro-vides the dynami path inversion pro edure to determine the feedforward
in-verse ontrol to drive the arti ulated vehi le from a given onguration to a
target onguration, along a
G 4
-path. Thispath an be any desiredG 4
-pathprovidedthatthepathendpointshaveCartesian oordinates,unittangent
ve -tor, urvature,andrstandse ondderivativesof urvatureina ordan ewith
the urrent vehi le onguration ( f. (2.17)-(2.20)). Hen e, the generation of
a
G 4
-path for the arti ulated vehi le must ensure interpolating onditions at theendpointsuptothese ondderivativeofthe urvature.Thisistheproblemthatis addressed, ina polynomialsetting, in the next subse tion.
2.2.2 The
η 4
-splinesConsidered the result relative to the smooth feedforward ontrol of the tru k
and trailer vehi le asexposedin the previous se tion (proposition 4),the
fol-lowing interpolationproblemin the Cartesianplane isintrodu ed.
Problem 3 Determinetheminimalorder polynomial urvewhi hinterpolates
two given endpoints
p A = [x A y A ] ′
andp B = [x B y B ] ′
with asso iated unittangent ve tors dened by angles
θ A
andθ B
, s alar urvaturesκ A
andκ B
,urvature derivatives
˙κ A
,˙κ B
andse ond-order derivativesofthe urvatureκ ¨ A
,¨
κ B
(bothderivativesare denedwithrespe ttothear length) (see gure 2.13).Assume that interpolating data
p A
,p B ∈ R 2
,θ A , θ B ∈ [0, 2π)
,κ A
,κ B ∈ R
,˙κ A
,˙κ B ∈ R
andκ ¨ A
,¨ κ B ∈ R
an be arbitrarily assigned.PSfragrepla ements
x y
p A , κ A , ˙κ A , ¨ κ A
p B , κ B , ˙κ B , ¨ κ B
θ A
θ B
Figure 2.13: Thepolynomial
G 4
-interpolatingproblem.The provisional solution for the above interpolating problem is given by a
ninth-orderpolynomial urve
p(u) = [α(u) β(u)] ′
,u ∈ [0, 1]
dened asfollowsα(u) =
9
X
i=0
α i u i ,
(2.30)β(u) =
9
X
i=0
β i u i ,
(2.31)where oe ients
α i , β i i = 0, . . . , 9
are to be determined a ording to theabove interpolatingproblem. Asknown fromthe theoryof planar urves, the
unittangent ve tor
τ
and urvatureκ
an be expressed asτ (u) = [ ˙α ˙ β] ′
( ˙α + ˙ β) 1/2 ,
(2.32)κ(u) = ˙α ¨ β − ¨ α ˙ β
( ˙α + ˙ β) 3/2 .
(2.33)Dedu tionofthe rstandse ondderivativeofthe urvature
κ
with respe ttothe ar length leads to the following formulae:
dκ
ds (u) = ( ˙α
...
β −
...α ˙ β)( ˙α 2 + ˙ β 2 ) − 3( ˙α ¨ β − ¨ α ˙ β)( ˙α ¨ α + ˙ β ¨ β)
( ˙α 2 + ˙ β 2 ) 3 ,
(2.34)d 2 κ
ds 2 (u) = h ( ˙α
....
β −
....α ˙ β + ¨ α
...
β −
...α ¨ β)( ˙α 2 + ˙ β 2 ) 2 − 7( ˙α
...
β −
...α ˙ β)( ˙α ¨ α + ˙ β ¨ β) ( ˙α 2 + ˙ β 2 ) − 3( ˙α ¨ β − ¨ α ˙ β)(¨ α 2 + ¨ β 2 + ˙α
...α + ˙ β
...
β )( ˙α 2 + ˙ β 2 ) + 18( ˙α ¨ β − ¨ α ˙ β)( ˙α ¨ α + ˙ β ¨ β) 2 i 1
( ˙α 2 + ˙ β 2 ) 5 .
(2.35)
Theimpositionofthe
G 4
-interpolating onditions ofthe aboveproblemonthe endpointsofp(u)
leads tothe following relations:p(0) = p A ,
(2.36)p(1) = p B ,
(2.37)˙p(0) = η 1
"
cos θ A
sin θ A
#
,
(2.38)˙p(1) = η 2
"
cos θ B
sin θ B
#
,
(2.39)κ(0) = κ A ,
(2.40)κ(1) = κ B ,
(2.41)dκ
ds (0) = ˙κ A ,
(2.42)dκ
ds (1) = ˙κ B ,
(2.43)d 2 κ
ds 2 (0) = ¨ κ A ,
(2.44)d 2 κ
ds 2 (1) = ¨ κ B .
(2.45)Note that relation (2.38) and (2.39), whi h ensure the interpolation of the
unit tangent ve tors, arewell posed provided that
η 1
andη 2
are any positiveparameters.
Relations (2.36)-(2.45) form a nonlinear algebrai systemof 14 equations
in the 20 unknowns
α i , β i
. Hen e this system may admit a solution set with6 degrees of freedom. This solution set an be parametrized a ording to the
introdu tion of further6 real parameters
η 3 , . . . , η 8
dened asfollows:h¨p(0) ,
"
cos θ A
sin θ A
#
i = η 3 ,
(2.46)h¨p(1) ,
"
cos θ B sin θ B
#
i = η 4 ,
(2.47)h
...p(0) ,
"
cos θ A sin θ A
#
i = η 5 ,
(2.48)h
...p (1) ,
"
cos θ B sin θ B
#
i = η 6 ,
(2.49)h
....p (0) ,
"
cos θ A sin θ A
#
i = η 7 ,
(2.50)h
....p (1) ,
"
cos θ B sin θ B
#
i = η 8 .
(2.51)Equations(2.36)-(2.45)and(2.46)-(2.51)formanalgebrai systemof20
equa-tions inthe 20unknowns
α i
,β i
,i = 0, . . . , 9
thatdependsonthe realparame-ters
η 1 , η 2 ∈ R +
andη 3 , . . . , η 8 ∈ R
.Thisparameters anbepa kedtoformtheeta ve tor
η := [η 1 . . . η 8 ] ′
belonging to the parameter spa eH := R 2 + × R 6
.From equations (2.36) and(2.38) we determine
α 0 = x A , β 0 = y A , α 1 = η 1 cos θ A , β 1 = η 1 sin θ A .
(2.52)
Equations (2.37) and(2.39) lead to the linearequations
α(1) =
9
X
i=0
α i = x B , β(1) =
9
X
i=0
β i = y B ,
(2.53)˙α(1) =
9
X
i=1
i α i = η 2 cos θ B , β(1) = ˙
9
X
i=1
i β i = η 2 sin θ B .
(2.54)From equation(2.40) and solution
α 1 , β 1
givenby(2.52) we obtain−2η 1 sin θ A α 2 + 2η 1 cos θ A β 2 = η 1 3 κ A ,
(2.55)and from(2.46)
2 cos θ A α 2 + 2 sin θ A β 2 = η 3 .
(2.56)Equations (2.55) and(2.56) give the solutions
α 2 = 1
2 η 3 cos θ A − 1
2 η 1 2 κ A sin θ A ,
(2.57)β 2 = 1
2 η 3 sin θ A − 1
2 η 1 2 κ A cos θ A .
(2.58)Taking into a ount relation(2.34), equation(2.42) be omes
(6α 1 β 3 − 6β 1 α 3 )η 1 2 − 12(α 1 β 2 − α 2 β 1 )(α 1 α 2 + β 1 β 2 ) = η 1 6 ˙κ A ,
(2.59)and from(2.48) we obtain
6 cos θ A α 3 + 6 sin θ A β 3 = η 5 .
(2.60)Bysubstitution ofsolutions (2.52),(2.57),and (2.58),equations (2.59),(2.60)
form a linear algebrai system in the unknowns
α 3
,β 3
whi h has a uniquesolution be ause the determinant of its oe ient matrix is equal to
6η 3 1
anditdiers fromzero onthe assumption
η 1 > 0
. This solutionis given byα 3 = − 1
2 η 1 η 3 κ A + 1 6 η 3 1 ˙κ A
sin θ A + 1
6 η 5 cos θ A ,
(2.61)β 3 = 1
2 η 1 η 3 κ A + 1 6 η 1 3 ˙κ A
cos θ A + 1
6 η 5 sin θ A .
(2.62)Using relation(2.35), equation(2.44) be omes
12(2α 1 β 4 − 2α 4 β 1 + α 2 β 3 − α 3 β 2 )η 4 1 − 84(α 1 β 3 − α 3 β 1 )(α 1 α 2 + β 1 β 2 )η 1 2
−12(α 1 β 2 − α 2 β 1 )(2α 2 2 + 2β 2 2 + 3α 1 α 3 + 3β 1 β 3 )η 2 1 +144(α 1 β 2 − α 2 β 1 )(α 1 α 2 + β 1 β 2 ) 2 = η 1 10 κ ¨ A ,
(2.63)