Sammanfattning Att få

(1)

INOM

EXAMENSARBETE MASKINTEKNIK, AVANCERAD NIVÅ, 30 HP

STOCKHOLM SVERIGE 2019,

Investigation of Polynomial

Trajectory Planner for Emergency Maneuver

LUDVIG BJÄRKEBACK

(2)

KTH Industriell teknik och management

Godkänt

2019-04-15

Examinator

Lei Feng

Uppdragsgivare

Examensarbete TRIT A-ITM-EX 2019: 128

Undersökning av Polynomial Trajectory Planner För nödmanövrering

Ludvig Bjärkeback

Handledare

Xinhai Zhang

Kontaktperson

AVL Södertälje lman Delshad

Sammanfattning

Att få en bil att köra autonomt är en komplex uppgift som är genomförd i steg. Idag finns det många halvautonoma körassistanssystem i bilar som till exempel Adaptive Cruise Control och Lane Keeping Aid vilket förbättrar komfort och säkerhet för föraren. Vissa system bromsar bilen ifall en olycka är nära, men vad detta examensarbete kommer att undersöka är lösningar för att göra en nödmanöver ifall ett statiskt hinder framför bilen blir sent upptäckt av sensorerna.

Ifall ett hinder upptäcks sent så kommer en nödrutin att aktiveras. Planeraren som är vald är en polynombaserad planerare i ett Frenet koordinatsystem. Planeraren söker igenom ett

tillståndsrum för position i lateral och longitudinell riktning. Algoritmen kommer att skapa ett 4:e eller 5:e ordningens polynom mellan nuvarande position och önskad slutposition. En Pure Pursuit regulator är implementerad för att säkerställa att fordonet följer rutten. Rutten är vald med hjälp av en kostnadsfunktion. Kostnadsfunktionen kommat att bedöma olika faktorer i rutten och ge dem en kostnad.

Målet med detta examensarbete är att undersöka hur sensorräckvidd, planeringsfrekvens och ruttäthet av planeraren påverkar prestanda när det gäller att undvika hinder. Prestandan är bedömd av hur stor andel av 50 försök som inte resulterar i kollision. Andelen framgångsrika försök beror linjärt på räckvidden, upp till 23 meter och därefter blir algoritmen kollisionsfri.

Planeringsfrekvensen hade också ett linjärt samband till förmågan att undvika hinder. Den sista faktorn som undersöktes var ruttätheten, den hade liten inverkan vid hög täthet. När rutten har låg täthet så hade ändringar i tätheten en stor inverkan. Ett problem som uppkom var att simuleringsprogrammet tillät för höga accelerationer, vilket leder till att delar av resultatet får sämre validitet. Sensorräckvidden där en manöver kan utföras säkert är 25 meter i detta experiment.

(3)

KTH Industrial Engineering and Management

Approved

2019-04-15

Abstract

Examiner

Lei Feng

Commissioner

Master of Science TRITA-ITM-EX 2019:128

Investigation of Polynomial Trajectory Planner for Emergency Maneuver

Ludvig Bjarkeback

Supervisor

Xinhai Zhang

Contact person

AVL Södertalje Iman Delshad

Getting an autonomous car to work is a complex task, and it is developed in steps. Today there are many semi-autonomous Advanced Driver Aid Systems in cars such as Adaptive Cruise Control and Lane Keeping Aid to improve comfort for the driver. Some systems will help brake a vehicle in case an accident is imminent, but this thesis will attempt to investigate solutions for doing emergency evading maneuvers when a static obstacle is late detected ahead of the car.

When an obstacle is late detected an emergency routine will be triggered. The planner chosen is a polynomial based planner in a Frenet coordinate system. The planner is searching a state space for position in lateral and longitudinal direction. The algorithm will find qupintic and quartic polynomials between current and end positions. A Pure Pursuit path tracker will make the car follow these curves. The trajectory chosen is selected using a cost function. The cost function will assess different factors of the trajectories and give them a cost to compare.

The goal of the thesis is to investigate how sensor range, replan rate and trajectory density in the trajectory planner affect the performance in terms of obstacle avoidance. The performance is judged by the portion of 50 trials that do not result in a collision. It was found that detection

distance has a linear relation to success rate up to a range of 23 meters, after that it is basically collision free. The replan rate also has a proportional relation to success rate, until a point where the performance is capped. The last factor investigated was the trajectory density. It has little effect between the highest densities, but when it is more sparse will have a significant impact.

An issue that was encountered was that accelerations were excessively large in the simulator.

This causes the reliability of some maneuvers to be uncertain and reduced the legitimacy of the results. The sensor range where a maneuver can be made with reasonable accelerations is 25 meters.

(4)

Acknowledgements

I would like to thank my supervisor Xinhai Zhang from KTH for guiding me through the process of writing a thesis. Iman Delshad, my supervisor from AVL, for supporting me with ideas and feedback throughout the project and giving me the opportunity to complete it in cooperation with a company. At KTH Lars Svensson has been of great help with his expertise in trajectory planning and always answering all questions in good depth. Without Jos´e S´anchez and Naveen Mohan at KTH I would never have gotten PreScan up and running.

support with both technical aspects and understanding of the tool have been very valuable. I would also like to thank Emma Thilen from AVL for helping me get started in ROS and giving feedback on the progress of the project.

(5)

List of Figures

1 Decision making architecture for autonomous vehicles . . . 5 2 Image showing how the local Fren´et coordinate system describes

the position of a car when deviating from the desired path. Source:

[1]. . . 8 3 Bicycle model representation of a car. Source: [2] . . . 10 4 Image coordinate systems and collision detection box . . . 11 5 Picture of scenario where ego car (black) is making an evasive

maneuver for the stationary obstacle (green). . . 13 6 Diagram describing the architecture of the setup. . . 14 7 Lateral state space explored. Gray is impossible trajectories,

black are possible and green is optimal trajectory. Source: [1]. . . 18 8 Flowchart of the longitudinal controller. a,b,c are PI components

and e is variable used to stop the car in the initialisation phase. . 21 9 Simplified flowchart of how a Cascade controller could be imple-

mented. . . 22 10 Plot of how the ROS implementation views its data. Black crosses

are obstacles. Cyan triangle is ego position, red is trajectory and blue line is desired path. Axles are global coordinates. . . 23 11 Graph showing how sensor range affect success rate. Made from

