Real-time path planning in dynamic environments for unmanned aerial vehicles using the curve-shortening flow method

This article proposes a new algorithm for real-time path planning in dynamic environments based on space-discretized curve-shortening flows. The so-called curve-shortening flow method shares working principles with the well-established elastic bands method and overcomes some of its drawbacks concerning numerical robustness and parameterability. This is achieved by efficiently applying semi-implicit time integration for evolving the path and secondly by developing a methodology for setting the algorithm’s parameters based on physical quantities. Different short- and long-term validation scenarios are performed with three interlinked instances of the curve-shortening flow method each running on an individual industrial control and driving a real or a simulated unmanned aerial vehicle.


Introduction
Current developments in production engineering are driven by the demand for a high degree of individualization of consumer products. The accompanying variation in the sequence of the individual production steps and the demand-dependent occupancy of available processing machines requires intralogistics that are just as flexible. For transportation of small parts, the usage of unmanned aerial vehicles (UAVs) is currently widely studied. In contrast to fixed ground-and line-bound transportation devices (e.g. automated guided vehicles or conveyer belts), UAVs provide the possibility to exploit the normally unused airspace within the manufacturing plants. However, tasks that are independently assigned to the UAVs and executed in parallel can create an environment with high risk of collision, in which the individual flight movements of the UAVs cannot be planned in advance.
This leads to a dynamic and time-variant pathplanning problem that can only be solved in real-time while considering the environment's current state. With the so-called curve-shortening flow method (CFM), a new online path-planning algorithm capable of adjusting collision-free paths to a changing environment is presented.

Related work
The research field of mobile robotics provides numerous methods to solve for this type of path-planning problem that can also be applied to UAVs in general and especially to the here considered rotary wing UAVs like heli-, quad-, or multicopters which are able to hover and vertically start and land. These path-planning methods are typically classified into reactive, global, and mixed approaches. 1 Reactive path-planning methods such as virtual force field, 2 vector field histogram, [3][4][5] nearness diagram, 6 or dynamic window approach 7 with origins in the potential field approach 8 plan only short sections of the path based on local information without considering continuous connectivity to the goal. They primarily serve as collision-avoidance strategies and therefore are intricately linked with the robot's motion control due to their ability to directly generate control signals in real time. 9 At the same time, these methods are vulnerable to getting stuck in local minima or blockage and oscillation in close proximity to narrow passages 10,11 whereby a path to the goal cannot be guaranteed although it exists. Moreover, such reactive methods were developed for floor-bound mobile robots and are not applicable to threedimensional path-planning problems in many cases.
Global path-planning methods are based on extensions of rapidly exploring random trees 12 (RRT) or probabilistic roadmaps 13 (PRM) and represent a dynamic environment as a modifiable tree-or net-shaped connectivity graph of discrete and randomly picked collision-free points (samples) within the robot's workspace. For instance, the RRT x14 and Real-time-RRT* 15 methods are capable of rewiring the arising tree structure in regards to dynamic obstacles at runtime. Dynamic roadmaps 16,17 or elastic roadmaps 18 disable or relocate individual samples of a PRM net when occupied by dynamic obstacles.
As RRT-based methods pick random samples only during runtime and PRM-based methods need to execute an online-capable search algorithm such as Anytime A* 19 or D*Lite, 20 the computation time to obtain a valid path is not predictable and makes them difficult to execute under hard real-time conditions. The large variation in execution times of typically more than 100 ms 1,21-23 does also not guarantee a safe flight for dynamic UAVs using these methods. 24 Mixed path-planning methods like the elastic bands method 25 (EBM) or the elastic strips method 26 use local optimization by means of virtual forces in order to adapt an initially collision-free path to changes of the environment while simultaneously maintaining start and goal points as boundary conditions. The EBM matches the problem statement formulated in the introduction best because it can be efficiently reduced to a discrete description of a one-dimensional and deformable path in space with continuous curvature, global connectivity, and rapid response. A significant weakness of the EBM lies in its insufficient numerical robustness due to updating the path using an explicit transition rule. 27 This insufficient numerical robustness leads to unpredictable oscillations or even instabilities when facing obstacles in close proximity and can only be compensated by a smaller step size and finer discretization of the path, which in turn worsens convergence and computational efficiency. While further developments of the EBM bypassed these stability issues using an optimization method, 27 the EBM's original and computationally more efficient approach 25 to evolve the path with numerical integration was not continued by recent applications of the EBM in the field of mobile robotics, 28 service robotics, 29,30 or autonomous driving. 31 These shortcomings are overcome by introducing the CFM. While maintaining the EBM's working principle, the CFM provides numerically robust solvability under hard real-time conditions with cycle times of just a few milliseconds which are characteristic for industrial controls.

