Addressing Data Annotation Challenges in Multiple Sensors: A Solution for Scania Collected Datasets

Data annotation in autonomous vehicles is a critical step in the development of Deep Neural Network (DNN) based models or the performance evaluation of the perception system. This often takes the form of adding 3D bounding boxes on time-sequential and registered series of point-sets captured from active sensors like Light Detection and Ranging (LiDAR) and Radio Detection and Ranging (RADAR). When annotating multiple active sensors, there is a need to motion compensate and translate the points to a consistent coordinate frame and timestamp respectively. However, highly dynamic objects pose a unique challenge, as they can appear at different timestamps in each sensor's data. Without knowing the speed of the objects, their position appears to be different in different sensor outputs. Thus, even after motion compensation, highly dynamic objects are not matched from multiple sensors in the same frame, and human annotators struggle to add unique bounding boxes that capture all objects. This article focuses on addressing this challenge, primarily within the context of Scania collected datasets. The proposed solution takes a track of an annotated object as input and uses the Moving Horizon Estimation (MHE) to robustly estimate its speed. The estimated speed profile is utilized to correct the position of the annotated box and add boxes to object clusters missed by the original annotation.

Fully Sparse Long Range 3D Object Detection Using Range Experts and Multimodal Virtual Points

3D object detection at long-range is crucial for ensuring the safety and efficiency of self-driving cars, allowing them to accurately perceive and react to objects, obstacles, and potential hazards from a distance. But most current state-of-the-art LiDAR based methods are limited by the sparsity of range sensors, which generates a form of domain gap between points closer to and farther away from the ego vehicle. Another related problem is the label imbalance for faraway objects, which inhibits the performance of Deep Neural Networks at long-range. Although image features could be beneficial for long-range detections, and some recently proposed multimodal methods incorporate image features, they do not scale well computationally at long ranges or are limited by depth estimation accuracy. To address the above limitations, we propose to combine two LiDAR based 3D detection networks, one specializing at near to mid-range objects, and one at long-range 3D detection. To train a detector at long range under a scarce label regime, we further propose to weigh the loss according to the labelled objects' distance from ego vehicle. To mitigate the LiDAR sparsity issue, we leverage Multimodal Virtual Points (MVP), an image based depth completion algorithm, to enrich our data with virtual points. Our method, combining two range experts trained with MVP, which we refer to as RangeFSD, achieves state-of-the-art performance on the Argoverse2 (AV2) dataset, with improvements at long range. The code will be released soon.

Multi-Stage NMPC for a MAV based Collision Free Navigation under Varying Communication Delays

Time delays in communication networks are one of the main concerns in deploying robots with computation boards on the edge. This article proposes a multi-stage Nonlinear Model Predictive Control (NMPC) that is capable of handling varying network-induced time delays for establishing a control framework being able to guarantee collision-free Micro Aerial Vehicles (MAVs) navigation. This study introduces a novel approach that considers different sampling times by a tree of discretization scenarios contrary to the existing typical multi-stage NMPC where system uncertainties are modeled by a tree of scenarios. Additionally, the proposed method considers adaptive weights for the multi-stage NMPC scenarios based on the probability of time delays in the communication link. As a result of the multi-stage NMPC, the obtained optimal control action is valid for multiple sampling times. Finally, the overall effectiveness of the proposed novel control framework is demonstrated in various tests and different simulation environments.

Reactive Navigation of an Unmanned Aerial Vehicle with Perception-based Obstacle Avoidance Constraints

In this article we propose a reactive constrained navigation scheme, with embedded obstacles avoidance for an Unmanned Aerial Vehicle (UAV), for enabling navigation in obstacle-dense environments. The proposed navigation architecture is based on Nonlinear Model Predictive Control (NMPC), and utilizes an on-board 2D LiDAR to detect obstacles and translate online the key geometric information of the environment into parametric constraints for the NMPC that constrain the available position-space for the UAV. This article focuses also on the real-world implementation and experimental validation of the proposed reactive navigation scheme, and it is applied in multiple challenging laboratory experiments, where we also conduct comparisons with relevant methods of reactive obstacle avoidance. The solver utilized in the proposed approach is the Optimization Engine (OpEn) and the Proximal Averaged Newton for Optimal Control (PANOC) algorithm, where a penalty method is applied to properly consider obstacles and input constraints during the navigation task. The proposed novel scheme allows for fast solutions, while using limited on-board computational power, that is a required feature for the overall closed loop performance of an UAV and is applied in multiple real-time scenarios. The combination of built-in obstacle avoidance and real-time applicability makes the proposed reactive constrained navigation scheme an elegant framework for UAVs that is able to perform fast nonlinear control, local path-planning and obstacle avoidance, all embedded in the control layer.

Collision avoidance for multiple MAVs using fast centralized NMPC

This article proposes a novel control architecture using a centralized nonlinear model predictive control (CNMPC) scheme for controlling multiple micro aerial vehicles (MAVs). The control architecture uses an augmented state system to control multiple agents and performs both obstacle and collision avoidance. The optimization algorithm used is OpEn, based on the proximal averaged Newton type method for optimal control (PANOC) which provides fast convergence for non-convex optimization problems. The objective is to perform position reference tracking for each individual agent, while nonlinear constrains guarantee collision avoidance and smooth control signals. To produce a trajectory that satisfies all constraints a penalty method is applied to the nonlinear constraints. The efficacy of this proposed novel control scheme is successfully demonstrated through simulation results and comparisons, in terms of computation time and constraint violations, while are provided with respect to the number of agents.

