Introduction

In this paper, we explore the use of quintic splines for more sophisticated robot pathing in FTC. Traditionally, FTC robot autonomous motion consists of a series of linear movements (including holonomic drive strafing) and point turns. Although these simple path primitives generally suffice, they are not time-optimal under reasonable kinematic constraints, especially for nonholonomic (e.g., tank/differential) drivetrains. That is, the fastest route between two poses in general is not two point turns and a line.¹ To address this, we propose the use of quintic splines to achieve quick, smooth motion on the field.

In the first part of the paper, we will describe the problem in depth and give motivation for splines. In the second half, we explore some of the mathematics behind quintic splines (and parametric curves more generally) including interpolation, reparametrization, and heading computation.

Problem

In FTC, it’s standard to have a sequence a poses that you want the robot to follow in autonomous (although pre-planned motions can also be utilized in TeleOp). For example, in the Relic Recovery game, a common movement task may be moving from one pose on the balancing stone to another in front of the cryptobox. This is traditionally handled with a series of straight lines and turns that are executed with 1D position PID controllers and potentially motor-based velocity PID (e.g., RUN_USING_ENCODER).

For many game tasks and teams, this is a perfectly viable approach. However, in scenarios where speed is desirable (such as the effectively unlimited scoring potential in Relic Recovery autonomous), conventional methods tend to become less effective. Higher speeds usually lead to greater wheel accelerations and slippage, hindering odometry. Additionally, the feedback controllers may have more chaotic transient behavior or decreased steady-state performance.

In this case, motion profiling can be used to achieve higher speeds without sacrificing accuracy by observing the robot’s physical constraints (e.g., maximum velocity, acceleration).². However, this is only part of the solution; in fact, motion profiling amplifies the discontinuities between the elements of a conventional pose-to-pose path. Each transition between rotation and translations demands a full deceleration-acceleration cycle.

To eliminate this unnecessary pause, we seek to combine the rotation and translation into a single smooth curve. Although many curves suffice, splines are typically employed for this purpose. For simplicity, this paper only considers polynomial splines³. Quintic splines in particular were selected for their balance between continuity and curvature although the methods described can be easily extended to polynomials of arbitrary odd degree.

Preliminaries

Coordinate System

Before constructing paths, it is imperative to define a consistent coordinate system. With this coordinate frame, points on the field can be uniquely described. This allows for the specification of robot poses which consist of the position and a heading direction. This heading is typically encoded as angle from the $x$-axis. [fig:coordinate_system] shows a sample coordinate system for the Relic Recovery field. When determining a coordinate system, it is advantageous to choose axes around the symmetries of the environment. For the standard square FTC field, this comprises putting the origin at the center and placing the planar axes perpendicular to the field walls.

Vectors and Parametric Curves

To fully understand the following mathematics, it helps to grasp a little vector calculus (don’t be scared — only basic knowledge of single-variable calculus is required). Recall that a Euclidean vector $\mathbf{v}$ in $n$ dimensions ($\mathbf{v}\in {\mathbb{R}}^n$) uniquely encodes a single point in the space (commonly represented as an arrow from the origin to the point). This vector can be decomposed into $n$ components: $$\mathbf{v}=(x_1,x_2,\dots ,x_n)$$ The length of a vector is given by its norm: $$|\mathbf{v}|=\sqrt{x_1^2+x_2^2+\dots +x_n^2}$$

Aside from vector addition and scalar multiplication, two additional operations are commonly defined between Euclidean vectors. First, the dot product is defined as the scalar sum of the element-wise products of two vectors: $$\mathbf{v}\cdot \mathbf{w}=v_1{w}_1+v_2{w}_2+...+v_n{w}_n$$ Note that the norm can be alternatively expressed as $|\mathbf{v}|=\sqrt{\mathbf{v}\cdot \mathbf{v}}$. Second, the cross product is defined for three-dimensional vectors as the pseudo-determinant of this quasi-matrix: $$\mathbf{v}\times \mathbf{w}=\left|\begin{array}{ccc}\hat{ı}& \hat{ȷ}& \hat{k}\\ v_1& v_2& v_3\\ w_1& w_2& w_3\end{array}\right|$$

