03 April 2020
[ robotics , math ]

A B-spline is a piecewise polynomial parameterized by a set of control points, a knot vector. The key to the b-spline representation is that it has the convex hull property, which allows the higher order derivatives (velocity, acceleration, jerk, etc) to be checked against dynamic feasibility subject to higher order derivative constraints. B-spline representations implicitly guarantee continuity between higher order derivatives between subsequent control points, which is a helpful property for concatenating sequential b-splines.

Definition. Value of a B-spline of degree $k{-}1$ is given by:

\gamma(t) = \sum_{i=0}^n P_i B_{i,k}(t)

where $P_i$ are the control points $i$ , $i \in [0, \dots, n]$ for $n$ control points, and $B_{i, k}(t)$ are the basis functions that can be computed using the DeBoor-Cox recursive formula. These basis functions blend the control points.

For a uniform b-spline, $\Delta t$ is a fixed constant value. For a trajectory that is continuous up to snap (4th derivative of position), we would require quintic splines.

At time $t \in [t_i, t_{i{+}1}]$ , the value of $\gamma(t)$ for a $k{-}1$ b-spline depends only on $k$ local control points $\left[t_{i{-}\left(\lfloor\frac{k}{2}\rfloor -1 \right)}, \; t_{i{+}\lfloor\frac{k}{2}\rfloor}\right]$ . For a quintic b-spline, $\gamma(t)$ would depend on 6 control points:

\begin{bmatrix} t_{i{-}2}, & t_{i{-}1}, & t_{i}, & t_{i{+}1}, & t_{i{+}2}, & t_{i{+}3} \end{bmatrix}

The blending functions $B_{i, k}(t)$ are only nonzero for $i{-}2, i{-}1, i, i{+}1, i{+}2, i{+}3$ .

Evaluation. To simplify calculations, we transform time $t$ to a uniform representation $s(t) = \frac{(t-t_0)}{\Delta t}$ . Then, the control points transform into $i \in [0, ..., n]$ . The time elapsed since the start of the segment is given by $u(t) = s(t) - i$ .

The B-spline curve of $k{-}1$ degree can be evaluated using the DeBoor-Cox formula:

\gamma_i(u(t)) = \mathbf{b}_{k}^\top\mathbf{M}_{k} \mathbf{P}_i

For a quintic B-spline:

\begin{aligned} \gamma_i(u(t)) & = \mathbf{b}_6^\top\mathbf{M}_6 \mathbf{P}_i\\ & = \begin{bmatrix} 1 & u & u^2 & u^3 & u^4 & u^5 \end{bmatrix} \mathbf{M}_6 \begin{bmatrix} P_{i{-}2} \\ P_{i{-}1} \\ P_{i} \\ P_{i{+}1} \\ P_{i{+}2} \\ P_{i{+}3}\end{bmatrix}\end{aligned}

where

\begin{aligned} \mathbf{M}_6 = \frac{1}{5!} \begin{bmatrix} 1 &26 &66 &26 &1 &0 \\ -5 &-50 &0 &50 &5 &0 \\ 10&20 &-60 &20 &10 &0 \\ -10&20 &0 &-20 &10 &0 \\ 5&-20 &30 &-20 &5 &0 \\ -1&5 &-10 &10 &-5 &1 \end{bmatrix}\end{aligned}

The $d$ -th time derivative of a $k{-}1$ degree b-spline is given by:

\frac{\partial\gamma(u(t))^d}{\partial^dt} = \gamma^{(d)} (u(t)) = \frac{1}{\Delta t^d} {\mathbf{b}_k^{(d)}}^\top\mathbf{M}_k \mathbf{P}_i

E.g. for a quintic b-spline, its first and second derivatives are:

\begin{aligned} \dot \gamma(u(t)) & = \frac{1}{\Delta t} \begin{bmatrix} 0 & 1 & 2u & 3u^2 & 4 u^3 & 5u^4 \end{bmatrix}^\top\mathbf{M}_6 \mathbf{P}_i \qquad \text{velocity}\\ \ddot \gamma(u(t)) &= \frac{1}{\Delta t^2} \begin{bmatrix} 0 & 0 & 2 & 6u & 12u^2 & 12u^2 & 20u^3\end{bmatrix}^\top\mathbf{M}_6 \mathbf{P}_i \qquad \text{acceleration}\end{aligned}

The derivatives of a b-spline is also a b-spline. The control points for higher order derivatives (velocity, acceleration) are:

V_i = \frac{1}{\Delta t} (P_{i{+}1} - P_i) \qquad \qquad A_i = \frac{1}{\Delta t} (V_{i{+}1} - V_i)

Integral over squared time derivatives. The local computation of the integral over squared time derivatives of degree $d$ for a $k{-}1$ degree b-spline is given by:

\begin{aligned} E^d_{i,k} & = \int_{t_i}^{t_{i{+}1}} {\biggl( \gamma^{(d)} (u(t))\biggr)}^2 dt\\ & = \int_{t_i}^{t_{i{+}1}} \frac{1}{\Delta t^d}\biggl( {\mathbf{b}_k^{(d)}}^\top\mathbf{M}_k {\mathbf{P}_i\biggr)}^\top\frac{1}{\Delta t^d} \biggl( {\mathbf{b}_k^{(d)}}^\top\mathbf{M}_k \mathbf{P}_i \biggr) dt\\ & = \int_{t_i}^{t_{i{+}1}} \frac{1}{\Delta t^{2d}}\biggl( {\mathbf{P}_i }^\top{\mathbf{M}_k}^\top\mathbf{b}_k^{(d)} {\mathbf{b}_k^{(d)}} ^\top\mathbf{M}_k \mathbf{P}_i \biggr) dt\end{aligned}