COMPRA: A COMPact Reactive Autonomy framework for subterranean MAV based search-and-rescue operations

This work establishes COMPRA, a compact and reactive autonomy framework for fast deployment of MAVs in subterranean Search-and-Rescue missions. A COMPRA-enabled MAV is able to autonomously explore previously unknown areas while specific mission criteria are considered e.g. an object of interest is identified and localized, the remaining useful battery life, the overall desired exploration mission duration. The proposed architecture follows a low-complexity algorithmic design to facilitate fully on-board computations, including nonlinear control, state-estimation, navigation, exploration behavior and object localization capabilities. The framework is mainly structured around a reactive local avoidance planner, based on enhanced Potential Field concepts and using instantaneous 3D pointclouds, as well as a computationally efficient heading regulation technique, based on contour detection on an instantaneous camera stream. Those techniques decouple the collision-free path generation from the dependency of a global map and are capable of handling imprecise localization occasions. Field experimental verification of the overall architecture is performed in relevant unknown GPS-denied environments.

Geometry Aware NMPC Scheme for Morphing Quadrotor Navigation in Restricted Entrances

Geometry-morphing Micro Aerial Vehicles (MAVs) are gaining more and more attention lately, since their ability to modify their geometric morphology in-flight increases their versatility, while expanding their application range. In this novel research field, most of the works focus on the platform design and on the low-level control part for maintaining stability after the deformation. Nevertheless, another aspect of geometry morphing MAVs is the association of the deformation with respect to the shape and structure of the environment. In this article, we propose a novel Nonlinear Model Predictive Control (NMPC) structure that modifies the morphology of a quadrotor based on the environmental entrances geometrical shape. The proposed method considers restricted entrances as a constraint in the NMPC and modifies the arm configuration of the MAV to provide a collision free path from the initial position to the desired goal, while passing through the entrance. To the authors' best knowledge, this work is the first to connect the in-flight morphology with the characteristics of environmental shapes. Multiple simulation results depict the performance and efficiency of the proposed scheme in scenarios where the quadrotor is commanded to pass through restricted areas.

Nonlinear MPC for Collision Avoidance and Controlof UAVs With Dynamic Obstacles

This article proposes a Novel Nonlinear Model Predictive Control (NMPC) for navigation and obstacle avoidance of an Unmanned Aerial Vehicle (UAV). The proposed NMPC formulation allows for a fully parametric obstacle trajectory, while in this article we apply a classification scheme to differentiate between different kinds of trajectories to predict future obstacle positions. The trajectory calculation is done from an initial condition, and fed to the NMPC as an additional input. The solver used is the nonlinear, non-convex solver Proximal Averaged Newton for Optimal Control (PANOC) and its associated software OpEn (Optimization Engine), in which we apply a penalty method to properly consider the obstacles and other constraints during navigation. The proposed NMPC scheme allows for real-time solutions using a sampling time of 50 ms and a two second prediction of both the obstacle trajectory and the NMPC problem, which implies that the scheme can be considered as a local path-planner. This paper will present the NMPC cost function and constraint formulation, as well as the methodology of dealing with the dynamic obstacles. We include multiple laboratory experiments to demonstrate the efficacy of the proposed control architecture, and to show that the proposed method delivers fast and computationally stable solutions to the dynamic obstacle avoidance scenarios.

A Unified NMPC Scheme for MAVs Navigation with 3D Collision Avoidance under Position Uncertainty

This article proposes a novel Nonlinear Model Predictive Control (NMPC) framework for Micro Aerial Vehicle (MAV) autonomous navigation in constrained environments. The introduced framework allows us to consider the nonlinear dynamics of MAVs and guarantees real-time performance. Our first contribution is to design a computationally efficient subspace clustering method to reveal from geometrical constraints to underlying constraint planes within a 3D point cloud, obtained from a 3D lidar scanner. The second contribution of our work is to incorporate the extracted information into the nonlinear constraints of NMPC for avoiding collisions. Our third contribution focuses on making the controller robust by considering the uncertainty of localization and NMPC using the Shannon entropy. This step enables us to track either the position or velocity references, or none of them if necessary. As a result, the collision avoidance constraints are defined in the local coordinates of MAVs and it remains active and guarantees collision avoidance, despite localization uncertainties, e.g., position estimation drifts. Additionally, as the platform continues the mission, this will result in less uncertain position estimations, due to the feature extraction and loop closure. The efficacy of the suggested framework has been evaluated using various simulations in the Gazebo environment.

Subterranean MAV Navigation based on Nonlinear MPC with Collision Avoidance Constraints

Micro Aerial Vehicles (MAVs) navigation in subterranean environments is gaining attention in the field of aerial robotics, however there are still multiple challenges for collision free navigation in such harsh environments. This article proposes a novel baseline solution for collision free navigation with Nonlinear Model Predictive Control (NMPC). In the proposed method, the MAV is considered as a floating object, where the velocities on the $x$, $y$ axes and the position on altitude are the references for the NMPC to navigate along the tunnel, while the NMPC avoids the collision by considering kinematics of the obstacles based on measurements from a 2D lidar. Moreover, a novel approach for correcting the heading of the MAV towards the center of the mine tunnel is proposed, while the efficacy of the suggested framework has been evaluated in multiple field trials in an underground mine in Sweden.