Dynamic interpolation for obstacle avoidance on Riemannian manifolds

This work is devoted to studying dynamic interpolation for obstacle avoidance. This is a problem that consists of minimizing a suitable energy functional among a set of admissible curves subject to some interpolation conditions. The given energy functional depends on velocity, covariant acceleration and on artificial potential functions used for avoiding obstacles. We derive first-order necessary conditions for optimality in the proposed problem; that is, given interpolation and boundary conditions we find the set of differential equations describing the evolution of a curve that satisfies the prescribed boundary values, interpolates the given points and is an extremal for the energy functional. We study the problem in different settings including a general one on a Riemannian manifold and a more specific one on a Lie group endowed with a left-invariant metric. We also consider a sub-Riemannian problem. We illustrate the results with examples of rigid bodies, both planar and spatial, and underactuated vehicles including a unicycle and an underactuated unmanned vehicle.Comment: Comments welcom

Fast, Safe, and Propellant-Efficient Spacecraft Planning under Clohessy-Wiltshire-Hill Dynamics

This paper presents a sampling-based motion planning algorithm for real-time and propellant-optimized autonomous spacecraft trajectory generation in near-circular orbits. Specifically, this paper leverages recent algorithmic advances in the field of robot motion planning to the problem of impulsively-actuated, propellant-optimized rendezvous and proximity operations under the Clohessy-Wiltshire-Hill (CWH) dynamics model. The approach calls upon a modified version of the Fast Marching Tree (FMT*) algorithm to grow a set of feasible trajectories over a deterministic, low-dispersion set of sample points covering the free state space. To enforce safety, the tree is only grown over the subset of actively-safe samples, from which there exists a feasible one-burn collision avoidance maneuver that can safely circularize the spacecraft orbit along its coasting arc under a given set of potential thruster failures. Key features of the proposed algorithm include: (i) theoretical guarantees in terms of trajectory safety and performance, (ii) amenability to real-time implementation, and (iii) generality, in the sense that a large class of constraints can be handled directly. As a result, the proposed algorithm offers the potential for widespread application, ranging from on-orbit satellite servicing to orbital debris removal and autonomous inspection missions.Comment: Submitted to the AIAA Journal of Guidance, Control, and Dynamics (JGCD) special issue entitled "Computational Guidance and Control". This submission is the journal version corresponding to the conference manuscript "Real-Time, Propellant-Efficient Spacecraft Planning under Clohessy-Wiltshire-Hill Dynamics" accepted to the 2016 IEEE Aerospace Conference in Big Sky, MT, US

Provably Safe and Robust Drone Routing via Sequential Path Planning: A Case Study in San Francisco and the Bay Area

Provably safe and scalable multi-vehicle path planning is an important and urgent problem due to the expected increase of automation in civilian airspace in the near future. Hamilton-Jacobi (HJ) reachability is an ideal tool for analyzing such safety-critical systems and has been successfully applied to several small-scale problems. However, a direct application of HJ reachability to large scale systems is often intractable because of its exponentially-scaling computation complexity with respect to system dimension, also known as the "curse of dimensionality". To overcome this problem, the sequential path planning (SPP) method, which assigns strict priorities to vehicles, was previously proposed; SPP allows multi-vehicle path planning to be done with a linearly-scaling computation complexity. In this work, we demonstrate the potential of SPP algorithm for large-scale systems. In particular, we simulate large-scale multi-vehicle systems in two different urban environments, a city environment and a multi-city environment, and use the SPP algorithm for trajectory planning. SPP is able to efficiently design collision-free trajectories in both environments despite the presence of disturbances in vehicles' dynamics. To ensure a safe transition of vehicles to their destinations, our method automatically allocates space-time reservations to vehicles while accounting for the magnitude of disturbances such as wind in a provably safe way. Our simulation results show an intuitive multi-lane structure in airspace, where the number of lanes and the distance between the lanes depend on the size of disturbances and other problem parameters.Comment: Submitted to AIAA Journal of Guidance, Control, and Dynamics. arXiv admin note: substantial text overlap with arXiv:1611.0836

