ICRA 2021 Papers

Our novel skin-feature visual-tracking algorithm enables anatomic vSLAM and (by extension) localization of clinical tools relative to the patient’s body. Tracking naturally occurring features is challenging due to patient uniqueness, deformability, and lack of an accurate a-priori 3D geometric model. Our method (i) tracks skin features in a smartphone-camera video sequence, (ii) performs anatomic Simultaneous Localization And Mapping (SLAM) of camera motion relative to the patient’s 3D skin surface, and (iii) utilizes existing visual methods to track clinical tool(s) relative to the patient’s reconstructed 3D skin surface. (We demonstrate tracking of a simulated ultrasound probe relative to the patient by using an Apriltag visual fiducial). Our skin-feature tracking method utilizes the Fourier-Mellin Transform for robust performance, which we incorporated and extend an existing Phase Only Correlation (POC) based algorithm to be suitable for our application of free-hand smartphone video, wherein the distance of the camera fluctuates relative to the patient. Our SLAM approach further utilizes Structure from Motion and Bundle Adjustment to achieve an accurate 3D model of the human body with minimal drift-error in camera trajectory. We believe this to be the first freehand smartphone-camera tracking of natural skin features for anatomic tracking of surgical tools, ultrasound probe, etc.

The matching and fusing of local maps generated by multiple robots can greatly enhance the performance of relative localization and collaborative mapping. Currently, existing semantic matching methods are partly based on classical iterative closet point (ICP), which typically fail in cases with large initial error. What’s more, current semantic matching algorithms have high computation complexity in optimizing the transformation matrix. To address the challenge of map matching with large initial error, this paper proposes a novel semantic map matching algorithm with large convergence region. The key novelty of this work is the designing of the initial transformation optimization algorithm and the probabilistic registration model to increase the convergence region. To reduce the initial error before the iteration process, the initial transformation matrix is optimized by estimating the credibility of the data association. At the same time, a factor reflecting the uncertainty of the initial error is calculated and introduced to the formulation of the probabilistic registration model, thereby accelerating the convergence process. The proposed algorithm is performed on public datasets and compared with existing methods, demonstrating the significant improvement in terms of matching accuracy and robustness.

We present a tightly-coupled multi-sensor fusion architecture for autonomous vehicle applications, which achieves centimetre-level accuracy and high robustness in various scenarios. In order to realize robust and accurate point-cloud feature matching we propose a novel method for extracting structural, highly discriminative features from LiDAR point clouds. For high frequency motion prediction and noise propagation, we use incremental on-manifold IMU pre-integration. We also adopt a multi-frame sliding window square root inverse filter, so that the system maintains numerically stable results under the premise of limited power consumption. To verify our methodology, we test the fusion algorithm in multiple applications and platforms equipped with a LiDAR-IMU system. Our results demonstrate that our fusion framework attains state-of-the-art localization accuracy, high robustness and a good generalization ability.

To enable robots to smoothly interact with humans during their travels together as a group, robots need the ability to adapt their motions under environmental changes and ensure all group members’ routes are feasible. To achieve this ability, robots require knowledge of the final destination and the subgoals in between. In practice, such information is seldom shared explicitly among group members, and may be frequently updated. Under this uncertain setting, maintaining travel efficiency and behavior appropriateness becomes a challenge. Previous literature approached the problem by generating compliant coordinating motions inspired by human groups, with subgoal uncertainty remaining isolated from the plan evaluation process. We show that such coordination can lead the robot to "bad" transient states where inefficient planning and lost tracking may incur. We propose to resolve the problem by formulating the coordinating motion as a Bayesian stochastic game, to plan for the robot as a group member, in the meanwhile considering the long-term effect of uncertainty during path coordination. We show that the approach improves travel efficiency and partner tracking robustness, by preventing assertive decisions during the inference update process. Moreover, the approach presents "agency", in the sense that it can generate human-like motions, which can be applied and contribute to the pedestrian simulation literature; the approach also affords variants from the human-like motions to generate robot behaviors based on sensing capabilities, contributing to the methodology of robot behavior design.

