IN DEGREE PROJECT VEHICLE ENGINEERING,SECOND CYCLE, 30 CREDITS

, STOCKHOLM SWEDEN 2018

Trajectory Planning for Four Wheel Steering Autonomous Vehicle

ZEXU WANG

KTH ROYAL INSTITUTE OF TECHNOLOGYSCHOOL OF ENGINEERING SCIENCES

Abstract (English)

This thesis work presents a model predictive control (MPC) based trajectory planner for

high speed lane change and low speed parking scenarios of autonomous four wheel steer-

ing (4WS) vehicle. A four wheel steering vehicle has better low speed maneuverability

and high speed stability compared with normal front wheel steering(FWS) vehicles. The

MPC optimal trajectory planner is formulated in a curvilinear coordinate frame (Frenet

frame) minimizing the lateral deviation, heading error and velocity error in a kinematic

double track model of a four wheel steering vehicle. Using the proposed trajectory plan-

ner, simulations show that a four wheel steering vehicle is able to track different type of

path with lower lateral deviations, less heading error and shorter longitudinal distance.

Index: trajectory planning, frenet frame, MPC, 4WS

i

Abstract (Swedish)

I detta avhandlingsarbete presenteras en modellbaserad prediktiv kontroll (MPC) -baserad

banplaneringsplan for hoghastighetsbanan och laghastighetsparametrar for autonomt

fyrhjulsdrift (4WS). Ett fyrhjulsdrivna fordon har battre manovrerbarhet med lag hastighet

och hoghastighetsstabilitet jamfort med vanliga framre hjulstyrningar (FWS). MPC-optimal

banplanerare ar formulerad i en krokt koordinatram (Frenet-ram) som minimerar sidoforlangnin-

gen, kursfel och hastighetsfel i en kinematisk dubbelsparmodell av ett fyrhjulsstyrda for-

don. Med hjalp av den foreslagna banaplaneraren visar simuleringar att ett fyrhjulsstyr-

fordon kan spara olika typer av banor med lagre sidoforlangningar, mindre kursfel och

kortare langsgaende avstand.

Index: banplanering, Frenet-ram, MPC, 4WS

ii

Abbreviations

Symbol Description

NLMPC Nonlinear model predictive control

MPC Model predictive control

4WS Four wheel steering

FWS Front wheel steering

ITRL Integrated Transportation Research Lab

RCV Research Concept Vehicle

LIDAR light detection and ranging

GPS Global Positioning System

ROS Robot Operating System

iii

Contents

1 Introduction 1

1.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

1.2 Research focus . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2

1.3 State of Art (Literature Review) . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.3.1 4WS robots and vehicles . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.3.2 Curvilinear coordinate transformation . . . . . . . . . . . . . . . . . . 3

1.3.3 Motion Planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

1.3.4 MPC fundamentals . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2 System and Modelling 6

2.1 Transformation of the coordinate systems . . . . . . . . . . . . . . . . . . . . 6

2.1.1 From Cartesian to Frenet . . . . . . . . . . . . . . . . . . . . . . . . . 6

2.1.2 From Frenet to Cartesian . . . . . . . . . . . . . . . . . . . . . . . . . 7

2.2 Single Point Model (SPM) . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

2.3 Bicycle Model (Single Track Model) . . . . . . . . . . . . . . . . . . . . . . . 8

2.4 Double Track Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

2.5 Combined model in the Frenet frame . . . . . . . . . . . . . . . . . . . . . . . 10

3 Stability and maneuverability analysis 13

3.1 Stability analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

3.2 Maneuverability between 4WS and 2WS . . . . . . . . . . . . . . . . . . . . . 14

4 Model Predictive Framework for the Planner 16

4.1 Equality constraints of MPC . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

4.2 Inequality Constraints of MPC . . . . . . . . . . . . . . . . . . . . . . . . . . 17

4.3 Pre-process of the Path . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

4.4 MPC formulations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

5 Results and Discussion 21

5.1 Planning in different types of roads . . . . . . . . . . . . . . . . . . . . . . . . 21

5.2 Comparisons between 4WS and FWS . . . . . . . . . . . . . . . . . . . . . . 27

6 Conclusion 30

7 Future Work 31

iv

8 Acknowledgement 32

References 33

Appendix 35

v

1 Introduction

In this report, a trajectory planner is used between a path generator and a lower level con-

troller. From position information given by the path generator, trajectory planner calculates

an optimal temporal position and velocity profile and these two profiles will be passed to the

lower level controller as reference. Then the optimization problem is transformed into a ref-

erence following problem. This thesis deals with the trajectory planning problem in a four

wheel steering vehicle using the method of Nonlinear Model Predictive Control (NLMPC).

In following parts, the report will give some background introduction of four wheel steering

research concept vehicle, autonomous driving, model predictive control and show the method

of approaching the four wheel steering (4WS) vehicle trajectory planner.

1.1 Background

In the study of autonomous vehicles, maneuverability and stability are always two key prop-

erties to concern about. Under the context of autonomous driving, maneuverability stands for

vehicle’s ability to handle different driving maneuvers such as sharp turning while stability

indicates vehicle’s ability to stay stable under critical situations such as double lane change.

To improve the maneuverability and stability, 4WS is introduced to autonomous vehicles and

proved to have satisfactory performance shown in [1].

The integrated transportation research lab (ITRL) at KTH aims to solve vehicle and transporta-

tion problems arising in the process of vehicles and transportation developments. Under this

