ICRA 2022 Papers

3D Multi-Object Tracking (MOT) plays a crucial role in efficient and safe operation of automatic driving, especially in scenarios of occlusion or poor visibility. Most 3D MOT methods leverage only positional distance, which is insufficient for scenes with high density of objects or drastic changes in the motion state. In order to address this, we propose a new 3D MOT model which fuses information pertaining to positional distance and geometric similarity. Our proposed solution comprises of four parts: a) a feature extraction mechanism integrated into a commonly used detector to extract individual features for each detection, b) computation of two distance matrices based on Euclidean distance and feature similarity, c) conversion of the distance matrices to probability matrices by a cluster based Earth-Mover Distance (EMD) algorithm, and d) a data association method that fuses both sources to boost the tracking accuracy. Our proposed model demonstrates state-of-the-art performance on the nuScenes tracking dataset, with extensive experiments attesting to an improved tracking accuracy over baselines that operate solely on positional distance.

Due to overly relying on appearance information or adopting direct static feature fusion, most of the existing action recognition methods based on multi-modality have poor robustness and insufficient consideration of modality differences. To address these problems, we propose a two-stream adaptive weight integration network with a three-dimensional parallel attention module, PA-AWCNN. Firstly, a three-dimensional Parallel Attention (PA) module is proposed to effectively extract features of spatial, temporal and channel dimensions and reduce the cross-dimensional interference, to achieve better robustness. Secondly, a Common Feature-driven (CFD) feature integration module is proposed to dynamically integrate appearance and depth features with adaptive weights, utilizing modality differences to redeem the lack of each feature, thereby balance the influence of both. The proposed PA-AW CNN uses the representative integrated feature generated by attention enhancement and feature integration for action recognition; it can not only get higher recognition accuracy but also improve the performance of distinguishing similar actions. Experiments illustrate that the proposed method achieves com-parable performances to state-of-the-art methods and obtains the accuracy of 92.76% and 95.65% on NTU RGB+D Dataset and SBU Kinect Interaction Dataset, respectively. The code is publicly available at: https://github.com/Luu-Yao/PA-AWCNN.

For a robot to perform a grasping and manipulation task, it has to determine possible robot placements in the workspace, from which target objects or environmental elements relevant to the given task are reachable. This work presents a novel approach for finding placements for the mobile base of a humanoid robot in an unknown environment with multiple support planes. We propose a novel type of reachability map - the Oriented Surface Reachability Map - that takes inclined surfaces in the environment into account and has the same complexity as reachability maps designed for flat surfaces. The resulting robot placements are not limited to SE(2) but can be applied to arbitrarily oriented planes in 3D space. The proposed method was evaluated in simulation and on the humanoid robot ARMAR-6 in real-world grasping experiments. The results show that a placement can be found for over 80% of the poses that are reachable in complicated, simulated environments, with only a small runtime overhead.

Continuum robots are typically slender and flexible with infinite freedoms in theory, which poses a challenge for their control and application. The shape reconstruction of continuum robots is vital to realize closed-loop control. This paper proposes a novel general real-time shape reconstruction framework of continuum robots based on the piecewise polynomial curvature (PPC) kinematics model. We illustrate the coupling between orientation and position at any given location of the continuum robots. Further, the coupling relation could be bridged by the PPC kinematics. Therefore, we propose to estimate the shape through multi-attitude solving, using the off-the-shelf orientation sensors, e.g., IMUs, mounted on certain locations. The approach gives a valuable framework to real-time shape reconstruction of continuum robots, which is general, accurate and convenient. The accuracy of our approach is verified in the experiments of distinct physical prototypes.

