1 l tan δ s

In document Optimal motion planning of wheeled mobile robots (Page 69-114)

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 tor

z ∈ Z

with

Z := [− ˙κ M , ˙κ M ] 4 × R 4 + × R 2 × [0, 2π) × [−κ M , κ M ]

whi h is a

twelve-dimensional sear h spa e.

Case

h = 2

with

{p 1 (u; η 1 ), p + 2 (u; η 2 )}

(one ba kward movement plus a a

forward 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 the

pre-sented ases.Table2.1reportsthedimension andstru tureofthesear hspa e

Z

as a fun tion of

h

. In parti ular, when the parking is done with

h

splines,

the dimension ofthe sear h spa eis

8h − 4

: every added splinein reases of 8

the dimension of

Z

.

Remark The proposed approximation s heme repla es ea h path

Γ i

of

se-quen e

1 , Γ 2 , . . . , Γ h }

with only one

η 3

-spline to avoid ex essive in reasing

of the dimension of the sear h spa e

Z

. Yet, it would be possible within the

sameproposedframeworkto 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 e

Z

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 of

G 3

-paths

in-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 the

h

splines.Theseindexesarerespe tively denotedby

κ max

,

˙κ max

, and

s tot

and

dependontheparameterve tor

z ∈ Z

.They anbedeterminedasfollows(the

h

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 and

p 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]

i ds (u)

,

and

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 ar

length (for brevity the dependen y on

u

is omitted in the right side of the

above 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 as

S .

=

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)

. Dene

the obsta le region

O

as the union of all the obsta les, i.e.

O .

= ∪ n i=1 B i

and

the vehi le avoids all the obsta les along a path planning if and only if the

interse tionof

S(z)

and

O

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 the

smoothparkingproblem)Giventhenumber

h

ofpaths, onsiderthe

param-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 minimized

in(2.10) anbefreely hoseninordertoweightthesmoothnessoftheresulting

maneuver paths(whi hisrelatedto lowvalues ofboth

κ max

and

˙κ max

)versus

the 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 maneuver

sequen e to prefer

{p + 1 , p 2 , . . . , p h }

or

{p 1 , p + 2 , . . . , p h }

; (3) an initial

esti-mate of the parameter ve tor

z

. Reasonably, this data an be determined by usinglook-uptablesthat anbe onstru tedo-linebyextensiveoptimizations

su 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 Cartesian

plane

P

is hosen to be inside the parking lot that the ar hasto rea h. The

arhasstart 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 es

to 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 to

the 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 ausethe

multi-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 parking

maneuvers.

Forthe two splines maneuver the multi-optimizationof

{p 1 , p + 2 }

leadsto

a 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. This

solution 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. This

solution 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 }

in

ex-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 }

in

ex-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 simulation

in 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,theinitial

and 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

-trailer

ve-hi le (i.e. an arti ulated vehi le onsisting of a tru k towing

n

trailers) was

onsidered 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

-trailersystemintoaGoursat

normal 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 the

polynomial

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 arbitrary

G 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 theinterpolation

onditions 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. We

PSfragrepla 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 ttothe

x

axis.Thetru ka tuates

the 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 axleandtherearaxle

of 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 nonlinear

kinemati 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 urve

p(u)

, with

u ∈ [u 0 , u 1 ]

, has

k

-th order geometri ontinuity, and we say

p(u)

is a

G k

- urve, if

p(u)

is a

G k−1

- urve,

du d k k p (u) ∈ P C([u 0 , u 1 ])

, and the

(k − 2)

-th order derivative of

the 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 aCartesianplaneisa

G 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 be

C 1

-fun tions. Su h a ontinuity of these vehi le inputs is linked tothe fourth-ordergeometri ontinuityofthe trailerpath asstated by

the following proposition.

Proposition 4 Assignany

t f > 0

. For model (2.15), onsider smooth inputs

v(t), δ(t) ∈ C 1 ([0, t f ])

,with

v(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 ])

, isa

G 4

-path.Conversely,givena

G 4

-path

Γ

thereexistsmooth

inputs

v(t), δ(t) ∈ C 1 ([0, t f ])

