Trajectory Planning for Four Wheel Steering Autonomous Vehicle

IN

DEGREE PROJECT VEHICLE ENGINEERING, SECOND CYCLE, 30 CREDITS

,

STOCKHOLM SWEDEN 2018

Trajectory Planning for Four Wheel

Steering Autonomous Vehicle

ZEXU WANG

Abstract (English)

Abstract (Swedish)

I detta avhandlingsarbete presenteras en modellbaserad prediktiv kontroll (MPC) -baserad banplaneringsplan f¨or h¨oghastighetsbanan och l˚aghastighetsparametrar f¨or autonomt fyrhjulsdrift (4WS). Ett fyrhjulsdrivna fordon har b¨attre man¨ovrerbarhet med l˚ag hastighet och h¨oghastighetsstabilitet j¨amf¨ort med vanliga fr¨amre hjulstyrningar (FWS). MPC-optimal banplanerare ¨ar formulerad i en kr¨okt koordinatram (Frenet-ram) som minimerar sidof¨orl¨angnin-gen, kursfel och hastighetsfel i en kinematisk dubbelsp˚armodell av ett fyrhjulsstyrda

for-don. Med hj¨alp av den f¨oreslagna banaplaneraren visar simuleringar att ett fyrhjulsstyr-fordon kan sp˚ara olika typer av banor med l¨agre sidof¨orl¨angningar, mindre kursfel och kortare l¨angsg˚aende avst˚and.

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

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

8 Acknowledgement 32

References 33

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 transportatransporta-tion 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]

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.

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

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 optiopti-mization 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.

min . N−1

∑

(x_i|tTQ₁x_i|t+ uT_i|tRu_i|t) + xT_N|tQ_fx_N|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,

Qis 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|tis applied to the system and shift the whole problem to t + 1 to continue

the same optimizing progress.

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 ); θmis yaw angle of the vehicle; θcis path angle at point C.

Figure 3: Coordinate transformation between Cartesian and Frenet

7.

s= arc length( ~OC) (4)

e_y= sign · |VC| (5) C_c= ∂ θc ∂ s = θ 0 c (6) θc= arctan( dY dX) (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, θcis 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 − e_ysin(θc)

y= Y + eycos(θ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:

The parameters in the equation is clarified in Table 1

Table 1: Parameters clarification of the dynamic in Frenet frame

s Curvilinear Abscissa

ey 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 Path

c_c Curvature of the path

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

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 model

C Center of Gravity of the vehicle

δf Front wheel steering angle

δr Rear wheel steering angle

β Crabbing angle of the RCV

l_f Distance from the front axle to C

lr Distance from the rear axle to C

v Velocity at the COG

v_f Velocity at the front axle

vr Velocity at the rear axle

2.4

Double Track Model

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 of

Rear 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 = 2 tan δf1tan δf2 tan δf1+ tan δf2 (13) tan δr= 2 tan δr1tan δ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= v sin(θ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 δf1, δf2, δr1and δr2. Therefore, the problem now is to construct

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

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. β =lfδr+ lrδf l_f+ lr (19) κ = δf− β l_f = δf− δr l_f+ lr (20) δf = 2δf1δf2 δf1+ δf2 (21) δr= 2δr1δr2 δr1+ δr2 (22)

The relationship between κ − β and δf− δrin 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.05 0.05 0 0

-0.05 0.2 0.15 0.2 0 0.15 rear steering 0.1 front steering 0.1 0.05 0.05 0.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.

β + lfκ = δ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 ≤ β + lfκ ≤ 0.4 (27)

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]

  ˙ β ¨ ψ  =   −Cf+Cr mv Cflf−Crlr mv2 + 1 Cflf−Crlr Jz − Cfl2f+Crl2r Jzv     β ˙ ψ  +   −Cf_mv Cr_mv Cflf Jz − Crlr Jz     δf δr   (29a)   ˙ β ¨ ψ  =   −Cf_mv+Cr Cflf−Crlr mv2 + 1 Cflf−Crlr Jz − Cfl2f+Crl2r Jzv     β ˙ ψ  +   −Cf mv Cflf 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 δris turned into zero, 4WS will be identical

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+ l_r ≤ κ ≤

2δmax

l_f+ l_r (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.

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.

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.

s_k+1= s_k+ T_s·vcos(θm− θc+ β )

1 − ey,k·Cc

(32a)

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

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

v_k+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

s₁= sini (33a)

ey,1 = ey,ini (33b)

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

v₁= vini (33d)

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

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.

−1 2w− ylim≤ ey≤ 1 2w+ ylim (34a) a_dec≤ a ≤ aacc (34b) −δf,lim≤ β − lfκ ≤ δ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,limstands 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

v_{re 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 θcwith refer to sre f and S.

de-fine vre f as

v_{re 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.

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

∑

xT_i,tQ1xi,t+uTi,tRui,t+ xTN,tQfxN,t (39a)

s.t. s1= s∗1 (39b) ey,1= e∗y (39c) θm,1= θm∗ (39d) v₁= v∗_k (39e) sk+1= sk+ Ts·  v cos(θm− θc+ β ) 1 − y ·Cc  (39f)

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

θm,k+1= θm,k+ Ts· κv (39h) vk+1= vk+ Tsa (39i) −1 2w− ylim≤ ey≤ 1 2w+ ylim (39j) a_dec≤ a ≤ aacc (39k) −δf,lim≤ β − lfκ ≤ δ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. Q₁=        q_s 0 0 0 0 qy 0 0 0 0 q_θc 0 0 0 0 q_v        (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

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

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

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

0 2 4 6 8 10 12 time [s] 4 5 6 velocity [m/s] vehicle velocity reference velocity 0 2 4 6 8 10 12 14 time [s] 1 2 3 heading angle m c 0 2 4 6 8 10 12 14 time [s] 0 1 2 lateral deviation [m]

Figure 14: Velocity, heading angle and lateral deviation profile from the planner in the case shown in Figure 12 0 2 4 6 8 10 12 time [s] 0 2 4 6 velocity [m/s] vehicle velocity reference velocity 0 2 4 6 8 10 12 time [s] 0 0.5 1 1.5 heading angle m c 0 2 4 6 8 10 12 time [s] 0 1 2 lateral deviation [m]

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

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

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

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 velocity [m/s] vehicle velocity reference velocity 0 2 4 6 8 10 12 time [s] 0 0.5 1 1.5 heading angle m c 0 2 4 6 8 10 12 time [s] 0 1 2 lateral deviation [m]

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

-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

-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 velocity [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 heading angle m c 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 time [s] 0 1 2 lateral deviation [m]

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

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

-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

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 vehicle in the planner. With no crabbing ability, head-ing angle of the vehicle is always at the same direction 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

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.

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.

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 generaimplementa-tion 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 usdriv-ing RRT. IEEE/RSJ International Conference on Intelligent Robots and Systems, Sept, 22-26, 2008.

[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. M˚artenssen. 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.

Appendix

Geometry of single track model

According to Fig. (4) and use theorem of sines, one can get: |OC| sin(π 2− δf) = lf sin(δf− β ) (43) |OC| sin(π 2+ δr) = lr sin(β − δr) (44)

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

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

lf+ lr

(45)

Similarly, in Fig. (5), the geometry can be found as: |OC| sin(π 2− δf) = lf sin(δf− β ) (46) |OC| sin(π 2− δr) = lr sin(β + δr) (47)

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

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

l_f+ lr

(48)

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

TRITA -SCI-GRU 2018:329 ISSN 1651-7660