Smooth Three-Dimensional Route Planning for Fixed-Wing Unmanned Aerial Vehicles With Double Continuous Curvature

This paper presents a smooth flight path planner for maneuvering in a 3D Euclidean space, which is based on two new space curves. The first one is called “Elementary Clothoid-based 3D Curve (ECb3D)”, which is built by concatenating two symmetric Clothoid-based 3D Curves (Cb3D). The combination of these curves allows to reach an arbitrary orientation in 3D Euclidean space. This new curve allows to generate continuous curvature and torsion profiles that start and finish with a null value, which means that they can be concatenated with other curves, such as straight segments, without generating discontinuities on those variables. The second curve is called “Double Continuous Curvature 3D Curve (DCC3D)” which is built as a concatenation of three straight line segments and two ECb3D curves, allowing to reach an arbitrary configuration in position and orientation in the 3D Euclidean space without discontinuities in curvature and torsion. This trajectory is applied for autonomous path planning and navigation of unmanned aerial vehicles (UAVs) such as fixed-wing aircrafts. Finally, the results are validated on the FlightGear 2018 flight simulator with the UAV kadett 2400 platform.


I. INTRODUCTION
In the last decades, the aeronautics industry has maintained a continuous and vertiginous development, particularly in the military field [1].The literature in the different fields of research is extensive.A relevant field of study focuses on path planning, which has been approached from different perspectives, such as communication networks [2], [3] or computational intelligence based on path planning algorithms [4], [5].On the other hand, applications for civilian missions are currently in high demand [6], such as rescue missions [7], [8], or work in agriculture [9].
The associate editor coordinating the review of this manuscript and approving it for publication was Zhenbao Liu .
The aim of flight planning for aerial vehicles is to generate 26 a path joining initial and final configurations, while passing 27 through several intermediate target points.In case of potential 28 collisions with static or dynamic objects, such as other UAVs 29 flying nearby, the initial route must be replanned to guarantee 30 collision-free paths [10], [11].

31
In particular, this paper addresses the 3D path planning 32 task for UAVs with nonholonomic characteristics [12], [13], 33 i.e., fixed-wing aircrafts.In this sense, it should be high-34 lighted that one of the most relevant particularities of this 35 kind of paths is the continuity of the curve, since a fixed-wing 36 UAV cannot perform abrupt maneuvers during its flight time.37 Therefore, the starting point to create a smooth flight path 38 must consider the particular maneuverability capabilities of 39 the UAV.Several studies have been proposed in this context, 40 where 3D path planning algorithms use cubic Bezier spiral curves to satisfy the curvature constraint are presented in [14], [15], [16], while [17] proposes a seventh-order Bézier curve as a continuous curvature path approximation, which does not exceed the kinematic constraints of an aerial vehicle.
The authors of [18] have performed a fusion between two heuristic methodologies, with the aim of solving the smooth path planning problem in a mountainous environment, for which the characteristics of B-spline curves are exploited.
The authors of [19] study curvature constraints in path planning and solve this problem through Dubins curves.
In [20], the smooth trajectory planning problem with continuous curvature is solved through an optimization algorithm based on Pythagorean curves, which satisfies the kinematic constraints of the UAV, in a similar approach, the authors of [21] propose a path planning generated through a multiobjective optimization problem operating with standard genetic operators.In [22] coordinated path planning for multiple UAVs is performed, starting from an ant colony optimization algorithm smoothed through a k-degree smoothing method.
While in [23] a Rauch-Tung-Striebel (RTS), smoothing is used, a procedure that permits smoothing the path produced by a Particle Swarm Optimization (PSO) algorithm, while in [24] an RTS smoothing is also used for the control of mobile robots with nonholonomic wheels.A particular approach is presented in [25], where a smooth path planning algorithm based on a Gaussian spectrum function is built, which aims to optimize the smooth path.Finally, in [26] model-based smooth paths are proposed for the estimation of the optimal geometric parameters, through polynomial spline curves, the results are used in industrial robots, with the aim of improving the productivity.
Focusing on works with good results based on clothoid curves, [27] solves the problem of generating continuous curvature paths by composing multiple clothoids.The relevance of clothoid curves and their application to nonholonomic vehicles can also be appreciated in [28], [29], and [30].In [31] and [32], clothoids are approximated using Bézier curves to minimize curvature profiles and thus guarantee higher-order geometric continuity while minimizing error.In [33], smooth paths based on clothoid curves are proposed for planning high-speed wheeled vehicle paths, for which a numerical optimization is performed within the constraints of convex regions.In [34], autonomous valet parking service path planning is performed.Finally, the authors of [35]  A new 3D smooth curve, called Elementary Clothoid-based 3D Curve or ECb3D, is proposed, which is built by combining two symmetric Cb3D [36].An ECb3D is capable of reaching an arbitrary direction in 3D space, being its curvature and torsion profiles equal to zero at both the beginning and the end of the curve.That property allows to build more complex curves combining them with straight line segments or other ECb3D curves.In this sense, a second 3D smooth curve 97 is introduced, coined as Double Continuous Curvature 3D 98 Curve (DCC3D), which is a concatenation of two ECb3D 99 curves and three straight line segments.A DCC3D curve can 100 reach any arbitrary position and orientation in 3D Euclidean 101 space.Finally, it should be emphasized that collision avoid-102 ance is out of the scope of this paper, although the proposed 103 path could be used as a primitive in both global and local 104 planners to generate collision-free paths.