60 trials at each point . . . 25 12 Graph showing how sensor range affect max acceleration. Made

from 50 trials at each point . . . 25 13 Graph showing how replan rate affect success rate. Made from

around 50 trials at each point . . . 27 14 Graph showing how replan rate affect the average acceleration of

successes. Made from 50-100 trials at each point . . . 27 15 Graph showing how trajectory density affect max success rate.

Made from 50 trials at each point. . . 28 16 Graph showing how trajectory density affect max acceleration.

Made from 50 trials at each point. . . 29 17 Graph showing the slip angle of the front wheels. Made from one

successful trial with 17m sensor range and 0.2s replan rate and density. . . 32 18 Image displaying worst and best case reaction time for a period

of 0.25 seconds. . . 34

(8)

Acronym List

ACC Adaptive Cruise Control ROS Robot Operating System

ADAS Automated Driver Assistance System LiDAR Light Detection and Ranging OS Operating System

PI Proportional-Integral g Acceleration due to gravity

RRT Rapid-Exploring Random Trees A* A-star (Planning algorithm) MPC Model Predictive Control

(9)

1 Introduction

This thesis is focusing on trajectory planning, but this section is explaining where the problem comes from and how it will be investigated.

1.1 Background

Getting a car to drive autonomously is a complex task, including the development of complex functionalities such as perception and path planning. Reliable sensors need to be selected and their data need to be processed into useful information. Path planning, the topic of the thesis, includes tasks from planning a route through the street network to navigating an intersection. Precise control and actuation to throttle and steering in order to follow the desired route are also necessary. Since programming is inherently deterministic, the hardest part about an autonomous cars is dealing with situations the developers had not quite planned for. Emergency routines to deal with new situations must be in place and tools to deal with new road scenarios need to be available and reliable.

The first automation feature readily available in cars was the cruise control, a longitudinal speed controller. These aids have developed today, some cars have more advanced tools such as Lane Keeping Aid and Adaptive Cruise Con- trol. Some manufacturers even provide a an autopilot as primary driver. These systems work in specified environments such as highways, so there are of course limitations.

One scenario which is dangerous is a cut-out scenario. This is when a car ahead in the lane suddenly changes lane to reveal an obstacle right in front of the ego vehicle. The circumstances is similar to what will be experienced when having a late detection of an obstacle. Another example is an issue that has arisen from radar limitations causing some cars to have problem detecting stationary objects such as a fire truck at work on a highway. This perception limitation has led to multiple accidents with sometimes fatal results[3]. This thesis will not work to solve the detection issue, but rather will examine actions to take in the case of a late detection of a static obstacle.

When traveling in cars, there are complex things a human driver can achieve such as perception and problem solving in unexpected scenarios. A significant problem with human drivers however, is the response time to react to a new situation. At all speeds, the distance traveled while the driver is reacting to an obstacle in roughly the same as the actual stopping distance. For example at 70km/h the reaction distance is 29m and the brake distance is 27m, causing the total stopping distance to travel after an obstacle is observed to be 56m [4].

A machine will still have some processing time between obstacle detection and actuating. The computation cycle is a lot quicker which is a contributing factor for autonomous systems to potentially have a higher safety than human drivers.

(10)

1.2 Problem Statement

When faced with the situation of the ego vehicle approaching a static obstacle at speed, it is important that the configuration of the emergency maneuver is optimal to minimize chance of collision. A polynomial planner is chosen to plan the maneuver. It is a method that fits polynomial functions to the initial and final conditions of a trajectory to calculate a continuous derivable trajectory in between. To reduce the scope of the thesis only a polynomial planner will be investigated, but other planners will be mentioned in Section 2.2. In a more focused wording it can be phrased as the following research question:

How will the configurations of a polynomial based planner affect the performance (in terms of evasion) in the scenario of a delayed obstacle detection?

The polynomial planner has multiple settings that can be investigated. The frequency the planner will run at is of great interest. This frequency has two different aspects, the trajectory density which is density of the data points in the trajectory and the replan rate which is how often a new trajectory is planned. The two aspects will be investigated separately. The distance to the obstacle at detection time will also be investigated, essentially how long margin the trajectory planner has to maneuver.

In this scenario where the algorithm will be investigated, geometry of the environment and such will be completely known except for the position of obstacle to be avoided. The environment will be constructed and tests will run in PreS- can. PreScan is a physics based simulation tool for ADAS development with environment simulation, vehicle dynamics and sensor models [5]. The scenario explored is meant to mimic a situation where a late detection has occurred. The implementation of the scenario is done by the ego car driving the road with a sensor that has limited range. The algorithm’s task is to do an evasive maneuver to avoid this obstacle as soon as the limited sensor detects it. In order to reduce sources of uncertainty true sensors will be used in the simulator, giving noise free and guaranteed correct data to the algorithms.

1.3 Research Design

The goal of this thesis is to investigate a polynomial based planner to determine how different parameters influence its effectiveness in planning evasive emergency maneuvers. In order to produce reliable results the primary means of exploration and experimentation is simulations in a tool called PreScan. Doing an implementation towards simulation will allow for some simplifications, for example a true sensor value will be fed to the controller and planner directly, reducing the need for sensor processing and interpretation. In order to make the simulation less deterministic some randomness will be added to the obstacle and thus a data set of success rate can be acquired. The randomness includes X

(11)

and Y coodrinate of the obstacle and initialisation time. The car decision making and virtual actuation output will be running on an Ubuntu computer and the simulator with the car dynamics and environment on a different Windows computer. Having the two programs on the same computer would simplify implementation, but the computation workload is too large for one PC. Therefore, executing the two programs on different PCs can greatly decrease the simulation time.

The algorithm is implemented in ROS and interfaced to PreScan using the MATLAB ROS toolbox. This setup was used because PreScan is very closely linked to MATLAB Simulink, and using their toolbox made for simple implementation and it is functioning well enough. The trajectory planning algorithm is based on [1]. For the implementation, [6] will be the bases and modification will be done to run on ROS, interface with the simulator and function in the scenario.

The sensor configuration of the car will be just an ideal point cloud sensor.

