Strategic Trajectory Planning of Highway Lane Change Maneuver with Longitudinal Speed Control

A Thesis

Presented in Partial Fulﬁllment of the Requirements for the Degree Master of Science in the Graduate School of The Ohio State University

Yuhao Shui, B.S.

Graduate Program in Department of Electrical and Computer Engineering and Department of Mechanical Engineering

The Ohio State University

2015

Master’s Examination Committee:

Umit¨ Ozg¨uner,Advisor¨ Junmin Wang, Advisor Lisa Fiorentini Chia-Hsiang Menq c Copyright by

Yuhao Shui

2015 Abstract

Even though extensive research has been conducted on vehicle high-way longitudinal control under simple driving scenario, real life implementation of such system requires considering of surrounding traffic situations and movements such as lane change, weaves, on-ramp and off-ramp merges. In this thesis, the concept of driver being responsible of lateral control with automated longitudinal control is deployed in order to realize the mobility and safety benefits and a fundamental framework has been built to investigate high-way longitudinal control with lane change trajectory generated by geometric high order polynomial.

With the assumption of surrounding vehicles’ position and velocity is available, high-way two lane situation with driver being advised by the lane change module for the operation is studied. This system consists of several diﬀerent modules: vehicle single lane following control module, maneuver generation module, lane change trajectory generation module and Model Predictive Control (MPC) control while lane changing. Three controllers: PID cruise controller, transitional trajectory and vehicle following controller are used to achieve the functionality of vehicle following with a Finite State Machine (FSM) designed for controller switch decision making based on surrounding traﬃc movements. The following controller is needed to follow the preceding slower vehicle when driver does not intend to make a lane change. The

ii vehicle maneuver generation module is ﬁrst designed to inform the system, for example, whether an acceleration or deceleration lane change is needed to lead or follow the vehicle in the adjacent lane. Both Time to Collision and Inter Vehicular Time are used as collision indicators to ensure safety. All possible cases of high-way two lane situation with one slow preceding vehicle and one surrounding vehicle in the adjacent lane are integrated into the maneuver generation FSM design. Based on the suggested maneuver, the lane change trajectory generation module provides a desired trajectory for the longitudinal controller to follow. A pure geometric high order polynomial trajectory planning method is used to design obstacle avoidance lane change trajectory. In the end, an MPC controller is utilized to control the speed of the vehicle while lane changing.

iii Dedication

I would like to dedicate this study to my advisor professor Umit Ozguner. Thank

you for providing me this precious research opportunity and oﬀering valuable

guidance and discussion through the project. It is been a great pleasure for me to

work at Control and Intelligent Transportation Research lab. I would also like to

thank prof. Junmin Wang for your assistance when I was ﬁrst admitted to

Mechanical Engineering program and opening the door of vehicle dynamics and control for me, thank prof. Lisa Fiorentini for uncountable times of encouragement and advise through my graduate study and thank prof. Chia-Hsiang Menq for your two wonderful control courses and your understanding, time and support. Finally, I

want to thank my parents Hang Shui and Qing Shui. I would not be who I am today without your dedicated education, your patience and constant support in my

every depression moment and thank Ruiqi for continuously accompanying and

standing by me.

iv Acknowledgments

This research is partially supported by Leidos, Inc and Federal Highway Admin- istration (FHWA).

v Vita

August 27, 1991 ...... Born - Shanghai, China

2014 ...... B.S. Electrical and Computer Engi- neering, The Ohio State University. 2013-present ...... Electrical and Computer Engineering, Mechanical Engineering, The Ohio State University.

Fields of Study

Major Field: Electrical and Computer Engineering; Mechanical Engineering

vi Table of Contents

Page

Abstract ...... ii

Dedication ...... iv

Acknowledgments ...... v

Vita ...... vi

List of Tables ...... x

List of Figures ...... xi

1. Introduction ...... 1

1.1 Literature Review ...... 1 1.1.1 Adaptive Cruise Control (ACC) ...... 1 1.1.2 Lane Change and Merge ...... 3 1.1.3 Model Predictive Control ...... 8 1.2 Overview of Lane Change Trajectory Planning and Longitudinal Control module ...... 9 1.3 Structure of the Thesis ...... 14

2. Vehicle Following Controller Design ...... 17

2.1 Vehicle Dynamic Modeling ...... 17 2.1.1 Longitudinal Point Mass Model ...... 17 2.1.2 Lateral Point Mass Model ...... 18 2.1.3 Bicycle Model ...... 19 2.2 Vehicle Longitudinal Controller Design ...... 21 2.2.1 System Overall Construction ...... 21 2.2.2 Cruise Control ...... 23

vii 2.2.3 Vehicle Following Control ...... 25 2.2.4 Transitional Trajectory ...... 29 2.3 Finite State Machine (FSM) Design ...... 29 2.3.1 Lane Change Command Input ...... 33 2.4 Vehicle Following Simulation ...... 34

3. Vehicle Maneuver Generation ...... 38

3.1 Introduction ...... 38 3.2 Collision Indicator - TTC & TIV ...... 39 3.3 Lane Change Maneuver Decision Making ...... 42 3.3.1 Lance Change Problem Formulation ...... 43 3.3.2 Faster Vehicle in the Adjacent Lane ...... 45 3.3.3 Slower Vehicle in the Adjacent Lane ...... 51 3.3.4 Finite State Machine for Lane Change Decision Making . . 52

4. Lane Change Trajectory Generation ...... 57

4.1 Lane Change Trajectory Initial Design ...... 58 4.1.1 Calculation of trajectory parameters ...... 60 4.1.2 Trajectory Optimization ...... 63 4.1.3 Simulation of Sample Trajectories Generation ...... 68 4.2 Generation of Trajectories with One Obstacle ...... 69 4.2.1 Prediction of Surrounding Vehicles ...... 71 4.2.2 Overview of Trajectory Parameters Design ...... 72 4.2.3 Collision Detector - Gap between Vehicles ...... 74 4.2.4 Collision Avoidance Trajectory ...... 76 4.2.5 Vehicle Dynamic Constraints ...... 81 4.2.6 Simulation of Sample Trajectories with One Obstacle . . . . 82

5. MPC Longitudinal Controller Design for Lane Changing ...... 91

5.1 MPC Controller Design ...... 91 5.1.1 Vehicle Longitudinal Dynamic ...... 91 5.1.2 Controller Synthesis ...... 92 5.1.3 Cost Function ...... 93 5.2 Vehicle Lane Change Control Simulation ...... 94

6. Conclusion and Future Work ...... 97

6.1 Conclusion ...... 97 6.2 Future Work ...... 98

viii Bibliography ...... 100

ix List of Tables

Table Page

2.1 Bicycle Model Parameters ...... 21

2.2 Threshold Meanings ...... 30

2.3 Events and Threshold for Vehicle Following FSM ...... 32

3.1 State of Maneuver FSM ...... 54

3.2 Events and Threshold for Maneuver Generation ...... 55

3.3 Output Variables from Maneuver FSM ...... 55

3.4 Constant Parameters in Maneuver FSM ...... 56

3.5 State Variables in Maneuver FSM ...... 56

4.1 Boundary Conditions for Trajectory Generation I ...... 83

4.2 Boundary Conditions for Trajectory Generation II ...... 87

x List of Figures

Figure Page

2.1 Bicycle Vehicle Model ...... 20

2.2 Hybrid Controller Design ...... 23

2.3 Feedbakc Control Loop for Cruise Control ...... 25

2.4 String of Vehicles ...... 27

2.5 Threshlod Distance for ACC System ...... 30

2.6 States Logic Flow for FSM ...... 31

2.7 Desired Steering Wheel Angle ...... 33

2.8 Velocity for Both Vehicles ...... 34

2.9 Lateral Position of the Ego Vehicle ...... 35

2.10 Inter-Vehicle Distance ...... 35

2.11 Longitudinal Position of Both Vehicles ...... 36

3.1 Time to Collision Possibility ...... 41

3.2 Inter-Vehicular Time Possibility ...... 42

3.3 Decision Making Problem Formulation ...... 44

3.4 Lane Change Event 1 - Example ...... 45

xi 3.5 Lane Change Event 2 - Example ...... 46

3.6 Lane Change Event 3 - Example ...... 47

3.7 Collision Free Selection of d ...... 48

3.8 Lane Change Event 5 - Example ...... 49

3.9 Lane Change Event 11 - Example ...... 52

3.10 Finite State Machine for Lane Change Maneuver Generation . . . . . 54

4.1 Single Lane Change with No Obstacle ...... 59

4.2 Lane Change Trajectory 5th order ...... 68

4.3 Longitudinal Velocity and Acceleration of the Trajectory 5th order . . 69

4.4 Lateral Velocity and Acceleration of the Trajectory 5th order . . . . . 69

4.5 Distance to Obstacle ...... 75

4.6 Vehicle Representation - Circle ...... 78

4.7 Selection of a6 ...... 84

4.8 Collision Free Path for Event 3 ...... 85

4.9 Position in x and y Direction for Event 3 ...... 85

4.10 Velocity and Acceleration in x Direction for Event 3 ...... 86

4.11 Velocity and Acceleration in y Direction for Event 3 ...... 86

4.12 Relative Distance between Two Vehicles for Event 3 ...... 86

4.13 Selection of a6 with Dynamic Constraints ...... 88

4.14 Collision Free Path for Event 11 ...... 88

4.15 Position in x and y Direction for Event 11 ...... 89

xii 4.16 Velocity and Acceleration in x Direction for Event 11 ...... 89

4.17 Velocity and Acceleration in y Direction for Event 11 ...... 89

4.18 Relative Distance between Two Vehicles for Event 11 ...... 90

5.1 Reference Trajectory versus Driver Trajectory ...... 95

5.2 Controlled Trajectory Comparion ...... 96

xiii Chapter 1: Introduction

1.1 Literature Review

1.1.1 Adaptive Cruise Control (ACC)

Adaptive Cruise Control (ACC) can help to improve high way driving safety and increase the high way capacity. Also, by applying automated vehicle following system, the inter-vehicle spacing can be adequately reduce. Extensive research has been done on the longitudinal speed control of vehicles [27], [45]. However, most of them are implemented in simple scenarios. The final goal of the project is to implement longitudinal speed control during various traffic movements like lane changes, weaves, merges, and de-merges. In this thesis, vehicle following controller has been built first with lane change maneuver generation to make decisions. Then, a geometric high order polynomial method is applied to generate a lane change trajectory. At the end, a MPC controller is used to control the vehicle’s velocity catching the desired trajectory.

Adaptive Cruise Control

ACC is an extension to standard cruise control system. It can not only control the speed of the vehicle to the desired velocity set by the driver, but also adapt to the preceding vehicle’s velocity while maintaining a desired inter-vehicle space.

1 Thus, by implementing the system It can enhance driver comfort and convenience and increase safety on highways. In this project, it is assumed that the leading vehicle’s dynamics is unknown, but both relative position and relative space can be obtained either by an on-board radar or through Vehicle-to-Vehicle Communication. Diﬀerent methods can be applied to achieve this vehicle following functionality. For example, a comprehensive ACC system analysis using reachability analysis has been done in

[31], [32] to ensure safe ACC design with collision avoidance. In terms of car following policy, different algorithms have been introduced [20], [46]. [17] designed an ACC system with lane change support using MPC to generate a lane change trajectory as well as control input i.e the lateral acceleration. In this thesis, in order to achieve car following, three different controllers are used. The first one is the standard cruise control. Plus a transitional control to drive the ego vehicle at the same velocity as the preceding vehicle. And a car following vehicle controller applying constant time-gap policy is implemented. There are many discussions in terms of which spacing policy should be used for ACC system. The fundamental requirements of the system are it should ensure individual stability and it should also has string stability [33]. Assume the system is stable, other requirements like traffic-flow stability can be explored and the system becomes more complicated. Detailed system analysis will be given in later section

Hybrid System

When systems both have continuously dynamics and discrete states with respect to time, a hybrid system using a ﬁnite state machine to represent the discrete states can be implemented. For discrete system, transition between states will only occur when the appearance of a event or condition is met. And state machine is used to represent

2 those states. And hybrid systems theory is the method to analysis and implement complex systems containing both continuous dynamics and discrete states [1]. Hybrid system analysis can be complicated, and here only the overall idea is introduced. In the designed car following model, a hybrid control system is implemented where the

ﬁnite state machine is responsible of making decisions based on transition conditions or events and the inputs of the ﬁnite state machine come from the continuous vehicle dynamics. An interface subsystem is designed in order to transfer continuous signal into discrete states such that the state machine can make decisions based on that.

The output of the system is the state where the system should become. We need the hybrid system because there are three different controllers in the model and each of them corresponding to different discrete states. A finite state machine is needed to make the high level decision either in terms of either changing control gains of the controller or switching to different controller.

1.1.2 Lane Change and Merge Strategy Level

Recent years, highway lane change problem draw more and more attentions and

[23] gave a critical review of the recent design of lane changing models. In order to execute different maneuvers, in this case lane change and weaving, automatically while they are still under the control of the driver, traffic situation assessment is es- sential. [38] designed a system that can assess traffic situation based on the vehicle’s environment and give recommendations in terms of lane changing to drivers. Image processing techniques for lane and vehicle detection are discussed first and unscented

Kalman ﬁltering for estimation and tracking the true state of the vehicle’s surrounding are used. Moreover, the method of Bayesian network (BN) is applied for situation

3 assessment and decision algorithm that can handle uncertainty from the perception to the decision state. In [19], a high level cooperative adaptive cruise control with highway merging control algorithms were discussed. Since Bayesian network can be potentially important for decision making, [36] discussed the Bayesian network or called novel method to percept different situations. This method is based on probabilistic decisions. In that system, the probabilistic situation and maneuver statements are extracted from a probabilistic network combined with a fuzzy allocation. [6] proposed a framework for asking the question relevant to changing lanes like a human driver’s process of thinking and using the answers to derive feasible answers and the model is focus on the urban driving situation meaning influencers like traffic signals, obstructions and heavy vehicles are all considered in the decision making process. The goal is to ensure the vehicle can behave logically when commonly encountered situations happened. An example of lane-changing model complements a car-following model is explained.

Although decision making is critical, the policy of lane assignment is probably more critical, otherwise, there is no foundation for the decision to be made. [18] looked into lane assignment problem in the Automated Highway Systems (AHS). The purpose is to increase highway throughput with many merging situation may happen.

The lane assignment problem is formulated into a reasonably complex mathematical optimization problem focusing on the eﬀect of lane scheduling on highway capacity as to ﬁnd proper positions of partitions on an itinerary matrix and Genetic Algorithm

(GA) is used to sole the optimization problem. It started with a simple case of three lanes with 1 entry point and k exit points and extended to general case. A traﬃc simulator was developed to test the lane change logic.

4 On the spirit of guaranteeing safety and minimizing the disturbance in the traﬃc

flow while lane changing and merging is taking place, [15] analyzed the problem of collision-free merging and lane changing when vehicles in the vicinity may need to modify their speed and position to coordinate with this type of maneuvers. Various scenarios were studied and an algorithm for calculating the minimum safety spacing for lane changing (MSSLC) was proposed. In this algorithm, the desired inter-vehicle spacing during a merging or lane changing maneuver is calculated first to avoid possi- bilities of collision if any emergency happens. Moreover, the lane changing policy, one of the options is using the lane assignment algorithm discussed earlier, of the merging vehicles needs to be addressed. It also considered both the longitudinal and lateral acceleration model for the merging vehicle to derive the safe and collision-free lane changing strategies. [43] presents a combined lateral and longitudinal rule based upper level control algorithm for a co-operative vehicle. The system applies a two level hierarchy control where the upper level controller does the decision making by using a finite state machine and the lower level controller implements speed and steering control. A nonlinear single track model and Dugoff tire model are used in the Mat- lab/Simulink model. A detailed flow chart of lane change algorithm is given describing the decision making process in terms of when to implement which controller and three different scenarios are considered for the simulation: highway following, Joining platoons and lane changing platoons. [16] proposed a general model called minimizing overall braking induced by lane change (MOBIL) to derive lane-changing rules for both discretionary and mandatory lane changes. Longitudinal acceleration is used to determine the risk in terms of lane changing as well as the utility of a given lane.

5 From that, the safety and incentive criteria for both symmetric and asymmetric pass- ing rules is formulated where incentive criterion determines if a lane change improves the individual local traﬃc situation of a driver and safety criterion prevents critical lane changes and collisions. Moreover, a “politeness factor” is introduced such that the lane changing can vary from purely egoistic to more cooperative driving behavior.

