Trajectory Planning for Autonomous Vehicles and...

45
INOM EXAMENSARBETE ELEKTROTEKNIK, AVANCERAD NIVÅ, 30 HP , STOCKHOLM SVERIGE 2016 Trajectory Planning for Autonomous Vehicles and Cooperative Driving BENJAMIN NORDELL KTH SKOLAN FÖR ELEKTRO- OCH SYSTEMTEKNIK

Transcript of Trajectory Planning for Autonomous Vehicles and...

Page 1: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

INOM EXAMENSARBETE ELEKTROTEKNIK,AVANCERAD NIVÅ, 30 HP

, STOCKHOLM SVERIGE 2016

Trajectory Planning for Autonomous Vehicles and Cooperative Driving

BENJAMIN NORDELL

KTHSKOLAN FÖR ELEKTRO- OCH SYSTEMTEKNIK

Page 2: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Abstract

Autonomous vehicles have been the subject of intense research, resultingin many of the latest cars being at least partly self driving. Cooperativedriving extends this to a group of vehicles called a platoon, relying on com-munication between the vehicles in order to increase safety and improve the�ow of tra�c. This thesis is partly done in context of Grand CooperativeDriving Challenge (GCDC) 2016 where KTH has participated with a Scaniatruck and the Research Concept Vehicle (RCV), an electric prototype car.

Trajectory planning is investigated for the longitudinal control of boththe truck and the RCV. This planner is to ensure that the vehicles reacheda position in a given time and a desired velocity. This is done using Pon-tryagin's minimum principle and interpolation.

A more advanced planner based on Model Predictive Control (MPC) isused to avoid collisions in two di�erent scenarios. One considers obstacleavoidance in the form of an overtake and the other a lane change scenariowere the vehicle needs to decide how to position itself relative to the othervehicles.

Simulations of the longitudinal control and planning of the truck didshow that it could time the position and speed with a position error of lessthan 2m and speed error less than 0.2 m/s, assuming a distance of 120-200m, a time interval of 40s and goal speed of 7m/s. The same simulation forthe RCV had a distance error of less than 0.3m and a speed error below0.2m.

Simulations of the RCV using MPC planners showed that overtakingand lane changes could be performed. When performing the lane change theRCV managed to maintain a longitudinal distance of at least 1m, even if theother vehicles are slowing down or increasing their speed. The overtakingcould also be successfully performed although with small margins, having alateral distance of 0.5 m to the vehicle being overtaken.

2

Page 3: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Sammanfattning

Autonoma fordon har länge varit ett intensivt forskningsområde vilketresulterat i att många av de senaste bilana är åtminstone delvis självkörande.Cooperative driving utvidgar detta till en grupp fordon som kommunicerarmed varandra för att öka säkerheten och få tra�ken att �yta bättre. Den häruppsatsen baseras på Grand Cooperative Divers Challange (GCDC) 2016där KTH deltog med en Scania lastbil och en elbil kallad Research ConceptVehicle (RCV).

Rörelseplannering har undersökts för longitudinell kontrol av lastbilenoch RCV. Den här planeraren ska se till att fordonet når en given positioninom en viss tid och med en önskad hastighet. För detta ändamål användsvad som kallas "Pontryagin's minimum principle" och interpolation.

En mer avancerad planerare baserad på Model Predictive Control (MPC)används för att undvika kollisioner i två olika situationer. Den ena simuleraren omkörning och den andra ett �lbyte med �era andra fordon i den intillig-gande �len.

Simuleringar av den longitudinella kontrollen av lastbilen visade att denkunde nå en position och hastighet med ett fel mindre än 2m respektive0,2m/s då sträckor mellan 120-200m, ett tidsintervall på 40s och önskadhastighet på 7m/s används. Samma simuleringar med RCV hade ett posi-tionsfel mindre än 0,3m och hastighetsfel under 0,2m/s.

Simuleringar med RCV då MPC används visade att omkörningar och �l-byten kunde genomföras. Filbyten kunde genomföras med ett longitudinelltavstånd på minst 1m, även då övriga fordon saktar ner eller ökar farten.Omkörningar kunde också genomföras om än med små marginaler. Det lat-erala avståndet var 0,5m till det omkörda fordonet.

3

Page 4: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Acknowledgments

The author would like to thank examiner Jonas Mårtensson, supervi-sors Rui Oliveira, Lars Svensson, Stefanos Kokogias, Pedro Lima and fellowmaster students Jonathan Fagerström and Gonçalo Collares Pereira.

4

Page 5: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Contents

1 Introduction 7

1.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71.2 Problem De�nition . . . . . . . . . . . . . . . . . . . . . . . . 9

1.2.1 GCDC Highway Scenario . . . . . . . . . . . . . . . . 101.2.2 GCDC Emergency Vehicle Scenario . . . . . . . . . . . 111.2.3 GCDC Intersection Scenario . . . . . . . . . . . . . . . 111.2.4 Autonomous Lane Change . . . . . . . . . . . . . . . . 111.2.5 Autonomous Overtaking . . . . . . . . . . . . . . . . 13

1.3 Objective . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 131.4 Related Work . . . . . . . . . . . . . . . . . . . . . . . . . . . 141.5 Report Outline . . . . . . . . . . . . . . . . . . . . . . . . . . 16

2 Trajectory Planning 17

2.1 Model Predictive Control . . . . . . . . . . . . . . . . . . . . 172.1.1 Generating the Constraints . . . . . . . . . . . . . . . 21

2.2 Longitudinal Planner . . . . . . . . . . . . . . . . . . . . . . . 212.2.1 Pontryagin's Minimum Principle . . . . . . . . . . . . 222.2.2 Interpolation Methods . . . . . . . . . . . . . . . . . . 242.2.3 Constant Acceleration planner . . . . . . . . . . . . . 25

3 Implementation and Simulation 26

3.1 Modeling the RCV . . . . . . . . . . . . . . . . . . . . . . . . 263.1.1 Parameter Values . . . . . . . . . . . . . . . . . . . . . 283.1.2 Controllers . . . . . . . . . . . . . . . . . . . . . . . . 293.1.3 Decision Making and Constraints. . . . . . . . . . . . 293.1.4 Modeling the Other Vehicles . . . . . . . . . . . . . . . 30

3.2 Truck Model . . . . . . . . . . . . . . . . . . . . . . . . . . . 303.3 Implementing the Longitudinal Planner . . . . . . . . . . . . 30

4 Results 32

4.1 Lane Change . . . . . . . . . . . . . . . . . . . . . . . . . . . 324.1.1 Case 1: Constant Speed . . . . . . . . . . . . . . . . . 324.1.2 Case 2: Slow Down . . . . . . . . . . . . . . . . . . . . 32

5

Page 6: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Contents 6

4.1.3 Case 3: Increased Speed . . . . . . . . . . . . . . . . . 354.2 Overtaking . . . . . . . . . . . . . . . . . . . . . . . . . . . . 354.3 Longitudinal Planning . . . . . . . . . . . . . . . . . . . . . . 37

4.3.1 Truck . . . . . . . . . . . . . . . . . . . . . . . . . . . 374.3.2 RCV . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

5 Conclusion 42

5.1 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

Page 7: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 1

Introduction

Signi�cant progress has been made regarding the safety of vehicles, bothto protect the occupants in the event of a crash and to reduce the risk ofan accident occurring. This includes passive safety features such as airbagsand crumble zones intended to prevent injuries caused by a collision. Theincreased capacity of embedded systems has made it possible to automatizean increasing number of functions, making active safety features such aslane keeping assist and stability systems more prevalent. Instead of simplyprotecting the occupants in the event of a crash the vehicle may prevent itfrom happening in the �rst place by assisting or warning the driver.

