Safety is a core challenge of autonomous robot motion planning, especially in the presence of dynamic and uncertain obstacles. Many recent results use learning and deep learning-based motion planners and prediction modules to predict multiple possible obstacle trajectories and generate obstacle-aware ego robot plans. However, planners that ignore the inherent uncertainties in such predictions incur collision risks and lack formal safety guarantees. In this paper, we present a computationally efficient safety filtering solution to reduce the collision risk of ego robot motion plans using multiple samples of obstacle trajectory predictions. The proposed approach reformulates the collision avoidance problem by computing safe halfspaces based on obstacle sample trajectories using distributionally robust optimization (DRO) techniques. The safe halfspaces are used in a model predictive control (MPC)-like safety filter to apply corrections to the reference ego trajectory thereby promoting safer planning. The efficacy and computational efficiency of our approach are demonstrated through numerical simulations.

We introduce a new trajectory optimization method for robotic grasping based on a point-cloud representation of robots and task spaces. In our method, robots are represented by 3D points on their link surfaces. The task space of a robot is represented by a point cloud that can be obtained from depth sensors. Using the point-cloud representation, goal reaching in grasping can be formulated as point matching, while collision avoidance can be efficiently achieved by querying the signed distance values of the robot points in the signed distance field of the scene points. Consequently, a constrained nonlinear optimization problem is formulated to solve the joint motion and grasp planning problem. The advantage of our method is that the point-cloud representation is general to be used with any robot in any environment. We demonstrate the effectiveness of our method by performing experiments on a tabletop scene and a shelf scene for grasping with a Fetch mobile manipulator and a Franka Panda arm.1

Robust autonomy stacks require tight integration of perception, motion planning, and control layers, but these layers often inadequately incorporate inherent perception and prediction uncertainties, either ignoring them altogether or making questionable assumptions of Gaussianity. Robots with nonlinear dynamics and complex sensing modalities operating in an uncertain environment demand more careful consideration of how uncertainties propagate across stack layers. We propose a framework to integrate perception, motion planning, and control by explicitly incorporating perception and prediction uncertainties into planning so that risks of constraint violation can be mitigated. Specifically, we use a nonlinear model predictive control based steering law coupled with a decorrelation scheme based Unscented Kalman Filter for state and environment estimation to propagate the robot state and environment uncertainties. Subsequently, we use distributionally robust risk constraints to limit the risk in the presence of these uncertainties. Finally, we present a layered autonomy stack consisting of a nonlinear steering-based distributionally robust motion planning module and a reference trajectory tracking module. Our numerical experiments with nonlinear robot models and an urban driving simulator show the effectiveness of our proposed approaches.

Despite their success, policy gradient methods suffer from high variance of the gradient estimator, which can result in unsatisfactory sample complexity. Recently, numerous variance-reduced extensions of policy gradient methods with provably better sample complexity and competitive numerical performance have been proposed. After a compact survey on some of the main variance-reduced REINFORCE-type methods, we propose ProbAbilistic Gradient Estimation for Policy Gradient (PAGE-PG), a novel loopless variance-reduced policy gradient method based on a probabilistic switch between two types of update. Our method is inspired by the PAGE estimator for supervised learning and leverages importance sampling to obtain an unbiased gradient estimator. We show that PAGE-PG enjoys a $\mathcal{O}\left( \epsilon^{-3} \right)$ average sample complexity to reach an $\epsilon$-stationary solution, which matches the sample complexity of its most competitive counterparts under the same setting. A numerical evaluation confirms the competitive performance of our method on classical control tasks.

With advances in image processing and machine learning, it is now feasible to incorporate semantic information into the problem of simultaneous localisation and mapping (SLAM). Previously, SLAM was carried out using lower level geometric features (points, lines, and planes) which are often view-point dependent and error prone in visually repetitive environments. Semantic information can improve the ability to recognise previously visited locations, as well as maintain sparser maps for long term SLAM applications. However, SLAM in repetitive environments has the critical problem of assigning measurements to the landmarks which generated them. In this paper, we use k-best assignment enumeration to compute marginal assignment probabilities for each measurement landmark pair, in real time. We present numerical studies on the KITTI dataset to demonstrate the effectiveness and speed of the proposed framework.