Curve-shortening flow method
Similar to the EBM, the CFM describes the path between a start p s and a goal p g as a sequence of n discrete and consecutive nodes p i whose positions are continuously adjusted to the current state of the environment by virtual forces in order to keep them collision-free. Figure 1 illustrates the working principle of the CFM.
The obstacles' geometries serve as the source of the repulsive forces f i displacing the path, while retractive forces k i which are proportional to the path's curvature push the path into a state of minimum length. The path thereby converges toward an equilibrium representing a Pareto-optimal state between these two criteria. The path behavior (path dynamics and stationary displacement) is tuned via the scalar dynamics parameter T and the scalar repulsion parameter K.

Mathematical description
The evolution of a single node p i ¼ ðx i ; y i ; z i Þ T during runtime can be mathematically expressed by the following generalized ordinary differential equation (ODE) which applies for all n nodes along the path.
The linear description of the unamplified retractive force k i depends on the path's curvature and is built upon the sum of two vectors each pointing from p i toward one of its adjacent nodes. This formulation traces back to a space-discretized approximation 32,33 of continuous curve flows 33,34 (which the CFM is named after) and implicitly ensures that k i always points toward the inner side of the path's curvature. The unamplified repulsive force f i is typically derived from nonlinear distance calculations between p i and a problem-specific representation of the obstacles' geometries h. An exemplary model of the environment will be presented later in the "Modeling of the environment" section.
Each spatial component (row) of the generalized ODE in equation (1) can be written out in a linear form of state space system to represent the positions of the path's n coupled nodes along this axis. Due to similarity only the subsystem for the y-axis is shown here.
Because the vector f y of the repulsive forces' components along the y-axis depends on all components of p i , f y forms a generally nonlinear coupling between all spatial subsystems. The vector b y contains the y-coordinates of the start p s ¼ ðx s ; y s ; z s Þ T and the goal p g ¼ ðx g ; y g ; z g Þ T . The state space representations for the x-and the z-directions can be expressed analogously. Figure 2 illustrates the qualitative effect of the scalar parameters T and K on the evolution and the stationary displacement of a planar path (p 2 R 2 ) under influence of three individual point-obstacles.

Parameter influence
The figure shows how a fivefold amplification of the repulsion parameter K at constant T (column-wise reading) increases the distance to the obstacles. However, an amplification of the dynamics parameter T by a factor of five at constant K (row-wise reading) causes a slower convergence of the path without affecting its stationary displacement (equilibrium state). The latter can be recognized by the higher density of the path evolution steps that were taken at equidistant time intervals for all four settings. The arrows indicate the repulsive (red) and retractive (green) force vectors acting on the initial and the stationary path.

Analytical parameterization
The dependencies of the path behavior from the parameters and the number of nodes will be quantitatively analyzed in order to enable a systematic procedure for setting the parameters K and T for a desired path behavior and a given number of nodes n.