105
This paper is organized as follows: in section II, the for-106 mulations of the preliminary works related to this article are 107 discussed.Section III describes the problem to be solved.108 In Section IV, the methodology to design smooth curves 109 to reach arbitrary target orientation is explained in depth, 110 whereas Section V describes how to generate 3D Double 111 Continuous Curvature Curves that allow to reach arbitrary 112 position and orientation.Section VI presents the results that 113 validate the application of this new curve through flight simu-114 lations performed on a fixed-wing UAV.Finally, conclusions 115 and further work are described in Section VII.

117
A nonholonomic constraint cannot be expressed only in posi-118 tion variables but includes the time derivative of one or several 119 variables.In direct reference to fixed-wing UAVs, these con-120 straints are directly related to their maneuverability in flight.121 There are different approaches used for the construction 122 of smooth paths, whether they are heuristic [37] or geo-123 metrical [38], [39], [40].Thus, this work takes a geometric 124 approach for constructing smooth paths as a starting point, 125 based on the criterion of 3D continuous curves.Thereafter, 126 a set of concepts necessary for the development of this article 127 are defined.

129
A curve in space R n , can be defined as a vector function [41] 130 such that: where, the points C(a) and C(b) are the initial and final 133 boundaries of the curve.In particular, a curve in the 134 three-dimensional space R 3 can be defined as C(s) = (x(s) i + 135 y(s) j +z(s) k ), where i, j, k refer to the unit vectors of the global 136 reference frame.(2) 141 • N(s) is the unit normal vector, given by the ratio of the 142 derivative of T(s) to its length: 149 On the other hand, κ(s) defines the curvature of the curve, 150 such that: and τ (s) is the torsion, defined as: Therefore, for a continuous curve defined in R 3 , as the one shown in Figure 1, T (5) and ( 6), and can be computed as follows: Then, the orthogonal basis of the system is defined as A planar clothoid (C2D) [42], also known as the Euler Spiral, 171 defined in R 2 (see Figure 2), is a curve whose curvature varies 172 linearly with respect to the arc length, being: where, σ κ := dκ(s)/ds is referred to as the curvature sharpness, which is related to the homotopy factor K , being σ κ := π/K 2 .Hence, the tangent angle of the clothoid is defined as: The C2D is a curve that has contributed in various aspects, both in development and construction of roads and/or railroads [43], [44], and also in research [45], giving nonholonomic vehicles a good tracking control, due to its various geometric properties such as curvature and tangent angle.Being the tangent vector computed as: Consequently, a planar clothoid curve contained in the plane XY , is defined by the equations ( 8), (10) and (11).Hence, C(s, σ κ ) can be solved using the Fresnel integrals, as follows: where C(s, σ κ ) and S(s, σ κ ) are the Fresnel integrals in cosine and sine, respectively.Finally, it should be noted that without loss of generality, it is assumed that the clothoid starts from the origin of coordinates, being C(0) = 0 according to the equation (8).
The concept of Euler Spiral defined in R 3 , also known as a 3D clothoid (C3D), was introduced by [46].The curvature of a C3D varies as a function of the equation ( 9), while its torsion is defined as: where σ τ := dτ /ds is the first geometric derivative (also known as torsion sharpness).The development of this 3D smooth curve allows arbitrary configurations to be achieved, either in position or orientation in 3D space, but not both at the same time.

