Point-to-Point Trajectory Interpolation Concepts and Implementation

Preface

Point-to-point trajectory interpolation is among the simplest smooth trajectory generation techniques. This post summarizes cubic and quintic point-to-point trajectory interpolation mathematics by following 2017@Lynch , Section 9.2. In addition, this post also shows their C++ implementation for future reference.

Parameterization

Videos by Prof. Lynch are available at 9.1 and 9.2. Point-to-Point Trajectories (Part 1 of 2) and 9.1 and 9.2. Point-to-Point Trajectories (Part 2 of 2). Since the videos perfectly illustrated the theory, here we only list the critical equations for a quick review.

The parameterized trajectory is given by $$ \theta (s) = \theta_{\text{start}} + s(\theta_{\text{end}} - \theta_{\text{start}}) $$ for $s\in [0, 1]$.

Cubic Interpolation

To get the coefficient of a cubic interpolation trajectory, one has $$ s(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 $$ $$ \dot{s}(t) = a_1 + 2a_2 t + 3 a_3 t^2 $$ with $$ s(0) = 0, \quad \dot{s}(0) = 0 $$ $$ s(T) = 1, \quad \dot{s}(T) = 0 $$ Solving for these equations gives $$ \theta (s) = \theta_{\text{start}} + \Big(\frac{3t^2}{T^2} - \frac{2t^3}{T^3}\Big) (\theta_{\text{end}} - \theta_{\text{start}}) $$ $$ \dot{\theta} (s) = \Big(\frac{6t}{T^2} - \frac{6t^2}{T^3}\Big) (\theta_{\text{end}} - \theta_{\text{start}}) $$ $$ \ddot{\theta} (s) = \Big(\frac{6}{T^2} - \frac{12t}{T^3}\Big) (\theta_{\text{end}} - \theta_{\text{start}}) $$

Quintic Interpolation

To get the coefficient of a quintic interpolation trajectory, one has $$ s(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5 $$ $$ \dot{s}(t) = a_1 + 2 a_2 t + 3 a_3 t^2 + 4 a_4 t^3 + 5 a_5 t^4 $$ $$ \ddot{s}(t) = 2 a_2 + 6 a_3 t + 12 a_4 t^2 + 20 a_5 t^3 $$ with $$ s(0) = 0, \quad \dot{s}(0) = 0, \quad \ddot{s}(0) = 0 $$ $$ s(T) = 1, \quad \dot{s}(T) = 0, \quad \ddot{s}(T) = 0 $$ Solving for these equations gives $$ \theta (s) = \theta_{\text{start}} + \Big( \frac{10t^3}{T^3} - \frac{15t^4}{T^4} + \frac{6t^5}{T^5} \Big) (\theta_{\text{end}} - \theta_{\text{start}}) $$ $$ \dot{\theta} (s) = \Big( \frac{30t^2}{T^3} - \frac{60t^3}{T^4} + \frac{30t^4}{T^5} \Big) (\theta_{\text{end}} - \theta_{\text{start}}) $$ $$ \ddot{\theta} (s) = \Big( \frac{60t}{T^3} - \frac{180t^2}{T^4} + \frac{120t^3}{T^5} \Big) (\theta_{\text{end}} - \theta_{\text{start}}) $$

Implementation

For cubic interpolation,

For quintic interpolation,

Results generated by the above C++ implementation are plotted below for verification.

Derivation

The derivation results of $s(t), \dot{s}(t), \ddot{s}(t)$ can be verified by sympy. For cubic interpolation,

For quintic interpolation,