We propose a methodology for robust, real-time place recognition using an imaging lidar, which yields image-quality high-resolution 3D point clouds. Utilizing the intensity readings of an imaging lidar, we project the point cloud and obtain an intensity image. ORB feature descriptors are extracted from the image and encoded into a bag-of-words vector. The vector, used to identify the point cloud, is inserted into a database that is maintained by DBoW for fast place recognition queries. The returned candidate is further validated by matching visual feature descriptors. To reject matching outliers, we apply PnP, which minimizes the reprojection error of visual features’ positions in Euclidean space with their correspondences in 2D image space, using RANSAC. Combining the advantages from both camera and lidar-based place recognition approaches, our method is truly rotation-invariant, and can tackle reverse revisiting and upside down revisiting. The proposed method is evaluated on datasets gathered from a variety of platforms over different scales and environments. Our implementation and datasets are available at https://git.io/image-lidar.

Motion planning for high degree-of-freedom (DOF) robots is challenging, especially when acting in complex environments under sensing uncertainty. While there is significant work on how to plan under state uncertainty for low-DOF robots, existing methods cannot be easily translated into the high-DOF case, due to the complex geometry of the robot’s body and its environment. In this paper, we present a method that enhances optimization-based motion planners to produce robust trajectories for high-DOF robots for convex obstacles. Our approach introduces robustness into planners that are based on sequential convex programming: We reformulate each convex subproblem as a robust optimization problem that "protects" the solution against deviations due to sensing uncertainty. The parameters of the robust problem are estimated by sampling from the distribution of noisy obstacles, and performing a first-order approximation of the signed distance function. The original merit function is updated to account for the new costs of the robust formulation at every step. The effectiveness of our approach is demonstrated on two simulated experiments that involve a full body square robot, that moves in randomly generated scenes, and a 7-DOF Fetch robot, performing tabletop operations. The results show nearly zero probability of collision for a reasonable range of the noise parameters for Gaussian and Uniform uncertainty.

This paper presents a vision-based modularized drone racing navigation system that uses a customized convolutional neural network (CNN) for the perception module to produce high-level navigation commands and then leverages a state-of-the-art planner and controller to generate low-level control commands, thus exploiting the advantages of both data- based and model-based approaches. Unlike the state-of-the-art method, which only takes the current camera image as the CNN input, we further add the latest three estimated drone states as part of the inputs. Our method outperforms the state-of-the-art method in various track layouts and offers two switchable navigation behaviors with a single trained network. The CNN-based perception module is trained to imitate an expert policy that automatically generates ground truth navigation commands based on the pre-computed global trajectories. Owing to the extensive randomization and our modified dataset aggregation (DAgger) policy during data collection, our navigation system, which is purely trained in simulation with synthetic textures, successfully operates in environments with randomly-chosen photo-realistic textures without further fine-tuning.

Recently, the motion averaging method has been introduced as an effective means to solve the multi-view registration problem. This method aims to recover global motions from a set of relative motions, where the original method is sensitive to outliers due to using the Frobenius norm error in the optimization. Accordingly, this paper proposes a novel robust motion averaging method based on the maximum correntropy criterion (MCC). Specifically, the correntropy measure is used instead of utilizing Frobenius norm error to improve the robustness of motion averaging against outliers. According to the half-quadratic technique, the correntropy measure based optimization problem can be solved by the alternating minimization procedure, which includes operations of weight assignment and weighted motion averaging. Further, we design a selection strategy of adaptive kernel width to take advantage of correntropy. Experimental results on benchmark data sets illustrate that our method has superior performance on accuracy and robustness for multi-view registration. What’s more, it can be applied to robot mapping.

