Interaction-Aware Motion Planning for Autonomous Vehicles with Multi-Modal Obstacle Uncertainty Predictions

This paper proposes an interaction and safety-aware motion-planning method for an autonomous vehicle in uncertain multi-vehicle traffic environments. The method integrates the ability of the interaction-aware interacting multiple model Kalman filter (IAIMM-KF) to predict interactive multi-modal maneuvers of surrounding vehicles, and the advantage of model predictive control (MPC) in planning an optimal trajectory in uncertain dynamic environments. The multi-modal prediction uncertainties, containing both the maneuver and trajectory uncertainties of surrounding vehicles, are considered in computing the reference targets and designing the collision-avoidance constraints of MPC for resilient motion planning of the ego vehicle. The MPC achieves safety awareness by incorporating a tunable parameter to adjust the predicted obstacle occupancy in the design of the safety constraints, allowing the approach to achieve a trade-off between performance and robustness. Based on the prediction of the surrounding vehicles, an optimal reference trajectory of the ego vehicle is computed by MPC to follow the time-varying reference targets and avoid collisions with obstacles. The efficiency of the method is illustrated in challenging highway-driving simulation scenarios and a driving scenario from a recorded traffic dataset.Comment: 15 page

Multiagent Flight Control in Dynamic Environments with Cooperative Coevolutionary Algorithms

Dynamic flight environments in which objectives and environmental features change with respect to time pose a difficult problem with regards to planning optimal flight paths. Path planning methods are typically computationally expensive, and are often difficult to implement in real time if system objectives are changed. This computational problem is compounded when multiple agents are present in the system, as the state and action space grows exponentially. In this work, we use cooperative coevolutionary algorithms in order to develop policies which control agent motion in a dynamic multiagent unmanned aerial system environment such that goals and perceptions change, while ensuring safety constraints are not violated. Rather than replanning new paths when the environment changes, we develop a policy which can map the new environmental features to a trajectory for the agent while ensuring safe and reliable operation, while providing 92% of the theoretically optimal performanc

Graceful Navigation for Mobile Robots in Dynamic and Uncertain Environments.

The ability to navigate in everyday environments is a fundamental and necessary skill for any autonomous mobile agent that is intended to work with human users. The presence of pedestrians and other dynamic objects, however, makes the environment inherently dynamic and uncertain. To navigate in such environments, an agent must reason about the near future and make an optimal decision at each time step so that it can move safely toward the goal. Furthermore, for any application intended to carry passengers, it also must be able to move smoothly and comfortably, and the robot behavior needs to be customizable to match the preference of the individual users. Despite decades of progress in the field of motion planning and control, this remains a difficult challenge with existing methods. In this dissertation, we show that safe, comfortable, and customizable mobile robot navigation in dynamic and uncertain environments can be achieved via stochastic model predictive control. We view the problem of navigation in dynamic and uncertain environments as a continuous decision making process, where an agent with short-term predictive capability reasons about its situation and makes an informed decision at each time step. The problem of robot navigation in dynamic and uncertain environments is formulated as an on-line, finite-horizon policy and trajectory optimization problem under uncertainty. With our formulation, planning and control becomes fully integrated, which allows direct optimization of the performance measure. Furthermore, with our approach the problem becomes easy to solve, which allows our algorithm to run in real time on a single core of a typical laptop with off-the-shelf optimization packages. The work presented in this thesis extends the state-of-the-art in analytic control of mobile robots, sampling-based optimal path planning, and stochastic model predictive control. We believe that our work is a significant step toward safe and reliable autonomous navigation that is acceptable to human users.PhDMechanical EngineeringUniversity of Michigan, Horace H. Rackham School of Graduate Studieshttp://deepblue.lib.umich.edu/bitstream/2027.42/120760/1/jongjinp_1.pd

A Decoupling Principle for Simultaneous Localization and Planning Under Uncertainty in Multi-Agent Dynamic Environments