Path dynamics
To be able to study the properties of the CFM and the influence of the parameters analytically, the linear and perpendicular line load with reach r 0 shown in Figure 3 is used as a reference obstacle.
As here the vector of repulsive forces has an effect only in the y-direction, the analysis can be reduced to this component. With the above-described obstacle model f i;y ¼ r 0 À i y , the linear state space representation from equation (2) can be expressed as follows.
Since the system matrix A Ã is of tridiagonal Toeplitz form, its n eigenvalues l i can be analytically determined in dependency with K, T, and n. 35 With K ¼ 0, the system is unexcited and represents the resetting of an initially displaced path to a straight line in absence of obstacles (eigenbehavior).
Based on the theory of linear systems and control, 36 the following characteristics of the path can be derived from the resulting eigenvalues: The path dynamics are stable due to the real component of all eigenvalues being negative at all times. The path dynamics are not capable of self-oscillation as all eigenvalues are solely real-valued at all times. The dynamics of every individual node can be regarded as a series of n PT 1 -elements and thus every node shows low-pass behavior of nth order in its core.
In addition, the path's inertia can be estimated on the basis of the largest eigenvalue l max ¼ l 1 , since l 1 defines the slowest mode which dominates the system's overall dynamics by partially acting on all n nodes' positions (system states). In decoupled form, this mode describes a firstorder lag element (PT 1 ) whose settling time T max ¼ 3=jl max j correlates with the average settling time T 95;i of the coupled nodes. The settling time T 95;i denotes the duration until the ith node has decayed by 95%. The matching between T max and T : was verified using simulation for several parameter settings as shown in Figure 4. Figure 5 illustrates the evolution of the path under influence of a line load for the exemplary parameter setting highlighted in Figure 4.

Stationary displacement
Influenced by solely static obstacles, the nodes converge into a state of stationary displacement p for t ! 1. This state can be used as a measure of the path's safety distance. In analogy to the dynamic behavior, again the line load (see Figure 3) is used as a reference obstacle for the analysis of the y-component of the path.
For calculation of the stationary displacement of the nodes y 1 , equation (3) can be transformed into the linear system of equations (5) by zeroing its time derivatives _ y ¼ 0. Since the solution y 1 is proportional to the reach r 0 , only the unified reach r 0 ¼ 1 ðmÞ is used in the following. Figure 6 shows the analytically calculated stationary paths for various repulsion parameters K and numbers of nodes n.
It becomes clear that for K ! 1, the stationary displacement of the nodes y is bounded by the reach r 0 of the obstacles. Furthermore, an increase of n at constant K amplifies the displacement.

Calculation of the repulsion parameter
To achieve a desired stationary displacement, the corresponding value for K must be computed. To measure the overall displacement of the path with a single scalar indicator, the mean node displacement according to equation (6) is used.
By specifying the desired mean node displacement y ;d with 0 < y ;d < r 0 , the following boundary condition in equation (7) has to be fulfilled.  jjy jj À y ;d ¼ 0 In combination with equation (5), the nonlinear system of equations Fðy F Þ ¼ 0 shown in equation (8) can be derived. Numerically solving for the vector of unknowns y F ¼ ðy 1 ; y 2 ; y 3 ; . . . ; y n ; KÞ T ¼ ðy; KÞ T is accomplished using the Newton-Raphson method. 37 Calculation of the dynamics parameter The path dynamics are controlled via the dynamics parameter T. The earlier introduced average settling time T : % T max is used as a measure for the inertia of the path. By specifying the desired settling time T max;d and considering the already known values for K and n in equation (4), the dynamics parameter T can be calculated according to equation (9).

