**1. 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–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

**Citation:** Kielas-Jensen, C.; Cichella, V.; Berry, T.; Kaminer, I.; Walton, C.; Pascoal, A. Bernstein Polynomial-Based Method for Solving Optimal Trajectory Generation Problems. *Sensors* **2022**, *22*, 1869. https:// doi.org/10.3390/s22051869

Academic Editor: Roberto Teti

Received: 6 December 2021 Accepted: 2 February 2022 Published: 27 February 2022

**Publisher's Note:** MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

**Copyright:** © 2022 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https:// creativecommons.org/licenses/by/ 4.0/).

determine, in real time, what the next control input should be. On the other hand, openloop methods determine control values or compute motion trajectories out to a specified time horizon with the use of the system's model.

One common closed-loop technique that originally stemmed from maze solving algorithms is the bug algorithm. The bug algorithm, e.g., [9,10], uses local knowledge of the environment and a global goal to either follow a wall or move in a straight line towards the goal. This algorithm can be implemented on very simple devices due to typically requiring only two tactile sensors. However, it does not account for a vehicle's dynamics and constraints. Moreover, bug algorithms are non-optimal methods and cannot be used for the execution of complex missions that require the optimization of some cost. For a review and comparison of bug algorithms, the reader is referred to [11].

Rather than working on an agent's positions, the velocity obstacle (VO) algorithm uses relative velocities between the agent and obstacles to determine trajectories which will avoid collisions. The original term velocity obstacle was presented in [12]. Variations on the VO method include Common Velocity Obstacle [13], Nonlinear Velocity Obstacles [14], and Generalized Velocity Obstacles [15]. Other relevant closed-loop methods use artificial potential fields, which leverage a potential function providing attractive forces towards the goal and repulsive forces away from obstacles [16–18].

Among the advantages of closed-loop methods are fast computation and the ability to react to changing environments and unforeseen events. Furthermore, theoretical tools aimed at deriving safety guarantees of closed-loop methods are fairly well developed, and are mostly rooted in nonlinear systems analysis and robust and adaptive control. Despite these benefits, closed-loop methods are difficult to employ for multiple vehicle teams. They also generally lack the capability of presenting a human operator with a predicted trajectory and act rather like a black box, which can result in a lack of trust between the operator and the autonomous system.

In contrast to closed-loop methods, open-loop methods can generate solutions in one-shot for the whole mission time, and are therefore able to present an operator with an intuitive representation of the future trajectory. This representation is typically shown as a 2D or 3D path and may also include speed, acceleration, and higher derivatives of the vehicle's motion. Randomized algorithms such as probabilistic roadmaps (PRMs) [19] and rapidly exploring random trees (RRT, RRT\*) [20,21] randomly sample the work space to reduce computational complexity. PRMs randomly sample feasible regions within the work space to construct a dense graph. A graph-based solver can then be used to determine the optimal route. RRTs compute trajectories by using directed sampling to build trees. This approach can find feasible solutions in situations involving both a high number of constraints and high dimensional search spaces. Unfortunately, random sampling algorithms may be difficult to use in real-time applications due to computational complexity and may end up exploring regions that will not lead to a solution.

Similar to PRMs, other graph-based approaches aim to efficiently build and then search a graph. Cell decomposition methods, e.g., [22,23], build a graph of their environment by recursively increasing the resolution of areas of interest resulting in a few large nodes of open space and many small nodes near obstacles. Once a graph has been built, a graph solver can be used. A popular graph solver is the A\* algorithm [24], which is an extension of Dijkstra's algorithm that uses a heuristic function to improve the search speed. Many modifications to the A\* algorithm also exist, such as Lifelong Planning A\* [25], which replans a path anytime an obstacle appears on the existing path and utilizes Dijkstra's algorithm to transition the robot from its current pose to the new path. Similarly, Anytime Dynamic A\* (ADA\*) [26] iteratively decreases a suboptimality bound to improve the plan's optimality within a specified maximum computation time limit. Iterative approaches such as ADA\*, sometimes called "anytime" methods, compute a coarse solution and then refine it until a computation timeout is reached. For example, Ref. [27] investigates the addition of committed trajectories and branch-and-bound tree adaptation to the RRT\* algorithm to produce an online anytime method.

