# Research

## Active Research Projects

#### Generalized Reciprocal Collision Avoidance(pdf)

Daman Bareiss, Jur van den Berg

*Abstract:*
Reciprocal collision avoidance has become a popular area of research over recent years. Many approaches have been developed for a variety of dynamic systems ranging from single integrators to arbitrary linear dynamics. There are extensions to more complicated dynamics such as car-like and differential-drive robots. In this paper, we present two contributions. First, we provide a unification of these previous approaches under a single generalized representation using Control-Obstacles. In particular, we present examples for Velocity Obstacles, Acceleration Velocity Obstacles, Continuous Control Obstacles, and LQR-Obstacles. Secondly, we present an extension of Control-Obstacles to non-linear, non-homogeneous cases where the robots may have different state spaces and dynamics. This is in contrast to previous approaches to reciprocal collision avoidance, which use a relative dynamics formulation and can therefore only apply be applied to homogeneous, linear systems. Our approach allows for robots to independently select a new control input while avoiding collision with other robots, assuming the robots can observe the state of the other robots and that all robots have the same "type" of control input. These assumptions are reasonable for robots that use a controller to abstract to higher-level control inputs such as desired velocity which can be observed with a Kalman Filter, for example. We implemented our approach for numerous simulated mobile robots: differential-drive, differential-drive with a trailer, car-like, and hovercrafts. We also performed physical experiments with differential-drive, differential-drive with a trailer, and car-like robots. Our results show that our approach is capable of letting a group of robots with non-linear, non-homogeneous dynamics safely avoid collisions with other robots at real-time computation rates.

#### Online Parameter Estimation via Real-Time Replanning of Continuous Gaussian POMDPs (pdf)

Dustin J. Webb, Kyle L. Crandall, Jur van den Berg

*Abstract:* An accurate dynamics model of a robot is an important ingredient of many algorithms used to solve robotics problems, including motion planning, control, localization, and mapping. Models derived from first principles often contain parameters (e.g. mass, moment of inertia, arm lengths, etc.) for which values are unknown. Those which cannot be easily measured must be estimated from the observed behavior of t he robot. A good approach to address this problem is to plan control policies for the robot that elicit maximal amounts of information about the parameters of the system, while still achieving other objectives specified for the robot. In case of parameters subject to drift, this must be done continuously over the lifetime of the robot if costly re-calibrations are to be avoided. In this paper, we introduce a new method that formulates the parameter estimation problem as a continuous partially-observable Markov decision process (POMDP), which plans control policies that optimally trade-off the effort spent on learning parameters and effort spent on achieving regular robot objectives (exploration vs. exploitation), and allow for online, continual parameter estimation. While POMDPs have, until recently, been mostly of theoretical interest due to their inherent complexity, we build on recent advances that allow continuous, Gaussian POMDPs to be approximately-optimally solved in near-real-time rates. We show that the computed control policies lead to improved convergence of the belief of the parameters compared to system identification approaches based on applying random controls.

#### Efficient Exact Collision-Checking of 3-D Rigit Body Motions using Linear Transformations and Distance Computations in Worksapce(pdf)

Liang He, Jur van den Berg

*Abstract:*This paper presents a new method for efficient and exact collision-checking of linear motions of 3-D rigid bodies. 3-D rigid bodies have 6-D configuration spaces (three degrees of freedom for position and three for orientation), and constitute an important subclass of motion planning problems. Our method can be used with any collision-checker that is capable of performing linear transformations and distance computations on 3-D geometry. As previous work has shown, computing the distance between the rigid body in some configuration and the workspace obstacles immediately determines the collision-status of surrounding configurations. Using a recursive procedure one can then determine exactly whether an entire motion of the rigid body is collision-free. In this paper, we will show that by performing an optimally selected linear transformation on the workspace, the collision-status of rigid body motions can be determined using significantly fewer (costly) distance computations. Since collision-checking is often the computational bottleneck in sampling-based motion planning, our approach allows for significant performance improvements of algorithms such as PRM and RRT when planning for 3-D rigid bodies. We demonstrate the benefit of our approach when used in combination with RRT to construct a planning tree in an illustrative benchmark motion planning scenario.

#### Automatic Collision Avoidance for Manually Tele-operated Unmanned Aerial Vehicles (pdf)

Jason Israelsen, Matt Beall, Daman Bareiss, Dan Stuart, Eric Keeney, Jur van den Berg

*Abstract:* In this paper we present an approach that aids the human operator of unmanned aerial vehicles by automatically performing collision avoidance with obstacles in the environment so that the operator can focus on the global direction of motion of the vehicle. As opposed to systems that override operator control as a last resort in order to avoid collisions (such as those found in modern automobiles), our approach is designed such that the operator can rely on the automatic collision avoidance, enabling intuitive and safe operator control of vehicles that may otherwise be difficult to control. Our approach continually extrapolates the future flight path of the vehicle given the current operator control input. If an imminent collision is predicted our algorithm will override the operator's control input with the nearest control input that will actually let the vehicle avoid collisions with obstacles. This ensures safe flight while simultaneously maintaining the intent of the human operator as closely as possible. We successfully implemented our approach on a physical quadrotor system in a laboratory environment. In all experiments the human operator failed to crash the vehicle into floors, walls, ceilings, or obstacles, even when deliberately attempting to do so.

#### 3-D Reciprocal Collision Avoidance on Physical Quadrotor Helicopters with On-Board Sensing for Relative Positioning (pdf)