This would be similar to a rotating LiDAR in reality, but there will be no focus on sensing in this thesis so an ideal, noiseless sensor is used. Implementation in a real car will need the redundancy of multiple sensors, but this is unnecessary when using an ideal sensor. A Car dynamics model was chosen from the PreScan library. An old Audi A8 Sedan was selected because it is rather ordinary, it has an average size and performance.

In order to assess the results, the success rate of the algorithm is calculated.

The success rate is achieved from letting the algorithm run multiple times in the same scenario with just some random difficulty variation in the form of varying obstacle position, and look at the percentage of collisions avoided. The random difficulty between trials will ensure no trials are identical. The randomness will also give a better spread in the result so that there will not be a point when it is impossible, but rather the success rate will approach a value over several data points. If there are enough trials, the average difficulty will be same for all setting configurations.

This thesis is completed in cooperation with the company AVL MTC in S¨odert¨alje, Stockholm. With their focus on the automotive industry, any investigation in autonomous driving is of course of interest.

1.4 Ethical Consideration

The thesis will only be carried out virtually, so the chance of any physical harm is non-existent. The main application of the findings of this thesis is autonomous driving. The Trolley Problem is an ethical thought experiment where the subject has two options. One option is to do nothing and let five people die, the other option is to act and that will ensure that one other person dies [7]. This problem is a scenario that this algorithm could encounter, i.e., it tries to avoid a collision but the decision may harm to a different group.

The algorithm is not sophisticated enough to do any decisions based on value

(12)

of objects to collide with. If a collision is unavoidable, it will not attempt to maneuver due to its low complexity in implementation.

(13)

2 State Of The Art

This Chapter is presenting the most relevant research results that the thesis is based upon.

2.1 Autonomous Vehicle Decision Making Hierarchy

According to the 2016 survey [2] the decision making of an autonomous vehicle can be divided into four layers as displayed in Figure 1. The top layer is the Route Planning layer where the global path planning is done. This is where road network and user defined destination are processed and a road legal route is computed. The second layer is the behavioural layer. This is where the traffic is observed and the ego behaviour is adapted accordingly. Example of actions is to stop when a red light is observed or decide to follow the car ahead in the lane in dense traffic. The layer below this is the motion planning layer, this is where dynamically and collision free trajectories are found and decided to be executed. The final layer is the vehicle control, this is where actuation is done in order to follow the trajectory decided by the higher layers.

This thesis will deal with components of multiple planning layers. The behaviour layer that is implemented is to determine when it is necessary to execute an evasive maneuver. The biggest portion of the thesis is the motion planning, which determines a collision free dynamically feasible path to avoid the obstacle.

There is also some implementation of control layer in the form of Pure Pursuit and a longitudinal PI controller.

Figure 1: Decision making architecture for autonomous vehicles

2.2 Trajectory Planning

Trajectory planning determines the motion of a robot between two positions [8].

Trajectory planning is part of all top three layers of decision making. Trajec- tory planning differs from path planning because trajectory has a time variable, meaning it specifies how quick or slow a path is executed [9]. Multiple algorithms, for example many of the basic versions such as A* and RRT will only do path planning as it is simpler. For robotics where speeds and accelerations are low this is often proficient. When traveling at high speed with high inertia such

Percieved agents, obstades Estimated pose and

Road network data and signage collision tree space Estimate of vehicle state

User defined Steering And

destination

Local Feelback ^Throttle Route Planning Behavioral layer Motion Planning

Control

(14)

as with a car, a planned trajectory is better than a separate speed controller that will not have full awareness of the scenario.

Trajectory planning is often divided into two different areas, the global trajectory planning and the local one. The global path planning deals with how to navigate on the infrastructure and road maps, it is the highest level and can be viewed as the Route planning of the diagram in Figure 1. Local planning deals with the next few seconds of exact planning, as well as maneuvers that do not appear on a road map, this contains aspects from behavioral layer and motion planning layer. Important aspects to solve is for example lane change or overtaking and a parking in an open parking lot.

Mentioned by Milan´es et al.[8] graph search, sampling based planners, interpolating curve planners and numerical methods are the four main categories for trajectory and path planners that are currently implemented in the industry and academia. They will be discussed in more detail in their respective section.

2.2.1 Graph Search

The basic idea of graph search is to traverse a state space to go between two points. The algorithm will visit some or all states in this space. The solution determined is more or less optimal depending on the algorithm chosen. Exam- ples are the basic Dijkstra’s algorithm and its more efficient and well known derivative A*. These algorithms are most suitable for global path planning, but there are derivatives methods suitable for multiple tasks.

2.2.2 Sampling Based Planners

These planners will often try to solve a high dimensional space. This could be vehicle path planning with many constraints such as acceleration and posture.

The algorithm could for example also be used for motion planning of a robot arm in 3 dimensions with multiple joints. The algorithms will take random samples in the state space, looking for connectivity inside it. Due to the randomness of the algorithm, optimality can not be guaranteed. The most famous sampling based algorithm is RRT, Rapid-Exploring Random Trees, and its derivative algorithms. These algorithms can be suitable for both global and local planning.

2.2.3 Interpolating Curve Planners

These algorithms will interpolate a set of way points to create a more smooth set of point to get benefits of trajectory continuity and dynamic environmen- tal constraints. Common methods to be used is Lines and Circles, Clothoid curves, Polynomial curves, Spline curves and B´ezier curves. Generally these methods have the advantage of low computation cost and is most suitable for local planning with focus on comfort, meaning acceleration and jerk are low.

The exception is Clothoid curves which are very effective for comfort but ex-

(15)

pensive to compute and is therefore hard to do in real time, though solutions have been provided in [10].

2.2.4 Numerical Methods

These methods aim to minimize a cost function based on different variables the user wants to optimise. The parameters could be variables such as speed, steering speed, acceleration, jerk etc. This method is quite computationally heavy as the cost function for every available trajectory must be computed at each time step. This is the method used in this thesis for determining the trajectory to execute. An advantage with this method is the viewpoint, simplifying ego-vehicle constraints, obstacles and road limitations to be taken into account easily [8].

2.3 Path Tracking

In order to ensure that the virtual car is following the path produced by the algorithm a path tracking algorithm is needed. Similar to path planning, there are multiple methods that can be found in the literature that can solve the problem. The main categories are kinematic models, linear and non-linear MPC which are explored in further depth in [2].