One approach towards increased safety could be the use of cooperativedriving relying on extensive communication between vehicles. This also hasthe potential to improve the �ow of tra�c as the vehicle can drive in awell coordinated manner, having the same speed and maintaining properdistance to each other. A strictly autonomous vehicle on the other would beless dependent on the communication with other vehicles but instead requiremore advanced algorithms for estimation and decision making.

In the GCDC the cars drive together in three di�erent scenarios, whichare further explained below. These di�erent scenarios serve as basis for thesimulations done in this thesis. A longitudinal planner is used for testing thetiming of position and speed while a more advanced planner allow the vehicleto perform more complex maneuvers without colliding with other vehicles .

1.1 Background

In the Grand Cooperative Driver Challenge (GCDC) a group of partlyautonomous vehicles are suppose to drive together and handle di�erent sce-narios that requires cooperation. KTH participated in the GCDC contestwith a modi�ed Scania truck and the Research Concept Vehicle (RCV). TheScania R450 tractor unit was used to perform longitudinal control whilesteering was still done by a human driver. The RCV was meant to perform

7

Page 8: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 1. Introduction 8

both longitudinal and lateral control but battery problems prevented it fromparticipating in the contest. Instead simulations of the RCV and the truckwere done for this thesis using Matlab and Simulink. A supervisory layerhad been made by a group of students in a previous project which performsthe tracking of vehicles and the overall decision making needed to participatein the contest.

In the GCDC the vehicles use cooperative driving, meaning that the vehi-cles communicate with each other to drive in di�erent formations dependingthe required maneuver. The information including GPS position and speedis transmitted between the vehicles using a wireless Vehicle-to-Vehicle (V2V)communication system. The supervisory layer is used to make the basic de-cisions, for example when a lane changing maneuver is to be performed orwhen the vehicle is allowed to cross an intersection.

In the intersection scenario the vehicles are required to reach the Com-petition Zone (CZ) at a given time with a certain velocity. The CZ is simplythe area at the intersection where there is a risk of collision as the vehi-cles perform their crossing maneuvers. This synchronization ensures that allvehicles start at the same position and velocity before the cooperative ma-neuvers begin. The longitudinal trajectory planing is used in this thesis totime the position of a vehicle, simulated using the RCV and a Scania truck.

When performing a lane change, such as the merging of two lanes per-formed in GCDC, a lateral trajectory is also needed. In this case the timingrequirement is less strict but the trajectory planner must generate properreferences values for the lateral control without causing too sharp turns.The generated trajectory for the lane change should make the maneuver asfast as possible while staying within parameters such as maximum lateralacceleration and keeping the distance to the other vehicles. A lane change issimulated for the RCV in this thesis but without the cooperative approach.

When using cooperative driving it is common to use platooning. Herea group of vehicles are driving together while maintaining the same speedand distance to each other. By driving really close to each other this couldreduce fuel consumption due to the reduced air drag, in particular whenusing heavy duty commercial vehicles. The preferred inter vehicle distanceis calculated as

d(t) = r(t) + hsafev(t) (1.1)

where r(t) is the preferred stand still distance and hsafe is the headwayproportional to the vehicles speed v(t). This is a form of Adaptive CruiseControl (ACC) while the front vehicle maintains a reference speed with asimple Cruise Controller (CC). Although platooning is not the focus of thisthesis, a proper distance keeping relative other vehicles is one of the mainproblems that have been investigated.

The general architecture used in GCDC is seen in Figure 1.1. The su-pervisory layer manages the communication with the other vehicles and co-

Page 9: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 1. Introduction 9

Figure 1.1 The overall structure of the control system used in GCDC andthis thesis.

ordinates the di�erent control agents. The high level controllers calculatethe desired torque and steering angle, while the low level controller thenapplies the corresponding input to the motor and actuators. The planner isactivated in the intersection and merging scenario, otherwise the referencespeed and distance to the other vehicles is given by the supervisory layer.This is essentially the same architecture used for this thesis except the plan-ner is active all the time. The low level controllers are not simulated, insteadassuming that the desired torque and steering angle are always applied.

1.2 Problem De�nition

In order for a self driving vehicle to safely navigate through tra�c it isnecessary to not only perform the basic controls but also to make properdecisions based on other vehicles positions and various tra�c rules. Whilea manually driven vehicle has its own speed readings and more recently, theGPS position, an autonomous vehicle is very dependent on several di�erentsensor data. As a bare minimum, it is necessary to know the ego vehiclesposition relative the road centerline and the distance to the other vehicles.This would be su�cient when driving on the highway when the ego vehiclecan simply adjust the steering to stay in the middle of the lane and controlthe speed to maintain the distance to the vehicle in front.

However, many situations in tra�c requires much more than simplymaintaining speed and heading. The ego vehicles must be able to deter-

Page 10: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 1. Introduction 10

Figure 1.2 The highway scenario where the merging of two lanes is required.The vehicles in the right lane can be seen opening slots between for theother platoon to occupy. The merging is then done before the start of theconstruction work. [Semsar-Kazerooni et al., 2015]

mine when it is safe to change lane or drive through an intersection usinginformation about other vehicles and the tra�c infrastructure. Using coop-erative driving, the vehicles solve this problem by exchanging informationbetween each other. These messages contain not only speed and positionbut also intended actions of the vehicles, such as an request to change laneor the intended direction in an intersection. The vehicles may also tell eachother when it is safe to perform a given maneuver.

The main drawback of cooperative driving is that a standardized protocolfor communication and decision making has to be shared between the vehicle.If a vehicle is to drive in todays tra�c with mostly human drivers, sucha protocol would not exist. This thesis will therefore focus on a purelyautonomous approach to investigate how trajectory planning can be used tonavigate a vehicle through todays tra�c. In this case the vehicle would needto make its own decisions using available information from its sensors andGPS data. It is assumed that the position and speed of the other vehiclescan be estimated with su�cient accuracy using radar or computer vision.

1.2.1 GCDC Highway Scenario

When one lane is blocked, for example due to construction work, the carsin the left lane need to switch to the right. This requires the vehicles drivingon the right lane to open the gap in order to safely perform the maneuver.

Page 11: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 1. Introduction 11

The scenario begins with two platoons of vehicles driving side by side in eachlane as seen in Figure 1.2. At some point the vehicles will receive a messagewarning of a construction work ahead that blocks the left lane. This requiresthe vehicles in the left lane to change lane before reaching an imaginary linecalled the competition zone (CZ). In order to safely perform this maneuverthe vehicle in the right lane must adapt the longitudinal distance to eachother.

A vehicle that intends to merge will �rst adapt its distance to the vehiclein front and in the adjacent lane. Once this has been done a request to mergeis sent to the rest of the platoon. The vehicle behind and in the adjacentlane then adapt its longitudinal distance to the vehicle intending to merge.When the distance is large enough a Safe to Merge (STOM) message is sentand the maneuver is performed.

1.2.2 GCDC Emergency Vehicle Scenario

In the case an emergency vehicle needs to get through, all the other carskeep to the edge of the road so that the emergency vehicle may drive alongthe centerline, see Figure 1.4. This scenario is not judged at the competitionbut was implemented as a demonstration of the e�ectiveness of cooperativedriving.

1.2.3 GCDC Intersection Scenario

