In this section, we initially present the theoretical formalization of our problem. We describe mathematically the robot’s constraints and the Pythagorean Hodograph formulation. We then provide the foundations for calculating the PH curves for which the union of several paths keeps the continuous curvature profile in the final plan. Finally, we show an optimization problem formulation that will allow us to generate short paths that are feasible by the robot.

### Problem formalization

We assumed an obstacle-free environment, where the only restrictions for navigation are imposed by its own kinematic constraints. A pair of waypoints, \({\langle {p_{i}},{p_{f}} \rangle }\), mark the initial and final poses, respectively, which define the position and (partially) the orientation of the robot at the extreme points of a single path. This path may be defined mathematically as a parametric curve \({\vec{r}}(t)\) in \(\mathbb{R }^3\), where \(t\) is a parameter that continuously varies in \(\mathbb{R }\). In this manner, planning a path between two waypoints can be formally posed as:

$$\begin{aligned} \begin{aligned}&{p_{i}}(x_{i},y_{i},z_{i},\psi _{i},\theta _{i})={\vec{r}}(t_{i}), \\&{p_{f}}(x_{f},y_{f},z_{f},\psi _{f},\theta _{f})={\vec{r}}(t_{f}), \end{aligned} \end{aligned}$$

(1)

where \(t_{i}\) and \(t_{f}\) are the initial and final values, respectively, for the curve parameter \(t\).

Each waypoint \({p_{}}\) is described by three-position (*x*, *y*, *z*) and two-orientation (\(\psi ,\theta \)) variables. The variable \(\psi \) is an angle that describes the waypoint orientation parallel to the *XY* plane in relation to the \(\mathbf X \) axis. We define \(\theta \) as the waypoint orientation measured in relation to the \(\mathbf X \) axis and parallel to *XZ* plane. Here, we must say that the roll angle (\(\phi \) in conventional aeronautical notation) is not considered, since it has no physical sense regarding the curve in the 3D space. In other words, the curve represents a trajectory to be followed, not the actual trajectory being executed by the robot.

Now, given a sequence of \({P}\) waypoints, \(\mathcal{P }={\langle {p_{1}},\ldots ,{p_{{P}}} \rangle }\), that must be reached by the robot, a total feasible path \({\mathcal{R }}\) will be the union of each \({\vec{r}}(t)\) calculated between each sequence pair in \(\mathcal{P }\). This is formalized in Eq. (2):

$$\begin{aligned} {\mathcal{R }}=\left\{ \bigcup _{k=1}^{{P}-1}{\vec{r}}_k \in \mathbb{R }: {\vec{r}}_k \;\mathrm{is\; a\; single\; feasible\; path}\right\} . \end{aligned}$$

(2)

#### Constraints

In order to be considered a feasible path for a given robot, the curve \({\vec{r}}(t)\) must respect the kinematic constraints of the vehicle. The three motion constraints mentioned before are the maximum curvature (\({\kappa _{\mathrm{max}}}\)), the maximum torsion (\({\tau _{\mathrm{max}}}\)) and the maximum climb (or dive) angle (\({\theta _{\mathrm{max}}}\)) that are realizable by the robot in the three-dimensional space. It is possible to completely define a curve in \(\mathbb{R }^3\) as a function of curvature and torsion only [16].

As far as the underlying physics of the system is concerned, the curvature may be defined as a quantity that is directly proportional to the lateral acceleration of the robot in the space. The value of \({\kappa _{\mathrm{max}}}\) is inversely proportional to the minimum curvature radius (\({\rho _{\mathrm{min}}}\)) of the curve that the vehicle is capable of executing, which is also proportional to the maximum lateral acceleration of the vehicle.

The *curvature function* of a curve in the \(n\)-dimensional space is given by Eq. (3):

$$\begin{aligned} \kappa (t)=\frac{|\dot{{\vec{r}}}(t)\times \ddot{{\vec{r}}}(t)|}{|\dot{{\vec{r}}}(t)|^3} . \end{aligned}$$

(3)

The torsion of the curve is directly proportional to the angular moment (roll moment) of the robot, which is also physically limited. Thus, the value of \({\tau _{\mathrm{max}}}\) is given as a function of the minimum torsion radius (\({\sigma _{\mathrm{min}}}\)) that the robot can describe in the space. The torsion of a curve can be computed by Eq. (4):

