Introduction
The problem we are trying to solve is to go from an initial configuration \(q_{i}\) to a final configuration \(q_f\) with an initial and final velocity constraint \(\dot{q}_{i}\) and \(\dot{q}_{f}\) respectively. Without loss of generality we are going to consider that a trajectory starts at \(t=0\) and finishes at \(t=1\). For the rest of the article we are going to focus on using only third-degree polynomials to generate the trajectory:
$$ \begin{equation} q(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 \end{equation} $$
By taking its time derivatives we have:
$$ \begin{equation} \dot{q}(t) = a_1 + 2 a_2 t + 3 a_3 t^2 \end{equation} $$
From the problem constraints we deduce the following relationships:
$$ \begin{aligned} q(0) &= q_i \implies a_0 = q_{i} \newline q(1) &= q_f \implies a_0 + a_1 + a_2 + a_3 = q_{f} \newline \dot{q}(0) &= \dot{q}_i \implies a_1 = \dot{q}_i \newline \dot{q}(1) &= \dot{q}_f \implies a_1 + 2 a_2 + 3 a_3 = \dot{q}_f \end{aligned} $$
The final system of equation is:
$$ \begin{gather} a_2 + a_3 = q_f - q_{i} - \dot{q}_i \\ 2 a_2 + 3 a_3 = \dot{q}_f - \dot{q}_i \end{gather} $$
By solving for \(a_2\) and \(a_3\) we can have all the polynomial coefficients of \(q(t)\) and thus the complete trajectory, in this case, position and velocity profiles.
Remarks
Note that we are able to handle initial and final position and velocity constraints with polynomials of order 3. If we follow the same reasoning using a fifth-degree polynomial, we will be able to handle in addition initial and final acceleration constraints. Moreover, if we use a seventh-degree polynomial, we can fix initial and final jerk constraints. For the rest of the article we will only consider third-degree polynomials.
Example
Let us consider the following constraints:
$$ \begin{aligned} q_i &= 1 \newline q_f &= 2 \newline \dot{q}_i &= 3 \newline \dot{q}_f &= 4 \end{aligned} $$
From above we directly deduce:
$$ \begin{aligned} a_0 &= 1 \newline a_1 &= 3 \end{aligned} $$
Now we need to solve:
$$ \begin{equation} \begin{bmatrix} 1 & 1 \\ 2 & 3 \end{bmatrix} \begin{bmatrix} a_2 \\ a_3 \end{bmatrix} = \begin{bmatrix} 0 \\ 1 \end{bmatrix} \end{equation} $$
We find \( a_2 = -1\) and \( a_3 = 1 \).
We can quickly test our result with the following code snippet:
using CairoMakie
a_0 = 1
a_1 = 3
a_2 = -1
a_3 = 1
t = 0:0.001:1
s = a_0 .+ a_1 .* t .+ a_2 * t.^2 .+ a_3 * t.^3
ds = a_1 .+ 2* a_2 * t .+ 3 * a_3 * t.^2
f = Figure()
ax = Axis(f[1, 1],
title = "Simple Spline Example",
xlabel = "Time [s]",)
lines!(ax,
t,
s,
label="Position")
lines!(ax,
t,
ds,
label="Speed")julia
axislegend(position = :rb)
current_figure()
We obtain the following figure:
Code
If you just want to jump straight to the source code you can find-it here
Polynomials
Let’s begin by defining some useful data structures:
struct Polynomial
coeffs
end
A Polynomial is just a list of the polynomial coefficients:
\( a_0, \ldots , a_N \).
As mentioned in Remarks, the polynomial used to generate the trajectory can be of degree 5 or 7 depending on the application. For higher-order polynomials, it is important to use numerically stable evaluation methods. Thus, we use Horner’s method for polynomial evaluation.
function poly_eval(poly, x)
rev_coeffs = reverse(poly.coeffs)
f = rev_coeffs[1]
for ai in rev_coeffs[2:end]
f = x * f + ai
end
return f
end
Splines
To represent the splines and trajectories we will use the following data structures:
struct Waypoint
pos
speed
end
struct Segment
ti # initial time
tf # final time
dt # timestep
wi # initial waypoint
wf # final waypoint
end
struct Trajectory
wpts
timepoints
timestep
end
A Waypoint contains the initial position and speed constraints. A
Segment is the combination of two Waypoint objects representing a
trajectory between them. It contains the initial and final timepoints,
and corresponding waypoints. In addition, it also contains the
timestep used to generate the time for the segment. Finally, a
Trajectory is only a list of waypoints, timepoints, and a single
timestep.
We use the function segment_to_poly_3 to solve the linear system of
equations previously presented, and obtain the polynomial coefficients
for the position and speed trajectories.
function segment_to_poly_3(seg)
a0 = seg.wi.pos
a1 = seg.wi.speed
A = [ 1 1 ;
2 3 ]
b = [ seg.wf.pos - seg.wi.pos - seg.wi.speed;
seg.wf.speed - seg.wi.speed]
x = A \ b
a2 = x[1]
a3 = x[2]
p = Polynomial([a0, a1, a2, a3])
dp = Polynomial([a1, 2*a2, 3*a3])
return p,dp
end
The function seg_to_poly_3_traj generates the trajectory for the
given segment:
function seg_to_poly_3_traj(seg)
t = 0:seg.dt:1
p, dp = segment_to_poly_3(seg)
q = map( (x) -> poly_eval(p, x), t)
dq = map( (x) -> poly_eval(dp, x), t)
t = seg.ti .+ (seg.tf - seg.ti) .* t
return t,q,dq
end
Finally, poly_3_traj iterates through the waypoints, creating a
segment between each consecutive pair. For each segment, it calls
seg_to_poly_3 to compute the polynomial trajectory. These segment
trajectories are then concatenated to form the complete trajectory,
which is returned.
function poly_3_traj(traj)
t = []
q = []
dq = []
for idx in firstindex(traj.wpts):lastindex(traj.wpts)-1
println("wpts[$idx]: ", traj.wpts[idx], " timepoints[$idx]: ", traj.timepoints[idx])
seg = Segment(traj.timepoints[idx],
traj.timepoints[idx+1],
traj.timestep,
traj.wpts[idx],
traj.wpts[idx+1])
ti, qi, dqi = seg_to_poly_3_traj(seg)
if isempty(t) && isempty(q) && isempty(dq)
t = ti
q = qi
dq = dqi
else
t, q, dq = concat_traj(t, q, dq, ti, qi, dqi)
end
end
return t, q, dq
end
Complete Test
The following test generates a trajectory between several waypoints:
wpt_1 = Waypoint(0, 0)
wpt_2 = Waypoint(2, 3)
wpt_3 = Waypoint(5, 3)
wpt_4 = Waypoint(3, 0)
wpts = [wpt_1, wpt_2, wpt_3, wpt_4]
timepoints = [0, 1, 4, 7]
timestep = 0.01
traj = Trajectory(wpts, timepoints, timestep)
t, q, dq = poly_3_traj(traj)
If we plot the results we obtain:
Things Left as an Exercice
- Implement 5th and 7th degree polynomial strategies to have acceleration and jerk continuity
- Improve the user interface API for a better user experience.
- Go 3D!