In a T-shaped intersection three di�erent vehicles will come from theirown direction and should get through without colliding, see Figure 1.3. Firsteach vehicle needs to reach the competition zone (CZ) at a given time witha certain speed. At this point the vehicles will proceed with the crossingby keeping a virtual distance to each other, calculated by the di�erence indistance to a midpoint in the CZ.

1.2.4 Autonomous Lane Change

In GCDC, a merging of two lanes is done using V2V communication be-tween the vehicles to make decisions and adapt the distance to each other.This is particularly e�ective in dense tra�c where a good coordination be-tween several vehicle is required. The main problem with cooperative drivingis that it requires a standard communication protocol and supervisory layerfor all vehicles.

However, one could also consider a scenario were this cooperation be-tween the vehicles does not exist to the same degree as outlined above. Thisis a more autonomous form of driving where each decision is done indepen-dently. This scenario is shown in Figure 1.5. Rather than two lanes beingreduced to one, this has a single vehicle intending to change to the adjacentlane occupied by a number of other vehicles which could be autonomous or

Page 12: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 1. Introduction 12

Figure 1.3 Illustration of the intersection scenario. The yellow vehicles arriveat the CZ simultaneously with the same speed. The crossing is then doneby maintaining the virtual distance between the vehicles. [Semsar-Kazerooniet al., 2015]

Figure 1.4 The emergency vehicle driving between the lanes. This is madepossible by having the other vehicles driver closer to the road delimiter.[Semsar-Kazerooni et al., 2015]

Page 13: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 1. Introduction 13

Figure 1.5 Lane change scenario where a vehicle is to �nd a slot betweenthe vehicles in the adjacent lane.

human driven. Such a scenario requires more decision making by individualvehicles and less assumptions can be made by the other vehicles position andintended actions.

In this scenario the vehicle that is to perform the lane change must �rstdecide which slot between the cars that is the most feasible. Once this hasbeen done the car should adapt its longitudinal position so that it is adjacentto the preferred slot, then perform the lane change.

1.2.5 Autonomous Overtaking

Aside from the scenarios described for the GCDC there are many othersituations that an autonomous vehicle must be able to handle. The statemachine used in the supervisory layer for the GCDC cannot make decisionsfor an overtaking scenario, thus a decision rule must be made for that. Thisplanner needs to take into account the speed and position of the vehicleahead as well as those in the adjacent lane, which could have approachingtra�c or not. If a collision with the opposing car is deemed too likely thecar should wait behind the obstacle before going around it. This scenariowould be particularly relevant in the case where one lane is being blockedby an obstacle, such as a stationary vehicle, or a very slow moving one e.g.a tractor.

1.3 Objective

The main objective in this thesis is to investigate how trajectory planningcan be used in autonomous vehicles. Two kinds of planners are developed,one for longitudinal control and another one which also does lateral planning.These planners are tested using simulation of the RCV and a Scania truck. In

Page 14: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 1. Introduction 14

Figure 1.6 The overtaking scenario simulated in addition to the GCDCscenarios. The vehicle in the lower lane encounter a stationary or slowervehicle while another one is driving in the opposite direction.

all cases it is assumed that the speed and position are known for all vehicles.The planner for longitudinal control is used to ensure that the vehicle

can reach a given position and speed at a speci�c time instant. This plannershould give a speed reference without excess acceleration and jerk in orderto make it easy to follow.

In order to safely drive in tra�c with other vehicles a more advancedplanner is needed to control both the lateral and longitudinal position. Thisplanner do not consider timing requirements but instead ensure that the egovehicles can avoid collisions with other vehicles. The planner must be ableto generate a trajectory without collision in a lane change and a overtakingscenario. Aside from the planner this also require a decision rule that candetermine when the given maneuver is safe.

1.4 Related Work

A trajectory may be generated by using interpolation between di�erentstates at the given time instants. This has been used by [You et al., 2015]to generate a trajectory for a lane changing maneuver. Similarly to GCDC,this was done in a cooperative manner where the vehicles communicate witheach other and share a supervisory layer. The �nal time instant tf shouldbe selected in a manner that makes the lane change quick yet ensure thecomfort and safety for the occupants of the car. This �nal time could becalculated as the expected time interval using maximum acceleration andjerk in order to make the maneuver as fast as possible.

This would then be an approximation of a time optimal planner whileallowing for recalculations in order to be more �exible for the current state

Page 15: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 1. Introduction 15

of the vehicle. The motivation for using a time optimal planner in the lanechanging scenario is to make the maneuver as quick as possible while stillbeing comfortable and safe. The limits on the lateral acceleration and jerkof the vehicle can not be guaranteed with this method, however. By scalingthe interpolation coe�cients with maximum acceleration or jerk as donein [Erkorkmaz et al., 2011] the trajectory will stay within these constraintsat the cost of increased time for the maneuver. As interpolation has beenproven to work by [You et al., 2015] it could be useful for the highway scenarioin GCDC.

An energy optimal planner is used to generate a trajectory from an initialstate x(t0) = x0 to the �nal state x(tf ) = xf while minimizing the requiredenergy. Such problems are commonly solved using Pontryagin's minimumprinciple as done in [Tokekar et al., 2014] to �nd an optimal trajectory for acar like robot, having a given starting and goal position as well as orientation.For the mobile robot the energy is calculated using the voltage and currentof the DC-motors. By assuming a prede�ned path that the robot is to moveon the problem is reduced to a one dimension for the trajectory with x(t)being the distance traveled on the path and v(t) the speed. Aside from theenergy minimization this approach is also useful when a given state is tobe achieved at a speci�c time instant. This is the case in the intersectionscenario in GCDC which requires the vehicles to have a certain speed andposition at some time instant. The simpli�cation to one dimension can alsobe made here since the vehicle will follow the centerline of the lane so thetrajectory only needs to give a proper velocity pro�le. While this could alsobe done using interpolation, the minimal squared acceleration ensure thatthe trajectory is easy to follow. The energy optimal planner could also beused for the lane changing maneuver but this does not necessarily give anacceleration equal to zero at the end point unless an additional constraint isput on the �nal states which increase the complexity on the calculations.

In [Falcone et al., 2008] Model Predictive Control (MPC) has been usedboth for trajectory planning and control of autonomous ground vehicles. Atrajectory is �rst calculated o�-line for the vehicle to follow. During the drivethis trajectory can be recalculated when triggered by certain events, such asa large tracking error or pop-up obstacles. The trajectory is calculated byminimizing a cost function of deviation from the desired path and velocityas well as a penalty on the acceleration. An MPC was also used as an ActiveFront Steering to select the steering angle that minimizes the heading error.As noted by the authors the MPC is useful for handling the constraints oninputs and states but the computational complexity limits the use to lowerspeed.

A similar problem has been studied in [Farrokhsiar et al., 2013] whereMPC is used for both motion planning and high level control of an au-tonomous robot. The trajectory here is calculated in two steps, �rst thenominal trajectory which the robot would ideally follow, then an auxiliary

Page 16: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 1. Introduction 16

controller which adjusts the trajectory with regard to the current state ofthe robot. As MPC is already used for the control of the RCV in GCDCa more simple trajectory generator is preferable to limit the computationalcost. It is however simulated for the autonomous lane change scenario.

As shown by [Grancharova et al., 2014] it is also possible to plan the mo-tion of several vehicles simultaneous, in this case rotary wing UAVs. Asidefrom constraints on acceleration, velocity and position the trajectory plan-ning also considered path losses for the radio communication. The latter isrelevant in cases where there are signi�cant distances between vehicles andpossibly obstacles in the communication path. This should not be the casewith cooperative driving as other vehicles are assumed to be close to eachother, instead assuming that the communication works without any problem.

