Bernstein Polynomial-Based Method for Solving Optimal Trajectory Generation Problems

This paper presents a method for the generation of trajectories for autonomous system operations. The proposed method is based on the use of Bernstein polynomial approximations to transcribe infinite dimensional optimization problems into nonlinear programming problems. These, in turn, can be solved using off-the-shelf optimization solvers. The main motivation for this approach is that Bernstein polynomials possess favorable geometric properties and yield computationally efficient algorithms that enable a trajectory planner to efficiently evaluate and enforce constraints along the vehicles’ trajectories, including maximum speed and angular rates as well as minimum distance between trajectories and between the vehicles and obstacles. By virtue of these properties and algorithms, feasibility and safety constraints typically imposed on autonomous vehicle operations can be enforced and guaranteed independently of the order of the polynomials. To support the use of the proposed method we introduce BeBOT (Bernstein/Bézier Optimal Trajectories), an open-source toolbox that implements the operations and algorithms for Bernstein polynomials. We show that BeBOT can be used to efficiently generate feasible and collision-free trajectories for single and multiple vehicles, and can be deployed for real-time safety critical applications in complex environments.


Introduction
The field of autonomous guidance has exploded in the past decade. Significant progress has been made in self driving vehicles, bringing them one step closer to reality [1]. Precision agriculture utilizes autonomous aerial vehicles to monitor crops and spray pesticides [2], and the development in autonomous weed pulling robots may reduce or eliminate the need for potentially harmful pesticides [3]. Underactuated marine surface vehicles can be controlled using a flatness-based approach [4]. Companies such as Amazon, Starship, and Zipline have already begun making autonomous deliveries [5][6][7]. In fact the first autonomous aerial vehicle has already been flown on a different planet [8]. This progress has led to high demand for computationally efficient algorithms that may yield safe and optimal trajectories to be planned for groups of autonomous vehicles. Our proposed method aims to accomplish these tasks by formulating the optimal trajectory generation problem as a nonlinear programming problem and exploiting the useful features of Bernstein polynomials.
Most techniques for planning and control of autonomous systems fall into one of two categories: closed-loop methods or open-loop methods. Closed-loop methods, sometimes referred to as feedback or reactive methods, use the current state knowledge to environments, and motion planning for vehicles operating in a Traveling Salesman mission. For the interested reader, an open-source implementation is provided. This paper extends the results initially presented in [39]. In particular, in [39] we focused on autonomous vehicle trajectories representation by Bernstein polynomials, and proposed a preliminary implementation of BeBOT for minimum distance computation and collision detection for safe autonomous operation. In the present paper, we extend previous work by exploiting properties and proposing algorithms for both Bernstein polynomials and rational Bernstein polynomials.
BeBOT includes an open-source Python implementation of these algorithms, which enables the user to exploit the properties of (rational) Bernstein polynomials for trajectory generation. Furthermore, we address several applications for multiple autonomous systems and show the efficacy of BeBOT in enabling safe autonomous operations. We added a new algorithm, the penetration algorithm, and several additional examples including air traffic control, navigation of a cluttered environment, vehicle overtaking, 1000 vehicle swarming, a marine vehicle example, and two examples of a vehicle routing problem. An implementation of the examples presented in this paper is available at our GitHub website [40] and can be customized to facilitate the toolbox's usability.
The goal of this manuscript is to provide a general framework which can be applied to a plethora of different systems ranging from mobile robots to manipulators. However, we do provide several numerical examples for mobile robots and include the governing motion equations for ease of implementation, e.g., see examples in Sections 5.1 and 5. 6.
In brief, the main contributions of this article are: 1.
Novel algorithms which exploit the useful properties of (rational) Bernstein polynomials for use in trajectory generation.

2.
Several examples implementing the aforementioned algorithms in realistic mission scenarios.
The paper is structured as follows. In Section 2 we introduce Bernstein polynomials and their properties. Section 3 introduces the use of Bernstein polynomials to parameterize 2D and 3D trajectories. In Section 4 we present computationally efficient algorithms for the computation of state and input constraints typical of trajectory generation applications. In Section 5 we demonstrate the efficacy of these algorithms through several numerical examples. The paper ends with Section 6, which draws some conclusions. A Python implementation of the properties and algorithms presented, as well as the scripts used to generate the plots and examples found throughout this paper, can be found on our GitHub webpage [40].

