As autonomous systems increasingly rely on onboard sensing for localization and perception, the parallel tasks of motion planning and state estimation become more strongly coupled. This coupling is well-captured by augmenting the planning objective with a posterior-covariance penalty -- however, prediction of the estimator covariance is challenging when the observation model depends on unknown landmarks, as is the case in Simultaneous Localization and Mapping (SLAM). This paper addresses these challenges in the case of landmark- and SLAM-based estimators, enabling efficient prediction (and ultimately minimization) of this performance metric. First, we provide an interval-based filtering approximation of the SLAM inference process which allows for recursive propagation of the ego-covariance while avoiding the quadratic complexity of explicitly tracking landmark uncertainty. Secondly, we introduce a Lie-derivative measurement bundling scheme that simplifies the recursive "bundled" update, representing significant computational savings for high-rate sensors such as cameras. Finally, we identify a large class of measurement models (which includes orthographic camera projection) for which the contributions from each landmark can be directly combined, making evaluation of the information gained at each timestep (nearly) independent of the number of landmarks. This also enables the generalization from finite sets of landmarks $\{\ell^{(n)} \}$ to distributions, foregoing the need for fully-specified linearization points at planning time and allowing for new landmarks to be anticipated. Taken together, these contributions allow SLAM performance to be accurately and efficiently predicted, paving the way for online, observability-aware trajectory optimization in unknown space.
University of Southern California（南加州大学）；Alpen-Adria-Universitat Klagenfurt（克拉根福阿尔卑斯-亚德里亚大学）
We study the nonlinear observability of a systems states in view of how well they are observable and what control inputs would improve the convergence of their estimates. We use these insights to develop an observability-aware trajectory-optimization framework for nonlinear systems that produces trajectories well suited for self-calibration. Common trajectory-planning algorithms tend to generate motions that lead to an unobservable subspace of the system state, causing suboptimal state estimation. We address this problem with a method that reasons about the quality of observability while respe…
Space & Terrestrial Autonomous Robotics Systems (STARS) Laboratory at the University of Toronto Institute for Aerospace Studies and Robotics Institute（多伦多大学航空航天研究所与机器人研究所空间与地面自主机器人系统实验室）；University of Toronto Institute for Aerospace Studies（多伦多大学航空航天研究所）；Robotics Institute（机器人研究所）；Vector Institute（向量研究所）
Ideally, robots should move in ways that maximize the knowledge gained about the state of both their internal system and the external operating environment. Trajectory design is a challenging problem that has been investigated from a variety of perspectives, ranging from information-theoretic analyses to leaning-based approaches. Recently, observability-based metrics have been proposed to find trajectories that enable rapid and accurate state and parameter estimation. The viability and efficacy of these methods is not yet well understood in the literature. In this paper, we compare two state-o…
Observability-Aware Active Calibration of Multi-Sensor Extrinsics for Ground Robots via Online Trajectory Optimization
Shenzhen Key Laboratory of Control Theory and Intelligent Systems（深圳控制理论与智能系统重点实验室）；Guangdong Provincial Key Laboratory of Fully Actuated System Control Theory and Technology（广东省全驱动系统控制理论与技术省级重点实验室）；Southern University of Science and Technology（南方科技大学）
SafeMove-RL: A Certifiable Reinforcement Learning Framework for Dynamic Motion Constraints in Trajectory Planning
Sino-German College of Intelligent Manufacturing, Shenzhen Technology University（深圳技术大学中德智能制造学院）；Faculty of Science and Technology, University of Macau（澳门大学科技学院）
This paper presents Lidar-based Simultaneous Localization and Mapping (SLAM) for autonomous driving vehicles. Fusing data from landmark sensors and a strap-down Inertial Measurement Unit (IMU) in an adaptive Kalman filter (KF) plus the observability of the system are investigated. In addition to the vehicle's states and landmark positions, a self-tuning filter estimates the IMU calibration parameters as well as the covariance of the measurement noise. The discrete-time covariance matrix of the process noise, the state transition matrix, and the observation sensitivity matrix are derived in clo…
Wuhan University（武汉大学）；Wuhan Institute of Quantum Technology（武汉量子技术研究院）；ETH Zürich（苏黎世联邦理工学院）；The Ohio State University（俄亥俄州立大学）
Camera-IMU (Inertial Measurement Unit) sensor fusion has been extensively studied in recent decades. Numerous observability analysis and fusion schemes for motion estimation with self-calibration have been presented. However, it has been uncertain whether both camera and IMU intrinsic parameters are observable under general motion. To answer this question, by using the Lie derivatives, we first prove that for a rolling shutter (RS) camera-IMU system, all intrinsic and extrinsic parameters, camera time offset, and readout time of the RS camera, are observable with an unknown landmark. To our kn…
Humans can routinely follow a trajectory defined by a list of images/landmarks. However, traditional robot navigation methods require accurate mapping of the environment, localization, and planning. Moreover, these methods are sensitive to subtle changes in the environment. In this paper, we propose a Deep Visual MPC-policy learning method that can perform visual navigation while avoiding collisions with unseen objects on the navigation path. Our model PoliNet takes in as input a visual trajectory and the image of the robot's current view and outputs velocity commands for a planning horizon of…
Considering visual localization accuracy at the planning time gives preference to robot motion that can be better localized and thus has the potential of improving vision-based navigation, especially in visually degraded environments. To integrate the knowledge about localization accuracy in motion planning algorithms, a central task is to quantify the amount of information that an image taken at a 6 degree-of-freedom pose brings for localization, which is often represented by the Fisher information. However, computing the Fisher information from a set of sparse landmarks (i.e., a point cloud)…
University of Klagenfurt（克拉根福大学）；ENS（巴黎高等师范学院）；Univ Rennes（雷恩大学）；Inria（法国国家信息与自动化研究所）；IRISA（法国信息与随机系统研究所）；CNRS（法国国家科学研究中心）
In this research, we aim to answer the question: How to combine Closed-Loop State and Input Sensitivity-based with Observability-aware trajectory planning? These possibly opposite optimization objectives can be used to improve trajectory control tracking and, at the same time, estimation performance. Our proposed novel Control & Observability-aware Planning (COP) framework is the first that uses these possibly opposing objectives in a Single-Objective Optimization Problem (SOOP) based on the Augmented Weighted Tchebycheff method to perform the balancing of them and generation of Bézier curve-b…
We present an online landmark selection method for distributed long-term visual localization systems in bandwidth-constrained environments. Sharing a common map for online localization provides a fleet of au- tonomous vehicles with the possibility to maintain and access a consistent map source, and therefore reduce redundancy while increasing efficiency. However, connectivity over a mobile network imposes strict bandwidth constraints and thus the need to minimize the amount of exchanged data. The wide range of varying appearance conditions encountered during long-term visual localization offer…
National Laboratory of Pattern Recognition, Institute of Automation, Chinese Academy of Sciences（中国科学院自动化研究所模式识别国家重点实验室）；School of Artificial Intelligence, University of Chinese Academy of Sciences（中国科学院大学人工智能学院）
Odometer-aided visual-inertial SLAM systems typically have a good performance for navigation of wheeled platforms, while they usually suffer from degenerate cases before the first turning. In this paper, firstly we perform an observability analysis w.r.t. the extrinsic parameters before the first turning, which is a complement of the existing results of observability analyses. Secondly, inspired by the above observability analyses, we propose a bidirectional trajectory computation method, by which the poses before the first turning are refined in the backward computation thread, and the real-t…
Synchronous Observer Design for Landmark-Inertial SLAM with Magnetometer and Intermittent GNSS Measurements
Centre for Systems and Control, Indian Institute of Technology Bombay（印度理工学院孟买分校系统与控制中心）；School of Aerospace, Mechanical and Mechatronic Engineering (AMME), Faculty of Engineering, University of Sydney（悉尼大学工程学院航空航天、机械与机电工程系）
Agentic Reward Modeling: Verifying GUI Agent via Progressive Trajectory-Grounded Interaction
MAIS, Institute of Automation, Chinese Academy of Sciences；School of Artificial Intelligence, University of Chinese Academy of Sciences；School of Computer Science \& Technology, Beijing Jiaotong University
BPDA-GMM: Bayesian Probabilistic Data Association via Gaussian Mixture Models for Semantic SLAM
School of Information Science, Japan Advanced Institute of Science and Technology（信息科学学系，日本科学技术大学）；University of Engineering and Technology, Vietnam National University（工程技术大学，越南国家大学）
Autonomous Systems Lab, ETH Zürich（苏黎世联邦理工学院自主系统实验室）；Alibaba Group（阿里巴巴集团）
External effects such as shocks and temperature variations affect the calibration of visual-inertial sensor systems and thus they cannot fully rely on factory calibrations. Re-calibrations performed on short user-collected datasets might yield poor performance since the observability of certain parameters is highly dependent on the motion. Additionally, on resource-constrained systems (e.g mobile phones), full-batch approaches over longer sessions quickly become prohibitively expensive. In this paper, we approach the self-calibration problem by introducing information theoretic metrics to asse…
This paper proposes a novel active Simultaneous Localization and Mapping (SLAM) method with continuous trajectory optimization over a stochastic robot dynamics model. The problem is formalized as a stochastic optimal control over the continuous robot kinematic model to minimize a cost function that involves the covariance matrix of the landmark states. We tackle the problem by separately obtaining an open-loop control sequence subject to deterministic dynamics by iterative Covariance Regulation (iCR) and a closed-loop feedback control under stochastic robot and covariance dynamics by Linear Qu…
Autonomous navigation requires an accurate model or map of the environment. While dramatic progress in the prior two decades has enabled large-scale SLAM, the majority of existing methods rely on non-linear optimization techniques to find the MLE of the robot trajectory and surrounding environment. These methods are prone to local minima and are thus sensitive to initialization. Several recent papers have developed optimization algorithms for the Pose-Graph SLAM problem that can certify the optimality of a computed solution. Though this does not guarantee a priori that this approach generates …
Positioning with one inertial measurement unit and one ranging sensor is commonly thought to be feasible only when trajectories are in certain patterns ensuring observability. For this reason, to pursue observable patterns, it is required either exciting the trajectory or searching key nodes in a long interval, which is commonly highly nonlinear and may also lack resilience. Therefore, such a positioning approach is still not widely accepted in real-world applications. To address this issue, this work first investigates the dissipative nature of flying robots considering aerial drag effects an…
Jet Propulsion Laboratory（喷气推进实验室）；California Institute of Technology（加州理工学院）
There has been an increase in interest in missions that drive significantly longer distances per day than what has currently been performed. Further, some of these proposed missions require autonomous driving and absolute localization in darkness. For example, the Endurance A mission proposes to drive 1200km of its total traverse at night. The lack of natural light available during such missions limits what can be used as visual landmarks and the range at which landmarks can be observed. In order for planetary rovers to traverse long ranges, onboard absolute localization is critical to the abi…
Proprioceptive localization refers to a new class of robot egocentric localization methods that do not rely on the perception and recognition of external landmarks. These methods are naturally immune to bad weather, poor lighting conditions, or other extreme environmental conditions that may hinder exteroceptive sensors such as a camera or a laser ranger finder. These methods depend on proprioceptive sensors such as inertial measurement units (IMUs) and/or wheel encoders. Assisted by magnetoreception, the sensors can provide a rudimentary estimation of vehicle trajectory which is used to query…
Australian Centre for Field Robotics, University of Sydney（悉尼大学澳大利亚野外机器人中心）；University Institute for Computer Research, Universidad de Alicante（阿利坎特大学计算机研究所）
Autonomous vehicle navigation in shared pedestrian environments requires the ability to predict future crowd motion both accurately and with minimal delay. Understanding the uncertainty of the prediction is also crucial. Most existing approaches however can only estimate uncertainty through repeated sampling of generative models. Additionally, most current predictive models are trained on datasets that assume complete observability of the crowd using an aerial view. These are generally not representative of real-world usage from a vehicle perspective, and can lead to the underestimation of unc…
A framework for online simultaneous localization, mapping and self-calibration is presented which can detect and handle significant change in the calibration parameters. Estimates are computed in constant-time by factoring the problem and focusing on segments of the trajectory that are most informative for the purposes of calibration. A novel technique is presented to detect the probability that a significant change is present in the calibration parameters. The system is then able to re-calibrate. Maximum likelihood trajectory and map estimates are computed using an asynchronous and adaptive o…