Simultaneous localization and planning for nonlinear stochastic systems under process and measurement uncertainties is a challenging problem. In its most general form, it is formulated as a stochastic optimal control problem in the space of feedback policies. The Hamilton-Jacobi-Bellman equation provides the theoretical solution of the optimal problem; but, as is typical of almost all nonlinear stochastic systems, optimally solving the problem is intractable. Moreover, even if an optimal solution was obtained, it would require centralized control, while multi-agent mobile robotic systems under dynamic environments require decentralized solutions. In this study, we aim for a theoretically sound solution for various modes of this problem, including the single-agent and multi-agent variations with perfect and imperfect state information, where the underlying state, control and observation spaces are continuous with discrete-time models. We introduce a decoupling principle for planning and control of multi-agent nonlinear stochastic systems based on a small noise asymptotics. Through this decoupling principle, under small noise, the design of the real-time feedback law can be decoupled from the off-line design of the nominal trajectory of the system. Further, for a multi-agent problem, the design of the feedback laws for different agents can be decoupled from each other, reducing the centralized problem to a decentralized problem requiring no communication during execution. The resulting solution is quantifiably near-optimal. We establish this result for all the above-mentioned variations, which results in the following variants: Trajectory-optimized Linear Quadratic Regulator (T-LQR), Multi-agent T-LQR (MT-LQR), Trajectory-optimized Linear Quadratic Gaussian (T-LQG), and Multi-agent T-LQG (MT-LQG). The decoupling principle provides the conditions under which a decentralized linear Gaussian system with a quadratic approximation of the cost, obtained by linearization around an optimally designed nominal trajectory can be utilized to control the nonlinear system. The resulting decentralized feedback solution at runtime, being decoupled with respect to the mobile agents, requires no communication between the agents during the execution phase. Moreover, the complexity of the solution vis-a-vis the computation of the nominal trajectory as well as the closed-loop gains is tractable with low polynomial orders of computation. Experimental implementation of the solution shows that the results hold for moderate levels of noise with high probability. Further optimizing the performance of this approach we show how to design a special cost function for the problem with imperfect state measurement that takes advantage of the fact that the estimation covariance of a linear Gaussian system is deterministic and not dependent on the observations. This design, which corresponds in our overall design to “belief space planning”, incorporates the consequently deterministic cost of the stochastic feedback system into the deterministic design of the nominal trajectory to obtain an optimal nominal trajectory with the best estimation performance. Then, it utilizes the T-LQG approach to design an optimal feedback law to track the designed nominal trajectory. This iterative approach can be used to further tune both the open loop as well as the decentralized feedback gain portions of the overall design. We also provide the multi-agent variant of this approach based on the MT-LQG method. Based on the near-optimality guarantees of the decoupling principle and the TLQG approach, we analyze the performance and correctness of a well-known heuristic in robotic path planning. We show that optimizing measures of the observability Gramian as a surrogate for estimation performance may provide irrelevant or misleading trajectories for planning under observation uncertainty. We then consider systems with non-Gaussian perturbations. An alternative heuristic method is proposed that aims for fast planning in belief space under non- Gaussian uncertainty. We provide a special design approach based on particle filters that results in a convex planning problem implemented via a model predictive control strategy in convex environments, and a locally convex problem in non-convex environments. The environment here refers to the complement of the region in Euclidean space that contains the obstacles or “no fly zones”. For non-convex dynamic environments, where the no-go regions change dynamically with time, we design a special form of an obstacle penalty function that incorporates non-convex time-varying constraints into the cost function, so that the decoupling principle still applies to these problems. However, similar to any constrained problem, the quality of the optimal nominal trajectory is dependent on the quality of the solution obtainable for the nonlinear optimization problem. We simulate our algorithms for each of the problems on various challenging situations, including for several nonlinear robotic models and common measurement models. In particular, we consider 2D and 3D dynamic environments for heterogeneous holonomic and non-holonomic robots, and range and bearing sensing models. Future research can potentially extend the results to more general situations including continuous-time models

A Decoupling Principle for Simultaneous Localization and Planning Under Uncertainty in Multi-Agent Dynamic Environments