The trajectory planning of autonomous vehicles along a prede�ned pathhas also been investigated by [Li et al., 2014]. This was done using a sampledbased approach to generate feasible trajectories in a dynamic environment.From the prede�ned path a number of �nal states including position (x, y),heading θ and curvature κ are sampled. A path is then generated for each�nal state using a numerical optimization based on the input constraints.The optimal path is then determined by a weighting function of deviationfrom the path, smoothness, obstacle proximity and length. The trajectoryis then generated by adding a velocity pro�le to this path. The motivationfor this approach is that a navigation system using a GPS could generate apath to a �nal destination but not account for vehicles and other dynamicalobstacles. Thus a local trajectory planner is needed which could adjust forsuch problems and also generate proper velocity pro�les.

1.5 Report Outline

In Chapter 2 the overall theory of the methods used for trajectory plan-ning is explained, including MPC, PMP and interpolation. The implementa-tion details are outlined in Chapter 3, including description of the hardware,system architecture, vehicle model and simulation setup. This is followedby the results which are discussed in Chapter 4 and the thesis ends with aconclusion and comments on future works.

Page 17: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 2

Trajectory Planning

Trajectory planning, also called motion planning, generates a path as afunction of time. Unlike a path which is simply a sequence of coordinates(xk, yk) that can be followed with an arbitrary speed, the trajectory can beexpressed as

(xk, yk, tk), k = 0, 1, 2...N (2.1)

In many applications the trajectory functions as a time varying refer-ence value for position and/or velocity being fed to a controller. Trajectoryplanning occur mainly in two di�erent areas, which is industrial robots andautonomous vehicles. There are several reasons for having a trajectory ratherthan a constant speed reference. In many cases a strong acceleration andjerk is undesired, which may be prevented by having the trajectory withinthe given limits of acceleration and jerk. In addition, the trajectory plannermay also function as a path �nder in order to avoid obstacles in the environ-ment. Finally, a movement may have timing requirements for the positionand speed which make trajectory planning necessary.

In this thesis, a Model Predictive Control (MPC) was used for the sim-ulation of overtaking and lane change scenario. For the longitudinal controlmore simple planners were selected as it was only required to control speedand position in one dimension and other vehicles were not considered.

2.1 Model Predictive Control

Model Predictive Control (MPC) is a form of optimal control, solving anoptimization problem with constraints. This approach is particularly usefulwhen the states and inputs are saturated. In the case of the autonomousvehicles this could include a maximum allowed acceleration or a minimum

17

Page 18: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 2. Trajectory Planning 18

distance to other vehicles. The optimization problem may be formulated as

Minimize∑N

k=1 J(xt+k, ut+k)subj. xt+k+1 = f(xt+k, ut+k)

g(xt+k, ut+k) ≤ blx ≤ xt+k ≤ uxlu ≤ ut+k ≤ uu

(2.2)

Here f(xt+k, ut+k) is a discrete time state space model of the system de�nedas

x(t+ 1) = Ax(t) +Bu(t) + v1(t)y(t) = Cx(t) + v2(t)

(2.3)

with states x(t), output y(t) and input u(t) while A,B, and C are matrices.The cost function J(xt+k, ut+k) that is to be minimized is

J(xt+k, ut+k) = Q1(xt+k − rt+k)2 +Q2u2t+k (2.4)

where rt+k is the reference value for the states while Q1 and Q2 are theweight matrices for the states and inputs, respectively. The process andmeasurement noise, v1 and v2, is modeled as zero mean Gaussian noise. Themaximum like-hood prediction for the future states and output are then

x(t+ k + 1|t) = Ax(t+ k|t) +Bu(t+ k)y(t+ k|t) = Cx(t+ k|t) (2.5)

Assuming that C = I These predictions may be formulated as [Glad andLjung, 2000]

x(t+N |t)......

x(t+ 1|t)

=

AN

...

...A

x(t|t) +

B . . . AN−1B

0. . . AN−2B

......

0 . . . B

u(t)

= Sx(t) +Gu(t)

(2.6)

The planner assumes a simple model on the form

X = vcos(θ)

Y = vsin(θ)v = a

θ = ωω = u

(2.7)

HereX and Y are the road coordinates while v is the speed, θ the headingand ω the yaw rate. The linear and angular acceleration a and u is treatedas input. Since the input are integrated twice they may be zero for most of

Page 19: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 2. Trajectory Planning 19

the prediction horizon while still causing a change in the system states. Theamount of prediction steps M for the input can then be much fewer than forthe output, thus reducing the computational cost. In order to use MPC thestate space model needs to be linearized and transformed to discrete timeusing Euler forward and Taylor approximation. The state space model usedis then

X(k + 1) = X(k) + Ts[v(k)cos(θ0)−∆θ(k)v0sin(θ0)]Y (k + 1) = Y (k) + Ts[v(k)sin(θ0) + ∆θ(k)v0cos(θ0)]v(k + 1) = v(k) + Tsa

∆θ(k + 1) = ∆θ(k) + Tsωω(k + 1) = ω(k) + Tsu

(2.8)

where v0 and θ0 are the point of linearizion and ∆θ(k) = θ(k)− θ0.

MPC As a QP Problem

By having all predicted states and inputs in a vector z this can be for-mulated as the Quadratic Problem (QP)

Minimize zHzT

subj. Aeqz = beqAz ≤ bzmin ≤ z ≤ zmax

(2.9)

which may be solved with numerical algorithms such as the simplex method.Having the vector z on the form

z = [xk+1 − rk+1 . . . xk+N − rk+N uk . . . uk+M ]T (2.10)

the equality constraint yields

beq = −Sx(t)Aeq = [−I G]

(2.11)

The Hessian matrixH is selected to give the cost function∑N

k=1 J(xt+k, ut+k).The saturation on z limits the speed, acceleration and ensure that vehiclemaintains a proper distance and stays on the road. The inequality constraintis used in the overtaking scenario to force the vehicle to the other lane andgo around the obstacle. This can be done by having the inequality constraint

Xk+i −dsafedlane

Yk+i ≤ Xobstacle − dsafe, i = 1, 2 . . . , N (2.12)

where Xobstacle is the position of the obstacle, dsafe the distance behind theobstacle where it should start performing the overtaking and dlane is thewidth of the lane. This constraint can be visualized as a straight line goingdiagonally past the obstacle into the adjacent lane, see �gure 2.1.

Page 20: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 2. Trajectory Planning 20

Figure 2.1 Illustration of the constraints on position, shown in red, whenan overtaking is performed. 1: While waiting for the opposing vehicle topass, the ego vehicle is contained in its own lane behind the obstacle. 2:When overtaking the obstacle these constraints are extended in order toguide the ego vehicle to the opposing lane. 3: While adjacent to the obstaclethe ego vehicle is con�ned to the opposing lane. 4: Having gained enoughlongitudinal distance the ego vehicle may return to its own lane.

Page 21: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 2. Trajectory Planning 21

2.1.1 Generating the Constraints

The constraints on the ego vehicle's position are necessary in order toavoid collision and to keep the vehicle on the road. As seen in �gure 2.1these constraints are adapted to four di�erent situations when an overtakingis performed:

1. If it is determined that an overtaking should not be done, the limits inthe lateral and longitudinal position takes the form of a rectangle. Theego vehicle will then be con�ned in its own lane behind the obstacle.