principle, a research concept vehicle (RCV) was built as a testing platform for students and

researchers to investigate cutting edge techniques. Schematic views of RCV can be found in

Figure 1.

Figure 1: Research concept vehicle (RCV)[2]

Sensors like LIDAR, radar, cameras and GPS are integrated on the RCV which gives the re-

searchers various of ways in explorations and experiments. Robot Operating System (ROS)

1

is used as a platform of implementing controllers and other algorithms which facilitates the

development process.

1.2 Research focus

The goal of this thesis work is to design an optimal trajectory planner based on model pre-

dictive control. Optimal planner for both 4WS and FWS will be designed and evaluated in a

simulation environment. The trajectory planners will be running in real time on different types

of path while comparisons will be made based on maneuverability and stability. Schematic

logic description of the optimal trajectory planner is shown in Figure 2.

Figure 2: Schematic of the planning structure

As shown in Figure 2, pre-designed GPS data points will be interpolated into a desired path of

the RCV; using the trajectory planner with the help of estimated vehicle states from the RCV,

an optimal trajectory can be calculated and sent to lower level path following controller which

directly control the steering and throttle/brake system of the RCV. For lower level trajectory

following controller, the aim at each sampling time is to follow a set point given by the planner.

The planner is designed for low speed maneuverability and high speed stability , a 4WS based

planner is introduced to meet the requirements. More details of analysis on maneuverability

and stability will be discussed in Section 3.

2

1.3 State of Art (Literature Review)

In the following part, research related to 4WS, curvilinear coordinate transformation , trajectory

planning and RCV will be presented.

1.3.1 4WS robots and vehicles

In the field of 4WS, much research have already been done on mobile robots and road vehicles.

Advantages of better tracking ability and larger turning curvature limit (more on section 3) gives

4WS more potential to fulfill requirements of higher accuracy and more critical situations. In

[3],[4] and [5], robots with 4WS are shown to have better handle of complicated situations,

like farmland, with high tracking ability. In road vehicles, 4WS gives drivers more degree of

freedom to control a vehicle but on the other hand also increase the difficulty of handling both

pairs of steering wheel at the same time. To fix this problem, the front and rear wheels are

designed to have interaction with each other. For example, if a driver make a left-turn with the

steering wheel, the front wheels of the vehicle will turn left with the rear wheels turning right.

The net effect of this operation is left-hand turn with larger curvature compared with on FWS.

However, the degree of freedom on steering control is then decreased and potential instability

might occur if the vehicle speed is relatively high. To fully utilize the degree of freedom in

4WS, autonomous 4WS is introduced. Using method of µ synthesis (optimal control synthesis

similar to H∞ synthesis while dealing with uncertain systems), 4WS autonomous vehicles can

be controlled to achieve good maneuverability, sufficiently robust stability, and attenuation

ability against serious disturbances in [6].

1.3.2 Curvilinear coordinate transformation

In a tracking or path following problem, the desired path lines are curving lines in most of the

cases while the designers are more interested in the relative position with respect to the refer-

ence line rather than absolute coordinate. In that sense, using a curvilinear coordinate system

will give easier interpretation of the path and lead to simpler expression of state functions. In

[4], a detailed formulation of Frenet (a specific type of curvilinear system) coordinate system

is given. In the Frenet frame, the curvilinear abscissa, lateral deviation with reference line and

heading error are considered and penalized. By penalizing lateral deviation and heading errors

with certain curvilinear abscissa, the vehicle would be guaranteed to stay on reference path.

More detailed formulation of coordinate transformation will be discussed in Section 2.1.

3

1.3.3 Motion Planning

In autonomous vehicle designs, trajectory/path planning is a important part for controlling the

motion of a vehicle. Gathering information from infrastructure/vehicles, designers utilize dif-

ferent strategies for motion planning to improve comfort, safety and energy consumption. Var-

ious motion planning techniques are mentioned in [7]. Based on different purposes, motion

planning can be roughly divided into two types, path planning and trajectory planning. Path

planner is designed for generating an optimal path from a starting pose to a end pose: [8]

uses Hybrid A∗ method for searching and leads to satisfactory simulation results; based on the

MIT DARPA Urban Challenge vehicle, [9] applied Rapidly-exploring Random Trees (RRT)

algorithm to find safe path under uncertainties and limited sensing. Trajectory is designed to

be followed by the lower level controllers, thus position and velocity data given from trajec-

tory planner should be temporal. Under this condition, [10] and [11] uses model predictive

controller to generate optimal trajectory for autonomous vehicles. This report will focus on

model predictive control based trajectory planning in Frenet frame mentioned in section 1.3.2.

In [12] , an optimization trajectory planner is computed and demonstrated to be working well,

however, the 4WS property can be added to get better optimization results. In [13], a linear

time-varying MPC controller is designed and implemented on a RCV to follow a given path.

By introducing a new degree of freedom, namely crabbing motion due to in-phase or opposite

phase steering of 4WS, the RCV is able to follow the path in a smoother way. However, using

MPC with time varying system can be computationally inefficient since the prediction horizon

of both states and inputs are designed to be relatively large to ensure good performance.

1.3.4 MPC fundamentals

In recent years, with the improvement in computing ability of computers, complicated opti-

mization problems can be solved in real-time. Within the optimization strategies, model pre-

dictive control (MPC) is proven to be reliable by industries. The idea of a MPC is solving a

finite horizon optimization problem in a receding horizon way with both states and inputs con-

