6.3.1. Define the Number of Segments to Use

Consider now the ordered sequence of *K* points obtained via the application of Algorithms 2 and 3 to the original 2-D point cloud. In order to fit the points with a parametric curve, we are required to attribute to each point *<sup>X</sup><sup>k</sup>* <sup>∈</sup> <sup>R</sup><sup>2</sup> a corresponding *<sup>γ</sup><sup>k</sup>* in the target parametric curve. This problem could be formulated as a nonlinear optimisation problem which is computationally demanding to solve for real-time applications. A non-optimal, but more efficient solution, proposed by Liu M. et al. [30] for Simultaneous Localisation and Mapping (SLAM) applications, is to consider *DX* to be the total distance between the points, given by:

$$D\_X := \sum\_{k=2}^{K} \|\mathbf{X}\_k - \mathbf{X}\_{k-1}\|\_\prime \tag{54}$$

and the corresponding vector of parametric values *γ* = [*γ*1, ..., *γk*] *<sup>T</sup>* to be given by:

$$\begin{cases} \gamma\_1 = 0, \\ \gamma\_k = \gamma\_{k-1} + \frac{\|\mathbf{X}\_k - \mathbf{X}\_{k-1}\|}{D\_\mathcal{X}} \gamma\_{\max}, k = \mathbf{2}, \dots, \mathbf{K}, \end{cases} \tag{55}$$

where *γmax* is the maximum parameter value of the parametric curve. For cubic B-splines, this number depends directly on the number of control points *NC* that the target curve will have, such that *γmax* = *NC* − 3. The number of control points also dictates how many spline segments are used for the fitting problem. The optimal number of control points can be obtained by solving yet another nonlinear optimisation problem, but due to the real-time nature of the problem, this option is disregarded. Given that a uniform cubic B-spline must have at least four control points to define one segment, and that a low number of sections can under-fit a long set of points whilst a high number leads to over-fitting issues, this number should not be a static constant either. A non-optimal yet dynamic way of defining the number of control points *NC* is by taking:

$$N\_{\mathbb{C}} := \max \left\{ \left\lfloor \frac{D\_{\mathbb{X}}}{\rho} \right\rfloor, 4 \right\} \,, \tag{56}$$

