Scalarizing Multi-Objective Robot Planning Problems using Weighted Maximization

When designing a motion planner for autonomous robots there are usually multiple objectives to be considered. However, a cost function that yields the desired trade-off between objectives is not easily obtainable. A common technique across many applications is to use a weighted sum of relevant objective functions and then carefully adapt the weights. However, this approach may not find all relevant trade-offs even in simple planning problems. Thus, we study an alternative method based on a weighted maximum of objectives. Such a cost function is more expressive than the weighted sum, and we show how it can be deployed in both continuous- and discrete-space motion planning problems. We propose a novel path planning algorithm for the proposed cost function and establish its correctness, and present heuristic adaptations that yield a practical runtime. In extensive simulation experiments, we demonstrate that the proposed cost function and algorithm are able to find a wider range of trade-offs between objectives (i.e., Pareto-optimal solutions) for various planning problems, showcasing its advantages in practice

High-Dimensional Lattice Planning with Optimal Motion Primitives

Lattice-based planning techniques simplify the motion planning problem for autonomous vehicles by limiting available motions to a pre-computed set of primitives. These primitives are then combined online to generate more complex maneuvers. A set of motion primitives t-span a lattice if, given a real number t at least 1, any configuration in the lattice can be reached via a sequence of motion primitives whose cost is no more than a factor of t from optimal. Computing a minimal t-spanning set balances a trade-off between computed motion quality and motion planning performance. In this work, we formulate this problem for an arbitrary lattice as a mixed integer linear program. We also propose an A*-based algorithm to solve the motion planning problem using these primitives. Finally, we present an algorithm that removes the excessive oscillations from planned motions -- a common problem in lattice-based planning. Our method is validated for autonomous driving in both parking lot and highway scenarios.Comment: 12 pages, 9 figures, 2 tables, to be submitted to IEEE Transactions on Intelligent Transportation System

Comparative evaluation of approaches in T.4.1-4.3 and working definition of adaptive module

The goal of this deliverable is two-fold: (1) to present and compare different approaches towards learning and encoding movements us- ing dynamical systems that have been developed by the AMARSi partners (in the past during the first 6 months of the project), and (2) to analyze their suitability to be used as adaptive modules, i.e. as building blocks for the complete architecture that will be devel- oped in the project. The document presents a total of eight approaches, in two groups: modules for discrete movements (i.e. with a clear goal where the movement stops) and for rhythmic movements (i.e. which exhibit periodicity). The basic formulation of each approach is presented together with some illustrative simulation results. Key character- istics such as the type of dynamical behavior, learning algorithm, generalization properties, stability analysis are then discussed for each approach. We then make a comparative analysis of the different approaches by comparing these characteristics and discussing their suitability for the AMARSi project

Spatio-temporal Motion Planning for Autonomous Vehicles with Trapezoidal Prism Corridors and B\'{e}zier Curves

Safety-guaranteed motion planning is critical for self-driving cars to generate collision-free trajectories. A layered motion planning approach with decoupled path and speed planning is widely used for this purpose. This approach is prone to be suboptimal in the presence of dynamic obstacles. Spatial-temporal approaches deal with path planning and speed planning simultaneously; however, the existing methods only support simple-shaped corridors like cuboids, which restrict the search space for optimization in complex scenarios. We propose to use trapezoidal prism-shaped corridors for optimization, which significantly enlarges the solution space compared to the existing cuboidal corridors-based method. Finally, a piecewise B\'{e}zier curve optimization is conducted in our proposed corridors. This formulation theoretically guarantees the safety of the continuous-time trajectory. We validate the efficiency and effectiveness of the proposed approach in numerical and CommonRoad simulations.Comment: Under Review at ACC 202

Statistically Distinct Plans for Multi-Objective Task Assignment

