Consensus-Based Multipath Planning with Collision Avoidance Using Linear Matrix Inequalities

Consensus theory has been widely applied to collective motion planning related to coordinated motion. However, when the collective motion is highly irregular and adversarial, the basic consensus theory does not guarantee collision avoidance by default. As collision avoidance is a central problem of path planning, the incorporation of avoidance into the consensus algorithm is a subject of research. This work presents a new method of incorporating collision avoidance into the consensus algorithm, by applying the concept of constrained orientation control, where orientation constraints are represented as a set of linear matrix inequalities (LMI) and solved by semidefinite programming (SDP). The developed algorithm is used to simulate consensus-based multipath planning with collision avoidance for a team of communicating soccer robots

Real-Time Optimal Slew Maneuver Planning for Small Satellites

As the small-satellite market grows, so does the demand for large-scale small-satellite missions with diverse payload functions. To facilitate mission planning, and to enable autonomous transition between payload operations, real-time on-board slew maneuver path planning is often required to reorient the spacecraft. The computed trajectory must take into account a variety of kinematic and dynamic constraints on the spacecraft attitude, angular velocity and actuator limits. It is also desirable to compute this trajectory such that it is optimal in some sense. To simplify computation, current applications typically employ a “rest-to-rest” assumption where the maneuver endpoints are assumed to be inertially fixed, leading to long settling times in practice when dynamic endpoints exist. This settling time prohibits command of maneuvers in quick succession, which can limit operational capabilities. By posing the calculation as a convex semidefinite programming optimization problem, this paper presents a computationally attractive path-planning algorithm that computes an optimal fixed-time trajectory between arbitrary endpoints, while satisfying a variety of common attitude control constraints. In addition, a closed-form iterative solution for a minimum-time maneuver is proposed. Both methods are validated in a simulation case involving the University of Toronto Institute for Aerospace Studies Space Flight Laboratory’s (UTIAS-SFL) next-generation DEFIANT-class spacecraft

Multi-Spacecraft Attitude Path Planning Using Consensus with LMI-Based Exclusion Constraints

Space missions involving multi-vehicle teams require the cooperative navigation and attitude slewing of the spacecraft or satellites, for such purposes as interferometry and optimal sensor coverage. This introduces extra constraints of exclusion zones between the spacecraft, in addition to the default exclusion constraints already introduced by damaging or blinding celestial objects. In this work, we present a quaternion-based attitude consensus protocol by using the communication topology of the spacecraft team. By using the Laplacian matrix of their communication graph and a semidefinite program, a synthesis of a time-varying optimal stochastic matrix P is done, which is used to generate various consensus and cooperative attitude trajectories from the initial attitudes of the spacecraft. The concept of quaternion-based quadratically constrained attitude control is then employed to satisfy cone avoidance constraints, where exclusion zones are identified, expressed as linear matrix inequalities (LMI), and solved by semidefinite programming (SDP)

Consensus-Based Attitude Maneuver of Multi-spacecraft with Exclusion Constraints

Some space missions involve cooperative multi-vehicle teams, for such purposes as interferometry and optimal sensor coverage, for example, NASA Terrestrial Planet Finder Mission. Cooperative navigation introduces extra constraints of exclusion zones between the spacecraft to protect them from damaging each other. This is in addition to external exclusion constraints introduced by damaging or blinding celestial objects. This work presents a quaternion-based attitude consensus protocol, using the communication topology of the team of spacecraft. The resulting distributed Laplacians of their communication graph are applied by semidefinite programming (SDP), to synthesize a series of time-varying optimal stochastic matrices. The matrices are used to generate various cooperative attitude maneuvers from the initial attitudes of the spacecraft. Exclusion constraints are satisfied by quaternion-based quadratically constrained attitude control (Q-CAC), where both static and dynamic exclusion zones are identified every time step, expressed as time-varying linear matrix inequalities (LMI) and solved by semidefinite programming

Multi-Path Planning on a Sphere with LMI-Based Collision Avoidance