Rearrange the equation, since $\mathbf{P}_i$ and $\mathbf{M}_k$ does not depend on $t$ :

\begin{aligned} E^d_{i,k} & = \mathbf{P}_i^\top\mathbf{M}_k^\top\biggl( \int_{t_i}^{t_{i{+}1}} \frac{1}{\Delta t^{2d}} \mathbf{b}_k^{(d)} {\mathbf{b}_k^{(d)}}^\top dt \biggr)\mathbf{M}_k \mathbf{P}_i \\ & = \mathbf{P}_i^\top\mathbf{M}_k^\top\biggl(\frac{1}{\Delta t^{2d}} \int_{t_i}^{t_{i{+}1}} \mathbf{b}_k^{(d)} {\mathbf{b}_k^{(d)}}^\top dt \biggr)\mathbf{M}_k \mathbf{P}_i \end{aligned}

We perform a change of integrals from $t$ to $u$ . Since

\begin{aligned} u &= s(t) - i\\ &= \left(\frac{t-t_0}{\Delta t}\right) - i\end{aligned}

Then

\begin{aligned} \frac{d u}{d t}& = \frac{1}{\Delta t}\\ dt& = \Delta t \, du\end{aligned}

Then the change of integral becomes

\begin{aligned} E^d_{i,k} & = \mathbf{P}_i^\top\mathbf{M}_k^\top\biggl(\frac{1}{\Delta t^{2d}} \int_{u=0}^{u=1} \mathbf{b}_k^{(d)} {\mathbf{b}_k^{(d)}}^\top\Delta t \, du \biggr)\mathbf{M}_k \mathbf{P}_i \\ & = \mathbf{P}_i ^\top\mathbf{M}_k ^\top\biggl(\frac{\Delta t}{\Delta t^{2d}} \int_{u=0}^{u=1} \mathbf{b}_k^{(d)} {\mathbf{b}_k^{(d)}}^\top du \biggr)\mathbf{M}_k \mathbf{P}_i \\ & = \mathbf{P}_i ^\top\mathbf{M}_k ^\top\biggl(\underbrace{\frac{1}{\Delta t^{(2d-1)}} \int_{u=0}^{u=1} \mathbf{b}_k^{(d)} {\mathbf{b}_k^{(d)}}^\top du }_{\mathbf{Q}^d}\biggr)\mathbf{M}_k \mathbf{P}_i \end{aligned}

\begin{aligned} \mathbf{Q}^d_k = \frac{1}{\Delta t^{(2d-1)}} \int_{0}^{1} \left(\frac{d\mathbf{b}_k^{(d)}}{d^{(d)}u} \right) \left(\frac{d\mathbf{b}_k^{(d)}}{d^{(d)}u}\right)^\top du\end{aligned}

For a quintic b-spline, the integral over squared acceleration can be computed as follows,

\begin{aligned} \mathbf{Q}^2_k = \frac{1}{\Delta t^{3}} \int_{0}^{1} \left(\frac{d\mathbf{b}_6^{2}}{d^{2}u} \right) \left(\frac{d\mathbf{b}_6^{2}}{d^{2}u}\right)^\top du &= \frac{1}{\Delta t^{3}} \begin{bmatrix} 0 &0 &0 &0 &0 &0 \\ 0 &0 &0 &0 &0 &0 \\ 0 &0 &8 &12 &16 &20 \\ 0 &0 &12 &24 &36 &48 \\ 0 &0 &16 &36 &57.6 &80 \\ 0 &0 &20 &48 &80 &114.286 \end{bmatrix}\end{aligned}

For a weighted integral over multiple squared time derivatives:

\begin{aligned} E_{i,k}^D &= \sum_{d=1}^D w_d E^d_{i,k}\\ & = \sum_{d=1}^D w_d \mathbf{P}_i^\top\mathbf{M}_k^\top\mathbf{Q}^d_k \mathbf{M}_k \mathbf{P}_i\end{aligned}

Convex Hull Property. Span of a b-spline $\gamma_i(u(t))$ lies within the convex hull of its local control points. This extends to its higher order derivatives, therefore feasibility check of:

\left\lVert\dot \gamma\right\rVert < v_\text{max} \qquad \left\lVert\ddot \gamma\right\rVert < a_\text{max}

can be satisfied by checking the control points:

V_i < v_\text{max} \qquad A_i < a_\text{max}

References

Vladyslav Usenko, Lukas von Stumberg, Andrej Pangercic, and Daniel Cremers. Real-time trajectory replanning for mavs using uniform b-splines and a 3d circular buffer. In 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pages 215–222. IEEE, 2017.
Wenchao Ding, Wenliang Gao, Kaixuan Wang, and Shaojie Shen. An efficient b-spline-based kinodynamic replanning framework for quadrotors. IEEE Transactions on Robotics, 35(6):1287–1306, 2019.
Boyu Zhou, Fei Gao, Luqi Wang, Chuhao Liu, and Shaojie Shen. Robust and efficient quadrotor trajectory generation for fast autonomous flight. IEEE Robotics and Automation Letters, 2019.