Mathematical Preliminaries
The motion planning problems addressed in this work can be in general formulated as optimal control problems. Letting the states and control inputs of the vehicles be denoted by x(t) and u(t), respectively, the optimal motion planning problem can formally be stated as follows: min x(t),u(t) I(x(t), u(t)) = E(x(0), x(t f )) + t f 0 F(x(t), u(t))dt (1) subject toẋ (t) = f (x(t), u(t)) , ∀t ∈ [0, t f ], (2) e(x(0), x(t f )) = 0 , h(x(t), u(t)) ≤ 0 , ∀t ∈ [0, t f ] , where I : R n x × R n u → R, E : R n x × R n x → R, F : R n x × R n u → R, f : R n x × R n u → R n x , e : R n x × R n x → R n e , and h : R n x × R n u → R n h . Here, I defined in Equation (1) is a Bolza-type cost functional, with end point cost E and running cost F. The constraint in Equation (2) enforces the dynamics of the vehicles considered, Equation (3) enforces the boundary conditions, e.g., initial and final position, speed, heading angles of the vehicles, and Equation (4) describes feasibility and mission specific constraints, e.g., minimum and maximum speed, acceleration, collision avoidance constraints, etc.
In previous work [36][37][38] we presented a discretization method to approximate state and input by nth order Bernstein polynomials. This approximation allows us to transcribe the optimal control problem into a non-linear programming problem, which can then be solved by off-the-shelf optimization solvers. In particular, we show that the solution to the non-linear programming problem converges to the solution of the original optimal control problem as n increases. The present paper focuses on the geometric properties of Bernstein polynomials and their implementation for computationally efficient and safe trajectory generation. In the following, we report the properties of Bernstein polynomials and rational Bernstein polynomials which are relevant to this paper.
An nth order Bernstein polynomial defined over an arbitrary interval [t 0 , t f ] is given by where P i,n ∈ R D is the ith Bernstein coefficient, D is the number of dimensions, and B i,n (t) is the Bernstein polynomial basis defined as for all i = 0, . . . , n. Typically the dimensionality of a Bernstein polynomial, D, is either 2 or 3 for 2D or 3D spatial curves, respectively. In this case, Bernstein polynomials are often referred to as Bézier curves and their Bernstein coefficients are known as control points. While Bézier's original work did not explicitly use the Bernstein basis [41,42], it was later shown that the original formulation is equivalent to the Bernstein form polynomial [43]. An nth order rational Bernstein polynomial, R n (t), is defined as where w i,n ∈ R, i = 0, . . . , n, are referred to as weights. A list of relevant properties of Bernstein polynomials used throughout this article can be found in Appendix A.

2D Trajectories
Here we will examine several illustrative examples of the properties of Bernstein polynomials and rational Bernstein polynomials in 2D. All the plots presented can be generated using the example code available at [40]. Figures 1-9 contain several examples of 2D trajectories in the spatial domain. Two trajectories are plotted in Figure 1 along with an obstacle. The trajectories C [1] (t) and C [2] (t) are defined as in Equation (5) with t 0 = 10 s and t f = 20 s where the Bernstein coefficients are temporally equidistant. The vector of Bernstein coefficients of trajectory C [1] (t) is P [1] 5 = 0 2 4 6 8 10 5 0 2 3 10 3 .
Useful operations can be efficiently performed on Bernstein polynomials by manipulating only their coefficients. The de Casteljau algorithm (Property A5) allows one to split a Bernstein polynomial into two separate polynomials. This is shown in Figure 4 where trajectories C [1] (t) and C [2] (t) are split at t div = 15 s. Degree elevation (Property A6) is performed on both trajectories in Figure 5. Note that in both Figures 4 and 5, the convex hulls are more accurate than the conservative convex hulls in Figure 3. This idea will be expanded upon in the next section. Trajectory C [1] (t) is drawn in blue and trajectory C [2] (t) is drawn in orange. The solid lines are the polynomials and the dashed lines connect the Bernstein coefficients for convenience. Note that the temporal aspect of the trajectories above has been omitted for clarity of presenting the geometric properties of Bernstein polynomials.

Figure 2.
Trajectories C [1] (t) (blue) and C [2] (t) (orange) with their endpoints highlighted in 2D. . Convex hulls drawn as black dotted lines around the Bernstein coefficients of trajectories C [1] (t) (blue) and C [2] (t) (orange). The dashed lines connect the control points of their corresponding trajectories.  [1] (t) (split into blue and green) and C [2] (t) (split into orange and red) split at t div = 15 s. Convex hulls are drawn around the Bernstein coefficients of the new split trajectories.

