Embodied Footprints: A Safety-guaranteed Collision Avoidance Model for Numerical Optimization-based Trajectory Planning

Numerical optimization-based methods are among the prevalent trajectory planners for autonomous driving. In a numerical optimization-based planner, the nominal continuous-time trajectory planning problem is discretized into a nonlinear program (NLP) problem with finite constraints imposed on finite collocation points. However, constraint violations between adjacent collocation points may still occur. This study proposes a safety-guaranteed collision-avoidance modeling method to eliminate the collision risks between adjacent collocation points in using numerical optimization-based trajectory planners. A new concept called embodied box is proposed, which is formed by enlarging the rectangular footprint of the ego vehicle. If one can ensure that the embodied boxes at finite collocation points are collide-free, then the ego vehicle's footprint is collide-free at any a moment between adjacent collocation points. We find that the geometric size of an embodied box is a simple function of vehicle velocity and curvature. The proposed theory lays a foundation for numerical optimization-based trajectory planners in autonomous driving.Comment: 12 pages, 13 figure

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

Robust aircraft trajectory optimization under meteorological uncertainty

Mención Internacional en el título de doctorThe Air Traffic Management (ATM) system in the busiest airspaces in the world is currently being overhauled to deal with multiple capacity, socioeconomic, and environmental challenges. One major pillar of this process is the shift towards a concept of operations centered on aircraft trajectories (called Trajectory-Based Operations or TBO in Europe) instead of rigid airspace structures. However, its successful implementation (and, thus, the realization of the associated improvements in ATM performance) rests on appropriate understanding and management of uncertainty. Due to its complex socio-technical structure, the design and operations of the ATM system are heavily impacted by uncertainty, proceeding from multiple sources and propagating through the interconnections between its subsystems. One major source of ATM uncertainty is weather. Due to its nonlinear and chaotic nature, a number of meteorological phenomena of interest cannot be forecasted with complete accuracy at arbitrary lead times, which leads to uncertainty or disruption in individual air and ground operations that propagates to all ATM processes. Therefore, in order to achieve the goals of SESAR and similar programs, it is necessary to deal with meteorological uncertainty at multiple scales, from the trajectory prediction and planning processes to flow and traffic management operations. This thesis addresses the problem of single-aircraft flight planning considering two important sources of meteorological uncertainty: wind prediction error and convective activity. As the actual wind field deviates from its forecast, the actual trajectory will diverge in time from the planned trajectory, generating uncertainty in arrival times, sector entry and exit times, and fuel burn. Convective activity also impacts trajectory predictability, as it leads pilots to deviate from their planned route, creating challenging situations for controllers. In this work, we aim to develop algorithms and methods for aircraft trajectory optimization that are able to integrate information about the uncertainty in these meteorological phenomena into the flight planning process at both pre-tactical (before departure) and tactical horizons (while the aircraft is airborne), in order to generate more efficient and predictable trajectories. To that end, we frame flight planning as an optimal control problem, modeling the motion of the aircraft with a point-mass model and the BADA performance model. Optimal control methods represent a flexible and general approach that has a long history of success in the aerospace field. As a numerical scheme, we use direct methods, which can deal with nonlinear systems of moderate and high-dimensional state spaces in a computationally manageable way. Nevertheless, while this framework is well-developed in the context of deterministic problems, the techniques for the solution of practical optimal control problems under uncertainty are not as mature, and the methods proposed in the literature are not applicable to the flight planning problem as it is now understood. The first contribution of this thesis addresses this challenge by introducing a framework for the solution of general nonlinear optimal control problems under parametric uncertainty. It is based on an ensemble trajectory scheme, where the trajectories of the system under multiple scenarios are considered simultaneously within the same dynamical system and the uncertain optimal control problem is turned into a large conventional optimal control problem that can be then solved by standard, well-studied direct methods in optimal control. We then employ this approach to solve the robust flight plan optimization problem at the planning horizon. In order to model uncertainty in the wind and estimating the probability of convective conditions, we employ Ensemble Prediction System (EPS) forecasts, which are composed by multiple predictions instead of a single deterministic one. The resulting method can be used to optimize flight plans for maximum expected efficiency according to the cost structure of the airline; additionally, predictability and exposure to convection can be incorporated as additional objectives. The inherent tradeoffs between these objectives can be assessed with this methodology. The second part of this thesis presents a solution for the rerouting of aircraft in uncertain convective weather scenarios at the tactical horizon. The uncertain motion of convective weather cells is represented with a stochastic model that has been developed from the output of a deterministic satellite-based nowcast product, Rapidly Developing Thunderstorms (RDT). A numerical optimal control framework, based on the pointmass model with the addition of turn dynamics, is employed for optimizing efficiency and predictability of the proposed trajectories in the presence of uncertainty about the future evolution of the storm. Finally, the optimization process is initialized by a randomized heuristic procedure that generates multiple starting points. The combined framework is able to explore and as exploit the space of solution trajectories in order to provide the pilot or the air traffic controller with a set of different suggested avoidance trajectories, as well as information about their expected cost and risk. The proposed methods are tested on example scenarios based on real data, showing how different user priorities lead to different flight plans and what tradeoffs are then present. These examples demonstrate that the solutions described in this thesis are adequate for the problems that have been formulated. In this way, the flight planning process can be enhanced to increase the efficiency and predictability of individual aircraft trajectories, which would lead to higher predictability levels of the ATM system and thus improvements in multiple performance indicators.El sistema de gestión del tráfico aéreo (Air Traffic Management, ATM) en los espacios aéreos más congestionados del mundo está siendo reformado para lidiar con múltiples desafíos socioeconómicos, medioambientales y de capacidad. Un pilar de este proceso es el gradual reemplazo de las estructuras rígidas de navegación, basadas en aerovías y waypoints, hacia las operaciones basadas en trayectorias. No obstante, la implementación exitosa de este concepto y la realización de las ganancias esperadas en rendimiento ATM requiere entender y gestionar apropiadamente la incertidumbre. Debido a su compleja estructura socio-técnica, el diseño y operaciones del sistema ATM se encuentran marcadamente influidos por la incertidumbre, que procede de múltiples fuentes y se propaga por las interacciones entre subsistemas y operadores humanos. Uno de los principales focos de incertidumbre en ATM es la meteorología. Debido a su naturaleza no-linear y caótica, muchos fenómenos de interés no pueden ser pronosticados con completa precisión en cualquier horizonte temporal, lo que crea disrupción en las operaciones en aire y tierra que se propaga a otros procesos de ATM. Por lo tanto, para lograr los objetivos de SESAR e iniciativas análogas, es imprescindible tener en cuenta la incertidumbre en múltiples escalas espaciotemporales, desde la predicción de trayectorias hasta la planificación de flujos y tráfico. Esta tesis aborda el problema de la planificación de vuelo de aeronaves individuales considerando dos fuentes importantes de incertidumbre meteorológica: el error en la predicción del viento y la actividad convectiva. Conforme la realización del viento se desvía de su previsión, la trayectoria real se desviará temporalmente de la planificada, lo que implica incertidumbre en tiempos de llegada a sectores y aeropuertos y en consumo de combustible. La actividad convectiva también tiene un impacto en la predictibilidad de las trayectorias, puesto que obliga a los pilotos a desviarse de sus planes de vuelo para evitarla, cambiado así la situación de tráfico. En este trabajo, buscamos desarrollar métodos y algoritmos para la optimización de trayectorias que puedan integrar información sobre la incertidumbre en estos fenómenos meteorológicos en el proceso de diseño de planes de vuelo en horizontes de planificación (antes del despegue) y tácticos (durante el vuelo), con el objetivo de generar trayectorias más eficientes y predecibles. Con este fin, formulamos la planificación de vuelo como un problema de control óptimo, modelando la dinámica del avión con un modelo de masa puntual y el modelo de rendimiento BADA. El control óptimo es un marco flexible y general con un largo historial de éxito en el campo de la ingeniería aeroespacial. Como método numérico, empleamos métodos directos, que son capaces de manejar sistemas dinámicos de alta dimensión con costes computacionales moderados. No obstante, si bien esta metodología es madura en contextos deterministas, la solución de problemas prácticas de control óptimo bajo incertidumbre en la literatura no está tan desarrollada, y los métodos propuestos en la literatura no son aplicables al problema de interés. La primera contribución de esta tesis hace frente a este reto mediante la introducción de un marco numérico para la resolución de problemas generales de control óptimo no-lineal bajo incertidumbre paramétrica. El núcleo de este método es un esquema de conjunto de trayectorias, en el que las trayectorias del sistema dinámico bajo múltiples escenarios son consideradas de forma simultánea, y el problema de control óptimo bajo incertidumbre es así transformado en un problema convencional que puede ser tratado mediante métodos existentes en control óptimo. A continuación, empleamos este método para resolver el problema de la planificación de vuelo robusta. La incertidumbre en el viento y la probabilidad de ocurrencia de condiciones convectivas son modeladas mediante el uso de previsiones de conjunto o ensemble, compuestas por múltiples predicciones en lugar de una única previsión determinista. Este método puede ser empleado para maximizar la eficiencia esperada de los planes de vuelo de acuerdo a la estructura de costes de la aerolínea; además, la predictibilidad de la trayectoria y la exposición a la convección pueden ser incorporadas como objetivos adicionales. El trade-off entre estos objetivos puede ser evaluado mediante la metodología propuesta. La segunda parte de la tesis presenta una solución para reconducir aviones en escenarios tormentosos en un horizonte táctico. La evolución de las células convectivas es representada con un modelo estocástico basado en las proyecciones de Rapidly Developing Thunderstorms (RDT), un sistema determinista basado en imágenes de satélite. Este modelo es empleado por un método de control óptimo numérico, basado en un modelo de masa puntual en el que se modela la dinámica de viraje, con el objetivo de maximizar la eficiencia y predictibilidad de la trayectoria en presencia de incertidumbre sobre la evolución futura de las tormentas. Finalmente, el proceso de optimizatión es inicializado por un método heurístico aleatorizado que genera múltiples puntos de inicio para las iteraciones del optimizador. Esta combinación permite explorar y explotar el espacio de trayectorias solución para proporcionar al piloto o al controlador un conjunto de trayectorias propuestas, así como información útil sobre su coste y el riesgo asociado. Los métodos propuestos son probados en escenarios de ejemplo basados en datos reales, ilustrando las diferentes opciones disponibles de acuerdo a las prioridades del planificador y demostrando que las soluciones descritas en esta tesis son adecuadas para los problemas que se han formulado. De este modo, es posible enriquecer el proceso de planificación de vuelo para incrementar la eficiencia y predictibilidad de las trayectorias individuales, lo que contribuiría a mejoras en el rendimiento del sistema ATM.These works have been financially supported by Universidad Carlos III de Madrid through a PIF scholarship; by Eurocontrol, through the HALA! Research Network grant 10-220210-C2; by the Spanish Ministry of Economy and Competitiveness (MINECO)'s R&D program, through the OptMet project (TRA2014-58413-C2-2-R); and by the European Commission's SESAR Horizon 2020 program, through the TBO-Met project (grant number 699294).Programa de Doctorado en Mecánica de Fluidos por la Universidad Carlos III de Madrid; la Universidad de Jaén; la Universidad de Zaragoza; la Universidad Nacional de Educación a Distancia; la Universidad Politécnica de Madrid y la Universidad Rovira iPresidente: Damián Rivas Rivas.- Secretario: Xavier Prats Menéndez.- Vocal: Benavar Sridha