A novel network is presented in [40] in order to assist driver of merging maneuvers.

The vehicle’s surrounding is monitored via sensors. The measurements from sensors are integrated into a representation called Dynamic Probabilistic Drivability Map

(DPDM) where this DPDM acts as a compact representation of the surroundings.

With the information of vehicle dynamics, other vehicles, and the road environment, the system is able to give recommendation of merge maneuvers to the driver. This maneuver is formulated into a dynamic programming problem whereas the minimum cost solution for merging is the recommendation. The system not only suggests when and how to merge, but also recommends the appropriate acceleration or deceleration for merging into the adjacent lane. [37] considered the problem of decision making using uncertain data and proposed the application of decision networks for this problem. Decision Networks (DN), which is based on a sound theoretical foundation, are an extension of Bayesian Networks (BN), which performs probabilistic reasoning, for decision making under uncertainty. Besides the chance nodes used in BNs, both decision nodes and utility nodes are utilized in Decision Networks. DN is applied because it is capable of dealing with numerous decision alternatives, can naturally handle uncertainties and multiple decision criteria can theoretically be incorporated into a DN.

6 Control Level

Besides strategic high level decision making, good control algorithms need to be applied or developed to execute the suggested maneuver. [24] considered the operation of overtaking a vehicle that is driving in the same lane. This system needs

High-precision GPS and a wireless network and requires a two-level architecture where the low level consists of two fuzzy steering controllers – one for path tracking and the other for lane change, both of them are used to mimic human behavior and reactions during the overtaking period, and the high level controller aims to evaluate the feasibility of overtaking. Longitudinal speed control is applied to keep the vehicle either at a constant speed or a safe distance from leading vehicle when overtaking cannot take place. A set of conditions are deﬁned to ensure a safe overtaking operation.

For the problem of merging a vehicle into through traffic at a ramp or into a platoon through lane changing on highways safely, [44] proposed a vehicle merging control algorithm using the concept of virtual vehicle. A virtual vehicle means mapping a vehicle on one lane onto another lane. Thus, longitudinal control can be utilized on both lanes simultaneously to ensure a smooth merge. Also, the data transmission algorithm is used for inter-vehicle communicate to generate the virtual vehicle. Different classification of merging and how to generate virtual vehicle under each circumstance is explained. [12] considered the problem of vehicles in a platoon doing lane change maneuver and in order to avoid the follower splitting from the current platoon before making a lane change, they proposed a lane change within platoons maneuver, in which case, neither platoon needs to be split. It requires align- ing and locking the longitudinal positions of the two platoons in adjacent lanes first.

Both a longitudinal controller that can ensure string stability of the platoons and a

7 leader law are designed to accomplish this maneuver. [4] designed a sliding mode controller to follow a virtual desired lane change trajectory on highway. And [34] did a experimental test of integrated lateral and longitudinal control.

1.1.3 Model Predictive Control

Recently, MPC control plays an more and more important role especially in adaptive cruise control mainly because it solves an optimization problem with prediction states and various constraints can be added to meet design specifications. In [35], a model predictive control based approach is applied to better perform the collaborative adaptive cruise control schemes. A comparison between PID controller and MPC controller is given to show the advantage of MPC controller. With the designed MPC controller, a rear end collisions with the following car can be avoided and meanwhile keep the safe distance away from the preceding vehicle. Two types of MPC are designed with and without rear collision detection and a switch is used based on the relative distance between the preceding vehicle and rear vehicle to change between two controllers. [22] considered the case of the lane change behavior with a cut-in vehicle at the beginning of the convoy. The MPC controller is designed for highway and with a cost function designed concerning riding comfort and spacing safety. The proposed cost function allows the controller be adaptive to different types of lane change. And it also analyzed the control effects on the spacing and velocity of the followers. The simulation results showed the good performance of the controller while different lane change cut-in situation is added. And in [21], a multi-objective MPC controller is designed for high way adaptive cruise control. Instead of the usual MPC

8 controller only takes safety issue into account, the MPC controller designed in this paper comprehensively considered possible issues like tracking capability, fuel eﬃciency as well as driver desired response. It utilized a hierarchical control architecture with high level MPC controller and lower level controller to track the desired acceleration given by MPC. A quadratic cost function is used to minimize tracking error, fuel consumption, riding comfort, driver permissible tracking range and rear end safety.

Based on optimization over all the previous criteria, the MPC controller output the desired acceleration for the vehicle to track and the beneﬁts of fuel economy and tracking capability while meet driver’s desired vehicle following distance are showed in simulation.

1.2 Overview of Lane Change Trajectory Planning and Lon- gitudinal Control module

In this project, the fundamental structure using high order polynomial to design vehicle high-way lane change trajectory with automated longitudinal speed control module was built and simulated. Although multiple researches have investigated highway single lane longitudinal control with diﬀerent goals such as ACC string stability or minimum fuel consumption considering road gradient, also, extensive interest in the lane change lateral steering control of the vehicle, the issue of longitudinal control while lane changing should also be studied. Especially because even though the automated longitudinal control is fairly mature and multiple methods have been applied and tested like MPC, constant time-gap policy, constant headway control and so on, but they focused on a simple highway single lane situations. This is not enough for real life implementation such as urban traﬃc situation or the cases discussed in this project i.e. lane changes, weaves, merges, and de-merges. Here, a new concept is

9 introduced and studied i.e. the case where driver is responsible for the vehicle lateral control while longitudinal control is fully automated during the lane change maneuver. This system will help to increase high safety, driver riding comfort and easiness and even for highway eﬃciency by increasing highway throughout.

Since the overall objective of this system is to control the vehicle longitudinally while driver steering for a lane change. The following scenario is considered and studied in the project. This is the case where driver set a desired speed for the vehicle to follow just like regular cruise control. Then, when the ego vehicle encounters a slower driving preceding vehicle on the same lane as the ego vehicle, in order to keep the desired speed or at least close to the desired speed, system should start to exam situations in the adjacent lane. If there is enough space for a lane change and the adjacent lane allows a faster speed for the ego vehicle to pass, then a lane change trajectory should be planned and generated. Meanwhile, system should give a lane change suggestion signal to the driver indicating a lane change trajectory is ready. Of course, since the steering lateral direction is fully controlled by the driver, system can only recommend for a lane change and the lane change model should always standby once a lane change is needed with the corresponding safe lane change trajectory calculated. If driver is not intended to make a lane change, then the vehicle should follow the slower preceding vehicle with a safe distance. And if the driver decides to make a lane change, the lane change trajectory and control module should be triggered. Since two diﬀerent controllers are used for single lane longitudinal control and for speed control while lane changing, system needs to receive driver’s intention as a switch to start a lane change module. Such signal can be received by adding threshold like steering angle or the turn signal light. Overall, system should do the

10 vehicle following ﬁrst and lane change module gives recommendation to the driver indicating a lane change is more desired. If driver is not making a lane change, then vehicle following controller is used while lane change module should still keep calculating corresponding lane change trajectory and give signals once the lane change is not possible anymore. And when driver wants to make a lane change based on the recommendation, then the lane change longitudinal control module can be triggered.

Once the lane change maneuver is ﬁnished, standard cruise control should be again applied and system starts to look for possible lane change trajectories again if the preceding vehicle in the adjacent lane is still slower than the desired vehicle even though it is faster than the original preceding vehicle.

In order to achieve the objective scenario described above, several modules are needed and thus designed and simulated separately in this thesis. First of all, when a preceding slower vehicle is detected, a longitudinal following controller is designed to follow the preceding vehicle. The single lane vehicle following controller implemented in this project consists of three diﬀerent controllers where the vehicle following parts adopt the constant time-gap policy. A standard cruise control is ﬁrst needed to drive the vehicle to the desired vehicle set by the driver. Then, when a preceding vehicle is detected, a transitional trajectory is needed to bring the vehicle to steady- state vehicle following condition where the ego vehicle has the same velocity as the preceding vehicle and the relative distance is at the desired value based on their speed.

Once both conditions are achieved, the vehicle following controller is applied to help maintain the desired relative distance between vehicles and the distance is decided by the constant time-gap policy i.e. the distance varies based on diﬀerent longitudinal speed. With the following controller, even the driver has no intention of making a

11 lane change, a safe distance will still be maintained with a slower velocity and wait until the driver decides to make a lane change.

The second part of the system is the vehicle maneuver generation module. This module is responsible of decision making in terms of lane change maneuver. When the preceding slower vehicle is detected, even though the vehicle following controller is controlling the speed, lane change module should also start to work in order to give lane change recommendations. This module considers all possible cases with one slow preceding vehicle and one surrounding vehicle in the adjacent lane. The surrounding vehicle may drive at a speed higher or slower than either the preceding or the ego vehicle. Notice that since the time to start a lane change is decided by the driver, all possible relative distance between the surrounding vehicle and the ego vehicle need to be taken into account. Thus, this maneuver generation module gives lane change recommendation to the driver based on the surrounding traﬃc situation and it also outputs needed information to the trajectory generation module to ensure the maneuver is safe. Two indicators are used: Time-to-Collision (TTC) and Inter-

Vehicular Distance (TIV) to ensure two vehicles maintaining the safe distance for the lane change maneuver and lane change trajectory generator calculates the desired lane change trajectory based on the given information .

In the lane change trajectory generation module, a geometric high order polynomial trajectory representation is used and mainly two cases are considered and designed. The first case is when there is no obstacle in the adjacent lane and the second case is to design a lane change trajectory when there’s obstacles around i.e. there’s a vehicle in the adjacent lane. For the first case, although two fifth order polynomials are not ideal, it builds the foundation of designing a obstacle avoidance lane

12 change trajectory. Six constraints i.e. the starting and the ﬁnal position, velocity and acceleration are used to calculate trajectory parameters. Two variables can be used to optimize the trajectory i.e. the total time used to ﬁnish the lane change trajectory

T and the overall distance traveled during the lane change maneuver d. The cost function used in this project is to minimize the total kinetic energy consumed during the lane change maneuver and the two optimization variables d and T are used to generate the trajectory since both the lateral and longitudinal trajectories designed above requires these information. When there is an obstacle occurred, the final position, velocity and the total time used to finish this lane change maneuver is received from the previous maneuver generation module. And the longitudinal trajectory is modified from a fifth order polynomial to a sixth order polynomial in order to have more freedom to avoid the obstacle. In order to decide the extra parameter for the sixth order, a mapping of obstacle and the the parameter is applied. The collision detector describes the criteria for two vehicle’s position to ensure a collision free path and this information is mapped to choose the parameter that ensures a safe trajectory. By now, the lane change trajectories for both cases are designed and set as a desired trajectory based on the decision making output from the maneuver generation module.

With the desired lane change trajectory designed, the ﬁnal step is to design a controller to longitudinally control the vehicle to the desired trajectory. Here, an

MPC controller is applied to achieve this goal. Since the system has no control to the lateral direction, assuming the driver is turning the steering angle bigger than the desired trajectory, then the controller should accelerate the vehicle in order to catch the desired trajectory. On the other hand, when the driver is turning at a

13 smaller angle, the vehicle should decelerate to catch the trajectory. MPC is a great tool because it involves an optimization process during the control law design. Also, multiple constraints of the system state variables as well as the control input can be added to ensure it is safe and feasible while lane changing. With these criteria met, the closer to the trajectory the better. However, because of vehicle dynamics, pure longitudinal control for lane changing has it’s limitation that is driver’s steering angle should originally be relatively close to the desired trajectory. In this project, only the optimal lane change trajectory case is simulated, but multiple trajectories with varying speed can be set as desired trajectory for the vehicle to follow.

1.3 Structure of the Thesis

Based on the system description discussed above, chapter 2 ﬁrst implemented the vehicle following controller and simulated with a preceding vehicle. Both the vehicle dynamics model and three diﬀerent controllers: cruise controller, transitional controller and following controller are designed and tested separately.

Chapter 3 discussed in detail about the decision making process of the lane changing maneuver i.e. the maneuver generation part. First, two collision indicators - TTC

& TIV are introduced to ensure a safe gap between two vehicles. Based on that information, multiple cases with either a faster vehicle or slower vehicle in the adjacent lane are considered with possible positions of the surrounding vehicle leading, following or next to the ego vehicle are discussed. Diﬀerent decisions are made based on diﬀerent scenarios. It is concluded with a overall Finite State Machine including all the cases discussed in this chapter.

14 Chapter 4 is the generation of trajectories using pure geometric high order polynomial method. To start, two ﬁfth order polynomials are designed and applied six constraints to calculate all the parameters used in the polynomial that will be used for later obstacle avoidance trajectory design. Two key variables T the overall time of the lane change maneuver and d the total distance traveled during the lane change maneuver used for trajectory generation are calculated from total kinetic optimization.

Then, with one obstacle in the environment, ﬁrst step is to predict the surrounding vehicle’s future position. And apply a collision detector to calculate the gap or relative distance between the vehicle to the obstacle. With the criteria of collision free is set, a six order polynomial in the longitudinal direction is applied to provide more freedom in order to meet the collision free criteria. Finally, with vehicle dynamic constraints added, the lane change trajectory is designed and set as a desired trajectory for the vehicle to follow.

Chapter 5 discusses the design of MPC controller to longitudinally control the vehicle following the desire lane change trajectory. Matlab MPC block is used to simulate the results of the design. Since only the longitudinal speed can be controller, a decoupled vehicle dynamics is used to simulate driver steering turn rate with the speed of the module is given from the controller. A look-up table is used to output desired longitudinal position based on the current lateral position and use that as desired position to the MPC controller. Multiple constraints are added in order to ensure safety, riding comfort and feasibility during the lane change maneuver.

Chapter 6 summarized the overall system designed in this project with corresponding results. Also, future work direction is proposed for multiple possible improvements

15 can be made to the current design. Also, potential test scenarios for this system is proposed for future experiment.

16 Chapter 2: Vehicle Following Controller Design

2.1 Vehicle Dynamic Modeling

In order to accomplish vehicle velocity control for single lane as well as for lane changing with the desired velocity either set by the driver or being the leading vehicle’s velocity, vehicle modeling for both longitudinal and lateral dynamics are needed.

Point mass model can be used for simply illustration and understanding the system while [11] designed a longitudinal vehicle controller applying sliding mode control with detailed power train dynamics. Once the model of the system has been built, more complex vehicle models can be used like bicycle model or full vehicle model to include more behavioral details like side-slip angle or diﬀerent tire forces of the vehicle. In this section, longitudinal point mass model has ﬁrst been introduced for cruise control and car following dynamics and lateral point mass dynamics is discussed next for lane changing and merging behavior.

2.1.1 Longitudinal Point Mass Model

The input force on the car is either the driving force or the braking force. Since the project is mainly focus on high lever controller design, meaning lower level controller including throttle and brake input is not within the scope, all these external forces are

17 currently represented by a lumped input force: F , which is calculated from controller and used as an input into the vehicle dynamics. In order to tune the eﬀect of the input force on the vehicle behavior as well as make it more realistic, a transmission coeﬃcient c can be also added. It can be set to one at the beginning and tuned later.

Except the motion of the vehicle itself, friction is another factor needs to be considered like road friction or wind friction. In this model, all those friction is represented by a damping coeﬃcient b and added to the equation. Thus, the driving input force should be equal to the vehicle motion plus the friction [28]:

F = cu = mx¨ + bx˙ (2.1.1)

where F should include both driving force and brake force: F = fd − fb, fd is driving force and fb is braking force on the vehicle. u is the control input into the vehicle dynamics calculated from diﬀerent controllers based on corresponding situation. m denotes the mass of the vehicle and x is the longitudinal position of the vehicle meaning the ﬁrst-order derivative of the positionx ˙ is the velocity and the second-order derivativex ¨ is the acceleration or the deceleration.

2.1.2 Lateral Point Mass Model

This lateral point mass model is used to represent the lateral dynamics of the vehicle for lane changing and merging behavior. Since the previous point mass model only has longitudinal displacement involved, for lateral position, that model does not apply. A model includes both lateral and longitudinal position with steering angle as