Online Sampling in the Parameter Space of a Neural Network for GPU-accelerated Motion Planning of Autonomous Vehicles

This paper proposes online sampling in the parameter space of a neural network for GPU-accelerated motion planning of autonomous vehicles. Neural networks are used as controller parametrization since they can handle nonlinear non-convex systems and their complexity does not scale with prediction horizon length. Network parametrizations are sampled at each sampling time and then held constant throughout the prediction horizon. Controls still vary over the prediction horizon due to varying feature vectors fed to the network. Full-dimensional vehicles are modeled by polytopes. Under the assumption of obstacle point data, and their extrapolation over a prediction horizon under constant velocity assumption, collision avoidance reduces to linear inequality checks. Steering and longitudinal acceleration controls are determined simultaneously. The proposed method is designed for parallelization and therefore well-suited to benefit from continuing advancements in hardware such as GPUs. Characteristics of proposed method are illustrated in 5 numerical simulation experiments including dynamic obstacle avoidance, waypoint tracking requiring alternating forward and reverse driving with maximal steering, and a reverse parking scenario.Comment: 8 pages, 8 figures, 3 tables, conference pape

A Distributed Model Predictive Control Framework for Road-Following Formation Control of Car-like Vehicles (Extended Version)

This work presents a novel framework for the formation control of multiple autonomous ground vehicles in an on-road environment. Unique challenges of this problem lie in 1) the design of collision avoidance strategies with obstacles and with other vehicles in a highly structured environment, 2) dynamic reconfiguration of the formation to handle different task specifications. In this paper, we design a local MPC-based tracking controller for each individual vehicle to follow a reference trajectory while satisfying various constraints (kinematics and dynamics, collision avoidance, \textit{etc.}). The reference trajectory of a vehicle is computed from its leader's trajectory, based on a pre-defined formation tree. We use logic rules to organize the collision avoidance behaviors of member vehicles. Moreover, we propose a methodology to safely reconfigure the formation on-the-fly. The proposed framework has been validated using high-fidelity simulations.Comment: Extended version of the conference paper submission on ICARCV'1

Formal Verification of Obstacle Avoidance and Navigation of Ground Robots

The safety of mobile robots in dynamic environments is predicated on making sure that they do not collide with obstacles. In support of such safety arguments, we analyze and formally verify a series of increasingly powerful safety properties of controllers for avoiding both stationary and moving obstacles: (i) static safety, which ensures that no collisions can happen with stationary obstacles, (ii) passive safety, which ensures that no collisions can happen with stationary or moving obstacles while the robot moves, (iii) the stronger passive friendly safety in which the robot further maintains sufficient maneuvering distance for obstacles to avoid collision as well, and (iv) passive orientation safety, which allows for imperfect sensor coverage of the robot, i. e., the robot is aware that not everything in its environment will be visible. We complement these provably correct safety properties with liveness properties: we prove that provably safe motion is flexible enough to let the robot still navigate waypoints and pass intersections. We use hybrid system models and theorem proving techniques that describe and formally verify the robot's discrete control decisions along with its continuous, physical motion. Moreover, we formally prove that safety can still be guaranteed despite sensor uncertainty and actuator perturbation, and when control choices for more aggressive maneuvers are introduced. Our verification results are generic in the sense that they are not limited to the particular choices of one specific control algorithm but identify conditions that make them simultaneously apply to a broad class of control algorithms

Funnel Libraries for Real-Time Robust Feedback Motion Planning