with

v(t) 6= 0

,

|δ(t)| < π 2

,

∀t ∈ [0, t f ]

and initial

onditions for whi h

|θ 0 (t) − θ 1 (t)| < π 2

,

∀t ∈ [0, t f ]

and the path generated by

system (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 h

isaregularCartesian urve.Indeed,itsderivative

[ ˙x 1 (t) ˙y 1 (t)] T

nevervanishes

over

[0, t f ]

be ause

v(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

κ

is

given by the derivative of the tangent angle

θ 1

with respe tto the ar length

s

, where

s = R t

0 ( ˙x 2 1 (ξ) + ˙y 2 1 (ξ)) 1 2

. 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 ontinuous

in

[0, t f ]

too.The derivative of the s alar urvature

κ

isgivenby

ds = 1

d 1 cos 30 − θ 1 )

 1

d 0 tan δ − 1

d 1 sin(θ 0 − θ 1 )



.

(2.19)

The urvaturederivative

ds

isthen ontinuousalong the urve be ause

θ 0

,

θ 1

and

δ

are ontinuous in

[0, t f ]

. Finally, the se ond derivative of the urvature

an beexpressed asfollows

d 2 κ

ds 2 = ˙δ

|v|d 0 d 1 cos 2 δ cos 40 − θ 1 ) − sgn(v)

1

d 0 tan δ − d 1 1 sin(θ 0 − θ 1 ) d 2 1 cos 30 − θ 1 )

+sgn(v) 3 h

1

d 0 tan δ − d 1 1 sin(θ 0 − θ 1 ) i 2

sin(θ 0 − θ 1 ) d 1 cos 50 − θ 1 ) .

(2.20)

Again, from the ontinuity of the state variables

θ 0

and

θ 1

and from the

hy-pothesis

v, δ ∈ C 1 ([0, t f ])

, the se ondderivative of the urvaturewith respe t

to the ar lengthis ontinuousin

[0, t f ]

. Thisshowsthat urve

[x 1 (t) y 1 (t)]

is

a

G 4

- urve,hen ethe image

"

x 1

y 1

#

([0, t f ])

isa

G 4

-path.

In order to prove the onverse part of the proposition, onsider the

G 4

- urve

p(s)

,where

s

isthear lengthon

Γ

and

p([0, s f ]) ≡ Γ

with

s f

beingthe

total 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 of

p(s)

respe tively.

Also onsider any

v 1 (t) ∈ C 1 ([0, t f ])

su hthat

v 1 (t) > 0

,

∀t ∈ [0, t f ]

and

Z 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 ]

and

v(t) ∈ C 1 ([0, t f ])

be ause

v 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 ])

(indeed

p(s)

is a

G 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 desired

G 4

-path

providedthatthepathendpointshaveCartesian 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.Thisistheproblem

thatis addressed, ina polynomialsetting, in the next subse tion.

2.2.2 The

η 4

-splines

Considered 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 ]

and

p B = [x B y B ]

with asso iated unit

tangent 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 the

above 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 tto

the ar length leads to the following formulae:

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 endpointsof

p(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)

ds (0) = ˙κ A ,

(2.42)

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 positive

parameters.

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 with

6 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 real

parame-ters

η 1 , η 2 ∈ R +

and

η 3 , . . . , η 8 ∈ R

.Thisparameters anbepa kedtoformthe

eta ve tor

η := [η 1 . . . η 8 ]

belonging to the parameter spa e

H := 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 unique

solution be ause the determinant of its oe ient matrix is equal to

3 1

and

itdiers 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 β 24 1 − 84(α 1 β 3 − α 3 β 1 )(α 1 α 2 + β 1 β 21 2

−12(α 1 β 2 − α 2 β 1 )(2α 2 2 + 2β 2 2 + 3α 1 α 3 + 3β 1 β 32 1 +144(α 1 β 2 − α 2 β 1 )(α 1 α 2 + β 1 β 2 ) 2 = η 1 10 κ ¨ A ,

(2.63)

In document Optimal motion planning of wheeled mobile robots (Page 69-114)

Related documents