The problem of path planning with collision avoidance for autonomous flying vehicles will become more critical as the density of such vehicles increase in the skies. Global aerial navigation paths can be modeled as a path-planning problem on a unit sphere. In this work, we apply consensus theory and semidefinite programming to constrained multi-path planning with collision avoidance for a team of communicating vehicles navigating on a sphere. Based on their communication graph, each vehicle individually synthesizes a time-varying Laplacian-like matrix which drives each of them from their initial positions to consensus positions on the surface of the sphere. The solution trajectories obtained on the unit sphere are transformed back to actual vehicle coordinates. Formation configurations are realized via consensus theory, while collision avoidance is realized via semidefinite programming. A Lyapunov-based stability analysis is also provided, together with simulation results to demonstrate the effectiveness of the approach

On Semidefinite Relaxations for Matrix-Weighted State-Estimation Problems in Robotics

In recent years, there has been remarkable progress in the development of so-called certifiable perception methods, which leverage semidefinite, convex relaxations to find global optima of perception problems in robotics. However, many of these relaxations rely on simplifying assumptions that facilitate the problem formulation, such as an isotropic measurement noise distribution. In this paper, we explore the tightness of the semidefinite relaxations of matrix-weighted (anisotropic) state-estimation problems and reveal the limitations lurking therein: matrix-weighted factors can cause convex relaxations to lose tightness. In particular, we show that the semidefinite relaxations of localization problems with matrix weights may be tight only for low noise levels. We empirically explore the factors that contribute to this loss of tightness and demonstrate that redundant constraints can be used to regain tightness, albeit at the expense of real-time performance. As a second technical contribution of this paper, we show that the state-of-the-art relaxation of scalar-weighted SLAM cannot be used when matrix weights are considered. We provide an alternate formulation and show that its SDP relaxation is not tight (even for very low noise levels) unless specific redundant constraints are used. We demonstrate the tightness of our formulations on both simulated and real-world data

Global Optimality via Tight Convex Relaxations for Pose Estimation in Geometric 3D Computer Vision

In this thesis, we address a set of fundamental problems whose core difficulty boils down to optimizing over 3D poses. This includes many geometric 3D registration problems, covering well-known problems with a long research history such as the Perspective-n-Point (PnP) problem and generalizations, extrinsic sensor calibration, or even the gold standard for Structure from Motion (SfM) pipelines: The relative pose problem from corresponding features. Likewise, this is also the case for a close relative of SLAM, Pose Graph Optimization (also commonly known as Motion Averaging in SfM). The crux of this thesis contribution revolves around the successful characterization and development of empirically tight (convex) semidefinite relaxations for many of the aforementioned core problems of 3D Computer Vision. Building upon these empirically tight relaxations, we are able to find and certify the globally optimal solution to these problems with algorithms whose performance ranges as of today from efficient, scalable approaches comparable to fast second-order local search techniques to polynomial time (worst case). So, to conclude, our research reveals that an important subset of core problems that has been historically regarded as hard and thus dealt with mostly in empirical ways, are indeed tractable with optimality guarantees.Artificial Intelligence (AI) drives a lot of services and products we use everyday. But for AI to bring its full potential into daily tasks, with technologies such as autonomous driving, augmented reality or mobile robots, AI needs to be not only intelligent but also perceptive. In particular, the ability to see and to construct an accurate model of the environment is an essential capability to build intelligent perceptive systems. The ideas developed in Computer Vision for the last decades in areas such as Multiple View Geometry or Optimization, put together to work into 3D reconstruction algorithms seem to be mature enough to nurture a range of emerging applications that already employ as of today 3D Computer Vision in the background. However, while there is a positive trend in the use of 3D reconstruction tools in real applications, there are also some fundamental limitations regarding reliability and performance guarantees that may hinder a wider adoption, e.g. in more critical applications involving people's safety such as autonomous navigation. State-of-the-art 3D reconstruction algorithms typically formulate the reconstruction problem as a Maximum Likelihood Estimation (MLE) instance, which entails solving a high-dimensional non-convex non-linear optimization problem. In practice, this is done via fast local optimization methods, that have enabled fast and scalable reconstruction pipelines, yet lack of guarantees on most of the building blocks leaving us with fundamentally brittle pipelines where no guarantees exist

Optimal Pose and Shape Estimation for Category-level 3D Object Perception

