Fast Approximation of Over-Determined Second-Order Linear Boundary Value Problems by Cubic and Quintic Spline Collocation

Abstract

We present an efficient and generic algorithm for approximating second-order linear boundary value problems through spline collocation. In contrast to the majority of other approaches, our algorithm is designed for over-determined problems. These typically occur in control theory, where a system, e.g., a robot, should be transferred from a certain initial state to a desired target state while respecting characteristic system dynamics. Our method uses polynomials of maximum degree three/five as base functions and generates a cubic/quintic spline, which is ${\mathcal{C}}^2$/${\mathcal{C}}^4$ continuous and satisfies the underlying ordinary differential equation at user-defined collocation sites. Moreover, the approximation is forced to fulfill an over-determined set of two-point boundary conditions, which are specified by the given control problem. The algorithm is suitable for time-critical applications, where accuracy only plays a secondary role. For consistent boundary conditions, we experimentally validate convergence towards the analytic solution, while for inconsistent boundary conditions our algorithm is still able to find a “reasonable” approximation. However, to avoid divergence, collocation sites have to be appropriately chosen. The proposed scheme is evaluated experimentally through comparison with the analytical solution of a simple test system. Furthermore, a fully documented C++ implementation with unit tests as example applications is provided.

1. Introduction

1.1. Literature Review

In almost every field of natural science and engineering we face differential equations, which are typically used for modeling dynamical systems. Especially in engineering, the more specific case of boundary value problems (BVP) is very prominent. In other words, one often searches for the behavior of the investigated system between some kind of fixed, i.e., known, spatial or temporal boundaries. One can try to derive the analytical solution to the problem, however for complex systems this can be difficult or even impossible. In such cases, it can be sufficient to approximate the solution for which a variety of techniques exists.

Methods based on finite differences approximate the derivatives by difference quotients to obtain a system of equations which depends only on the primal function. This allows a solution to be found by formulating a linear system of (algebraic) equations representing the transformed system at certain grid points. In contrast, shooting methods aim at the iterative solution of an equivalent initial value problem (IVP), which is typically easier to handle than the original BVP. While for single shooting the IVP is evaluated over the complete time interval, multiple shooting considers a partitioned time domain. Another technique is given by the finite element approach, which is mainly used for partial differential equations (PDE) as they occur, for example, in structural-, thermo-, and fluid dynamics. Typically finite element methods are based on a weak formulation of the residual and on splitting the considered domain into elements on which the local base functions to approximate the solution are defined. Although designed for PDEs, they can be applied to the simpler case of ordinary differential equations (ODE), which are the focus of this contribution (see, for example, [1]). A variety of other techniques exist. However, these three groups appear to be used the most in the field of engineering.

In the following, we approximate the solution through spline collocation using piecewise polynomial trial functions, which is a well-known technique to solve BVPs, see, for example, [2,3,4]. Over the past decades, various algorithms emerged, which can be classified into two types. The first type, also known as smoothest spline collocation (or just spline collocation as in [5]), aims at matching the differential equation at one collocation site per spline segment, which is typically a knot or the mid-point of the segment, while simultaneously forcing the spline to have maximum smoothness, i.e., highest possible continuity [5]. The second type, in [5] called Gaussian collocation, removes the constraints on higher order continuity and instead uses more collocation sites, which are typically chosen to be the Gaussian points of each segment. This class, which is also called orthogonal spline collocation [6], originates from [3] and aims at maximizing the order of convergence. In order to also provide a competitive convergence for the first type of methods, special variants for quadratic and quintic spline collocation have been proposed in [7] and [8], respectively. Optimal methods for quadratic and cubic splines on non-uniform grids, i.e., for an inhomogeneous segmentation of the spline, have been presented in [5].

Note that in addition to general purpose codes, such as COLSYS (FORTRAN) for non-linear mixed-order systems of multi-point boundary value ODEs [9], highly specialized algorithms, which aim at obtaining the best possible approximation for certain use cases, have also been published. Recent examples are methods for integro-differential equations [10] or fractional differential equations [11], which occur in certain material models exhibiting memory effects. Along with ODEs, various kinds of (multi-variable) PDEs have been investigated. See [6] for a survey on corresponding Gaussian collocation methods. Note that, for PDEs, the time domain is typically discretized using finite differences, e.g., by the Crank–Nicholson approach as used in [6], or the second-order backward difference in [10], while spline collocation is used for approximating the spacial variables. Lastly, methods for differential algebraic equations (DAE) have also been developed, e.g., the COLSYS extension COLDAE [12] for semi-explicit DAEs of index 2 and fully implicit DAEs of index 1.

In our opinion, despite the variety of techniques, there is still a lack of simple methods prioritizing execution time over approximation quality, which is essential for time critical control applications. The aim of this contribution is to provide an algorithm satisfying these needs while focusing on the special case of second-order linear ODEs, which are very common for dynamic systems. Moreover, we focus on over-determined BVPs, i.e., where more boundary conditions (BC) are given than necessary. This may at first seem to be a restriction of our algorithm since it needs more information than other implementations; however, it allows us to also consider inconsistent BVPs, i.e., the case where no exact solution to the problem exists.

1.2. Motivation

It might appear strange to search for an approximation of something that actually does not exist. However, we face exactly this situation during walking pattern generation for our humanoid robot LOLA, cf. Figure 1 left. In particular, we use a simplified model of the robot’s multi-body dynamics, cf. Figure 1 right, to plan the center of mass (CoM) motion over a certain time horizon. The planned CoM motion resembles the dynamics of the model, which can be formulated as second-order linear ODE, and is constrained to certain values on position-, velocity- and acceleration-level at the boundaries of the planning horizon. This leads to an overdetermined BVP of the type investigated in this contribution. The BCs at the beginning reflect the current state of the robot, e.g., the standing pose in front of a platform, while the BCs at the end represent the target state, e.g., the standing pose on the other side of the obstacle, cf. Figure 2. For a seamless motion of the robot it is crucial to guarantee the satisfaction of the BCs, i.e., a perfect match of the boundary states. In contrast, it is sufficient to approximate the dynamics of the underlying ODE since it is derived from a simplified model which is an approximation in itself. This is the key idea behind the formulation of an over-determined BVP, which may thus not have a proper “real” solution. To the authors knowledge, all comparable algorithms assume that a solution of the BVP exists, while most of them also require it to be unique and “sufficiently” smooth.

Explaining the technical details of the walking pattern generation framework of LOLA goes beyond the scope of this paper. Instead, the interested reader is referred to [13], in which the application of the proposed algorithm is presented. The publication [13] comes with an accompanying video, see [14], which visualizes the sequential steps of the walking pattern generation pipeline of LOLA. The simulations and experiments presented in the video show the successful application of our algorithm. In the following, we do not further restrict ourselves to this special application. Since the proposed algorithm is generic, we derive it in the most general way since it may be useful also for different applications in robotics.

1.3. Additional Remarks

Our method can be seen as smoothest spline collocation, i.e., it belongs to the “first” type as classified in Section 1.1. We do not apply special techniques to increase the order of convergence, but instead adhere to its basic form. This leads to a much simpler derivation and implementation. In addition it makes the algorithm faster by sacrificing approximation quality. This complies with the needs of our target application as explained in Section 1.2. We emphasize that our focus lies on simplicity, robustness, and efficiency. Thus we will not search for a mathematical formulation of the convergence order. Instead, the algorithm is evaluated mainly through experiments, where runtime performance is our primary concern.

As summarized in Section 1.1, there exist numerous methods for solving linear BVPs or spline interpolation/collocation in general. For most approaches, solving a large-sparse or small-dense linear system of equations (LSE) represents the main workload. To overcome this bottleneck, parallelized algorithms have been developed, which typically exploit the special structure of the involved matrices, e.g., the scheme proposed in [15] for staircase matrix structures. Although we aim at efficiency, we do not consider explicit parallelization as acceleration technique. This is because our algorithm is designed for embedded systems which typically feature only few physical CPU cores running also other time-critical tasks. Moreover, the use of GPUs, e.g., through CUDA or OPENCL, is often not feasible since the CPU–GPU interface lacks capabilities for hard real-time requirements. Finally, dedicated to our target application, we only consider (comparatively) small problems with runtimes $\approx 1\mathrm{m}\mathrm{s}$. For such systems the performance boost obtained through parallelization is likely to be canceled out by the synchronization overhead. Nevertheless, our algorithm may also be used for large scale problems. In this case an “off the shelf” parallel solver for dense LSEs may be used, cf. [16]. However, one should keep in mind that by using an iterative scheme execution time is not deterministic anymore, and, even worse, the solver might not converge. As an alternative, there exists an efficient way for the parallel solution of decoupled, multi-dimensional BVPs, which takes advantage of intermediate results, see Appendix C for details.

2. Materials and Methods

In the following, two versions of our algorithm are presented: one using a cubic spline and the other using a quintic spline for approximating the BVP. As the derivations and resulting algorithms are similar, we show the connecting links by presenting both methods in parallel. The version based on cubic splines is naturally simpler, although, using quintic splines leads to a smoother approximation. Indeed it is ${\mathcal{C}}^4$- instead of ${\mathcal{C}}^2$-continuous, which can be preferable for some use cases. Moreover, quintic splines allow us to directly preset first and second-order derivatives at both boundaries, which otherwise requires the introduction of virtual control points and in turn can lead to poor results, as discussed in Section 3. Although deriving the proposed collocation algorithm for quintic splines is more advanced than its cubic counterpart, overall performance is superior, because the same approximation quality can be obtained with less collocation sites and hence with less computational effort as shown in Section 3. However, this requires the underlying ODE to be sufficiently smooth. Note that, in contrast to our intention, most other investigations choose quintic splines for approximating fourth-order ODEs, e.g., in [8,10,11,17] (similar to choosing cubic splines for second-order ODEs).

We highlight that the proposed method is not only inspired by, but is also heavily based on the interpolation and collocation algorithms presented in [18,19], respectively. The main contribution of this paper is the combination, extension and runtime optimization of those methods. Moreover, we provide a detailed and self-contained derivation together with a fully documented open-source C++ reference implementation.

Having a background in mechanical engineering, the authors intention is to present a simple and self-contained derivation of the proposed algorithm, which can be easily understood, implemented, and extended by readers also lacking a dedicated mathematical background.

2.1. Problem Statement

Consider the second-order linear time-variant ODE

$$\alpha \left(t\right)\ddot{F}\left(t\right)+\beta \left(t\right)\dot{F}\left(t\right)+\gamma \left(t\right)F\left(t\right)=\tau \left(t\right)\text{for}t\in \left[t_0,t_n\right]$$

(1)

where $\dot{F}\left(t\right)$ and $\ddot{F}\left(t\right)$ denote the first and second derivative of the unknown function $F\left(t\right)$ with respect to time t, i.e.,

$$\dot{F}\left(t\right)=\frac{dF\left(t\right)}{dt}and\ddot{F}\left(t\right)=\frac{d^{2}F\left(t\right)}{d{t}^2}.$$

Note that t does not have to represent time. However, this synonym is used in the following due to the typical appearance of (1) in dynamical systems. The coefficients $\alpha$, $\beta$, $\gamma$, and the right-hand side $\tau$ are arbitrary, in general nonlinear, but known functions of t. Let system (1) be constrained by the BCs

$$\begin{array}{cccccc}\hfill F\left(t_0\right)& =F_0,\hfill & \hfill \dot{F}\left(t_0\right)& ={\dot{F}}_0,\hfill & \hfill \ddot{F}\left(t_0\right)& ={\ddot{F}}_0,\hfill \\ \hfill F\left(t_n\right)& =F_n,\hfill & \hfill \dot{F}\left(t_n\right)& ={\dot{F}}_n,\hfill & \hfill \ddot{F}\left(t_n\right)& ={\ddot{F}}_n,\hfill \end{array}$$

(2)

where $t_0$ and $t_n$ define the considered time interval $t\in [t_0,t_n]$ and $F_0$, ${\dot{F}}_0$, ${\ddot{F}}_0$, $F_n$, ${\dot{F}}_n$, ${\ddot{F}}_n$ are user-defined constants. Then system (1) together with the BCs (2) represents the second-order linear two-point BVP for which an approximation is to be found. Note that (2) considers Dirichlet, Neumann, and second-order BCs independently of each other. In contrast, various other algorithms assume Robin BCs, i.e., a linear combination of Dirichlet and Neumann BCs, which is not equivalent to our approach. Due to (2), the BVP is over-determined and the existence of a solution $F\left(t\right)$ depends on the consistency of the BCs with the ODE.