$$\begin{aligned} \tau (t)=\frac{[\dot{{\vec{r}}}(t)\times \ddot{{\vec{r}}}(t)]\cdot \dddot{{\vec{r}}}(t)}{|\dot{{\vec{r}}}(t)\times \ddot{{\vec{r}}}(t)|^2} . \end{aligned}$$

(4)

Finally, the climb (or dive) angle is proportional to the ascent (or descent) rate of the robot in the three-dimensional space. In other words, it captures the variation of the altitude (*z*) throughout the path. For vehicles with limited values of inclination angle, such as fixed-wing aircrafts, this is a key variable. The value of \({\theta _{\mathrm{max}}}\) may depend on many factors, as the translation speed and the spatial orientation of the robot. The climb angle function of a three-dimensional parametric curve is given by Eq. (5):

$$\begin{aligned} \theta (t)=\text{ tan}^{-1} \left(\frac{\dot{z}(t)}{\sqrt{\dot{x}(t)^2+\dot{y}(t)^2}}\right) . \end{aligned}$$

(5)

It is possible to show that this function is mathematically limited to the interval \([-\tfrac{\pi }{2},\tfrac{\pi }{2}]\). The same is true for the value of \({\theta _{\mathrm{max}}}\).

Finally, the path \({\vec{r}}(t)\) is valid for a vehicle if the modulus of the curvature, torsion and climb angle functions are smaller than the vehicle’s absolute maximum values, as stated in Eq. (6):

$$\begin{aligned} {\vec{r}}(t)&\!\!=\{{\vec{r}}\in \mathbb{R }: |\kappa (t)|\!<\!{\kappa _{\mathrm{max}}},|\tau (t)|\!<\!{\tau _{\mathrm{max}}},|\theta (t)| \!<\!{\theta _{\mathrm{max}}}\}. \nonumber \\ \end{aligned}$$

(6)

It is important to note that the above maximum values result from the dynamic constraints of the vehicle. Therefore, since our kinematic approach guarantees continuity of the curvature, torsion, and climb angle functions, by respecting the maximum variable values, indirectly the vehicle dynamics are accounted for. Discontinuities in the curvature function, for example, may induce infinite variations of lateral acceleration, which of course, are not realizable. The same is true for torsional values. In the case of the climb angle function, the lack of continuity implies in the tangential discontinuity of the curve itself.

In the next section, we detailed the necessary steps to realize the union in Eq. (2) in order to guarantee curvature and climb angle continuity for \({\mathcal{R }}\). As expressed by Eq. 3, the curve produced by the path planning algorithm must be continuously derivable, and it must also be second order differentiable (\(C^2\)).

#### Spatial Pythagorean Hodograph curves

Spatial Pythagorean Hodograph curves are a special kind of parametric polynomial curves defined in \(\mathbb{R }^3\). They are represented in general as:

$$\begin{aligned} {\vec{r}}(t)=[x(t),y(t),z(t)], \end{aligned}$$

and their derivatives (Hodograph components) satisfy the Pythagorean condition presented in Eq. (7):

$$\begin{aligned} \dot{x}(t)^2+\dot{y}(t)^2+\dot{z}(t)^2=h(t)^2, \end{aligned}$$

(7)

for some polynomial \(h(t)\). This means that the parametric “speed”, \(\dot{s}(t)\), of the curve can be described by a polynomial, making it possible to compute the length of the path, \(s\), as shown in Eq. (8):

$$\begin{aligned} s=\int \limits _{t_{i}}^{t_{f}}\sqrt{\dot{x}(t)^2+\dot{y}(t)^2+\dot{z}(t)^2}\,\mathrm{d}t=\int \limits _0 ^1 |h(t)|\,\mathrm{d}t. \end{aligned}$$

(8)

Spatial PH curves are further shaped as \(n\)-th order Bézier curves presented in Eq. (9):

$$\begin{aligned} \vec{r}(t)=\sum _{k=0}^{n}{\mathbf{p }_{k}}\genfrac(){0.0pt}{}{n}{k}(1-t)^{n-k}t^k;\quad t \in [0, 1] \end{aligned}$$

(9)

where \({\mathbf{p }_{k}}=[x_k,y_k,z_k]\) is the \(k\)-th control point of the Bézier function. \(N\)-th order Bézier curves always have \(n\) + 1 control points that define the curve shape according to their spatial position [16].

The path planning problem is then reduced to find a solution to an Hermite Interpolation problem. One important advantage of using this model is that the resulting curve is infinitely continuous, so that the curvature, torsion and inclination functions are always smooth.