Figure 5.
Convex hull drawn around the elevated Bernstein coefficients of trajectories C [1] (t) (blue) and C [2] (t) (orange). . Squared speed of the trajectories C [1] (t) and C [2] (t). A convex hull is drawn around the Bernstein coefficients. Note that even though the coefficients may be negative, the actual curve is not. Figure 7. Illustration of the quantity expressed in Equation (8) for trajectories C [1] (t) and C [2] (t).  [1] (t) and C [2] (t). Note that the angular rates are rational Bernstein polynomials. Bernstein polynomials can also be used to extract useful information about the dynamics of the trajectories. In Figure 6, Bernstein polynomials representing the squared speed of trajectories C [1] (t) = [x [1] (t), y [1] (t)] and C [2] (t) = [x [2] (t), y [2] (t)] are shown along with their corresponding coefficients and convex hulls. The squared speed is computed using the derivative and arithmetic operation properties (Properties A3 and A7) as follows (v [1] (t)) 2 = (ẋ [1] (t)) 2 + (ẏ [1] (t)) 2 Note that the squared speed of a trajectory described by a Bernstein polynomial is also a Bernstein polynomial.
Since the inverse tangent of a Bernstein polynomial is not a Bernstein polynomial, we take the tangent of both sides of the equation, yielding tan(ψ [1] which is a rational Bernstein polynomial. Figure 7 illustrates the quantity expressed in Equation (8) computed for trajectories C [1] (t) and C [2] (t).
Since the angular rate can be determined using Properties A3 and A7, it can be represented as a rational Bernstein polynomial. The angular rates of trajectories C [1] (t) and C [2] (t) are shown in Figure 8.
Finally, the Bernstein polynomial representing the squared distance between two trajectories at every point in time can be computed from d 2 (t) = (x [2] (t) − x [1] (t)) 2 + (y [2] (t) − y [1] The center point of a circular, static obstacle Obs(t), can be represented as a Bernstein polynomial whose coefficients are all identical and set to the position of the obstacle, i.e., The degree of the Bernstein polynomial representing the center point of the circular obstacle is equal to that of the order of the Bernstein polynomials representing the trajectories.

3D Trajectories
We now introduce two 3D Bernstein polynomials with t 0 = 10 s and t f = 20 s, where the coefficients are equidistant in time, and illustrate their properties in Figures 10-17. The Bernstein coefficients of trajectory C [3] (t) are P [3]  These polynomials are drawn in Figure 10. Figure 10. Two 3D Bernstein polynomial trajectories near a spherical obstacle. Trajectory C [3] (t) is drawn in blue and trajectory C [4] (t) is drawn in orange. Figures 11-14 illustrate the end points, convex hull, de Casteljau, and elevation properties, respectively. Figures 15 and 16 show the squared speed and squared acceleration of trajectories C [3] (t) and C [4] (t), respectively. These values were computed using the derivative and arithmetic properties. Finally, Figure 17 shows the squared Euclidean distance between the trajectories and the center of the spherical obstacle at every point in time. The distance was found using Property A7. Figure 11. 3D trajectories C [3] (t) and C [4] (t) with their endpoints highlighted. Figure 12. 3D convex hulls drawn as transparent blue surfaces around the Bernstein coefficients of trajectories C [3] (t) and C [4] (t).  [3] (t) and C [4] (t) split at t div = 15 s. Convex hulls are drawn around the Bernstein coefficients of the new split trajectories. Figure 14. Convex hull drawn around the elevated Bernstein coefficients of trajectories C [3] (t) and C [4] (t). Figure 15. Squared speed of the trajectories C [3] (t) and C [4] (t). A convex hull is drawn around the Bernstein coefficients. Note that even though the coefficients may be negative, the actual curve is not. Figure 16. Squared acceleration of the trajectories C [3] (t) and C [4] (t) with corresponding convex hulls.

Algorithms for (Rational) Bernstein Polynomials
This section contains algorithms and procedures for Bernstein polynomials that make use of the properties presented in Section 2. These functions include: evaluating bounds, using the convex hull property to quickly find conservative bounds; evaluating extrema, through an iterative procedure that computes a solution within a desired tolerance; minimum spatial distance, applying a similar iterative procedure to find the minimum spatial distance between two Bernstein polynomials; and collision detection, which quickly determines whether a collision may exist or does not exist.

Evaluating Bounds
Property A1 allows one to quickly establish conservative bounds on the Bernstein polynomial. For example, given the 2D Bernstein polynomial, see Equation (5), with coefficients given by P 5 = 0 1 2 3 4 5 5 0 2 5 7 5 , lower and upper bounds C min and C max satisfying C min ≤ C(t) ≤ C max , ∀t ∈ [t 0 , t f ] can be derived using Equation (A1). Figure 18 exhibits the Bernstein polynomial (solid blue line) given the above coefficients (orange dots connected with dashes). The most conservative estimate of the minimum and maximum Y values of the Bernstein polynomial is given by the coefficients with the lowest and highest Y values, respectively. The lower bound is 0 and the upper bound is 7. As mentioned in Property A6 and Equation (A8), the Bernstein coefficients converge to the curve as the polynomial is degree elevated. This fact can be used to derive tighter bounds. A degree elevation of 20 results in a lower bound of 1.93 and an upper bound of 5.89. This is a closer estimate of the actual minimum and maximum, 2.26 and 5.70, respectively (see red dotted lines and Section 4.2). Figure 18 also illustrates degree elevations of 5, 10, and 15. Since the degree elevation matrix, see Equation (A7), is independent of the Bernstein coefficients, a database of elevation matrices can be computed ahead of time to produce tight estimates of the bounds at a low computational cost.

Evaluating Extrema
The extrema of a Bernstein polynomial are calculated using an iterative procedure similar to the one proposed in [44]. This is done by recursively splitting the curve and using the Convex Hull (Property A1) to obtain an estimate within some desired tolerance. Algorithm 1 outlines the process for determining the maximum of a Bernstein polynomial and can easily be modified to determine the minimum of a Bernstein polynomial.
The inputs to Algorithm 1 are the Bernstein polynomial's coefficients, P = {P n }, P n = [P 0,n , . . . , P n,n ], an arbitrarily large negative global lower bound, α, and a desired tolerance, . Note that in order to compute a reliable maximum, α ≤ min (P). In practice, α is set to the lowest possible value that can be reliably represented in the computer.
Line 1 finds the maximum of the two endpoints of the Bernstein polynomial, where n is the degree of the polynomial. This makes use of the End Points (Property A2). Next, we determine the upper bound by simply finding the maximum of P. The if statement on line 3 determines whether the global lower bound should be replaced with the current lower bound. The next if statement, line 6, will prune the current set of Bernstein coefficients. This is valid because α always provides a lower bound on the global maximum. If the upper bound of any subset is below α, then we know that it is impossible for any point on that subset to be the global maximum. The final if statement, line 9, determines whether the difference between the upper and lower bounds is within the desired tolerance and returns the global minimum bound α if the tolerance is met.
The else statement, starting on line 11, splits the curve and then recursively calls Algorithm 1 again. The function split() utilizes the de Casteljau algorithm (Property A5). One of two different splitting points, t div , can be employed. The first option simply splits the curve in half, i.e., t div = t 0 + t f −t 0 2 . The second option uses the index of the largest valued coefficient, i ub = argmax(P ), to determine the splitting point, i.e., t div = t 0 + (t f − t 0 ) i ub n . Algorithm 1 (and its converse) is employed to find the minimum and maximum of the 5th degree Bernstein polynomial depicted in Figure 18 (red lines). The execution time to compute the minimum is 320 µs on a Lenovo ThinkPad laptop using an Intel Core i7-8550U, 1.80 GHz CPU. The implementation can be found in [40].

Algorithm 1: Evaluating Maximum Value of a Bernstein Polynomial
Input : P, α,

Minimum Spatial Distance
The minimum spatial distance between two Bernstein polynomials can be computed using the method outlined in [44]. This is done by exploiting the Convex Hull property (Property A1), the End Point Values property (Property A2), the de Casteljau Algorithm (Property A5), and the Gilbert-Johnson-Keerthi (GJK) algorithm [45]. The latter is widely used in computer graphics to compute the minimum distance between convex shapes.
The algorithm for the minimum spatial distance between two Bernstein polynomials is shown in Algorithm 2. The first two inputs to the function are the sets of Bernstein coefficients, P = {P m } and Q = {Q n }, which define the two Bernstein polynomials in question. The last two inputs are the global upper bound on the minimum distance, α, and a desired tolerance .
The upper_bound() function on line 1 finds the furthest distance between the end points of the two Bernstein polynomials, i.e., lower_bound(P, } where m and n are the degrees of the polynomials represented by P and Q, respectively. This is a valid upper bound on the minimum distance between the two polynomials due to End Point Values (Property A2).
The lower_bound() function on line 2 finds the lower bound on the distance between the two polynomials by using the GJK algorithm. This is a valid lower bound because of Property A1, Convex Hull. The next three if statements on lines 3, 6, and 9 are very similar to those seen in Algorithm 1. Line 3 updates the global upper bound α if the current upper bound is smaller. Line 6 prunes the current iteration since it is impossible the current lower bound, lower, to be the minimum distance if it is larger than the global upper bound. Line 9 returns α if the desired tolerance is met.
The lines within the else statement split the Bernstein polynomials defined by P and Q and recursively call Algorithm 2. Like in Algorithm 1, the first option for splitting would be to simply split at the halfway point. The second option for splitting the curves is outlined in [44] and uses the location at which the minimum distance occurs to choose the splitting point. Figure 19a illustrates the minimum distance between several different Bernstein polynomials. The code to generate this plot can be found in [40]. The execution time to compute the minimum spatial distance is 3.29 ms on a Lenovo ThinkPad laptop using an Intel Core i7-8550U, 1.80 GHz CPU.
Note that Algorithm 2 can also be employed to compute the minimum distance between a Bernstein polynomial and a point or a convex shape. This is shown in Figure 19b.

Collision Detection
In some cases it may be desirable to quickly check the feasibility of a trajectory rather than finding a minimum distance. The collision detection algorithm can be used in these cases. The two major differences between the Collision Detection Algorithm and the Minimum Distance Algorithm described previously are a modification of the GJK algorithm and a change in the stopping criteria. Rather than having the GJK algorithm return a minimum distance, it simply returns whether a collision has been detected (i.e., convex hulls intersecting). The stopping criteria is set to return the moment no collisions are found rather than continuing iterations to meet a desired tolerance. For example, if the original convex hulls of two Bernstein polynomials do not intersect, the collision detection algorithm will return no collision after the first iteration while the minimum distance algorithm will continue to iterate until the desired tolerance is met. Therefore, this algorithm is computationally inexpensive compared to the minimum distance algorithm, with the drawback that it only returns a binary value (no collision or collision possible) rather than a minimum distance.
The collision detection algorithm is shown in Algorithm 3. The inputs are the coefficients of the Bernstein polynomials being compared, P and Q, and the maximum number of iterations max_iter. The while loop beginning on line 2 runs until it is determined that a collision does not exist or until the maximum number of iterations is met. The find_collisions() function on line 3 uses the modified GJK algorithm to determine which convex hulls from the set P collide with those from the set Q. The if statement on line 4 checks to see whether collisions were found. If both P col and Q col are empty, then no collisions exist. If collisions do exist then the for loops starting on lines 7 and 11 split all the convex hulls that were found to collide and add them to the set to be checked. Note that the parent set that is split is removed from the set of convex hulls to check. If the maximum number of iterations is met, then the algorithm returns that a collision is possible. The execution time when a collision is possible is 1.10 ms on a Lenovo ThinkPad laptop using an Intel Core i7-8550U, 1.80 GHz CPU. However, when a collision does not exist, the execution time is only 7.25 µs.

Penetration Algorithm
If two convex shapes intersect, in order to derive information such as the penetration depth and vector, the EPA [46] can be used. A slight modification of the EPA algorithm is proposed here, which is referred to as the DEPA, whose objective is to find the penetration of one convex shape relative to another along a specific direction − → d . The top left plot of Figure 20 shows two shapes intersecting each other, and the remaining plots show examples of penetration vectors, i.e., the vector − → d needed to move the second shape so that it no longer intersects the first shape. The DEPA algorithm finds the shortest possible penetration vector. The pseudocode is reported below (see Algorithm 4).    Figure 22, which contains the origin and with a triangle simplex that contains the origin. This is the desired direction to move a polytope A (blue polytope) of Figure 22 such that it no longer contains polytope B (beige polytope). Once the norm of the point along the edge of the Minkowski Difference parallel to − → d is found, A can then move in the same direction with the same length to no longer intersect B.

Numerical Examples
In this section, numerical examples using the BeBOT toolkit and Python's Scipy Optimization package are examined (flight tests are available at [47]). The implementation of the following examples can be found in [40].

Dubins Car-Time Optimal
In this simple example, several trajectories for a vehicle with Dubins car dynamics are generated to illustrate the properties of Bernstein polynomials. We let the desired trajectory assigned to the vehicle be given by the Bernstein polynomial The square of the speed of the vehicle is a 1D Bernstein polynomial given by v 2 (t) = ||Ċ n (t)|| 2 .
The heading angle is and the angular rate is a 1D rational Bernstein polynomial given by The objective at hand is to find a trajectory that arrives at a desired destination in the minimum possible time while adhering to feasibility and safety constraints. In particular, the trajectory generation problem is as follows: We In the problem above, the initial and final constraints for position, heading, and speed are enforced using the End Point Values property (Property A2) together with Equations (A2), (12) and (13). Similarly, the same property is used to enforce the initial and final speeds and headings (see (A2)). Note that the norm squared of the speed and of the distance between the trajectory and the obstacles can be expressed as Bernstein polynomials (the sum, the difference, and the product between Bernstein polynomials are also Bernstein polynomials). A similar argument can be made for the norm square of the angular rate, which can be expressed as a rational Bernstein polynomial (see Property A7). Thus, the maximum speed and angular rate, and collision avoidance constraints can be enforced using the Evaluating Bounds or Evaluating Extrema procedures described in Sections 4.1 and 4.2. Figure 23 shows the results with n = 10. The blue curve is obtained by enforcing the constraints using the Evaluating Bounds procedure. The optimal time of arrival at the final destination is t f = 9.14 s. Next, we solve the same problem by enforcing the constraints using the Evaluating Bound procedure together with Degree Elevation. Recall that by degree elevating a Bernstein polynomial, the Bernstein coefficients converge towards the curve. Thus, degree elevation can be used to enforce constraints with tighter bounds. The orange and green lines show the trajectories obtained using degree elevations of 30 and 100, respectively. Degree elevation to degree 30 results in an optimal final time t f = 7.64 s. The elevation to degree 100 provides an optimal value t f = 7.12 s. Finally, the trajectory with smallest optimal final time, t f = 6.45 s, depicted as the red curve in Figure 23, is obtained by enforcing the constraints using the Evaluating Extrema algorithm (Section 4.2). While higher degree elevations or evaluating the exact extrema can produce more optimal trajectories, that optimality comes at the cost of additional computation time. Using a Lenovo Thinkpad P52s with an Intel Core i7-8550U CPU with a 1.8 GHz clock and 8 GB of memory, the computation time required for no degree elevation, a degree elevation of 30, a degree elevation of 100, and the exact extrema algorithm was 0.105 s, 0.146 s, 0.201 s, and 0.573 s, respectively. Figure 24 illustrates the squared speed of each example. Figure 25 shows the angular rate of each trial. It can be seen that the vehicle correctly adheres to the speed and angular rate constraints for each trial with the only differences being the final time and proximity to the obstacles.

Remark 2. The Exact Extrema function is a complex non-linear and non-smooth function.
When it is used to enforce constraints, gradient-based optimization solvers such as the one used in this work can fail to converge to a feasible solution, especially if the initial guess is not feasible. One option is to use an iterative procedure where (i) a feasible sub-optimal solution is obtained by enforcing the collision avoidance constraint using the Evaluating Bounds function, and (ii) this solution is then used as an initial guess to solve the (more accurate) problem with the Exact Extrema constraint.

Air Traffic Control-Time Optimal
In this example, we consider the problem of routing several commercial flights between major US cities in two dimensions (i.e., constant altitude). Assuming that each flight departs at the same time, the goal is to minimize the combined flight time of all the vehicles. Let the position, speed, heading, and angular rate of each vehicle under consideration be parameterized as in Section 5.1. We shall also make the assumption that the trajectories are on a 2D plane rather than on the surface of the Earth.
The goal is to compute cumulatively time optimal trajectories subject to maximum speed and angular velocity bounds, initial and final position, angle, and speeds. The vehicles must also maintain a minimum safe distance between each other. This problem can be formulated as follows: The initial and final position constraints are enforced using the End Point Values property (Property A2). Similarly, the same property is used to enforce the initial and final speeds and headings (see (A2)). Note that the norm square of the speed and the norm square of the distance between vehicles can be expressed as 1D Bernstein polynomials (the sum, difference, and product between Bernstein polynomials are also Bernstein polynomials). A similar argument can be made for the norm square of the angular rate, which can be expressed as a rational Bernstein polynomial (see Property A7). Thus, the maximum speed and angular rate, and collision avoidance constraints can be enforced using the Evaluating Bounds or Evaluating Extrema procedures described in Sections 4.1 and 4.2.
The optimized flight plans can be seen in Figure 26. The squared speed of each vehicle is shown in Figure 27. Note that each vehicle begins and ends with the same speed. The vehicles never slow down less than their initial speeds which means they never reach the minimum speed constrain, nor do the vehicles go faster than the maximum speed. In Figure 28, the angular velocity of each vehicle is shown. The minimum and maximum angular rate constraints are shown by the dotted lines. The vehicles' angular rates never approach the minimum or maximum angular rate constraints due to the large area being covered. Finally, the squared euclidean distance between vehicles is shown in Figure 29. As expected, the squared Euclidean distance between two vehicles never falls below the minimum safe distance. Note that curves within the constraint plots end at different times. This is expected since each vehicle has a different final time. The furthest time reached in Figure 29 is less than that of the other plots because the other vehicles have already reached their final time before the longest flight reaches its final time.

Cluttered Environment
In many real world scenarios, robots must safely traverse cluttered environments. In this example, three aerial vehicles traveling at a constant altitude must navigate around several obstacles while also adhering to dynamic and minimum safe distance constraints. Let the position, speed, heading angle, and angular rate of each vehicle be defined as in Section 5.1. The goal of this example is to compute trajectories whose arc length is minimized subject to maximum speed constraints along with initial and final positions, heading angles, and speeds. The vehicles should also adhere to a minimum safe distance between each other and between obstacles. We formulate the problem as follows: k,n || (14) subject to  [20,30] m, [0, 30] m, and [10,30] m. The final speeds and final heading angles are the same as the initial speeds and heading angles. The order of the Bernstein polynomials being used is 7, the final time is t f = 30 s, the minimum safe distance between vehicles is d s = 1 m, the minimum safe distance between vehicles and obstacles is d obs = 2 m, and the maximum speed is v max = 10 m s . The vehicles traversing the cluttered environment can be seen in Figure 30. This experiment has been repeated in the Cooperative Autonomous Systems (CAS) lab using three AR Drones 2.0. The flight tests can be viewed at [47].

Vehicle Overtake
Here we consider an autonomous driving example in which one vehicle attempts to overtake another vehicle while driving around a 90 • corner. The corner is defined by two arcs with a center point located at [140, 0] m. The inner track has a radius of r inner = 125 m and the outer track has a radius of r outer = 140 m. To clearly distinguish the vehicle being overtaken, it will be referred to as the opponent.
For simplicity, we consider the objective of minimizing the arc-length of the trajectory, which can be done by minimizing the sum of the squared Euclidean norm of consecutive control points, i.e., The desired endpoint of the vehicle is at the end of the corner. This is computed by measuring the angle between the vehicle's position and the end of the curve, where the function arctan 2 returns an angle in the correct quadrant [48]. Given Equations (15) and (16), we formulate the problem as min P n ((1 − α)E(P n ) + αA(P n ))β (17) subject to where α and β are tuning parameters. C 0 , ψ 0 , and v 0 are the initial position, heading, and speed of the vehicle, respectively. v max and ω max are the maximum speed and angular rate, respectively. The predicted trajectory of the opponent is represented as the Bernstein polynomial O n (t) and the minimum safe distance to the opponent is d s . Using a sensor such as a camera or LiDAR, one could measure the state of the opponent and then predict its future position using a method such as the one presented in [49]. At time t = t 0 , when planning occurs, the position of the vehicle is [5,0] m, its speed is 50 m s , and its initial heading angle is π 2 rad. The control points of the Bernstein polynomial representing the opponent's trajectory are The maximum speed is 65 m s , the maximum angular rate is π 5 rad s , and the minimum safe distance is 3 m. The tuning parameters are α = 1 − 10 −6 and β = 100. Figure 31 illustrates the optimized vehicle trajectory overtaking the opponent's trajectory. Figure 32 shows the squared speed of the vehicle along with the maximum speed constraint. As expected, the vehicle's speed approaches the maximum speed in order to successfully overtake the opponent. Figure 33 shows the vehicle's angular rate and its upper and lower constraints. It is clear that the vehicle remains within the desired bounds. Figure 34 shows the squared distance between the vehicle and the opponent. While the vehicle does come close to the opponent, it is never closer than the minimum safe distance.

Swarming
This section examines two methods for generating trajectories for large groups of autonomous aerial vehicles. The centralized method optimizes every trajectory at once. On the other hand, the decentralized method generates trajectories one at a time and compares them to previously generated trajectories.
The position of each vehicle in a swarm of m vehicles for the following examples is parameterized as a 3D Bernstein polynomial, i.e., n ∑ i=0 P [j] i,n B i,n (t) = C n ∈ R 3×n .

101 Vehicle-Centralized
The centralized method optimizes the trajectories for each vehicle simultaneously. The goal is to minimize the arc length of each trajectory. There are m vehicles with 3rd order Bernstein polynomials representing their trajectories which are constrained to a minimum safe distance between each other and initial and final positions. This is formulated as follows: The initial positions for each vehicle were chosen randomly from a 25 m × 25 m grid at an altitude of z = 0 m. The final positions were chosen to spell out "CAS", as seen in Figure 35, at an altitude of z = 100 m. In the next section we significantly reduce the number of dimensions in the optimization vector by using the decentralized approach.

101 Vehicle-Decentralized
The decentralized method iteratively computes trajectories for the ith vehicle. Each new iteration is compared to the previously computed trajectories so that the minimum safety distance constraint is met. The problem that is solved at each iteration is written as Note that the first vehicle does not need to satisfy the minimum safe distance constraint since no trajectories have been computed before it.
The parameters used in this example were identical to that of the previous subsection. The resulting figure has been omitted due to its similarity to Figure 35.

1000 Vehicle-Decentralized
The decentralized method can be used to compute 1000 trajectories. In this example, it is employed to generate the paths seen in Figure 36 to display the University of Iowa Hawkeye logo. The initial points are equally dispersed at an altitude of z = 0 m on a 100 m × 100 m grid. The final points are the pattern shown at an altitude of z = 100. The cost function aims to maximize the temporal distance between the current ith trajectory and the previously generated jth trajectories by taking the reciprocal of the sum of the Bernstein coefficients of the norm squared difference, i.e.
where P [norm,j] are the Bernstein coefficients of the Bernstein polynomial representing the squared temporal distance between the ith and jth trajectories, i.e., It should be noted that this formulation of cost function and constraints is used as a proof of concept. For other possible cost function and constraint formulations, the reader is referred to [50,51].

Marine Vehicle Model
In this example, we consider a marine vehicle model known as the medusa. The equations of motion of the medusa are as followṡ where x and y represent the vehicle's position, ψ is the orientation, u (surge) and v (sway) are the linear velocities, r is the turning rate, and τ = [τ u , τ r ] T is the vector of forces and torques due to thrusters/surfaces (control input).
In this example, we let the state, [x, y, ψ, u, v, r] , and input, [τ u , τ r ] , be approximated by Bernstein polynomials, and impose the vehicle's dynamics directly through Bernstein polynomial differentiation. Using Property A3, the dynamics constraints given by Equations (18) and (19) reduce to a set of algebraic constraints. Additional constraints imposed on this problem include collision avoidance and input saturation constraints. Figure 37 shows an example of motion planning for a medusa vehicle, which is required to reach a final destination in the minimum time. Ten markers are plotted along the trajectory (shown in blue) to represent the heading of the vehicle at that point in time. It can easily be seen that the vehicle's trajectory avoids the (inflated) unsafe region illustrated by the orange circle. We consider a problem where a single vehicle is supposed to visit M neighborhoods B i = {x ∈ R 2 : x − b i ≤ r } in minimum time t f . Here r > 0 and the vectors b i ∈ R 2 , i ∈ [1, M] represent a sequence of points of interest that has been generated by a Traveling Salesman Problem (TSP) algorithm. Let t i denote a time instance when the vehicle's position satisfies C n (t i ) ∈ B i . Then, the dynamic routing problem for a single vehicle can be formulated as follows, DR 1 min t i ,i∈ [1,M], P n t f (20) subject to Define Then the numerical solution of the dynamic routing problem DR 1 was obtained by solving the optimization problem min t i ,i∈ [1,M], P i n t f (23) subject to A simulation was performed illustrating a single agent visiting 30 neighborhoods. The resulting trajectory is shown in Figure 38. The agent is limited to velocities of arbitrary units ranging from −1 to 1 and is similarly limited to accelerations of arbitrary units also from −1 to 1. The velocities and accelerations of the vehicle can be seen in Figure 39 and Figure 40, respectively.

Multiple Vehicle Case
In this case, K drones are assigned a total of K neighborhood sets to visit. Each neighborhood set, P k , consists of an equal number of neighborhoods B ij which are defined by a set of points of interest b ik ∈ R 2 , i.e., Let t f k , k ∈ [1, K] denote the total time it takes for the kth vehicle to visit every neighborhood in the set P k once and let t ik denote a time instance when the kth vehicle's position satisfies C k n (t ik ) ∈ B ik . Using this notation, we propose the following definition of the multi-vehicle dynamic routing problem for given positive number w k and d DR 2 min t ik ,i∈ [1,M],k∈ [1,K], P k n K ∑ k=1 w k t f k (24) subject to Simulations were performed for a two agent and ten agent case each visiting 10 neighborhoods per agent. The resulting trajectories for the two agent case are shown in Figure 41 and for the ten agent case in Figure 42.

Conclusions
We presented a method to generate optimal trajectories by using Bernstein polynomials to transcribe the problem into a nonlinear programming problem. By exploiting the useful properties of Bernstein polynomials, our method provides computationally efficient algorithms that can also guarantee safety in continuous time which are useful in optimization routines. These algorithms include evaluating bounds, evaluating extrema, minimum spatial distance between two Bernstein polynomials, minimum spatial distance between a Bernstein polynomial and a convex polygon, collision detection, and the penetration algorithm. We also developed an open source toolbox which makes these transcription methods readily available in the Python programming language.
Numerical examples were provided to demonstrate the efficacy of the method. Simple cost functions and constraints were implemented to generate trajectories for several realistic mission scenarios including air traffic control, navigating a cluttered environment, overtaking a vehicle, trajectory generation for a large swarm of vehicles, trajectory generation for a marine vehicle, and navigation for vehicles operating in a Traveling Salesman mission. Our formulation offers a powerful tool for users to generate optimal trajectories in real time scenarios for single or multiple robot teams. Future work includes developing new cost functions, exploring different optimization frameworks, and replanning trajectories to react to a changing environment.

Conflicts of Interest:
The authors declare no conflict of interest.

Appendix A
In this section we present and examine relevant properties of Bernstein polynomials and rational Bernstein polynomials, which are used throughout the article.

Property A1. Convex Hull
Both the Bernstein polynomial, Equation (5), and the rational Bernstein polynomial (6) (provided that w i,n > 0, i = 0, . . . , n) satisfy min k∈{0,...,n} It follows that 2D (or 3D) Bernstein polynomials and rational Bernstein polynomials lie within the convex hull defined by their Bernstein coefficients. An example of this property is depicted in Figure A1, which shows a 2D Bernstein polynomial contained within its convex hull.

Property A2. End Point Values
The first and last Bernstein coefficients of the Bernstein polynomial introduced in Equation (5), as well as the rational Bernstein polynomial in Equation (6), are their endpoints, i.e., C n (t 0 ) = P 0 and C n (t f ) = P n .
Furthermore, the tangents to the curve at its first and last coefficients are the same as the lines connecting the first and second coefficients and the penultimate and ultimate coefficients, respectively. It follows that the first derivative of the Bernstein polynomial at the end points is as followsĊ C n (t f ) = n t f − t 0 (P n,n − P n−1,n ).

Property A3. Derivatives
The derivative of the Bernstein polynomial introduced in Equation (5) is an (n − 1)th order Bernstein polynomial given byĊ with the vector of Bernstein coefficients P n−1 = [P 0,n−1 , . . . , P n−1,n−1 ] given by P n−1 = P n D n .
In the equation above, D n denotes the differentiation matrix given by Remark A1. Properties A2 and A3 can be easily extended to compute higher order derivatives of Bernstein polynomials.

Property A4. Integrals
The definite integral of a Bernstein polynomial is given by Property A5. The de Casteljau Algorithm The de Casteljau algorithm computes a Bernstein polynomial defined over an interval [t 0 , t f ] at any given t div ∈ [t 0 , t f ]. Moreover, it can be used to split a single Bernstein polynomial into two independent Bernstein polynomials. Given the Bernstein polynomial introduced in Equation (5) with the vector of coefficients P n = [P 0,n , . . . , P n,n ], and a scalar t div ∈ [t 0 , t f ], the Bernstein polynomial at t div is computed using the following recursive relationship: P 0 i,n = P i,n , i = 0, . . . , n , with i = 0, . . . , n − j , and j = 1, . . . , n. Then, the Bernstein polynomial evaluated at t div is C n (t div ) = P n 0,n . Note that the superscript here signifies an index and not an exponent. Moreover, the Bernstein polynomial can be subdivided at t div into two nth order Bernstein polynomials with coefficients P 0 0,n , P 1 0,n , . . . , P n 0,n and P n 0,n , P n−1 1,n , . . . , P 0 n,n . A geometric example of a 3rd order Bernstein polynomial being split at t div = 0.5 using the de Casteljau is shown in Figure A2. Note that at the final iteration only a single point remains, P 3 0,n , and lies on the original curve. The points {P 0 0,n , P 1 0,n , P 2 0,n , P 3 0,n } become the Bernstein coefficients of the left curve and the points {P 3 0,n , P 2 1,n , P 1 2,n , P 0 3,n } become the coefficients of the right curve. Figure A2. Geometric example of the de Casteljau algorithm splitting a 2D Bernstein polynomial at t div = 0.5. The original curve is defined by the Bernstein coefficients {P 0 0,n , . . . , P 0 3,n }, the left hand curve is shown in red, the right hand curve is shown in blue, and the superscript corresponds to the current iteration of the algorithm.
Remark A2. For high order Bernstein polynomials, the exponential terms in Equation (5) can cause overflows. The de Casteljau algorithm can be used to overcome these issues on a computer when computing the value of a Bernstein polynomial, especially if lower bit floating point values are to be used such as 16 or 32 bits.

Property A6. Degree Elevation
The degree of a Bernstein polynomial can be elevated by one using the following recursive relationship P i,n+1 = i n + 1 P i−1,n + 1 − i n + 1 P i,n , where P 0,n+1 = P 0,n and P n+1,n+1 = P n,n . Furthermore, any Bernstein polynomial of degree n can be expressed as a Bernstein polynomial of degree m, m > n. The vector of coefficients of the degree elevated Bernstein polynomial, namely P m = [P 0,m , . . . , P m,m ], can be calculated as where E = {e j,k } ∈ R (n+1)×(m+1) is the degree elevation matrix with elements given by where i = 0, . . . , n and j = 0, . . . , m − n, all other values in the matrix are zero, and P n = [P 0,n , . . . , P n,n ] is the vector of Bernstein coefficients of the curve being elevated (see [52]). Because E is conveniently independent of the coefficients, it can be computed ahead of time and stored for efficient computation of degree elevated Bernstein polynomials. By degree elevating a Bernstein polynomial, the coefficients converge to the polynomial, i.e.
where k is a positive constant independent of m (see [53]).

Property A7. Arithmetic Operations
Arithmetic operations of Bernstein polynomials require that both curves be defined on the same time interval [t 0 , t f ]. In case they are not, the de Casteljau algorithm can be used to split them at an intersecting time interval.
The sum and difference of two polynomials of the same order can be performed by simply adding and subtracting their Bernstein coefficients, respectively. For Bernstein polynomials of different order, the Degree Elevation (Property A6) may be used to increase the order of the lower order Bernstein polynomial.
Given two Bernstein polynomials, F m (t) and G n (t), with degrees m and n, respectively, and having Bernstein coefficients X 0,m , . . . , X m,m and Y 0,n , . . . , Y n,n , the product C m+n (t) = F m (t)G n (t) is a Bernstein polynomial of degree (m + n) with coefficients P k,m+n , k ∈ {0, . . . , m + n} given by  The ratio between two Bernstein polynomials, F n (t) and G n (t), with coefficients X 0,n , . . . , X n,n and Y 0,n , . . . , Y n,n , i.e., R n (t) = F n (t)/G n (t), can be expressed as a rational Bernstein polynomial as defined in Equation (6), with coefficients and weights P i,n = X i,n Y i,n , w i,n = Y i,n , respectively.