UDACITY SDCE Nanodegree Term 3 — Project 1: Path Planning
The goal of this project was to design a path planner that is able to create smooth, safe paths for the car to follow along a 3-lane highway with traffic. The path planner should be able to keep inside its lane, avoid hitting other cars, and pass slower moving traffic all by using localization, sensor fusion, and map data.
Introduction
The goal of this project is to safely navigate a vehicle around a virtual highway. The requirement for this project is to design a path planner that creates smooth trajectories for the car to follow that are acceptably tolerable by a passenger, hence, the path should minimize the jerk. Through the simulations the car was able to travel more than 4.32 miles without an incident. In fact, I managed to drive more than 10 miles with the car without an incident.
The main components of my solution are spline fitting to generate trajectories and lane following, Frenet coordinates for collision detection, lane occupancy, and free space in each lane. A Finite State Machine for behavioural planning with logic for lance, changing, collision avoidance, overtaking and finding open space on the road.
Frenet Coordinates
With Frenet coordinates, we use the variables s and d to describe a vehicle’s position on the road. The s coordinate represents distance along the road (also known as longitudinal displacement) and the d coordinate represents side-to-side position on the road (also known as lateral displacement).
Why do we use Frenet coordinates? Imagine a curvy road like the one below with a Cartesian coordinate system laid on top of it. Using these Cartesian coordinates, we can try to describe the path a vehicle would normally follow on the road. And notice how curvy that path is! If we wanted equations to describe this motion it wouldn’t be easy!
Ideally, it should be mathematically easy to describe such common driving behavior. But how do we do that? One way is to use a new coordinate system . Here, we’ve defined a new system of coordinates. At the bottom we have s=0 to represent the beginning of the segment of road we are thinking about and d=0 to represent the center line of that road. To the left of the center line we have negative d and to the right d is positive.
Maintaining Speed limit and lane following
The first task when tackling the project is making the car drive within the speed limit. This is achieved by increasing and decreasing the car reference velocity by increments of 0.3 m/s. Using the Frenet coordinates and the sensor fusion data it is possible to predict the behaviour of other vehicles in the future. By projecting the s coordinates in the future, the vehicles that will be in front of us in the occupying lane are identified. Whenever such a collision is detected the speed is decreased. Furthermore, in order to account for vehicles that may rapidly change lane or speed the deceleration is also controlled by the factor of the velocity difference between the two vehicles. Hence, when the velocity difference the ego vehicle will break more aggressively. This results in the ego vehicle staying behind other vehicles for some time until sufficient space is available on the adjacent lane.
The lane following component is a combination of two parts: Frenet coordinates and spline fitting. The former is used to identify the current lane of the vehicle and through the helper function getXY() retrieve the corresponding XY positions for the centre of the lane. From there spline fitting is employed to generate a smooth path for a number of time instances. These steps ensured that the car adheres to the maximum acceleration and jerk requirements, stays within the lane limits and does not have any collisions. However, at this point it does not do anything else and is basically following which ever vehicle is in front of it. The spline and trajectory generation haven been adapted from the walkthrough so I will not go into it in this report.
Path Planning
The intelligence of the vehicle lies in its decision-making process which determines the vehicle’s actions. The ego vehicles has four states:
- Free — No obstacle ahead
- Lane Changing to an empty lane
- Change lane to overtake another vehicle
- Maintaining velocity behind another car
These are used to guide the planner to make the ego vehicle, change lane, accelerate/decelerate, or move to an empty lane.
Changing Lanes
To change a lane sufficient space must be available for the target lane and also no other vehicles should be in close proximity while the ego vehicle is changing lanes to avoid collision. This requires that first the available information from the sensor fusion data with all the data for all other vehicles is analysed. The data format for each vehicle is: [ id, x, y, vx, vy, s, d]. In particular, the d and s coordinates are useful in determining the capacity of a lane. First, for each lane the vehicles within distance are selected and their s position is calculated. If all vehicles are over a given threshold away then the lane is deemed safe for change.
Maximizing forward progress
In order to maximize the forward progress of a vehicle a margin per lane is used to keep track of the possible space in each line. The planner will attempt to move to the lane with the most available space given that the conditions for lane change are safe. The space/margin afforded in each lane is calculated by finding the closet vehicle in front of the ego vehicle for each lane. The lane with the highest number is preferred as it affords the ego vehicle more space to move. Due to this approach, a tricky situation may arise where the ego vehicle wants to move from the left lane to the right one and vice-versa. In such, cases the maximum acceleration was exceeded since the lateral acceleration is increased. Essentially, the ego vehicle changes lanes very rapidly. The solution is to make sure that the ego vehicle is sufficiently positioned within a lane before moving to the next, by checking that the d vehicle coordinate is within a small range about the center of the lane. This ensures that the vehicle is straight within a lane before moving to the adjacent one. However, this could result in the vehicle changing lanes very often without. Hence, a threshold is applied so that the gain in space should be significant in order to the ego vehicle to change lanes. For example, in the figure below the ego vehicle would stay in the center lane given that the margin is larger from the right lane but not so much from the left lane.
Planning
The planning algorithm is based on a Finite State Machine approach. The FSM first checks for collision alarm and slows down the vehicle if it is too fast. It then initiates the overtake action in order to the ego vehicle to bypass the succeeding vehicle when the adjacent lane is clear and has more available space than the current one. The latte is useful when multiple vehicles in front of the ego vehicle block the open space. When the ego vehicle has a bit more space (i.e., not directly blocked) the planner will attempt to position it to the lane with the most margin for movement. The ego vehicle keeps its lane until it senses that it approaches other vehicles in which case it again will proactively move to the lane with more margin.
Conclusions and Future Improvements
The path planning approach has worked well for this particular project. However, there are still improvements that can be made to the approach. In particular predicting mechanisms can be employed to identify when a vehicle will change lane and readjust the ego vehicle plan accordingly. In the present implementation, the lanes must be empty for the ego vehicle to make an action. This can be use in a highway scenario but for more complex problems more elaborate approaches are necessary. Finally, the car may get stack in some cases behind other cars while there may be a free lane, where more elaborate cases could for example have the vehicle reduce velocity so that the blocking cars can bypass it and the ego vehicle will have space to move to the free lane. Overall, this project has been a challenging but it was rewarding experience watching the car drive autonomously and without collisions.