To guarantee that the PH does not violate the kinematic constraints of the vehicle, Shanmugavel et al. [21] propose a modification to iteratively compute the PH. It uses gain factors to modify the position of some of the control points in every step of an iterative algorithm, until \({\vec{r}}(t)\) becomes realizable. We will discuss these characteristics later in this article. However, Shanmugavel’s method only consider the curvature and torsion constraints. As it will be shown later, for vehicles with bounded values of climb (or dive) angle, it is not possible to minimize the problem for that spatial cost function. Therefore, we propose a new cost function that incorporates all three variables along with limitations on their maximum values. The \({\theta _{\mathrm{max}}}\) constraint will be included in the PH computation, which will then generate feasible paths for robots in three-dimensional space, under all the aforementioned constraints.

### Smooth path calculation

In this section, we present a method to calculate Bézier curves that fulfills the three constraint conditions mentioned before. We begin by mentioning an interesting characteristic of Bézier curves. They are infinitely differentiable functions, which mean that one curve has continuous curvature profiles. However, if we connect two or more curves to compose a path \({\mathcal{R }}\) among several waypoints, it is possible that discontinuity in accelerations will show up at the connecting points, violating the \(C^2\) condition. Then, in order to keep the continuity, we must calculate Bézier curves whose curvature values at extreme points are identical.

According to [20], the initial curvature of a Bézier function can be mathematically determined by Eq. (10):

$$\begin{aligned} \kappa (t)|_{t=0}=\dfrac{(n-1)}{n}\dfrac{|{\mathbf{p }_{2}}-{\mathbf{p }_{1}}|}{|{\mathbf{p }_{1}}-{\mathbf{p }_{0}}|} \sin \sigma _i, \end{aligned}$$

(10)

where \(n\) is the Bézier order and \(\sigma _i\) represents the angle between the vectors \(\overrightarrow{{\mathbf{p }_{0}}{\mathbf{p }_{1}}}\) and \(\overrightarrow{{\mathbf{p }_{1}}{\mathbf{p }_{2}}}\).

It is possible to guarantee a null curvature value at the initial point by forcing collinearity of these vectors. The same idea can be extended to the final curvature value, as shown in Eq. (11):

$$\begin{aligned} \kappa (t)|_{t=1}=\dfrac{(n-1)}{n}\dfrac{|{\mathbf{p }_{7}}-{\mathbf{p }_{6}}|}{|{\mathbf{p }_{6}}-{\mathbf{p }_{5}}|} \sin \sigma _f, \end{aligned}$$

(11)

where \(\sigma _f\) is the angle between the vectors \(\overrightarrow{{\mathbf{p }_{5}}{\mathbf{p }_{6}}}\) and \(\overrightarrow{{\mathbf{p }_{6}}{\mathbf{p }_{7}}}\).

Then, we will always have zero curvature value at the extreme points of each Bézier, making its composition with others curves (\({\mathcal{R }}\)) a \(C^2\) function, as required. The main reason which led us to choose a seventh order function is now clear. We must keep the three initial and three final control points of the Bézier aligned. So we need at least other two points to create a function with at least one point of inflexion, in order to make a more flexible path.

With this in mind, we use an adaptation of the Pythagorean Hodograph formalism to compute these eight control points. Therefore, we may use the set of Eq. 12, which depends only on the initial and final configuration points, and represent an Hermite Interpolation system.

$$\begin{aligned} {\mathbf{p }_{0}}&= [x_{i},y_{i},z_{i}] ,\end{aligned}$$

(12)

$$\begin{aligned} {\mathbf{p }_{1}}&= {\mathbf{p }_{0}}+{c_0}\,{\mathcal{A }_{0}}\mathbf{i }{\mathcal{A }_{0}^*},\end{aligned}$$

(13)

$$\begin{aligned} {\mathbf{p }_{2}}&= {\mathbf{p }_{1}}+{c_0}\,{\mathcal{A }_{0}}\mathbf{i }{\mathcal{A }_{0}^*},\end{aligned}$$

(14)

$$\begin{aligned} {\mathbf{p }_{3}}&= {\mathbf{p }_{2}}+\dfrac{{c_0}}{2}({\mathcal{A }_{0}}\mathbf{i }{\mathcal{A }_{1}^*}+{\mathcal{A }_{1}}\mathbf{i }{\mathcal{A }_{0}^*}),\end{aligned}$$

(15)