In this paper, to achieve the goal of aiding the blind and visually impaired (BVI) to read any text not written in Braille, a custom-built, finger-wearable, and electro-tactile based Braille reading system with its Rapid Optical Character Recognition (R-OCR) method is developed. The R-OCR is capable of processing text information in real time using a miniature fish-eye imaging device mounted at the finger-wearable system. This allows real-time translation of printed text to electro-Braille along with natural movement of user's fingertip as if reading any Braille sign or book. An electro-tactile neuro-stimulation feedback mechanism is further proposed and incorporated with the reading system, which facilitates a new opto-electrotactile-feedback-based text line tracking control approach that enables text line following by user's fingertip during reading. Extensive experiments were designed and conducted to test the ability of blindfolded participants to read through and follow the printed text lines based on this optoelectrotactile-feedback method. The experimental results show that as the outcome of the opto-electrotactile-feedback, the users who involved in the feedback loop were able to maintain their fingertips within a 2mm2mm distance of the text while “reading” through a printed text line. Our work is a significant step to aid the BVI users with a portable means to read any printed texts to Braille through the following and the translation, whether in the digital realm or physically, on any surface.

Indirect trajectory optimization methods such as Differential Dynamic Programming (DDP) have found considerable success when only planning under dynamic feasibility constraints. Meanwhile, nonlinear programming (NLP) has been the state-of-the-art approach when faced with additional constraints (e.g., control bounds, obstacle avoidance). However, a naïve implementation of NLP algorithms, e.g., shooting-based sequential quadratic programming (SQP), may suffer from slow convergence – caused from natural instabilities of the underlying system manifesting as poor numerical stability within the optimization. Re-interpreting the DDP closed-loop rollout policy as a sensitivity-based correction to a second-order search direction, we demonstrate how to compute analogous closedloop policies (i.e., feedback gains) for constrained problems. Our key theoretical result introduces a novel dynamic programmingbased constraint-set recursion that augments the canonical “cost-to-go” backward pass. On the algorithmic front, we develop a hybrid-SQP algorithm incorporating DDP-style closedloop rollouts, enabled via efficient parallelized computation of the feedback gains. Finally, we validate our theoretical and algorithmic contributions on a set of increasingly challenging benchmarks, demonstrating significant improvements in convergence speed over standard open-loop SQP.

The next generation of Mars rotorcrafts requires on-board autonomous hazard avoidance landing. To this end, this work proposes a system that performs continuous multi-resolution height map reconstruction and safe landing spot detection. Structure-from-Motion measurements are aggregated in a pyramid structure using a novel Optimal Mixture of Gaus-sians formulation that provides a comprehensive uncertainty model. Our multiresolution pyramid is built more efficiently and accurately than past work by decoupling pyramid filling from the measurement updates of different resolutions. To detect the safest landing location, after an optimized hazard segmentation, we use a mean shift algorithm on multiple distance transform peaks to account for terrain roughness and uncertainty. The benefits of our contributions are evaluated on real and synthetic flight data.

We perform a systematic exploration of the principle of Space Utilization Optimization (SUO) as a heuristic for planning better individual paths in a decoupled multi-robot path planner, with applications to both one-shot and life-long multi-robot path planning problems. We show that the heuristic set, SU - I, preserves single path optimality and significantly reduces congestion that naturally happens when many paths are planned without coordination. Integration of SU - I into complete planners brings dramatic reductions in computation time due to the significantly reduced number of conflicts and leads to sizable solution optimality gains in diverse evaluation scenarios over medium and large maps, for both one-shot and life-long problem settings.

Wire arc additive manufacturing is a metal additive manufacturing process in which the material is deposited using arc welding technology. It is gaining popularity due to high material deposition rates and faster build time. It is en-abled using robotic manipulators and can build relatively large-scale parts faster when compared with other metal additive manufacturing processes. However, the size of the large-scale parts is limited by the size of the industrial manipulator being used for the process. This limitation is overcome by using a fixed configuration multi-robot cell in which manipulators work cooperatively to build large-scale parts quickly. A fixed multi-robot cell with closely spaced industrial manipulators has high flexibility, but it restricts the part size that can be built. If the manipulators are spread out, the cell loses its flexibility but can build relatively larger parts. This issue can be avoided by using larger size manipulators, which are expensive, or by moving the modest size manipulators based on the part geometries. This paper presents a novel algorithm to generate multi-robot placements for different part geometries to be built using wire arc additive manufacturing. Furthermore, the algorithm hierarchically optimizes the build time and the inverse kinematics consistency in robot paths to improve the process efficiency and part quality. We compare the results with fixed multi-robot cells and provide insights to users to make an informed decision on whether to use a fixed or a flexible multi-robot cell for wire arc additive manufacturing.