2. When an overtaking is deemed to be safe, the constraint on the lat-eral position is expanded to include the entire road. In addition, theinequality constraints on the position forms a diagonal line behind theobstacle. Since the ego vehicle tries to maintain the reference speed itwill drive to the opposing lane which allow it to drive further.

3. As son as the ego vehicle is in the opposing lane the constraints on thelongitudinal position are removed to allow the ego vehicle to drive pastthe obstacle. Only the lateral constraints are used in order to avoidcollision.

4. Having gained enough longitudinal distance to the obstacle the lateralconstraints are expanded to its own lane. Since the reference set pointfor the lateral position is in its own lane, the ego vehicle will completethe overtaking by steering back to said lane.

As for the lane change scenario, the position constraints takes two di�er-ent forms as seen in �gure 2.2. The inequality constraint is not used here,only box constraints on the position.

1. While the longitudinal distance to the other vehicles are too small theego vehicle will be con�ned to one lane. In this case the controllerwill adapt the longitudinal position to get the ego vehicle properlypositioned between the vehicles since the set point for the position isin the middle of the available slot.

2. Having adapted the longitudinal position, the constraints on the lateralposition is expanded to the whole road. Combined with the longitudi-nal constraints this forms a box around the ego vehicle to make surethat no collision occurs.

2.2 Longitudinal Planner

The longitudinal planner is used to time speed and position of the vehicle.It is simulated separately from the scenarios using MPC and does not take

Page 22: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 2. Trajectory Planning 22

Figure 2.2 The constraints shown in red when performing a lane change. 1.The ego vehicle is con�ned to its own lane when adjacent to another vehicle.2. Having adapted the longitudinal position th ego vehicle may perform thelane change.

the lateral position into account. Two di�erent planers are considered in thisthesis for longitudinal planning. One is based on Pontryagin's MinimumPrinciple (PMP) in order to minimize the required acceleration. A morestraightforward method using interpolation is also used as a comparison.The interpolation method was initially intended for the lateral planning inthe highway scenario used in GCDC but since the RCV could not participatein the contest it is instead compared with PMP.

2.2.1 Pontryagin's Minimum Principle

An energy optimal planner is used to generate a trajectory from an initialstate x(0) = x0 to the �nal state x(tf ) = xf while minimizing the requiredenergy. Aside from the energy minimization this approach is also useful whena given state is to be achieved at a speci�c time instant. This is the casein the intersection scenario in GCDC which requires the vehicles to have acertain speed and position at some time instant. While this could also bedone using interpolation, the minimal squared acceleration ensures that thetrajectory is easy to follow.

The optimization problem is formulated as [Tokekar et al., 2014]

min∫ tf

0 f0(x, u)dtsubj. x = f(x, u)

x(0) = x0

x(tf ) = xf

(2.13)

Page 23: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 2. Trajectory Planning 23

where the input u is the acceleration a and states x = [p v]T are theposition p and velocity v. By choosing f0(x, u) = u2 the square accelerationmay be minimized. In the one dimensional case the vehicle dynamics f(x, u)is modeled with a simple continuous time state space model as

x =

[0 10 0

]x+

[01

]u (2.14)

To �nd the optimal input the Hamiltonian function is de�ned as

H(λ, x, u) = f0(x, u) + λT f(x, u) = u2 + λ1v + λ2u (2.15)

where λ = [λ1 λ2] are the Lagrangian multipliers. These are de�ned by thedi�erential equations

λ = −∂H∂x

=

[0−λ1

](2.16)

Since λ1 = 0 and therefore λ1 = c1 the solution for λ2 is

λ2 = −λ1t+ c2 = −c1t+ c2 (2.17)

where c1 and c2 are unknown constants. The optimal control signal u∗ isthen found as the solution to

u∗ = argminu

H(λ, x, u) = argminu

[u2 + λ1v + λ2u] (2.18)

which is given by

u∗ = −λ2

2=c1t− c2

2(2.19)

Inserting the optimal input into the state space model yields

p = vv = c1t−c2

2

(2.20)

By integrating the acceleration to get the speed di�erence

vf − v0 =

∫ tf

0

c1t− c2

2=c1t

2

4− c1t

2(2.21)

the parameter c2 is obtained as

c2 =2

tf

(c1t

2f

4− vf + v0

)(2.22)

In order to �nd c1, the acceleration is integrated again to get the position as

pf = v0tf +c1t

3f

12−c2t

2f

4(2.23)

Page 24: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 2. Trajectory Planning 24

Substituting 2.22 into 2.23 then yields the solution

c1 = −12−(vf − v0)tf − 2tfv0 + 2pf

t3f(2.24)

2.2.2 Interpolation Methods

A trajectory can be generated by using interpolation between di�erentpoints at the selected time instant. This has been used by [You et al., 2015]to generate a trajectory for a lane changing maneuver. This interpolationcould be done between the initial states

S0 = (x0, x0, x0, y0, y0, y0) (2.25)

and the �nal states

Sf = (xf , xf , xf , yf , yf , yf ) (2.26)

where x and its time derivatives is the longitudinal position, velocity andacceleration while y is the lateral equivalents. The longitudinal and lateraltrajectories can be calculated separately assuming two di�erent polynomialsfor the positions on the form

x(t) =∑N

0 aiti

y(t) =∑M

0 biti

(2.27)

where N and M are the order of the polynomials while ai and bi are thecoe�cients. The time derivatives of these functions then give the velocitiesas

f(x, t) =d∑N

0 aiti

dt

f(y, t) =d∑M

0 biti

dt

(2.28)

Similarly, the acceleration is given by a second derivation. By, for exam-ple, using N = M = 6 the longitudinal trajectory may be calculated as theleast square solution to the equation

x0

x0

x0

xfxfxf

=

t50 t40 t30 t20 t0 15t40 4t30 3t20 2t0 1 020t30 12t20 6t0 2 0 0t5f t4f t3f t2f tf 1

5t4f 4t3f 3t2f 2tf 1 0

20t3f 12t2f 6tf 2 0 0

b5b4b3b2b1b0

(2.29)

Page 25: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 2. Trajectory Planning 25

and similarly for the lateral trajectory. If used to perform a lane changemaneuver the �nal states for the lateral trajectory would be equal to thecenterline of the adjacent lane while the lateral speed and acceleration shouldbe zero. In this case �nal time instant tf should be selected in a manner thatmakes the lane quick, yet ensures the comfort and safety for the occupants ofthe car. This �nal time could be calculated as the expected time interval forthe maximum acceleration, maximum jerk planner. However, in this thesisthe interpolation is used for the same purpose as the PMP.

2.2.3 Constant Acceleration planner

Since the planner described above was not ready for implementation atthe time the competition was held, a more simple planner was used instead.This planner calculates the constant acceleration required to reach the �nalvelocity and then keep the vf for the time required tf travel the distance pf .This acceleration a(t) may be calculated as

pf =vfT1

2 + vfT2 =v2f

2a(t) + vf (tf −vfa(t))

⇒ a(t) =v2f

2(vf tf−pf )

(2.30)

Page 26: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 3

Implementation and Simulation

'This chapter describes how the simulations were done in Matlab and

Simulink. First, the vehicle models used in the simulations are describedfollowed by the implementation of the planners.

3.1 Modeling the RCV

Figure 3.1 The bicycle model used to simulate the RCV.

The vehicle is simulated using the bicycle model as seen in Figure 3.1.This dynamical model assumes equal forces on the left and right wheels butdi�erent dynamics on the rear and front wheel pair [Turri, 2015]. Typicallythis is modeled as