$$\begin{aligned} {\mathbf{p }_{4}}&= {\mathbf{p }_{3}}-\dfrac{{c_7}}{2}({\mathcal{A }_{2}}\mathbf{i }{\mathcal{A }_{1}^*}+{\mathcal{A }_{1}}\mathbf{i }{\mathcal{A }_{2}^*}),\end{aligned}$$

(16)

$$\begin{aligned} {\mathbf{p }_{5}}&= {\mathbf{p }_{6}}-{c_7}\,{\mathcal{A }_{2}}\mathbf{i }{\mathcal{A }_{2}^*},\end{aligned}$$

(17)

$$\begin{aligned} {\mathbf{p }_{6}}&= {\mathbf{p }_{7}}-{c_7}\,{\mathcal{A }_{2}}\mathbf{i }{\mathcal{A }_{2}^*},\end{aligned}$$

(18)

$$\begin{aligned} {\mathbf{p }_{7}}&= [x_{f},y_{f},z_{f}]. \end{aligned}$$

(19)

where \({c_0}\) and \({c_7}\) are gain factors that determine the magnitude of the vectors and have unit values for PH with no constraints. The above set of equations was derived from [9], and according to the authors, it represents an underdetermined system. In other words, there are infinitely many spatial PH curves that interpolate the initial and final waypoints.

The PH provides several properties that can be considered advantageous in the path planning problem, of which, the most relevant for this study are: (i) the uniform distribution of points along the curve, which contributes to the smoothness of the path; (ii) the parametric speed (first order derivative) that provides a continuous velocity profile; and finally (iii) the capability to admit real-time interpolator algorithms for computer numerical control.

Once the Bézier points are computed, we must guarantee that the generated curve does note violate the kinematic constraints of the vehicle, according to the constraints conditions of Eq. (6). Hence, we must determine the values of \({c_0}\) and \({c_7}\) in Eq. (12). As there is no closed form solution, these values will be estimated, in the next section, as the solution to an optimization problem .

The problem is initially approached by assuming that the extreme points of the curve (\({\mathbf{p }_{0}}\) and \({\mathbf{p }_{7}}\)) are directly determined by the initial and final poses \({p_{i}}\) and \({p_{f}}\), respectively. All the remaining points will depend on these pose vectors and on the \({c_0}\) and \({c_7}\) gains.

In our development, we use the quaternion formulation, which allows for a more compact and elegant representation of the system, as well as providing a clearer geometric view of the solution. The parameters \({\mathcal{A }_{k}}\), with \(k=\{1,2,3\}\), are quaternions described by Eq. (20),

$$\begin{aligned} {\mathcal{A }_{k}}=a+a_x\mathbf i +a_y\mathbf j +a_z\mathbf k =\left[ \begin{array}{c} a \\ a_x \\ a_y \\ a_z \end{array} \right] , \end{aligned}$$

(20)

where \(a\) corresponds to the scalar part and \([a_x,a_y,a_z]\) to the vector elements of the quaternion. The conjugate of \({\mathcal{A }_{k}}\) is denoted as \({\mathcal{A }_{k}^*}\). A set of equations of this type presents a solution \({\mathbf{c }}\) which also is a pure vector quaternion, as shown by Eq. (21):

$$\begin{aligned} {\mathcal{A }_{}}\mathbf{i }{\mathcal{A }_{}^*}={\mathbf{c }}, \end{aligned}$$

(21)

where \(\mathbf{i }\) is a pure vector quaternion since its scalar part is null and \(\mathbf{i }^2=-1\).

Hence, a system which is composed of three equations and four unknown variables can be formed, leaving one degree of freedom yet to be determined. With this, the solution for every \({\mathcal{A }_{k}}\) is parameterized by an angular variable \(\phi _k\) (for a detailed explanation, the reader is referred to [10]). Deriving Eq. (21) with respect to \({\mathcal{A }_{0}}\), one obtains:

$$\begin{aligned} {\mathcal{A }_{0}}(\phi _0)=\sqrt{\frac{|{{\mathbf{d}}_{i}}|}{2}(1+\lambda _i)}\left[ \begin{array}{c} -\sin (\phi _0) \\ \cos (\phi _0) \\ \dfrac{\mu _i\cos (\phi _0)+\nu _i\sin (\phi _0)}{1+\lambda _i} \\ \dfrac{\nu _i\cos (\phi _0)-\mu _i\sin (\phi _0)}{1+\lambda _i} \end{array} \right], \nonumber \\ \end{aligned}$$

(22)