We consider a category-level perception problem, where one is given 3D sensor data picturing an object of a given category (e.g. a car), and has to reconstruct the pose and shape of the object despite intra-class variability (i.e. different car models have different shapes). We consider an active shape model, where -- for an object category -- we are given a library of potential CAD models describing objects in that category, and we adopt a standard formulation where pose and shape estimation are formulated as a non-convex optimization. Our first contribution is to provide the first certifiably optimal solver for pose and shape estimation. In particular, we show that rotation estimation can be decoupled from the estimation of the object translation and shape, and we demonstrate that (i) the optimal object rotation can be computed via a tight (small-size) semidefinite relaxation, and (ii) the translation and shape parameters can be computed in closed-form given the rotation. Our second contribution is to add an outlier rejection layer to our solver, hence making it robust to a large number of misdetections. Towards this goal, we wrap our optimal solver in a robust estimation scheme based on graduated non-convexity. To further enhance robustness to outliers, we also develop the first graph-theoretic formulation to prune outliers in category-level perception, which removes outliers via convex hull and maximum clique computations; the resulting approach is robust to 70%-90% outliers. Our third contribution is an extensive experimental evaluation. Besides providing an ablation study on a simulated dataset and on the PASCAL3D+ dataset, we combine our solver with a deep-learned keypoint detector, and show that the resulting approach improves over the state of the art in vehicle pose estimation in the ApolloScape datasets

Conic Programming Approaches for Polynomial Optimization: Theory and Applications

Historically, polynomials are among the most popular class of functions used for empirical modeling in science and engineering. Polynomials are easy to evaluate, appear naturally in many physical (real-world) systems, and can be used to accurately approximate any smooth function. It is not surprising then, that the task of solving polynomial optimization problems; that is, problems where both the objective function and constraints are multivariate polynomials, is ubiquitous and of enormous interest in these fields. Clearly, polynomial op- timization problems encompass a very general class of non-convex optimization problems, including key combinatorial optimization problems.The focus of the first three chapters of this document is to address the solution of polynomial optimization problems in theory and in practice, using a conic optimization approach. Convex optimization has been well studied to solve quadratic constrained quadratic problems. In the first part, convex relaxations for general polynomial optimization problems are discussed. Instead of using the matrix space to study quadratic programs, we study the convex relaxations for POPs through a lifted tensor space, more specifically, using the completely positive tensor cone and the completely positive semidefinite tensor cone. We show that tensor relaxations theoretically yield no-worse global bounds for a class of polynomial optimization problems than relaxation for a QCQP reformulation of the POPs. We also propose an approximation strategy for tensor cones and show empirically the advantage of the tensor relaxation.In the second part, we propose an alternative SDP and SOCP hierarchy to obtain global bounds for general polynomial optimization problems. Comparing with other existing SDP and SOCP hierarchies that uses higher degree sum of square (SOS) polynomials and scaled diagonally sum of square polynomials (SDSOS) when the hierarchy level increases, these proposed hierarchies, using fixed degree SOS and SDSOS polynomials but more of these polynomials, perform numerically better. Numerical results show that the hierarchies we proposed have better performance in terms of tightness of the bound and solution time compared with other hierarchies in the literature.The third chapter deals with Alternating Current Optimal Power Flow problem via a polynomial optimization approach. The Alternating Current Optimal Power Flow (ACOPF) problem is a challenging non-convex optimization problem in power systems. Prior research mainly focuses on using SDP relaxations and SDP-based hierarchies to address the solution of ACOPF problem. In this Chapter, we apply existing SOCP hierarchies to this problem and explore the structure of the network to propose simplified hierarchies for ACOPF problems. Compared with SDP approaches, SOCP approaches are easier to solve and can be used to approximate large scale ACOPF problems.The last chapter also relates to the use of conic optimization techniques, but in this case to pricing in markets with non-convexities. Indeed, it is an application of conic optimization approach to solve a pricing problem in energy systems. Prior research in energy market pricing mainly focus on linear costs in the objective function. Due to the penetration of renewable energies into the current electricity grid, it is important to consider quadratic costs in the objective function, which reflects the ramping costs for traditional generators. This study address the issue how to find the market clearing prices when considering quadratic costs in the objective function