mx = Fxfcos(δ) + Fxr − Fyf sin(δ)− Fm − FI x2 +mΨy

my = Fxf sin(δ) + Fyfcos(δ) + Fyr −mΨx

IΨ = df [Fyfcos(δ) + Fxf sin(δ)]− drFyr

(3.1)

26

Page 27: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 3. Implementation and Simulation 27

where m is the vehicle mass, δ the steering angle, Ψ the heading while dfand dr is the distance to the center of gravity from the front and real axis,respectively. The x and y coordinates are the longitudinal and lateral posi-tion relative the vehicle. These can be transformed to the roads coordinatesystem (X,Y ) as

X = xcos(Ψ)− ysin(Ψ)

Y = xcos(Ψ) + ysin(Ψ)(3.2)

The driving forces Fxf and Fxr on the front and rear wheel par is calcu-lated as

Fxf = ksplitTr

Fxr = (1− ksplit)Tr(3.3)

where T is the torque, ksplit the percentage of torque to the front and r thewheel radius. Given a vehicle with the mass m, air resistance c and frontalarea A, the resistive longitudinal forces Fm and Fi are calculated as

Fm = mgfrFI = 1

2ρcA(3.4)

Here Fm is the motion resistance due to friction between the tires and theroad while FI is the proportional constant of the squared longitudinal speedassuming the gravitational constant g, air density ρ and friction coe�cientfr. The lateral forces on the front and rear axis, Fyf and Fyr respectively,are calculated as

Fyf = −Cfαf

Fyr = −Crαr(3.5)

where Cf and Cr are the cornering sti�ness for the front and rear axis whiletheir slip angles are calculated as

αf = tan−1(y+Ψdf

x

)− δ

αr = tan−1(y−Ψdr

x

) (3.6)

The discrete time state space model is then

X(k + 1) = X(k) + Ts[vx(k)cosΨ(k)− vy(k)sinΨ(k)]Y (k + 1) = Y (k) + Ts[vx(k)cosΨ(k) + vy(k)sinΨ(k)]Ψ(k + 1) = Ψ(k) + Tsω(k)

vx(k + 1) = vx(k) + Tsm [Fxfcos(δ) + Fxr − Fyf sin(δ)− Fm − FIvx(k)2 +mω(k)vy(k)]

vy(k + 1) = vy(k) + Tsm [Fxf sin(δ) + Fyfcos(δ) + Fyr −mω(k)vx(k)]

ω(k + 1) = ω(k) + TsI [df [Fyfcos(δ) + Fxf sin(δ)]− drFyr]

(3.7)where vx and vy is the longitudinal and lateral speed, respectively, while Tsis the sampling time and ω the angular velocity.

Page 28: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 3. Implementation and Simulation 28

3.1.1 Parameter Values

The overtaking and �le change scenarios are simulated in Matlab usingthe discrete time bicycle model of the RCV with a sample time of Ts = 0.01s.The parameters used in the simulation of the RCV are the following:

Parameter Abbreviation Value

Vehicle mass m 608 kgMoment of Inertia I 1000 kgm2

Equivalent mass of wheel inertia mj 40 kgGravity constant g 9.81 m/s2

Distance from front axle to CoG lf 1.0921 mDistance from rear axle to CoG lr 0.9079 mWheel radius r 0.3 mAir density ρ 1.23Projected frontal area A 2.25 m2

Friction coe�cient fr 0.0927Air drag coe�cient c 1.0834Cornering Sti�ness front wheel Cf 2.5669 ·10−4 N/radCornering Sti�ness rear wheel Cr 2.5669 ·10−4 N/rad

The other vehicles are assumed to have an approximately constant speedand stay in their lane. It is also assumed that their speed and position canbe estimated with su�cient accuracy.

Recalling that x = [X Y v Ψ ω]T and u = [a uΨ]T the weight matricesused for the lane change scenario are

Q1 =

1 0 0 0 00 10 0 0 00 0 100 0 00 0 0 1 00 0 0 0 1

Q2 =

[1 00 1

](3.8)

And for the overtaking

Q1 =

1 0 0 0 00 10 0 0 00 0 1 0 00 0 0 1 00 0 0 0 1

Q2 =

[1 00 1

](3.9)

More weight were selected for the speed in the lane change scenario sincethe RCV needs to keep up with the speed of the vehicles in the adjacentlane. The weight for the lateral position is to ensure that the RCV stays in

Page 29: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 3. Implementation and Simulation 29

the middle of the lane as much as possible. The prediction horizon used hadN = 20 prediction steps for the states and M = 1 for the inputs, while theprediction step time Tpre where 0.2 seconds, giving a prediction horizon of 2seconds.

3.1.2 Controllers

Simple lateral and longitudinal controllers were made to test the planners.The longitudinal controller output torque T proportional to the speed error

T = PT (vr − v) (3.10)

where vr is the speed reference given by the MPC and v the current speed.The lateral controller changes the steering angle δ proportionally to the errorin yaw rate:

δ = P (ωr − ω) (3.11)

where ωr and ω is the preferred and current yaw rate, respectively. For thelongitudinal controller Pt was set to 2000 while the lateral controller hadP = 10 for the obstacle avoidance and P = 5 for the lane change.

Since the MPC planner use linear and angular acceleration as inputswhile the control instead use speed and yaw rate, the MPC outputs thepredicted vehicles state to the controllers rather than its input. Simple givingthe vehicle the speed v(k + 1|k) and yaw rate ω(k + 1|k) would give thepreferred speed and yaw rate in 0.2s rather than how the vehicle shouldbehave right now. By instead using the simulation sample time Ts = 0.01the references for the controller is generated as

vr = v + Tsaωr = ω + TsuΨ

(3.12)

3.1.3 Decision Making and Constraints.

In the overtaking scenario the vehicle �rst decides if it should wait behindthe obstacle or perform the overtaking. This is done by calculating the ex-pected time it takes for the opposing vehicle to be adjacent to the obstacle. Ifthis time is less than 10 seconds the MPC gets a constraint on the maximumlongitudinal position equal to the obstacle position minus a safe distance of8m. Otherwise the overtaking is performed using the inequality constraintto force the vehicle into the other lane. Once the vehicle is approximatelyadjacent to the obstacle this inequality is replaced by a lower limit on thelateral position.

In the lane changing scenario the vehicle is �rst con�ned to its own laneby the upper and lower limits on the lateral position. The slot closest to thevehicle is chosen as a reference position. When its longitudinal position isclose enough this constraint is extended to the adjacent lane and the vehiclestarts performing the lane change.

Page 30: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 3. Implementation and Simulation 30

3.1.4 Modeling the Other Vehicles

The other vehicles simulated for the lane change and overtaking did nothave a detailed model. The position pi(k) for vehicle i is calculated as

pi(k + 1) = pi(k) + Tsvi(k) (3.13)

using the current speed vi(k) and sample time Ts.

3.2 Truck Model

In order to simulate the truck a Simulink function block written in Cis used. This discrete time model provided by Scania takes the current andinitial states of the truck as well as the speed request, using the trucks built inCC. This function block also takes a curvature request which was set to zeroin these simulations since only longitudinal control was investigated. Thismodel outputs the next states of the vehicle, in this case the longitudinalspeed and position was used.

3.3 Implementing the Longitudinal Planner

Figure 3.2 Block diagram illustrating how the longitudinal planner is imple-mented.