straints.

The overall formulation of a MPC problem can be found in Equation 1

4

min .N−1

∑i=1

(xTi|tQ1xi|t +uT

i|tRui|t)+ xTN|tQ f xN|t (1)

s.t. f (x,u) = 0 (2)

g(x,u)≤ 0

where N is the prediction horizon, xi|t is the state of the system at time t and predicted horizon i,

Q is the weight on states, R is the weight on input, f (x,u) stands for equality constraints while

g(x,u) stands for inequality constraints. Using optimization solvers to optimize the objective

matrix in Equation 1, one can get a sequence of optimal control input

u∗t = {u∗1|t ,u∗2|t , ...,u

∗N−1|t}. (3)

Only the first signal u∗1|t is applied to the system and shift the whole problem to t+1 to continue

the same optimizing progress.

As one can see that, between the sampling time t and t +1, one step of optimization must give

an input signal for the whole progress to continue. Therefore relative short computing time is

critical to the whole model predictive problem.

5

2 System and Modelling

In this section, the system that is used in the controller and modelling method will be discussed.

2.1 Transformation of the coordinate systems

In the optimization problem, states of the vehicle will be penalized inside a Frenet frame and

optimal output states will be in Frenet frame. To compute the absolute pose of the vehicle,

transformation between Frenet frame and Cartesian frame is needed. In the following, s will

stand for curvilinear abscissa, ey for lateral deviation in Frenet frame. On the other hand,

position of a path are always given as a series (x,y) data. To formulate a MPC problem, the

path data should be transformed into Frenet frame.

2.1.1 From Cartesian to Frenet

As shown in Figure 3, O is the starting point of the path defined by s = 0; V is the vehicle

position with Cartesian coordinate (x,y); C is the projection from V to the path with Cartesian

coordinate (X ,Y ); θm is yaw angle of the vehicle; θc is path angle at point C.

Figure 3: Coordinate transformation between Cartesian and Frenet

From the aforementioned clarification, the Frenet coordinate can be found in Equation 4,5 and

6

7.

s = arc length( ~OC) (4)

ey = sign · |VC| (5)

Cc =∂θc

∂ s= θ

′c (6)

θc = arctan(dYdX

) (7)

In Equation 5, if vehicle is on the left side of the path, sign = 1; if the vehicle is on the right

side of the path, sign = −1. In Equation 6, θc is the curvature of path at C. In Equation 7, the

path angle is approximated by the slope around point C. Note that, the Frenet coordinate of the

path can be calculated in the same way with y = 0.

2.1.2 From Frenet to Cartesian

Given (s,y), the Cartesian coordinate can be found as follows

1. Using interpolation to find the Cartesian coordinate (X ,Y ) of C (Since the path is given

as series of Cartesian coordinate, and C has the same s value with V )

2. Find the path angle at C using Equation 7.

3. The Cartesian coordinate of V can be found as

x = X− ey sin(θc)

y = Y + ey cos(θc)

2.2 Single Point Model (SPM)

In order to formulate the nonlinear MPC, the state space model should be found in advance. To

describe the behaviour of the RCV conveniently, a Frenet coordinate system is used. According

to [4] and [12], the kinematic model can be shown as:

s =vcos(θm−θc +β )

1− y · cc(8)

ey = vsin(θm−θc +β ) (9)

θm = ω (10)

7

The parameters in the equation is clarified in Table 1

Table 1: Parameters clarification of the dynamic in Frenet frames Curvilinear Abscissaey Lateral distance from the RCV to the pathθm Yaw angle of the RCVθm Yaw rate of the RCVβ Crabbing angle of the RCVθc Angle of the tangent vector to the Pathcc Curvature of the pathv Velocity of the vehicle

Note that here in the model, the curvature of the path cc is not controlled directly by the con-

troller, in other word, this is not a control input but can be viewed as a parameter that changes

along the path.

2.3 Bicycle Model (Single Track Model)

In a single point model, input signals are crabbing angle and curvature of the vehicle while

those two inputs need to relate to physical properties (front and rear steering angle) of the ve-

hicle. To further describe properties of motion with respect to steering wheel angle, a more

complicated model that is known as bicycle model is introduced. In a bicycle model, two dif-

ferent maneuvers need to be considered. First, the front wheels are steered in the same direction

as the rear; second, the front wheels are steered in opposite direction than the rear as can be

shown in Figure 4 and Figure 5 respectively.

In order for tires to have no side slips, direction of tire velocity should be the same as direction

of tire orientation and velocity vectors of the front wheels and rear wheels should be concentric

and also be the same direction as tire orientations as shown in Figure 4 and 5. This type of

steering geometry is also known as Ackermann steering geometry.

8

Figure 4: Bicycle Model of first maneuver Figure 5: Bicycle Model of second maneuver

Using the input signal δ f and δr, the crabbing angle β and curvature of the vehicle motion κ

can be obtained by Equation 11 and Equation 12 for both maneuvers. The derivation can be

found in Section 8.

β = arctan(l f · tanδr + lr · tanδ f

l f + lr) (11)

κ =sin(δ f −β )

l f · cosδ f(12)

The meaning of the parameters can be clarified in Table 2

Table 2: Clarifications of the parameters in bicycle modelC Center of Gravity of the vehicleδ f Front wheel steering angleδr Rear wheel steering angleβ Crabbing angle of the RCVl f Distance from the front axle to Clr Distance from the rear axle to Cv Velocity at the COGv f Velocity at the front axlevr Velocity at the rear axle