This paper proposes a method to compute camera 6 DoF poses to achieve a user defined coverage. The camera placement problem is modeled as a combinatorial optimization where given the maximum number of cameras, a camera set is selected from a larger pool of possible camera poses. We propose to minimize the squared error between the desired and the achieved coverage, and formulate the non-linear cost function as a mixed integer linear programming problem. A camera lens model is utilized to project the camera's view on a 3D voxel map to compute a coverage score which makes the optimization problem in real environments tractable. Experimental results in two real retail store environments demonstrate the better performance of the proposed formulation in terms of coverage and overlap for triangulation compared to existing methods.

We present an algorithm, based on the Differential Dynamic Programming framework, to handle trajectory optimization problems in which the horizon is determined online rather than fixed a priori. This algorithm exhibits exact one-step convergence for linear, quadratic, time-invariant problems and is fast enough for real-time nonlinear model-predictive control. We show derivations for the nonlinear algorithm in the discrete-time case, and apply this algorithm to a variety of nonlinear problems. Finally, we show the efficacy of the optimal-horizon model-predictive control scheme compared to a standard MPC controller, on an obstacle-avoidance problem with planar robots.

We formalize and study the multi-goal task assignment and path finding (MG-TAPF) problem from theoretical and algorithmic perspectives. The MG-TAPF problem is to compute an assignment of tasks to agents, where each task consists of a sequence of goal locations, and collision-free paths for the agents that visit all goal locations of their assigned tasks in sequence. Theoretically, we prove that the MG-TAPF problem is NP-hard to solve optimally. We present algorithms that build upon algorithmic techniques for the multi-agent path finding problem and solve the MG-TAPF problem optimally and bounded-suboptimally. We experimentally compare these algorithms on a variety of different benchmark domains.

Recent advances in Model Predictive Control (MPC) algorithms and methodologies, combined with the surge of computational power of available embedded platforms, allows the use of real-time optimization-based control of fast mechatronic systems. This paper presents an implementation of an optimal guidance, navigation and control (GNC) system for the motion control of a small-scale electric prototype of a thrust-vectored rocket. The aim of this prototype is to provide an inexpensive platform to explore GNC algorithms for automatic landing of sounding rockets. The guidance and trajectory tracking are formulated as continuous-time optimal control problems and are solved in real-time on embedded hardware using the PolyMPC library. An Extended Kalman Filter (EKF) is designed to estimate external disturbances and actuators offsets. Finally, indoor and outdoor flight experiments are performed to validate the architecture.

Inverted landing is a challenging feat to perform in aerial robots, especially without external positioning. However, it is routinely performed by biological fliers such as bees, flies, and bats. Our previous observations of landing behaviors in flies suggest an open-loop causal relationship between their putative visual cues and the kinematics of the aerial maneuvers executed. For example, the degree of rotational maneuver (the amount of body inversion prior to touchdown) and the amount of leg-assisted body swing both depend on the flies' initial body states while approaching the ceiling. In this work, inspired by the inverted landing behavior of flies, we used a physics-based simulation with experimental validation to systematically investigate how optimized inverted landing maneuvers depend on the initial approach velocities with varied magnitude and direction. This was done by analyzing the putative visual cues (that can be derived from onboard measurements) during optimal maneuvering trajectories. We identified a three-dimensional policy region, from which a mapping to a global inverted landing policy can be developed without the use of external positioning data. Through simulation, we also investigated the effects of an array of landing gear designs on the optimized landing performance and identified their advantages and disadvantages. The above results have been partially validated using limited experimental testing and will continue to inform and guide our future experiments, for example by applying the calculated global policy.

Aerial Manipulators with Elastic Suspension (AMES) may be seen as a hybrid robot mixing properties of classical Aerial Manipulators (AMs) and Cable-Driven Parallel Robots (CDPRs). The optimal design and control of an AMES using unidirectional thrusters are considered in this paper. To maximize the workspace, an optimization algorithm is proposed. The position and orientation of the thrusters are optimized by adapting methods borrowed from both the AM and CDPR communities. The resulting design is used to build a prototype. Preliminary experimentations are carried out to validate the theoretical workspace and assess the trajectory tracking performance of this AMES. Experiments highlight the significant improvements with respect to a previous suboptimal prototype.

