Pure Pursuit Robot Navigation Following Interpolated Cubic Splines

I've been working to improve my school VEX team's autonomous code for my robotics class, and have created a pure pursuit robotics PID that I thought I would share. The code here is in Python, and I'm only using matplotlib as an output to visualize the robot's movement. However, I do hope to rewrite this code in C++ soon, getting a movement vector output which will then be applied to a VEX robot.

First is the spline class. I'm currently using a simple parametric cubic spline class. Keep in mind that this is a really bad way to implement splines, as it demands increasing x-values along the domain which isn't ideal for a robot's path. I am definitely going to rewrite all of this in the future to have a periodic domain, but I thought I would share what I have right now anyways because it might be usef

A spline is defined as a piecewise function of polynomials, and in the case of a cubic spline, the polynomials of choice are cubic polynomials. Therefore, the first method we should write in the class is a function to create a parameterized spline between two guide points as follows.

def calculate_spline(self, guide_pairs, t):
    for i in range(len(guide_pairs)):
        if t >= guide_pairs[i][0][0] and t <= guide_pairs[i][1][0]:
            a = guide_pairs[i][0][1]
            b = 0
            c = 3 * (guide_pairs[i][1][1] - guide_pairs[i][0][1]) / (guide_pairs[i][1][0] - guide_pairs[i][0][0]) ** 2
            d = -2 * (guide_pairs[i][1][1] - guide_pairs[i][0][1]) / (guide_pairs[i][1][0] - guide_pairs[i][0][0]) ** 3
            return (t, a + b * (t - guide_pairs[i][0][0]) + c * (t - guide_pairs[i][0][0]) ** 2 + d * (t - guide_pairs[i][0][0]) ** 3)
    return (0, 0)

Next, we can write a method to calculate any t-value for any number of points, by checking which of the guide points the t-value is between and calculating the value on the corresponding cubic polynomial.

def calculate(self, t):
    guide_pairs = []
    for i in range(len(self.guides) - 1):
        guide_pairs.append([self.guides[i], self.guides[i + 1]])
    return self.calculate_spline(guide_pairs, t)

I also added a method to calculate the first and second derivatives of the spline (since cubic splines have C^2 continuity, that's pretty much all we need.) Slowing our robot down where the second derivative is high will allow it to smoothly move along sharp turns in the path.

def deriv(self, t, dt, n):
    (x, y) = self.calculate(t)
    (x1, y1) = self.calculate(t + dt)
    (x2, y2) = self.calculate(t - dt)
    if n == 1:
        return ((x1 - x) / dt, (y1 - y) / dt)
    elif n == 2:
        return ((x1 - 2 * x + x2) / dt ** 2, (y1 - 2 * y + y2) / dt ** 2)
    else:
        return (0, 0)

Finally, the constructor and representation methods. The constructor calculates the points of the spline beforehand, which the PID can then use for pure pursuit.

def __init__(self, guides, res=10, ds=1):
    self.guides = guides
    self.points = []
    self.res = 1/res
    self.ds = ds/10
    t = 0
    while t < self.guides[-1][0]:
        (x, y) = self.calculate(t)
        self.points.append([x, y, t])
        t += self.res

def __repr__(self):
    return self.points

Now, we can move on to the actual pure pursuit algorithm and class. While I won't be delving into the specifics of how the PurePursuit class works on a theoretical level, I would recommend this article from the Algorithms for Automated Driving page, which is also what I used to design my implementation.

First, let's declare our constants, including velocity multiplier, the theta value, proportional gain, time step, and look-forward distance, as well as the points in our spline to use.

VMULT = 1.0
THETA = 1.0
PROP_GAIN = 20.0
DT = 0.1
LFC = 20.0

CURR_POINT_INDEX = 0

POINTS = [
    [x1, y1],
    [x2, y2],
    [x3, y3]
]

I've used placeholder values here, but you will have to tune these values, mostly the proportional gain and look-forward, as desired.

Next is the constructor, which just takes a whole bunch of parameters and assigns them to locally scoped variables.

def __init__(self, path, v_mult, theta, prop_gain, lfc, start_pos):
    self.path = path
    self.v_mult = v_mult
    self.theta = theta
    self.orig_prop_gain = prop_gain
    self.prop_gain = prop_gain
    self.lfc = lfc
    self.curr_point_index = 0
    self.start_pos = start_pos

    self.v = 0

The track_target() method below will update the velocity and, as the name suggests, track the target by updating the current point index.

def track_target(self, x, y, yaw):
    t = self.path.points[self.curr_point_index][2]

    (dx, dy) = self.path.deriv(t, self.path.ds, 2)
    self.v = self.v_mult * abs(1 / (math.sqrt(dx ** 2 + dy ** 2) + 1))
    if self.curr_point_index < (len(self.path.points) - 2 if len(self.path.points) % 2 == 0 else len(self.path.points) - 1):
        if math.hypot(self.path.points[self.curr_point_index + 1][0] - x, self.path.points[self.curr_point_index + 1][1] - y) < math.hypot(self.path.points[self.curr_point_index][0] - x, self.path.points[self.curr_point_index][1] - y):
            self.curr_point_index += 1
    else:
        self.v = 0
    ind = self.curr_point_index
    return ind

The calc_next() method will calculate movement towards the next point with trig.

def calc_next(self, x, y, theta):
    ind = self.track_target(x, y, theta)
    if ind < 0:
        ind = 0
    if ind >= len(self.path.points):
        ind = len(self.path.points) - 1
    tx = self.path.points[ind][0]
    ty = self.path.points[ind][1]
    alpha = math.atan2(ty - y, tx - x) - theta
    delta = math.atan2(2.0 * self.lfc * math.sin(alpha) / self.prop_gain, 1.0)
    return delta, ind

The animate() method uses matplotlib to visualize everything per frame.

def animate(self, target_ind, x, y):
    plt.cla()

    spline_x, spline_y = ([], [])
    for point in self.path.points:
        spline_x.append(point[0])
        spline_y.append(point[1])

    plt.plot(spline_x, spline_y, 'r')

    plt.grid(True)
    plt.axis("equal")

    plt.grid(True)
    plt.plot(x, y, "xr")

    plt.plot(self.path.points[target_ind][0], self.path.points[target_ind][1], "ob")
    plt.pause(0.001)

Finally, the run() method puts everything together, tracking the target and moving towards it until the bot's velocity is zero.

def run(self):
    x, y, yaw, v = self.start_pos[0], self.start_pos[1], 0.0, 0.0
    target_ind = self.track_target(x, y, yaw)
    while (self.v != 0):
        delta, target_ind = self.calc_next(x, y, yaw)
        yaw += self.v / self.prop_gain * math.tan(delta) * DT
        x += self.v * math.cos(yaw) * DT
        y += self.v * math.sin(yaw) * DT
        self.animate(target_ind, x, y)

        if plt.waitforbuttonpress(0.01):
            break

All encapsulated inside of a class, the code for pure pursuit can be called in just two lines of code.

pure_pursuit = PurePursuit(sp.SplinePath(POINTS))
pure_pursuit.run()

And that's it, we have an animated robot PID up and running. The animate() method can be replaced with another method to get the bot's XY velocity as a vector, which can then be applied to locomotion.

Thanks for reading!