QP Trajectory Generation

This page describes a method to generate polynomial trajectories by solving a Quadratic Programming (QP) problem. Concepts are explained through two examples. The frist example shows how to generate a minimum acceleration trajectory that passes through two points. The second example shows how to generate a minimum snap trajectory that passes through several waypoints. Python code is provided.

Minimum Acceleration Trajectory

The first example is to find a trajectory with minimum acceleration that moves from x0=1 to xf=2 in T=10 seconds. The minimum degree of a polynomial to ensure smoothness for a minimum acceleration (2nd derivative) trajectory is 3 (i.e. cubic polynomial):

The goal is to find the four coefficients p0 to p3. To do so, we rephrase the problem as a QP problem of the form:

where p is the vector of the polynomial coefficients:

Once we rephrase it, the problem can be solved efficiently with readily available QP libraries.

Since we want to minimize the acceleration, the cost function can be written as:

The derivatives of P(t) can be calculated as,

Substituting this into the cost function yields:

The Q matrix is found by taking the hessian matrix of J:

Next are the constraints:

which is equivalent to,

This can be rewritten in matrix form as:

So the A matrix and b vector are given by:

Below is a Python script to solve the QP for the above example:

import numpy as np
from cvxopt import matrix, solvers

x0 = 1.0
xf = 2.0
T = 10.0

Q = np.array([
    [0, 0, 0, 0],
    [0, 0, 0, 0],
    [0, 0, 8*T, 12*(T**2)],
    [0, 0, 12*(T**2), 24*(T**3)],
])

f = np.zeros(4)

A = np.array([
    [1, 0, 0, 0],
    [1, T, T**2, T**3]
])
b = np.array([x0, xf])

sol = solvers.qp(matrix(Q), matrix(f), None, None, matrix(A), matrix(b))
print(sol['x'])

The resulting polynomial is:

which is a line (zero acceleration) that passes through P(0)=1 and P(10)=2.

Minimum Snap Trajectory

The next example is to generate a minimum snap (4th derivative) trajectory that passes through 4 waypoints at the following times:

i t x
0 0 0
1 10 5
2 30 5
3 40 3

Since there are 4 waypoints, it is going to be a piecewise polynomial function of 3 parts. Each polynomial joins two consecutive waypoints. We will use a septic polynomial (7th degree polynomial) for each segment because that is the minimum degree required to satisfy the minimum snap requirement. Polynomial Pi(t) joins waypoints i and i+1:

where Si are the times at the beginning of each trajectory:

There are 3*8=24 unknown coefficients. Again the problem is set up as a QP problem:

Where p is a vector of all 24 coefficients:

The Q matrix will be a 24x24 matrix. It is a block diagonal matrix made of three parts (one for each polynomial).

Each submatrix can be calculated this time using symbolic math:

import sympy

t, T = sympy.symbols('t T')
p0, p1, p2, p3, p4, p5, p6, p7 = sympy.symbols('p0 p1 p2 p3 p4 p5 p6 p7')

P = p7*t**7 + p6*t**6 + p5*t**5 + p4*t**4 + p3*t**3 + p2*t**2 + p1*t + p0
P1 = sympy.diff(P, t) # Speed
P2 = sympy.diff(P1, t) # Acceleration
P3 = sympy.diff(P2, t) # Jerk
P4 = sympy.diff(P3, t) # Snap
J = sympy.integrate(P4**2, (t, 0, T))
Q = sympy.hessian(J, (p0, p1, p2, p3, p4, p5, p6, p7))

sympy.pprint(Q)

This results in:

where Ti is the time interval between waypoints i and i+1:

Next are the constraints (Ap=b). We will apply 14 constraints in total. A is 14x24 matrix where each row is a constraint.

Putting all of this together in matrix form Ap = b:

Solving the QP:

import numpy as np
from cvxopt import matrix, solvers
import matplotlib.pyplot as plt

def get_Q(T):
    return np.array([
        [0, 0, 0, 0, 0, 0, 0, 0],
        [0, 0, 0, 0, 0, 0, 0, 0],
        [0, 0, 0, 0, 0, 0, 0, 0],
        [0, 0, 0, 0, 0, 0, 0, 0],
        [0, 0, 0, 0, 1152*T, 2880*T**2, 5760*T**3, 10080*T**4],
        [0, 0, 0, 0, 2880*T**2, 9600*T**3, 21600*T**4, 40320*T**5],
        [0, 0, 0, 0, 5760*T**3, 21600*T**4, 51840*T**5, 100800*T**6],
        [0, 0, 0, 0, 10080*T**4, 40320*T**5, 100800*T**6, 201600*T**7]
    ])