In the following, we investigate the approximation of $F\left(t\right)$ through spline collocation, i.e., we generate a spline $y\left(t\right)$ which satisfies the underlying ODE (1) at a user-defined set of distinct collocation sites $\left\{t_k\right\}$, numbered in increasing order, which lie within the considered interval $(t_0,t_n)$, i.e.,

$$\alpha \left(t_k\right)\ddot{y}\left(t_k\right)+\beta \left(t_k\right)\dot{y}\left(t_k\right)+\gamma \left(t_k\right)y\left(t_k\right)=\tau \left(t_k\right)\text{for}{t}_0<t_k<t_{k+1}<t_n.$$

(3)

Moreover, $y\left(t\right)$ is forced to fulfill the BCs (2) at $t_0$ and $t_n$, i.e.,

$$\begin{array}{cccccc}\hfill y\left(t_0\right)& =y_0=F_0,\hfill & \hfill \dot{y}\left(t_0\right)& ={\dot{y}}_0={\dot{F}}_0,\hfill & \hfill \ddot{y}\left(t_0\right)& ={\ddot{y}}_0={\ddot{F}}_0,\hfill \\ \hfill y\left(t_n\right)& =y_n=F_n,\hfill & \hfill \dot{y}\left(t_n\right)& ={\dot{y}}_n={\dot{F}}_n,\hfill & \hfill \ddot{y}\left(t_n\right)& ={\ddot{y}}_n={\ddot{F}}_n.\hfill \end{array}$$

(4)

Here we use $y\left(t\right)$ to denote the approximating spline while the exact solution is represented by $F\left(t\right)$. For clarity, we also use different denominations for $\left\{t_k\right\}$ and $\left\{y_k\right\}$ by using the terms collocation sites and collocation points, respectively. While $\left\{t_k\right\}$ are user-defined parameters, $\left\{y_k\right\}$ describe the solution to be found.

Since the proposed collocation algorithm is strongly related to the interpolation of cubic/quintic splines, which may not be common to some readers, spline interpolation is recapitulated in Section 2.3. Then, the proposed collocation method is derived in Section 2.7. Moreover, we reuse core elements of the interpolation algorithm during collocation, thus we cannot omit its derivation.

2.2. Spline Parametrization

Before diving into the derivation of algorithms, one first has to decide which spline representation to use. In the literature, formulations such as basis splines (B-splines) are common, since they feature inherent continuity and local control, which typically leads to banded systems [20]. In general, B-splines do not pass through their control points, which seems to make interpolation difficult at first sight; however, efficient algorithms for interpolation and collocation exist, see, for example, [21]. In [20], it has been shown that B-splines might not be as stable and efficient as other representations, namely monomial and Hermite type bases, especially when it comes to implementation. In particular, monomial bases have been recommended due to their superior condition, and thus lower roundoff errors. For all three forms, B-spline, Hermite type, and monomial, the core operation during Gaussian collocation is typically the solution of an almost block diagonal (ABD) linear system of equations [6,21]. A generic solver for these type of systems is SOLVEBLOK [22], while the special structure occurring for monomial bases is exploited by ABDPACK [23] with increased speed and lower memory consumption. Unfortunately, for smoothest spline collocation as presented in the following, the corresponding collocation matrix is dense, thus we cannot apply these algorithms. However, when compared to Gaussian collocation, the count of collocation sites and thus the dimension of the corresponding LSE is much smaller, which can lead to comparable performance.

Despite the popularity of B-splines, we use the so-called piecewise polynomial (PP) form [21], which describes the spline through the coefficients of interconnected, but independently defined, polynomial segments. We use a special type of monomial bases, namely the canonical form of the polynomials, which may not be as efficient as the choice in [20]; however, it makes our algorithm much simpler. By using this formulation, continuity between the spline segments needs to be explicitly established. The evaluation of the resulting spline, however, boils down to the evaluation of a single polynomial belonging to the corresponding segment, which is in general much quicker than evaluating the equivalent B-spline form. The evaluation of B-splines of degree p with the well-known de Boor’s algorithm [24] takes $\mathcal{O}\left(p^2\right)+\mathcal{O}\left(p\right)$ operations [25]. There are optimized versions of it as proposed in [25,26]; however, these are numerically less stable [27]. In contrast, evaluating a polynomial of degree p with Horner’s method [28] takes only $2p$, i.e., $\mathcal{O}\left(p\right)$, operations [29]. This is essential for time-critical applications, where the resulting spline has to be evaluated as quickly as possible. Note that we are free to construct the spline in B-spline formulation and convert it to the corresponding PP form in a post-processing step, see [21]. However, this introduces an additional (expensive) step which we try to avoid since, in our case, not only the evaluation but also the construction of the spline is time critical.

Let the spline $y\left(t\right)$ be defined as

$$y\left(t\right)=s_i\left({\xi }_i\left(t\right)\right)\text{for}{t}_0\le t_i\le t<t_{i+1}\le t_n\text{with}i=0,\cdots ,n-1,$$

(5)

where $s_i$ represents the i-th of the $n>1$ spline segments parametrized by the normalized interpolation parameter ${\xi }_i$. We call $t\in [t_0,t_n]$ and ${\xi }_i\in [0,1]$ the global and local interpolation parameters, respectively, for which we choose the linear mapping

$${\xi }_i\left(t\right)=\frac{1}{h_i}t-\frac{t_i}{h_i}=\left(t-t_i\right)g_i$$

(6)

with $h_i>0$ as the duration of the i-th segment $h_i=t_{i+1}-t_i$ and its reciprocal $g_i=1/h_i$. The partitioning of the spline into n segments is visualized in Figure 3. In the following, we predominantly derive expressions in local segment space, i.e., with respect to ${\xi }_i$, since this makes the notation clearer, especially in Section 2.7. Note that, in contrast to some other approaches, we do not require homogeneous partitioning, thus the segmentation can be chosen arbitrarily as long as spline knots do not coincide. However, in Section 2.3, we show that for best numerical stability uniform partitioning should be used.

In the case of cubic splines (left subscript C), each segment ${}_{\mathrm{C}}{s}_i$ represents a polynomial of degree three,

$${}_{\mathrm{C}}{s}_i\left({\xi }_i\right)={}_{\mathrm{C}}{a}_i{\xi }_i^3+{}_{\mathrm{C}}{b}_i{\xi }_i^2+{}_{\mathrm{C}}{\mathrm{C}}_i{\xi }_i+{}_{\mathrm{C}}{d}_i\text{with}i=0,\cdots ,n-1$$

(7)

where ${}_{\mathrm{C}}{a}_i$, ${}_{\mathrm{C}}{b}_i$, ${}_{\mathrm{C}}{\mathrm{C}}_i$, and ${}_{\mathrm{C}}{d}_i$ are its constant coefficients. The first two derivatives of ${}_{\mathrm{C}}{s}_i\left({\xi }_i\left(t\right)\right)$ with respect to t are obtained by applying the chain rule:

$$\begin{array}{cc}\hfill {}_{\mathrm{C}}{\dot{s}}_i\left({\xi }_i\right)& =\left(\frac{d{}_{\mathrm{C}}{s}_i}{dt}\right)=\left(\frac{\partial {}_{\mathrm{C}}{s}_i}{\partial {\xi }_i}\right)\left(\frac{d{\xi }_i}{dt}\right)=\frac{3}{h_i}{}_{\mathrm{C}}{a}_i{\xi }_i^2+\frac{2}{h_i}{}_{\mathrm{C}}{b}_i{\xi }_i+\frac{1}{h_i}{}_{\mathrm{C}}{\mathrm{C}}_i,\hfill \end{array}$$

(8)

$$\begin{array}{cc}\hfill {}_{\mathrm{C}}{\ddot{s}}_i\left({\xi }_i\right)& =\left(\frac{d^2{}_{\mathrm{C}}{s}_i}{d{t}^2}\right)=\left(\frac{{\partial }^2{}_{\mathrm{C}}{s}_i}{\partial {\xi }_i^2}\right){\left(\frac{d{\xi }_i}{dt}\right)}^2+\left(\frac{\partial {}_{\mathrm{C}}{s}_i}{\partial {\xi }_i}\right)\underset{=0}{\underbrace{\left(\frac{d^2{\xi }_i}{d{t}^2}\right)}}=\frac{6}{h_i^2}{}_{\mathrm{C}}{a}_i{\xi }_i+\frac{2}{h_i^2}{}_{\mathrm{C}}{b}_i.\hfill \end{array}$$

(9)

For quintic splines (left subscript Q), each segment ${}_{\mathrm{Q}}{s}_i$ represents a polynomial of degree five,

$${}_{\mathrm{Q}}{s}_i\left({\xi }_i\right)={}_{\mathrm{Q}}{a}_i{\xi }_i^5+{}_{\mathrm{Q}}{b}_i{\xi }_i^4+{}_{\mathrm{Q}}{\mathrm{C}}_i{\xi }_i^3+{}_{\mathrm{Q}}{d}_i{\xi }_i^2+{}_{\mathrm{Q}}{e}_i{\xi }_i+{}_{\mathrm{Q}}{f}_i\text{with}i=0,\cdots ,n-1$$

(10)

where ${}_{\mathrm{Q}}{a}_i$, ${}_{\mathrm{Q}}{b}_i$, ${}_{\mathrm{Q}}{\mathrm{C}}_i$, ${}_{\mathrm{Q}}{d}_i$, ${}_{\mathrm{Q}}{e}_i$, and ${}_{\mathrm{Q}}{f}_i$ are its constant coefficients. As in the cubic case, we obtain the first four derivatives of ${}_{\mathrm{Q}}{s}_i\left({\xi }_i\left(t\right)\right)$ with respect to t through the chain rule:

$$\begin{array}{cc}\hfill {}_{\mathrm{Q}}{\dot{s}}_i\left({\xi }_i\right)& =\left(\frac{d{}_{\mathrm{Q}}{s}_i}{dt}\right)=\frac{5}{h_i}{}_{\mathrm{Q}}{a}_i{\xi }_i^4+\frac{4}{h_i}{}_{\mathrm{Q}}{b}_i{\xi }_i^3+\frac{3}{h_i}{}_{\mathrm{Q}}{\mathrm{C}}_i{\xi }_i^2+\frac{2}{h_i}{}_{\mathrm{Q}}{d}_i{\xi }_i+\frac{1}{h_i}{}_{\mathrm{Q}}{e}_i,\hfill \end{array}$$

(11)

$$\begin{array}{cc}\hfill {}_{\mathrm{Q}}{\ddot{s}}_i\left({\xi }_i\right)& =\left(\frac{d^2{}_{\mathrm{Q}}{s}_i}{d{t}^2}\right)=\frac{20}{h_i^2}{}_{\mathrm{Q}}{a}_i{\xi }_i^3+\frac{12}{h_i^2}{}_{\mathrm{Q}}{b}_i{\xi }_i^2+\frac{6}{h_i^2}{}_{\mathrm{Q}}{\mathrm{C}}_i{\xi }_i+\frac{2}{h_i^2}{}_{\mathrm{Q}}{d}_i,\hfill \end{array}$$

(12)

$$\begin{array}{cc}\hfill {}_{\mathrm{Q}}{s}_i^{\left(3\right)}\left({\xi }_i\right)& =\left(\frac{d^3{}_{\mathrm{Q}}{s}_i}{d{t}^3}\right)=\frac{60}{h_i^3}{}_{\mathrm{Q}}{a}_i{\xi }_i^2+\frac{24}{h_i^3}{}_{\mathrm{Q}}{b}_i{\xi }_i+\frac{6}{h_i^3}{}_{\mathrm{Q}}{\mathrm{C}}_i,\hfill \end{array}$$

(13)

$$\begin{array}{cc}\hfill {}_{\mathrm{Q}}{s}_i^{\left(4\right)}\left({\xi }_i\right)& =\left(\frac{d^4{}_{\mathrm{Q}}{s}_i}{d{t}^4}\right)=\frac{120}{h_i^4}{}_{\mathrm{Q}}{a}_i{\xi }_i+\frac{24}{h_i^4}{}_{\mathrm{Q}}{b}_i.\hfill \end{array}$$