Simultaneous localization and planning for nonlinear stochastic systems under process and measurement uncertainties is a challenging problem. In its most general form, it is formulated as a stochastic optimal control problem in the space of feedback policies. The Hamilton-Jacobi-Bellman equation provides the theoretical solution of the optimal problem; but, as is typical of almost all nonlinear stochastic systems, optimally solving the problem is intractable. Moreover, even if an optimal solution was obtained, it would require centralized control, while multi-agent mobile robotic systems under dynamic environments require decentralized solutions. In this study, we aim for a theoretically sound solution for various modes of this problem, including the single-agent and multi-agent variations with perfect and imperfect state information, where the underlying state, control and observation spaces are continuous with discrete-time models. We introduce a decoupling principle for planning and control of multi-agent nonlinear stochastic systems based on a small noise asymptotics. Through this decoupling principle, under small noise, the design of the real-time feedback law can be decoupled from the off-line design of the nominal trajectory of the system. Further, for a multi-agent problem, the design of the feedback laws for different agents can be decoupled from each other, reducing the centralized problem to a decentralized problem requiring no communication during execution. The resulting solution is quantifiably near-optimal. We establish this result for all the above-mentioned variations, which results in the following variants: Trajectory-optimized Linear Quadratic Regulator (T-LQR), Multi-agent T-LQR (MT-LQR), Trajectory-optimized Linear Quadratic Gaussian (T-LQG), and Multi-agent T-LQG (MT-LQG). The decoupling principle provides the conditions under which a decentralized linear Gaussian system with a quadratic approximation of the cost, obtained by linearization around an optimally designed nominal trajectory can be utilized to control the nonlinear system. The resulting decentralized feedback solution at runtime, being decoupled with respect to the mobile agents, requires no communication between the agents during the execution phase. Moreover, the complexity of the solution vis-a-vis the computation of the nominal trajectory as well as the closed-loop gains is tractable with low polynomial orders of computation. Experimental implementation of the solution shows that the results hold for moderate levels of noise with high probability. Further optimizing the performance of this approach we show how to design a special cost function for the problem with imperfect state measurement that takes advantage of the fact that the estimation covariance of a linear Gaussian system is deterministic and not dependent on the observations. This design, which corresponds in our overall design to “belief space planning”, incorporates the consequently deterministic cost of the stochastic feedback system into the deterministic design of the nominal trajectory to obtain an optimal nominal trajectory with the best estimation performance. Then, it utilizes the T-LQG approach to design an optimal feedback law to track the designed nominal trajectory. This iterative approach can be used to further tune both the open loop as well as the decentralized feedback gain portions of the overall design. We also provide the multi-agent variant of this approach based on the MT-LQG method. Based on the near-optimality guarantees of the decoupling principle and the TLQG approach, we analyze the performance and correctness of a well-known heuristic in robotic path planning. We show that optimizing measures of the observability Gramian as a surrogate for estimation performance may provide irrelevant or misleading trajectories for planning under observation uncertainty. We then consider systems with non-Gaussian perturbations. An alternative heuristic method is proposed that aims for fast planning in belief space under non- Gaussian uncertainty. We provide a special design approach based on particle filters that results in a convex planning problem implemented via a model predictive control strategy in convex environments, and a locally convex problem in non-convex environments. The environment here refers to the complement of the region in Euclidean space that contains the obstacles or “no fly zones”. For non-convex dynamic environments, where the no-go regions change dynamically with time, we design a special form of an obstacle penalty function that incorporates non-convex time-varying constraints into the cost function, so that the decoupling principle still applies to these problems. However, similar to any constrained problem, the quality of the optimal nominal trajectory is dependent on the quality of the solution obtainable for the nonlinear optimization problem. We simulate our algorithms for each of the problems on various challenging situations, including for several nonlinear robotic models and common measurement models. In particular, we consider 2D and 3D dynamic environments for heterogeneous holonomic and non-holonomic robots, and range and bearing sensing models. Future research can potentially extend the results to more general situations including continuous-time models

Intelligent Hybrid Approach for Multi Robots- Multi Objectives Motion Planning Optimization