Time discretization
Solving the systems of ODEs in equation (2) is carried out using numerical time integration. The continuous time-  derivative _ p is thereby approximated by a transition formula between two consecutive states of the path p k ¼ pðt k Þ and p k þ1 ¼ pðt kþ1 Þ with k indicating a time-discrete calculation step at time t k with the constant step size The backward Euler method which has A-stability for linear time-invariant systems is used to allow for numerically robust time integration. To efficiently perform the integration steps, the (nonlinear) repulsive force f is evaluated explicitly (thus for the current state of the path p k ) and then fed component-wise into the ODEs as an input vector. This results in a semi-implicit execution of the backward Euler method which consequently loses its A-stability. Nevertheless, the method's high degree of numerical damping still provides extraordinarily robust time integration.
Applying the discretization rules of the backward Euler method on equation (2) provides the following (semi-implicit) transition formula shown here for the y-component.
Rearranging equation (10) for the unknown y kþ1 gives the linear system of equations (11) with the identity matrix I.
Equation (12) shows the tridiagonal coefficient matrix C written out.
As C depends only on the initially specified parameters n, Dt, and T, and therefore is constant at runtime, the linear system of equations (11) can be solved very efficiently using LU decomposition. Thereby, the coefficient matrix is at first decomposed into a lower triangular matrix L and an upper triangular matrix U, so that C ¼ L Á U applies. Due to the fact that the matrices L and U are also constant, only the explicit evaluation of a forward and backward substitution is required when subsequently solving the linear system of equations (11) for varying input vectors r.
In this case, the LU decomposition can be performed without pivoting, since C is strictly dominant in accordance with the row sum criterion (row-wise the absolute value of the diagonal element is greater than the sum of all remaining elements). 38 With the introduction of an auxiliary vectorŷ, equation (11) can be solved using LU decomposition according to equation (13).
Due to the coefficient matrix C being tridiagonal, L and U form a lower (L) respectively an upper (U) bidiagonal matrix. A highly efficient algorithm for solving this special form of a linear system of equations is the Thomas algorithm. 39 It reduces the complexity with respect to the number of nodes n compared to a fully occupied coefficient matrix from quadratic to linear order. This applies not only to the cyclic forward and backward substitution but also to the initial LU decomposition. 40 For p 2 R 3 , the former can be executed in 36 Á n À 5 ðFLOPsÞ with arithmetic operations weighted according to the Livermore Loops benchmark. 41

Comparison to elastic bands method
The CFM was compared to an implementation of the EBM according to its original formulation. 25 A planar scenario was created with a path discretized with n ¼ 10 nodes and an initially relaxed length of L 0 ¼ 10 ðmÞ. The path was then displaced by two inversely approaching pointobstacles with a velocity of v ¼ 5 ðm=sÞ and a reach of r 0 ¼ 2:5 ðmÞ.

Path behavior equalization
At first, a parameterization meeting the path planning dynamics for an UAV was empirically set with k c ¼ k r ¼ 10 for the EBM, whereby k c and k r represent the proportional gains of the repulsive and retractive forces of the EBM. 25 To achieve a quantitatively equal path behavior for the CFM, the path generated by the EBM was excited with the line load serving as reference obstacle. Figure 7 shows the identification of the nodes' average settling time T : ¼ 0:225 ðsÞ and average stationary displacement jjyjj ¼ 2:11 ðmÞ from the step response of the path.
Using these values, the CFM was analytically parameterized to approximately achieve an identical path behavior as with the EBM:  For n ¼ 10, r 0 ¼ 2:5 ðmÞ, and the demand y :;d ¼ 2:11 ðmÞ, the repulsion parameter results in K ¼ 0:73 according to equations (7) and (8).
For n ¼ 10, K ¼ 0:73, and the demand T max;d ¼ 0:225 ðsÞ, the dynamics parameter results in T ¼ 0:0608 according to equation (9). Figure 8 shows the described experiment executed with a small, and for the EBM still stable, step size of Dt ¼ 0:01 ðsÞ. The similar node displacements at the end of the simulation runs prove nearly identical transient behavior of the methods for repulsion and retraction.