18 the input can be used:   x˙ = Vs cos Θ   y˙ = Vs sin Θ (2.1.2)    Θ˙ = u where x still represents the longitudinal position of the vehicle while y represents the lateral position of the vehicle in this case. Vs denotes the velocity of the vehicle along the heading direction, which can be decomposed into longitudinal and lateral direction like described in the equation. Θ is the steering wheel angle i.e. the angle between the longitudinal and the lateral direction of the vehicle and u in this model is the steering wheel turn rate (rad/sec). One thing needs to be paid attention to is that this model has the assumption of driving at constant speed. This assumption can be hold in this project because lane change command will be given during the phase of either constant speed during cruise control or during car following. By having this model, if a steering wheel angle is applied, the motion of the vehicle can be obtained.

2.1.3 Bicycle Model

Bicycle model, also called two-wheel model, can represent the lateral dynamics of the vehicle with more details than point mass model but less details than full vehicle model. Since in this project, the trajectories of lane changing is more concerned than tire forces or stability of the vehicle, bicycle model can achieve this goal very good.

And the model is described as below:

19 Figure 2.1: Bicycle Vehicle Model

Bicycle model is derived by adding both forces and moments around the vertical axis at the center of gravity and it shows the vehicle lateral motion dynamics as well as the rotational dynamics. The free body diagram is showed in ﬁgure 1. The linearized bicycle model equations are:

kf + kr akf − bkr 1 Tf v˙ = − v − u + r + kf + δ (2.1.3) mu mu m Rf 2 2 akf − bkr a kf + b kr a Tf r˙ = − v − r + kf + δ (2.1.4) Izu Izu Iz Rf

where, for vehicle variables, u and v are longitudinal and lateral velocity of the vehicle respectively. r is the yaw rate and δ is the steering angle of the front wheel.

Those variables need to be updated in real time meaning they are changing with time and they need to be feedback to the vehicle dynamics for the model to work.

For other constant vehicle parameters, kf and kr are the front and rear tire lateral stiﬀness coeﬃcient respectively. m is the total mass of the vehicle, a and b are the distance from center of gravity (CG) to the front and rear axle respectively. And Iz is

20 rotational inertia in the horizontal plane. In this project, both Rf , the eﬀective rolling radius of the tire, and Tf , the driving torque on the front tire, can be neglected.

Model Parameters for OSU Vehicles a 1.35 m distance from CG to front axle b 1.37 m distance from CG to rear axle Iz 272.4 Ns/rad rotational inertia m 1569 kg vehicle total mass 4 kf 5.96 × 10 N/rad front tire cornering stiﬀness 4 kr 8.66 × 10 N/rad rear tire cornering stiﬀness

Table 2.1: Bicycle Model Parameters

Table 2.1 shows the typical vehicle model parameters for OSU vehicles as well as the value used in this project. It should be noticed that this model is only valid in the situation where the velocity of the vehicle is constant and steering input is small because otherwise the operating point might move to nonlinear region of the tire model.

2.2 Vehicle Longitudinal Controller Design

2.2.1 System Overall Construction

Currently, the main goal of the system is to achieve longitudinal velocity control with and without a leading vehicle. Also, when a command of lane changing is given, the vehicle should be able to accomplish a single lane change. When there is no leading vehicle, cruise control is implemented and the vehicle should travel at the speed set by driver. If a leading slower vehicle is detected in front of the ego vehicle and becomes closer than a certain threshold, the system should switch from cruise control

21 to desired spacing control i.e. the ego vehicle will maintain a desired distance away from the leading vehicle and have approximately the same velocity as the leading vehicle at steady state. Moreover, when the command of lane changing is given, vehicle moves to the next line and if there is no preceding vehicle detected, cruise control mode should be turned back on. Thus, two controllers are definitely needed: standard cruise controller and adaptive cruise system controller. However, if the initial distance between two vehicles has a big difference with final desired distance, the system will have undesirable oscillation causing both uncomfortable experience for vehicle passengers and potential safety concerns. And from the simulation results, it can be showed that if the initial distance is relatively close to the desired distance, the space controller works well. Thus, a transitional trajectory controller is also needed to bring the ego vehicle to the same speed as the preceding vehicle and then switch to space controller. These two controllers have different functionality. The transitional one focus on bring the ego vehicle’s velocity down to about the same as the preceding vehicle’s velocity and the space controller focus on controlling the space between two vehicles and maintain it at the desired distance [1].

Now, since there are three diﬀerent controllers, a Finite State Machine is designed to determine which controllers to use based on diﬀerent driving scenarios. States and thresholds will be discussed in the FSM design section. Also, methods used to design both cruise control and adaptive cruise control are explained. There are two things need to be especially taken care of for ACC. One is which spacing policy to use and the other one is if this ACC controller design can ensure vehicles string stability i.e. if it is implemented in every vehicle on the road, will the system occur big oscillation?

Lane change model used in the model is also discussed in this section.

22 Figure 2.2: Hybrid Controller Design

First of all, each controller should be designed and work well individually. Then, based on the state selection from ﬁnite state machine, a switch can be used to choose which controller to implement during that speciﬁc driving situation. As discussed above, three controllers need to be used for this model and each of them is discussed and designed in this section.

2.2.2 Cruise Control

The objective of a cruise controller is to control the vehicle’s velocity at a desired value set by the driver. For a standard cruise control system, it includes an upper level controller and a lower level controller where the upper level controller calculates the desired acceleration for the vehicle and the lower level controller determines what throttle or brake input to use in order to achieve the desired acceleration. In this model, it does not concern about the engine model as well as throttle or brake input.

Thus, there’s only one controller used here with input being desired vehicle speed and

23 output being the control input to the vehicle dynamics i.e. the total external force

needed to drive the vehicle.

There are several system requirements in terms of controller gain tuning. One is

there should not be any steady state tracking error of the system meaning when the

system reaches steady state, the velocity of the vehicle should be exactly the same as

the system input: the desired speed. Also, overshoot should not occur, of course this

includes no oscillation of the speed, cause once the speed is higher than the desired

speed, potential safety concern might be raised. And it is desired for the system to

reach steady state in a short amount of time, but not too fast to cause uncomfortable

riding experience. The control algorithm used for this model is a PI controller using

error, which is the diﬀerence between the reference speed and a feedback of current

vehicle velocity, as the input into the controller and output the total external force

needed to drive the vehicle to desired speed. The PI control algorithm is as follows:

Z t u = −Kp(Vx − Vref ) − KI (Vx − Vref )dt (2.2.1) 0

where both Kp and KI are controller proportional and integral gain respectively.

These gains can be tuned to meet the system requirements later and for diﬀerent

speed setting, the gain should be diﬀerent correspondingly. Vref is the desired vehicle

speed set by the driver and Vx is the vehicle’s current longitudinal velocity.

Figure 2.3 shows the unity feedback loop of the cruise control system. The controller is implementing the PI control and the vehicle model is the longitudinal point mass model discussed in previous section.

24 Figure 2.3: Feedbakc Control Loop for Cruise Control

2.2.3 Vehicle Following Control

Adaptive Cruise Control is one of the Advanced Driver Assistant System (ADAS) that not only controls the velocity set by the driver, but also able to adapt to the leading vehicle’s velocity and keep a constant desired distance away from the preceding vehicle when it occurs. Based on the system functionality, ACC should have two operating modes for the system to work. The ﬁrst one is the same as the standard cruise control system. When there is no preceding vehicle detected, ego vehicle should drive at the speed set by the driver using the same PI controller and point mass longitudinal dynamics. And when there is a preceding vehicle detected especially with a slower speed than ego vehicle, ACC system should switch from cruise control system to a vehicle following i.e. space control algorithm. In this case, the ACC controlled vehicle should be able to slow down to the same velocity as the preceding vehicle and keep a desired distance between two vehicles. The distance or gap size should be able to be chosen by the driver. Besides that, the controller should also be prepared for transitional maneuvers to not only ensure a smooth transition from cruise control to space control, but also brings the ACC controlled vehicle to the desired spacing distance from the preceding vehicle when it occurs and brings it back

25 to the desired speed when the preceding vehicle makes a lane change, exit or drives

faster. Because there are two operating modes and transitional maneuvers of the

system, the system itself should be able to decide which mode to use and switch

between two diﬀerent modes by itself. This is where the ﬁnite state machine comes

into play and will be discussed later.

Constant Time-Gap Spacing Policy In this part, the control law needed to follow a car and maintain a desired distance from the preceding vehicle is discussed.

Also, when the car following control system is implemented, both individual vehicle stability and string stability needs to be satisfied. For individual stability, it will be hold if the spacing distance error converges to zero when it reaches steady state where spacing error is the difference between the actual distance from the ACC controlled vehicle to the preceding vehicle and the desired inter-vehicle distance between the two. Figure 3 is used to develop mathematical notation. From the figure, it is clear that the inter-vehicle spacing is [33]:

ei = xi − xi−1 + li−1 (2.2.2)

where xi denote the ACC controlled vehicle’s longitudinal position and xi−1 is

the preceding vehicle’s longitudinal position and li−1 represents the length of the

preceding vehicle.

The spacing error is deﬁned as:

δi = xi − xi−1 + Ldes (2.2.3)

26 Figure 2.4: String of Vehicles

where Ldes be the desired spacing between two vehicles. This is the goal distance

for the car following system to maintain.

Typically, there are two ways to develop the spacing policy. One is constant

spacing policy and the other one is constant time-gap policy. Although from the

traﬃc-ﬂow stability perspective, spacing policy has its advantage over constant time-

gap policy, but the spacing policy needs to be modiﬁed to be stable and unconditional,

otherwise, it cannot ensure the string stability of the system. On the other hand, the

constant time-gap spacing policy can ensure string stability and easier to implement.

Thus, time-gap spacing policy is used in this model.

In the constant time-gap policy, the desired inter-vehicle spacing is designed to be

a function of velocity instead of a constant:

Ldes = li−1 + hx˙ i (2.2.4)

where h is the constant time gap (CTG), typically is between 1 and 2 seconds.

Andx ˙ i is, of course, the velocity of the ACC controlled vehicle. In this case, h can be chosen directly be drivers to change the desired spacing away from the preceding

27 vehicle. Now the spacing error deﬁne earlier becomes:

δi = xi − xi−1 + Ldes

= xi − xi−1 + li−1 + hx˙ i

Since

ei = xi − xi−1 + li−1

⇒ δi = ei + hx˙ i (2.2.5)

And from the relative distance ei, relative velocity can be achieved as well:

e˙i =x ˙ i − x˙ i−1 (2.2.6)

Now, the following constant time-gap spacing control law can be implemented [5]: 1 u(t) = − (e ˙ + λδ ) (2.2.7) h i i where λ is the control gain that determines the convergence rate of spacing error

String Stability Ensuring the string stability meaning the spacing error δi at the

end of the string will not amplify as the number of vehicles in the string increases.

To ensure string stability, let the transfer function be: δ H(s) = i (2.2.8) δi−1 the system is string stable if:

a) ||H(s)||∞ ≤ 1

b) The impulse response of the system h(t) has the same sign for all time t.

And it has been proved [33] that this control law based on constant time-gap policy can ensure string stability.

28 2.2.4 Transitional Trajectory

A transitional controller is needed if a preceding vehicle is detected and system

should bring the vehicle to the steady-state following distance. If there is no transi-

tional trajectory, the initial acceleration might be too big if a cut-in situation happens.

Thus, an additional transitional controller is used. The control law implemented is:

u(t) = −Kp(xi − xi−1) − Kd(x ˙ i − x˙ i−1) (2.2.9)

It should be noticed that both control gains are not the same as used for cruise control and need to be tuned based on experiments. Thus, if a preceding vehicle with slower velocity is detected, the ACC controlled vehicle should maintain its current speed until a threshold and apply transitional controller to bring both the speed and the distance to the desired value and then apply constant time-gap control law to maintain the desired inter-vehicle distance as well as speed.

2.3 Finite State Machine (FSM) Design

Because we have two operating modes plus transitional maneuvers for the adaptive cruise control system, the system itself should has the ability to decide which mode to use and switch between two different modes. Thus, a finite state machine can be used and implemented here to make decisions based on different driving scenarios.

Since we have already developed three controllers plus lane change model, they can be equal to four different states. And now, thresholds in terms of velocity and relative distance needs to be developed such that the finite state machine can switch states based on that. Figure 2.5 shows three different areas of interest. Since in this

29 Figure 2.5: Threshlod Distance for ACC System

model, it is assumed that there is no cut-in situation yet, active brake control area is

not considered.

Threshold Meaning dmax maximum following distance for ACC dset+ Upper bound tolerance for oscillation dset− Lower bound tolerance for oscillation dset the desired setting following distance Area A Keeps at cruise control Area B Transitional controller applied Area C Vehicle following area

Table 2.2: Threshold Meanings

The meaning of these thresholds are listed in table 2.2. To be more clear, at dmax, the preceding vehicle is being taken into account. It means even if there is a vehicle driving ahead of the ego vehicle, system stays at the cruise control and maintain the desired speed set by the driver if the inter-vehicle distance is bigger than dmax. When the distance is closer than dmax, it is now in area B. System switches to

transitional controller, tries to decelerate to the preceding vehicle’s speed and reaching

the desired inter-vehicle space. When two vehicle get closer than dset+, meanwhile

30 the relative speed between two vehicles are very small, 0.4 sec is used in the model, vehicle following controller is implemented and area C is the acceptable tolerance of oscillation during vehicle following. In terms of the value of these thresholds, dmax = TVx being a function of the ego vehicle’s velocity and T = 3 sec is used in the model. And dset+ = Ldes + dtol where dtol is the maximum tolerance for the system starts following algorithm and can be tuned. Currently, dtol = 10 m is used.

Figure 2.6: States Logic Flow for FSM

Figure 2.6 shows the discrete high level states and should be able to react to corresponding driving situations discussed above. Every state has a corresponding controller. Thus, the logic ﬂow of the ﬁnite state machine is showed and threshold has been explained. Now, events to trigger the state switch are needed.

E1 should be triggered when the inter-vehicle distance is smaller than dmax meaning it enters to area B and the velocity of the preceding vehicle is smaller than the

31 Events Conditions to generate events E1 ei ≤ dmax and Vx > Vp E2 ei > dmax and Vx ≤ Vp E3 ei ≤ dset+ or Vx − Vp < Vtol E4 Lane change command received

Table 2.3: Events and Threshold for Vehicle Following FSM

ACC controller vehicle. After E1, system should enter into the approaching stage i.e. transitional controller to bring the vehicle to desired inter-vehicle distance and about the same speed as the preceding vehicle. E3 happens when the vehicle enters into area C or the speed diﬀerence to be less than the threshold Vtol. Then, the vehicle following controller should be applied. E2 is generated when both the inter-vehicle distance is larger than dmax and the vehicle’s velocity is slower than the preceding vehicle. It is clear that when E2 happens, no matter what state the controller is currently on, they all should go back to cruise control status.

An interface block is designed to calculate all the variables needed for the ﬁnite state machine and is showed in next section. The inputs to the FSM are the inter- vehicle space ei, both the ego vehicle and the preceding vehicle’s velocity, dmax and dset+. They are calculated in the interface subsystem as described above and the output of the FSM is the state of the system i.e. which controller the system should use based on the current driving situation. And a switch is used from the output of the FSM.

32 2.3.1 Lane Change Command Input

Since the project is built to explore lane change and merge concept on highways, a basic lane change model needs to be built to accomplish a single lane change. The following desired steering wheel angle is used as an input:

Figure 2.7: Desired Steering Wheel Angle

For the next phase of this project, steering input will be given by a human driver.

And this signal aims to mimic the steering wheel angle given by a driver during a single lane change. Both lateral point mass model and bicycle vehicle dynamics can be used for lane changing. With the input steering angle and a constant speed, lane change can be accomplished.

33 2.4 Vehicle Following Simulation

Overall System Behavior Here, the complete simulation results are showed including velocity comparison during preceding vehicle slowing down and lane change is made. Four graphs are verifying the system. the lateral position of the vehicle shows that a lane change maneuver is made and the ego vehicle back to cruise control states.

Longitudinal Velocity of Ego and Preceding Vehicle 30

Velocity (m/s) 15 Ego Vehicle Preceding Vehicle 10 0 10 20 30 40 50 60 70 80 90 100 Time(sec)

Figure 2.8: Velocity for Both Vehicles

Case I: Preceding vehicle has higher velocity This is the phase where the preceding vehicle is driving at a speed faster than both the initial velocity of the ego vehicle and the desired velocity set by the driver. First of all, cruise control module is showed here. From 0-2 sec, the velocity of the ego vehicle rises from 10 m/s to 25 m/s with no overshoot, no oscillation and ﬁnish the process at around 2 sec. Since the preceding vehicle has faster velocity, the inter-vehicle distance is getting bigger and bigger. Although, initially, the distance between two vehicles are zero, since the