We consider the problem of generating motion plans for a robot that are guaranteed to succeed despite uncertainty in the environment, parametric model uncertainty, and disturbances. Furthermore, we consider scenarios where these plans must be generated in real-time, because constraints such as obstacles in the environment may not be known until they are perceived (with a noisy sensor) at runtime. Our approach is to pre-compute a library of "funnels" along different maneuvers of the system that the state is guaranteed to remain within (despite bounded disturbances) when the feedback controller corresponding to the maneuver is executed. We leverage powerful computational machinery from convex optimization (sums-of-squares programming in particular) to compute these funnels. The resulting funnel library is then used to sequentially compose motion plans at runtime while ensuring the safety of the robot. A major advantage of the work presented here is that by explicitly taking into account the effect of uncertainty, the robot can evaluate motion plans based on how vulnerable they are to disturbances. We demonstrate and validate our method using extensive hardware experiments on a small fixed-wing airplane avoiding obstacles at high speed (~12 mph), along with thorough simulation experiments of ground vehicle and quadrotor models navigating through cluttered environments. To our knowledge, these demonstrations constitute one of the first examples of provably safe and robust control for robotic systems with complex nonlinear dynamics that need to plan in real-time in environments with complex geometric constraints.Comment: International Journal of Robotics Research (To Appear

Reach-Avoid Problems with Time-Varying Dynamics, Targets and Constraints

We consider a reach-avoid differential game, in which one of the players aims to steer the system into a target set without violating a set of state constraints, while the other player tries to prevent the first from succeeding; the system dynamics, target set, and state constraints may all be time-varying. The analysis of this problem plays an important role in collision avoidance, motion planning and aircraft control, among other applications. Previous methods for computing the guaranteed winning initial conditions and strategies for each player have either required augmenting the state vector to include time, or have been limited to problems with either no state constraints or entirely static targets, constraints and dynamics. To incorporate time-varying dynamics, targets and constraints without the need for state augmentation, we propose a modified Hamilton-Jacobi-Isaacs equation in the form of a double-obstacle variational inequality, and prove that the zero sublevel set of its viscosity solution characterizes the capture basin for the target under the state constraints. Through this formulation, our method can compute the capture basin and winning strategies for time-varying games at no additional computational cost with respect to the time-invariant case. We provide an implementation of this method based on well-known numerical schemes and show its convergence through a simple example; we include a second example in which our method substantially outperforms the state augmentation approach.Comment: Submitted to 18th International Conference on Hybrid Systems: Computation and Control (HSCC 2015) v2: Corrected typos and introduced minor esthetic changes (equation 11 moved to the top of page 4); conclusion slightly shortened for concisenes

On the Construction of Safe Controllable Regions for Affine Systems with Applications to Robotics

This paper studies the problem of constructing in-block controllable (IBC) regions for affine systems. That is, we are concerned with constructing regions in the state space of affine systems such that all the states in the interior of the region are mutually accessible through the region's interior by applying uniformly bounded inputs. We first show that existing results for checking in-block controllability on given polytopic regions cannot be easily extended to address the question of constructing IBC regions. We then explore the geometry of the problem to provide a computationally efficient algorithm for constructing IBC regions. We also prove the soundness of the algorithm. We then use the proposed algorithm to construct safe speed profiles for different robotic systems, including fully-actuated robots, ground robots modeled as unicycles with acceleration limits, and unmanned aerial vehicles (UAVs). Finally, we present several experimental results on UAVs to verify the effectiveness of the proposed algorithm. For instance, we use the proposed algorithm for real-time collision avoidance for UAVs.Comment: 17 pages, 18 figures, under review for publication in Automatic

Distributed Obstacle and Multi-Robot Collision Avoidance in Uncertain Environments

This paper tackles the distributed leader-follower (L-F) control problem for heterogeneous mobile robots in unknown environments requiring obstacle avoidance, inter-robot collision avoidance, and reliable robot communications. To prevent an inter-robot collision, we employ a virtual propulsive force between robots. For obstacle avoidance, we present a novel distributed Negative-Imaginary (NI) variant formation tracking control approach and a dynamic network topology methodology which allows the formation to change its shape and the robot to switch their roles. In the case of communication or sensor loss, a UAV, controlled by a Strictly-Negative-Imaginary (SNI) controller with good wind resistance characteristics, is utilized to track the position of the UGV formation using its camera. Simulations and indoor experiments have been conducted to validate the proposed methods