where \({{\mathbf{d}}_{i}}\) is the vector direction of the pose \({p_{i}}\), computed as

$$\begin{aligned} {{\mathbf{d}}_{i}}={c_0}[\cos (\psi _i)\cos (\theta _i),\sin (\psi _i)\cos (\theta _i),\sin (\theta _i)] \end{aligned}$$

and

$$\begin{aligned}{}[\lambda _i,\mu _i,\nu _i]=\dfrac{{{\mathbf{d}}_{i}}}{|{{\mathbf{d}}_{i}}|}. \end{aligned}$$

Equivalently, \({\mathcal{A }_{2}}\) can be expressed as:

$$\begin{aligned} {\mathcal{A }_{2}}(\phi _2)\!=\!\sqrt{\frac{|\mathbf{d}_{f}|}{2}(1+\lambda _f)}\left[ \begin{array}{c} -\sin (\phi _2) \\ \cos (\phi _2) \\ \dfrac{\mu _f\cos (\phi _2)+\nu _f\sin (\phi _2)}{1+\lambda _f} \\ \dfrac{\nu _f\cos (\phi _2)-\mu _f\sin (\phi _2)}{1+\lambda _f} \end{array} \right], \nonumber \\ \end{aligned}$$

(23)

where \(\mathbf{d}_{f}\) is the vector direction of the pose \({p_{f}}\), given by:

$$\begin{aligned} \mathbf{d}_{f}={c_7}[\cos (\psi _f)\cos (\theta _f),\sin (\psi _f)\cos (\theta _f),\sin (\theta _f)] \end{aligned}$$

and

$$\begin{aligned}{}[\lambda _f,\mu _f,\nu _f]=\dfrac{\mathbf{d}_{f}}{|\mathbf{d}_{f}|}. \end{aligned}$$

Once the values of \({\mathcal{A }_{0}}\) and \({\mathcal{A }_{2}}\) are determined by setting the values of \(\phi _0\) and \(\phi _2\), \({\mathcal{A }_{1}}\) can be defined as a function of a third angle variable \(\phi _1\):

$$\begin{aligned} {\mathcal{A }_{1}}(\phi _1)=-\dfrac{3}{4}({\mathcal{A }_{0}}+{\mathcal{A }_{2}})+\dfrac{\sqrt{\dfrac{|{\mathbf{c }}|}{2}(1+\lambda )}}{4} {\mathcal{A }_{c}}(\phi _1) , \end{aligned}$$

(24)

where

$$\begin{aligned} {\mathcal{A }_{c}}(\phi _1)=\left[ \begin{array}{c} -\sin (\phi _1) \\ \cos (\phi _1) \\ \dfrac{\mu \cos (\phi _1)+\nu \sin (\phi _1)}{1+\lambda } \\ \dfrac{\nu \cos (\phi _1)-\mu \sin (\phi _1)}{1+\lambda } \end{array} \right] , \end{aligned}$$

\({\mathbf{c }}\) is a pure vector quaternion given by

$$\begin{aligned} {\mathbf{c }}=120({\mathbf{p }_{f}}-{\mathbf{p }_{i}})-15({{\mathbf{d}}_{i}}+\mathbf{d}_{f})+5({\mathcal{A }_{0}}\mathbf{i }{\mathcal{A }_{2}^*}+{\mathcal{A }_{2}}\mathbf{i }{\mathcal{A }_{0}^*}) \nonumber \\ \end{aligned}$$

(25)

and

$$\begin{aligned}{}[\lambda ,\mu ,\nu ]=\dfrac{{\mathbf{c }}}{|{\mathbf{c }}|} . \end{aligned}$$

### Iterative algorithm

To compute the spatial PH, it is necessary to determine the values of the angles \(\phi _0\), \(\phi _1\), \(\phi _2\) and the gains \({c_0}\) and \({c_7}\). The angular variables are defined for the range \([-\tfrac{\pi }{2},\tfrac{\pi }{2}]\). In [10], authors claim (empirically) that it is possible to represent the solution space in extensive form enough, by the quantization of the values of each of these angles according to:

$$\begin{aligned} \phi _k=\left[-\tfrac{\pi }{2},-\tfrac{\pi }{4},0,\tfrac{\pi }{4},\tfrac{\pi }{2}\frac{}{}\right]. \end{aligned}$$

