Optimal Sampling-Based Motion Planning under Differential Constraints: the Drift Case with Linear Affine Dynamics

In this paper we provide a thorough, rigorous theoretical framework to assess optimality guarantees of sampling-based algorithms for drift control systems: systems that, loosely speaking, can not stop instantaneously due to momentum. We exploit this framework to design and analyze a sampling-based algorithm (the Differential Fast Marching Tree algorithm) that is asymptotically optimal, that is, it is guaranteed to converge, as the number of samples increases, to an optimal solution. In addition, our approach allows us to provide concrete bounds on the rate of this convergence. The focus of this paper is on mixed time/control energy cost functions and on linear affine dynamical systems, which encompass a range of models of interest to applications (e.g., double-integrators) and represent a necessary step to design, via successive linearization, sampling-based and provably-correct algorithms for non-linear drift control systems. Our analysis relies on an original perturbation analysis for two-point boundary value problems, which could be of independent interest

Asymptotically Optimal Sampling-Based Motion Planning Methods

Motion planning is a fundamental problem in autonomous robotics that requires finding a path to a specified goal that avoids obstacles and takes into account a robot's limitations and constraints. It is often desirable for this path to also optimize a cost function, such as path length. Formal path-quality guarantees for continuously valued search spaces are an active area of research interest. Recent results have proven that some sampling-based planning methods probabilistically converge toward the optimal solution as computational effort approaches infinity. This survey summarizes the assumptions behind these popular asymptotically optimal techniques and provides an introduction to the significant ongoing research on this topic.Comment: Posted with permission from the Annual Review of Control, Robotics, and Autonomous Systems, Volume 4. Copyright 2021 by Annual Reviews, https://www.annualreviews.org/. 25 pages. 2 figure

Topology-Guided Path Integral Approach for Stochastic Optimal Control in Cluttered Environment

This paper addresses planning and control of robot motion under uncertainty that is formulated as a continuous-time, continuous-space stochastic optimal control problem, by developing a topology-guided path integral control method. The path integral control framework, which forms the backbone of the proposed method, re-writes the Hamilton-Jacobi-Bellman equation as a statistical inference problem; the resulting inference problem is solved by a sampling procedure that computes the distribution of controlled trajectories around the trajectory by the passive dynamics. For motion control of robots in a highly cluttered environment, however, this sampling can easily be trapped in a local minimum unless the sample size is very large, since the global optimality of local minima depends on the degree of uncertainty. Thus, a homology-embedded sampling-based planner that identifies many (potentially) local-minimum trajectories in different homology classes is developed to aid the sampling process. In combination with a receding-horizon fashion of the optimal control the proposed method produces a dynamically feasible and collision-free motion plans without being trapped in a local minimum. Numerical examples on a synthetic toy problem and on quadrotor control in a complex obstacle field demonstrate the validity of the proposed method.Comment: arXiv admin note: text overlap with arXiv:1510.0534

Feedback MPC for Torque-Controlled Legged Robots

The computational power of mobile robots is currently insufficient to achieve torque level whole-body Model Predictive Control (MPC) at the update rates required for complex dynamic systems such as legged robots. This problem is commonly circumvented by using a fast tracking controller to compensate for model errors between updates. In this work, we show that the feedback policy from a Differential Dynamic Programming (DDP) based MPC algorithm is a viable alternative to bridge the gap between the low MPC update rate and the actuation command rate. We propose to augment the DDP approach with a relaxed barrier function to address inequality constraints arising from the friction cone. A frequency-dependent cost function is used to reduce the sensitivity to high-frequency model errors and actuator bandwidth limits. We demonstrate that our approach can find stable locomotion policies for the torque-controlled quadruped, ANYmal, both in simulation and on hardware.Comment: Paper accepted to IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2019

A Partially Randomized Approach to Trajectory Planning and Optimization for Mobile Robots with Flat Dynamics