In this work we augment our prior state-of-the-art visual-inertial odometry (VIO) system, OpenVINS [1], to produce accurate dense depth by filling in sparse depth estimates (depth completion) from VIO with image guidance – all while focusing on enabling real-time performance of the full VIO+depth system on embedded devices. We show that noisy depth values with varying sparsity produced from a VIO system can not only hurt the accuracy of predicted dense depth maps, but also make them considerably worse than those from an image-only depth network with the same underlying architecture. We investigate this sensitivity on both an outdoor simulated and indoor handheld RGB-D dataset, and present simple yet effective solutions to address these shortcomings of depth completion networks. The key changes to our state-of-the-art VIO system required to provide high quality sparse depths for the network while still enabling efficient state estimation on embedded devices are discussed. A comprehensive computational analysis is performed over different embedded devices to demonstrate the efficiency and accuracy of the proposed VIO depth completion system.

This paper addresses robust landing stabilization in humanoid locomotion on uneven terrain. The core idea is to find a configuration of the robot that results in small impulsive force when an unexpected obstacle is encountered, and to adjust post-contact reference for swing foot with which the pose of the foot is stabilized on the obstacle. This can be achieved by walking with heel strike motion (validated by the impact map analysis) and by employing hybrid admittance control combining the admittance control with reset of post-contact reference, embedded into the momentum-based whole-body control framework. The validity of the proposed algorithm is verified by simulation with a physics engine.

In this paper, we propose a novel iterative learning control (ILC) scheme for precise state tracking of pneumatic muscle (PM) actuators. Two critical issues are considered in our scheme: 1) state constraints on PM position and velocity; 2) uncertainties of the PM model. Based on the three-element form, a PM model is constructed that takes both parametric and nonparametric uncertainties into consideration. By introducing the composite energy function (CEF) approach incorporated with a barrier Lyapunov function (BLF), full state constraints of PM will not be violated and uncertainties are effectively compensated. Through rigorous analysis, we show that under proposed ILC scheme, uniform convergence of PM state tracking errors are guaranteed. Simulation results validate the performance of the proposed scheme.

Recent works on semantic Simultaneous Localization and Mapping (SLAM) utilizing object landmarks have shown superiority in terms of robustness and accuracy in tracking and localization. 3D object landmarks represented by a cubic or quadric surface are inferred from 2D object bounding boxes which are typically captured from multiple views by an object detector. Nevertheless, bounding box noises and small camera baseline may lead to an inaccurate 3D object landmark inference. Inspired by the dual quadric enveloping property, in this work, we introduce the horizontal support assumption to constrain rotation w.r.t. roll and pitch for a quadric representation. As the result, we reduce the number of quadric parameters and narrow down the solution space, and ultimately produce a relatively accurate inference. Extensive experimental evaluations under both simulated and real scenarios are conducted in this paper. Quantitative results demonstrate that our approach outperforms the state-of-the-art.

Robotic teleoperation provides human-in-the-loop capabilities of complex manipulation tasks in dangerous or remote environments, such as for planetary exploration or nuclear decommissioning. This work proposes a novel telemanipulation architecture using a passive Fractal Impedance Controller (FIC), which does not depend upon an active viscous component for guaranteeing stability. Compared to a traditional impedance controller in ideal conditions (no delays and maximum communication bandwidth), our proposed method yields higher transparency in interaction and demonstrates superior dexterity and capability in our telemanipulation test scenarios. We also validate its performance with extreme delays up to 1 s and communication bandwidths as low as 10 Hz. All results validate a consistent stability when using the proposed controller in challenging conditions, regardless of operator expertise.

State of the art mapping algorithms can produce high-quality maps. However, they are still vulnerable to clutter and outliers which can affect map quality and in consequence hinder the performance of a robot, and further map processing for semantic understanding of the environment. This paper presents ROSE, a method for building-level structure detection in robotic maps. ROSE exploits the fact that indoor environments usually contain walls and straight-line elements along a limited set of orientations. Therefore metric maps often have a set of dominant directions. ROSE extracts these directions and uses this information to segment the map into structure and clutter through filtering the map in the frequency domain (an approach substantially underutilised in the mapping applications). Removing the clutter in this way makes wall detection (e.g. using the Hough transform) more robust. Our experiments demonstrate that (1) the application of ROSE for decluttering can substantially improve structural feature retrieval (e.g., walls) in cluttered environments, (2) ROSE can successfully distinguish between clutter and structure in the map even with substantial amount of noise and (3) ROSE can numerically assess the amount of structure in the map.