Pure Pursuit is an algorithm that is widely used because of its simple computation and good robustness [11]. Pure pursuit is a lateral path tracker and with the help of a longitudinal PI controller it allows for tracking the trajectory generated by the algorithm. What Pure Pursuit does is to calculate the curvature of the movement that will take the car from its current position to a position some distance ahead along the path. This distance ahead is called ”the lookahead distance” and it is the only parameter that can be tuned in the simple algorithm. The lookahead distance often consists of a constant component and a component varying with speed, giving a longer lookahead distance with higher speeds to improve stability.

A shorter lookahead distance will yield a faster response to sharp turns in a trajectory, but may produce more oscillations. A longer lookahead distance will be more stable but will ignore fine maneuvers in a trajectory. The lookahead distance was tuned manually, with careful consideration since the application has quite high speed but the maneuver is also fine.

A drawback with this method is that it allows for instant, discontinuous output to the steering wheel. The simulated dynamics model can actuate these discontinuous inputs. Since any trajectory planned is reasonably smooth due to the chosen method, the discontinuity is small and is not deemed to be an issue.

Since Pure Pursuit only determines the steering necessary to follow a path, a simple PI controller has been implemented which will attempt to match the trajectory speed and position.

(16)

Figure 2: Image showing how the local Fren´et coordinate system describes the position of a car when deviating from the desired path. Source: [1].

2.4 State Space Exploration

The idea of using Fren´et coordinates and the concept of the algorithm is explored in [1]. The state space of possible trajectories is expressed and explored in Fren´et coordinates. These are coordinates that follow the vehicle as it moves along.

The longitudinal direction is always in the direction in which the vehicle is facing and the lateral direction is perpendicular to this. The trajectories are conceived in this Frenet coordinate system, but global Cartesian coordinates are still used to for example to define way points and obstacles.

The global coordinate system can be thought of being stuck to the road. The local coordinate system is following the desired center line path as can be seen in Figure 2. The center line is the path that should be followed. The trajectory is the actual path taken, deviating from the center line because of dynamic constraints or obstacles. The position of a point in a trajectory is expressed in terms of s(t) and d(t) which are the Fren´et coordinates. s is how far along the center line the point is, and d is the perpendicular distance between the center line and the point.

In the algorithm implemented separate polynomials will be calculated for lateral and longitudinal movement. The state space is explored in three dimensions, one that affect both, one for the longitudinal and one for the lateral direction. The first dimension is the predicted time to execute the maneuver, this is necessary for both polynomials as they need this information about their final state. The lateral offset to the desired final position is the state space explored by the lateral polynomial. While it seems trivial that the vehicle should always be on the desired path, it might not always be possible due to obstacles, thus making this exploration important. The last dimension of exploration is the final velocity, this is the purpose of the longitudinal polynomial, thus determining the speed profile of the maneuver.

As Howard et al. mentioned [12], the control output can be explored in

-

/

tx //

V '/

/ / /

/ / / /