Batch Informed Trees (BIT*): Informed Asymptotically Optimal Anytime Search

Path planning in robotics often requires finding high-quality solutions to continuously valued and/or high-dimensional problems. These problems are challenging and most planning algorithms instead solve simplified approximations. Popular approximations include graphs and random samples, as respectively used by informed graph-based searches and anytime sampling-based planners. Informed graph-based searches, such as A*, traditionally use heuristics to search a priori graphs in order of potential solution quality. This makes their search efficient but leaves their performance dependent on the chosen approximation. If its resolution is too low then they may not find a (suitable) solution but if it is too high then they may take a prohibitively long time to do so. Anytime sampling-based planners, such as RRT*, traditionally use random sampling to approximate the problem domain incrementally. This allows them to increase resolution until a suitable solution is found but makes their search dependent on the order of approximation. Arbitrary sequences of random samples approximate the problem domain in every direction simultaneously and but may be prohibitively inefficient at containing a solution. This paper unifies and extends these two approaches to develop Batch Informed Trees (BIT*), an informed, anytime sampling-based planner. BIT* solves continuous path planning problems efficiently by using sampling and heuristics to alternately approximate and search the problem domain. Its search is ordered by potential solution quality, as in A*, and its approximation improves indefinitely with additional computational time, as in RRT*. It is shown analytically to be almost-surely asymptotically optimal and experimentally to outperform existing sampling-based planners, especially on high-dimensional planning problems.Comment: International Journal of Robotics Research (IJRR). 32 Pages. 16 Figure