Derivative based optimization methods are efficient at solving optimal control problems near local optima. However, their ability to converge halts when derivative information vanishes. The inference approach to optimal control does not have strict requirements on the objective landscape. However, sampling, the primary tool for solving such problems, tends to be much slower in computation time. We propose a new method that combines second order methods with inference. We utilise the Kullback Leibler (KL) control framework to formulate an inference problem that computes the optimal controls from an adaptive distribution approximating the solution of the second order method. Our method allows for combining simple convex and non convex cost functions. This simplifies the process of cost function design and leverages the strengths of both inference and second order optimization. We compare our method to Model Predictive Path Integral (MPPI) and iterative Linear Quadratic Gaussian controller (iLQG), outperforming both in sample efficiency and quality on manipulation and obstacle avoidance tasks.

Scene classification is a well-established area of computer vision research that aims to classify a scene image into pre-defined categories such as playground, beach and airport. Recent work has focused on increasing the variety of pre-defined categories for classification, but so far failed to consider two major challenges: changes in scene appearance due to lighting and open set classification (the ability to classify unknown scene data as not belonging to the trained classes). Our first contribution, SceneVLAD, fuses scene classification and visual place recognition CNNs for appearance invariant scene classification that outperforms state-of-the-art scene classification by a mean F1 score of up to 0.1. Our second contribution, OpenSceneVLAD, extends the first to an open set classification scenario using intra-class splitting to achieve a mean increase in F1 scores of up to 0.06 compared to using state-of-the-art openmax layer. We achieve these results on three scene class datasets extracted from large scale outdoor visual localisation datasets, one of which we collected ourselves.

Online state-time trajectory planning in highly dynamic environments remains an unsolved problem due to the curse of dimensionality of the state-time space. Existing state-time planners are typically implemented based on randomized sampling approaches or path searching on discrete graphs. The smoothness, path clearance, or planning efficiency is sometimes not satisfying. In this work, we propose a gradient-based planner on the state-time space for online trajectory generation in highly dynamic environments. To enable the gradient-based optimization, we propose a Timed-ESDT that supports distance and gradient queries with state-time keys. Based on the Timed-ESDT, we also define a smooth prior and an obstacle likelihood function that are compatible with the state-time space. The trajectory planning is then formulated to a MAP problem and solved by an efficient numerical optimizer. Moreover, to improve the optimality of the planner, we also define a state-time graph and conduct path searching on it to find a better initialization for the optimizer. By integrating the graph searching, the planning quality is significantly improved. Experiments on simulated and benchmark datasets demonstrate the superior performance of our proposes method over conventional ones.

In the foreseeable future, connected and auto-mated vehicles (CAVs) and human-driven vehicles will share the road networks together. In such a mixed traffic environment, CAVs need to understand and predict maneuvers of surrounding vehicles for safer and more efficient interactions, especially when human drivers bring in a wide range of uncertainties. In this paper, we propose a learning-based lane-change prediction algorithm that considers the driving behaviors of the target human driver. To provide accurate maneuver prediction, we adopt a hierarchical structure that seamlessly seals both the lane-change decision prediction and the vehicle trajectory pre-diction together. Specifically, we propose a lane-change decision prediction method based on a Long-Short Term Memory (LSTM) network, and a trajectories prediction considering driver preference and vehicular interactions based on Inverse Reinforcement Learning (IRL). To validate the performance of the proposed methodology, a case study of an on-ramp merging scenario is conducted on a uniquely built human-in-the-loop simulation platform that can provide an immersive driving environment, collect data of lane-change behaviors, and test drivers' reactions to the prediction results in real time. It is shown in the simulation results that we can predict the lane-change decision 3 seconds before the vehicle crosses the line to another lane, and the Mean Euclidean Distance between the predicted trajectory and ground truth is 0.39 meters within a 4-second prediction window.