d(t')

, ~~--

--....-- .2: n : r ~[

/ x(s,dJ

e _s(t')

,'

r(s) \ cen"'' line

/ tra3ector)'

(17)

state space or in control space. A control space approach would mean exploring possible control input and assessing the resulting trajectory. A state space exploration means that final states will be generated and then a trajectory from initial to this final state is found. In this thesis the path will be generated with the polynomial planner. A state space exploration is done in the Fren´et coordi- nated in this thesis because it is easier to make sure the trajectory conform with the environment and this is central as this paper deals with avoiding obstacles.

2.5 Cost Function

The cost function is used to determine which of the dynamically possible and collision free trajectories that is executed. It is an equation that calculates the goodness of all feasible trajectories. The trajectory with the lowest cost is executed until it is time for replanning. The factors of the equation are proposed in [1] and based of their success it was used in this thesis as well. Weights were tuned to suite the scenario.

2.5.1 Longitudinal

The cost function for the longitudinal movement is

Cv = kjJt(s) + ktT + ks[ ˙sf− ˙st]² (1) where all the kj, kt, ksare adjustable weight factors. Jtis the longitudinal jerk, and the square is integrated over the longitudinal trajectory (s(t)) and s₁ is initial point of the trajectory.

J_t(s) = Z sf

s₁

...s (s)²ds (2)

The jerk is the derivative of acceleration, it is quite significant for human comfort. T is the time it takes to execute the trajectory and the final term of equation 1 is deviation in final velocity ( ˙sf) from the target velocity ( ˙st).

2.5.2 Lateral

The cost function for the lateral movement is

C_d= k_jJ_t(d) + k_tT + k_dd²_f (3) and it can be seen that the form is similar to the longitudinal. Differences are that the jerk is summed over the lateral movement(d(t)).

Jt(d) = Z d_f

d1

...d (d)²dd (4)

(18)

Figure 3: Bicycle model representation of a car. Source: [2]

The deviation that is penalized this time is final position (df) distance from desired lateral position which is always zero due to the nature of the coordinate system.

2.6 Vehicle Dynamics

The software routine has a significantly simplified dynamic model of the vehicle to reduce complexity of implementation and minimize computation. When doing the dynamic feasibility check of a trajectory candidate, the algorithm simply checks that acceleration is lower than a defined max, the same for curvature.

This is a very rough method of checking the dynamics, but there are two main points of justification. Firstly, it is computationally very cheap. Secondly, it does not need to be very precise. The cost function will likely pick the function far away from dynamic limits since they are more likely to be expensive. Once triggered the cost function will likely decide to execute a path far away from the extreme limits. The simulator has more detailed dynamics model to guarantee more reliable results.

In order to determine what steering angle is necessary to correct the car orientation, Pure Pursuit uses the bicycle model to predict the movement of the car. The bicycle model [2] is a popular model for the steering of the car due to its cheap computation cost and high accuracy. The model replaces each wheel pair with one wheel in the middle between them, producing the likes of a bicycle as can be seen in Figure 3.

The following differential equations can be derived for a point midway between the rear wheels of a car, represented by the rear wheel in the model. v_r is the net velocity of the car, and the other variables are defined as in Figure 3.

˙

xr= vrcos(θ) (5)

(19)

Figure 4: Image coordinate systems and collision detection box

˙

yr= vrsin(θ) (6)

θ =˙ v_r

l tan(δ) (7)

The simulator uses a more complex dynamic model to better mimic how a real system would respond to the software routine. This model is developed by the simulation software and simulates engine, gearbox, physics etc separately.

The movement is also based on the bicycle model. For the tyres there are some slip as well, preventing perfect movement but will give something more realistic instead. For this application an automatic gearbox was selected, so that only variable to enter is steering angle, throttle and break to see how that car reacts.

2.7 Collision Detection

The collision detection is made in an pessimistic manner that is simple to implement and cheap to compute. The car that has basically a rectangular shape is checked along each global axis to see if any obstacles are within its border.

To avoid extra calculation the car is assumed to always be oriented along the x-axis as can be seen in Figure 4. This is an acceptable assumption as in this scenario the car will only turn a maximum of 17 degrees, which was observed

d

Frenet coordinates

Coll ision box

y

.___ _ _ _ _.,, X Global cood inates

XXX X

obstacle

s

(20)

in the tests. In order to compensate for possible error, the algorithm collision detection checks with a 25% larger car. If a trajectory contains at least one point where an obstacle is within the rectangle of the car, the entire trajectory is disqualified.

PreScan has a different collision detection system that works with the actual size of the car models and yields perfect results. It is these collision triggers that are used to assess the effectiveness of a set of planner settings.

Collisions are not checked against any road barriers. This means a maneuver that sways so far out when evading an obstacle that it will go outside the road, is still counted as a success. This assumption is made because often there is a roadside margin preventing such maneuvers from resulting is collisions. It will also simplify the analysis of the results. No path will be planned to go into the region where a crash barrier would be placed, because the lateral state space explored is confined to be within the borders of the road. A situation where the car is outside the road could only occur in the event the path tracker struggles to keep up. Thus making the case where a barrier collision unlikely and the implementation no necessary.

2.8 Vehicle Simulation

Simulations have many advantages over physical testing. For example it is often cheaper to simulate since it for example allows for multiple destructive experiments without having to destroy assets. It is very good to allow testing

”what if” scenarios [13]. The simulation tool chosen is PreScan because it is user friendly with a strong interface to Simulink, allowing for interfacing with other software. Another strength is its Dynamics model of the vehicle which was used. Another option for simulation is the open source platform Autoware.

It has its own dynamics and control algorithms and uses Rviz for visualisation.

Autoware lacks good sensor models, but will still have the tools necessary to implement the theory introduced in this thesis.

When doing the simulation in PreScan some ideal information was used, this allowed for less uncertainties when investigating the algorithm in question. The position of the car, the orientation and the velocity was taken as true values from the simulator. The sensor used is a perfect point cloud sensor, it has no noise and will give the global coordinate of any object it detects in the predetermined point cloud grid.

(21)

3 Method Implementation

In this section it will be explained how the research question ”How will the configurations of a polynomial based planner affect the performance (in terms of evasion) in the scenario of a late detection?” will be answered. It will explore the algorithm implemented in ROS, that then were tested using PreScan simulation.

3.1 Scenario Description

The basic scenario is the car driving on the highway at 20m/s or 72km/h in the right lane. The algorithm will try to keep the position of the car in the middle of the lane, but an obstacle will be present in the lane. The stationary obstacle is present the entire time, but the car sensor has a certain limited range that will emulate the late detection of the obstacle. The late detection range investigated is between 16 and 30 meters, this can be compared to a LiDAR range of 300m for the best of automotive models [14]. The car will attempt to make a maneuver into the other lane to avoid the obstacle such as can be seen in Figure 5.

The algorithms and sensors will use two different coordinate systems. One global coordinate system which is fixed to the road, and is used for sensors and collision detection mentioned in section 2.7. The other coordinate system if fixed in the car and is used for the trajectory planner and state space exploration, explained in section 3.4.

Figure 5: Picture of scenario where ego car (black) is making an evasive maneuver for the stationary obstacle (green).

The conditions of the road will be good and the vision is good, and there will be no other traffic. In the simulation tool true sensors will be used to generate perfect data, so sensor uncertainties will not be needed to take into account.

The traction to the road is good, and except some slip it may allow too high accelerations as will be explored in the discussion, section 5.

(22)

Figure 6: Diagram describing the architecture of the setup.

3.2 Experiment Design

To investigate the scenario, The path planner runs in ROS and an environment simulation in PreScan. In ROS there will be two nodes running simultaneously.

A node is a task with its own code which can run in parallel. PreScan commu- nicates to the ROS nodes via Simulink which in turn can access the ROS topics.

ROS topics are communication channels in the ROS network. It is in PreScan the environment is simulated, it is also where the car dynamics and actuation is happening. Ideal sensors are used and true position of the ego vehicle in the global coordinate system is sent from PreScan to ROS with Simulink as a middle wear. The architecture of the setup can be seen in Figure 6. Simulations will be run multiple times with slight random variations. The percentage of successful instances will be the used as the result.

There are three aspects of the selected polynomial planner that will be investigated:

• Detection range. How far away the obstacle can be detected.

• Replan rate. How often a new trajectory is generated

• Trajectory density. How close the waypoints in a trajectory is.

These concepts will be explored further in this chapter. In order to generate a data set of unique situations, the obstacle position is a random component.

It is positioned in lateral direction ±1m of being straight ahead of the ego car and between 100 and 105 meters away from starting position. Due to the startup procedure and synchronization between the computers the ego speed at detection point will also vary a little, speeds between 18 and 21 m/s are accepted results. This will further adding some randomness to separate the trials. This will make sure no two trials have identical outcome which would otherwise be the case of a simulation.

Ubuntu Computer Windows Computer

/ ' /

'

ROS Simulink PreScan

----

Sensors & Position

I

Trajectory Planner Environment

l

1

Path to follow Interface l.i!----o

[ Vehicle Dynamics ]

Sleering, Throttle & Break

[::]

Control Routine with Pure Pursu~

'

(23)

The arrows that can be seen in Figure 6 are ROS topics for the ones that go to and from ROS, and PreScans own interface between it and Simulink. The sensor information sent is from a single point cloud sensor, where all points are sent to be processed in the ROS node. The car position sent is in global coordinates. Each point, in global coordinates, of the trajectory to follow is sent between the nodes. The steering, breaking and throttle is sent simply as one value each.

3.3 ROS Implementation

Robot Operating System (ROS) is used to run the trajectory planning and path tracker in a realistic environment. The code has multiple aspects with different purposes that will be discussed in further detail. The implementation is divided in two nodes. One that does the planning at a constant rate that will be investigated. The other node is the car actuation node that will run at a higher frequency of 20 Hz, same as the simulation tool.

Except the large number of variables initialized, several ROS topics need to be initialized. One topic is to receive the true information about position and velocity, one more is to receive the point cloud sensor data. Two topics for communication, between the nodes and to send vehicle actuation commands.

Furthermore the path to be followed is calculated based on any number of way points. The way points are connected using a spline, although for this experiment it is just a straight line of the simulated highway lane. Virtual obstacles for the car to avoid can also be added here. This implementation originated from [6].

3.4 Trajectory planning

The trajectory planning ROS node that can be seen in Figure 6 is the main part of the implementation and the aspect that is investigated. The theory and more practical aspects of the implementation will be explained here.

The implementation that is executed in every cycle of this ROS node can be seen in algorithm 1.

(24)

Algorithm 1: algorithm of every executed cycle of trajectory planner Result: ROS cycle to Generate optimal trajectory

1 receive obstacles from sensor;

2 generate 180 trajectories in state space;

3 for all trajectories do

4 if dynamically infeasible then

5 discard trajectory;

6 else if Collision then

7 discard trajectory;

8 else

9 calculate cost function ;

10 end

11 end

12 send lowest cost trajectory to path tracker;

13 update estimated position

3.4.1 Polynomial Trajectory

To create a trajectory between two different points, the method selected in this thesis is with a polynomial function. This is done 180 times in line 2 of algorithm 1. Depending on the number of end conditions that are known, the degree of this polynomial will vary. The implementation uses 4th order for longitudinal planning. The following explanation will be done with a 4th degree polynomial, and there is not much difference when doing derivations with a higher degree.

The form of the polynomial can be seen in equation 8.

x(t) = a₀+ a₁t + a₂t²+ a₃t³+ a₄t⁴ (8) We can derive this polynomial to receive additional equations to help solve for the unknown variables a0− a4. The endpoints of the polynomial will be known; we know where we are and we know where we want to go according to the state space algorithm discussed in section 3.4.2. By deriving at the endpoints the following equations can be acquired for solving the variables. Where t0 is initial time and t_f is time when trajectory is finished.

x(t₀) = a₀+ a₁t₀+ a₂t₀²+ a₃t³₀+ a₄t⁴₀ (9)

˙

x(t0) = a1+ 2a2t0+ 3a3t²₀+ 4a4t³₀ (10)

¨

x(t0) = 2a2+ 6a3t0+ 12a4t²₀ (11)

˙

x(tf) = a1+ 2a2tf+ 3a3t²_f+ 4a4t³_f (12)

¨

x(tf) = 2a2+ 6a3tf+ 12a4t²_f (13) These particular equations were chosen because these variables are explored

(25)

in the sate space, x(t_f) will be used in the quintic polynomial. These equations can all be assembled to a matrix that can be solved exactly to acquire all the coefficients to know the exact path according to equation 8. For the implementation it was solved using the linag.solve in the numpy python toolbox.







1 t0 t²₀ t³₀ t⁴₀ 0 1 2t0 3t²₀ 4t³₀ 0 0 2 6t₀ 12t²₀ 0 1 2tf 3t²_f 4t³_f 0 0 2 6tf 12t²_f











 a0

a1

a₂ a3

a4







=





 x(t0)

˙ x(t0)

¨ x(t₀)

˙ x(tf)

¨ x(tf)







(14)

3.4.2 Trajectory Generation

In order to determine the endpoints of the polynomials discussed in section 3.4.1, we need an algorithm to determine this systematically. The algorithm used will explore the state space mentioned in section 2.4, solve equation 14 and get a trajectory for each state space point. This is performed in line 2 in Algorithm 1. The state space exploration will cycle through a predetermined range of tf, this means different time length of maneuver executions. t_f varies between 2 and 4 seconds with interval of 0.2 seconds. It will cycle though different valued of x(tf) which is the final lateral position in the quintic polynomial. In the implementation the size of the road was used and resulted in x(tf) varying between 0 and 4.5 m, with increments of 0.25m. It is also possible to cycle though different ˙x(t_f) which is the longitudinal speed, but it was elected not to in order to save computation time.

The lateral state space grid can look such as in Figure 7. As it can be seen many trajectories are generated, and it is the job of the cost function in section 2.5 to determine which one is the best. For example the trajectory in Figure 7 marked a has a tf or 1 second and x(tf) of -2.2 m.

3.4.3 Cost Function

In order to determine which trajectory is optimal, and therefore selected, the algorithm will minimize a cost functions introduced in section 2.5. This is performed in line 9 of Algorithm 1. When doing this in this implementation the cost function is calculated for all feasible trajectories and compared to each other. The trajectory that is collision free and has the lowest cost function is then selected in line 12 of algorithm 1 to be executed. The polynomial planner generates a separate polynomial for the lateral and longitudinal movement, as it is a simple and efficient way to implement. The cost functions for lateral

(26)

Figure 7: Lateral state space explored. Gray is impossible trajectories, black are possible and green is optimal trajectory. Source: [1].

and longitudinal polynomials vary slightly. As previously introduced, the cost function for the longitudinal movement is equation 1 and for lateral movement is equation 3. This means that both equations will penalize a high jerk, long execution time and deviations from the desired position or speed.

In the implementation done in this thesis, a factor three to one weight was put in place to indicate a higher importance of the lateral cost function. Oth- erwise the weights were the same for lateral and longitudinal movement. The jerk has a low factor, kj, of 0.1. Due to implementation simplifications, the jerk is squared and will still heavily penalising very high jerks. The cost factor for time, kt, was 1 to give it an average impact. Finally the kd was given a high factor of 5, strongly encouraging the planner to choose a path that close to the desired path. This will force the car to make maneuvers close to the obstacle, but it will reduce strong steering input.

3.4.4 ROS Implementation of Trajectory Planner

The polynomial trajectory planner originates from [6] but has been modified to work with the scenario. It is designed after section 3.4.1 and 3.4.2 and works as such. The most significant modification that has been made is that the planner initially will plan a route from the location of the car received from PreScan, and after that it will predict the location of the next time step of the trajectory.

Additions to the code that has been made is also the obstacle detection that allows for detection of obstacles live as the car drives in the environment. New obstacles will of course force the trajectory planner to plan new routes.

The trajectory planner also has a very basic collision detection algorithm in 3

2

0

-1

-2

0 2 3 4 5 6 7 8 9 10

t/ ^s

(27)

order to disqualify bad trajectories. Initially the car was assumed to have the shape of two circles, and all obstacles were checked if they were within these circles. This was however too computationally expensive so the car is currently assumed to have the shape of a rectangle with the orientation along the global x-axis. This is a good solution as it is much quicker to check if a value is less than the car bounding box. It is a case specific solution since the car wont change orientation significantly in the scenario. In the general case an angle of the car would need to be included. The car is running at high speed along the x-axis so any maneuvers done will not significantly affect the orientation of the car, thus allowing for this orientation assumption to be made.

The planner was implemented such that it only uses the car position for the first iteration. Thereafter it will assume the car follows the path selected and therefore move one time step along the path and plan from there, with new obstacles and information taken into account. The reason for this implementation is that it provides a much more stable trajectory than if a path is planned from current information all the time. A slight unpredictable car movement will not be considered by the planner and not escalated.

A technical issue with planning from the current position is the reliability and accuracy of the communication between the simulator and control software.

It is difficult to implement perfect synchronisation to ensure the planner receive good, continuous information and it is outside the scope of the thesis. If one data point received is little off or one cycle old, it can result in the planner planning an unnecessary break or a false turn. The current setup is not reliable enough for the current implementation to use current position in every time step.

The trajectory planner is designed such that the final longitudinal speed of the trajectory is a parameter. This parameter was however set to 20 m/s at all times because it greatly reduced the number of calculated paths, reducing the calculation time allowing faster experiments to be performed. It was observed that trajectories involving breaking was not often favored, although this will depend completely on the choice of cost function. More importantly breaking is avoided because it will hinder the steering capabilities. A car wheel and only exert a certain amount of force with its friction, if some of it is used to break it will cripple the steering.

3.5 Path Tracker

The purpose of a path tracker is to ensure that the vehicle, whether it is virtual or real, will follow the path generated. The tracker will take the generated trajectory as input and output throttle, steering and breaking for the car to actuate. Pure Pursuit is used for lateral control, and will thus output the steering angle. A longitudinal PI controller will ensure the correct velocity is held, thus managing the throttle and break output.

(28)

3.5.1 Lateral Control

The pure pursuit algorithm introduced in Section 2.3 relies on two equations for implementation. The first one figures out the error angle θ_err in heading, to face the point on the trajectory the algorithm aims at.

θerr= tan⁻¹ t_y− y tx− x

− θ (15)

where tx, tyis the coordinates of the point in the trajectory the algorithm is aim- ing at. x, y, θ is the coordinates and current heading of ego vehicle respectively.

The steering wheel angel δ is acquired by equation

δ = tan⁻¹ 2 · l · sin(θerr) L_l

(16) where L is the axle length of the car and L_lis the lookahead distance. An image over the variables can be seen in Figure 3.

Pure Pursuit was implemented with the code [15] as a base, but then modified and tuned to suite the scenario. The algorithm runs on its own node at a constant 20Hz, same frequency as the simulator. The path this algorithm will attempt to follow, is the calculated path by the polynomial planner and not the ideal path that follows the center of the road lane. The lookahead distance discussed in Section 2.3 can be visualised as what point of the red trajectory in Figure 10 the algorithm will aim at. An advantage of using Pure Pursuit is its simplicity in implementation and cheap computation cost. One more main feature is that Pure Pursuit is very robust, however this is not a focus in this well defined scenario and has instead lead to problems with precision.

3.5.2 Longitudinal Control

The longitudinal control proved to be a tricky task as there are both a desired speed and a desired position when looking at a trajectory. The ego position used by the planner is deduced assuming the planned maneuver is done. The only time the actual position of the car is used is in the first instance, as mentioned in Chapter 3.4.4.

For the Pure Pursuit to yield a good result, it is important that the car is actually at the correct point of the trajectory at the appropriate speed. If the car is too far back, the lookahead distance will be too large and the car will take shortcuts to follow the path and possibly colliding with an obstacle. If the car is too far forward, the steering becomes unstable. It is also important that the car has the desired speed as it will significantly affect the maneuver. The implemented controller for longitudinal control is a PI. In Figure 8 a flowchart of the longitudinal controller can be seen.

The controller is configured in such a way that the main output will come from a saturated integral. This will give a similar effect to a constant controller

(29)

Figure 8: Flowchart of the longitudinal controller. a,b,c are PI components and e is variable used to stop the car in the initialisation phase.

with a constant throttle but with some control still. It was implemented in this way because the car would immediately fall behind the target. The issue was that the car initiated just behind the desired position, the controller is close to the desired position and will give a low throttle and result in the car falling behind the moving target of the trajectory. The integral would catch up but either too slow or with a too large overshoot. The constant aspect will ensure that the car will always get a sufficient throttle to prevented the car from falling behind the moving target.

The controller makes assumptions such as the target speed is constant and it is rather inflexible. It also has a term of comparing current speed to desired speed. This is a not proper way of implementing a PI controller and it was detrimental to the stability. Implementing a Cascade Controller with PID con- trollers is more complex, but would be a much more stable method of controlling both speed and position. An outer loop for the position control and an inner loop for speed control as in Figure 9. This would allow for more careful control of the speed to help keep it within the desired interval for the experiment. Ini- tiating behind the target could also be useful as it would be quicker to reach a steady state with zero error which is necessary for the pure pursuit to function properly.

The path tracker was set to run at a higher frequency than the trajectory planner. This is because the tracker rate will greatly affect the performance,

investigated frequency 2-10 H.z desired_speed, desired_x, desired_y From Trajectory planner

PosNow (valrable)

STOP(vanable)

PJOcontroller(function) 20Hz

a=2.3*(desired_speed -current speed)

b=0.4'"(x_dlesire-x_current+y_desire-y_currenl)

integrator=+b

e=-150 saturation

0<integrator<S c=0.03*integrator

sum

iff<1:ouf=2.7 else out=in

throttle

Recieve ground truth from] J

simulation: f<l< , - - - - ----1 __ _ A_c_tu_a_tio_n- ~1<1• - - - I Scaling and convert

current speed, j sign to brake

current_x~ourrent_y

(30)

Figure 9: Simplified flowchart of how a Cascade controller could be implemented.

so it is important that its constant and sufficiently high across all trials. This means that a seldom replanned trajectory will be followed quite far before it is planned anew. However if there is nothing unexpected happening during a planning cycle, the path will be followed safely. This will overall improve the viability of slow planners.

3.6 Visual Representation of ROS Data

Figure 10 is a visual representation of what the ROS algorithm sees and does.

The axis are in global coordinates, X and Y direction. The blue line is the desired path we want to follow, in this case it is just straight line. The black crosses are detected obstacles, an algorithm has been developed to take the sensor data and mark the edges of a large obstacle. The red line is the planned trajectory, this is replanned every cycle to ensure it is up to date with any obstacles that may appear. The circles on the trajectory is the discrete points where position, speed and acceleration are calculated.

Usually, the points are ”the distance that the vehicle travel in one cycle”

away from each other. If interpolation is used this does not have to be the case, so the density of these points will be one aspect that is going to be investigated in this report. The blue triangle is the current position of the ego vehicle fetched from the simulator. It is the job of the path tracker to ensure that the blue triangle is following the red trajectory.

3.7 Simulation

Simulink is used to interface the simulation tool PreScan to ROS. A lot of settings have been selected in the Simulation software. The speed selected to all the experiments is 20 m/s, equating to 72km/h. This speed is set as the initial speed of the car. Since the incident will occur some distance down the road, there will be some randomness in the speed of the incident. Accepted speeds to be added to there results are between 18 and 21 m/s.

The sensor used was an ideal point cloud sensor. Due to the specific scenario with a known environment, the number of points in the cloud was significantly reduced by only having one vertical level. This is done to achieve faster computation with no effect on performance. Only one sensor with an arc of 60 degrees

desired_x, des1red_y from trajectory

Po~ltlOrl PIO ccntroller

velOCltyPIO

controller Actuation

current_speed_x,current_speed_y

current_x, currenLy

Receive ground truth from simulation.

Currenl_speed, current_x, current_y

(31)

Figure 10: Plot of how the ROS implementation views its data. Black crosses are obstacles. Cyan triangle is ego position, red is trajectory and blue line is desired path. Axles are global coordinates.

was used, this is acceptable since we know the scenario so well. For example we know obstacles will only appear ahead of the car. In reality an implementation would need multiple sensors to cover the entire 360 degree arc, and more vertical density to see obstacles at different levels.

As mentioned the sensor used is an ideal one, meaning it will have no noise in the data and the information received is also given in global coordinates. This makes the current implementation of the collision detection simple and cheap to compute. Compared to a real implementation however, simplifications has been made and things such as sensor filtering, object clustering and coordinate transformation must be done.

A lot of the variables that are present in the scenario are left to default. The main ones are the road and weather conditions. The weather is dry, allowing for good traction between the tyre and road. The car dynamics model chosen is an Audi A8 Sedan, and all the variables are left to default, for example the mass is 1820kg and width is 2.04m.

No control is done in PreScan or its connection of Simulink. All that is implemented in the simulation software are tools to send and receive information via ROS topics, as well as test automation.

10.0

7.5

5.0

- - - -

2.5

~ - - - -

,-

^)I!(^X

0.0 _I><

-2.5

-5.0 -7.5

70 80 90 100 110 120

(32)

4 Results

This is a section presenting the results acquired from the experiments made based on Chapter 3.

The main way of getting a measurable result is the success rate. The success rate is the percentage of attempts with the same control parameters that do not result in a collision. The average acceleration of successful trials are also presented. In order to get a range of the results, the obstacle was moved in a range of 2 meters in lateral direction. The obstacle placement was uniformly randomly distributed. This means the difficulty of the trials will vary. A better parameter setup will be able to succeed with a more difficult obstacle position yielding a greater success rate. However, even poor settings might still succeed on easy obstacles. This setup will help getting a nice range of results and preventing sharp trends in the results.

4.1 Detection Range

As the intuition tells us, the detection range has a very significant impact on the success rate. Longer range will improve the chance of success.

All the tests were performed with a replan rate and trajectory density of 0.2 seconds or 5Hz. This is a rather high rate compared to the trials made in Section 4.2 where higher rates are limited by computation and performance.

The results are achieved by running 60 trials for each sensor range, which should be large enough to give a good picture of the average behaviour. As can be seen in Figure 11 the success rate is consistently increasing between 16 and 23 meters. At 23 meters the maneuver is always successful for avoiding collision.

It can be seen that a few of the data points above 23m are at 95% success rate, this is likely due to an unlucky timing together with a hard obstacle. A hard obstacle is when the obstacles is at a place such that a large evasive maneuver is necessary. An easy obstacle requires just a small change in trajectory. Another explanation can also be error in the implementation and communication, causing some lag.

Something very important to note is that the lateral accelerations that the car is experiencing are very high. It is generally agreed that the traction between the wheel and road can produce no more than 1 g of lateral acceleration as its difficult to attain a friction coefficient larger than 1. As can be seen in Figure 12, the average accelerations of successful trials is higher than 9.8 m/s²for the majority of sensor ranges. This doesn’t mean the trajectories are impossible, but it’s unlikely they will go quite as planned and the outcome is then uncertain.

It can thus be concluded that it is first at 25 meters the maneuver can be guaranteed made safely in good conditions.

This investigation was done first, and based on this 20 meter sensor range was used for the frequency and density tests. The reason this was chosen was that the success rate was quite average, meaning there is space for both improvement

(33)

Figure 11: Graph showing how sensor range affect success rate. Made from 60 trials at each point

Figure 12: Graph showing how sensor range affect max acceleration. Made from 50 trials at each point

100

90

~ 80

QJ

~

_<J) 70

QJ u u :, (/)

60

50

40

16 18 20 22 24 26 28 30

Sensor range [m]

20

Average acceleration vs. Sensor range

18

"' -

~ 16

C

~ 0 _QJ¹⁴

-.; u u 12

"'

QJ en ~

QJ

~ ¹⁰

8

18 20 22 24 26 28 30