Two-dimensional curves can be represented as graphs of one-dimensional functions ($y=f(x)$). However, this puts unnecessary constraints on the set of possible curves (e.g., no two points on the curve can share the same x-value, no self-intersections, no vertical tangents), especially in higher dimensions. Instead, we will represent curves as a series of vectors that trace out the shape. This is analogous to a parametric curve $\mathbf{r}(t)$ which maps a single real parameter $t$ to the corresponding path vector ($\mathbf{r}:\mathbb{R}\to {\mathbb{R}}^n$). Intuitively, $\mathbf{r}(t)$ can be thought of as the trajectory of a fly located at $\mathbf{r}(t_0)$ in the instant $t=t_0$.

Parametric curves can be represented as a vector of single-variable functions: $$\mathbf{v}(t)=(x_1(t),x_2(t),\dots ,x_n(t))$$ As one might expect, $\mathbf{r}(t)$ can be differentiated component-by-component: $${\mathbf{v}}^{\prime }(t)=(x_1^{\prime }(t),x_2^{\prime }(t),\dots ,x_n^{\prime }(t))$$ Derivatives of dot products and cross products can also be computed: $$\begin{array}{rl}\frac{d}{dt}[{\mathbf{r}}_1(t)\cdot {\mathbf{r}}_2(t)]& ={\mathbf{r}}_1(t)\cdot {\mathbf{r}}_2^{\prime }(t)+{\mathbf{r}}_1^{\prime }(t)\cdot {\mathbf{r}}_2(t)\\ \frac{d}{dt}[{\mathbf{r}}_1(t)\times {\mathbf{r}}_2(t)]& ={\mathbf{r}}_1(t)\times {\mathbf{r}}_2^{\prime }(t)+{\mathbf{r}}_1^{\prime }(t)\times {\mathbf{r}}_2(t)\end{array}$$

A parametric curve is said to be $C^n$ if its nth-order derivatives are defined and continuous everywhere on its domain ($n$ here is distinct from the $n$ in ${\mathbb{R}}^n$). For the rest of the paper, we will restrict our attention to two-dimensional parametric curves $\mathbf{r}(t)=(x(t),y(t))$.

Interpolation