34 Lateral Positiono of the Ego Vehicle

Lateral Position (m) 0

0 10 20 30 40 50 60 70 80 90 100 Time(sec)

Figure 2.9: Lateral Position of the Ego Vehicle

velocity of preceding vehicle is always greater than the ego vehicle, system will stay at cruise control states.

Intervehicle Distance 80

-20

-40

Relative Distance (m) -60

-80

-100

-120 0 10 20 30 40 50 60 70 80 90 100 Time(sec)

Figure 2.10: Inter-Vehicle Distance

35 Longitudinal Position of Ego and Preceding Vehicle 2500

2000

1500

Position (m) 1000

500

Ego Vehicle Preceding Vehicle 0 0 20 40 60 80 100 Time(sec)

Figure 2.11: Longitudinal Position of Both Vehicles

Case II: Preceding vehicle is slowing down In this case, velocity of the preceding vehicle is slowing down and become 10 m/s slower than the ego vehicle at the end of the period. Based on the design of the ﬁnite state machine, even though the preceding vehicle’s velocity is slower than the ego vehicle’s, it should stay at cruise control state until the inter-vehicle distance is shorter than dmax. From the inter vehicle distance graph, we can see that the distance reaches maximum when the velocity of two vehicles become to be the same and the distance begins to decrease.

Case III: Vehicle following From the inter-vehicle distance graph, we can see that when the distance between to vehicles reaches around 80 m, which across the threshold of dmax and the system starts to implement transition controller and the speed of the ego vehicle is brought to be about the same velocity of the preceding vehicle as well

36 within 3 sec. Then, at around 37 sec, both conditions: the inter-vehicle distance is

less than dset+ and the diﬀerence between velocity of two vehicles is less than Vtol, are met. Thus, system switches to vehicle following mode and applying the constant time-gap policy. From the graph, we can see that there’s minor oscillation in terms of speed but they are controlled within the tolerance. Once the vehicle following algorithm is implemented, the inter-vehicle distance becomes almost a constant as well.

Case VI: Lane Change When the vehicle following system reaches steady state, a lane change command is given. From the lateral position graph, we can see a single left lane change is given. After the lane change, there’s no preceding vehicle detected.

Thus, cruise control system is applied again and reaches the desired velocity set by the driver. Once the lane change is ﬁnished and the speed of the ego vehicle becomes faster than the original preceding vehicle, from the longitudinal position, the ego vehicle passes the preceding vehicle. But since they are not on the same lane, this is what the system is meant to operate.

37 Chapter 3: Vehicle Maneuver Generation

3.1 Introduction

The overall lane change trajectory planning procedure involves two computational steps. First, a high-level lane change maneuver recommendation should be provided including stay in the current lane or make a lane change. If it recommends a lane change maneuver, then, the speed proﬁle i.e. the vehicle should keep the current velocity, make an acceleration or deceleration will be decided by the maneuver module. Also, system should decide whether the ego vehicle should follow the vehicle in the target lane or drive ahead of it. The decision making is based on both velocity and position of ego vehicle and surrounding vehicles. [14] calculated the minimum longitudinal safety spacing between the merging vehicle and leading vehicle considering acceleration of both vehicles. And [26] presented an algorithm to make strategic decision making in terms of lane change and overtake maneuver. A mixed logical dy- namical system is designed to solve through MPC while [41] formulated the Dynamic

Probabilistic Drivability Map [DPDM] as a representation of the surrounding lane, road, obstacle, ego vehicle dynamic and give recommendations based on the minimum cost for merging. Here, we assume the surrounding vehicles are driving at constant speed. The ﬁrst section will discuss two thresholds - Time To Collision (TTC) and

38 Inter-Vehicular Time (TIV) - that the system used to provide a safe lane change maneuver recommendation. From calculation of the maneuver generation module, it will output three key values for the trajectory generator to design the lane change trajectory. These values are first, T , the total time it takes to finish the lane change maneuver; d, the overall length for the maneuver and vf the final vehicle longitudinal velocity i.e. the velocity the vehicle should achieve at the end of the lane change maneuver. With these information, the trajectory generator can then designed the trajectory either making an acceleration to drive ahead of the surrounding vehicle or decelerate to following the vehicle in the adjacent lane. The next section explains in detail how does the system make a decision based on different driving scenarios as well as how to utilize the calculated collision indicator in the process.

3.2 Collision Indicator - TTC & TIV

Two of the main types of highway driving potential danger is one with very low inter-vehicle distance and one with high speed differences. Two indicators defined in [10] can be used to indicate the potential dangerous of the two situation. The inter-vehicular time indicator is used for the case where two vehicles are driving too close to each other and the time to collision indicator is used to for two vehicles having big speed differences. And both of the indicators are used in the maneuver generation module to decide whether it is safe to make a lane change or whether the

ﬁnal predicted vehicle position if safe after lane change has been conducted.

Time to Collision (TTC)

The ﬁrst indicator is the time to collision indicator and it is deﬁned as the time required for two vehicle to collide if both vehicles continue to drive at their current

39 speed and, of course, the direction or heading angle of the vehicle is also kept as it

is. TTC can be calculated as,

x − x TTC = f r (3.2.1) vr − vf where the numerator shows the relative distance between two vehicles (xf is the front or preceding vehicle’s position and xr is the rear or following vehicle’s position)and the denominator is the relative velocity (vf is the velocity of the leading vehicle and vr is the velocity of the following vehicle). Based on projects like AR-

COS (www.arcos2004.fr) and PREVENT (www.preventip.org) as listed in [7], several thresholds are deﬁned to indicate the potential dangerous based on the calculated value of TTC

1. TTC = 10s, two vehicles are suggested having no interactions.

2. TTC = 1.5s, used as the ﬁrst level warning

3. TTC < 1.3s, used as the second level warning

4. TTC < 1s, automated force brake control is suggested to be applied

This means that when TTC goes below 1s, it is highly possible for a collision to

occur and this is also the same time for driver to make reaction. The following graph

3.1 shows the possibility of a collision based on the value of TTC

The possibility graph is given based on the four criteria discussed above. Possi-

bility is set to zero when TTC is greater than or equal to 10s while the possibility

reaches 1 when TTC= 1. To ensure the minimum safety of the vehicle while lane

changing, the TTC value has to be kept greater than 1s no matter in the case of

40 Figure 3.1: Time to Collision Possibility

deciding if a lane change maneuver should be initiated or setting the ﬁnal distance to the preceding vehicle.

Inter-Vehicular Time (TIV)

The second indicator is the inter-vehicular time indicator. We need this indicator as well because as discussed in the introduction, two types of dangerous are the big relative velocity and small relative distance. And TTC can only consider the case where two vehicles have big velocity diﬀerence. But if two vehicles are driving at the same speed, and driving at extremely close distance, this is still the considering high possibility of dangerous while TTC still give 0 possibility because the relative speed is zero. To also take this kind of cases into consideration, the indicator TIV is coming into play. TIV is deﬁned as follows,

x − x TIV = f r (3.2.2) vf

41 The following ﬁgure 3.2 shows the corresponding possibility of collision in terms of

TIV.

Figure 3.2: Inter-Vehicular Time Possibility

In the next section, these two indicators are used for decision making to ensure a safe trajectory.

3.3 Lane Change Maneuver Decision Making

In this section, the dynamic conditions are deﬁned to distinguish the safe and unsafe lane change maneuver. No matter in which condition, both the starting point and ﬁnish point of the maneuver has a safe longitudinal distance away from the surrounding vehicle to execute a safe lane change. However, trajectory during the lane change maneuver is not considered, which is the trajectory generator’s job to ensure a safe lane change trajectory. The following decision making process utilizes both the relative speed and relative velocity between the surrounding vehicle and

42 the ego vehicle and it also considers the eﬀect of a slower blocking vehicle as well

as the desired velocity deﬁned by the driver. Diﬀerent cases are discussed in terms

of decision making. In terms of thresholds, as discussed in the previous section, the

longitudinal distance and relative speed between two vehicles should yield a time

to collision greater than 1s and has a inter-vehicular time greater than 0.5s to be considered safe. Here, the analysis only talks about the situation where the left lane has a surrounding vehicle and the ego vehicle need to make a left lane change. The situation for the lane change to the right is just symmetric. [13] developed a method for the system to be adaptive to diﬀerent drivers’ characteristics.

3.3.1 Lance Change Problem Formulation

Now, the lane change decision making problem need to be formulated including all the critical variables that are needed for the decision making process. [2] discussed and examined the dynamic conditions in terms of safe and unsafe lane changes in terms of relative distances and velocities. Following the same logic, from ﬁgure 3.3, it can be seen that the cases where three vehicles are taken into account. The proceeding vehicle a is always there with the slowest velocity. Because the goal for this project is to help maintain the longitudinal speed at the desired speed set by the driver through lane change if there’s a slower preceding vehicle. No lane change is needed if the preceding vehicle is faster than the ego vehicle. And since by the previous designed vehicle following controller, the ego vehicle and the preceding vehicle a will always maintain a safe longitudinal distance and, thus, cases of the relative distance between these two are not needed and ego vehicle can make a lane change directly if there is no other vehicle in the adjacent. Thus, besides these two trivial cases, in this

43 section, one vehicle driving in the target lane is considered with positions (xb, yb)can be either in in front of the ego vehicle’s position (xE, yE), behind the ego vehicle or longitudinally have intersection. Diﬀerent cases also discussed based on diﬀerent vehicle b’s velocity vb. It can be either faster or slower than either the current ego vehicle’s speed or the desired ego vehicle’s speed. In this project, it is assumed that both surround vehicles have a constant velocity with no acceleration or deceleration and make no lane change during the process.

Figure 3.3: Decision Making Problem Formulation

Based on the formulation of the problem, it is divided into two main subsection where there is a faster vehicle in the adjacent lane and there’s a slower vehicle in the adjacent lane. And within each subsection, more detailed subsection is built based on ego vehicle’s current velocity, desired velocity as well as the relative distance between the ego vehicle and the surrounding vehicle. The corresponding results are presented in the next section.

44 3.3.2 Faster Vehicle in the Adjacent Lane

Case I: va < vdes < vb

E1 : va < vdes < vb and xb > xE + 0.5lE + 0.5lb

This is the case where vehicle b has the fastest velocity and already driving

longitudinally in front of the ego vehicle. In this case, a normal optimized lane

change trajectory should be initiated i.e. the trajectory designed for the case

where there is no obstacle. ⇒ Normal lane change

Figure 3.4: Lane Change Event 1 - Example

E2 : va < vdes < vb and xE − 0.5lE − 0.5lb ≤ xb ≤ xE + 0.5lE + 0.5lb

Figure 3.5 shows the example case for this event. And in this case, since vehicle

b is driving at a higher speed than the current vehicle, ego vehicle should wait

until vehicle b pass i.e. enters into the case of event 1. Then, the command for

event 1 will be triggered. ⇒ Wait

E3 : va < vdes < vb and xb < xE − 0.5lE − 0.5lb and

xE −xb−0.5le−0.5lb xE −xb−0.5le−0.5lb TTC = > 1s and TIV = > 0.5s and vb −va < vT vb−vE vb

45 Figure 3.5: Lane Change Event 2 - Example

When vehicle b is behind the ego vehicle, the longitudinal relative distance come into play. And in order to execute a lane change while vehicle is driving at a higher speed than the ego vehicle, both TTC and TIV as discussed above is used as a decision making criteria. And in this case, if the relative distance is small enough based on TTC and TIV for a lane change, then a lane change command can be given and the trajectory generator will try to generate a safe trajectory.

Also, the boundary vT on relative speed is also added because vehicle b is faster than the desired velocity set by the driver. Although the driver may accept accelerate to a certain higher speed in order to pass around the slow preceding vehicle, but the velocity cannot be too high. ⇒ Acceleration Lane change with VEf = Vb (Lead vehicle b)

Moreover, since both the relative distance and relative velocity yields a safe longitudinal distance for the vehicle to make a acceleration lane change, this maneuver generator module is responsible for providing both the value of T , time required for the vehicle to ﬁnish lane change and d, the total length for the lane change. To calculate these two values, again, both collision indicators

- TTC & TIV are used ensuring that after the lane change, vehicle b will still

46 Figure 3.6: Lane Change Event 3 - Example

maintain a safe distance and relative speed with ego vehicle. It can be noticed that since the final longitudinal velocity of the ego vehicle is set to be equal to vehicle b, thus, the indicator TTC is not needed because ideally there will be no difference between two vehicle at the final stage. In terms of TIV, the following criteria needs to be met,

x − x − 0.5l − 0.5l TIV = Ef bf E b > 0.5 (3.3.1) vb

Based on the geometry given in 3.6, the following relationship is hold,

x = x + d Ef Ei (3.3.2) xbf = xbi + vbT = xEi − 0.5lE − 0.5lb − di + vbT

Plug these into 3.3.1 yields,

d > vbT + 0.5vb − di (3.3.3)

where xEf and xbf are the longitudinal position of the Ego vehicle and vehicle b at the ﬁnal point of the lane change maneuver. lE and lb are the length of ego and vehicle b respectively. With this relationship given, ﬁrst, calculate

T using the approximated optimized solution, details are discussed in section

47 4.1.2. Then, apply this inequality to derive a safe d, which is used for later

trajectory generation.

Figure 3.7: Collision Free Selection of d

From ﬁgure 3.8, it can be seen that as long as the selection of distance of

traveling while lane changing is in the upper area of the lines, then two vehicles

will still maintain a safe longitudinal distance after the maneuver. Several

sample values are used for the simulation with diﬀerent vb and diﬀerent initial

relative distance. The vertical line is the optimized total Lance change time T

calculated based on approximate optimization and the value of vb. Also, the

desired d when there is no obstacle is also plotted, but as it shows in the graph,

it’s in the collision area and thus inadmissible.

E4 : va < vdes < vb and otherwise

48 The ﬁnal event under this case is when vehicle b is driving with a higher speed

than the ego vehicle very closely. In this case, a command of wait is given

until the vehicle b passed the ego vehicle and then initiate a normal lane change

command. ⇒ Wait

Case II: va < vE < vb < vdes

E5 : va < vE < vb < vdes and xb > xE + 0.5lE + 0.5lb

Here, this is the case very similar to E1. However, the diﬀerence is the ego

vehicle wants to drive at a speed faster than vehicle b. In this case,

still make an acceleration lane change but with the ﬁnal velocity equal to the

vehicle b and following behind it.

⇒ Acceleration Lane Change with VEf = Vb (Follow vehicle b)

Figure 3.8: Lane Change Event 5 - Example

Since an acceleration lane change need to be made, both d and T again need to

be calculated. At the end of the maneuver, TIV indicates the following must

be hold,

x − x − 0.5l − 0.5l TIV = bf Ef E b > 0.5 (3.3.4) ve

49 Apply the geometry relationships

x = x + d Ef Ei (3.3.5) xbf = xbi + vbT = xEi + 0.5lE + 0.5lb + di + vbT

Plug these into equation 3.3.4 yields,

d < vbT − 0.5vE + di (3.3.6)

E6 : va < vE < vb < vdes and xE − 0.5lE − 0.5lb ≤ xb ≤ xE + 0.5lE + 0.5lb

In this case, since the velocity of vehicle b is higher, ego vehicle should just

choose to wait until vehicle b pass. And then, it enters into the case of event 5

where an acceleration lane change maneuver will be initiated. ⇒ Wait

E7 : va < vE < vb < vdesand xb < xE − 0.5lE − 0.5lb and

TTC = xE −xb−0.5le−0.5lb > 1s and TIV = xE −xb−0.5le−0.5lb > 0.5s vb−vE vb

Similar to the case of Event 3, but since vehicle b is driving at a slower velocity

than the desired velocity set by the driver, an acceleration lane change maneuver

should be initiated with the ﬁnal velocity of the ego vehicle equal to the desired

velocity. ⇒ Acceleration Lane Change with VEf = Vdes (Lead vehicle b)

And the output of this event will need to meet the following criteria based on

TIV,

d > vb(T + 0.5) − di (3.3.7)

E8 : va < vE < vb < vdes and otherwise The other relative positions under this

velocity will need to wait until one of the other event is triggered. Then, corre-

sponding lane change maneuver can be generated. ⇒ Wait

50 3.3.3 Slower Vehicle in the Adjacent Lane

Case I: vb < va < vdes

E9 : vb < va < vdes and xb − xE ≥ 0