We study the problem of finding statistically distinct plans for stochastic planning and task assignment problems such as online multi-robot pickup and delivery (MRPD) when facing multiple competing objectives. In many real-world settings robot fleets do not only need to fulfil delivery requests, but also have to consider auxiliary objectives such as energy efficiency or avoiding human-centered work spaces. We pose MRPD as a multi-objective optimization problem where the goal is to find MRPD policies that yield different trade-offs between given objectives. There are two main challenges: 1) MRPD is computationally hard, which limits the number of trade-offs that can reasonably be computed, and 2) due to the random task arrivals, one needs to consider statistical variance of the objective values in addition to the average. We present an adaptive sampling algorithm that finds a set of policies which i) are approximately optimal, ii) approximate the set of all optimal solutions, and iii) are statistically distinguishable. We prove completeness and adapt a state-of-the-art MRPD solver to the multi-objective setting for three example objectives. In a series of simulation experiments we demonstrate the advantages of the proposed method compared to baseline approaches and show its robustness in a sensitivity analysis. The approach is general and could be adapted to other multi-objective task assignment and planning problems under uncertainty

Efficient Min-cost Flow Tracking with Bounded Memory and Computation

This thesis is a contribution to solving multi-target tracking in an optimal fashion for real-time demanding computer vision applications. We introduce a challenging benchmark, recorded with our autonomous driving platform AnnieWAY. Three main challenges of tracking are addressed: Solving the data association (min-cost flow) problem faster than standard solvers, extending this approach to an online setting, and making it real-time capable by a tight approximation of the optimal solution

Lattice-Based Motion Planning with Optimal Motion Primitives

In the field of navigation for autonomous vehicles, it is the responsibility of a local planner to compute reference trajectories that are then be followed by a tracking controller. These trajectories should be safe, kinematically feasible, and optimize certain desirable features like low travel time and smoothness/comfort. Determining such trajectories is known as the motion planning problem and is the focus of this work. In general, the motion planning problem is intractable, and simplifications must be made in order to compute reference trajectories quickly and in real time. A common strategy involves adopting a simple kinematic model for the trajectory. However, overly simplified models can lead to references that are infeasible for the vehicle. These are hard for a tracking controller to follow resulting in large tracking error and frequent re-planning. In contrast, lattice-based motion planning simplifies the motion planning problem by restricting the set of allowable motions. In detail, lattice-based motion planning works by discretizing the configuration space of a vehicle into a regularly repeating grid called a lattice. The set of all optimal feasible trajectories between vertices of this lattice are pre-computed and a subset called a control set is selected. Trajectories of this pre-computed subset are then joined together online to form more complex compound maneuvers. Because trajectories between lattice vertices are pre-computed, the complexities of the motion planning problem are considered offline. While not every trajectory is available to a lattice-based planner, every trajectory that is available is feasible and optimal. Selecting a control set is an important step in lattice-based motion planning since the optimality of each element of the control set does not guarantee the optimality of compound maneuvers. These control sets are often selected based on intuition and experience. Broadly, the size of a control set has a positive effect on the quality of computed trajectories, but at the expense of run time performance. A control set is said to t-span a lattice if trajectories between lattice vertices can be approximated to within a factor of t as compound maneuvers of elements of the control set. Given an acceptable allowance t on the sub-optimality of compound maneuvers in a lattice, the problem of computing the smallest control set that t-spans the lattice is called the minimum t-spanning control set problem. In essence, this problem seeks to optimize a trade-off between the quality of compound maneuvers and the time required to compute them. This work details solutions and applications of the minimum t-spanning control set problem in autonomous vehicle navigation. In particular, we first investigate an instance of the problem that can be solved efficiently, provide an intuitive solution, and outline the applications of this instance in the field of any-angle path planning in a two dimensional environment. Next, we provide a novel method to compute trajectories that optimize an adjustable trade-off between certain desirable features. The relative importance of each of these features may differ by user, and the techniques developed here are able to reflect these preferences. The NP-completeness of the general minimum t-spanning control set problem is established here, and we present a mixed integer linear program that encodes the problem. The trajectories we propose in conjunction with the mixed integer linear program, result in a method to compute a minimum t-spanning control set whose elements are kinematically feasible and reflect the preferences of a user if those preferences are known. Finally, we consider the problem of simultaneously learning the preferences of a single user from demonstrations and computing sparse control sets for that user. We propose a technique to solve this problem that leverages a separation principle: first estimate the preferences of the user based on demonstrations, then compute a control set of trajectories that are optimal given the estimated preferences. We show that this approach optimally solves the problem. Combining the work of this thesis results in a method by which tailored control sets that reflect the preferences of a user can be determined from the demonstrations of that user. These control sets have the following beneficial attributes: 1) each element of the control set is optimal for the estimated preferences of the user, and 2) the control set optimizes a trade-off between the quality of compound maneuvers between lattice vertices -- as defined by the estimated preferences of the user -- and time required to compute them