Numerical robustness
To illustrate the numerically robust solvability of the CFM, the repulsion experiment was repeated for larger step sizes. It could be observed that the EBM started oscillating for Dt ! 0:02 ðsÞ and finally became unstable for Dt ! 0:08 ðsÞ. In contrast, the CFM solution to the underlying system is numerically stable for arbitrarily large step sizes, of which Figure 9 shows for Dt ¼ 0:1 ðsÞ and Dt ¼ 0:25 ðsÞ. Such large step-sizes become necessary when the CFM is executed on less powerful embedded hardware or microcontrollers.

Time efficiency
To determine the computational efficiency, both methods were implemented on an industrial computer (Beckhoff C6930-0050, Intel ® Core™ i7-4700EQ, 64bit, 2.4 GHz, 8 GB RAM) under real-time conditions (using TwinCAT v3.14020.14 as the real-time operating system) and the computation times per cycle depending on the number of nodes n were measured as shown in Figure 10.
This measurement confirms the linear complexity of the CFM as well as its time efficiency. Even with many nodes n, the computational effort for a cyclic update of the path by solving equation (11) lies within a single-digit microsecond range. Thereby, it is ensured that there is still enough computation time available for the problem-specific calculation     of the repulsive force with more complex obstacles and for further control functionalities when executing the CFM with a cycle time characteristic of industrial controls in the range of a few milliseconds.

Improvements
In summary, this study shows that the CFM has the following significant advantages over the EBM while sharing the same operating principal: Numerically robust solvability, analytical parameterability by means of physical quantities, and a slightly better computational efficiency.

Modeling of the environment
The modeling of the environment typically consists of various obstacles h and is problem specific. As the application of the CFM discussed here represents a gross path planning problem (workspace available for path-planning is much larger than the manipulator itself), an approximation of the objects within the workspace with geometric primitives as their bounding hull is sufficient. 42 Therefore, the model of the aerial space inside the production facility used here is based on bounding spheres (e.g. for UAVs) and bounding capsules (cylinder with hemispherical ends e.g. for supply lines or the supporting structure of the building) as well as planes (e.g. for walls) for limiting the workspace available for flight movements controlled by the CFM. As these geometric shapes are described with only a few parameters, the calculation of the distance dðp i ; hÞ between a single node and the obstacles can be implemented very efficiently. Using the gradient of the scalar force field Hðp i ; hÞ ¼ max 0; r 0 À dðp i ; hÞ , the repulsive  force f i ðp i ; hÞ ¼ Hðp i ; hÞ Á ÀrHðp i ; hÞ jrHðp i ; hÞj is numerically evaluated for every node.
On the left side of Figure 11, the scalar force field H is shown for an exemplary static arrangement of three spheric, two capsular, and two planar obstacles represented by its equipotential surfaces. For numerical reasons, the field is additionally smoothed at the transitions between the range of the individual obstacles. On the right side, the evolution of an exemplary path under influence of this field is shown.

Results
To validate the method, a test setup with three parallel instances of the CFM each running on an individual industrial control (see Figure 12(a), hard-and software configuration as described in the "Time efficiency" section) and driving a separate UAV was brought into service. The CFM was implemented as a new type of (collision-free) point-topoint interpolation method within these controls. This enabled the use of sequential programs to fly the UAVs to consecutive goals without having to explicitly consider  the potential for collisions during programming. For the path interpolation along the nodes, an accelerationlimited motion profile with velocity and acceleration limited to v max ¼ 1 ðm=sÞ and a max ¼ 0:25 ðm=s 2 Þ and smoothed node transitions was implemented. The controls were interlinked via a local cloud server to enable a close to real-time exchange of the state data from the UAVs (representing the dynamic obstacles) and to provide information about the remaining static obstacles. 43 Because the experimental flights with real UAVs could be carried out only in a secured test cell (see Figure 12(b)), the available workspace for all scenarios was limited to the dimensions of the cell measuring roughly 4 ðmÞ Â 5 ðmÞ Â 4:5 ðmÞ. Only the top three meters were used as workspace for flight movements with the CFM. The section underneath was reserved for secure start and landing maneuvers. In addition to the workspace limits that were modeled as planar obstacles, some exemplary peripheral elements within the workspace (e.g. pipelines and supporting structure of the building) were modeled using various capsular obstacles as shown in Figures 14,  15, and 18. All instances of the CFM were parameterized with n ¼ 20, y ;d ¼ 0:375 ðmÞ, and T max; d ¼ 1 ðsÞ resulting in K ¼ 0:114 and T ¼ 0:0454. The controls operated at a cycle time of 10 ms and therefore the step size of the CFM was identically set to Dt ¼ 0:01 ðsÞ.

