trajectory planning for four wheel steering autonomous...
TRANSCRIPT
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