III. PROBLEM DEFINITION
Let us assume R as a UAV of nonholonomic characteristics, such as a fixed-wing aircraft, whose state space In addition to this, curvature and torsion at start and goal configurations must be zero, i.e., κ S = κ G = 0 and τ S = τ G = 0.

IV. ELEMENTARY CLOTHOID-BASED 3D CURVE (ECb3D)
The aim is to construct a new three-dimensional curve, in the space R 3 × S 2 , able to reach an arbitrary configuration in orientation with continuous curvature and torsion (CC).This new curve will be composed of two segments of the Clothoidbased 3D curve (Cb3D) [36].
The procedure starts with the description of the Cb3D curve.Afterwards, the generation process of the new Elementary Clothoid-based 3D Curve (ECb3D), generated from the concatenation of two symmetrical Cb3D curves is detailed.Specifically, the ECb3D will allow reaching an arbitrary orientation, but the position will be given according to the shape and size of the constructive clothoid parameters.

A. CLOTHOID-BASED 3D CURVE (Cb3D)
The authors of [36], propose a new Clothoid-based 3D Curve (Cb3D), capable of achieving an arbitrary configuration in position or orientation in 3D space.The Cb3D is built from two C2D curves generated in the orthogonal XY and XZ planes.Thus, Cb3D projects a C2D curve in the XY plane with arc length s, while the clothoid in the orthogonal XZ plane depends on the length of the C2D curve in the XY plane (see Figure 3).The curve Cb3D achieves relevant results due to its analytical solution.Moreover, the curve presents a set of interesting properties/operations such as scalability, symmetry, monotonicity, and smoothness along the curve.Appendix A includes the most relevant aspects of the curve [36], which have been included in this work to justify the computations of the proposed methodology.

B. ECb3D CURVE GENERATION 260
The concept of Elementary Curve in R 2 (E2D) was intro-261 duced by [47], a curve developed to build appropriate paths 262 for mobile wheeled vehicles.In that sense, the E2D is built 263 by combining two symmetric C2D curves that have the same 264 homotopy factor and the same length.The goal is to create 265 smooth trajectories that do not exceed certain physical limits 266 associated with comfort and safety in mobile vehicles [45], 267 leading to proper path following.

268
In particular, the ECb3D curve seeks to extend the original 269 concept of E2D curve to the space R 3 , by concatenating two 270 symmetric Cb3D curves [36].287

308
where the first column of the right-side expression corre-309 sponds to the symbolic expression for computing T G .Thus, 310 with equations ( 14) and ( 18) we can compute the intermediate Therefore, the parameters necessary to build Cb3D curves are computed by means of the following expressions (see Appendix): being s the length of one Cb3D, that is, the half length of the ECb3D curve.
On the other hand, the transformation governing the rotation and translation of the curve C * (s, µ, ρ) to obtain C † (s, µ, ρ) (see Figure 4) is expressed as: where, it can be seen that, as the C † curve has been defined, when s = 0 the curve is in the q M configuration, while when s = s, the curve ends in the q G configuration.
Finally, as a result of the concatenation of the curves C and C † , the ECb3D curve is defined as: Hence, the ECb3D curve is evaluated for s ∈ [0 2s], with no discontinuity.
Figure 5, shows a comparative example of the ECb3D curve (blue line) and the Cb3D curve (dashed red line), both of which point to a goal orientation of θ G = −π/4 and G = π/2 with s = 0.5.In Figure 5(b), the variation of the pitch θ and yaw Euler angles is displayed, while in Figures 5(c) and 5(d) the behavior in the curvature profiles and their sharpness in the orthogonal XY and XZ planes can be appreciated.It can be highlighted that the ECb3D curve starts and ends with curvature and torsion equal to zero (κ = τ = 0), characteristic that allows the ECb3D to link with another curve without loss of continuity, a property lacking in the Cb3D.