Parker Conroy, Daman Bareiss, Matt Beall, Jur van den Berg

*Abstract:* In this paper, we present an implementation of 3-D reciprocal collision avoidance on real quadrotor helicopters where each quadrotor senses the relative position and velocity of other quadrotors using an on-board camera. We show that using our approach, quadrotors are able to successfully avoid pairwise collisions GPS and motion-capture denied environments, without communication between the quadrotors, and even when human operators deliberately attempt to induce collision. To our knowledge, this is the first time that reciprocal collision avoidance has been successfully implemented on real robots where each agent independently observes the others using on-board sensors. We theoretically analyze the response of the collision-avoidance algorithm to the violated assumptions by the use of real robots. We quantitatively analyze our experimental results. A particularly striking observation is that at times the quadrotors exhibit "reciprocal dance" behavior, which is also observed when humans move past each other in constrained environments. This seems to be the result of sensing uncertainty, which causes both robots involved to have a different belief about the relative positions and velocities and, as a result, choose the same side on which to pass.

#### Extended LQR: Locally-Optimal Feedback Control for Systems with Non-Linear Dynamics and Non-Quadratic Cost(pdf)

Jur van den Berg

*Abstract:* We present Extended LQR, a novel approach to locally-optimal control for robots with non-linear dynamics and non-quadratic cost functions. Our formulation is conceptually different from existing approaches, and is based on the novel concept of LQR-smoothing (analogous to Kalman smoothing). Our approach iteratively performs both a backward Extended LQR pass, which computes approximate *cost-to-go* functions, and a forward Extended LQR pass, which computes approximate *cost-so-far* functions. The states at which the sum of these functions is minimal provide an approximately optimal sequence of states for the control problem, and we use these points to linearize the dynamics and quadratize the cost functions in the subsequent iteration. Our results indicate that Extended LQR converges quickly and reliably to a locally-optimal solution of the non-linear, non-quadratic optimal control problem. In addition, we show that our approach is easily extended to include *temporal optimization*, in which the duration of a trajectory is optimized as part of the control problem. We demonstrate the potential of our approach on two illustrative non-linear control problems involving simulated and physical differential-drive robots and simulated quadrotor helicopters in environments with obstacles.

#### Kinodynamic RRT*: Asymptotically Optimal Motion Planning for Robots with Linear Dynamics (pdf)

Dustin Webb, Jur van den Berg

*Abstract:* We present Kinodynamic RRT*, an incremental sampling-based approach for asymptotically optimal motion planning for robots with linear differential constraints. Our approach extends RRT*, which was introduced for holonomic robots [8], by using a fixed-final-state-free-final-time controller that exactly and optimally connects any pair of states, where the cost function is expressed as a trade-off between the duration of a trajectory and the expended control effort. Our approach generalizes earlier work on extending RRT* to kinodynamic sys- tems, as it guarantees asymptotic optimality for any system with controllable linear dynamics, in state spaces of any dimension. Our approach can be applied to non-linear dynamics as well by using their first-order Taylor approximations. In addition, we show that for the rich subclass of systems with a nilpotent dynamics matrix, closed-form solutions for optimal trajectories can be derived, which keeps the computational overhead of our algorithm compared to traditional RRT* at a minimum. We demonstrate the potential of our approach by computing asymptotically optimal trajectories in three challenging motion planning scenarios: (i) a planar robot with a 4-D state space and double integrator dynamics, (ii) an aerial vehicle with a 10-D state space and linearized quadrotor dynamics, and (iii) a car- like robot with a 5-D state space and non-linear dynamics.

#### Reciprocal Collision Avoidance for Robots with Linear Dynamics using LQR-Obstacles (pdf)

Daman Bareiss, Jur van den Berg

*Abstract:* In this paper we present a formal approach to reciprocal collision avoidance for multiple mobile robots sharing a common 2-D or 3-D workspace whose dynamics are subject to linear differential constraints. Our approach defines a protocol for robots to select their control input independently (i.e. without coordination with other robots) while guaranteeing collision-free motion for all robots, assuming the robots can perfectly observe each other's state. To this end, we use the concept of LQR-Obstacles (a generalization of Velocity Obstacles to robots with dynamics) that define sets of forbidden control inputs that lead a robot to collision with obstacles, and extend it for reciprocal collision avoidance among multiple robots. We implemented and tested our approach in 3-D simulation environments for reciprocal collision avoidance of quadrotor helicopters, which have complex dynamics in 16-D state spaces. Our results suggest that our approach avoids collisions among over a hundred quadrotors in tight workspaces at real-time computation rates.

#### Meso-Scale Planning for Multi-Agent Navigation (pdf)

Liang He, Jur van den Berg

*Abstract:* We introduce a new concept; *meso-scale* planning in real-time multi-agent navigation. Whereas many traditional approaches to multi-agent navigation typically consist of two-levels -- a macro-scale level providing agents with a global direction of motion around (large) static obstacles, and a micro-scale level in which agents seek to avoid collision with other agents -- our approach adds a meso-scale level to give agents realistic behavior in scenarios where groups of other agents (e.g. families or crowds in a virtual world) form coherent entities. Rather than moving straight through such groups, our approach lets agents move around them. Our formulation considers each agent as an individual that may perceive sets of other agents as a group, and plans its motion accordingly. We base our approach on the velocity obstacle concept, and we show using simulation results that our method dramatically improves the quality of the trajectories computed for the agents.