2.4 Double Track Model

To further describe the details of the motion, a double track model can be used. According to

the definition of bicycle model, the steering effect of the left and right wheels are equivalently

replaced by one wheel. Thus, to extend a bicycle model to a double track model, the method is

9

to find the left and right steering wheel angle from the equivalent steering angle in the bicycle

model.

Figure 6: Double Track Model of Front wheels Figure 7: Double Track Model ofRear wheels

As shown in Figure 6 and Figure 7, the relationship with the equivalent front wheel steering

can be found in Equation 13 and Equation 14.

tanδ f =2tanδ f 1 tanδ f 2

tanδ f 1 + tanδ f 2(13)

tanδr =2tanδr1 tanδr2

tanδr1 + tanδr2(14)

In this manner, the single track model is extended to the double track model.

2.5 Combined model in the Frenet frame

Combine the behavior of the COG with the double track model one can find that:

s =vcos(θm−θc +β )

1− ey · cc(15)

ey = vsin(θm−θc +β ) (16)

θm = κv (17)

while the input signal κ and β can be found from Equation 11 to Equation 14. Note that in

practice, the control signals are δ f 1, δ f 2, δr1 and δr2. Therefore, the problem now is to construct

a combined relationship of κ and β as a function of δi.

To this end, the limitation of steering wheel angle is needed. For RCV, the limitations of each

10

steering wheel are set in Equation 18.

|δi| ≤ 0.4 rad (18)

In this limitation, a small angle approximation can be made to simplify the RCV model. In this

case, relationship between steering angle, crabbing angle and vehicle curvature can be found in

Equation 19, 20, 21 and 22.

β =l f δr + lrδ f

l f + lr(19)

κ =δ f −β

l f=

δ f −δr

l f + lr(20)

δ f =2δ f 1δ f 2

δ f 1 +δ f 2(21)

δr =2δr1δr2

δr1 +δr2(22)

The relationship between κ−β and δ f −δr in the linear mapping can be found in Figure 8 and

Figure 9.

0

0.2

0.05

0.15 0.2

0.1

0.15

0.15

rear steering

0.1

front steering

0.1

0.2

0.050.05

0 0

Figure 8: Relationship between β and δ

11

-0.05

0.2

0.15 0.2

0

0.15

rear steering

0.1

front steering

0.1

0.05

0.050.05

0 0

Figure 9: Relationship between κ and δ

In this linear relationship, the ’new’ input signals can be found from the physical input signal

in Equation 23 and Equation 24.

β + l f κ = δ f (23)

β − lrκ = δr (24)

Thus if a limit of δ f and δr is setup using Eq. (21) and Eq. (22), one can have:

|δ f ,max|= 0.4 rad (25)

|δr,max|= 0.4 rad (26)

Thus, the constraints in the MPC can be formulate as shown in Eq. (27) and Eq. (28).

−0.4≤ β + l f κ ≤ 0.4 (27)

−0.4≤ β − lrκ ≤ 0.4 (28)

12

3 Stability and maneuverability analysis

By introducing one more degree of freedom, i.e.rear wheel steering, the vehicle is expected to

have better stability at high speed and higher maneuverability at high speed. In the following

part, stability will be explained using dynamic bicycle model while maneuverability will be

analyzed by investigating curve turning ability and tracking ability.

3.1 Stability analysis

In [14], dynamic bicycle model for a 4WS vehicle can be found in Equation 29a and dynamic

model for FWS can be found in Equation 29b. Schematic description of a bicycle model can

be found in Figure 10.

Figure 10: single track model of a four wheel steered vehicle[14]

β

ψ

=

−C f+Crmv

C f l f−Crlrmv2 +1

C f l f−CrlrJz

−C f l2f+Crl2

rJzv

β

ψ

+−C f

mvCrmv

C f l fJz

−CrlrJz

δ f

δr

(29a)

β

ψ

=

−C f+Crmv

C f l f−Crlrmv2 +1

C f l f−CrlrJz

−C f l2f+Crl2

rJzv

β

ψ

+−C f

mvC f l f

Jz

δ (29b)

As shown in Equation 29a, the 4WS system has one more input compared with FWS case

in Equation 29b. Therefore, from a intuitive point of view, the stability of 4WS should be no

worse than that of the FWS since if the input signal δr is turned into zero, 4WS will be identical

to FWS.

13

3.2 Maneuverability between 4WS and 2WS

Limits of curvatures under different velocities are critical to compare the maneuverability of

2-wheel steering and 4-wheel steering. From Equation 21, Equation 25 and Equation 26 one

can see that the limit of curvature for a 4WS vehicle can be found as:

− 2δmax

l f + lr≤ κ ≤ 2δmax

l f + lr(30)

.

On the other hand, the curvature of a FWS vehicle can be found in Equation 31

− δmax

l f + lr≤ κ ≤ δmax

l f + lr(31)

From Equation 30 and Equation 31 one can see that 4WS will have larger unsaturated operating

region than FWS given the same steering wheel limit for wheels.

If the vehicle is given a specific steering angle, same steering angle for front and rear, curvatures

of both case can be found in Figure 11. As shown 4WS in Figure 11, R1 stands for turning

radius of FWS while R2 for 4WS; given the same steering angle, 4WS will have a significant

decrease in turning radius, meaning that 4WS will give a larger turning curvature which enable

the vehicle to handle sharper turns.

Figure 11: Curvature comparison between 4WS and FWS