In addition to graph-based representations of trajectories, polynomial approximation methods can be used as well. In [28], trajectories are represented as piecewise polynomial functions and are generated in a manner that minimizes their snap. In TrajOpt [29], a sequential quadratic program is solved to generate optimal polynomial trajectories while performing continuous time collision checking.

Other open-loop methods include CHOMP [30,31], STOMP [32], and HOOP [33]. In CHOMP, infeasible trajectories are pulled out of collisions while simultaneously smoothing the trajectories using covariant gradient descent. STOMP adopts a similar cost function to that found in CHOMP, but generalizes to cost functions whose gradients are not available. This is done by stochastically sampling noisy trajectories. HOOP utilizes a problem formulation which computes vehicle trajectories in two steps. In the first step, a path is planned quickly by considering only the vehicle's kinematics. The second step then refines this trajectory into a higher order piecewise polynomial using a quadratic program. Other methods, such as WASPAS-mGqNS [34], balance the optimality of motion plans with respect to the mission objectives against exploring unknown environments.

Open-loop methods provide useful tools for dealing with high dimensional problems such as multiple vehicles and several constraints. They are also capable of producing trajectories that accomplish multiple goals. However, due to the curse of dimensionality, the computational complexity of open-loop methods grows significantly with the number of vehicles, constraints, and goals. For the most part, motion planning methods trade optimality and/or safety for computational speed. Our goal is to introduce a method that mitigates this trade-off, and that provides provably safe solutions for high dimensional problems while retaining the computational efficiency of low-order trajectory planning algorithms. This is achieved by exploiting the useful features of Bernstein polynomials.

The Bernstein basis was originally introduced by Sergei Natanovich Bernstein (1880– 1968) in order to provide a constructive proof of Weierstrass's theorem. Bernstein polynomials were not widely used until the advent of digital computers due to their slow convergence as function approximants. Widespread adoption eventually occurred when it was realized that the coefficients of Bernstein polynomials could be intuitively manipulated to change the shape of curves described by these polynomials. In the 1960s, two French automotive engineers became interested in using this idea: Paul de Faget de Casteljau and Pierre Étienne Bézier.

Designing complex shapes for automobile bodies by sculpting clay models proved to be a time consuming and expensive process. To combat this, de Casteljau and Bézier sought to develop mathematical tools that would allow designers to intuitively construct and manipulate complex shapes. Due to de Casteljau publishing most of his research internally at his place of employment, Bézier's name became more widely associated with Bernstein polynomials, frequently referred to as Bézier curves. Building on existing research and modern technology, Bernstein polynomials provide several useful properties for many fields.

The Bernstein basis provides numerical stability [35], as well as useful geometric properties and computationally efficient algorithms that can be used to derive and implement efficient algorithms for the computation of trajectory bounds, trajectory extrema, minimum temporal and spatial separation between two trajectories and between trajectories and obstacles, and collision detection. Bernstein polynomials also allow for the representation of continuous time trajectories using low-order approximations.

Our method for trajectory generation builds upon [36–38], where Bernstein polynomials were introduced as a tool to approximate the solutions of nonlinear optimal control problems with provable convergence guarantees. While the results concerning the convergence of the Bernstein approximation method are out of the scope of the present paper, here we focus on the design of algorithms and functions for Bernstein polynomials. These include evaluating bounds, minimum spatial distance, collision detection, and penetration distance. Additionally, we show how these properties can be used for trajectory generation in realistic mission scenarios such as trajectory generation for swarms, navigating cluttered

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:


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].

In what follows, vectors are denoted by bold letters, e.g., **p** = [*px* , *py*] and || · || denotes the Euclidean norm (or magnitude), e.g., ||**p**|| = - *p*2 *<sup>x</sup>* + *p*<sup>2</sup> *y*.