Scaling Robot Motion Planning to Multi-core Processors and the Cloud

Imagine a world in which robots safely interoperate with humans, gracefully and efficiently accomplishing everyday tasks. The robot's motions for these tasks, constrained by the design of the robot and task at hand, must avoid collisions with obstacles. Unfortunately, planning a constrained obstacle-free motion for a robot is computationally complex---often resulting in slow computation of inefficient motions. The methods in this dissertation speed up this motion plan computation with new algorithms and data structures that leverage readily available parallel processing, whether that processing power is on the robot or in the cloud, enabling robots to operate safer, more gracefully, and with improved efficiency. The contributions of this dissertation that enable faster motion planning are novel parallel lock-free algorithms, fast and concurrent nearest neighbor searching data structures, cache-aware operation, and split robot-cloud computation. Parallel lock-free algorithms avoid contention over shared data structures, resulting in empirical speedup proportional to the number of CPU cores working on the problem. Fast nearest neighbor data structures speed up searching in SO(3) and SE(3) metric spaces, which are needed for rigid body motion planning. Concurrent nearest neighbor data structures improve searching performance on metric spaces common to robot motion planning problems, while providing asymptotic wait-free concurrent operation. Cache-aware operation avoids long memory access times, allowing the algorithm to exhibit superlinear speedup. Split robot-cloud computation enables robots with low-power CPUs to react to changing environments by having the robot compute reactive paths in real-time from a set of motion plan options generated in a computationally intensive cloud-based algorithm. We demonstrate the scalability and effectiveness of our contributions in solving motion planning problems both in simulation and on physical robots of varying design and complexity. Problems include finding a solution to a complex motion planning problem, pre-computing motion plans that converge towards the optimal, and reactive interaction with dynamic environments. Robots include 2D holonomic robots, 3D rigid-body robots, a self-driving 1/10 scale car, articulated robot arms with and without mobile bases, and a small humanoid robot.Doctor of Philosoph

GLAS: Global-to-Local Safe Autonomy Synthesis for Multi-Robot Motion Planning with End-to-End Learning

We present GLAS: Global-to- Local Autonomy Synthesis, a provably-safe, automated distributed policy generation for multi-robot motion planning. Our approach combines the advantage of centralized planning of avoiding local minima with the advantage of decentralized controllers of scalability and distributed computation. In particular, our synthesized policies only require relative state information of nearby neighbors and obstacles, and compute a provably-safe action. Our approach has three major components: i) we generate demonstration trajectories using a global planner and extract local observations from them, ii) we use deep imitation learning to learn a decentralized policy that can run efficiently online, and iii) we introduce a novel differentiable safety module to ensure collision-free operation, thereby allowing for end-to-end policy training. Our numerical experiments demonstrate that our policies have a 20% higher success rate than optimal reciprocal collision avoidance, ORCA, across a wide range of robot and obstacle densities. We demonstrate our method on an aerial swarm, executing the policy on low-end microcontrollers in real-time

Incremental Visual-Inertial 3D Mesh Generation with Structural Regularities

Visual-Inertial Odometry (VIO) algorithms typically rely on a point cloud representation of the scene that does not model the topology of the environment. A 3D mesh instead offers a richer, yet lightweight, model. Nevertheless, building a 3D mesh out of the sparse and noisy 3D landmarks triangulated by a VIO algorithm often results in a mesh that does not fit the real scene. In order to regularize the mesh, previous approaches decouple state estimation from the 3D mesh regularization step, and either limit the 3D mesh to the current frame or let the mesh grow indefinitely. We propose instead to tightly couple mesh regularization and state estimation by detecting and enforcing structural regularities in a novel factor-graph formulation. We also propose to incrementally build the mesh by restricting its extent to the time-horizon of the VIO optimization; the resulting 3D mesh covers a larger portion of the scene than a per-frame approach while its memory usage and computational complexity remain bounded. We show that our approach successfully regularizes the mesh, while improving localization accuracy, when structural regularities are present, and remains operational in scenes without regularities.Comment: 7 pages, 5 figures, ICRA accepte