The combinatorial arrangement of these five values for each \(\phi _k\) leads to a total of 125 solutions for the PH curve. The authors also argue that for most cases, \(\phi _1\) may be equal to \(-\tfrac{\pi }{2}\), without the loss of generality, reducing the number of solutions to 25. This value, generally, makes the final result to minimize the variation under the \(\mathbf Z \) axis, what also reduces the climb angle function.

Each of these solutions has different curvature, torsion and climb angle functions. The best solution is the one that minimizes the cost function of the path, or the elastic bending energy function [8]:

$$\begin{aligned} \mathcal{E }=\int \limits _0 ^1\omega (t)^2 \,|\dot{\vec{r}}(t)|\,\mathrm{d}t, \end{aligned}$$

(26)

where \(\omega (t)\) is the total curvature of the spatial PH, which is given by:

$$\begin{aligned} \omega (t)=\sqrt{\kappa (t)^2+\tau (t)^2}. \end{aligned}$$

(27)

This solution, however, is not satisfactory for the problem considered here, since it does not take into account the climb angle function in the energy computation. As it will be shown later, the PH curves that minimize the cost function, may present climb (or dive) angles unattainable for a given robot.

To tackle this issue, the following elastic bending energy function is proposed in [2]:

$$\begin{aligned} \omega (t)=\sqrt{\left(\frac{\kappa (t)}{{\kappa _{\mathrm{max}}}}\right)^2+\left( \frac{\tau (t)}{{\tau _{\mathrm{max}}}}\right)^2+\left(\frac{\theta (t)}{{\theta _{\mathrm{max}}}} \right)^2}. \end{aligned}$$

(28)

Besides minimizing the rate of growth of the vehicle’s climbing action in three-dimensional space, this function still takes into account the three aforementioned constraints and their maximum values in a normalized form. The problem with this is the increase in the computational complexity in relation to the previous equation.

We show in [1] that a more satisfactory result can be obtained using the following cost function:

$$\begin{aligned} \omega (t)=|\theta (t)|. \end{aligned}$$

(29)

Instead of considering the curvature and torsion profiles, this new function takes into account only the aforementioned climb constraint of the vehicle, which minimizes the cost of algorithm and contributes to the smoothing of the climb rate of the curve. It is possible to demonstrate that, by minimizing \(\theta (t)\), which means to reduce the \(\dot{z}(t)\) over the \(XY\) plane, the curvature and, mainly, the torsion function also decrease.

We verified experimentally that, in most cases, the smallest values of \(\mathcal{E }\) were obtained when \(\phi _1=-\tfrac{\pi }{2}\) (as seen in the previous case), and when the difference between \(\phi _0\) and \(\phi _2\) was the largest possible. Therefore, taking \(\phi _0\) in the \(\phi _k\) interval and \(\phi _2=-\phi _0\), makes it possible to reduce the number of solutions to only five.

Finally, the values of \({c_0}\) and \({c_7}\) (the gains for which the spatial PH fulfills the requirements described in Eq. (6) still remains to be determined. This naturally leads to an optimization approach, since there is no closed solution for this problem. Increasing these gains tends to minimize the function \(\omega (t)\) as a whole, which is indirectly linked to the inverse of the parametric “speed” of the curve. However, that produces, as a consequence, an increase in path length. Thus, the ideal values of \({c_0}\) and \({c_7}\) are those which produce a feasible curve for a given robot, but also minimizes the length of \(s\).

In the first step of the algorithm, the values of these gain factors are set to one. In each iteration of the algorithm, the conditions of Eq. (6) are observed, and gains are increased by

$$\begin{aligned} {c_0}&= {c_0}+\frac{\mathcal{E }_0}{\mathcal{E }}({\rho _{\mathrm{min}}}+{\sigma _{\mathrm{min}}}) ,\end{aligned}$$

(30)

$$\begin{aligned} {c_7}&= {c_7}+\frac{\mathcal{E }_7}{\mathcal{E }}({\rho _{\mathrm{min}}}+{\sigma _{\mathrm{min}}}), \end{aligned}$$

(31)

where

$$\begin{aligned} \mathcal{E }_0=\int \limits _0 ^{0.5}\omega (t)^2 \,|\dot{\vec{r}}(t)|\,\mathrm{d}t \end{aligned}$$

and

$$\begin{aligned} \mathcal{E }_7=\int \limits _{0.5}^1\omega (t)^2 \,|\dot{\vec{r}}(t)|\,\mathrm{d}t . \end{aligned}$$

This method promotes a convergence to the result with a very small number of iterations, producing a curve of reasonable length.