Semiannual final report, 1 October 1991 - 31 March 1992

A summary of research conducted at the Institute for Computer Applications in Science and Engineering in applied mathematics, numerical analysis, and computer science during the period 1 Oct. 1991 through 31 Mar. 1992 is presented

Trajectory Planning and Subject-Specific Control of a Stroke Rehabilitation Robot using Deep Reinforcement Learning

There are approximately 13 million annual new stroke cases worldwide. Research has shown that robotics can provide practical and efficient solutions for expediting post-stroke patient recovery. Assistive robots provide automatic limb training, which saves a great deal of time and energy. In addition, they facilitate the use of data acquisition devices. The data is beneficial in terms of quantitative evaluation of the patient progress. This research focused on the trajectory planning and subject-specific control of an upper-extremity post-stroke rehabilitation robot. To find the optimal rehabilitation practice, the manipulation trajectory was designed by an optimization-based planner. A linear quadratic regulator (LQR) controller was then applied to stabilize the trajectory. The integrated planner-controller framework was tested in simulation. To validate the simulation results, hardware implementation was conducted, which provided good agreement with simulation. One of the challenges of rehabilitation robotics is the choice of the low-level controller. To find the best candidate for our specific setup, five controllers were evaluated in simulation for circular trajectory tracking. In particular, we compared the performance of LQR, sliding mode control (SMC), and nonlinear model predictive control (NMPC) to conventional proportional integral derivative (PID) and computed-torque PID controllers. The real-time assessment of the mentioned controllers was done by implementing them on the physical hardware for point stabilization and circular trajectory tracking scenarios. Our comparative study confirmed the need for advanced low-level controllers for better performance. Due to complex online optimization of the NMPC and the incorporated delay in the method of implementation, performance degradation was observed with NMPC compared to other advanced controllers. The evaluation showed that SMC and LQR were the two best candidates for the robot. To remove the need for extensive manual controller tuning, a deep reinforcement learning (DRL) tuner framework was designed in MATLAB to provide the optimal weights for the controllers; it permitted the online tuning of the weights, which enabled the subject-specific controller weight adjustment. This tuner was tested in simulation by adding a random noise to the input at each iteration, to simulate the subject. Compared to fixed manually tuned weights, the DRL-tuned controller presented lower position-error. In addition, an easy to implement high-level force controller algorithm was designed by incorporating the subject force data. The resulting hybrid position/force controller was tested with a healthy subject in the loop. The controller was able to provide assist as needed when the subject increased the position-error. Future research might consider model reduction methods for expediting the NMPC optimization, application of the DRL on other controllers and for optimization parameter adjustment, testing other high-level controllers like admittance control, and testing the final controllers with post-stroke patients

A methodology for robust optimization of low-thrust trajectories in multi-body environments

Issued as final reportThales Alenia Spac

Implications of Motion Planning: Optimality and k-survivability

We study motion planning problems, finding trajectories that connect two configurations of a system, from two different perspectives: optimality and survivability. For the problem of finding optimal trajectories, we provide a model in which the existence of optimal trajectories is guaranteed, and design an algorithm to find approximately optimal trajectories for a kinematic planar robot within this model. We also design an algorithm to build data structures to represent the configuration space, supporting optimal trajectory queries for any given pair of configurations in an obstructed environment. We are also interested in planning paths for expendable robots moving in a threat environment. Since robots are expendable, our goal is to ensure a certain number of robots reaching the goal. We consider a new motion planning problem, maximum k-survivability: given two points in a stochastic threat environment, find n paths connecting two given points while maximizing the probability that at least k paths reach the goal. Intuitively, a good solution should be diverse to avoid several paths being blocked simultaneously, and paths should be short so that robots can quickly pass through dangerous areas. Finding sets of paths with maximum k-survivability is NP-hard. We design two algorithms: an algorithm that is guaranteed to find an optimal list of paths, and a set of heuristic methods that finds paths with high k-survivability