Motion planning problems are characterized by huge search spaces and complex obstacle structures with no concise mathematical expression. The fixed-wing airplane application considered in this thesis adds differential constraints and point-wise bounds, i. e. an infinite number of equality and inequality constraints. An optimal trajectory planning approach is presented, based on the randomized Rapidly-exploring Random Trees framework (RRT*). The local planner relies on differential flatness of the equations of motion to obtain tree branch candidates that automatically satisfy the differential constraints. Flat output trajectories, in this case equivalent to the airplane's flight path, are designed using Bézier curves. Segment feasibility in terms of point-wise inequality constraints is tested by an indicator integral, which is evaluated alongside the segment cost functional. Although the RRT* guarantees optimality in the limit of infinite planning time, it is argued by intuition and experimentation that convergence is not approached at a practically useful rate. Therefore, the randomized planner is augmented by a deterministic variational optimization technique. To this end, the optimal planning task is formulated as a semi-infinite optimization problem, using the intermediate result of the RRT(*) as an initial guess. The proposed optimization algorithm follows the feasible flavor of the primal-dual interior point paradigm. Discretization of functional (infinite) constraints is deferred to the linear subproblems, where it is realized implicitly by numeric quadrature. An inherent numerical ill-conditioning of the method is circumvented by a reduction-like approach, which tracks active constraint locations by introducing new problem variables. Obstacle avoidance is achieved by extending the line search procedure and dynamically adding obstacle-awareness constraints to the problem formulation. Experimental evaluation confirms that the hybrid approach is practically feasible and does indeed outperform RRT*'s built-in optimization mechanism, but the computational burden is still significant.Bewegungsplanungsaufgaben sind typischerweise gekennzeichnet durch umfangreiche Suchräume, deren vollständige Exploration nicht praktikabel ist, sowie durch unstrukturierte Hindernisse, für die nur selten eine geschlossene mathematische Beschreibung existiert. Bei der in dieser Arbeit betrachteten Anwendung auf Flächenflugzeuge kommen differentielle Randbedingungen und beschränkte Systemgrößen erschwerend hinzu. Der vorgestellte Ansatz zur optimalen Trajektorienplanung basiert auf dem Rapidly-exploring Random Trees-Algorithmus (RRT*), welcher die Suchraumkomplexität durch Randomisierung beherrschbar macht. Der spezifische Beitrag ist eine Realisierung des lokalen Planers zur Generierung der Äste des Suchbaums. Dieser erfordert ein flaches Bewegungsmodell, sodass differentielle Randbedingungen automatisch erfüllt sind. Die Trajektorien des flachen Ausgangs, welche im betrachteten Beispiel der Flugbahn entsprechen, werden mittels Bézier-Kurven entworfen. Die Einhaltung der Ungleichungsnebenbedingungen wird durch ein Indikator-Integral überprüft, welches sich mit wenig Zusatzaufwand parallel zum Kostenfunktional berechnen lässt. Zwar konvergiert der RRT*-Algorithmus (im probabilistischen Sinne) zu einer optimalen Lösung, jedoch ist die Konvergenzrate aus praktischer Sicht unbrauchbar langsam. Es ist daher naheliegend, den Planer durch ein gradientenbasiertes lokales Optimierungsverfahren mit besseren Konvergenzeigenschaften zu unterstützen. Hierzu wird die aktuelle Zwischenlösung des Planers als Initialschätzung für ein kompatibles semi-infinites Optimierungsproblem verwendet. Der vorgeschlagene Optimierungsalgorithmus erweitert das verbreitete innere-Punkte-Konzept (primal dual interior point method) auf semi-infinite Probleme. Eine explizite Diskretisierung der funktionalen Ungleichungsnebenbedingungen ist nicht erforderlich, denn diese erfolgt implizit durch eine numerische Integralauswertung im Rahmen der linearen Teilprobleme. Da die Methode an Stellen aktiver Nebenbedingungen nicht wohldefiniert ist, kommt zusätzlich eine Variante des Reduktions-Ansatzes zum Einsatz, bei welcher der Vektor der Optimierungsvariablen um die (endliche) Menge der aktiven Indizes erweitert wird. Weiterhin wurde eine Kollisionsvermeidung integriert, die in den Teilschritt der Liniensuche eingreift und die Problemformulierung dynamisch um Randbedingungen zur lokalen Berücksichtigung von Hindernissen erweitert. Experimentelle Untersuchungen bestätigen, dass die Ergebnisse des hybriden Ansatzes aus RRT(*) und numerischem Optimierungsverfahren der klassischen RRT*-basierten Trajektorienoptimierung überlegen sind. Der erforderliche Rechenaufwand ist zwar beträchtlich, aber unter realistischen Bedingungen praktisch beherrschbar