(14)

2.3. Spline Interpolation: Preliminaries

In the following, we recall how a given set of $n+1$ data points $\left\{(t_i,y_i)\right\}$ with $i=0,\cdots ,n$ can be interpolated with a ${\mathcal{C}}^2$ or ${\mathcal{C}}^4$ smooth cubic or quintic spline, respectively. The derivation explicitly uses the PP form of the spline and leads to the same algorithm as presented in [18], except for slight modifications in notation. Note that [18] only deals with quintic splines. However, the method for cubic segments presented in this paper is a simplified version of the same scheme. Moreover, the derivation is investigated in more detail than it is in [18]. Readers not interested in these details are referred to Algorithm 1 which summarizes the results of this section.

In contrast to [18], we only consider the case of predefined first- and second-order derivatives at the boundaries of the quintic spline, i.e., we assume ${\dot{y}}_0$, ${\ddot{y}}_0$, ${\dot{y}}_n$, and ${\ddot{y}}_n$ to be given (as indicated in Figure 3). For the cubic counterpart, we lose two degrees of freedom, allowing us to predefine only two constraints out of $\{{\dot{y}}_0,{\ddot{y}}_0,{\dot{y}}_n,{\ddot{y}}_n\}$. For the remainder of this section, we restrict ourselves to the case of predefined second-order time derivatives ${\ddot{y}}_0$ and ${\ddot{y}}_n$ as this allows an efficient algorithm similar to the one presented for quintic splines. Note that this choice includes the common case of natural cubic splines, i.e., ${\ddot{y}}_0={\ddot{y}}_n=0$. For cubic splines, we postpone the enforcement of the remaining boundary conditions, i.e., ${\dot{y}}_0$ and ${\dot{y}}_n$, to the end of Section 2.7.

In the following, we only consider distinct and ascending interpolation sites, i.e., $t_0\le t_i<t_{i+1}\le t_n$ for $i=0,\cdots ,n-1$.

2.4. Cubic Spline Interpolation: Derivation

Since our goal is a spline which passes through the given data points $\left\{(t_i,y_i)\right\}$, we enforce the interpolation constraints

$$\begin{array}{ccc}\hfill {}_{\mathrm{C}}{s}_i\left({\xi }_i\right){|}_{{\xi }_i=0}& \stackrel{!}{=}{y}_i\hfill & \hfill \text{for }i=0,\cdots ,n-1(n\text{ eqs}.),\\ \hfill {}_{\mathrm{C}}{s}_i\left({\xi }_i\right){|}_{{\xi }_i=1}& \stackrel{!}{=}{y}_{i+1}\hfill & \hfill \text{for }i=0,\cdots ,n-1(n\text{ eqs}.).\end{array}$$

(15)

Furthermore, we aim at ${\mathcal{C}}^2$ continuity, thus we further require that

$$\begin{array}{ccc}\hfill {}_{\mathrm{C}}{\dot{s}}_i\left({\xi }_i\right){|}_{{\xi }_i=1}& \stackrel{!}{=}{}_{\mathrm{C}}{\dot{s}}_{i+1}\left({\xi }_{i+1}\right){|}_{{\xi }_{i+1}=0}={\dot{y}}_{i+1}\hfill & \hfill \text{for }i=0,\cdots ,n-2(n-1\text{ eqs}.),\\ \hfill {}_{\mathrm{C}}{\ddot{s}}_i\left({\xi }_i\right){|}_{{\xi }_i=1}& \stackrel{!}{=}{}_{\mathrm{C}}{\ddot{s}}_{i+1}\left({\xi }_{i+1}\right){|}_{{\xi }_{i+1}=0}={\ddot{y}}_{i+1}\hfill & \hfill \text{for }i=0,\cdots ,n-2(n-1\text{ eqs}.).\end{array}$$

(16)

Inserting (7) and (9) into (15) and in the second row of (16) allows us to reformulate the spline coefficients as

$$\begin{array}{ccccc}\hfill {}_{\mathrm{C}}{a}_i& =& & -\frac{1}{6}{\ddot{y}}_i{h}_i^2& +\frac{1}{6}{\ddot{y}}_{i+1}{h}_i^2,\\ \hfill {}_{\mathrm{C}}{b}_i& =& & +\frac{1}{2}{\ddot{y}}_i{h}_i^2,\\ \hfill {}_{\mathrm{C}}{\mathrm{C}}_i& =& -y_i& +y_{i+1}& -\frac{1}{3}{\ddot{y}}_i{h}_i^2& -\frac{1}{6}{\ddot{y}}_{i+1}{h}_i^2,\\ \hfill {}_{\mathrm{C}}{d}_i& =& +y_i,& \end{array}$$

(17)

where ${\ddot{y}}_i$ for $i=1,\cdots ,n-1$ are the $n-1$ unknowns which have to be computed using the remaining $n-1$ equations given by the first row of (16). For this purpose, we expand the first row of (16) with (8) and insert ${}_{\mathrm{C}}{a}_i$, ${}_{\mathrm{C}}{b}_i$, ${}_{\mathrm{C}}{\mathrm{C}}_i$ and ${}_{\mathrm{C}}{\mathrm{C}}_{i+1}$ from (17) to obtain

$$h_{i-1}{\ddot{y}}_{i-1}+2\left(h_{i-1}+h_i\right){\ddot{y}}_i+h_i{\ddot{y}}_{i+1}=\frac{6}{h_i}\left(y_{i+1}-y_i\right)-\frac{6}{h_{i-1}}\left(y_i-y_{i-1}\right),$$

(18)

which holds for $i=1,\cdots ,n-1$. This equation can be considered as defining intermediate unknowns ${\ddot{y}}_i$ and can be written as LSE of the form

$${}_{\mathrm{C}}\mathit{A}{}_{\mathrm{C}}\mathit{X}={}_{\mathrm{C}}\mathit{B}$$

(19)

with ${}_{\mathrm{C}}\mathit{A}\in \alpha {\mathbb{R}}^{(n-1)\times (n-1)}$ and ${}_{\mathrm{C}}\mathit{X}$, ${}_{\mathrm{C}}\mathit{B}\in \alpha {\mathbb{R}}^{n-1}$ given by

$${}_{\mathrm{C}}\mathit{A}=\left[\begin{array}{ccc}{}_{\mathrm{C}}{D}_1& {}_{\mathrm{C}}{U}_1& 0\\ {}_{\mathrm{C}}{L}_2& {}_{\mathrm{C}}{D}_2& {}_{\mathrm{C}}{U}_2\\ \ddots & \ddots & \ddots \\ {}_{\mathrm{C}}{L}_i& {}_{\mathrm{C}}{D}_i& {}_{\mathrm{C}}{U}_i\\ \ddots & \ddots & \ddots \\ {}_{\mathrm{C}}{L}_{n-2}& {}_{\mathrm{C}}{D}_{n-2}& {}_{\mathrm{C}}{U}_{n-2}\\ 0& {}_{\mathrm{C}}{L}_{n-1}& {}_{\mathrm{C}}{D}_{n-1}\end{array}\right],{}_{\mathrm{C}}\mathit{X}=\left[\begin{array}{c}{}_{\mathrm{C}}{X}_1\\ {}_{\mathrm{C}}{X}_2\\ ⋮\\ {}_{\mathrm{C}}{X}_i\\ ⋮\\ {}_{\mathrm{C}}{X}_{n-2}\\ {}_{\mathrm{C}}{X}_{n-1}\end{array}\right],{}_{\mathrm{C}}\mathit{B}=\left[\begin{array}{c}{}_{\mathrm{C}}{B}_1\\ {}_{\mathrm{C}}{B}_2\\ ⋮\\ {}_{\mathrm{C}}{B}_i\\ ⋮\\ {}_{\mathrm{C}}{B}_{n-2}\\ {}_{\mathrm{C}}{B}_{n-1}\end{array}\right],$$

(20)

and their elements

$$\begin{array}{ccc}\hfill {}_{\mathrm{C}}{D}_i& =2\left(h_{i-1}+h_i\right),{}_{\mathrm{C}}{X}_i={\ddot{y}}_i\hfill & \hfill \text{for }i=1,\cdots ,n-1,\end{array}$$

(21)

$$\begin{array}{ccc}\hfill {}_{\mathrm{C}}{U}_i& =h_i,{}_{\mathrm{C}}{L}_{i+1}={}_{\mathrm{C}}{U}_i\hfill & \hfill \text{for }i=1,\cdots ,n-2,\end{array}$$

(22)

$$\begin{array}{ccc}\hfill {}_{\mathrm{C}}{B}_i& =\frac{6}{h_i}\left(y_{i+1}-y_i\right)-\frac{6}{h_{i-1}}\left(y_i-y_{i-1}\right)+{}_{\mathrm{C}}{\Lambda }_i\hfill & \hfill \text{for }i=1,\cdots ,n-1,\end{array}$$

(23)