14

In a tracking problem, rear wheel steering can help the vehicles to follow the direction of front

wheels which will give less lateral deviations and shorter longitudinal tracking distance. An

example will be given and discussed in more details in section 5.

Consider the advantage of larger steering limit, better curve turning ability and tracking ability,

4WS is able to handle more maneuvers than FWS, thus in development of trajectory planner, a

4WS based planner is considered.

15

4 Model Predictive Framework for the Planner

In this part, formulation details of a MPC based trajectory planner will be shown. In such

formulation, equality constraints, inequality constraints and pre-processing of path data will be

presented.

4.1 Equality constraints of MPC

In an MPC formulation, the kinematic model described in Equation 8, 9 and 10 is used as

updating vehicle model. As mentioned in section 1.3.4, the model should be in discrete time

domain. Thus discrete kinematic model can be found in Equation 32a, 32b, 32c and 32d.

sk+1 = sk +Ts ·vcos(θm−θc +β )

1− ey,k ·Cc(32a)

ey,k+1 = ey,k +Ts · vsin(θm−θc +β ) (32b)

θm,k+1 = θm,k +Ts ·κv (32c)

vk+1 = vk +Tsa (32d)

As shown in Figure 2, current states of vehicle will feedback to the trajectory planner, therefore,

at each optimization, the initial value of states are defined in Equation 33a,33b, 33c and 33d

s1 = sini (33a)

ey,1 = ey,ini (33b)

θm,1 = θm,ini (33c)

v1 = vini (33d)

where sini, ey,ini, θm,ini and vini are the curvilinear abscissa, lateral deviation, heading angle and

velocity of the vehicle at current time.

16

4.2 Inequality Constraints of MPC

Due to physical limitation of RCV and requirements for a tracking problem, inequality con-

straints can be formulated in Equation 34a, 34b, 34c and 34d.

−12

w− ylim ≤ ey ≤12

w+ ylim (34a)

adec ≤ a≤ aacc (34b)

−δ f ,lim ≤ β − l f κ ≤ δ f ,lim (34c)

−δr,lim ≤ β − lrκ ≤ δr,lim (34d)

In Equation 34a, ylim stands for the limit of lateral deviation allowed; in Equation 34b, aacc

stands for the maximum allowed acceleration and adec stands for the maximum allowed decel-

eration; in Equation 34c, δ f ,lim stands for the maximum allowed steering angle for front wheels

and in Equation 34d stands for the maximum allowed steering angle for real wheels.

4.3 Pre-process of the Path

For a tracking problem, the reference state is not a set point but rather a time-varying sequence

of state points. Under this assumption, the given (X ,Y ) series data of the path need to be pre-

processed such that the vehicle is able to track a desired path. Steps of finding a reference path

can be found as:

1. From the given (X ,Y ) data series of a path, find the corresponding (S,θc).

2. Find approximation of (S(t),θc(t)) by interpolation. If vehicle is expected to drive in a

constant reference velocity, the situation can be defined as normal driving. For normal

driving scenarios, define number of steps (n) to finish the tracking by Eq. (35).

n =S

vre f(35)

Desired reference curvilinaer abscissa sre f is the linear space vector with n steps from

S(i) to S(end) and reference path angle θc,re f can be found as corresponding linear inter-

polation of θc with refer to sre f and S.

For accelerating or decelerating scenarios, assume constant acceleration driving and de-

17

fine vre f as

vre f =vmax + vmin

2(36)

. Note that assumptions in Eq. (35) and (36) are for pre-processing of data only, meaning

that the vehicle is not necessary to run in either constant velocity or constant acceleration.