Hardware-in-the-loop simulation
Initially, the controls were set up using a hardware-in-theloop simulation with both environment and UAVs being simulated. The latter was realized as a state-controlled rigid body model with six degrees of freedom. 44 Scenario 1 consisted of three simultaneously executed programs with predefined goals that were consecutively approached by the UAVs (see Table 1).  The three individual programs were manually started one after the other. Therefore, their execution occurred temporally independent of each other. Figure 13 shows the positions of the individual UAVs in x-, y-and z-direction and the distance d to the closest obstacle over time for an exemplary execution of scenario 1. The position data for actual (bold), desired (fine), and goal (dashed) values are charted. The sections in which the UAVs are in an initial/ waiting position or performing a secured start/landing are greyed out and irrelevant for distance and collision monitoring. The zero crossings of d represent the transitions between workspace and the start/landing space.
Additionally, Figure 14 shows the corresponding threedimensional trajectories of the actual (bold with marks one second apart) and the desired (fine) positions and Figure 15 shows an excerpt from the online visualization of scenario 1 including the momentary state of the dynamically adjusted global paths.
For scenario 2, several long-term tests were performed with a length of at least 15 min. These consisted of a repetitive cycle with random goals followed by a start and a landing maneuver. Figure 16 shows the timeline for each of the UAVs distance d to the closest obstacle. Start and landing phases which are not relevant to collision monitoring are illustrated using a fine line.

Real UAVs
Scenario 3 was carried out with real UAVs (Crazyflie 2.0 by Bitcraze, see Figure 12(c)) within the test cell and again predefined goals were used. In concrete, this means that besides the three UAVs themselves, the workspace boundaries were also physically present in this scenario. The remaining static obstacles within the test cell were modeled virtually. As the sensors of the Crazyflie for position detection (Loco Positioning System by Bitcraze in combination with an optical flow sensor) cannot be utilized near walls, the new goals for scenario 3 listed in Table 2 differ from those used in scenario 1. Figure 17 shows the position and the distance to the closest obstacle over time for an exemplary execution of scenario 3. Figure 18 shows the corresponding threedimensional paths of the actual and desired positions.  Additionally, Figure 19 shows the paths of the actual positions tracked by a camera. Scenarios 1 and 2 demonstrated the ability of the CFM to autonomously generate reference trajectories that continuously adapt to the environment's current state for independently triggered flight movements. The resulting distance timelines show the absence of collisions of the actual trajectories flown by the UAVs. Besides proving the functionality of the CFM, scenarios 1 and 2 also evidence that the CFM is implementable on industrial hardware under real-time conditions with cycle times of only a few milliseconds. In addition, scenario 3 proves the applicability of the CFM for real UAVs.

Conclusion
In this article, the CFM was introduced as a new method for online path planning in dynamic environments. The CFM was theoretically derived and compared to the functionally similar EBM. It was shown that the CFM has improved numerical robustness, parameterability, and computational efficiency. Several validation scenarios with three interlinked instances of the CFM for control of UAVs in the context of intralogistics were performed. The autonomously generated and collision-free trajectories of the UAVs thereby proved the functionality of the CFM and demonstrated its executability on industrial hardware in real-time.