The connectivity of distributed networked multi-robot systems is a crucial operational specification, since the involved robots interact/communicate locally only with their immediate neighbors. Thus, in this work, we propose a distributed algorithm to estimate the algebraic connectivity of the underlying communication graph, which stands as a valid connectivity metric. Our method establishes robustness and fast convergence properties that can be adjusted independently via the appropriate selection of certain design parameters. Finally, we confirm the theoretical findings through simulated paradigms and verify the superiority of our method against a well-established solution of the related literature.

We tackle the problem of multiple quadrotors transporting a cable-suspended point-mass load. The quadrotors are treated as a virtual leader-follower algorithm, where a multi-layer graph encapsulates the communication and physical interaction. On the one hand, the communication stands for the approach of following the reference trajectory of a virtual leader. On the other hand, the load exerts a distributed tension force on each cable which is modeled as the well-known spring-damping system to each quadrotor establishing an interconnected dynamic. We assume cables are stretchable and have neglectable mass. Both objectives are accomplished through a Model Reference Adaptive Control approach with a robust modification that treats uncertainties and perturbations given by error in parameters, noise in the signal, and the wind drag forces. We prove stability based on Lyapunov approach and the results are shown through simulation.

In this paper, we present a novel preconditioning strategy for the classic 8-point algorithm (8-PA) for estimating an essential matrix from 360-FoV images (i.e., equirectangular images) in spherical projection. To alleviate the effect of uneven key-feature distributions and outlier correspondences, which can potentially decrease the accuracy of an essential matrix, our method optimizes a non-rigid transformation to deform a spherical camera into a new spatial domain, defining a new constraint and a more robust and accurate solution for an essential matrix. Through several experiments using random synthetic points, 360-FoV, and fish-eye images, we demonstrate that our normalization can increase the camera pose accuracy about 20% without significantly overhead the computation time. In addition, we present further benefits of our method through both a constant weighted least-square optimization that improves further the well known Gold Standard Method (GSM) (i.e., the non-linear optimization by using epipolar errors); and a relaxation of the number of RANSAC iterations, both showing that our normalization outcomes a more reliable, robust, and accurate solution.

Generating locally optimal UAV-trajectories is challenging due to the non-convex constraints of collision avoidance and actuation limits. We present the first local, optimization-based UAV-trajectory generator that simultane-ously guarantees validity and asymptotic optimality for known environments. Validity: Given a feasible initial guess, our algo-rithm guarantees the satisfaction of all constraints throughout the process of optimization. Asymptotic Optimality: We use an asymptotic exact piecewise approximation of the trajectory with an automatically adjustable resolution of its discretization. The trajectory converges under refinement to the first-order stationary point of the exact non-convex programming problem. Our method has additional practical advantages including joint optimality in terms of trajectory and time-allocation, and robustness to challenging environments as demonstrated in our experiments.

We explore how high-speed robot arm motions can dynamically manipulate ropes and cables to vault over obstacles, knock objects from pedestals, and weave between obstacles. In this paper, we propose a self-supervised learning framework that enables a UR5 robot to perform these three tasks. The framework finds a 3D apex point for the robot arm, which, together with a task-specific trajectory function, defines an arcing motion that dynamically manipulates the cable to perform a task with varying obstacle and target locations. The trajectory function computes minimum-jerk motions that are constrained to remain within joint limits and to travel through the 3D apex point by repeatedly solving quadratic programs to find the shortest and fastest feasible motion. We experiment with 5 physical cables with different thickness and mass and compare performance against two baselines in which a human chooses the apex point. Results suggest that a baseline with a fixed apex across the three tasks achieves respective success rates of 51.7 %, 36.7 %, and 15.0 %, and a baseline with human-specified, task-specific apex points achieves 66.7 %, 56.7 %, and 15.0 % success rate respectively, while the robot using the learned apex point can achieve success rates of 81.7 % in vaulting, 65.0 % in knocking, and 60.0 % in weaving. Code, data, and supplementary materials are available at https://sites.google.com/berkeley.edu/dynrope/home.