Since the vehicle b is driving at the lowest speed among the three vehicles, if

the vehicle b is leading the ego vehicle, then the ego vehicle can just wait until

passed vehicle b ⇒ Wait

E10 : vb < va < vdes and xb − xE < 0

Now the vehicle b is surpassed by the ego vehicle or, in other words, the ego vehi-

cle is leading, then a normal optimized lane change trajectory can be generated

⇒ Normal lane change

Case II: va < vb < vE < vdes

E11 : va < vb < vE < vdes and xb > xE + 0.5lE + 0.5lb

Although both vehicle a and vehicle b are driving at a slower velocity than the

desired velocity set by the driver, the adjacent lane i.e. the lane of vehicle b is

still faster than the current lane. Thus, a lane change maneuver is recommended,

but the ﬁnal velocity of the ego vehicle should equal to the velocity of vehicle

b. Thus, this is a deceleration lane change maneuver.

⇒ Deceleration Lane Change with VEf = Vb (Follow vehicle b)

Again, trajectory parameters need to be calculated for this event. With the

TIV > 0.5 holds, d and T must satisfy the following condition

d < vbT − 0.5vb + di (3.3.8)

51 Figure 3.9: Lane Change Event 11 - Example

E12 : va < vb < vE < vdes and xE − 0.5lE − 0.5lb ≤ xb ≤ xE + 0.5lE + 0.5lb In this

case, the decision making can be depending on the type of driver. Some drivers

might want to have an acceleration lane change trajectory while some drivers

prefer to wait until the vehicle b pass and then do the lane change. Here, to be

more conservative and ensuring safety, system recommends wait and then make

the lane change ⇒ Wait

E13 : va < vb < vE < vdes and xb < xE − 0.5lE − 0.5lb

In this case, since vehicle is already behind the ego vehicle and driving at a

slower velocity than the ego vehicle, a lane change maneuver should be initiated.

Moreover, because currently, ego vehicle is slower than the desired velocity,

which might be caused by driver’s delay of making a lane change, an acceleration

lane change trajectory is desired. ⇒ Acceleration Lane Change with VEf =

Vdes (Lead vehicle b)

3.3.4 Finite State Machine for Lane Change Decision Making

Based on all the cases discussed above, the following Finite State Machine is designed showed in ﬁgure 3.10. This Maneuver FSM is the lane change decision making module, which is used as an combination of the vehicle longitudinal following

52 decision making FSM. When a preceding vehicle is detected, or in other words, the

relative distance between the preceding vehicle and the ego vehicle becomes smaller

than the threshold dmax, which is the maximum vehicle following distance, the lane change module is triggered and started to plan. The MPC controller designed later is used to controller the vehicle driving on the lane change trajectory generated from the trajectory generator and the vehicle following controller designed above is used for the vehicle following control. Since the system is designed to help drivers’ speed control all the time and has no authority on lateral control, both the maneuver FSM and lane change trajectory generator starts to working once the preceding vehicle is detected but MPC controller is not triggered until a lane change command is given by the driver. Before that, system keeps using the longitudinal vehicle following controller. And all the cases with 2 lane, three vehicles are considered in the ﬁnite state machine if the other two surrounding vehicles keep their speed as constant.

And the most important job for the maneuver ﬁnite-state machine is to inform the trajectory generator what kind of trajectory should be designed. It is either the optimal lane change trajectory with no obstacles in the adjacent lane or the trajectory with one obstacle is applied. For the latter case, the maneuver FSM should also calculate the needed information of T , d and vf based on collision detector to ensure

a safe lane change trajectory.

53 Figure 3.10: Finite State Machine for Lane Change Maneuver Generation

State of the FSM Model Switches S1: Normal Lane Change Optimized lane change trajectory with no obstacle S2: Wait Continuing to used current vehicle following controller S3: Acceleration Lane Change with vEf = vb Accelerating lane change with vEf = vb (Lead Vehicle b after lane change) and d > vbT + 0.5vb − di S4: Acceleration Lane Change with vEf = vb Accelerating lane change with vEf = vb (Follow Vehicle b after lane change) and d < vbT − 0.5vb + di S5: Acceleration Lane Change with vEf = vdes Accelerating lane change with vEf = vdes (Lead Vehicle b after lane change) and d > vbT + 0.5vb − di S6: Deceleration Lane Change with vEf = vb Decelerating lane change with vEf = vb (Follow Vehicle b after lane change) and d < vbT − 0.5vb + di S7: Acceleration Lane Change with vEf = vdes Accelerating lane change with vEf = vdes (Lead Vehicle b and used d∗ for lane change) and d = d∗ S8: Cruise Control Standard cruise control

Table 3.1: State of Maneuver FSM

54 Events Interface Conditions to generate the events E1 va < vdes < vb and xb > xE + 0.5lE + 0.5lb E2 va < vdes < vb and xE = 0.5lE − 0.5lb ≤ xb ≤ xE + 0.5lE + 0.5lb E3 va < vdes < vb and xb < xE − 0.5lE − 0.5lb and TTC = xE −xb−0.5lE −0.5lb > 1s vb−vE xE −xb−0.5lE −0.5lb and TIV = > 0.5 and vb = va < vT vb E4 va < vdes < vb and otherwise E5 va < vE < vb < vdes and xb > xE + 0.5lE + 0.5lb E6 va < vE < vb < vdes and xE − 0.5lE − 0.5lb ≤ xb ≤ xE + 0.5lE + 0.5lb E7 va < vE < vb < vdes and xb < xE − 0.5lE − 0.5lb and TTC = xE −xb−0.5lE −0.5lb > 1s and TIV = xE −xb−0.5lE −0.5lb > 0.5 vb−vE vb E8 va < vdes < vb and otherwise E9 vb < va < vdes and xb − xE ≥ 0 E10 vb < va < vdes and xb − xE < 0 E11 va < vb < vE < vdes and xb > xE + 0.5lE + 0.5lb E12 va < vb < vE < vdes and xE = 0.5lE − 0.5lb ≤ xb ≤ xE + 0.5lE + 0.5lb E13 va < vb < vE < vdes and xb < xE − 0.5lE − 0.5lb E14 yE = 1.5w E15 ei < dmax

Table 3.2: Events and Threshold for Maneuver Generation

Output Variables Meaning vEf Velocity of the ego vehicle at the end of lane change maneuver d Total length of lane change maneuver T Total time of lane change maneuver

Table 3.3: Output Variables from Maneuver FSM

55 Constant Parameters Meaning lE Length of the ego vehicle lb Length of vehicle b vdes Desired longitudinal speed set by driver vT Relative velocity threshold dmax Maximum following distance for ACC

Table 3.4: Constant Parameters in Maneuver FSM

State Variables Meaning va Velocity of the vehicle a vb Velocity of the vehicle b vE Velocity of the ego vehicle xE Center position of the ego vehicle xb Center position of vehicle b di Longitudinal relative distance between vehicle b and the ego vehicle d∗ Optimized total length of lane change maneuver ei Longitudinal relative distance between vehicle a and the ego vehicle

Table 3.5: State Variables in Maneuver FSM

56 Chapter 4: Lane Change Trajectory Generation

Based on previous maneuver generation of lane change, now, a desired lane change trajectory need to be generated in order to the vehicle to follow. This also means the trajectory generation module should generate an optimal trajectory according to the decision made in the decision making module. Based on the recommendation received from the maneuver generation module, the lane change trajectory generation module will generate a corresponding desired trajectory and later used in the MPC controller design as a reference for the vehicle to follow.

Different situations should be considered for lane change trajectory. In order to design an obstacle avoidance lane change trajectory, first of all, an optimal trajectory while there are no other vehicles in the adjacent line should be generated considering both the desired longitudinal velocity and riding comfort where the desired longitudinal velocity is set by the driver and the system should be able to generate corresponding optimal lane change trajectory based on different desired longitudinal velocity the driver set. And to achieve riding comfort, both the vehicle lateral acceleration and maximum lateral jerk should be bounded. An optimization procedure is involved in the generation. The cost function is designed to minimize the overall kinetic energy for the lane change maneuver with constraints on both the velocity and acceleration. And an online approximation method is applied to calculate two

57 variables in the optimization problem. Tow variables are total length of the lane change maneuver and total time to complete the maneuver.

In the case of a vehicle is encountered in the adjacent lane during the lane change maneuver, the optimal lane change trajectory designed when there’s no obstacle might not be applied because collision might occur. Then, we can solve the problem by increasing the order of the lane change trajectory polynomial. With the same boundary conditions and higher order of the polynomial, trajectory is changed to avoid collision with other vehicles. Because the longitudinal direction has more freedom while lane changing, a fifth order polynomial can be increased to be a sixth order polynomial, thus modifying the trajectory to avoid the other vehicles. In order to calculate the extra coefficient, we can first predict the trajectory of the detected obstacle vehicles cause the position of obstacles is definitely needed to calculate collision avoidance lane change trajectory. Next, the nearby vehicle can be represented by infinite number of circles. Both information is used for the extra parameter. Detailed calculation of the trajectory generation is given in the following sections.

4.1 Lane Change Trajectory Initial Design

When there are no other vehicles in the adjacent lane, two 5th polynomials one for lateral displacement and one for longitudinal displacement was designed based on the criteria of minimum total kinetic energy during the lane change maneuver with constraints on both lateral acceleration and minimum jerk to ensure the rider’s comfort. The polynomial can be derived by having three constraints at the initial point and three constraints at the end of the lane change maneuver. Those constraints are displacement, velocity and acceleration at initial and ﬁnal point in terms of lateral

58 and longitudinal directions.For simplicity, two assumptions are made. One is that the acceleration in both directions at initial and ﬁnal state is zero. Also, it’s assumed that ego vehicle’s velocity is the same at the starting point of the maneuver and the end of the maneuver. Although this assumption is not guaranteed to be true, it’s a method of simplifying the trajectory design and wont’ have big eﬀects on later control cause disturbance and robustness should also be considered in controller design.

Figure 4.1: Single Lane Change with No Obstacle

Since the trajectory generation involves an optimization procedure to meet the requirements of minimum total kinetic energy, two variables are deﬁned for this purpose: the total length d and total time T . Both are unknown and determined by optimization. Over all parameters needed are deﬁned as follows,

59 d := total longitudinal distance traveled during the lane change maneuver T := total time used for lane change w := the width of the lane v := velocity of the ego vehicle at inital and ﬁnal state

Amax := the maximim lateral acceleration of the ego vehicle

4.1.1 Calculation of trajectory parameters

Three initial values of location, velocity and acceleration at the beginning of the lane change as well as three ﬁnal values of position, velocity and acceleration in both longitudinal and lateral directions form in total six constraints to design the lane change trajectory. In order to meet all those constraints, a ﬁfth-order polynomial function of time in each direction can be formulated and denote x as the longitudinal driving direction and y as the lateral lane change direction. Both polynomials describe the position of the vehicle in terms of time and it’s formulated as below:

5 4 3 2 x(t) = a5t + a4t + a3t + a2t + a1t + a0 (4.1.1)

5 4 3 2 y(t) = b5t + b4t + b3t + b2t + b1t + b0 (4.1.2)

Now, those six coeﬃcients need to be calculated using six constraints as discussed above. First, the boundary conditions for longitudinal direction is,       x0 x0 0 X = x˙  = v  = v  (4.1.3) 0  0  x0   des

x¨0 ax0 0       xf xf d X = x˙  = v  = v  (4.1.4) f  f   xf   des

x¨f axf 0 where d is the total distance traveled along the longitudinal direction while lane

changing. vx0 and vxf are initial and ﬁnal velocity of the lane change maneuver

60 respectively. If in the ideal situation meaning there are no other vehicles in the

adjacent lane, then both velocities should be equal to the desired driving velocity set

by the driver. To calculate the coeﬃcients, both ﬁrst and second derivative of the

polynomial need to be used

x(0) = x0 ⇒ a0 = x0 = 0 (4.1.5)

x˙(0) = vx0 ⇒ a1 = vx0 = vdes (4.1.6) 1 x¨(0) = a ⇒ 2a = a ⇒ a = a = 0 (4.1.7) x0 2 x0 2 2 x0 1 x(T ) = a T 5 + a T 4 + a T 3 + a T 2 + v T + x = x (4.1.8) 5 4 3 2 x0 x0 0 f 4 3 3 x˙(T ) = 5a5T + 4a4T + 3a3T + ax0 T + vx0 = vxf (4.1.9)

3 2 x¨(T ) = 20a5T + 12a4T + 6a3T = axf (4.1.10)

For easier calculation, 4.1.8 - 4.2.11 should be formulated into a matrix represen-

tation. And based on the boundary condition, further simpliﬁcation can be made

 3 4 5     1 2   T T T a3 xf − x0 − vx0 T − 2 ax0 T d − vdesT 3T 3 4T 3 5T 4  a  =  v − v − a T  =  0 (4.1.11)    4  xf x0 x0    2 3 6T 12T 20T a5 axf 0

From the above calculation, all coeﬃcients can be represented as function of d, vdes and T

a0  0  a   v   1  des      a2  0    =   (4.1.12) a   10 (d − T v )   3  T 3 des     15  a4 − T 4 (d − T vdes) 6 a5 T 5 (d − T vdes) The same procedure can be applied to derive coeﬃcients of the trajectory along the lateral direction. The boundary conditions for lateral direction is,

61       y0 y0 0 Y = y˙  = v  = 0 (4.1.13) 0  0  y0   

y¨0 ay0 0       yf yf w Y = y˙  = v  = 0 (4.1.14) f  f   yf   

y¨f ayf 0 In this constraints, the initial lateral position of the vehicle is set at 0 instead of

w in the middle of the lane i.e. 2 because this yields easier calculation of the coeﬃ- cients, which will be needed for later optimization of two variables T and d. And

w the trajectory representation with starting point at 2 will also be calculated with the optimized T and D plugged in as the actual desired trajectory used for controller.

y(0) = y0 ⇒ b0 = y0 = 0 (4.1.15)

y˙(0) = vy0 ⇒ b1 = vy0 = 0 (4.1.16)

y¨(0) = ay0 ⇒ b2 = ay0 = 0 (4.1.17)

5 4 3 y(T ) = b5T + b4T + b3T = w (4.1.18)

4 3 3 y˙(T ) = 5b5T + 4b4T + 3b3T = 0 (4.1.19)

3 2 y¨(T ) = 20b5T + 12b4T + 6b3T = 0 (4.1.20)

Again, formulate 4.1.18 - 4.1.20 into a matrix representation yields,

 3 4 5      T T T b3 w 3T 3 4T 3 5T 4  b  0    4 =   (4.1.21) 2 3 6T 12T 20T b5 0 Then, the coeﬃcients of the lateral trajectory are,

   10  b3 w T 3 b  = −w 15  (4.1.22)  4  T 4  6 b5 w T 5 62 with b0 = b1 = b2 = 0. Now, the longitudinal and lateral trajectory while lane changing can be represented as,

t3 t4 t5 x(t) = v t + 10(d − T v ) − 15(d − T v ) + 6(d − T v ) (4.1.23) des des T 3 des T 4 des T 5

w t3 t4 t5 y(t) = + 10w − 15w + 6w (4.1.24) 2 T 3 T 4 T 5

From the equation for both trajectories, besides the parameter vdes is pre-deﬁned by the driver, the other parameters d and T are still be to be determined.

4.1.2 Trajectory Optimization

Since this is the desired trajectory designed for the controller to follow, an optimization process is preferred to determine these two parameters. If the criteria of this lane change maneuver is to minimize the overall maneuver time with ﬁxed constraints meaning that to formulate a time-optimal control problem with bounded lateral acceleration and lateral jerk, then apply the Pontryagin Principle to solve the

Hamiltonian for the time optimal problem is a good choice [9]. However, time optimal of the lane change maneuver requires the driver or the controller to apply maximum lateral acceleration through the process to ﬁnish the lane change maneuver, which will cause discomfort to the passenger in the vehicle. Thus, here, the cost function to minimize the total kinetic energy is used [39] and taking both the velocity and the acceleration constraints into account. In this case, not only driver comfort is considered, but also using the minimal energy, for example fuel energy to ﬁnish the lane change maneuver. The relationship between the extra force needed for a lane change and the extra kinetic energy is

1 ∆F d = m(v − v )2 (4.1.25) 2 f i

63 where ∆F is the extra force needed to accelerate the vehicle to vf , which is the ﬁnal velocity at the end of the maneuver and vi is the initial velocity. d is the overall distance traveled during the lane change maneuver. Since the mass of the vehicle is a constant, then, smaller velocity implies smaller external forces needed. Thus, the cost function is formulated as,