def get_coefficients(T, wp):
    N = 8 # Septic polynomial has 8 coefficients
    M = len(T) # Number of polynomial segments
    L = 14 # Number of constraints

    # Q Matrix
    Z = np.zeros((N, N))
    Q0 = get_Q(T[0])
    Q1 = get_Q(T[1])
    Q2 = get_Q(T[2])
    Q = np.block([
        [Q0, Z, Z],
        [Z, Q1, Z],
        [Z, Z, Q2]
    ])

    f = np.zeros(N*M)

    # Constraints (Ap=b)
    A = np.zeros((L, N*M))
    # Start position of polynomials
    A[0,:8] = [1, 0, 0, 0, 0, 0, 0, 0]
    A[1,8:16] = [1, 0, 0, 0, 0, 0, 0, 0]
    A[2,16:] = [1, 0, 0, 0, 0, 0, 0, 0]
    # End position of polynomials
    A[3,:8] = [1, T[0], T[0]**2, T[0]**3, T[0]**4, T[0]**5, T[0]**6, T[0]**7]
    A[4,8:16] = [1, T[1], T[1]**2, T[1]**3, T[1]**4, T[1]**5, T[1]**6, T[1]**7]
    A[5,16:] = [1, T[2], T[2]**2, T[2]**3, T[2]**4, T[2]**5, T[2]**6, T[2]**7]
    # Continuous velocity
    A[6,:16] = [0, 1, 2*T[0], 3*T[0]**2, 4*T[0]**3, 5*T[0]**4, 6*T[0]**5, 7*T[0]**6, 0, -1, 0, 0, 0, 0, 0, 0]
    A[7,8:] = [0, 1, 2*T[1], 3*T[1]**2, 4*T[1]**3, 5*T[1]**4, 6*T[1]**5, 7*T[1]**6, 0, -1, 0, 0, 0, 0, 0, 0]
    # Continuous acceleration
    A[8,:16] = [0, 0, 2, 6*T[0], 12*T[0]**2, 20*T[0]**3, 30*T[0]**4, 42*T[0]**5, 0, 0, -2, 0, 0, 0, 0, 0]
    A[9,8:] = [0, 0, 2, 6*T[1], 12*T[1]**2, 20*T[1]**3, 30*T[1]**4, 42*T[1]**5, 0, 0, -2, 0, 0, 0, 0, 0]
    # Continuous jerk
    A[10,:16] = [0, 0, 0, 6, 24*T[0], 60*T[0]**2, 120*T[0]**3, 210*T[0]**4, 0, 0, 0, -6, 0, 0, 0, 0]
    A[11,8:] = [0, 0, 0, 6, 24*T[1], 60*T[1]**2, 120*T[1]**3, 210*T[1]**4, 0, 0, 0, -6, 0, 0, 0, 0]
    # Continuous snap
    A[12,:16] = [0, 0, 0, 0, 24, 120*T[0], 360*T[0]**2, 840*T[0]**3, 0, 0, 0, 0, -24, 0, 0, 0]
    A[13,8:] = [0, 0, 0, 0, 24, 120*T[1], 360*T[1]**2, 840*T[1]**3, 0, 0, 0, 0, -24, 0, 0, 0]

    b = np.zeros(L)
    b[:6] = [wp[0], wp[1], wp[2], wp[1], wp[2], wp[3]]

    sol = solvers.qp(matrix(Q), matrix(f), None, None, matrix(A), matrix(b))
    return list(sol['x'])

# Waypoints
wp_t = np.array([0., 10., 30., 40.])
wp_x = np.array([0., 5., 5., 3.])
T = np.ediff1d(wp_t)

p = get_coefficients(T, wp_x)

N = 500
t = np.linspace(wp_t[0], wp_t[-1], N)
pos = [None] * N
vel = [None] * N
acc = [None] * N
jrk = [None] * N
snp = [None] * N
for i in range(N):
    j = np.nonzero(t[i] <= wp_t)[0][0] - 1
    j = np.max([j, 0])
    ti = t[i] - wp_t[j]
    x_coeff = np.flip(p[8*j:8*j+8])
    v_coeff = np.polyder(x_coeff)
    a_coeff = np.polyder(v_coeff)
    j_coeff = np.polyder(a_coeff)
    s_coeff = np.polyder(j_coeff)
    pos[i] = np.polyval(x_coeff, ti)
    vel[i] = np.polyval(v_coeff, ti)
    acc[i] = np.polyval(a_coeff, ti)
    jrk[i] = np.polyval(j_coeff, ti)
    snp[i] = np.polyval(s_coeff, ti)

plt.subplot(5, 1, 1)
plt.plot(t, pos, label='Position')
plt.plot(wp_t, wp_x, '.', label='Waypoint')
plt.legend()
plt.subplot(5, 1, 2)
plt.plot(t, vel, label='velocity')
plt.legend()
plt.subplot(5, 1, 3)
plt.plot(t, acc, label='acceleration')
plt.legend()
plt.subplot(5, 1, 4)
plt.plot(t, jrk, label='jerk')
plt.legend()
plt.subplot(5, 1, 5)
plt.plot(t, snp, label='snap')
plt.legend()
plt.xlabel('Time')
plt.show()

Output:

It can be seen that the trajectory passes through all the waypoints and it is continuous up to the snap.

References