$${}_{\mathrm{C}}{\Lambda }_i=\left\{\begin{array}{ccc}-h_0{\ddot{y}}_0-h_{n-1}{\ddot{y}}_n\hfill & \text{for }n=2,\hfill & (\equiv -{}_{\mathrm{C}}{L}_1{}_{\mathrm{C}}{X}_0-{}_{\mathrm{C}}{U}_{n-1}{}_{\mathrm{C}}{X}_n)\hfill \\ -h_0{\ddot{y}}_0\hfill & \text{for }n>2\wedge i=1,\hfill & (\equiv -{}_{\mathrm{C}}{L}_1{}_{\mathrm{C}}{X}_0)\hfill \\ -h_{n-1}{\ddot{y}}_n\hfill & \text{for }n>2\wedge i=n-1,\hfill & (\equiv -{}_{\mathrm{C}}{U}_{n-1}{}_{\mathrm{C}}{X}_n)\hfill \\ 0\hfill & \text{else}\hfill \end{array}\right..$$

(24)

Herein ${}_{\mathrm{C}}{L}_i$, ${}_{\mathrm{C}}{D}_i$, and ${}_{\mathrm{C}}{U}_i\in \alpha \mathbb{R}$ represent the lower diagonal, diagonal, and upper diagonal elements of ${}_{\mathrm{C}}\mathit{A}$, respectively. Note that ${}_{\mathrm{C}}\mathit{A}$ only depends on the choice of $\left\{t_i\right\}$, thus it can be reused in case we need to perform further interpolations with the same segmentation $\left\{t_i\right\}$. While ${}_{\mathrm{C}}\mathit{A}$ represents the interpolation sites $\left\{t_i\right\}$, the interpolation points $\left\{y_i\right\}$ are encoded in ${}_{\mathrm{C}}\mathit{B}$. The additional term ${}_{\mathrm{C}}{\Lambda }_i$ incorporates the BCs such that we can write the system in the neat form of (20). From the solution ${}_{\mathrm{C}}\mathit{X}$ we can directly obtain the unknowns ${\ddot{y}}_i$ which in turn can be used together with the data points $\left\{(t_i,y_i)\right\}$ and BCs to compute the segment coefficients (17) such that the spline $y\left(t\right)$ is fully defined. Note that ${}_{\mathrm{C}}\mathit{A}$ is symmetric, i.e., ${}_{\mathrm{C}}\mathit{A}={}_{\mathrm{C}}{\mathit{A}}^T$; however, we do not make use of this property.

Although one can compute ${}_{\mathrm{C}}\mathit{X}$ from (19) using an arbitrary solver for linear systems of equations, there is a more efficient way for doing so: since ${}_{\mathrm{C}}\mathit{A}$ is tridiagonal, we can solve (19) with the Thomas algorithm [30,31]. Derived from an $LU$ decomposition of ${}_{\mathrm{C}}\mathit{A}$, one performs a recursive forward elimination

$$\begin{array}{cc}\hfill {}_{\mathrm{C}}{H}_i& :=\left\{\begin{array}{cc}{\displaystyle \frac{{}_{\mathrm{C}}{U}_1}{{}_{\mathrm{C}}{D}_1}}\hfill & \text{for }i=1,\hfill \\ {\displaystyle \frac{{}_{\mathrm{C}}{U}_i}{{}_{\mathrm{C}}{D}_i-{}_{\mathrm{C}}{L}_i{}_{\mathrm{C}}{H}_{i-1}}}\hfill & \text{for }i=2,\cdots ,n-2\hfill \end{array}\right.,\hfill \end{array}$$

(25)

$$\begin{array}{cc}\hfill {}_{\mathrm{C}}{P}_i& :=\left\{\begin{array}{cc}{\displaystyle \frac{{}_{\mathrm{C}}{B}_1}{{}_{\mathrm{C}}{D}_1}}\hfill & \text{for }i=1,\hfill \\ {\displaystyle \frac{{}_{\mathrm{C}}{B}_i-{}_{\mathrm{C}}{L}_i{}_{\mathrm{C}}{P}_{i-1}}{{}_{\mathrm{C}}{D}_i-{}_{\mathrm{C}}{L}_i{}_{\mathrm{C}}{H}_{i-1}}}\hfill & \text{for }i=2,\cdots ,n-1\hfill \end{array}\right.\hfill \end{array}$$

(26)

followed by a backward substitution

$$\begin{array}{c}\hfill {}_{\mathrm{C}}{X}_i=\left\{\begin{array}{cc}{}_{\mathrm{C}}{P}_{n-1}\hfill & \text{for }i=n-1,\hfill \\ {}_{\mathrm{C}}{P}_i-{}_{\mathrm{C}}{H}_i{}_{\mathrm{C}}{X}_{i+1}\hfill & \text{for }i=n-2,n-3,\cdots ,1\hfill \end{array}\right..\end{array}$$

(27)

Computing ${}_{\mathrm{C}}\mathit{X}$ out of ${}_{\mathrm{C}}\mathit{A}$ and ${}_{\mathrm{C}}\mathit{B}$ thus boils down to $5n-9$ operations. Note that in contrast to [31] where $A\in \alpha {\mathbb{R}}^{n\times n}$ in our case ${}_{\mathrm{C}}\mathit{A}\in \alpha {\mathbb{R}}^{(n-1)\times (n-1)}$, thus n changes to $n-1$ for computing the count of operations in total [31]. Since ${}_{\mathrm{C}}\mathit{A}$ is symmetric and positive definite, one may think of using an algorithm based on Cholesky factorization instead of $LU$ decomposition, as this has proven to be approximately twice as efficient where applicable. However, this rule of thumb seems to be no longer valid for the special case of tridiagonal matrices: the Cholesky factorization $T=L{D}_L^{-1}{L}^T$ in [32], where the computation of the lower diagonal matrix L exploits the special structure and the diagonal matrix $D_L$ is used to avoid evaluating expensive square roots, leads to $7n-10$ operations in total.

The numerical stability of cubic spline interpolation is shown in Appendix A.1.

2.5. Quintic Spline Interpolation: Derivation

As previously mentioned, the following is a detailed version of the derivation given in [18]. Just as in the cubic case, our task is to pass through the given data points $\left\{(t_i,y_i)\right\}$, thus we enforce the interpolation constraints

$$\begin{array}{ccc}\hfill {}_{\mathrm{Q}}{s}_i\left({\xi }_i\right){|}_{{\xi }_i=0}& \stackrel{!}{=}{y}_i\hfill & \hfill \text{for }i=0,\cdots ,n-1(n\text{ eqs}.),\\ \hfill {}_{\mathrm{Q}}{s}_i\left({\xi }_i\right){|}_{{\xi }_i=1}& \stackrel{!}{=}{y}_{i+1}\hfill & \hfill \text{for }i=0,\cdots ,n-1(n\text{ eqs}.).\end{array}$$

(28)

We use the additional degrees of freedom to enforce not only ${\mathcal{C}}^2$, but instead ${\mathcal{C}}^4$ continuity with

$$\begin{array}{ccc}\hfill {}_{\mathrm{Q}}{\dot{s}}_i\left({\xi }_i\right){|}_{{\xi }_i=1}& \stackrel{!}{=}{}_{\mathrm{Q}}{\dot{s}}_{i+1}\left({\xi }_{i+1}\right){|}_{{\xi }_{i+1}=0}={\dot{y}}_{i+1}\hfill & \hfill \text{for }i=0,\cdots ,n-2(n-1\text{ eqs}.),\\ \hfill {}_{\mathrm{Q}}{\ddot{s}}_i\left({\xi }_i\right){|}_{{\xi }_i=1}& \stackrel{!}{=}{}_{\mathrm{Q}}{\ddot{s}}_{i+1}\left({\xi }_{i+1}\right){|}_{{\xi }_{i+1}=0}={\ddot{y}}_{i+1}\hfill & \hfill \text{for }i=0,\cdots ,n-2(n-1\text{ eqs}.),\\ \hfill {}_{\mathrm{Q}}{s}_i^{\left(3\right)}\left({\xi }_i\right){|}_{{\xi }_i=1}& \stackrel{!}{=}{}_{\mathrm{Q}}{s}_{i+1}^{\left(3\right)}\left({\xi }_{i+1}\right){|}_{{\xi }_{i+1}=0}=y_{i+1}^{\left(3\right)}\hfill & \hfill \text{for }i=0,\cdots ,n-2(n-1\text{ eqs}.),\\ \hfill {}_{\mathrm{Q}}{s}_i^{\left(4\right)}\left({\xi }_i\right){|}_{{\xi }_i=1}& \stackrel{!}{=}{}_{\mathrm{Q}}{s}_{i+1}^{\left(4\right)}\left({\xi }_{i+1}\right){|}_{{\xi }_{i+1}=0}=y_{i+1}^{\left(4\right)}\hfill & \hfill \text{for }i=0,\cdots ,n-2(n-1\text{ eqs}.).\end{array}$$

(29)

Inserting (10), (11), and (12) into (28) and into the first two rows of (29) allows us to reformulate the spline coefficients to

$$\begin{array}{cccccccc}\hfill {}_{\mathrm{Q}}{a}_i& =& -6y_i& +6y_{i+1}& -3{\dot{y}}_i{h}_i& -3{\dot{y}}_{i+1}{h}_i& -\frac{1}{2}{\ddot{y}}_i{h}_i^2& +\frac{1}{2}{\ddot{y}}_{i+1}{h}_i^2,\\ \hfill {}_{\mathrm{Q}}{b}_i& =& +15y_i& -15y_{i+1}& +8{\dot{y}}_i{h}_i& +7{\dot{y}}_{i+1}{h}_i& +\frac{3}{2}{\ddot{y}}_i{h}_i^2& -{\ddot{y}}_{i+1}{h}_i^2,\\ \hfill {}_{\mathrm{Q}}{\mathrm{C}}_i& =& -10y_i& +10y_{i+1}& -6{\dot{y}}_i{h}_i& -4{\dot{y}}_{i+1}{h}_i& -\frac{3}{2}{\ddot{y}}_i{h}_i^2& +\frac{1}{2}{\ddot{y}}_{i+1}{h}_i^2,\\ \hfill {}_{\mathrm{Q}}{d}_i& =& & & +\frac{1}{2}{\ddot{y}}_i{h}_i^2,\\ \hfill {}_{\mathrm{Q}}{e}_i& =& & +{\dot{y}}_i{h}_i,& \\ \hfill {}_{\mathrm{Q}}{f}_i& =& +y_i,& & \end{array}$$

(30)

where ${\dot{y}}_i$ and ${\ddot{y}}_i$ for $i=1,\cdots ,n-1$ are the $2(n-1)$ unknowns which we still have to determine using the remaining $2(n-1)$ equations given by the last two rows of (29). In particular, we expand the fourth row of (29) with (14) and insert ${}_{\mathrm{Q}}{a}_i$, ${}_{\mathrm{Q}}{b}_i$, and ${}_{\mathrm{Q}}{b}_{i+1}$ from (30) to obtain

$$\begin{array}{cc}\hfill & -56g_{i-1}^3{\dot{y}}_{i-1}-8g_{i-1}^2{\ddot{y}}_{i-1}-64{\dot{y}}_i(g_{i-1}^3+g_i^3)+12{\ddot{y}}_i(g_{i-1}^2-g_i^2)-56g_i^3{\dot{y}}_{i+1}+8g_i^2{\ddot{y}}_{i+1}=\hfill \\ \hfill & =-120g_i^4(y_{i+1}-y_i)-120g_{i-1}^4(y_i-y_{i-1}).\hfill \end{array}$$

(31)

In the same manner, we expand the third row of (29) with (13) and insert ${}_{\mathrm{Q}}{a}_i$, ${}_{\mathrm{Q}}{b}_i$, ${}_{\mathrm{Q}}{\mathrm{C}}_i$ and ${}_{\mathrm{Q}}{\mathrm{C}}_{i+1}$ from (30) which leads to

$$\begin{array}{cc}\hfill & -8g_{i-1}^2{\dot{y}}_{i-1}-g_{i-1}{\ddot{y}}_{i-1}-12{\dot{y}}_i(g_{i-1}^2-g_i^2)+3{\ddot{y}}_i(g_{i-1}+g_i)+8g_i^2{\dot{y}}_{i+1}-g_i{\ddot{y}}_{i+1}=\hfill \\ \hfill & =20g_i^3(y_{i+1}-y_i)-20g_{i-1}^3(y_i-y_{i-1}).\hfill \end{array}$$

(32)

Both (31) and (32) hold for $i=1,\cdots ,n-1$ which allows casting them in the form of

$${}_{\mathrm{Q}}\mathit{A}{}_{\mathrm{Q}}\mathit{X}={}_{\mathrm{Q}}\mathit{B}$$

(33)

with ${}_{\mathrm{Q}}\mathit{A}\in \alpha {\mathbb{R}}^{2(n-1)\times 2(n-1)}$ and ${}_{\mathrm{Q}}\mathit{X}$, ${}_{\mathrm{Q}}\mathit{B}\in \alpha {\mathbb{R}}^{2(n-1)}$ given by

$${}_{\mathrm{Q}}\mathit{A}=\left[\begin{array}{ccc}{}_{\mathrm{Q}}{\mathit{D}}_1& {}_{\mathrm{Q}}{\mathit{U}}_1& \mathbf{0}\\ {}_{\mathrm{Q}}{\mathit{L}}_2& {}_{\mathrm{Q}}{\mathit{D}}_2& {}_{\mathrm{Q}}{\mathit{U}}_2\\ \ddots & \ddots & \ddots \\ {}_{\mathrm{Q}}{\mathit{L}}_i& {}_{\mathrm{Q}}{\mathit{D}}_i& {}_{\mathrm{Q}}{\mathit{U}}_i\\ \ddots & \ddots & \ddots \\ {}_{\mathrm{Q}}{\mathit{L}}_{n-2}& {}_{\mathrm{Q}}{\mathit{D}}_{n-2}& {}_{\mathrm{Q}}{\mathit{U}}_{n-2}\\ \mathbf{0}& {}_{\mathrm{Q}}{\mathit{L}}_{n-1}& {}_{\mathrm{Q}}{\mathit{D}}_{n-1}\end{array}\right],{}_{\mathrm{Q}}\mathit{X}=\left[\begin{array}{c}{}_{\mathrm{Q}}{\mathit{X}}_1\\ {}_{\mathrm{Q}}{\mathit{X}}_2\\ ⋮\\ {}_{\mathrm{Q}}{\mathit{X}}_i\\ ⋮\\ {}_{\mathrm{Q}}{\mathit{X}}_{n-2}\\ {}_{\mathrm{Q}}{\mathit{X}}_{n-1}\end{array}\right],{}_{\mathrm{Q}}\mathit{B}=\left[\begin{array}{c}{}_{\mathrm{Q}}{\mathit{B}}_1\\ {}_{\mathrm{Q}}{\mathit{B}}_2\\ ⋮\\ {}_{\mathrm{Q}}{\mathit{B}}_i\\ ⋮\\ {}_{\mathrm{Q}}{\mathit{B}}_{n-2}\\ {}_{\mathrm{Q}}{\mathit{B}}_{n-1}\end{array}\right],$$

(34)

and their block components

$$\begin{array}{ccc}\hfill {}_{\mathrm{Q}}{\mathit{D}}_i& =\left[\begin{array}{cc}64(g_{i-1}^3+g_i^3)& 12\kappa (g_{i-1}^2-g_i^2)g_i\\ 12\kappa (g_{i-1}^2-g_i^2)g_i& 3{\kappa }^2(g_{i-1}+g_i)g_i^2\end{array}\right],{}_{\mathrm{Q}}{\mathit{X}}_i=\left[\begin{array}{c}-{\dot{y}}_i\\ \frac{1}{\kappa g_i}{\ddot{y}}_i\end{array}\right]\hfill & \hfill \text{for }i=1,\cdots ,n-1,\end{array}$$

(35)

$$\begin{array}{ccc}\hfill {}_{\mathrm{Q}}{\mathit{U}}_i& =\left[\begin{array}{cc}56g_i^3& 8\kappa g_i^2{g}_{i+1}\\ -8\kappa g_i^3& -{\kappa }^2{g}_i^2{g}_{i+1}\end{array}\right],{}_{\mathrm{Q}}{\mathit{L}}_{i+1}={}_{\mathrm{Q}}{\mathit{U}}_i^T\hfill & \hfill \text{for }i=1,\cdots ,n-2,\end{array}$$

(36)

$$\begin{array}{ccc}\hfill {}_{\mathrm{Q}}{\mathit{B}}_i& =\left[\begin{array}{c}-120g_i^4(y_{i+1}-y_i)-120g_{i-1}^4(y_i-y_{i-1})\\ 20\kappa g_i^4(y_{i+1}-y_i)-20\kappa g_{i-1}^3{g}_i(y_i-y_{i-1})\end{array}\right]+{}_{\mathrm{Q}}{\Lambda }_i\hfill & \hfill \text{for }i=1,\cdots ,n-1,\end{array}$$

(37)

$${}_{\mathrm{Q}}{\Lambda }_i=\left\{\begin{array}{cc}\left[\begin{array}{c}56g_0^3{\dot{y}}_0+8g_0^2{\ddot{y}}_0+56g_{n-1}^3{\dot{y}}_n-8g_{n-1}^2{\ddot{y}}_n\\ 8\kappa g_0^2{g}_1{\dot{y}}_0+\kappa g_0{g}_1{\ddot{y}}_0-8\kappa g_{n-1}^3{\dot{y}}_n+\kappa g_{n-1}^2{\ddot{y}}_n\end{array}\right]\hfill & \begin{array}{c}\text{for }n=2\hfill \\ (\equiv -{}_{\mathrm{Q}}{\mathit{L}}_1{}_{\mathrm{Q}}{\mathit{X}}_0-{}_{\mathrm{Q}}{\mathit{U}}_{n-1}{}_{\mathrm{Q}}{\mathit{X}}_n)\hfill \end{array},\hfill \\ \left[\begin{array}{c}56g_0^3{\dot{y}}_0+8g_0^2{\ddot{y}}_0\\ 8\kappa g_0^2{g}_1{\dot{y}}_0+\kappa g_0{g}_1{\ddot{y}}_0\end{array}\right]\hfill & \begin{array}{c}\text{for }n>2\wedge i=1\hfill \\ (\equiv -{}_{\mathrm{Q}}{\mathit{L}}_1{}_{\mathrm{Q}}{\mathit{X}}_0)\hfill \end{array},\hfill \\ \left[\begin{array}{c}56g_{n-1}^3{\dot{y}}_n-8g_{n-1}^2{\ddot{y}}_n\\ -8\kappa g_{n-1}^3{\dot{y}}_n+\kappa g_{n-1}^2{\ddot{y}}_n\end{array}\right]\hfill & \begin{array}{c}\text{for }n>2\wedge i=n-1\hfill \\ (\equiv -{}_{\mathrm{Q}}{\mathit{U}}_{n-1}{}_{\mathrm{Q}}{\mathit{X}}_n)\hfill \end{array},\hfill \\ \mathbf{0}\hfill & \text{else}\hfill \end{array}\right..$$

(38)

Just as in the cubic case, ${}_{\mathrm{Q}}{\mathit{L}}_i$, ${}_{\mathrm{Q}}{\mathit{D}}_i$, and ${}_{\mathrm{Q}}{\mathit{U}}_i\in \alpha {\mathbb{R}}^{2\times 2}$ represent the lower diagonal, diagonal, and upper diagonal blocks of ${}_{\mathrm{Q}}\mathit{A}$, respectively. As suggested in [18], the additional parameter $\kappa$ in the definitions (35)–(38) is chosen as

$$\kappa =\sqrt{\frac{64}{3}}.$$

(39)

From an analytical point of view, $\kappa$ has no influence on the solution ${}_{\mathrm{Q}}\mathit{X}$ (at least if $\kappa \ne 0$). However, it improves numerical stability which is verified in Appendix A.2.

As for the cubic spline ${}_{\mathrm{Q}}\mathit{A}$ is symmetric and only depends on the choice of $\left\{t_i\right\}$, while the interpolation points $\left\{y_i\right\}$ are contained in ${}_{\mathrm{Q}}\mathit{B}$. As before, the additional term ${}_{\mathrm{Q}}{\Lambda }_i$ incorporates the BCs such that we can write the system in the form of (34). In contrast to the cubic case, the solution ${}_{\mathrm{Q}}\mathit{X}$ represents not only ${\ddot{y}}_i$, but also ${\dot{y}}_i$, which is now additionally required to compute the segment coefficients from (30).

Since ${}_{\mathrm{Q}}\mathit{A}$ is block-tridiagonal, we can again solve (33) efficiently with the generalization of the Thomas algorithm to block-tridiagonal matrices [31]. Based on an $LU$ decomposition of ${}_{\mathrm{Q}}\mathit{A}$, we first run a recursive forward elimination

$$\begin{array}{cc}\hfill {}_{\mathrm{Q}}{\mathit{H}}_i& :=\left\{\begin{array}{cc}{}_{\mathrm{Q}}{\mathit{D}}_1^{-1}{}_{\mathrm{Q}}{\mathit{U}}_1\hfill & \text{for }i=1,\hfill \\ {\left({}_{\mathrm{Q}}{\mathit{D}}_i-{}_{\mathrm{Q}}{\mathit{L}}_i{}_{\mathrm{Q}}{\mathit{H}}_{i-1}\right)}^{-1}{}_{\mathrm{Q}}{\mathit{U}}_i\hfill & \text{for }i=2,\cdots ,n-2\hfill \end{array}\right.,\hfill \end{array}$$

(40)

$$\begin{array}{cc}\hfill {}_{\mathrm{Q}}{\mathit{P}}_i& :=\left\{\begin{array}{cc}{}_{\mathrm{Q}}{\mathit{D}}_1^{-1}{}_{\mathrm{Q}}{\mathit{B}}_1\hfill & \text{for }i=1,\hfill \\ {\left({}_{\mathrm{Q}}{\mathit{D}}_i-{}_{\mathrm{Q}}{\mathit{L}}_i{}_{\mathrm{Q}}{\mathit{H}}_{i-1}\right)}^{-1}\left({}_{\mathrm{Q}}{\mathit{B}}_i-{}_{\mathrm{Q}}{\mathit{L}}_i{}_{\mathrm{Q}}{\mathit{P}}_{i-1}\right)\hfill & \text{for }i=2,\cdots ,n-1\hfill \end{array}\right.\hfill \end{array}$$

(41)

followed by a backward substitution

$$\begin{array}{c}\hfill {}_{\mathrm{Q}}{\mathit{X}}_i=\left\{\begin{array}{cc}{}_{\mathrm{Q}}{\mathit{P}}_{n-1}\hfill & \text{for }i=n-1,\hfill \\ {}_{\mathrm{Q}}{\mathit{P}}_i-{}_{\mathrm{Q}}{\mathit{H}}_i{}_{\mathrm{Q}}{\mathit{X}}_{i+1}\hfill & \text{for }i=n-2,n-3,\cdots ,1\hfill \end{array}\right..\end{array}$$

(42)

Computing ${}_{\mathrm{Q}}\mathit{X}$ out of ${}_{\mathrm{Q}}\mathit{A}$ and ${}_{\mathrm{Q}}\mathit{B}$ requires at a maximum $36n-60$ operations. Again [31] uses $A\in \alpha {\mathbb{R}}^{2n\times 2n}$ while in our case ${}_{\mathrm{Q}}\mathit{A}\in \alpha {\mathbb{R}}^{2(n-1)\times 2(n-1)}$ holds, thus n changes to $n-1$ for computing the count of operations in total [31]. For this upper bound, explicit computation of the inverse of ${}_{\mathrm{Q}}{\mathit{D}}_1$ and $\left({}_{\mathrm{Q}}{\mathit{D}}_i-{}_{\mathrm{Q}}{\mathit{L}}_i{}_{\mathrm{Q}}{\mathit{H}}_{i-1}\right)$ by Gaussian elimination is assumed, which in practice should be avoided by solving a $2\times 2$ LSE instead [31]. Thus, a corresponding implementation can be expected to require even less operations.

In Appendix A.2, the interested reader can find considerations on the numerical stability of quintic spline interpolation.

2.6. Algorithm for Cubic/Quintic Spline Interpolation

Since the presented derivation is rather lengthy, the key steps for interpolating cubic/quintic splines following the proposed method are summarized in Algorithm 1.

Algorithm 1: Cubic/Quintic Spline Interpolation.

For details on convergence order and approximation error of quintic spline interpolation, the interested reader is referred to [18] where these issues have been experimentally investigated for various examples.

2.7. Spline Collocation: Derivation

The following is based on the collocation algorithm presented in [19,33]. However, we extend the method from cubic to quintic splines. Moreover, we do not use natural splines, but instead integrate the boundary conditions directly into the scheme. Lastly, in contrast to [19,33], we do not need to modify the right-hand side of (1), thus leading to a “true” collocation of the ODE for all collocation sites, which are chosen to be the interior spline knots. As runtime performance is of the highest priority for our application, we choose smoothest spline collocation. This minimizes the count of (expensive) collocation sites, thus reduces the count of equations to solve, and instead uses the available degrees of freedom to force ${\mathcal{C}}^2$ (cubic spline) or ${\mathcal{C}}^4$ (quintic spline) continuity. Moreover, in our application, $y\left(t\right)$ is used as input for controlling the motion of a robot, thus a smooth $y\left(t\right)$ is equivalent to small changes in joint accelerations, i.e., motor jerks, which in turn improves overall stability during locomotion.

As stated in Section 2.1, we require the approximation $y\left(t\right)$ to fulfill the underlying ODE at certain collocation sites $\left\{t_k\right\}$, see (3), while simultaneously satisfying the boundary conditions as specified in (4). Note that we use the index k instead of i to highlight that our new task consists in collocating the ODE at the interior knots, i.e., $k=1,\cdots ,n-1$ rather than the previously investigated interpolation at all knots, i.e., $i=0,\cdots ,n$. Furthermore, it should be pointed out that although (3) holds, this does not imply that $y\left(t_k\right)=F\left(t_k\right)$, $\dot{y}\left(t_k\right)=\dot{F}\left(t_k\right)$, or $\ddot{y}\left(t_k\right)=\ddot{F}\left(t_k\right)$. In other words, $y\left(t\right)$ will not coincide with the real solution $F\left(t\right)$ at the collocation sites $\left\{t_k\right\}$. However, it will behave similarly at these spots (meaning that they will satisfy the same Equation (1)), which is illustrated in Figure 4.

As first step, we introduce the auxiliary variables $\mathit{\lambda }$, $\mathit{\eta }$, and $\mathit{r}$ which are defined as

$$\begin{array}{cccccc}\hfill {}_{\mathrm{C}}\mathit{\lambda }& :={\left[\begin{array}{c}{y}_1,\cdots ,y_{n-1}\end{array}\right]}^T,\hfill & \hfill {}_{\mathrm{C}}\mathit{\eta }& :={\left[\begin{array}{c}{\ddot{y}}_1,\cdots ,{\ddot{y}}_{n-1}\end{array}\right]}^T,\hfill & \hfill {}_{\mathrm{C}}\mathit{r}& :={\left[\begin{array}{c}{y}_0,{\ddot{y}}_0,y_n,{\ddot{y}}_n\end{array}\right]}^T,\hfill \\ \hfill {}_{\mathrm{Q}}\mathit{\lambda }& :={\left[\begin{array}{c}{y}_1,\cdots ,y_{n-1}\end{array}\right]}^T,\hfill & \hfill {}_{\mathrm{Q}}\mathit{\eta }& :={\left[\begin{array}{c}{\dot{y}}_1,{\ddot{y}}_1,\cdots ,{\dot{y}}_{n-1},{\ddot{y}}_{n-1}\end{array}\right]}^T,\hfill & \hfill {}_{\mathrm{Q}}\mathit{r}& :={\left[\begin{array}{c}{y}_0,{\dot{y}}_0,{\ddot{y}}_0,y_n,{\dot{y}}_n,{\ddot{y}}_n\end{array}\right]}^T,\hfill \end{array}$$

(43)

where ${}_{\mathrm{C}}\mathit{\lambda }$, ${}_{\mathrm{C}}\mathit{\eta }$, ${}_{\mathrm{C}}\mathit{r}$ and ${}_{\mathrm{Q}}\mathit{\lambda }$, ${}_{\mathrm{Q}}\mathit{\eta }$, ${}_{\mathrm{Q}}\mathit{r}$ are the corresponding counterparts for the case of cubic and quintic splines, respectively. While $\mathit{\lambda }$ represents the (yet unknown) collocation points $\left\{y_k\right\}$, $\mathit{\eta }$ contains their corresponding first (and second) time derivatives, which can be seen as “internal” unknowns, as they will be implicitly defined through an embedded spline interpolation. Lastly, $\mathit{r}$ depicts the BCs, where we lack ${\dot{y}}_0$ and ${\dot{y}}_n$ in the case of cubic splines as has been previously explained.

From (17), (30), and (43) we observe that the spline segments $s_i$ are linear with respect to $\mathit{\lambda }$, $\mathit{\eta }$, and $\mathit{r}$, i.e.,

$$s_i\left({\xi }_i\right)=\underset{\text{known}}{\underbrace{\left(\frac{\partial s_i\left({\xi }_i\right)}{\partial \mathit{\lambda }}\right)}}\mathit{\lambda }+\underset{\text{known}}{\underbrace{\left(\frac{\partial s_i\left({\xi }_i\right)}{\partial \mathit{\eta }}\right)}}\mathit{\eta }+\underset{\text{known}}{\underbrace{\left(\frac{\partial s_i\left({\xi }_i\right)}{\partial \mathit{r}}\right)\mathit{r}}}\text{for }i=0,\cdots ,n-1$$

(44)

holds. The gradients are fully defined by the spline partitioning $\left\{t_i\right\}$, which is assumed to be known. Thus the construction of the spline $y\left(t\right)$ is equivalent to the search for a corresponding $\mathit{\lambda }$ and $\mathit{\eta }$. Note that to obtain (44), we used (17) and (30), which in turn were derived from fulfilling the interpolation condition together with enforcing continuity of the second time derivative (cubic spline), or first and second time derivative (quintic spline) at the interior knots. In order to accomplish full ${\mathcal{C}}^2$ and ${\mathcal{C}}^4$ continuity, we further make use of $\mathit{A}\mathit{X}=\mathit{B}$ from (19) and (33), which represents continuity of the first time derivative (cubic spline) or third and fourth time derivative (quintic spline), respectively. In particular, we observe from (23), (24), (37), and (38), that $\mathit{B}$ is linear with respect to $\mathit{\lambda }$ and $\mathit{r}$, thus

$$\mathit{A}\mathit{X}=\mathit{B}=\underset{\text{known}}{\underbrace{\left(\frac{\partial \mathit{B}}{\partial \mathit{\lambda }}\right)}}\mathit{\lambda }+\underset{\text{known}}{\underbrace{\left(\frac{\partial \mathit{B}}{\partial \mathit{r}}\right)\mathit{r}}}$$

(45)

holds, where the gradients again depend only on the known partitioning $\left\{t_i\right\}$. We further observe that, according to the definitions (21), (35) and (43), we can write the mapping

$$\mathit{X}=\mathit{S}\mathit{\eta }\text{with}{}_{\mathrm{C}}\mathit{S}:=\mathit{I}and{}_{\mathrm{Q}}\mathit{S}:=diag\left({}_{\mathrm{Q}}{\mathit{S}}_1,\cdots ,{}_{\mathrm{Q}}{\mathit{S}}_{n-1}\right)\text{where}{}_{\mathrm{Q}}{\mathit{S}}_i:=\left[\begin{array}{cc}-1& 0\\ 0& \frac{1}{\kappa g_i}\end{array}\right]$$

(46)

with $\mathit{I}$ being the identity matrix of appropriate size. Since $\mathit{A}$ and $\mathit{S}$ are constant, by “constant” we mean in this context that an expression does not depend on the yet unknown $\mathit{\lambda }$ or $\mathit{\eta }$, and assumed to be non-singular, it is clear from (45) that not only $\mathit{B}$, but also $\mathit{X}$ and thus $\mathit{\eta }$ are linear with respect to $\mathit{\lambda }$ and $\mathit{r}$. Hence one can write

$$\mathit{X}=\left(\frac{\partial \mathit{X}}{\partial \mathit{\lambda }}\right)\mathit{\lambda }+\left(\frac{\partial \mathit{X}}{\partial \mathit{r}}\right)\mathit{r}\text{and}\mathit{\eta }=\left(\frac{\partial \mathit{\eta }}{\partial \mathit{\lambda }}\right)\mathit{\lambda }+\left(\frac{\partial \mathit{\eta }}{\partial \mathit{r}}\right)\mathit{r}.$$

(47)

Note that $\mathit{A}$ and ${}_{\mathrm{Q}}\mathit{S}$ only depend on the known $\left\{t_i\right\}$, which allows us to safely differentiate (45) with respect to $\mathit{\lambda }$ and $\mathit{r}$ to obtain

$$\mathit{A}\underset{{\displaystyle \mathit{S}\left(\frac{\partial \mathit{\eta }}{\partial \mathit{\lambda }}\right)}}{\underbrace{\left(\frac{\partial \mathit{X}}{\partial \mathit{\lambda }}\right)}}=\underset{\text{known}}{\underbrace{\left(\frac{\partial \mathit{B}}{\partial \mathit{\lambda }}\right)}}\text{and}\mathit{A}\underset{{\displaystyle \mathit{S}\left(\frac{\partial \mathit{\eta }}{\partial \mathit{r}}\right)}}{\underbrace{\left(\frac{\partial \mathit{X}}{\partial \mathit{r}}\right)}}=\underset{\text{known}}{\underbrace{\left(\frac{\partial \mathit{B}}{\partial \mathit{r}}\right)}}.$$

(48)

which we can use to compute the yet unknown gradients in (47). Note that this can be done very efficiently due to the (block-)tridiagonal form of $\mathit{A}$ as already discussed in Section 2.3. Since $\mathit{S}$ is diagonal, this property also holds for the product $\mathit{A}\mathit{S}$. However, for best numerical stability, one should solve for the gradients of $\mathit{X}$ first and use the mapping (46) to obtain the gradients of $\mathit{\eta }$ afterwards, which is of negligible cost since $\mathit{S}$, and thus also ${\mathit{S}}^{-1}$, is diagonal. The right-hand sides necessary to solve (48) only depend on $\left\{t_i\right\}$ and are derived in Appendix B. Lastly, we insert $\mathit{\eta }$ from (47) into (44) and obtain

$$s_i\left({\xi }_i\right)=\left[\left(\frac{\partial s_i\left({\xi }_i\right)}{\partial \mathit{\lambda }}\right)+\left(\frac{\partial s_i\left({\xi }_i\right)}{\partial \mathit{\eta }}\right)\left(\frac{\partial \mathit{\eta }}{\partial \mathit{\lambda }}\right)\right]\mathit{\lambda }+\left[\left(\frac{\partial s_i\left({\xi }_i\right)}{\partial \mathit{r}}\right)+\left(\frac{\partial s_i\left({\xi }_i\right)}{\partial \mathit{\eta }}\right)\left(\frac{\partial \mathit{\eta }}{\partial \mathit{r}}\right)\right]\mathit{r}$$

(49)

or equivalently

$$s_i\left({\xi }_i\right)={\nabla }_{\mathit{\lambda }}{s}_i\left({\xi }_i\right)\mathit{\lambda }+{\nabla }_{\mathit{r}}{s}_i\left({\xi }_i\right)\mathit{r}$$

(50)

with the known spline gradients

$${\nabla }_{\mathit{\lambda }}{s}_i\left({\xi }_i\right):=\left[\left(\frac{\partial s_i\left({\xi }_i\right)}{\partial \mathit{\lambda }}\right)+\left(\frac{\partial s_i\left({\xi }_i\right)}{\partial \mathit{\eta }}\right)\left(\frac{\partial \mathit{\eta }}{\partial \mathit{\lambda }}\right)\right],{\nabla }_{\mathit{r}}{s}_i\left({\xi }_i\right):=\left[\left(\frac{\partial s_i\left({\xi }_i\right)}{\partial \mathit{r}}\right)+\left(\frac{\partial s_i\left({\xi }_i\right)}{\partial \mathit{\eta }}\right)\left(\frac{\partial \mathit{\eta }}{\partial \mathit{r}}\right)\right].$$

(51)

We can obtain the corresponding expressions for the first and second time derivatives by following the exact same scheme. In particular, we get

$${\dot{s}}_i\left({\xi }_i\right)={\nabla }_{\mathit{\lambda }}{\dot{s}}_i\left({\xi }_i\right)\mathit{\lambda }+{\nabla }_{\mathit{r}}{\dot{s}}_i\left({\xi }_i\right)\mathit{r},{\ddot{s}}_i\left({\xi }_i\right)={\nabla }_{\mathit{\lambda }}{\ddot{s}}_i\left({\xi }_i\right)\mathit{\lambda }+{\nabla }_{\mathit{r}}{\ddot{s}}_i\left({\xi }_i\right)\mathit{r}$$

(52)

with

$$\begin{array}{cccc}\hfill {\nabla }_{\mathit{\lambda }}{\dot{s}}_i\left({\xi }_i\right)& :=\left[\left(\frac{\partial {\dot{s}}_i\left({\xi }_i\right)}{\partial \mathit{\lambda }}\right)+\left(\frac{\partial {\dot{s}}_i\left({\xi }_i\right)}{\partial \mathit{\eta }}\right)\left(\frac{\partial \mathit{\eta }}{\partial \mathit{\lambda }}\right)\right],\hfill & \hfill {\nabla }_{\mathit{r}}{\dot{s}}_i\left({\xi }_i\right)& :=\left[\left(\frac{\partial {\dot{s}}_i\left({\xi }_i\right)}{\partial \mathit{r}}\right)+\left(\frac{\partial {\dot{s}}_i\left({\xi }_i\right)}{\partial \mathit{\eta }}\right)\left(\frac{\partial \mathit{\eta }}{\partial \mathit{r}}\right)\right],\hfill \end{array}$$

(53)

$$\begin{array}{cccc}\hfill {\nabla }_{\mathit{\lambda }}{\ddot{s}}_i\left({\xi }_i\right)& :=\left[\left(\frac{\partial {\ddot{s}}_i\left({\xi }_i\right)}{\partial \mathit{\lambda }}\right)+\left(\frac{\partial {\ddot{s}}_i\left({\xi }_i\right)}{\partial \mathit{\eta }}\right)\left(\frac{\partial \mathit{\eta }}{\partial \mathit{\lambda }}\right)\right],\hfill & \hfill {\nabla }_{\mathit{r}}{\ddot{s}}_i\left({\xi }_i\right)& :=\left[\left(\frac{\partial {\ddot{s}}_i\left({\xi }_i\right)}{\partial \mathit{r}}\right)+\left(\frac{\partial {\ddot{s}}_i\left({\xi }_i\right)}{\partial \mathit{\eta }}\right)\left(\frac{\partial \mathit{\eta }}{\partial \mathit{r}}\right)\right].\hfill \end{array}$$

(54)

Although computing the spline gradients can be done very efficiently, their mathematical representation is rather lengthy. A formulation of the gradients, which is ready for implementation, is given in Appendix B.

Lastly, we fulfill the dynamics of the ODE by inserting (50) and (52) into (3), which leads to

$$\alpha \left(t_k\right){\ddot{s}}_k({\xi }_k=0)+\beta \left(t_k\right){\dot{s}}_k({\xi }_k=0)+\gamma \left(t_k\right)s_k({\xi }_k=0)=\tau \left(t_k\right)$$

(55)

for $t_0<t_k<t_n$ and $t_k<t_{k+1}$ with $k=1,\cdots ,n-1$. This can be formulated as LSE

$${\mathit{A}}_{\mathrm{coll}}\mathit{\lambda }={\mathit{B}}_{\mathrm{coll}}$$

(56)

with

$${\mathit{A}}_{\mathrm{coll}}=\left[\begin{array}{c}{\mathit{A}}_{\mathrm{coll,1}}\\ ⋮\\ {\mathit{A}}_{\mathrm{coll,}k}\\ ⋮\\ {\mathit{A}}_{\mathrm{coll,}n-1}\end{array}\right]\in \alpha {\mathbb{R}}^{(n-1)\times (n-1)},{\mathit{B}}_{\mathrm{coll}}=\left[\begin{array}{c}{B}_{\mathrm{coll,1}}\\ ⋮\\ B_{\mathrm{coll,}k}\\ ⋮\\ B_{\mathrm{coll,}n-1}\end{array}\right]\in \alpha {\mathbb{R}}^{(n-1)}$$

(57)

where the k-th row of ${\mathit{A}}_{\mathrm{coll}}$ and ${\mathit{B}}_{\mathrm{coll}}$ corresponds to the collocation site $t_k$ and is given by

$$\begin{array}{cc}\hfill {\mathit{A}}_{\mathrm{coll,}k}& =\alpha \left(t_k\right){\nabla }_{\mathit{\lambda }}{\ddot{s}}_k\left(0\right)+\beta \left(t_k\right){\nabla }_{\mathit{\lambda }}{\dot{s}}_k\left(0\right)+\gamma \left(t_k\right){\nabla }_{\mathit{\lambda }}{s}_k\left(0\right)\in \alpha {\mathbb{R}}^{1\times (n-1)},\hfill \end{array}$$

(58)

$$\begin{array}{cc}\hfill B_{\mathrm{coll,}k}& =\tau \left(t_k\right)-\left[\alpha \left(t_k\right){\nabla }_{\mathit{r}}{\ddot{s}}_k\left(0\right)+\beta \left(t_k\right){\nabla }_{\mathit{r}}{\dot{s}}_k\left(0\right)+\gamma \left(t_k\right){\nabla }_{\mathit{r}}{s}_k\left(0\right)\right]\mathit{r}\in \alpha \mathbb{R}.\hfill \end{array}$$

(59)

Note that we choose a partitioning of the spline such that the collocation sites coincide with the starting (“left”) knot of each segment, i.e., ${\xi }_k=0$, see Figure 4. This allows us to skip the computation of certain spline gradients. Since the underlying ODE is of order two, only gradients of the last three coefficients, i.e., ${}_{\mathrm{C}}{b}_i$, ${}_{\mathrm{C}}{\mathrm{C}}_i$, ${}_{\mathrm{C}}{d}_i$ (cubic) or ${}_{\mathrm{Q}}{d}_i$, ${}_{\mathrm{Q}}{e}_i$, ${}_{\mathrm{Q}}{f}_i$ (quintic), have to be computed, for details see Appendix B. This simplifies the implementation and improves the overall performance. Solving (56) for $\mathit{\lambda }$ represents the key operation (i.e., bottleneck for large n) of the proposed collocation method, since ${\mathit{A}}_{\mathrm{coll}}$ is in general dense while all other operations are either simple explicit expressions or linear systems in (block-)tridiagonal form, which can be solved efficiently. This justifies our strategy to minimize the count of collocation points (and thus the dimension of ${\mathit{A}}_{\mathrm{coll}}$) and instead force high order continuity.

As soon as $\mathit{\lambda }$ has been obtained, we can compute $\mathit{\eta }$ directly from (47), since the gradients of $\mathit{\eta }$ with respect to $\mathit{\lambda }$ and $\mathit{r}$ are already available as by-products of computing $\mathit{\lambda }$, see (48). From $\mathit{\lambda }$, $\mathit{\eta }$ and $\mathit{r}$ the segment coefficients can finally be computed using (17) or (30).

2.8. Satisfying First Order Boundary Conditions for Cubic Splines

The presented method for cubic spline collocation respects $y_0$, ${\ddot{y}}_0$, $y_n$, and ${\ddot{y}}_n$ as BCs. However, our task was to fulfill all BCs given in (4), which seems at first to be only possible with quintic spline collocation. Also satisfying the first-order BCs, i.e., ${\dot{y}}_0$ and ${\dot{y}}_n$, can be achieved with moderate effort. For that purpose we insert two auxiliary knots, the so-called virtual control points $t_{\mathrm{virt,1}}$ and $t_{virt,2}$, which give us the necessary degrees of freedom to introduce additional constraints. The term “virtual” highlights that these points are not used as collocation sites. Both, $t_{\mathrm{virt,1}}$ and $t_{virt,2}$, have to lie within the specified start- and endtime and must not coincide with the collocation sites. This way the spline remains properly partitioned. For simplicity, we place the virtual control points at the centers of the (originally) first and last segments, i.e.,

$$t_{\mathrm{virt,1}}:=\frac{t_{\mathrm{orig,0}}+t_{\mathrm{orig,1}}}{2}\text{and}{t}_{\mathrm{virt,2}}:=\frac{t_{\mathrm{orig,}n-1}+t_{\mathrm{orig,}n}}{2}.$$

(60)

Obviously, inserting two knots leads to a different segmentation of the spline, i.e., $n\to n+2$; however, the boundaries and collocation sites remain unchanged. If we adapt the indexing such that $t_1:=t_{\mathrm{virt,1}}$ and $t_{n-1}:=t_{\mathrm{virt,2}}$, all findings derived so far are also valid for this case. The only difference is that we do not force $y\left(t\right)$ to fulfill the underlying ODE at $t_1$ and $t_{n-1}$ anymore. Instead, we satisfy the BCs ${\dot{y}}_0$ and ${\dot{y}}_n$ by replacing the first and last row of ${\mathit{A}}_{\mathrm{coll}}$ and ${\mathit{B}}_{\mathrm{coll}}$ with

$$\begin{array}{cccc}\hfill {\mathit{A}}_{\mathrm{coll,1}}& ={\nabla }_{\mathit{\lambda }}{\dot{s}}_0({\xi }_0=0),\hfill & \hfill B_{\mathrm{coll,1}}& ={\dot{y}}_0-{\nabla }_{\mathit{r}}{\dot{s}}_0({\xi }_0=0)\mathit{r},\hfill \\ \hfill {\mathit{A}}_{\mathrm{coll,}n-1}& ={\nabla }_{\mathit{\lambda }}{\dot{s}}_{n-1}({\xi }_{n-1}=1),\hfill & \hfill B_{\mathrm{coll,}n-1}& ={\dot{y}}_n-{\nabla }_{\mathit{r}}{\dot{s}}_{n-1}({\xi }_{n-1}=1)\mathit{r}.\hfill \end{array}$$

(61)

In this way the resulting cubic spline fulfills all BCs given in (4), satisfies the underlying ODE at the specified collocation sites, and is ${\mathcal{C}}^2$ continuous at the interior spline knots (which include $t_{\mathrm{virt,1}}$ and $t_{\mathrm{virt,2}}$).

2.9. Algorithm for Cubic/Quintic Spline Collocation

We summarize our findings in Algorithm 2. Note that, for the case of cubic splines, the modification necessary to satisfy first-order BCs is already integrated.

Algorithm 2: Cubic/Quintic Spline Collocation.

3. Results

3.1. Implementation

Algorithm 2 delivers a detailed description of the proposed collocation method which can be used as a reference during implementation. However, we also provide a fully documented C++ implementation through our free and open-source library BROCCOLI [34] (see Supplementary Materials). For best usability, the library is designed to be header-only and uses the (also header-only) cross-platform linear algebra system EIGEN [35] as sole dependency. Although BROCCOLI also has other dependencies, only EIGEN is required for the functionality discussed in this paper. Aside from basic matrix and vector operations, we use EIGEN to solve dense linear systems of equations such as ${\mathit{A}}_{\mathrm{coll}}^{-1}{\mathit{B}}_{\mathrm{coll}}$ in (56), but also ${}_{\mathrm{Q}}{\mathit{D}}_1^{-1}(·)$ and ${({}_{\mathrm{Q}}{\mathit{D}}_i-{}_{\mathrm{Q}}{\mathit{L}}_i{}_{\mathrm{Q}}{\mathit{H}}_{i-1})}^{-1}(·)$ in (40) and (41). In particular, we use a Householder rank-revealing QR decomposition with column-pivoting, see ColPivHouseholderQR in EIGEN, since it provides the best trade-off between speed, accuracy, and robustness for our use case. Note that for large scale systems solving ${\mathit{A}}_{\mathrm{coll}}^{-1}{\mathit{B}}_{\mathrm{coll}}$ turns out to be the bottleneck, cf. Figure 11. Thus, in this case one might choose a parallel solver for (56) instead, for example PartialPivLU in EIGEN with OPENMP enabled. In addition to the variants CubicSplineCollocator and QuinticSplineCollocator for spline collocation, see ode module of BROCCOLI, cubic and quintic spline interpolation, see curve module of BROCCOLI, as specified in Algorithm 1, is also implemented. An overview over the structure of the source code related to our collocation method is given in Appendix C. Lastly, unit tests, similar to the evaluation presented in the following, are shipped with BROCCOLI. These can be used as example applications.

3.2. Test System

In order to evaluate the proposed method and its variants, a simple mass-spring-damper system is considered, see Figure 5 (left). For the sake of simplicity, we do not consider external excitation, i.e., $\tau \left(t\right):=0$. The corresponding ODE (1) describing the system dynamics simplifies to

$$\alpha \ddot{F}\left(t\right)+\beta \dot{F}\left(t\right)+\gamma F\left(t\right)=0$$

(62)

where $\alpha$, $\beta$, and $\gamma$ are constants representing the mass and (linear) damping/stiffness coefficients of the system, respectively.

Using textbook mathematics, we can find the analytical solution given by

$$F\left(t\right)=\left\{\begin{array}{cc}{\displaystyle e^{{\sigma }_{1}t}\left[F_{0}cos\left({\sigma }_{2}t\right)+\left(\frac{{\dot{F}}_0-F_0{\sigma }_1}{{\sigma }_2}\right)sin\left({\sigma }_{2}t\right)\right]}\hfill & \begin{array}{c}{\displaystyle \mathrm{for }\frac{{\beta }^2}{4{\alpha }^2}-\frac{\gamma }{\alpha }<0}\hfill \\ \left(\mathrm{underdamped}\right)\hfill \end{array},\hfill \\ {\displaystyle \left(\frac{{\dot{F}}_0-F_0\left({\sigma }_1-{\sigma }_2\right)}{2{\sigma }_2}\right)e^{\left({\sigma }_1+{\sigma }_2\right)t}+\left(\frac{F_0\left({\sigma }_1+{\sigma }_2\right)-{\dot{F}}_0}{2{\sigma }_2}\right)e^{\left({\sigma }_1-{\sigma }_2\right)t}}\hfill & \begin{array}{c}{\displaystyle \mathrm{for }\frac{{\beta }^2}{4{\alpha }^2}-\frac{\gamma }{\alpha }>0}\hfill \\ \mathrm{overdamped})\hfill \end{array},\hfill \\ {\displaystyle e^{{\sigma }_{1}t}\left[F_0+\left({\dot{F}}_0-F_0{\sigma }_1\right)t\right]}\hfill & \begin{array}{c}{\displaystyle \text{for }\frac{{\beta }^2}{4{\alpha }^2}-\frac{\gamma }{\alpha }=0}\hfill \\ \left(\mathrm{critically damped}\right)\hfill \end{array}\hfill \end{array}\right.$$

(63)

where we used the initial conditions $F(t_0=0)=F_0$ and $\dot{F}(t_0=0)={\dot{F}}_0$, and the abbreviations

$${\sigma }_1:=-\frac{\beta }{2\alpha }\text{and}{\sigma }_2:=\sqrt{\left|\frac{{\beta }^2}{4{\alpha }^2}-\frac{\gamma }{\alpha }\right|}.$$

(64)

The characteristic shape of each branch of (63) is visualized in Figure 5 (right) for the parametrization $\alpha =1$, $\beta =1$, $\gamma =10$ in the underdamped case, $\alpha =1$, $\beta =10$, $\gamma =10$ in the overdamped case, and $\alpha =1$, $\beta =10$, $\gamma =25$ in the critically damped case. For the rest of this paper we will adhere to this parametrization. Moreover, we assume the initial conditions to be given with $F_0=1$, ${\dot{F}}_0=0$. Note that by choosing ${\sigma }_1<0$ we obtain asymptotically stable behavior. As we never constrained the underlying ODE to be stable, our algorithm can also be used to approximate instable systems. Since tests with $\beta =-1$ showed results comparable to the underdamped case (with $\beta =1$), we omit an explicit discussion of this case for brevity. Lastly, we point out that although (62) describes a very simple system, we also successfully applied the proposed algorithm to the much more complex walking-pattern generation system of our humanoid robot LOLA, cf. [13]. However, the properties and characteristics of the proposed method can be better investigated and explained by means of a less complex test system.

3.3. Convergence for Consistent and Inconsistent Boundary Conditions

As already mentioned, we focus on over-determined BVPs. Thus, we consider two cases for evaluating our algorithm. For the first analysis, we use the initial conditions $y_0=F_0=1$, ${\dot{y}}_0={\dot{F}}_0=0$ and correspondingly ${\ddot{y}}_0={\ddot{F}}_0=-\gamma /\alpha$, see (62), together with the analytical solution given in (63) to compute the BCs $y_n=F_n$, ${\dot{y}}_n={\dot{F}}_n$, and ${\ddot{y}}_n={\ddot{F}}_n$ at $t_n=5$. In this way, the BCs are guaranteed to be consistent because they belong to the same analytic solution $F\left(t\right)$. In the second case, we keep the initial BCs unchanged, but force $y_n={\dot{y}}_n={\ddot{y}}_n=0$ for $t_n=5$ which deviates from the previous solution $F_n$, ${\dot{F}}_n$, and ${\ddot{F}}_n$, especially in the underdamped case, as can be clearly seen in Figure 5 right. Thus, the second analysis handles inconsistent BCs.

In the following, we only focus on the underdamped and overdamped case, since the critically damped case can be seen as a special form of these with ${\sigma }_2\to 0$. By evaluating both cases, we aim at covering oscillating and non-oscillating dynamics. Moreover, we run tests for different counts of collocation sites $\nu :=\left|\right\{t_k\left\}\right|$, where we use a uniform segmentation of the spline, i.e., $h_i=h_{i+1}\forall i$. For cubic and quintic spline collocation without virtual control points $\nu =n-2$ holds. In contrast, $\nu =n-4$ holds for cubic spline collocation with virtual control points. Note that an inhomogeneous partitioning $h_i\ne h_{i+1}$ is evaluated per default in the unit tests provided with BROCCOLI. The approximation of the BVP by spline collocation with consistent BCs and $\nu =1,\cdots ,100$ is depicted in Figure 6. Note that for cubic spline collocation without virtual control points, the BCs ${\dot{y}}_0$ and ${\dot{y}}_n$ are violated (see Section 2.7).

For the case of consistent BCs, we observe that the approximation $y\left(t\right)$ indeed converges for increasing $\nu$ to the analytical solution $F\left(t\right)$. From a qualitative point of view, the convergence order using a quintic spline (Figure 6 right column) is clearly higher than the corresponding cubic counterparts (Figure 6 left and center column). In order to compare convergence using a quantitative measure, we use the root mean square (RMS) of the approximation error $e\left(t\right)$ and of the residual $r\left(t\right)$, defined as

$$\begin{array}{cccc}\hfill RMS\left(e\right)& :=\sqrt{\frac{1}{t_n-t_0}{\int }_{t_0}^{t_n}{\left[e\left(t\right)\right]}^{2}dt}\hfill & \hfill & \mathrm{with}e\left(t\right):=F\left(t\right)-y\left(t\right),\hfill \end{array}$$

(65)

$$\begin{array}{cccc}\hfill RMS\left(r\right)& :=\sqrt{\frac{1}{t_n-t_0}{\int }_{t_0}^{t_n}{\left[r\left(t\right)\right]}^{2}dt}\hfill & \hfill & \mathrm{with}r\left(t\right):=\alpha \ddot{y}\left(t\right)+\beta \dot{y}\left(t\right)+\gamma y\left(t\right).\hfill \end{array}$$

(66)

For numerical evaluation of (65) and (66), we discretize the embedded integral using a time step size of $\Delta t=0.01$ for which we obtain the results presented in Figure 7 left. Since we stop the test series at ${\nu }_{\max }=100$, we find the optimum for all variants to be at ${\nu }_{\mathrm{opt}}={\nu }_{\max }$. However, due to convergence, we expect the theoretical optimum to be at ${\nu }_{\mathrm{opt}}\to \infty$. We observe that also from an quantitative point of view, quintic spline collocation clearly outperforms the cubic variants for the same $\nu$. Furthermore, forcing first-order BCs through virtual control points of the cubic spline seems to have a negative influence, especially for increasing $\nu$. The influence of virtual control points on the residual is visualized in Figure 8 where peaks in $r\left(t\right)$ occur at these spots. Note that without virtual control points, the first-order boundary conditions at $t_0=0$ and $t_n=5$ are missed. Thus, in contrast to the collocation sites, the residual does not drop to zero at the boundaries. By comparing the left and right plot of Figure 8, we observe that $r\left(t\right)$ behaves similarly for consistent and inconsistent BCs.

For consistent BCs, we can state that, at least for the investigated test system, the proposed algorithm leads to an approximation which converges to the real solution where the “speed” of convergence depends on the chosen variant of 2. For inconsistent BCs, however, our analysis draws a different picture: while we can find an optimal count of collocation sites ${\nu }_{\mathrm{opt}}$ for each variant and test case, the approximation diverges for $\nu \to \infty$, see Figure 7 right and Figure 9. Note that this was expected since we are attempting to find an approximation of a solution which does not actually exist. For small $\nu$, we still find a “reasonable” $y\left(t\right)$ where the spline is still able to smooth out the wrong BCs. However, as we refine the segmentation of the spline, it gets harder to compensate the error, which in turn leads to undesired oscillations of $y\left(t\right)$. Note that although we call this behavior divergence, our collocation algorithm still satisfies all constraints that we defined: using a given partitioning, it fulfills the BCs and satisfies the underlying ODE at the given collocation sites. Thus, the divergence is not a fault of the proposed algorithm, but rather is due to the non-existence of the solution $F\left(t\right)$. Moreover, the sensitivity to undesired oscillations depends heavily on the underlying BVP: for our target application of real-time motion planning, we did not observe any oscillations up to $\nu ={10}^4$. The critical value for $\nu$ is expected to be even higher. However, we could not determine it due to memory limitations of our test system.

In order to avoid undesired oscillations, an optimal $\nu$ has to be chosen, which seems to be difficult at the first sight, since in practice we do not know the analytical solution, and thus cannot evaluate $e\left(t\right)$. However, one can use the residual $r\left(t\right)$ as measure for the approximation error and use this to formulate a governing optimization for finding ${\nu }_{\mathrm{opt}}$. Moreover, one can also use a non-uniform segmentation to give specific sections more weight. Such adaptive techniques for automatic mesh refinement have also been developed for other collocation methods, see, for example, [36]. However, for our application it is sufficient to choose $h_i$ once (fixed), thus we withhold this idea for future investigations.

3.4. Runtime Analysis

In the following, we present measurements, which have been made by using the implementation given in BROCCOLI with the version primo (v1.0.0 - commit 89280c69). We used an AMD Ryzen 7 1700X 8x (16x) @3.40GHz with 32GB DDR4 RAM @2133MHz as hardware backend and Ubuntu 18.04.2 LTS 64bit (Linux kernel 4.15.0-51) together with Clang (version 6.0.0-1) on optimization level 3 as software basis. Although our algorithm is sequential, we run different test cases on four physical cores of the CPU in parallel. For all tests simultaneous multi-threading (SMT) was disabled in the BIOS. For runtime evaluation, we take 1000 samples for every code section in Algorithm 2 and choose the minimum execution time as reference to minimize the risk of wrong measurements due to high system load and context switching effects.

In Figure 10 (left), we present runtime measurements for all three variants of our algorithm. We restrict our analysis to the case of an underdamped parametrization and consistent BCs since we expect comparable results for the other test cases. Since our algorithm has a deterministic runtime which only depends on the chosen variant (cubic/quintic, with/without virtual control points) and $\nu$, there is no reason why it should be faster or slower with another parametrization. We observe that quintic spline collocation is more expensive than the cubic counterparts for the same $\nu$, which complies with theory since the (block-)tridiagonal system for quintic splines is twice the size of the tridiagonal system for cubic splines. However, for increasing $\nu$, the gap becomes smaller since solving the collocation equations, where ${\mathit{A}}_{\mathrm{coll}}$ is of the same dimension for cubic and quintic splines, becomes the bottleneck, see Figure 11. Note that there is only a small difference in runtime between cubic spline collocation with and without virtual control points. This also complies with theory, since the only difference is two additional spline knots, which increases the dimension of ${\mathit{A}}_{\mathrm{coll}}$ by two. Lastly, the small ripples in Figure 10 (left) are due to vectorization and SIMD optimizations handled by EIGEN and the compiler, which give slightly better performance if the dimension of arrays in memory are a multiple of two.

Comparing runtimes for the same $\nu$ might not be a meaningful basis for choosing a method. Instead, one is typically interested in getting the best approximation in the shortest time. For this purpose, the RMS of the residual $r\left(t\right)$ is plotted over runtime in Figure 10 (right). As can be seen, quintic spline collocation significantly outperforms both other variants. Moreover, bad convergence of cubic spline collocation with virtual control points is also visible in this comparison. In addition to measuring total runtime, we also performed a detailed analysis on the relative cost of each code section of Algorithm 2 during quintic spline collocation. Figure 11 demonstrates that in the vicinity of $\nu \approx 160$, evaluating the block-tridiagonal systems to enforce BCs and continuity, and solving ${\mathit{A}}_{\mathrm{coll}}\mathit{\lambda }={\mathit{B}}_{\mathrm{coll}}$ for actual collocation share approximately the same portion of total runtime. With increasing $\nu$, solving for $\mathit{\lambda }$ becomes more relevant since the corresponding system is dense while the block-tridiagonal LSE can be solved efficiently using the recursive scheme discussed in Section 2.3.

Lastly, we want to point out that the condition of ${\mathit{A}}_{\mathrm{C}oll}$ gets worse for increasing count of collocation sites $\nu$, see Figure 10 left. Thus, there might be an upper limit of $\nu$ for the proposed algorithm.

4. Discussion

As shown in the previous section, the presented algorithm performs well if the BCs are consistent and fully known. Even in the case of inconsistent BCs, we still obtain a reasonable approximation as long as we carefully pick the collocation sites. However, if we exceed the optimum, undesired oscillations may occur, which is an indicator for putting too much emphasis on satisfying the underlying ODE while simultaneously trying to compensate the “broken” BCs. In order to automatically determine an optimal partitioning of the spline, a higher level optimization may be applied, which will be the focus of further investigations.

Obviously, if the investigated BVP is well-posed, i.e., if it is not over-determined, other techniques should be preferred over our approach. However, for applications where the enforcement of certain BCs is more important than approximating the underlying dynamics, the proposed method seems to be a valid approach. Moreover, we want to emphasize that no variable recursion or iteration is involved. This makes execution time predictable, which is especially relevant for real-time applications such as in our use case.

Comparing different variants of our algorithm showed that collocation using a quintic spline is in general superior to using the somewhat simpler cubic splines. Note that although its derivation is more involved, the final implementation is of approximately the same complexity, since virtual control points have to be introduced in the cubic case. At this point, we also want to emphasize that, based on the results of our study, we do not recommend to use cubic spline collocation with virtual control points. Although the full set of BCs is satisfied, the additional knots seem to significantly downgrade convergence. However, other choices of $t_{\mathrm{virt,1}}$ and $t_{\mathrm{virt,2}}$ may lead to different results.

Within this contribution, we only considered second-order BVPs. An extension to ODEs of higher order seems to be straightforward, since only the collocation equations, see (58) and (59) have to be extended by the corresponding gradients while the overall dimension of ${\mathit{A}}_{\mathrm{coll}}$ and ${\mathit{B}}_{\mathrm{coll}}$ stays the same. Moreover, our approach may be applied to nonlinear systems as well, by using the common approach of linearization and embedding the scheme into a Newton iteration.

Lastly, we want to highlight that our focus lies on runtime efficiency. However, for embedded systems especially, memory consumption may also be a limiting factor. Although we expect our algorithm to have similar requirements when compared to other techniques, we have not looked into this issue so far.