Z T J = (x ˙ 2 +y ˙2)dt (4.1.26) 0

Substitute both equation 4.1.23 and 4.1.24 in to the cost function and with some number manipulation can represent the cost function as the following

10 J = (w2 + (d − vT )2) + 2v(d − vT ) + v2T (4.1.27) 7T

With the cost function formulated, two constraints mentioned above of the optimization problem need to be considered. They are the velocity constraint and the acceleration constraint.

Optimization Constraints

1. First, the velocity constraint is used to make sure that the longitudinal velocity

of the vehicle is always positive meaning the vehicle is moving forward meaning

thatx ˙(t) > 0. From 4.1.23, velocity of the trajectory can be represented as

30 60 30 f (t) =x ˙(t) = v + (d − T v )t2 − (d − T v )t3 + (d − T v )t4 v des T 3 des T 4 des T 5 des (4.1.28)

To ensure the velocity of the vehicle is always greater than zero, we only need

to check the minimum velocity along the trajectory. Based on the optimality

suﬃcient conditions [3], if x∗ ∈ Rn where ∇f(x∗) = 0 and ∇2f(x∗) > 0 i.e. positive deﬁnite, then x∗ is a local minimum of the function. Thus,the time for

64 the vehicle to achieve the minimum velocity can be calculated from the gradient

of fv(t) 60 180 120 ∇f (t∗) = (d − T v ) t∗ − t∗2 + t∗3 = 0 (4.1.29) v des T 3 T 4 T 5

⇒ t∗ = 0 or t∗ = T (4.1.30)

Since the variable t is bounded i.e. t ∈ [0,T ], the Hessian matrix is not needed

and from the calculation of the gradient, one can obtain that the minimum

velocity of this trajectory is at vdes and thus the ﬁrst constrain is also met no

matter what T and D is chosen.

2. Second, the maximal acceleration or deceleration should also be bounded in

order to ensure the driver comfort while the lane change. From 4.1.23 and

4.1.24, the vehicle acceleration can be represented as,

p 2 2 fa(t) = x¨ +y ¨ (4.1.31) s 60 180 120 2 60w 180w 120w 2 = (d − T v )2 t − t2 + t3 + t − t2 + t3 des T 3 T 4 T 5 T 3 T 4 T 5 (4.1.32)

Instead of considering the minimum value like the velocity, the maximum accel-

eration should be bounded to ensure the riding comfort. And it can be shown

that the maximum acceleration can be represented as, √ 10 3 f (t) = pw2 + (d − vT )2 (4.1.33) a max 3T 2

The boundary of this acceleration can be reasonably pre-deﬁned as Amax, which

is the norm of the maximal acceleration of the ego vehicle that ensures the riding

comfort. Thus, the constraint can be deﬁned in two ways

fa(t) ≤ Amax (4.1.34)

65 or √ 10 3 f (t) = pw2 + (d − vT )2 = A (4.1.35) a max 3T 2 max

Since the second option will be easier for optimization calculation, we choose

this way as the constraint.

Thus, the overall optimization problem for the lane change trajectory with two

optimization variables T and d can be formulated in this way

10 min J = (x2 + (d − vT )2) + 2v(d − vT ) + v2T (4.1.36) √ 7 10 3 s.t. pw2 + (d − vT )2 = A (4.1.37) 3T 2 max

Uniqueness of the cost function

For this optimization problem, a unique global solution is preferred to make the problem useful and can be applied back to the lane change trajectory. And the convexity of the cost function implies the uniqueness [3]. Thus, in this section, the convexity of the cost function is proofed. From 4.1.35, it shows that

3A2 T 4 w2 + (d − vT )2 = max (4.1.38) 10 s 3A2 T 4 2 ⇒ d − vT = max − w2 (4.1.39) 10 Substitute 4.1.39 into 4.1.27 yields,

3 v √ J(T ) = A2T 3 + v2T − 3A2T 4 − 100w2 (4.1.40) 70 5

In order to check convexity, apply the deﬁnition of convexity based on [3] and it is to

check if the following holds,

f(αx + (1 − α)y) ≤ αf(x) + (1 − α)f(y) (4.1.41)

66 Now, plug 4.1.40 into 4.1.41 and check, 3 f(αx + (1 − α)y) = A2(αx3 + (1 − α)y3) + v2(αx + (1 − α)y) 70 (4.1.42) v − p3A2(αx + (1 − α)y)4 − 100w2 5 3 v √ αf(x) + (1 − α)f(y) = α A2x3 + αv2x − α 3A2x4 − 100w2 70 5 (4.1.43) 3 v p +(1 − α) A2y3 + (1 − α)v2y − (1 − α) 3A2y4 − 100w2 70 5

⇒ f(αx + (1 − α)y) − αf(x) − (1 − α)f(y) √ p = α 3A2x4 − 100w2 + (1 − α) 3A2y4 − 100w2 − p3A2(αx + (1 − α)y)4 − 100w2

< 0 (4.1.44)

It should be noted that inequality 4.1.44 not only holds for less than and equal, but holds for strictly less than. Based on convexity theorem of optimization, when the cost function is convex, then the local minimum is equivalent to the global minimum.

And when the cost function is strictly convex, then there has at most only one global minimum. Thus, it can be concluded that the cost function is strict convex i.e. has a unique global minimum solution.

Numerical Approximating of T ∗ and d∗

With the optimization problem formulated and convexity of the cost function proved, now the solution of the problem need to be found. Here, since an analytic solution is very diﬃcult to ﬁnd, the numerical method together with the optimization software Lingo proposed in [39] is used. And based on the results, it can be concluded that with high enough velocity, the optimal solutions for the problem can be obtained

67 from

r w d∗ ≈ 2.4v (4.1.45) Amax

3 √ √ 2 r ∗ w Amax w T ≈ 3 2 + 2.4 (4.1.46) v Amax

4.1.3 Simulation of Sample Trajectories Generation

A sample lane change trajectory is simulated here using the ﬁfth order polynomial and both the total time of lane change T and the overall distance traveled while lane changing d are calculated using the numerical approximating method. In this example, vehicle is driving at vx = 20 and distance traveled is d = 50.34m. The following graph shows the generated trajectory as well as it’s velocity and acceleration.

Figure 4.2: Lane Change Trajectory 5th order

68 Figure 4.3: Longitudinal Velocity and Acceleration of the Trajectory 5th order

Figure 4.4: Lateral Velocity and Acceleration of the Trajectory 5th order

4.2 Generation of Trajectories with One Obstacle

In the case when there is an obstacle like another vehicle driving either on the same lane of the ego vehicle or the adjacent lane, the previous trajectory designed with ﬁfth order polynomial will result in a collision because it is not taking the obstacle into consideration while designing. In terms of designing the lane change trajectory with obstacles, extensive research has been conducted for the emergency lane change maneuvers. The vehicle path panning problem is formulated as a Model

69 Predictive Control (MPC) in [25] where the path is generated as the solution of a constrained optimal control problem over a ﬁnite time horizon. It considers the constraints on vehicle dynamics, physical constraints and collision avoidance constraints in terms of making the lane changing within the safe longitudinal distance relative to the surrounding vehicles. It is worth mentioned that within the collision avoidance constraints, both Forward Collision Constraint (FCC) and Rear Collision Constraint

(RCC) are included. In [8], a time optimal approach considering less longitudinal distance traveled during the lane change maneuver. A gap alignment trajectory is designed for the ego vehicle to align with the target gap then followed by a moverover trajectory. A general description of trajectory generation with a simple point vehicle model is used. Also, a more complicated and complete parameter optimized trajectory is formulated in [42] subject to constraints on both lateral and longitudinal acceleration. It applies a vehicle bicycle model with calculation of the maximum tire force can be obtained to design the emergency lane change trajectory. However, since both of them are more focused on emergency situation, thus driver comfort is not considered and optimal bang-bang control is designed to ﬁnish the lane change maneuver. In this project, more attention is being paid on the case where either the preceding slower vehicle can be detected at a safer distance or a surrounding vehicle is driving at the adjacent lane of the ego vehicle. A more comfortable and safer lane change trajectory can be designed.

In this section, a sixth order polynomial on the longitudinal direction is applied to modify the origin lane change trajectory in order to achieve the goal of collision avoidance. Two steps are needed to design the trajectory. One is to predict the evolution of the surrounding vehicle’s position. More complicated driver lane change

70 prediction can be studied, but is out of the scope of this project. Here, it only

considers the longitudinal behavior or surrounding vehicles. The second step is to

represent the extra trajectory coeﬃcient a6 with all known parameters including the

predicted surrounding vehicles’ position. Then, by using a simple map of a6 − t, the value of a6 can be chosen that ensures collision free.

4.2.1 Prediction of Surrounding Vehicles

First of all, the future position as well as velocity of the surrounding vehicles need to be predicted in order to design the lane change trajectory for the ego vehicle. In

[7], an experiment was conducted with complete software architecture. A maneuver module was designed to analyze the state of the ego vehicle and of the environment.

Moreover, since the information received is from real sensor’s perception of the environment, data fusion was discussed and predict the motion of every detected vehicles using a Kalman ﬁlter and uncertainty is also included. Thus, in general, the future motion of other traﬃc participants can be represented as

x(k + 1) x(k) +x ˙(k) · ∇T  x˙(k + 1) x˙(k) +x ¨(k) · ∇T        =   (4.2.1) y(k + 1) y(k) +y ˙(k) · ∇T  y˙(k + 1) y˙(k) +y ¨(k) · ∇T

This is the general prediction expression of the surrounding vehicles where x(k)

and y(k) are the longitudinal position and lateral position of the surround vehicle respectively. ∇T is the sampling rate set for the simulation. And since, in this project, assume the other traﬃc participants only drive along the longitudinal direction and won’t make a lane change, then, the lateral prediction can be omitted. However, if there is a need to include the prediction of lateral behavior, [7] also talks about

71 the method of calculating the probability of three potential lateral maneuvers i.e. a) staying in the current lane; b) making a left lane change; c) making a right lane change.

And all information of position and speed of both ego and surrounding vehicles is combined to calculate the probability. Here, by omitting the lateral prediction and denote the state vector X(k) = [x(k), x˙(k)], a predictive phase of a Kalman ﬁlter can be formulated as following, " # " # 1 ∆T 0 X(k + 1) = X(k) + u (4.2.2) 0 1 ∆T 0

where u is the input i.e. the acceleration, to the system at the initial time. And

X(k+1) is the predicted state and the predicted horizon can be modiﬁed. In the later simulation, the predicted horizon is set as the same time length as the lane change maneuver. If needed, some uncertainty can be added into the prediction process because in real time implementation, both sensors and data-fusion module will cause some uncertainty to the measurement. Overall, the predictive phase is

X(k + 1) = AX(k) + Bu0 (4.2.3)

P (k + 1) = AT P (k)A + Q (4.2.4) where the matrices A, B, Q are deﬁned as, " # " # " # 1 ∆T 0 es esx˙ A = ,B = ,Q = (4.2.5) 0 1 ∆T 0 ex˙ where ei is the signal error, which forms the overall matrix Q as the noise matrix from the process. And P (k) is the covariance of the system.

4.2.2 Overview of Trajectory Parameters Design

If there is a slower preceding vehicle in front of the ego vehicle or a surrounding vehicle driving at the adjacent lane besides the ego vehicle, a collision might occur

72 if the previous fifth order polynomial lane change trajectory is used. Because the previous one only considers the smoothness, boundary on velocity and both longitudinal and lateral acceleration. When the obstacle vehicle comes into play, first, a collision detector needs to be applied with the prediction of the future position of both vehicles to ensure a collision free path. And then, based on the information given from the collision detector, a better trajectory considering collision avoidance can be generated. Based on [29], if increase the order of the previous fifth order polynomial trajectory, with the same boundary condition applied, the shape of the trajectory changes and that is going to help avoid the other vehicles. Moreover, using a sixth order polynomial not only ensures the trajectory itself is continuous, but also ensures the first and second derivative i.e. vehicle’s velocity and acceleration are continuous and bounded. With the addition of vehicle dynamics constraints, as a result, the trajectory is definitely feasible and ensuring riding comfort for the rider. Also, after certain calculation and simplification, this is a relatively easy method to be used for real-time implementation.

There are also several choices available after the decision of increasing the order of polynomials has been made. We can choose to increase both longitudinal and lateral lane change trajectory’s degree or only one of them. For calculation simplicity, increasing both trajectories’ order is not considered for now. Moreover, it can be seen that increasing the order of the longitudinal direction will bring more freedom to the overall trajectory. Because of the lateral trajectory position deviation is only restricted at around 3m while the longitudinal trajectory provides freedom at around

50m based on diﬀerent velocity. Also, both the lateral velocity and lateral acceleration have constraints much smaller than the longitudinal direction. Thus, only the order

73 of the longitudinal trajectory should be increased. In the following section, ﬁrst, the

collision detector will be designed to formulate the criteria of a collision free path.

Then, the longitudinal trajectory with higher order will be calculated.

4.2.3 Collision Detector - Gap between Vehicles

The job of the collision detector is to check the distance between the ego vehicle

and the obstacle at every time instance through the lane change maneuver and provide

the safe distance calculation between two vehicle while lane changing. There are

multiple ways to represent the obstacle which need to be mathematically modeled to

analytically calculate the safe distance between. In [8], a point mass model is applied.

Although this is a easier way for calculation and faster implementation, it’s highly

inaccurate and may lose the eﬃciency of vehicle traveling arrangement. On the other

hand, [30] deploys the method of representing the obstacle as one circle that encloses

the entire vehicle. Although it ensures safety, spaces that are safe are wasted cause

part of the safe space is also enclosed into the circle modeled as an obstacle. Thus,

an inﬁnite number of circles can be used to represent the vehicle [29] that can ensure

the eﬃcient usage of the road or trajectory planning and it also guarantees collision

free.

Here, for the collision detector, because of simplicity, the vehicle is modeled as one

circle that enclose the entire vehicle with a radius R to demonstrate the idea of vehicle to obstacle distance [48]. Also, extra spaces considered for the collision detector might be good because of uncertainty occurred from sensors, system modeling and so on.

It can at least ensure the safety of both vehicle. And in the next section, a number

74 of circles will be used to represent the obstacle for more accurate calculation of the trajectory based on the distance to obstacle calculated in this section.

Figure 4.5: Distance to Obstacle

From ﬁgure 4.5, each vehicle is represented by one circle with radius R which is half of the length of the vehicle. The distance between two vehicles i.e. the distance to obstacle is

p 2 2 dist = (xo − xE) + (yo − yE) (4.2.6)

And in order to ensure the safety of both vehicles, the following inequality should hold for all the time

dist − (Ro + RE) > 0 (4.2.7)

This equation can be intuitively explained from the geometry location of two vehicles.

The circle representing the vehicle enclose all the physical parts of the vehicle and thus, as long as the distance between the center point of each vehicle is greater than

75 the summation of half the vehicles’ length, then it’s collision free. Also, the position of both vehicles should be taken at every sampling time instance from the beginning to the end of the lane change maneuver. This is the reason why prediction of the surrounding vehicles are needed. Based on the collision check, the following collision avoidance lane change trajectory can be designed.

4.2.4 Collision Avoidance Trajectory

In this section, the actual lane change trajectory with one obstacle should be generated. Based on the discussion above, the longitudinal direction trajectory should be increased to higher order yields,

6 5 4 3 2 x(t) = a6t + a5t + a4t + a3t + a2t + a1t + a0 (4.2.8)

By applying the same boundary condition as mentioned in the previous design, it

is easier to see that a0 = x0 = 0, a1 = vx0 , a2 = 0. For the others,

6 5 4 3 x(T ) = a6T + a5T + a4T + a3T + vx0 T + x0 = xf (4.2.9)

5 4 3 3 x˙(T ) = 6a6T + 5a5T + 4a4T + 3a3T + vx0 = vxf (4.2.10)

4 3 2 x¨(T ) = 30a6T + 20a5T + 12a4T + 6a3T = 0 (4.2.11)

Putting three equations again into matrix form,

 3 4 5     6  T T T a3 d − vx0 T − a6T 3T 3 4T 3 5T 4  a  = v − v − 6a T 5 (4.2.12)    4  xf x0 6  2 3 4 6T 12T 20T a5 −30a6T