We propose a two-phase risk-averse architecture for controlling stochastic nonlinear robotic systems. We present Risk-Averse Nonlinear Steering RRT* (RANS-RRT*) as an RRT* variant that incorporates nonlinear dynamics by solving a nonlinear program (NLP) and accounts for risk by approximating the state distribution and performing a distributionally robust (DR) collision check to promote safe planning. The generated plan is used as a reference for a low-level tracking controller. We demonstrate three controllers: finite horizon linear quadratic regulator (LQR) with linearized dynamics around the reference trajectory, LQR with robustness-promoting multiplicative noise terms, and a nonlinear model predictive control law (NMPC). We demonstrate the effectiveness of our algorithm using unicycle dynamics under heavy-tailed Laplace process noise in a cluttered environment.

In this article, we present a distributed formation control strategy for agents with a variety of dynamics to achieve a desired planar formation. Our approach is based on the barycentric-coordinate-based (BCB) control, which is fully distributed, does not require interagent communication or a common sense of orientation, and can be implemented using relative position measurements acquired by agents in their local coordinate frames. This removes the need for global positioning or alignment of local coordinate frames, which are required across several existing strategies. We show how the BCB control for agents with the simplest dynamical model, i.e., the single-integrator dynamics, can be extended to agents with higher order dynamics such as quadrotors, and nonholonomic agents such as unicycles and cars. Specifically, our extension preserves the desired convergence and robustness guarantees of the BCB approach and is provably robust to saturations in the input and unmodeled linear actuator dynamics for unicycle and car agents. We further show that under our proposed BCB control design, the agents can move along a rotated and scaled control direction without affecting the convergence to the desired formation. This observation is used to design a fully distributed collision avoidance strategy, which is often not considered in the formation control literature. We demonstrate the proposed approach in simulations and further present a distributed robotic platform to test the strategy experimentally. Our experimental platform consists of off-the-shelf equipment that can be used to test and validate other multiagent algorithms. The code and implementation instructions for this platform are available online.

We present a distributed control strategy for a team of agents to autonomously achieve a desired 3D formation. Our approach is based on local relative position measurements and can be applied to multirotor aerial vehicles. We assume that agents have a common sense of direction, which is used to align the z-axes of their local coordinate frames. However, this assumption is not crucial, and our approach is provably robust to misalignments in the local coordinate frames or measurement inaccuracies. In particular, agents can move along any direction that projects positively onto the desired direction of motion. This property is exploited to design a fully-distributed collision avoidance strategy. We validate the proposed approach experimentally and show that a team of quadrotors can achieve a desired 3D formation without collisions.

Multistage decision policies provide useful control strategies in high-dimensional state spaces, particularly in complex control tasks. However, they exhibit weak performance guarantees in the presence of disturbance, model mismatch, or model uncertainties. This brittleness limits their use in high-risk scenarios. We present how to quantify the sensitivity of such policies in order to inform of their robustness capacity. We also propose a minimax iterative dynamic game framework for designing robust policies in the presence of disturbance/uncertainties. We test the quantification hypothesis on a carefully designed deep neural network policy; we then pose a minimax iterative dynamic game (iDG) framework for improving policy robustness in the presence of adversarial disturbances. We evaluate our iDG framework on a mecanum-wheeled robot, whose goal is to find a ocally robust optimal multistage policy that achieve a given goal-reaching task. The algorithm is simple and adaptable for designing meta-learning/deep policies that are robust against disturbances, model mismatch, or model uncertainties, up to a disturbance bound. Videos of the results are on the author's website: https://goo.gl/JhshTB, while the codes for reproducing our experiments are on github: https://goo.gl/3G2VBy. A self-contained environment for reproducing our results is on docker: https://goo.gl/Bo7MBe.

We propose a distributionally robust incremental sampling-based method for kinodynamic motion planning under uncertainty, which we call distributionally robust RRT (DR-RRT). In contrast to many approaches that assume Gaussian distributions for uncertain parameters, here we consider moment-based ambiguity sets of distributions with given mean and covariance. Chance constraints for obstacle avoidance and internal state bounds are then enforced under the worst-case distribution in the ambiguity set, which gives a coherent assessment of constraint violation risks. The method generates risk-bounded trajectories and feedback control laws for robots operating in dynamic, cluttered, and uncertain environments, explicitly incorporating localization error, stochastic process disturbances, unpredictable obstacle motion, and uncertain obstacle location. We show that the algorithm is probabilistically complete under mild assumptions. Numerical experiments illustrate the effectiveness of the algorithm.