Quintic splines consist of a series of segments assembled together into a single piecewise curve. Each of the segments is a parametric curve with a quintic polynomial for each component. The ith segment of a two-dimensional quintic spline of $n$ segments can be represented as the following: $$\{\begin{array}{l}{x}^{(i)}(t)=a_x^{(i)}{t}^5+b_x^{(i)}{t}^4+c_x^{(i)}{t}^3+d_x^{(i)}{t}^2+e_x^{(i)}t+f_x^{(i)}\\ y^{(i)}(t)=a_y^{(i)}{t}^5+b_y^{(i)}{t}^4+c_y^{(i)}{t}^3+d_y^{(i)}{t}^2+e_y^{(i)}t+f_y^{(i)}\end{array}\text{where}0\le t\le 1$$ Now the goal of interpolation is to “fit” these polynomials between a series of $n+1$ points (commonly referred to as knots) labeled $(x_i,y_i)$. To accomplish this, we can impose the conditions $x^{(i)}(0)=x_i$ and $x^{(i)}(1)=x_{i+1}$ (and correspondingly for $y^{(i)}(t)$). Additionally, to ensure greater continuity, we also match the first and second derivatives at each knot point⁴: $$\begin{array}{rl}\frac{d{x}^{(i)}}{dt}{|}_{t=0}& =x_i^{\prime }\\ \frac{d{x}^{(i)}}{dt}{|}_{t=1}& =x_{i+1}^{\prime }\\ \frac{d^2{x}^{(i)}}{d{t}^2}{|}_{t=0}& =x_i^{\prime \prime }\\ \frac{d^2{x}^{(i)}}{d{t}^2}{|}_{t=1}& =x_{i+1}^{\prime \prime }\end{array}$$ $$(\text{and analogously for }y)$$ Collectively, these constraints guarantee that the overall spline will be (by construction) $C^2$. They also fully define the polynomial coefficients for each spline segment. To actually compute the coefficients, we simply need to solve the linear system with following matrix representation: $$\left[\begin{array}{cccccc}0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 2& 0& 0\\ 1& 1& 1& 1& 1& 1\\ 5& 4& 3& 2& 1& 0\\ 20& 12& 6& 2& 0& 0\end{array}\right]\left[\begin{array}{c}{a}_x\\ b_x\\ c_x\\ d_x\\ e_x\\ f_x\end{array}\right]=\left[\begin{array}{c}{x}_i\\ x_i^{\prime }\\ x_i^{\prime \prime }\\ x_{i+1}\\ x_{i+1}^{\prime }\\ x_{i+1}^{\prime \prime }\end{array}\right]$$ This equation can then be solved to yield the coefficients with your favorite matrix library (or put in row echelon form to yield a set of back-substitutable equations).

Reparametrization

Arc Length

Now that we’ve interpolated the spline segments, we have to join together all of the spline segments into a single piecewise function of a unified variable. To accomplish this, we’re going to reparametrize $\mathbf{r}(t)$ from $t\in [0,1]$ to the arc length parameter $s$. Although it sounds complex, $s$ is just the true displacement along the path, and it traces out the curve with unit speed ($|{\mathbf{r}}^{\prime }(s)|=\sqrt{(\frac{dx}{ds}{)}^2+(\frac{dy}{ds}{)}^2}=1$). This procedure is completely generic, allowing you to combine arbitrary parametric curve with (basically) the same knot derivatives.

To shift from $t$ to $s$, we first have to define $t(s)$. This is difficult to represent analytically although the inverse function $s(t)$ is relatively simple: $$s(t)={\int }_0^t|{\mathbf{r}}^{\prime }(\tau )|d\tau ={\int }_0^t\sqrt{(\frac{dx}{d\tau }{)}^2+(\frac{dy}{d\tau }{)}^2}d\tau$$ In practice, $t(s)$ can be obtained by numerically evaluating the above integral and stopping when the integral sum reaches $s$.

By computing $t(s)$, we can determine $\mathbf{r}(s)$ by composition: $\mathbf{r}(s)=\mathbf{r}(t(s))$. Next, differentiating yields the new parametrized derivative: $$\begin{array}{rl}{\mathbf{r}}^{\prime }(s)& ={\mathbf{r}}^{\prime }(t)t^{\prime }(s)\\ & =\frac{{\mathbf{r}}^{\prime }(t)}{s^{\prime }(t)}\\ & =\frac{{\mathbf{r}}^{\prime }(t)}{|{\mathbf{r}}^{\prime }(t)|}\end{array}$$ This makes sense intuitively as $\frac{{\mathbf{r}}^{\prime }(t)}{|{\mathbf{r}}^{\prime }(t)|}$ is just a normalized version of ${\mathbf{r}}^{\prime }(t)$ that satisfies the condition $|{\mathbf{r}}^{\prime }(s)|=1$. We similarly obtain the new second and third derivatives by differentiating again⁵${}^{,}$⁶: $$\begin{array}{rl}{\mathbf{r}}^{″}(s)& =\frac{{\mathbf{r}}^{″}(t)}{|{\mathbf{r}}^{\prime }(t){|}^2}-\frac{{\mathbf{r}}^{\prime }(t){\mathbf{r}}^{″}(t)\cdot {\mathbf{r}}^{\prime }(t)}{|{\mathbf{r}}^{\prime }(t){|}^4}\\ & =\frac{{\mathbf{r}}^{″}(t){\mathbf{r}}^{\prime }(t)\cdot {\mathbf{r}}^{\prime }(t)-{\mathbf{r}}^{\prime }(t){\mathbf{r}}^{″}(t)\cdot {\mathbf{r}}^{\prime }(t)}{|{\mathbf{r}}^{\prime }(t){|}^4}\\ & =\frac{{\mathbf{r}}^{\prime }(t)\times ({\mathbf{r}}^{″}(t)\times {\mathbf{r}}^{\prime }(t))}{|{\mathbf{r}}^{\prime }(t){|}^4}\\ {\mathbf{r}}^{‴}(s)& =\frac{{\mathbf{r}}^{\prime }(t)\times ({\mathbf{r}}^{‴}(t)\times {\mathbf{r}}^{\prime }(t))+{\mathbf{r}}^{″}(t)\times ({\mathbf{r}}^{″}(t)\times {\mathbf{r}}^{\prime }(t))}{|{\mathbf{r}}^{\prime }(t){|}^9}\end{array}$$

Trajectory

Now, all of the spline segments and various other parametric curves are combined into a single curve $\mathbf{r}(s)$. However, this still needs to be combined with the motion profile to yield the robot’s kinematic state over time.

Let $s(t)$ be the generated motion profile (note: this $t$ actually refers to time; it’s different from the $t$ used earlier). Now the velocity and acceleration of the robot can be determined: $$\begin{array}{rl}\mathbf{v}(t)& =\frac{d}{dt}[\mathbf{r}(s(t))]\\ & ={\mathbf{r}}^{\prime }(s(t))s^{\prime }(t)\\ \mathbf{a}(t)& =\frac{d}{dt}[{\mathbf{r}}^{\prime }(s(t))s^{\prime }(t)]\\ & ={\mathbf{r}}^{″}(s(t))[s^{\prime }(t){]}^2+{\mathbf{r}}^{\prime }(s(t))s^{″}(t)\end{array}$$

Heading

The discussion in the previous sections has been limited to the translational components of the path. This, of course, must be accompanied by some sort of angular motion. For holonomic drives, the heading can be controlled completely independently. In this case, heading can be treated as a third independent path component that can be specified by an arbitrary parametric curve.

However, for nonholonomic drives, the heading is constrained to the direction of travel. For parametric curves, this direction is given by the vector ${\mathbf{r}}^{\prime }(s)$: $$\theta (s)=\arctan \frac{y^{\prime }(s)}{x^{\prime }(s)}$$ In practice, $\theta (s)$ is computed with the two-argument version of $\arctan$, eliminating issues arising from signs and cases when $x^{\prime }(s)=0$. Of course, the derivatives are also necessary: $$\begin{array}{rl}{\theta }^{\prime }(s)& =\frac{1}{(\frac{y^{\prime }(s)}{x^{\prime }(s)}{)}^2+1}\cdot \frac{x^{\prime }(s)y^{″}(s)-y^{\prime }(s)x^{″}(s)}{[x^{\prime }(s){]}^2}\\ & =\frac{x^{\prime }(s)y^{″}(s)-y^{\prime }(s)x^{″}(s)}{[x^{\prime }(s){]}^2+[y^{\prime }(s){]}^2}\\ & =x^{\prime }(s)y^{″}(s)-y^{\prime }(s)x^{″}(s)\\ {\theta }^{″}(s)& =[x^{″}(s)y^{″}(s)+x^{\prime }(s)y^{‴}(s)]-[y^{″}(s)x^{″}(s)-y^{\prime }(s)x^{‴}(s)]\\ & =x^{\prime }(s)y^{‴}(s)-y^{\prime }(s)x^{‴}(s)\end{array}$$ Notice that the expressions for ${\theta }^{(n)}(s)$ involve $\mathbf{r}(s)$ derivatives of order $n+1$ and lower. Hence, $C^2$ $\mathbf{r}(s)$ only guarantees $C^1$ $\theta (s)$. This heading derivative continuity is one of the primary reasons for quintic over cubic splines.

Conclusion

This paper discussed the motivation for splines in FTC and some mathematics for generating basic quintic splines. The author hopes this will help further the proliferation of advanced motion planning techniques in FTC.