Therefor, the coeﬃcients a3, a4, a5 can be written as,

   1 6  a3 − T 3 (6vx0 T − 10d + 4vxf T + a6T ) a  =  1 (8v T − 15d + 7v T + 3a T 6)  (4.2.13)  4  T 4 x0 xf 6  3 6 a5 − T 5 (vx0 T − 2d + vxf T + a6T )

76 Isolating a6 out and all the other coeﬃcients can be represented in terms of a6 as,

ai = αi + βia6 (4.2.14) where α, β can be written in matrix as,     α0 0     β0 0 α1  vx     0  β1  0          α2  0  β2  0    =  2  &   =  3  (4.2.15) β3 −T α3  T 3 (5d − 3vx0 T − 2vxf T )             2     1  β4  3T  α4 − T 4 (15d − 8vx0 T − 7vxf T ) 3 β5 −3T α5 T 5 (2d − vx0 T − vxf T ) Now, plug equation 4.2.15 into 4.2.8 and the longitudinal trajectory x(t) can be written in terms of a6 i.e.

x(t) = k1 + k2a6 (4.2.16) where

3 5 1 4 k1 = 5 (2d − vx0 − vxf T )t − 4 (15d − 8vx0 T − 7vxf T )t T T (4.2.17) 2 + (5d − 3v T − 2v T )t3 + v t T 3 x0 xf x0

6 5 2 4 3 3 k2 = t − 3T t + 3T t − T t (4.2.18)

Now, the extra coeﬃcient a6 is free to be designed in order to avoid other vehicles.

And the idea of collision detector mentioned in the previous section is applied. First, in order to generate more accurate trajectory for the lane changing, the vehicle is now represented a line of circles with the radius equal to the width instead of using just one circle to enclose the whole vehicle. Figure 4.6 explains the representation of the vehicle. And during the calculation, the front and rear circle are the most important and the other circles can be represented using these two. This representation method is relatively fast and more accurate.

77 Figure 4.6: Vehicle Representation - Circle

The rectangular shape is used to represent the original vehicle. And the rectangular shape is now replaced by a series of circles with the radius of R. And there are three coordinates c0, c1 and x0 to show the front, rear and center position of the vehicle,

P = Pr + µ(Pf − Pr) (4.2.19)

where Pr and Pf are the vectors of the front and the rear circle centers. Now, trans- form equation 4.2.19 into Cartesian coordinates,

x = xr + µ(xf − xr) (4.2.20)

y = yr + µ(yf − yr) (4.2.21) where x and y can be interpolated as either the collection of all the dynamic circles or the line segment connecting the front circle Pf and Pr. Here the parameter µ ∈ [0, 1] formulates a line function connecting the front and the rear circle. For example, when µ = 0, then x = xr and when µ = 1, the x = xf . And all the values of µ

78 in between represents one point on the line connecting these two circles. Also, xr, xf , yr, yf are the longitudinal and lateral coordinates of the rear circle and the front circle respectively. Now, combining the collision avoidance criteria in equation 4.2.7, if there is another preceding vehicle besides the ego vehicle, the trajectory will be safe if

2 2 2 (xE − xP ) + (yE − yP ) > (RE + RP ) (4.2.22)

where the subscript E represents the ego vehicle’s position while P represents the

preceding vehicle’s position. And RE, RP are the same as the width of the ego vehicle

and the preceding vehicle correspondingly. Now, the goal is to build a relationship

between the longitudinal trajectory and the collision avoidance. Meaning that the

new coeﬃcient a6 should be able to ensure the collision avoidance criteria is always

met throughout the lane change maneuver. Plug equation 4.2.20 and 4.2.21 into

4.2.22 yields,

2 [xEr + µE(xEf − xEr) − xP r − µP (xP f − xP r)] (4.2.23) 2 2 +[yEr + µE(yEf − yEr) − yP r − µP (yP f − yP r)] > (RE + RP )

In order to simplify the above equation, an assumption need to be made, i.e. the

vehicle having a very small yaw angle while the lane change maneuver. Although this

assumption is not completely true during practice, but it is a reasonable assumption

because even during the emergency lane change case where the maximum lateral jerk

as well as maximum lateral acceleration are applied, the lateral yaw angle still remains

relatively small. With this assumption, the term,

yEf − yEr ≈ 0 (4.2.24)

79 For the longitudinal direction,  xEr + µE(xEf − xEr)  lE lE with x = x − lE ⇒ x(t) − + µ l = k + k a − + µ l (4.2.25) Er E0 2 2 E E 1 2 6 2 E E xEf − xEr = lE  Now, plug 4.2.25 into 4.2.23 yields,

2 (ζ1 + k2a6) + ζ2 > 0 (4.2.26)

where ζ1 and ζ2 are represented as follows,

l ζ = k − E + µ l − x − µ (x − x ) = k + l (µ − 0.5) − x − µ l 1 1 2 E E P r P P f P r 1 E E P r P P (4.2.27) 2 2 2 2 ζ2 = [yEr − yP r − µP (yP f − yP r)] − (RE + RP ) = [yEr − yP r] − (RE + RP ) (4.2.28)

Since the goal right now is to build a relationship between the coeﬃcient a6 and

the collision avoidance criteria. Now, rewrite 4.2.26 in terms of a6,

2 2 2 k2a6 + 2ζ1k2a6 + ζ1 + ζ2 > 0 (4.2.29)

Thus, we need to ﬁnd the range of a6 that ensures the above equation holds.

Cause as long as it holds, there will be no collision between the ego vehicle and the

preceding vehicle during the lane change process. In can be noted that the coeﬃcient

2 of a6 i.e. k2 is guaranteed to be greater than zero, thus, only two roots of the second

order polynomial need to be found and as long as a6 is chosen outside of these two

roots, the path will be collision free. That is √ √ −2ζ1k2 − ∆ −2ζ1k2 + ∆ a6 < 2 ∪ a6 > 2 (4.2.30) 2k2 2k2 where ∆ is represented as,

2 2 2 ∆ = (2ζ1k2) − 4k2(ζ1 + ζ2) (4.2.31)

80 With these all formulated, the next step is just plotting the solution to 4.2.29 at every time instance and select the value of a6 that lies outside of these roots to ensure a collision free path. Then, in the simulation section, examples of chosen appropriate a6 is given.

4.2.5 Vehicle Dynamic Constraints

In the first design of trajectory with no obstacles, two fifth order polynomials are generated with T and d determined by the numerical approximation method. Since vehicle dynamic constraints are added as constraints during the optimization process, both trajectories are guaranteed to be feasible. However, when an obstacle occurs, a sixth order polynomial is used instead o fifth on the longitudinal direction and vehicle dynamic constraints might be violated. Since there is no change made on the lateral trajectory and the overall time needed to finish the lane change maneuver, which is needed in deciding the lateral trajectory parameters, is still obtained from the optimized formula. Even though the values of d, the overall distance traveled through the lane change maneuver, need to be changed to meet the criteria of TTC and TIV of safety, d has no effect on the lateral trajectory parameter and d is calculated based on the given optimized T as well as criteria of TTC and TIV. These ensures that adding one more parameter in longitudinal trajectory will not effect the lateral trajectory.

However, extra constraints need to be added to ensure the feasibility of the trajectory. With sixth order trajectory, the longitudinal acceleration of the trajectory is,

4 3 2 x¨ = 30a6t + 20a5t + 12a4t + 6a3t (4.2.32)

81 and this need to be bounded by maximum acceleration aacc and maximum deceleration

adec achievable of the vehicle. Based on 4.2.15, vehicle acceleration can be represented by a6 alone,

x¨ = ϕ1 + ϕ2a6 (4.2.33)

where two parameters ϕ1 and ϕ2 can be represented as

60 3 12 2 ϕ1 = − 5 (vx0 − 2d + vxf T )t + 4 (8vx0T − 15d + 7vxf T )t T T (4.2.34) 6 − (6v T − 10d + 4v T )t T 3 x0 xf 4 3 2 2 3 ϕ2 = (30t − 60T t + 36T t − 6T t)a6 (4.2.35)

Thus, after this simpliﬁcation, the boundary conditions of acceleration and decelera-

tion can be written as,

ades − ϕ1 aacc − ϕ1 adec < ϕ1 + ϕ2a6 < aacc ⇒ < a6 < (4.2.36) ϕ2 ϕ2

And it should be noticed that aacc is a positive number whereas ades is a negative number. Moreover, both ϕ1 and ϕ2 are functions of t and this dynamic constraints can be plotted on the same a6 − t diagram for choosing a collision free a6. And if an a6 can be found meeting both constraints, that one will be chosen and used in the collision free trajectory generation.

4.2.6 Simulation of Sample Trajectories with One Obstacle Simulation I

In this simulation, event 3 discussed in the maneuver generation FSM is simulated. This is the case where vehicle b is driving faster than the ego vehicle and is longitudinally behind the ego vehicle. Thus, based on the maneuver FSM decision

82 making, an acceleration lane change trajectory should be generated and leading ve-

hicle b at the end of the maneuver. Here, vehicle dynamic constraints are not added.

And boundary conditions of this simulation are given as:

Boundary Condition vE0 vEf vb xE xb T d Values 25m/s 28m/s 28m/s 0m −17m 3.576s 101m

Table 4.1: Boundary Conditions for Trajectory Generation I

With the given conditions, the dynamic positions obstacle vehicle b is also sim-

ulated using the prediction formula explained above. And vehicle b is driving at a

constant speed with no change of its lateral position. In terms of longitudinal posi-

tion, vehicle b started 17m behind the ego vehicle, which satisﬁes the condition for

event 3. The ﬁnal longitudinal velocity of the ego vehicle is expected to be the same

as vehicle b with the safe relative distance.

Apply the design techniques discussed in section 4.1.2 to ﬁrst generate a map of a6 versus t and is plotted in 4.7. It should be noted that every values of µ corresponds

to one speciﬁc plot of root. However, since 4.2.30 is function of µ meaning that the

minimum and maximum acceptable acceptable values of a6 are also derived at the minimum and maximum values of µ. Thus, without loss of generality, only two roots are plotted and they are corresponding to the front and rear end circles of the ego vehicle. From the plot, it can be seen that, for example, choose a2 = −0.2 might result a collision at 0.7s or 4s. And a6 should be chosen outside of the root encircled area to ensure a collision free trajectory. And thus, a6 = 0.05 is chosen in the following

simulation because it has no interaction with the roots.

83 Figure 4.7: Selection of a6

With the selection of a6, the generated collision free trajectory is plotted. In

ﬁgure 4.8, the path is plotted along with the obstacle vehicle. They started at 17m longitudinal diﬀerence and after the lane change maneuver, the ego vehicle is still

17m behind vehicle b. From figure 4.12, the relative distance between two vehicle’s center position is plotted. And it starts bigger than 17m because two vehicles are in two different lanes and the lateral position difference is also added. At the end of the maneuver where two vehicles are on the same lane, the relative distance becomes to be 17m which meets the safe TTC and TIV criteria.

Also, all the characteristics of the trajectory is plotted. From the longitudinal velocity and acceleration plot, it can be seen that the velocity of the trajectory achieves

28m/s which is the desired ﬁnal longitudinal speed and is the same as vehicle b. But

84 for acceleration, since no vehicle dynamics constraints added to the choice of a6, the

maximum acceleration achieves 4m/s2 during the lane change while the acceleration constraints are g ≤ ax ≤ 0.3g. For lateral acceleration, since the lateral trajectory is

only related to T , the total time for the vehicle to ﬁnish the lane change maneuver,

and in this case T is derived from the optimized formula T ∗(v) which is bounded by

the maximum lateral acceleration, then the lateral acceleration is guaranteed to be

bounded no matter what value of a6 is chosen.

Figure 4.8: Collision Free Path for Event 3

Figure 4.9: Position in x and y Direction for Event 3

85 Figure 4.10: Velocity and Acceleration in x Direction for Event 3

Figure 4.11: Velocity and Acceleration in y Direction for Event 3

Figure 4.12: Relative Distance between Two Vehicles for Event 3

86 Simulation II

For the second simulation, event 11 discussed in the maneuver generation FSM

is simulated. This is the case where vehicle b is driving at a slower velocity than

the ego vehicle and is longitudinally ahead of the ego vehicle. Thus, an deceleration

lane change trajectory should be generated and following behind vehicle b after the

maneuver. Here, vehicle dynamic constraints are also added to the map as showed in

ﬁgure 4.13 where the left ﬁgure is overall mapping and the right side is the zoom in

image. It can bee seen that a6 need to be chosen outside of both roots and from −0.1 to 0.1 can be chosen. In the simulation a6 = −0.01 is chosen and the other boundary conditions are listed blow.

Boundary Condition vE0 vEf vb xE xb T d Values 30m/s 25m/s 25m/s 0m 3m 3.585s 80m

Table 4.2: Boundary Conditions for Trajectory Generation II

With a6 chosen from 4.13, the deceleration lane change trajectory is simulated.

Trajectory of vehicle b is also simulated using the same method in simulation I and

plotted along with ego vehicle’s trajectory in ﬁgure 4.14. Vehicle starts at 3m ahead

of the ego vehicle and the ﬁnal relative distance is more than 12.6m from ﬁgure 4.18.

This is a safe and desired relative distance based on the collision indicator TIV. In

this simulation, vehicle acceleration constraints are added and a6 is chosen to meet

the constraints. From ﬁgure 4.16, ego vehicle’s longitudinal velocity begins at 30m/s

87 Figure 4.13: Selection of a6 with Dynamic Constraints

and ends with the same velocity at vehicle b at 25m/s and the longitudinal acceleration is within the boundary 0.3g. For ego vehicle’s lateral velocity, the maximum lateral acceleration becomes larger but is still within the boundary of 0.2g. The lateral velocity increased because the overall time to ﬁnish this lane change maneuver becomes smaller.

Figure 4.14: Collision Free Path for Event 11

88 Figure 4.15: Position in x and y Direction for Event 11

Figure 4.16: Velocity and Acceleration in x Direction for Event 11

Figure 4.17: Velocity and Acceleration in y Direction for Event 11

89 Figure 4.18: Relative Distance between Two Vehicles for Event 11

90 Chapter 5: MPC Longitudinal Controller Design for Lane Changing

5.1 MPC Controller Design

5.1.1 Vehicle Longitudinal Dynamic

In this chapter, a longitudinal controller is designed to track the desired lane

change trajectory. However, since the coupling of vehicle’s lateral and longitudinal

dynamics and lateral is fully controlled by the driver, this control algorithms are lim-

ited to a small steering angle turning variation compared to the desired trajectory.

Assuming the driver is turning the steering angle relatively close to the desired tra-

jectory, then, the vehicle speed control can be used to track the desired trajectory.

For this MPC controller, ﬁrst, the vehicle dynamic state space model needs to be

derived. As discussed in Chapter 2, the vehicle longitudinal dynamics [22] with the

traction and brake force as input can be represented as,

mp¨(t) + αp˙(t) = Ft − Fb (5.1.1)

where α is the road friction coeﬃcient and Ft, Fb are the vehicle traction and brake force respectively. p(t) is the current vehicle’s position. In order to design a MPC controller, ﬁrst, the vehicle dynamics need to be put into state-space representation

91 and then discretize the system. By deﬁne the state vector

x = [p, p˙]T (5.1.2)

Vehicle longitudinal dynamics can be represented into state-space form as

x˙(t) = Ax(t) + Bu(t) (5.1.3)

0 1 0 A = α & B = 1 (5.1.4) 0 − m m

and u(t) = Ft − Fb is the overall force acted on the vehicle. Then, Euler approxi-

mation can be applied to discretize the system with sampling time ∆T yields,

x(k + 1) = Adx(k) + Bdu(k) (5.1.5)

1 ∆T 0 Ad = α∆T & Bd = ∆T (5.1.6) 0 1 − m m

5.1.2 Controller Synthesis

In order to predict the system states, all the future states within the horizon should be updated from current state as introduced in [47]. Deﬁning Nc as the

control horizon which is the number of parameters used for future control trajectory

and Np as the number of samples that the future state variables are predicted. Then, the predicted states can be written iteratively as,

x(ki + 1)|ki) = Ax(ki) + Bu(ki)

2 x(ki + 2)|ki) = Ax(ki + 1|ki) + Bu(ki + 1) = A x(ki) + ABu(ki) + Bu(ki + 1) . .

Np Np−1 Np−Nc x(ki + Np|ki) = A x(ki) + A Bu(ki) + ··· + A Bu(ki + Nc − 1) (5.1.7)