In order to calculate the longitudinal trajectory needed to reach the CZwith a given velocity in a certain time a Matlab function calculates thereference acceleration ar(t). This is done using PMP or interpolation as wasshown in section 2.2, using a �nal position pf , speed vf and time tf as wellas initial speed v0. In the case of PMP the function parameters c1 and c2

are calculated once then outputting the reference acceleration at each timeinstant as

ar(t) =c1t− c2

2(3.14)

For some combinations of the �nal states tf , pf and vf the referenceacceleration may be initially negative, thus giving a negative speed reference

Page 31: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 3. Implementation and Simulation 31

which is generally not desirable. This typically happens when the averagespeed required by the vehicle is much lower than the �nal speed vf . Toprevent this the planner will wait until the the average speed required toreach the the �nal position pf is at least a third of the �nal speed, that is

pftf≥vf3

(3.15)

Since the vehicle uses a Cruise Controller, the reference acceleration isintegrated to give the reference speed vr(t) as seen in �gure 3.2. If the vehiclefollowed this speed all the time it would also arrive at the CZ within thepreferred time instant. However, since the vehicle's speed is likely to di�erfrom this reference, an error in position will accumulate and thus cause itto arrive at the CZ at the wrong time. To adjust for this error in positiona simple feedback loop is added in order to adjust for the error in position.This adjustment is simply a P-controller, where the constant K determineshow this error a�ects the adjusted speed reference vref (t).

The reference position pr(t) is calculated simply by integrating vr(t) whilethe vehicles position p(t) is calculated as

p(t) = d0 − d(t) (3.16)

where d0 is the initial distance to CZ, used as pf , and d(t) is the currentdistance. The distance is calculated by a separate Matlab function using theGPS position of the vehicle and the CZ.

Page 32: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 4

Results

This chapter shows the main results that were obtained from simula-tions. First the Lane change scenario with RCV is showed, followed by theovertaking scenario. The lane change scenario is simulated in three di�erentscenarios with constant, increasing and decreasing speed, respectively. Theovertaking is performed both with a stationary target and a slow movingvehicle. Finally, longitudinal planning and control are shown for both theRCV and the truck, �rst with PMP and then interpolation.

4.1 Lane Change

4.1.1 Case 1: Constant Speed

In Figure 4.1 a simulation of the lane change using a MPC planner isshown. All vehicles have an initial speed of 10 m/s and those in the adjacentlane are spaced 10 m from each others point position. The controlled vehiclestarted at 40 m and selected the slot behind the lead vehicle in the adjacentlane. Before crossing the roads centerline, indicated by its position beingplotted in blue, the ego vehicle is approximately adjacent to the lead vehicle.It then slows down to �t between the lead vehicle and the one right behind.

In �gure 4.2 the lateral position of the ego vehicle is shown as it movesbetween the centerlines of each lane, a distance of 3m. The maneuver isdone in approximately 10 seconds and the ego vehicle is 5m behind the leadvehicle when crossing the road centerline. Since this is the distance betweenthe vehicles' point positions, the actual distance would be approximately 1massuming a vehicle length of 4m.

4.1.2 Case 2: Slow Down

The same scenario is shown in 4.3, except here the speed of the othervehicles is not constant. After 10 second the speed slows down from the initial10 m/s with an acceleration of -2m/s2. This slow down starts approximately

32

Page 33: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 4. Results 33

Figure 4.1 Simulation of a vehicle �nding a slot between the other vehiclesand changing lane, showing the longitudinal position as a function of time.The ego vehicle is shown in blue while in the its initial lane, then in red whencrossing the road centerline. The other vehicles position are shown in green.

Figure 4.2 The lateral position of the ego vehicle during the lane changemaneuver.

at the same time as the ego vehicle crosses the roads centerline. This slowdown can also be seen at the lateral position shown in �gure 4.4. Compared

Page 34: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 4. Results 34

with case 1, the ego vehicle has greater di�culty in performing the lanechange as it must also adapt the speed to the vehicles slowing down.

Figure 4.3 The lane change performed as in case 1, except for a slow downin speed after 10 seconds.

Figure 4.4 The lateral position of the ego vehicle when changing lane whileslowing down.

Page 35: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 4. Results 35

4.1.3 Case 3: Increased Speed

The simulation was also done with an increase in speed after 10s, usingan acceleration of 2 m/s2. As seen in �gure 4.5 the ego vehicle start crossthe road centerline just before the other vehicles increases their speed. Theego vehicle completes the lane change as seen in 4.6 but does not reducethe lateral speed which might cause the vehicle to get too close to the roaddelimiter.

Figure 4.5 The lane change performed as in case 1, except for an increasein speed after 10 seconds.

4.2 Overtaking

A simulation of an overtaking scenario is shown in �gure 4.7. The obstacleis stationary and initially 50 m ahead of the vehicle while an opposing carwith a speed of 10 m/s (36 km/h) is driving in the opposite direction, starting100 m in front of the car. The vehicle being guided by the MPC has thesame initial speed as the opposing vehicle and drives past the obstacle atapproximately 2.5 m distance from the cars central axis. Assuming that thecars are approximately 2m wide this should be enough although barely. Asseen in the speed pro�le in Figure 4.9 the vehicle had to stop for a briefmoment to let the opposing vehicle pass.

In Figure 4.8 a similar situation is shown except the obstacle is movingwith a speed of 2 m/s. The RCV then takes longer time to pass the obstaclebut maintain a lateral distance of approximately 2.5m. The ego vehicle doesnot completely stop as seen in �gure 4.11, which is expected as the vehiclein front is not stationary. The turning is also less sharp compared to thestationary case as seen in �gure 4.12.

Page 36: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 4. Results 36

Figure 4.6 The lateral position of the ego vehicle when changing lane in-creasing the speed.

Figure 4.7 The vehicles trajectory seen in blue overtaking a stationary objectmarked in red.

Page 37: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 4. Results 37

Figure 4.8 The overtaking scenario with a slow vehicle driving at 2 m/s.

Figure 4.9 Speed for the vehiclewhen overtaking an stationary ob-stacle.

Figure 4.10 The heading of the ve-hicle relative the road in the samescenario.

4.3 Longitudinal Planning

4.3.1 Truck

Longitudinal control of the truck was simulated with the PMP to see howwell it can achieve the desired time, speed and position for the intersectionscenario. The truck is generally more di�cult to control than the RCV,mainly due to the slow dynamics of its gearbox. The delay caused by the gearchange can be seen in the speed pro�le in Figure 4.13 showing small intervalswhere the acceleration is low. The reference speed is approximatively 2 m/sfor the �rst 17-18 seconds of the simulations as the planner waits for the timeleft to be smaller and thus give a better speed pro�le. The truck managed toreach the speed of 7 m/s after 40s and also reach the longitudinal position

Page 38: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 4. Results 38

Figure 4.11 Speed for the vehiclewhen overtaking an moving vehi-cle.

Figure 4.12 The heading of the ve-hicle relative the road in the samescenario.

Figure 4.13 The speed of the truckin blue and the reference speed inred.

Figure 4.14 The position of thetruck in blue and reference in red.

of 100m as seen in Figure 4.14.In all simulations the �nal speed was 7m/s, goal time 40s and initial speed

2m/s, while the �nal position was varied. The simulations were then repeatedusing interpolation instead of PMP. As seen in �gure 4.15 the position erroris approximately the same for PMP and interpolation for distances up to200m. At 240 and 280m the interpolation method does however give aposition error that is at least 2m greater than for PMP. The exact errorsfor both speed and position are seen in the table below. While the errorin speed is insigni�cant in both cases the planning using PMP performedbetter regarding the position error when a greater distance was covered.The improved performance could be a result of PMP minimizing the squareacceleration.