V. DOUBLE CONTINUOUS CURVATURE 3D CURVE (DCC3D)
This section defines a curve that reaches an arbitrary configuration in position and orientation in the 3D Euclidean space.The new Double Continuous Curvature 3D Curve (DCC3D) is built by three straight line segments and two ECb3D curves, as can be seen in Figure 6.
In this sense, since there are two ECb3D paths in a DCC3D, a subscript has been added in order to refer to the corresponding ECb3D.Therefore, configurations q S , q M and q G of an ECb3D have been renamed as q S,i , q M ,i and q G,i , respectively, where i = 1 refers to the first ECb3D, E 1 , and i = 2 to the second one, E 2 .Thus, the following equalities  relative to configurations' orientations hold: q S ≡ q S 1 ,

361
Since there are multiple solutions to the problem, the 362 deflection angle q G,1 is assumed to be known, which allows 363 determining the straight lines and clothoids' lengths.That is possible because derivatives of curvature and torsion are performed at the maximum admissible sharpness, that is |µ| ≤ µ max and |ρ| ≤ ρ max .This allows to obtain the shortest path for the given deflection angle in a preliminary step.Thus, this states a numerical optimization problem that aims to obtain the shortest curve by finding the intermediate orientation q G,1 that minimizes the overall length, while ensuring that the path satisfies some constraints to avoid abrupt changes in curvature and torsion.As shown later on, the lengths of the first and third straight lines act as relaxation variables in order to satisfy position constraints for a given q G,1 .

A. DCC3D CURVE GENERATION
Let us assume that the intermediate orientation of the DCC3D curve is given, that is, q G,1 .Let us also assume that in order to satisfy position constraints imposed by q G , we allow maneuvers, that is, changes in the direction of the curve that would force a robot stop to avoid abrupt changes in curvature and torsion.The steps to generate the DCC3D curve are described next.

1) Compute the first ECb3D: the intermediate deflection angle q
T is determined from equation ( 19) using the orientation of the goal config- ] T (assuming that the start 388 configuration q S,1 = [0 0] T ).Otherwise, a rotation 389 must be applied to compute q G,1 with respect to the 390 local frame of q S,1 , as in equation ( 14).

391
Assuming that the curve is computed with the max-392 imum allowable torsion sharpness ρ 1 := ρ max , the 393 arc-length of each Cb3D curve (of the ECb3D), and the 394 curvature sharpness to reach the deflection angle q M ,1 395 can be obtained from ( 20) and ( 21) as follows: 397 If |µ 1 | > µ max , then it means that the assumption 398 of the previous point was incorrect and, therefore, the 399 curve with the maximum admissible curvature sharp-400 ness µ 1 := µ max must be computed as: 402 2) Compute the second ECb3D: repeat the previous step 403 to obtain curvature µ 2 and torsion ρ 2 sharpness param-404 eters, as well as the arc-length s2 based on deflection 405 angle between q S,2 ≡ q G,1 and q G,2 ≡ q G .
412 Thus, line segments must generate the remainder 413 position displacement in order to satisfy position 414 constraints:

416
As a consequence, lengths of line segments can be 417 computed as: where † denotes the Moore-Penrose pseudo-inverse, 420 The aim now is to compute the curve with the shortest length, being q G,1 decision variables of an optimization process: Note that values of L 1 , L 2 , L 3 , s1 and s2 depend on the deflection angle as discussed in the previous section.Since this procedure looks for solutions of maximum sharpness in torsion or curvature of the clothoid arcs, the curve resulting after solving (30) will produce the shortest length that joins the initial and goal configurations without discontinuities.Also note that the positiveness constraints imposed to L 1 , L 2 and L 3 ensure that the obtained curve will imply no maneuvers, and thus are suitable for fixed-wing UAVs.
Lemma 1: The optimal solution will imply that lengths for the first and third lines are zero.
Proof: For the unbounded problem, where ρ max = ∞ and µ max = ∞, the clothoid lengths have zero length, i.e.: s1 = 0 and s2 = 0, which implies an instantaneous change of orientation between the line segments.In that case, it is obvious that the optimal solution joining two points in the Cartesian space is q * G,1 = [arctan( which implies that the lengths of the line segments are L 1 = 0, L 2 = | p| and L 3 = 0.The optimal solution of the bounded problem will generate clothoids with the shortest possible length such as the angle between T G,1 and p is minimum, which implies that numerically L 1 → 0 and L 3 → 0, details left for brevity.

C. COMPUTATION TIME
In order to test the computational effort to generate a DCC3D curve, a total of 10 4 arbitrary goal positions and orientations have been used to analyse time performance.The computational time has been divided into two stages.In the first stage, the optimization process to find the intermediate configuration angle q * G,1 that minimizes the overall length, obtaining an average time of t q * G,1 = 0.110 s.The second timing analysis focuses on determining the computational time to get µ and ρ values for each Elementary segment, E1 and E2.The mean time to compute such parameters is t E = 0.0024 s.
The computer used for this analysis has the following specifications: CPU Intel i5-9400F 4.100GHz, GPU NVIDIA GeForce GT 610 and 8 GB DDR4 memory, under an Ubuntu 18.04.4LTS x86_64 OS with Kernel 5.4.0-120-generic.

D. CASE STUDY
To achieve a better understanding of the results obtained, a specific case study with three variants is described next.Figure 7 shows an example of DCC3D curves joining the initial configuration q S = [0 0 0 0 0] T with a goal configuration q G = [170 120 90 π/4 π/6] T .The aim is to describe the behavior of the DCC3D curve by setting three different

498
In order to realistically reproduce the Kadett 2400 fixed-wing The UAV has four actuators δ e (elevation), δ a (aileron), δ r (rudder) and δ th (throttle), which are controlled by three PID regulators implementing low-level control of the yaw * and pitch θ * angles, as well as the ground speed vel * .The yaw and pitch references are obtained from the yaw and pitch angles of the designed smooth curve, while the forward velocity reference is kept constant at vel * = 18 m/s throughout the simulation.Notice that in our implementation the aileron and rudder angles are actuated by the same controller, which directly affects the yaw orientation.
As mentioned before in the article, a nice characteristic of the DCC3D curve is to maintain zero curvature and torsion values, both at the beginning and at the end of the path.Therefore, a concatenation of DCC3D curves can be easily performed, producing a smooth flight path without any in-flight lift loss.Figure 9 shows an example of a smooth flight, where three DCC3D paths have been concatenated, being the maximum values of Cb3D sharpness parameters µ max = 0.001 rad/m 2 and ρ max = 0.001 rad/m 2 .Without loss of generality, the path begins at initial configuration q S (the  UAV in horizontal flight), passes through two intermediate 525 configurations q A and q B , and finishes at goal configura-526 tion q G : 527 q S = 0.0 0.0 0.0 0.0 0.0 0.0

VII. CONCLUSION
The work developed in this paper presents a novel 3D smooth path planner based on a concatenation of Clothoid-based 3D curves (Cb3D) and straight line segments.The planner solves the problem of joining two arbitrary configurations (position and orientation) in 3D space (without loss of continuity and smoothness), making it suitable for navigation of fixed-wing UAVs.
To generate DCC3D paths, a new 3D smooth curve called Elementary Clothoid-based 3D Curve (ECb3D) has been presented.An ECb3D can achieve any orientation with zero curvature and torsion values both at initial and final configurations.That feature allows ECb3D paths to be used as primitives for path generation, since it is possible to smoothly concatenate several ECb3D paths and straight lines without loss of curvature and torsion continuity.
The computational time of the curve, including finding the curve with the shortest distance takes, in average about 0.1 on an Intel i5 (seed details in section 5).Hence, the fast computation of the proposed DCC3D, allows to generate 3D smooth paths in real-time.
As further work, we aim to include the proposed trajectory in global planners such as Randomized Path Planner (RPP), Probabilistic Road Map Method (PRM), Rapidly Exploring Random Tree (RRT).A different use of DCC3D curve would be the development of a local path planner for obstacle avoidance in 3D maneuvers.Finally, the same methodology could be implemented using other transition curves in order to compare the performance against the Cb3D curves used in this work.

A. CLOTHOID-BASED 3D CURVE
Without loss of generality it is assumed that the Cb3D starts from the origin, therefore, the generic parameterization of the curve is defined by the vector p, being C(s, p).Thus, p contains the vector of design parameters of the curve, such that p := {µ, ρ} represent the sharpness parameters of the 2D clothoids contained in the XY and XZ planes.Therefore, the objective is to compute p * := {µ * , ρ * } given a target tangent vector T * to be achieved.Specifically, the curve C2D contained in the XY plane is computed as: where, q is the arc length, while: propose a path smoothing, based on clothoid curves, parameterized by the arc length.The aim of this work is to generate a smooth flight path in the context of fixed-wing UAV autonomous navigation.

137
Tangent, normal and binormal vectors are defined as: 138 • T(s) is the unit vector tangent to the curve, pointing tp 139 the direction of movement:

161whereTFIGURE 1 .
FIGURE 1. Curve in the space R 3 , where three orthogonal local systems are shown, which are defined by T vectors (red arrows), N vectors (green arrows) and B vectors (blue arrows).

164R
(s) := [T(s) N(s) B(s)], which can be integrated from (7), 165 based on the functions of κ(s) and τ (s), from an initial value, 166 such that, R(0) := [T(0) N(0) B(0)].Hence, the position can 167 be determined by integrating the tangent vector.IN SPACE R 2 (C2D) 170 and orientation coordinates q R = [θ R R ] T , pitch and yaw angles.Whereas the input parameters to the system are given by the curvature sharpness σ κ R and the torsion sharpness σ τ R .It is important to remark that the roll angle of the curve is not relevant for computing the curve geometrically and, for this reason, it is not considered as part of the configuration.Assuming that R can perform motions within its kinematic boundaries of maneuverability (boundaries set by the particular aerodynamic constraints of the UAV) and that the values of the geometric derivatives of curvature and torsion (defined as curvature sharpness and torsion sharpness, respectively), are within the set boundaries, being σ κ R ∈ σ κ min σ κ max and σ τ R ∈ σ τ min σ τ max .Then, the aim is to build a new smooth curve G 1 to join two arbitrary configurations in position and orientation, starting from q S = [x S y S z S θ S S ] T to

FIGURE 3 .
FIGURE 3. Clothoid-based 3D Curve (Cb3D).The dotted red line shows the C2D construction in the XY plane, the green line shows the C2D in the XZ plane while the blue line shows the Cb3D curve.Image taken from [36].

FIGURE 4 .
FIGURE 4. ECb3D curve generation from the symmetry of two concatenated Cb3D curves, through an intermediate deflection angle q M .It is assumes an initial orientation configuration q S = [θ S S ] T = [0 0] T , while the goal orientation configuration is qG = [θ G G ] T .
expressed with respect to a global frame and 291 R is a rotation matrix 292 R(φ, θ, 293 where R x (•), R y (•) and R z (•) are basic rotation matrices 294 around each axis of the global reference frame; being roll φ, 295 pitch θ and yaw the Euler angles.296 Then, the deflection angle q G = [θ G G ] T can be comx, y and z refer to each component of the 301 tangent vector T G .302 On the other hand, for the computation of the intermediate 303 deflection angle q M , the following rotations must be applied 304 to ensure that the curve is continuous (in position, curvature 305 and torsion):

406 3 )
Compute line segments: it is interesting to remark that 407 the resulting curve, as a consequence of concatenating 408 the two ECb3D, satisfies orientation constraints, but it 409 does not satisfy position constraints.Indeed, the posi-410 tion displacement imposed by the clothoid arcs is:

) 422 Obviously
, when the rank of T S T G,1 T G is not 3, i.e., 423 the matrix has one or two singular values close to zero, 424 it implies that line segments are redundant.In those 425 cases, we can force to zero one of the line segments and 426 compute the others.In particular, we propose to force 427 to zero L 1 and L 3 before forcing to zero L 2 , details left 428 to the reader.429 B. SHORTEST CURVE

499
UAV dynamic model, a setup with the Flight-Gear 2019 sim-500 ulator and Matlab R2020b has been used for the low-level 501 control of the different subsystems shown in Figure8.For this experimentation the same computer as in Section V-B has been used.

FIGURE 9 .
FIGURE 9. DCC3D curve and flight simulation with four concatenated configurations.

T 528 q 1 T 531 Figure 9 Figure 9
Figure9(a) shows the reference path followed by the UAV.