92 Deﬁne a state vector X as

T X = x(ki + 1)|ki) x(ki + 2)|ki) ··· x(ki + Np|ki) (5.1.8) and input U as

U = u(ki) u(ki + 1) ··· u(ki + Nc − 1) (5.1.9)

The, al predicted states are organized in to the easy form to be represented by the current state x(ki) and future control input U as

X = F x(ki) + ΦU(ki) (5.1.10)

 B 0 ··· 0   A  2  AB B ··· 0   A   2  F =  .  & Φ =  A B AB ··· 0  (5.1.11)  .   . . .     . . .  ANp   ANp−1BANp−2B ··· ANp−Nc B

5.1.3 Cost Function

The key idea of the MPC controller is to solve a constrained optimization problem at every discrete time instance to ﬁnd the current optimal control input to the system.

The cost function used here is deﬁned as [35],

Np−1 X T J(k) = [r(k + i + 1) − y(k + i + 1)] Q [r(k + i + 1) − y(k + i + 1)] (5.1.12) i=0 T T + ∆u(k + i) R∆u∆u(k + i) + u(k + i) Ruu(k + i) (5.1.13)

where k is the current step and Np is the prediction horizon. r is the reference desired signal and in this problem, it’s the desired longitudinal position derived from the trajectory generation module. And Q, R∆u, Ru are weight of the tracking error,

93 control input changing rate and the control input of the cost function respectively.

Constraints of the optimization are

umin ≤u(k) ≤ umax (5.1.14) ∆umin ≤∆u(k) ≤ ∆umax where umax, umin, ∆umax and ∆umin are the maximum and minimum values of the input signal as well as the input signal incremental values respectively. Besides the input signal, vehicle’s longitudinal velocity should also be bounded as

vmin ≤ p˙(k) ≤ vmax (5.1.15)

where vmin and vmax are the minimum and maximum velocity for the vehicle.

5.2 Vehicle Lane Change Control Simulation

The case of vehicle tracking the desired lane change trajectory is simulated with a steering angle predeﬁned and given that is not driving on the desired trajectory as showed in 5.1. Constraints of the system are

−10000 ≤F ≤ 10000

−3000 ≤ ∆F ≤ 3000 (5.2.1)

0 ≤p˙ ≤ 40

Initially, driver is turning at a bigger steering angle than the desired trajectory. Since lateral control is by driver, in order to control the vehicle’s velocity, a desired longitudinal position is needed for the controller to check. Thus, from the trajectory generation module, a look-up table can be formulated as the lateral position of the lane change trajectory versus the longitudinal position. Based on the current lateral position, a desired longitudinal position from the desired lane change trajectory

94 can be achieved as the desired position to track. This simulation is done in Matlab

Simulink using the Model Predictive Control Toolbox.

Figure 5.1: Reference Trajectory versus Driver Trajectory

Figure 5.2 shows the controlled trajectory output from the simulation. With this

MPC controller, it can be seen that if the driver’s steering angle is not too far away from the desired trajectory, by changing the longitudinal speed, vehicle’s lane change trajectory can be controlled to the reference trajectory designed by the trajectory generation module.

95 Figure 5.2: Controlled Trajectory Comparion

96 Chapter 6: Conclusion and Future Work

6.1 Conclusion

In this project, a fundamental framework of the vehicle highway lane change scenario with automated longitudinal control has been built. Four different modules are designed and tested. With this system, if no surrounding vehicles, then of course a standard cruise control is used to control the vehicle at the desired speed set by the driver. And when a slower preceding vehicle occurs, system will automatically start to use the vehicle following control module to control the vehicle following the preceding vehicle at a desired constant time-gap distance. This following control system includes three different controllers and each controller was tested and simulated. The system is able to detect the preceding vehicle and apply the transitional controller to first drive the vehicle to the same velocity as the preceding vehicle and apply the following controller to control the vehicle at the desired space away from the preceding vehicle.

Meanwhile, the lane change module of the system will start to check the feasibility of a lane change and to see if a lane change trajectory is possible in order for the driver to keep driving at the desired speed. A Finite State Machine (FSM) is designed to include all the possible situations of highway two lane with one slower driving preceding vehicle and one surrounding vehicle with acceleration equals zero. There

97 are two key parameters during the lane change trajectory generation, one is T the total time it takes to ﬁnish the lane change maneuver and the other one is d the overall distance traveled during lane change. Also, the ﬁnal longitudinal velocity of the lane change maneuver should be informed from the maneuver generation module.

Thus, based on the prediction of surrounding vehicle’s future position, the maneuver generation module decides what kind maneuver should be conducted and output the three key parameters to the trajectory generation module: T , d and vf . Both T and

d are decided based on two collision indicators TTC & TIV to ensure safe beginning

distance and ﬁnal distance.

With the information passed to the lane change trajectory, two cases were con-

sidered. One is the case with no surrounding vehicles and optimized T and d were

calculated to have a minimum total kinetic energy lane change trajectory. When

there are obstacles i.e. surrounding vehicles, then based on the collision detector,

a collision avoidance trajectory is designed and simulated in the system. With the

desired trajectory is generated, a MPC controller is applied to control the vehicle

on the desired lane change trajectory. And simulation results were discussed in the

thesis.

6.2 Future Work

The next testing stage may involve real life testing of scenarios where the driver

and the vehicle share control such as the driver is responsible for the steering operation

while the longitudinal control is fully automated. Also, more complicated scenarios

can be considered. In this project, it’s assumed that the surrounding vehicle are

driving at a constant velocity and acceleration cases can be studied and discussed.

98 Also, the overall system integration should be conducted. Currently, each module need to be activated manually and parameters need to be entered instead of receiving from other module directly. The integration is deﬁnitely needed for real time testing and implementation. Matlab code needs to be transferred to vehicle ﬁrmware C code for testing. More accurate vehicle lateral dynamic considering varying velocity is also one possibility to increase the control performance. The lateral dynamic used to simulate driver behavior is now a simple point mass module and it is not giving good response when the longitudinal velocity is varying.

In terms of lane change trajectory generation, better method can be studied such as overall optimization process considering lane change control as well as lane change trajectory because the method used in this project generates a trajectory with as- sociated velocity and acceleration itself and they might not be optimized during the process. At the end, the system should be integrated together and tested in vehicle on road with real life traﬃc movements. System should be able to trajectory optimized lane change trajectory with optimal control for the vehicle to drive at the desired velocity as close as possible.

99 Bibliography

[1] Daniel Axehill and Johan Sj¨oberg. Adaptive cruise control for heavy vehicles: Hybrid control and mpc. 2003.

[2] Jose L Bascunana. Analysis of lane change crash avoidance. Technical report, SAE Technical Paper, 1995.

[3] Dimitri P Bertsekas. Nonlinear programming. Athena scientiﬁc Belmont, 1999.

[4] Wonshik Chee and Masayoshi Tomizuka. Lane change maneuver of automo- biles for the intelligent vehicle and highway system (ivhs). In American Control Conference, 1994, volume 3, pages 3586–3587. IEEE, 1994.

[5] Swaroop Darbha and KR Rajagopal. Intelligent cruise control systems and traﬃc ﬂow stability. Transportation Research Part C: Emerging Technologies, 7(6):329– 352, 1999.

[6] Peter G Gipps. A model for the structure of lane-changing decisions. Trans- portation Research Part B: Methodological, 20(5):403–414, 1986.

[7] Sebastien Glaser, Benoit Vanholme, Sa¨ıdMammar, Dominique Gruyer, and Ly- die Nouveliere. Maneuver-based trajectory planning for highly autonomous vehicles on real road with traﬃc and driver interaction. Intelligent Transportation Systems, IEEE Transactions on, 11(3):589–606, 2010.

[8] Datta N Godbole, Veit Hagenmeyer, Raja Sengupta, and D Swaroop. Design of emergency manoeuvres for automated highway system: obstacle avoidance problem. In Decision and Control, 1997., Proceedings of the 36th IEEE Conference on, volume 5, pages 4774–4779. IEEE, 1997.

[9] Cem Hatipoglu, Umit Ozguner, and Keith A Redmill. Automated lane change controller design. Intelligent Transportation Systems, IEEE Transactions on, 4(1):13–22, 2003.

[10] John C Hayward. Near-miss determination through use of a scale of danger. Highway Research Record, (384), 1972.

100 [11] JK Hedrick, D McMahon, V Narendran, and D Swaroop. Longitudinal vehicle controller design for ivhs systems. In American Control Conference, 1991, pages 3107–3112. IEEE, 1991.

[12] Roberto Horowitz, Chin-Woo Tan, and Xiaotian Sun. An eﬃcient lane change maneuver for platoons of vehicles in an automated highway system. California Partners for Advanced Transit and Highways (PATH), 2004.

[13] P Ioannou et al. Personalized driver/vehicle lane change models for adas. 2013.

[14] Hossein Jula, Elias B Kosmatopoulos, and Petros A Ioannou. Collision avoidance analysis for lane changing and merging. Vehicular Technology, IEEE Transac- tions on, 49(6):2295–2308, 2000.

[15] Alexander Kanaris, Elias B Kosmatopoulos, and Petros A Loannou. Strategies and spacing requirements for lane changing and merging in automated highway systems. Vehicular Technology, IEEE Transactions on, 50(6):1568–1581, 2001.

[16] Arne Kesting, Martin Treiber, and Dirk Helbing. General lane-changing model mobil for car-following models. Transportation Research Record: Journal of the Transportation Research Board, 1999(1):86–94, 2007.

[17] Dongwook Kim, Seungwuk Moon, Jaemann Park, H Jin Kim, and Kyongsu Yi. Design of an adaptive cruise control/collision avoidance with lane change support for vehicle autonomous driving. In ICCAS-SICE, 2009, pages 2938–2943. IEEE, 2009.

[18] Kwangsoo Kim, Dong-il Cho, and Juraj V Medanic. Lane assignment using a genetic algorithm in the automated highway systems. In Intelligent Transportation Systems, 2005. Proceedings. 2005 IEEE, pages 540–545. IEEE, 2005.

[19] Wouter Klein Wolterink, Geert Heijenk, and Georgios Karagiannis. Automated merging in a cooperative adaptive cruise control (cacc) system. 2011.

[20] S Kumaragovindhan and Rajesh Rajamani. On spacing policies for adaptive cruise control. In Proceeding of the American Controls Conference, Chicago, Illinois, 2000.

[21] Shengbo Li, Keqiang Li, Rajesh Rajamani, and Jianqiang Wang. Model predictive multi-objective vehicular adaptive cruise control. Control Systems Technol- ogy, IEEE Transactions on, 19(3):556–566, 2011.

[22] Peng Liu, Arda Kurt, and Umit Ozguner. Trajectory prediction of a lane changing vehicle based on driver behavior estimation and classiﬁcation. In Intelligent Transportation Systems (ITSC), 2014 IEEE 17th International Conference on, pages 942–947. IEEE, 2014.

101 [23] Sara Moridpour, Majid Sarvi, and Geoﬀ Rose. Lane changing models: a critical review. Transportation letters, 2(3):157–173, 2010.

[24] Jos´eEugenio Naranjo, Carlos Gonzalez, Ricardo Garcia, and Teresa de Pedro. Lane-change fuzzy control in autonomous vehicles for the overtaking maneuver. Intelligent Transportation Systems, IEEE Transactions on, 9(3):438–450, 2008.

[25] Julia Nilsson, Mohammad Ali, Paolo Falcone, and Jonas Sj¨oberg. Predictive manoeuvre generation for automated driving. In 16th International IEEE Annual Conference on Intelligent Transportation Systems, 2013.

[26] Julia Nilsson and J Sjoberg. Strategic decision making for automated driving on two-lane, one way roads using model predictive control. In Intelligent Vehicles Symposium (IV), 2013 IEEE, pages 1253–1258. IEEE, 2013.

[27] Steven A Nobe and Fei-Yue Wang. An overview of recent developments in automated lateral and longitudinal vehicle controls. In Systems, Man, and Cybernet- ics, 2001 IEEE International Conference on, volume 5, pages 3447–3452. IEEE, 2001.

[28] Umit Ozguner, Tankut Acarman, and Keith Alan Redmill. Autonomous ground vehicles. Artech House, 2011.

[29] Iakovos Papadimitriou and Masayoshi Tomizuka. Fast lane changing computa- tions using polynomials. 1:48–53, 2003.

[30] Evangelos Papadopoulos, Ioannis Poulakakis, and Iakovos Papadimitriou. On path planning and obstacle avoidance for nonholonomic platforms with manip- ulators: A polynomial approach. the International Journal of Robotics research, 21(4):367–383, 2002.

[31] Jaeyong Park. Safe controller design for intelligent transportation system appli- cations using reachability analysis. Master’s thesis, The Ohio State University, 2013.

[32] Jaeyong Park and U Ozguner. Model based controller synthesis using reachability analysis that guarantees the safety of autonomous vehicles in a convoy. In Vehicular Electronics and Safety (ICVES), 2012 IEEE International Conference on, pages 134–139. IEEE, 2012.

[33] Rajesh Rajamani. Vehicle dynamics and control. Springer Science & Business Media, 2011.

102 [34] Rajesh Rajamani, Han-Shue Tan, Boon Kait Law, and Wei-Bin Zhang. Demon- stration of integrated longitudinal and lateral control for the operation of automated vehicles in platoons. Control Systems Technology, IEEE Transactions on, 8(4):695–708, 2000. [35] Feyyaz Emre Sancar, Baris Fidan, Jan P Huissoon, and Steven L Waslander. Mpc based collaborative adaptive cruise control with rear end collision avoidance. In Intelligent Vehicles Symposium Proceedings, 2014 IEEE, pages 516–521. IEEE, 2014. [36] Joerg Schneider, Andreas Wilde, and Karl Naab. Probabilistic approach for modeling and identifying driving situations. In Intelligent Vehicles Symposium, 2008 IEEE, pages 343–348. IEEE, 2008. [37] Robin Schubert. Evaluating the utility of driving: Toward automated decision making under uncertainty. Intelligent Transportation Systems, IEEE Transac- tions on, 13(1):354–364, 2012. [38] Robin Schubert, Karsten Schulze, and Gerd Wanielik. Situation assessment for automatic lane-change maneuvers. Intelligent Transportation Systems, IEEE Transactions on, 11(3):607–616, 2010. [39] Tzila Shamir. How should an autonomous vehicle overtake a slower moving vehicle: Design and analysis of an optimal trajectory. Automatic Control, IEEE Transactions on, 49(4):607–610, 2004. [40] Sayanan Sivaraman, Mohan M Trivedi, Mario Tippelhofer, and Trevor Shannon. Merge recommendations for driver assistance: A cross-modal, cost-sensitive approach. In Intelligent Vehicles Symposium (IV), 2013 IEEE, pages 411–416. IEEE, 2013. [41] Thomas Stanger and Luigi del Re. A model predictive cooperative adaptive cruise control approach. In American Control Conference (ACC), 2013, pages 1374–1379. IEEE, 2013. [42] Satish Sundar. Emergency lane-change maneuvers of autonomous vehicles. 1998. [43] MC Turan, AE Hartavi, and E Altug. Development of a rule based upper level control algorithm for a co-operative vehicle in automated highway system. In Vehicular Electronics and Safety (ICVES), 2012 IEEE International Conference on, pages 454–459. IEEE, 2012. [44] Atsuya Uno, Takeshi Sakaguchi, and Sadayuki Tsugawa. A merging control algorithm based on inter-vehicle communication. In Intelligent Transportation Systems, 1999. Proceedings. 1999 IEEE/IEEJ/JSAI International Conference on, pages 783–787. IEEE, 1999.

103 [45] Ardalan Vahidi and Azim Eskandarian. Research advances in intelligent collision avoidance and adaptive cruise control. Intelligent Transportation Systems, IEEE Transactions on, 4(3):143–153, 2003.

[46] Junmin Wang and Rajesh Rajamani. Should adaptive cruise-control systems be designed to maintain a constant time gap between vehicles? Vehicular Technol- ogy, IEEE Transactions on, 53(5):1480–1490, 2004.

[47] Liuping Wang. Model predictive control system design and implementation using MATLAB R . Springer Science & Business Media, 2009.

[48] Guoqing Xu, Li Liu, Yongsheng Ou, and Zhangjun Song. Dynamic modeling of driver control strategy of lane-change behavior and trajectory planning for collision prediction. Intelligent Transportation Systems, IEEE Transactions on, 13(3):1138–1155, 2012.

104