with (1/*ρ*) > 0 being a control point's density (tunning parameter defined a priori). A smaller *ρ* leads to a higher *NC*. Applying this method to the previous example, and considering *NC* = 7, *γmax* = *γ*<sup>11</sup> = 4, the result in Figure 8 is obtained.

**Figure 8.** Ordered set of points with parametric values associated to them (example).

6.3.2. Fitting the Points with a Uniform Cubic B-Spline

For fitting the ordered set of points *X*† with a non-clamped uniform cubic B-spline *C*(*γ*, **P**), an optimisation problem is formulated. Consider the objective function given by:

$$f(\mathbf{P}) := \underbrace{\sum\_{k=1}^{K} \left\| \mathbf{C}(\gamma\_{k\prime} \mathbf{P}) - \mathbf{X}\_k \right\|^2}\_{\text{goal}} + F\_{r\prime} \tag{57}$$

with:

$$F\_{\mathbf{r}} = \underbrace{\lambda \int\_{0}^{\gamma\_{\max}} \left\| \frac{\partial \mathcal{C}(\gamma\_{\prime}, \mathbf{P})}{\partial \gamma} \right\|^{2} d\gamma + \beta \int\_{0}^{\gamma\_{\max}} \left\| \frac{\partial^{2} \mathcal{C}(\gamma\_{\prime}, \mathbf{P})}{\partial \gamma^{2}} \right\|^{2} d\gamma}\_{\text{regularisation term}} \tag{58}$$

where **P** = [*P<sup>x</sup>* <sup>0</sup> , ..., *<sup>P</sup><sup>x</sup> Nc*−1, *<sup>P</sup><sup>y</sup>* <sup>0</sup> , ..., *<sup>P</sup><sup>y</sup> Nc*−1] *<sup>T</sup>* <sup>∈</sup> <sup>R</sup>2*Nc* is the vector of control points that defines the target curve. The first term minimises the distance between the target B-spline curve and the set of points, whilst *Fr* is a regularisation term and *α*, *γ* ≥ 0 are the regularisation variables. The integral of the *L*<sup>2</sup> <sup>2</sup> norm of the first derivative penalises the total length of the curve, while the integral of the *L*<sup>2</sup> <sup>2</sup> norm of the second derivative penalises bends in the path. This objective function can also be expressed using vector notation, according to:

$$f(\mathbf{P}) = \underbrace{\left\|B(\boldsymbol{\gamma})\mathbf{P} - \mathbf{X}\right\|^2}\_{\text{goal}} + \underbrace{\lambda \mathbf{P}^T R\_1 \mathbf{P} + \beta \mathbf{P}^T R\_2 \mathbf{P}}\_{\text{regularisation term}}\tag{59}$$

where **X** = [*X<sup>x</sup>* <sup>1</sup> , ..., *<sup>X</sup><sup>x</sup> <sup>K</sup>*, *<sup>X</sup><sup>y</sup>* <sup>1</sup> , ..., *<sup>X</sup><sup>y</sup> <sup>K</sup>*] denotes the points to fit, and *R*1, *R*<sup>2</sup> are constant matrices that can be computed numerically (see Appendix D).

In order to define the new path, it would not suffice to discard the previously planned curve defined after *γ<sup>s</sup>* and minimise the objective function with respect to the control points. To guarantee <sup>C</sup><sup>2</sup> continuity between the previous path and the newly planned one, linear equality constraints should be imposed on the values of **C***new*(0), **C***new* (0), and **C***new* (0) of the new curve. Moreover, it is a requirement to save the old curve up to *γs*, as it may still be in use by other vehicles in the network.

Consider the re-planning point **p***<sup>s</sup>* introduced previously, chosen such that it corresponds to the transition between the spline segment that the virtual target of the drone is "sitting on", and the next segment, according to:

$$\mathbf{p}\_s = \mathbf{C}^{old}(\gamma\_s) \text{ with } \gamma\_s = \lceil \gamma\_{dronc} \rceil,\tag{60}$$

where *γdrone* corresponds to the quadrotor's virtual target at time instant *tk*. With this choice of *γs*, it is possible to take advantage of the local support property of B-splines and simplify the equality constraints of the problem, while at the same time simplifying the storage of the curves in memory.

Considering that **p***<sup>s</sup>* is dictated by (60), the old curve segments that are described by parametric values such as *γ* ≥ *γ<sup>s</sup>* should be discarded and replaced by a newer curve. Since each curve segment is defined by only four control points, discarding those segments is equivalent to removing control points with indexes *i* ≥ *γ<sup>s</sup>* + 3 from the old control points vector. This operation results in a vector given by:

$$\mathbf{P}^{old} = [\mathbf{P}^{x}\_{0}, \mathbf{P}^{x}\_{1}, \dots, \mathbf{P}^{x}\_{\gamma\_{s} \prime}, \mathbf{P}^{x}\_{\gamma\_{s} + 1 \prime}, \mathbf{P}^{x}\_{\gamma\_{s} + 2 \prime}, \mathbf{P}^{y}\_{0}, \mathbf{P}^{y}\_{1}, \dots, \mathbf{P}^{y}\_{\gamma\_{s} \prime}, \mathbf{P}^{y}\_{\gamma\_{s} + 1 \prime}, \mathbf{P}^{y}\_{\gamma\_{s} + 2}]^{T}. \tag{61}$$

*f*(**P***new*)

For the particular example in Figure 9, spline 1 (in green) should be discarded given that *γdrone* ∈ [0, 1); hence, *γ<sup>s</sup>* = 1 and spline 0 are kept. To achieve this, all the control points with indexes *<sup>i</sup>* <sup>≥</sup> <sup>1</sup> <sup>+</sup> 3 should be removed from the control points vector **<sup>P</sup>***old*, i.e., *P*<sup>4</sup> = (*P<sup>x</sup>* <sup>4</sup> , *<sup>P</sup><sup>y</sup>* 4 ).

Making use of the local support property once more, it is known that <sup>C</sup><sup>2</sup> continuity between two consecutive cubic spline segments is guaranteed, as long as the last three control points of the first segment coincide with the first three control points of the second segment. A trivial way of generating a new B-spline with guarantees of <sup>C</sup><sup>2</sup> continuity in the transition with the old curve, without explicitly defining equality constraints on the derivatives of the function, is to solve the following optimisation problem:

> **P***new* = argmin **P***new*

subject to

$$
\begin{bmatrix} P\_0^{x \ n \nu w} \\ P\_1^{x \ n \nu w} \\ P\_2^{x \ n \nu w} \\ P\_0^{y \ n \nu w} \\ P\_1^{y \ n \nu w} \\ P\_2^{y \ n \nu w} \end{bmatrix} = \begin{bmatrix} P\_{\gamma\_s}^{x} \\ P\_{\gamma\_s + 1}^{x} \\ P\_{\gamma\_s + 2}^{x} \\ P\_{\gamma\_s}^{y} \\ P\_{\gamma\_s + 1}^{y} \\ P\_{\gamma\_s + 2}^{y} \end{bmatrix} \tag{62}
$$

where **P***new* = [*Px new* <sup>0</sup> , ..., *<sup>P</sup>x new NC*−1, *<sup>P</sup>y new* <sup>0</sup> , ..., *<sup>P</sup>y new NC*−1] *<sup>T</sup>* is a new control points vector.

**Figure 9.** Solving the optimisation problem (example).

To keep track of old and new curves, it is possible to concatenate only the new control points vector **P***new* with the old control points vector **P***old*, ignoring the first three control points, i.e., *Pnew* <sup>0</sup> , *<sup>P</sup>new* <sup>1</sup> , and *<sup>P</sup>new* <sup>2</sup> , which are repeated as a result of the equality constraints imposed by (62). Applying this methodology to the previous example, the final control points vector is given according to Figure 10.

**Figure 10.** Final curve with control points concatenated (example).

These series of procedures are summarised in Algorithm 4. For the sake of simplicity, the separation between the X- and Y-coordinates of the control points was omitted.

#### **Algorithm 4** Fitting the points—growing a uniform cubic B-spline


$$\mathbf{P} = \begin{bmatrix} P\_{\mathbf{0}\prime} \boldsymbol{P}\_{\mathbf{1}\prime} \dots \boldsymbol{P}\_{\boldsymbol{\gamma}\prime \prime} \boldsymbol{P}\_{\boldsymbol{\gamma}\prime + 1\prime} \boldsymbol{P}\_{\boldsymbol{\gamma}\_{\rm s} + 2\prime} \boldsymbol{P}\_{\boldsymbol{\gamma}\_{\rm s} + 3\prime} \boldsymbol{P}\_{\boldsymbol{\gamma}\_{\rm s} + 4\prime} \dots \boldsymbol{P}\_{\mathbf{n}} \end{bmatrix}^{T};\tag{63}$$

3: Remove control points (corresponding to splines to be re-planned) from the original control points vector, such that:

$$\mathbf{P}^{old} = \begin{bmatrix} P \mathbf{0}, P\_1, \dots, P\_{\gamma\_s}, P\_{\gamma\_s + 1}, P\_{\gamma\_s + 2} \end{bmatrix}^T; \tag{64}$$

4: Solve the optimisation problem in (62) and obtain a new vector with *NC* control points:

$$\begin{aligned} \mathbf{P}^{\rm ncw} &= \left[ P\_0^{\rm ncw}, P\_1^{\rm ncw}, P\_2^{\rm ncw}, \dots, P\_{N\_\mathbb{C}-1}^{\rm ncw} \right]^T, \\ \text{with } P\_0^{\rm ncw} &= P\_{\gamma\_\*}, P\_1^{\rm ncw} = P\_{\gamma\_\* + 1}, P\_2^{\rm ncw} = P\_{\gamma\_\* + 2}. \end{aligned} \tag{65}$$

5: Concatenate the new vector with the old vector (ignoring the first three control points, which are repeated):

$$\mathbf{P}^{final} = \begin{bmatrix} P\_{\rm{0}}, P\_{1\prime}, \dots, P\_{\gamma\_{\rm{sr}}}, P\_{\gamma\_{\rm{s}}+1}, P\_{\gamma\_{\rm{s}}+2}, P\_{\rm{3}}^{new}, \dots, P\_{\rm{N}\_{\rm{C}}-1}^{new} \end{bmatrix}^{T}. \tag{66}$$
