Reflections on Designing a Virtual Highway Path Planner (Part 2/3)

5 min readAug 1, 2017

Right now it swerves like a crazy wishy-washy undecided driver and ironically too conservative at keeping its lane at the same time.

Read: Part 1 of 3 | Read: Part 2 of 3| Read: Part 3 of 3

Broad Overview of My Pipeline

At first, start the engine and drive to a sufficient speed

Every time the vehicle controller’s about to run out of something to do:

Get sensor data about our vehicle and nearby dynamic cars (Where am I? How fast am I going? How fast are nearby cars going? Where are they?)
Use this data to decide what behavior to do ( Should I go left? Go right? Stay on my lane?)
Based on the decided behavior and the sensor data I’ve mentioned earlier, we should generate an actual path that would provide a smooth, comfortable and safe ride to accomplish the goal behavior.
Convert this path into something that the controller understands before finally sending it to the controller

Something Fishy

You might smell something fishy about this pipeline. Three smelly fishy things actually. I’ll get back to that in the final article! I’ll discuss the data structures first!

DATA STRUCTURES

Ultimately, I ended up having five data classes to implement my pipeline.

`Vehicle`

This object is responsible for making sense and storing data about a vehicle. Vehicles like our vehicle and other vehicles nearby.
It can answer things such as: where and and how fast is it? Is there a lane in its left? Which lane is in its right? How close are we from vehicle in front of us and how fast is it since we last checked?

`BehaviorPlanner`

This object is responsible for suggesting which behavior to do based on the information we have about our vehicle and nearby vehicles. For simplicity, it returns whether it thinks we should go left, turn right, or stay on its lane.
IMPORTANT NOTE: This is probably not good design practice but as of writing but because it is used in behavior determination, this class contains a function the distance of the closest vehicle in a given lane and position (front or back) given a set of vehicles to a vehicle of interest. As a result it also updates respective members of the vehicle class. (Might inject this out if I have the time)

`JMT`

JMT stands for Jerk Minimized Trajectory.
This object represents a quintic polynomial function of a number which has six coefficients. Once instantiated, you can evaluate this polynomial by giving a value.
The six coefficients are determined by giving seven numbers upon instantiation.
Jerk is the instantaneous change in acceleration over time like acceleration is the instantaneous change in speed over time. They say that people find rides with jerks really uncomfortable and are kinda okay with having acceleration as long as the acceleration is constant or not so high. This is why we want to minimize the jerk of our path. Luckily there’s an algorithm to get that. (Thanks math!)
Suppose that position can be represented by as a function of time, then we know from high school physics that by definition velocity is the derivative of position and acceleration is the derivative of velocity and jerk is the derivative of acceleration.

Suppose we call a set of position, velocity and acceleration a `state`. If we have our starting state, our desired goal state, and how much time we have to get from one state to another, there is actually a formula on how to get the polynomial path position function that minimizes the squared jerk to get from the start state to the desired final state or goal state given the duration. This start state, final state, and time duration are the seven numbers we give and the resulting polynomial function this object represents is that path position function that minimizes the squared jerk. (BTW, we minimize the square of the jerk and not just the jerk itself because we care about the magnitude of the jerk, it doesn’t matter if it’s positive or negative.)

`Trajectory`

This object is responsible to output the actual path along the road that we want the vehicle to take given the desired behavior and current predicted state (latency considered) of the vehicle. This path is encapsulated by two `JMT` objects. One `JMT` object represent the path that the vehicle takes with respect to the road moving forward. The other object represents the path the vehicle takes with respect to the road moving sideways.

To generate the two `JMT` objects we must specify a start state, target final state and a time duration that this object will calculate based on the information we have about our vehicle in relation to nearby vehicles. This Trajectory object is responsible to selecting a suitable target state.

`PathConverter`

This object has a representation of the global map of the highway. It takes two `JMT` objects which represents the path we wish to take along the road sideways and forward. This is something the controller does not understand. The controller understands discrete points along the map in cartesian coordinates.

So this PathConverter object is responsible for converting this path into discrete points when we give it the distance between points along the path and number of points.