Survey on Motion Planning for Multirotor Aerial Vehicles in Plan-based Control Paradigm

In general, optimal motion planning can be performed both locally and globally. In such a planning, the choice in favour of either local or global planning technique mainly depends on whether the environmental conditions are dynamic or static. Hence, the most adequate choice is to use local planning or local planning alongside global planning. When designing optimal motion planning both local and global, the key metrics to bear in mind are execution time, asymptotic optimality, and quick reaction to dynamic obstacles. Such planning approaches can address the aforesaid target metrics more efficiently compared to other approaches such as path planning followed by smoothing. Thus, the foremost objective of this study is to analyse related literature in order to understand how the motion planning, especially trajectory planning, problem is formulated, when being applied for generating optimal trajectories in real-time for Multirotor Aerial Vehicles, impacts the listed metrics. As a result of the research, the trajectory planning problem was broken down into a set of subproblems, and the lists of methods for addressing each of the problems were identified and described in detail. Subsequently, the most prominent results from 2010 to 2022 were summarized and presented in the form of a timeline

Kinodynamic Planning with μ-Calculus Specifications

Motion planning problems involve determining appropriate control inputs to guide a system towards a desired endpoint. Sampling-based motion planning was developed as a technique for discretizing the state space of systems with complex environments. This makes the sampling-based method especially useful in robotics, where robots are expected to perform tasks in unknown, changing, or cluttered environments. On the other hand, temporal logic presents a means of prescribing the desired behaviour of a system. In the area of formal methods, researchers seek to solve problems in such a way that synthesized solutions provably satisfy a given temporal logic specification. In this thesis, we investigate combining the flexibility of sampling-based planning with the ability to specify the high-level behaviour of an autonomous system with the temporal logic known as mu-calculus. While using temporal logic specifications with motion planning has been heavily researched, reliance on an available steering function is often impractical and suited only to basic problems with linear dynamics. This is because a steering function is a solution to an optimal two-point boundary value problem (OBVP); thus far, mathematicians have yet to find analytic solutions to such problems in all but the simplest of cases. Addressing this issue, we have developed a means of using the motion planning algorithm SST* in combination with a local model checking procedure to solve kinodynamic planning problems with deterministic mu-calculus specifications without using a steering function. The procedure involves combining only the most pertinent information from multiple Kripke structures in order to create one abstracted Kripke structure storing the best paths to all possible proposition regions of the state-space. A linear-quadratic regulator (LQR) feedback control policy is then used to track these best paths, effectively connecting the trajectories found from multiple Kripke structures. Simulations demonstrate that it is possible to satisfy a complex liveness specification involving infinitely often reaching specified regions of state-space using only forward propagation of the system dynamics. We proceed to repurpose this tool for real-time quadrotor motion planning with temporal logic specifications. The dynamical system is derived, and a real-time planning framework is presented based on a variant of the FMT* planning algorithm. Despite requiring a steering function, an argument is presented which allows finding OBVP solutions only for an approximation of the full dynamics. The notion of an abstracted Kripke structure is then applied in the context of quadrotor kinodynamic planning, allowing for rapid model checking and ensuring high-quality feasible solutions satisfying a given deterministic mu-calculus specification