Abstract: This paper proposes enhanced approach to find multi objective optimization and obstacle avoidance of motion planning problem for multi mobile robots that have to move smoothly, safely with a shorter time and minimum distance along curvature-constrained motion planning in completely known dynamic environments. The research includes two stages: the first stage is to find an multi objective optimal path and trajectory planning for each robot individually using the Enhanced GA with modified A*. The second stage consists of designing a fuzzy logic to control the movement of the robots with collision free. The global optimal trajectory is fed to fuzzy motion controller which has ability to regenerate the local trajectory of the robot based on the probability of having another dynamic robot in the area. A simulation of the strategy has been presented and the results show that the proposed approach is able to achieve multi objective optimization of motion planning for multi mobile robot in dynamic environment efficiently. Also, it has the ability to find a solution when the environment is complex and the number of obstacles is increasing. The performance of the above mentioned approach has been found to be satisfactory of dynamic obstacle avoidance

Voronoi-based trajectory optimization for UGV path planning

© 2017 IEEE. Optimal path planning in dynamic environments for an unmanned vehicle is a complex task of mobile robotics that requires an integrated approach. This paper describes a path planning algorithm, which allows to build a preliminary motion trajectory using global information about environment, and then dynamically adjust the path in real-time by varying objective function weights. We introduce a set of key parameters for path optimization and the algorithm implementation in MATLAB. The developed algorithm is suitable for fast and robust trajectory tuning to a dynamically changing environment and is capable to provide efficient planning for mobile robots

Realtime Motion Planning for Manipulator Robots under Dynamic Environments: An Optimal Control Approach

This report presents optimal control methods integrated with hierarchical control framework to realize real-time collision-free optimal trajectories for motion control in kinematic chain manipulator (KCM) robot systems under dynamic environments. Recently, they have been increasingly used in applications where manipulators are required to interact with random objects and humans. As a result, more complex trajectory planning schemes are required. The main objective of this research is to develop new motion control strategies that can enable such robots to operate efficiently and optimally in such unknown and dynamic environments. Two direct optimal control methods: The direct collocation method and discrete mechanics for optimal control methods are investigated for solving the related constrained optimal control problem and the results are compared. Using the receding horizon control structure, open-loop sub-optimal trajectories are generated as real-time input to the controller as opposed to the predefined trajectory over the entire time duration. This, in essence, captures the dynamic nature of the obstacles. The closed-loop position controller is then engaged to span the robot end-effector along this desired optimal path by computing appropriate torque commands for the joint actuators. Employing a two-degree of freedom technique, collision-free trajectories and robot environment information are transmitted in real-time by the aid of a bidirectional connectionless datagram transfer. A hierarchical network control platform is designed to condition triggering of precedent activities between a dedicated machine computing the optimal trajectory and the real-time computer running a low-level controller. Experimental results on a 2-link planar robot are presented to validate the main ideas. Real-time implementation of collision-free workspace trajectory control is achieved for cases where obstacles are arbitrarily changing in the robot workspace

Online Trajectory Optimization Using Inexact Gradient Feedback for Time-Varying Environments

This paper considers the problem of online trajectory design under time-varying environments. We formulate the general trajectory optimization problem within the framework of time-varying constrained convex optimization and proposed a novel version of the online gradient ascent algorithm for such problems. Moreover, the gradient feedback is noisy and allows us to use the proposed algorithm for a range of practical applications where it is difficult to acquire the true gradient. In contrast to the most available literature, we present the offline sublinear regret of the proposed algorithm up to the path length variations of the optimal offline solution, the cumulative gradient, and the error in the gradient variations. Furthermore, we establish a lower bound on the offline dynamic regret, which defines the optimality of any trajectory. To show the efficacy of the proposed algorithm, we consider two practical problems of interest. First, we consider a device to device (D2D) communications setting, and the goal is to design a user trajectory while maximizing its connectivity to the internet. The second problem is associated with the online planning of energy-efficient trajectories for unmanned surface vehicles (USV) under strong disturbances in ocean environments with both static and dynamic goal locations. The detailed simulation results demonstrate the significance of the proposed algorithm on synthetic and real data sets. Video on the real-world datasets can be found at {https://www.youtube.com/watch?v=FcRqqWtpf\_0}Comment: arXiv admin note: text overlap with arXiv:1804.0486