*6.4. From 2-D Path to Vehicle Formation*

To generate individual paths for each vehicle to follow, we can consider a reference path (obtained via the application of the previous algorithms) and offset each point according to an expression that captures a desired vehicle formation. Start by considering a formation vector denominated *<sup>μ</sup><sup>i</sup>* <sup>∈</sup> <sup>R</sup><sup>3</sup> for each vehicle *<sup>i</sup>*, with each distance defined in the tangential reference frame {*T*} to the virtual target's position in the original curve, according to Figure 11. According to Xie et al. [31], it is possible to define a desired path for each vehicle given by:

$$\mathbf{p}\_{di}(\gamma\_i) = \mathbf{C}(\gamma\_i) + {}\_T^{ll} \mathbf{R}(\gamma\_i) \boldsymbol{\mu}\_{i\prime} \tag{67}$$

where **p***di* is the desired path for the vehicle *i*, **C**(*γi*) is the planned curve, and *<sup>U</sup> <sup>T</sup> R*(*γi*) is a rotation matrix computed according to:

$$\mathbf{r}\_{T}^{\mathrm{II}} \mathcal{R}(\gamma\_{i}) = [\mathbf{r}\_{1}(\gamma\_{i}), \mathbf{r}\_{3}(\gamma\_{i}) \times \mathbf{r}\_{1}(\gamma\_{i}), \mathbf{r}\_{3}(\gamma\_{i})],\tag{68}$$

with:

$$\mathbf{r}\_1(\gamma\_i) = \frac{\partial \mathbf{p}\_d / \partial \gamma}{\left\| \partial \mathbf{p}\_d / \partial \gamma \right\|}, \text{ with } \left\| \partial \mathbf{p}\_d / \partial \gamma \right\| \neq 0 \qquad \mathbf{r}\_3(\gamma\_i) = \frac{\mathbf{r}\_d - \left( \mathbf{r}\_d \cdot \mathbf{r}\_1(\gamma\_i) \right) \mathbf{r}\_1(\gamma\_i)}{\left\| \mathbf{r}\_d - \left( \mathbf{r}\_d \cdot \mathbf{r}\_1(\gamma\_i) \right) \mathbf{r}\_1(\gamma\_i) \right\|}, \tag{69}$$

such that **r**<sup>1</sup> is the tangent to the curve. Moreover, since all vehicles will only be required to operate in a 2-D plane, a trivial definition for one of the axis of the tangential frame {*T*} is **r***<sup>d</sup>* = [0, 0, 1] *T*.

**Figure 11.** Formation vector.

A path might not be not parameterised according to the arc length and, for B-splines in particular, each spline segment is such that *γ* ∈ [0, 1]. Therefore, it is commonplace to define a constant required speed *V* ≤ *Vmax* for the vehicle and let the desired speed profile for the virtual targets be given by:

$$w\_L(\gamma) = \frac{V}{||\mathbf{p}'\_d(\gamma)||}. \tag{70}$$