Page 39: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 4. Results 39

Figure 4.15 The distance error for the truck simulated with di�erent goalpositions. The errors using PMP are shown in red and the errors withinterpolation are shown in blue.

Distance Distance error, PMP Speed error PMP

120m 2.0822m -0.0633/s160m 1.0015 -0.0229/s200m 0.3474m -0.0198m/s240m -0.3020 -0.1158m/s280m -3.4158m -0.7890m/s

Distance Distance error, interpolation Speed error, interpolation

120m 2.14352m -0.5999/s160m 0.6818m -0.0592/s200m 0.1663m -0.0198m/s240m -2.4886m -0.2951m/s280m -6.5315m 0.4617m/s

4.3.2 RCV

Similar simulations were done with the RCV which however had an initialspeed of 0m/s. The controller used was based on state feedback and set toperform CC. Using tf = 40s, pf = 120m and vf = 7m/s the position errorwas 0.13m and the speed error 0.006 m/s. The speed and position are seenin Figure 4.16 and 4.17, respectively.

As seen in the table below the position errors are much smaller for theRCV compared to the truck's, con�rming that its speed is easier to controlthan the truck. Also remarkable is how little the speed error varies as the

Page 40: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 4. Results 40

Figure 4.16 The speed of the RCVin blue and the reference speed inred.

Figure 4.17 The position of theRCV in blue and reference in red.

distance is increased compared to the truck. As seen in �gure 4.18 theposition errors di�er no more than 0.5m between PMP and interpolation.

Figure 4.18 Distance error for the RCV simulated with di�erent goal po-sitions. The errors using PMP are shown in red while the errors for theinterpolation methods are shown in blue.

Page 41: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 4. Results 41

Distance Distance error PMP Speed error PMP

120m 0.1342m 0.006m/s160m 0.0495m 0.0048m/s200m 0.0791m 0.0068m/s240m 0.2227m 0.0118m/s280m 0.4798m 0.0199m/s

Distance Distance error Interpolation Speed Interpolation

120m -0.2868m -0.0649m/s160m -0.2588m 0.0029m/s200m -0.2480m 0.0718m/s240m -0.2548m 0.1418m/s280m -0.2792m 0.2131m/s

Page 42: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 5

Conclusion

The purpose of this thesis was to investigate the use of trajectory plan-ning for autonomous and cooperative driving. Since the RCV could notparticipate in GCDC no planner were implemented in a real vehicle. Sim-ulations using the RCV with a MPC planner did however show that it waspossible to perform lane change maneuvers, although the safety marginsmight not be good enough to be used in real tra�c.

One of the must challenging aspects of developing the MPC was to setproper constraints on the ego vehicles position in order to avoid collision.These constraints had to be adapted depending on the intended maneuverand the other vehicles' positions. The main weakness of the planner is that itassumes constant speed of the other vehicles. If for example a vehicle in thefront slows down, the constraints on the position over the prediction horizonmight cause the ego vehicle to not reduce its speed enough.

A problem with the position constraints used by the MPC planner isthat it often forbids more positions than what is actually necessary to avoida collision. When the ego vehicle is adjacent to another vehicle the lateralconstraints limits its position to the current lane even when a lane change isto be performed. The diagonal position constraint used for overtaking couldalso be a problem since it limits the longitudinal position even if the adjacentlane is free from other vehicles. This might cause the vehicles to slow downtoo much during the overtake.

As for longitudinal planning, both PMP and interpolation methods wereproven to be useful for timing the position and speed of a vehicle. Simulationsshowed that the RCV was easier to control than the Scania Truck, which wasexpected as the truck has much slower acceleration and is a�ected by the gearchanges. This could however also be a result of the RCV being simulatedwith a more simple model. Comparing PMP with interpolation showed thatPMP was more e�ective in timing the position of the vehicles, in particularfor the truck.

In GCDC it was shown that cooperative driving can be used to safely

42

Page 43: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Chapter 5. Conclusion 43

perform maneuvers such as merging of lanes and intersection crossings. It didhowever require that all vehicles share communication protocols and have asimilar decision making process. While it is possible that cooperative drivingis implemented in the future, it would require extensive standardization inorder to be used on a larger scale.

5.1 Future Work

Further work on this project should focus on making a more �exibleand universal planner. In GCDC the planners had a more limited role asthey were only activated in speci�c situations. While the MPC plannerthat was simulated for this thesis was used all the time it had to be tunnedfor the di�erent scenarios and also only considered the other vehicles aspoint particles. In a real tra�c situation the planner would need to considerthe actual euclidean distance between the vehicles bodies and use that as aconstraint. This would give position constraints in the form of an oval aroundthe nearest vehicle. The prediction horizon would then properly consider thedi�erent actions that the ego vehicle can take.

In order to make the simulations more realistic the other vehicles wouldneed more advanced models and preferably some form of arti�cial intelli-gence in order to react properly to the ego vehicles actions. This plannershould then be implemented on a actual vehicle, for example the RCV. Inaddition to the planner a more uni�ed decision making is needed since dif-ferent algorithms were used for each scenario. More advanced controllersshould also be implemented since the real vehicle would be more di�cult tocontrol than the simpli�ed bicycle model used in the simulations.

Page 44: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

Bibliography

Erkorkmaz, K., Alzaydi, A., El�zy, A., and Engin, S. (2011). Time-optimizedhole sequence planning for 5-axis on-the-�y laser drilling. CIRP Annals

- Manufacturing Technology.

Falcone, P., Borrelli, F., Tseng, H. E., Asgari, J., and Hrovat, D. (2008). Ahierarchical model predictive control framework for autonomous groundvehicles. American Control Conference.

Farrokhsiar, M., Pavlik, G., and Najjaran, H. (2013). An integrated ro-bust probing motion planning and control scheme: A tube-based mpcapproach. Robotics and Autonomous Systems.

Glad, T. and Ljung, L. (2000). Control Theory, Multivariable and Nonlinear

Methods. CRC Press.

Grancharova, A., Grøtli, E. I., Ho, D.-T., and Johansen, T. A. (2014). Uavstrajectory planning by distributed mpc under radio communication pathloss constraints. J Intell Robot Syst.

Li, X., Sun, Z., Zhu, Q., and Liu, D. (2014). A uni�ed approach to localtrajectory planning and control for autonomous driving along a referencepath.

Semsar-Kazerooni, E., Bengtsson, H. H., and Med-ina, A. M. (2015). Interactive protocol.http://gcdc.net/images/doc/D2.1.Interaction.Protocol.pdf.

Tokekar, P., Karnad, N., and Isler, V. (2014). Energy-optimal trajectoryplanning for car-like robots. Auton Robot.

Turri, V. (2015). Fuel-e�cient and safe heavy-duty vehicle platooning through

look-ahead control. PhD thesis, KTH Royal Instiute of Technology.

You, F., Zhang, R., Lie, G., Wang, H., Wen, H., and Xu, J. (2015). Tra-jectory planning and tracking control for autonomous lane change ma-neuver based on the cooperative vehicle infrastructure system. Expert

Systems with Applications.

44

Page 45: Trajectory Planning for Autonomous Vehicles and ...kth.diva-portal.org/smash/get/diva2:1040724/FULLTEXT01.pdfcommunication system. The supervisory layer is used to make the basic de-cisions,

TRITA EE 2016:119

ISSN 1653-5146

www.kth.se