3. The resulting (sre f ,θc,re f can be treated as an approximation of (S(t),θc(t)).

Note that Matlab has strong tools of finding linear space vector and linear interpolation where

details can be found from Matlab command linspace and interp1.

4.4 MPC formulations

Before setting up MPC formulations, states and inputs are defined in Equation 37 and 38.

xi,t =

s(i+ t)− sre f (i+ t)

ey(i+ t)

θc(i+ t)−θc,re f (i+ t)

v(i+ t)− vre f (i+ t)

(37)

u =

κ

β

a

(38)

18

With equality constraints, inequality constraints and pre-procession of path data, the total MPC

formulation can be found from Equation 39a to 39m.

min.N−1

∑i=1

xTi,tQ1xi,t+uT

i,tRui,t + xTN,tQ f xN,t (39a)

s.t. s1 = s∗1 (39b)

ey,1 = e∗y (39c)

θm,1 = θ∗m (39d)

v1 = v∗k (39e)

sk+1 = sk +Ts ·(

vcos(θm−θc +β )

1− y ·Cc

)(39f)

ey,k+1 = ey,k +Ts · (vsin(θm−θc +β )) (39g)

θm,k+1 = θm,k +Ts ·κv (39h)

vk+1 = vk +Tsa (39i)

−12

w− ylim ≤ ey ≤12

w+ ylim (39j)

adec ≤ a≤ aacc (39k)

−δ f ,lim ≤ β − l f κ ≤ δ f ,lim (39l)

−δr,lim ≤ β − lrκ ≤ δr,lim (39m)

In the formulation of MPC, the weights of states and inputs are designed in Equation 40, 41

and 42.

Q1 =

qs 0 0 0

0 qy 0 0

0 0 qθc 0

0 0 0 qv

(40)

R =

rκ 0 0

0 rβ 0

0 0 ra

(41)

Q f = Q1 (42)

In this formulation, qs, qy, qθc and qv stands for weights of curvilinear abscissa, lateral devia-

tion, error of heading angle and velocity. A larger qi value means in the optimizer, more effort

19

is contributed to penalize state i, rκ , rβ and ra stands for weights of curvature, slip angle and

acceleration. A larger r j value means that the optimizer will use less of input j. Note that the

weight functions are tuning variables of MPC to get better performance.

20

5 Results and Discussion

In this part, 4WS trajectory planning on different types of paths will be presented and dis-

cussed. More result showing the comparisons between 4WS and 2WS based trajectory planner

in those paths will be discussed. In evaluating the performance of trajectory planner, ability of

attenuating lateral deviations, handling sharper turns and following path are considered.

5.1 Planning in different types of roads

In a circle path and sine-shaped path, the initial pose of the vehicle is defined to have a deviation

of a quarter of a lane width as shown in Figure 12 and 13.

0 5 10 15 20 25 30 35 40

x [m]

0

5

10

15

20

25

30

35

40

y [

m]

Figure 12: Planning in circle path

0 5 10 15 20

x [m]

0

5

10

15

20

y [

m]

Figure 13: Planning in sine-shaped path

Velocity, heading angle and lateral deviations of the vehicle for both cases are shown in Figure

14 and 15

21

0 2 4 6 8 10 12

time [s]

4

5

6

velo

city [m

/s] vehicle velocity

reference velocity

0 2 4 6 8 10 12 14

time [s]

1

2

3

headin

g a

ngle

m

c

0 2 4 6 8 10 12 14

time [s]

0

1

2

late

ral devia

tion [m

]

Figure 14: Velocity, heading angle and lateral deviation profile from the planner in the caseshown in Figure 12

0 2 4 6 8 10 12

time [s]

0

2

4

6

velo

city [m

/s] vehicle velocity

reference velocity

0 2 4 6 8 10 12

time [s]

0

0.5

1

1.5

headin

g a

ngle

m

c

0 2 4 6 8 10 12

time [s]

0

1

2

late

ral devia

tion [m

]

Figure 15: Velocity, heading angle and lateral deviation profile from the planner in the caseshown in Figure 13

With proper tuned value of weight matrix Q1, R and Q f , the trajectory planner is able to guide

the vehicle to track the center-line of a given path. When lateral deviation ey of the vehicle is

22

non-zero, the planner will control the vehicle to approaching the given center-line with least

changes in heading error (error between heading angle of the vehicle θm with reference to the

path angle θc). Note that by penalizing the heading error, the planner can ensure that once

vehicle tracks the reference, it would be able to continue running on the reference line. On the

other hand, if penalizing the heading error of vehicle is neglected or underestimated, shorter

time will be needed to attenuate lateral deviation while with relative high heading angle error,

vehicle will continue crossing the center-line and cause oscillation of tracking. From Figure

12 and 13, the planner is able to guide the vehicle to follow a circle and sine-shaped path.

By combining the properties of these two paths, a more realistic and complicated path can be

found in Figure 16. In Figure 16, wider path scenario stands for the pre-parking process while

narrower path scenario stands for parking with lower speed, less operating space and final target

position (blue dash box).

In Figure 16, with the guide of trajectory planner, vehicle is able to track the reference line in

high speed stage (on the wider segment), follow a relative high radius turning road in deceler-

ating stage (on the narrower segment) and stop at a desire position.

0 5 10 15 20 25 30 35

x [m]

-5

0

5

10

15

20

25

30

35

y [

m]

Figure 16: Planning in a parking scenario

In this scenario, the velocity, heading angle and lateral deviation profile from the planner is

shown in Figure 17. In the velocity profile, the reference velocity is designed to be at around

5 m/s during the high speed stage and 0 during the low speed stage in Figure 16. Due to the

23

constraints on acceleration and deceleration, the planner gives the vehicle relatively smooth

velocity guide. compromise between lateral deviation and heading angle is made since the

planner want the vehicle not only get into the reference line as soon as possible but also has the

same heading angle as the path angle.

0 2 4 6 8 10 12

time [s]

0

2

4

6

velo

city [m

/s] vehicle velocity

reference velocity

0 2 4 6 8 10 12

time [s]

0

0.5

1

1.5

headin

g a

ngle

m

c

0 2 4 6 8 10 12

time [s]

0

1

2

late

ral devia

tion [m

]

Figure 17: Velocity, heading angle and lateral deviation profile from the planner in the caseshown in Fig.(16)

Note that constraints can be modeled as tightening the path limit if obstacles like other vehicles

are at the parking lot position as shown in Fig. (18). In this model, black blocks stand for

occupied positions of parking sites and the vehicle should avoid running those positions. By

tightening the lateral constraints, vehicle is able to stop at desired position as shown in Fig.

(16).

24

-5 0 5 10 15 20 25 30 35

x [m]

0

5

10

15

20

25

30

35

y [

m]

Figure 18: Obstacle modeling in a parking lot environment

On the aforementioned conditions, paths are designed to be a continuous curve while in a

double lane change (DLC) case, the path is not continuous. As shown in Figure 19, a pre-

process of path is needed. In this case, the vehicle will be on the reference line from the initial

deviation of the road. Note that if the vehicle is on the path, ey = 0 . Therefore, if a discontinuity

is encountered after the lateral deviation has come to zero, the controller will still ’think’ the

vehicle is on the reference path since ey = 0 still holds. To keep the definition that ey = 0 stand

for on the path consistent, the discontinuous part of path is connected by a inclined line as

shown in Figure 19 by red dots. Note that a trade-off between smoother tracking and shorter

distance to change the lane should be considered. Meaning that the slope of the inclined line

should be properly tuned. The velocity, heading angle and lateral deviation profile of a DLC

planning is shown in Figure 20.

25

-20 -15 -10 -5 0 5 10 15 20 25 30

x [m]

0

5

10

15

20

25

30

35

40

y [

m]

Figure 19: Planning in DLC path at 10 m/s

0 0.5 1 1.5 2 2.5 3 3.5 4

time [s]

9.9

10

10.1

10.2

velo

city [m

/s] vehicle velocity

reference velocity

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5

time [s]

1

1.2

1.4

1.6

headin

g a

ngle

m

c

0 0.5 1 1.5 2 2.5 3 3.5 4 4.5

time [s]

0

1

2

late

ral devia

tion [m

]

Figure 20: Velocity, heading angle and lateral deviation profile from the planner in a DLC case

26

5.2 Comparisons between 4WS and FWS

As discussed in Section 3, 4WS based vehicle should have better maneuverability and better

performance compared with FWS ones. In this section, planning on aforementioned path are

presented to show the difference in 4WS and FWS.

5 10 15 20 25 30 35 40 45 50 55

x [m]

5

10

15

20

25

30

35

40

y [

m]

reference

FWS

4WS

Figure 21: Comparison planning in circle path

27

0 5 10 15 20

x [m]

0

2

4

6

8

10

12

14

16

18

20

y [

m]

reference

FWS

4WS

Figure 22: Comparison planning in parking path

0 5 10 15 20 25 30 35

x [m]

0

5

10

15

20

25

30

y [

m]

Reference

FWS

4WS

Figure 23: Comparison planning in parking path

28

-20 -15 -10 -5 0 5 10 15 20 25 30

x [m]

0

5

10

15

20

25

30

35

40

y [

m]

reference

FWS

4WS

Figure 24: Comparison planning in DLC

From Figure 21, 22, 23 and 24, one can see that, 4WS based trajectory planning has overall

better performance in lateral deviation attenuating, sharp turn tracking and path following.

29

6 Conclusion

In this thesis report, trajectory planning on a 4WS autonomous vehicle is explored under a

model predictive manner for different types of path. The research aims to solve the parking

related planning with 4WS with the help of double track kinematic model of vehicle. For the

convenience of formulating a MPC problem, the coordinate in this problem is transformed

into a Frenet frame. For tracking purpose, a pre-processed series of data for desired path is

formulated as time varying reference of MPC states. By penalizing lateral deviation, relative

heading error between the vehicle and the path, the MPC trajectory planner is able to give the

vehicle a satisfactory guide to follow different types of path. Using the same MPC framework,

a Front wheel steering (FWS) based trajectory planner is presented and compare with the 4WS

trajectory frame. Results show that the 4WS trajectory planner is able to handle sharper turns,

attenuating lateral deviations with shorter longitudinal displacement and better following the

path angle. This is due to the capacity of crabbing motion of 4WS. As shown in Figure 25 and

26, in 4WS based planning, vehicle is able to have more side-way motions than in the FWS

case. This property can guide the vehicle to attenuate the lateral deviation and keeping a small

heading error with path angle at the same time.

0 5 10 15 20 25 30 35

x [m]

0

5

10

15

20

25

y [

m]

Heading angle FWS

reference

FWS

Figure 25: Heading angle of the FWS vehiclein the planner. With no crabbing ability, head-ing angle of the vehicle is always at the samedirection of path angle of the vehicle

0 5 10 15 20 25 30 35

x [m]

0

5

10

15

20

25

30

y [

m]

Heading angle

reference

4WS

Figure 26: Heading angle of the 4WS vehiclein the planner. With crabbing ability, headingangle of the vehicle can have different direc-tions of path angle of the vehicle

30

7 Future Work

So far, the MPC trajectory planner works fine in Matlab environment, but they are still some

problems to fix when it comes to implement the planner on the real RCV.

First, the computing time. In order for the planner to work timely, computing time of the

optimization solver should be less than 100 ms for each optimizing step. So far, the computing

time for each optimizing step takes around 200 ms with nonlinear MPC solver ’Fmincon’. In

the future work, either a more advanced nonlinear solver will be explored or the model needs

to be linearized to formulate a sequential linear programming problem as in [15]. Due to the

nonlinearity of the model, obstacles can be hard to model. Modelling of obstacles in this

report is shown in Figure 18, but this way of modelling sometimes can be conservative. More

advanced obstacle avoidance property of the planner as in [16] should be explored.

Second, to implement a given planner to RCV, the control algorithm need to be transformed

into a type of code that can be operating in the ROS system and should be able to communicate

with signals from the vehicle.

Third, as a testing project, results from simulation only is far from satisfactory, thus more

experiments and editing to improve the performance of trajectory planner is expected on real

car testing.

31

8 Acknowledgement

I would first like to thank my thesis supervisor Lars Svensson, PhD student of the mechatronic

department at KTH. Thanks to his consistent support and kind understanding that allows me to

not only have the freedom of exploring interesting things on my own but also always be able

to on the right direction.I would also like to thank my examiner professor Mikael Nybakca of

vehicle engineering at KTH that offers me ideas of how to analysis vehicle related topics and

technique support during the whole process of my master thesis.

I would also like to thank Yuchao Li, PhD student of automatic control department at KTH,

Viktor Gustavsson, master student of vehicle engineering at KTH, and Max Ahlberg, master

student of vehicle engineering at KTH. With the help of Yuchao’s suggestion on editing the

report and future extension of the thesis, Viktor’s help of reviewing the Swedish version of

abstract and critical comments on the report and Max’s consistent discussion throughout the

whole process of my thesis work, I am able to improve my thesis work and present it in a better

way.

Finally, I must express my profound gratitude to my parents and grandparents for their contin-

uous encouragement and unfailing love during my whole master study. Thank you and I love

you all forever.

32

References

[1] D. Wang and F. Qi. Trajectory planning for a four-wheel-steering vehicle. Proceedings

of the 2001 IEEE International Conference on Robotics 8 Automation Seoul, Korea .May

21-26, 2001, 2001.

[2] M. Nybacka. Kth research concept vehicle. Itrl.kth.se https://www.itrl.kth.se/about-

us/labs/rcv-1.476469, 2017.

[3] T. Bak and H. Jakobsen. Agricultural robotic platform with four wheel steering for weed

detection. Biosystems Engineering (2004) 87 (2), 125–136, 2003.

[4] A. Micaelli and C. Samson. Trajectory tracking for unicycle-type and two-steering-wheels

mobile robots. [Research Report] RR-2097, INRIA. 1993. ¡inria-00074575¿, 2006.

[5] T. Bak, H. Jakobsen, B. Thuilot, and M. Berducat. Automatic guidance of a four-wheel-

steering mobile robot for accurate field operations. Journal of Field Robotics 26(6–7),

504–518 (2009), 2009.

[6] G.-D. Yin, N. Chen, J.-X. Wang, and J.-S. Chen. Robust control for 4ws vehicles consid-

ering a varying tire-road friction coefficient. International Journal of Automotive Tech-

nology, Vol. 11, No. 1, page 33 40, 2010.

[7] D. Gonzalez, J. Perze, V. Milanes, and F. Nashashibi. A review of motion planning tech-

niques for automated vehicles. IEEE Transactions on Intelligent Transportation Systems,

Vol. 17, NO. 4, April 2016, 2016.

[8] K. Kurzer. Path planning in unstructured environments-a real-time hybrid A* implementa-

tion for fast and deterministic path generation for the kth research concept vehicle. Degree

Project in Vehicle Engineering, KTH Royal Institue of Technology School of Engineering

Science, 2016.

[9] Y. Kuwata, G. A. Fiore, J. Teo, E. Frazzoli, and J. P. How. Motion planning for urban driv-

ing using RRT. IEEE/RSJ International Conference on Intelligent Robots and Systems,

Sept, 22-26, 2008.

[10] B. Nordell. Trajectory planning for autonomous vehicles and cooperative driving. Exam-

ensarbete Inom Elektroteknik, Advanced Niva, 30 HP Stockholm, Sverige 2016, 2016.

33

[11] P. Belvei. Implementation of model predictive control for path following with the kth

research concept vehicle. Degree Project In Degree Program in Electrical Engineering

300 Credits, Second Cycle. Stockholm, Sweden 2015, 2015.

[12] U. Rosolia, S. De Bruyne, A. G. Alleyne, and etc. Autonomous vehicle control: A non-

convex approach for obstacle avoidance. IEEE transactions on control systems technol-

ogy, Vol. 25, NO. 2, 2017.

[13] G. C. Pereira, L. Svensson, P. F. Lima, and J. Martenssen. Lateral model predictive control

for over-actuated autonomous vehicle. IEEE Intelligent Vehicles Symposium (IV), 2017.

[14] D. J. Leith, W. E. Leithead, and M. Vilaplana. Robust lateral controller for 4-wheel

steer cars with actuator constraints. IEEE Conference on Decision and Control, and the

European Control Conference 2005, 2005.

[15] M. G. Plessen, P. F. Lima, J. Martenssen, A. Bemporad, and B. Wahlberg. Trajectory

planning under vehicle dimension constraints using sequential linear programming. 2017

IEEE 20th International Conference on Intelligent Transportation Systems (ITSC), 2017.

[16] J. Ziegler, P. Bender, T. Dang, and C. Stiller. Trajectory planning for bertha - a local,

continuous method. 2014 IEEE Intelligent Vehicles Symposium (IV) June 8-11, 2014.

Dearborn, Michigan, USA, 2014.

34

Appendix

Geometry of single track model

According to Fig. (4) and use theorem of sines, one can get:

|OC|sin(π

2 −δ f )=

l f

sin(δ f −β )(43)

|OC|sin(π

2 +δr)=

lrsin(β −δr)

(44)

Combine Eq. (43) and Eq. (44), one can get

tanβ =l f · tanδr + lr · tanδ f

l f + lr(45)

Similarly, in Fig. (5), the geometry can be found as:

|OC|sin(π

2 −δ f )=

l f

sin(δ f −β )(46)

|OC|sin(π

2 −δr)=

lrsin(β +δr)

(47)

Combine Eq. (46) and Eq. (47), one can get

tanβ =lr · tanδ f − l f · tanδr

l f + lr(48)

To simplify the problem, in this paper, the positive direction of δr is defined as the same direc-

tion with δ f . Thus Eq. (48) is equivalent to Eq.(45).

35

TRITA -SCI-GRU 2018:329

ISSN 1651-7660

www.kth.se