Robot State Estimation Research Articles

Online tree-based planning for active spacecraft fault estimation and collision avoidance.

Autonomous robots operating in uncertain or hazardous environments subject to state safety constraints must be able to identify and isolate faulty components in a time-optimal manner. When the underlying fault is ambiguous and intertwined with the robot's state estimation, motion plans that discriminate between simultaneous actuator and sensor faults are necessary. However, the coupled fault mode and physical state uncertainty creates a constrained optimization problem that is challenging to solve with existing methods. We combined belief-space tree search, marginalized filtering, and concentration inequalities in our method, safe fault estimation via active sensing tree search (s-FEAST), a planner that actively diagnoses system faults by selecting actions that give the most informative observations while simultaneously enforcing probabilistic state constraints. We justify this approach with theoretical analysis showing s-FEAST's convergence to optimal policies. Using our robotic spacecraft simulator, we experimentally validated s-FEAST by safely and successfully performing fault estimation while on a collision course with a model comet. These results were further validated through extensive numerical simulations demonstrating s-FEAST's performance.

Set-valued rigid-body dynamics for simultaneous, inelastic, frictional impacts

Robotic manipulation and locomotion often entail nearly-simultaneous collisions—such as heel and toe strikes during a foot step—with outcomes that are extremely sensitive to the order in which impacts occur. Robotic simulators and state estimation commonly lack the fidelity and accuracy to predict this ordering, and instead pick one with a heuristic. This discrepancy degrades performance when model-based controllers and policies learned in simulation are placed on a real robot. We reconcile this issue with a set-valued rigid-body model which generates a broad set of outcomes to simultaneous frictional impacts with any impact ordering. We first extend Routh’s impact model to multiple impacts by reformulating it as a differential inclusion (DI), and show that any solution will resolve all impacts in finite time. By considering time as a state, we embed this model into another DI which captures the continuous-time evolution of rigid-body dynamics, and guarantee existence of solutions. We finally cast simulation of simultaneous impacts as a linear complementarity problem (LCP), and develop an algorithm for tight approximation of the post-impact velocity set with probabilistic guarantees. We demonstrate our approach on several examples drawn from manipulation and legged locomotion, and compare the predictions to other models of rigid and compliant collisions.

Enhancing accuracy in field mobile robot state estimation with GNSS and encoders

The enhancement of accuracy remains a significant concern in state estimation for field mobile robots. This paper introduces a novel sensor fusion method to refine the accuracy of state estimation, employing global navigation satellite systems (GNSS) and encoders. This technique integrates a nonlinear observer for estimating the state of nonlinear systems and an unscented Kalman filter (UKF) to ascertain the robot’s state variables using signals from GNSS and encoder-based position sensors. In the proposed method, the nonlinear observer updates sigma points through nonlinear transformation and the UKF optimizes state estimation. Parameters are optimized through the particle swarm optimization method with the objective of minimizing the root mean square error of the state. To demonstrate performance, numerical simulations compare the proposed method with well-known nonlinear estimation algorithms, illustrating its effectiveness in field mobile robot state estimation. The method is further validated through implementation on a real-world mobile robot platform.

Consensus-Based Information Filtering in Distributed LiDAR Sensor Network for Tracking Mobile Robots.

A distributed state observer is designed for state estimation and tracking of mobile robots amidst dynamic environments and occlusions within distributed LiDAR sensor networks. The proposed novel framework enhances three-dimensional bounding box detection and tracking utilizing a consensus-based information filter and a region of interest for state estimation of mobile robots. The framework enables the identification of the input to the dynamic process using remote sensing, enhancing the state prediction accuracy for low-visibility and occlusion scenarios in dynamic scenes. Experimental evaluations in indoor settings confirm the effectiveness of the framework in terms of accuracy and computational efficiency. These results highlight the benefit of integrating stationary LiDAR sensors' state estimates into a switching consensus information filter to enhance the reliability of tracking and to reduce estimation error in the sense of mean square and covariance.

Advancing robust state estimation of wheeled robots in degenerate environments: harnessing ground manifold and motion states

State estimation is crucial for enabling autonomous mobility in mobile robots. However, traditional localization methods often falter in degraded environments, including issues like visual occlusion, lidar performance degradation, and global navigation satellite system signal interference. This paper presents a novel estimation approach for wheeled robots, exclusively utilizing proprioceptive sensors such as encoders and inertial measurement units (IMU). Initially, the motion manifolds extracted from the historical trajectories are used to assist the encoder in realizing the orientation estimation. Furthermore, a hybrid neural network is designed to categorize the robot’s operational state, and the corresponding pseudo-constraints are added to improve the estimation accuracy. We utilize an error state Kalman filter for the encoder and IMU data fusion. Lastly, comprehensive testing is conducted using both datasets and real-world robotic platforms. The findings underscore that the integration of manifold and motion constraints within our proposed state estimator substantially elevates accuracy compared to conventional approaches. Compare with the methods commonly used in engineering, the accuracy of this method is improved by more than 20%. Crucially, this methodology enables dependable estimation even in degraded environments.

Unified Shape and External Load State Estimation for Continuum Robots.

Continuum robots navigate narrow, winding passageways while safely and compliantly interacting with their environments. Sensing the robot's shape under these conditions is often done indirectly, using a few coarsely distributed (e.g. strain or position) sensors combined with the robot's mechanics-based model. More recently, given high-fidelity shape data, external interaction loads along the robot have been estimated by solving an inverse problem on the mechanics model of the robot. In this paper, we argue that since shape and force are fundamentally coupled, they should be estimated simultaneously in a statistically principled approach. We accomplish this by applying continuous-time batch estimation directly to the arclength domain. A general continuum robot model serves as a statistical prior which is fused with discrete, noisy measurements taken along the robot's backbone. The result is a continuous posterior containing both shape and load functions of arclength, as well as their uncertainties. We first test the approach with a Cosserat rod, i.e. the underlying modeling framework that is the basis for a variety of continuum robots. We verify our approach numerically using distributed loads with various sensor combinations. Next, we experimentally validate shape and external load errors for highly concentrated force distributions (point loads). Finally, we apply the approach to a tendon-actuated continuum robot demonstrating applicability to more complex actuated robots.

Center of Mass Dynamics and Contact-Aided Invariant Filtering for Biped Robot State Estimation

Center of Mass Dynamics and Contact-Aided Invariant Filtering for Biped Robot State Estimation

Mode switching in organisms for solving explore-versus-exploit problems

Trade-offs between producing costly movements for gathering information (‘explore’) and using previously acquired information to achieve a goal (‘exploit’) arise in a wide variety of problems, including foraging, reinforcement learning and sensorimotor control. Determining the optimal balance between exploration and exploitation is computationally intractable, necessitating heuristic solutions. Here we show that the electric fish Eigenmannia virescens uses a salience-dependent mode-switching strategy to solve the explore–exploit conflict during a refuge-tracking task in which the same category of movement (fore-aft swimming) is used for both gathering information and achieving task goals. The fish produced distinctive non-Gaussian distributions of movement velocities characterized by sharp peaks for slower, task-oriented ‘exploit’ movements and broad shoulders for faster ‘explore’ movements. The measures of non-normality increased with increased sensory salience, corresponding to a decrease in the prevalence of fast explore movements. We found the same sensory salience-dependent mode-switching behaviour across ten phylogenetically diverse organisms, from amoebae to humans, performing tasks such as postural balance and target tracking. We propose a state-uncertainty-based mode-switching heuristic that reproduces the distinctive velocity distribution, rationalizes modulation by sensory salience and outperforms the classic persistent excitation approach while using less energy. This mode-switching heuristic provides insights into purposeful exploratory behaviours in organisms, as well as a framework for more efficient state estimation and control of robots.

State estimation of hydraulic quadruped robots using invariant-EKF and kinematics with neural networks

State estimation of hydraulic quadruped robots using invariant-EKF and kinematics with neural networks

Offline GNSS/Camera Extrinsic Calibration Using RTK and Fiducials

Accurate robotic state estimation often requires precise knowledge of inter-sensor offsets. In this paper we present a solution for inter-sensor calibration of systems that employ a combination of GNSS and visual-inertial sensors. RTK-GNSS and fiducial measurements are utilized to produce highly precise estimates. We present an offline batch estimation approach that utilizes a continuous-time spline on the Lie group SE(3). This approach simultaneously estimates the vehicle trajectory and sensor spatio-temporal offsets by choosing optimal spline control points and calibration parameters to solve a maximum likelihood estimation problem. Our calibration method is validated in both simulation and hardware, with comparison to an online extended Kalman filter. Hardware experiments are conducted in both a motion capture environment and outdoors. Results show that our method outperforms online methods and approaches millimeter-level accuracy.

State estimation and traversability map construction method of a quadruped robot on soft uneven terrain

AbstractQuadruped robots show excellent application prospects in complex environment detection and rescue. At present, scholars mainly focus on quadruped walking in rigid environments. However, quadruped robots often need to pass through uneven and soft unconstructed terrains, prone to slip and impact. The mismatch between the planned foothold position and the real one resulting from environmental uncertainties makes the robot unstable. In this paper, the state estimation and traversability map construction methods are proposed for quadruped robots to achieve stable walking in an unstructured environment, especially on soft terrains. First, the Error‐state Kalman Filter (ErKF) is extended by optimizing the leg odometry information to get an accurate robot state, especially in soft, uneven terrain. The ErKF method fuses the sensor data from the inertial measurement unit, laser, camera, and leg odometry. The leg odometry is optimized by considering the foot slippage, which easily occurs in soft uneven terrains. Then, the unstructured environment is parameterized and modeled by the terrain inclination, roughness, height, and stiffness. A traversability map, which is essential for robot path and foothold planning in autonomous movement, is constructed with the above parameters. Finally, the proposed method is verified by simulation and experiments. The results show that the quadruped robot can walk stably on different soft and uneven terrains.

Multidimensional Particle Filter for Long-Term Visual Teach and Repeat in Changing Environments

When a mobile robot is asked to navigate intelligently in an environment, it needs to estimate its own and the environment's state. One of the popular methods for robot state and position estimation is particle filtering (PF). Visual Teach and Repeat (VT&R) is a type of navigation that uses a camera to navigate the robot along the previously traversed path. Particle filters are usually used in VT&R to fuse data from odometry and camera to estimate the distance traveled along the path. However, in VT&R, there are other valuable states that the robot can benefit from, especially when moving through changing environments. We propose a multidimensional particle filter to estimate the robot state in VT&R navigation. Apart from the traveled distance, our particle filter estimates lateral and heading deviation from the taught path as well as the current appearance of the environment. This appearance is estimated using maps created across various environmental conditions recorded during the previous traversals. The joint state estimation is based on contrastive neural network architecture, allowing self-supervised learning. This architecture can process multiple images in parallel, alleviating the potential overhead caused by computing the particle filter over the maps simultaneously. We conducted experiments to show that the joint robot/environment state estimation improves navigation accuracy and robustness in a continual mapping setup. Unlike the other frameworks, which treat the robot position and environment appearance separately, our PF represents them as one multidimensional state, resulting in a more general uncertainty model for VT&R.

Tightly Coupled 3D Lidar Inertial SLAM for Ground Robot

This paper proposes a robotic state estimation and map construction method. The traditional lidar SLAM methods are affected by sensor measurement noise, which causes the estimated trajectory to drift, especially along the altitude direction caused by lidar noise. In this paper, ground parameters in the environment are extracted to construct the ground factors to compress the trajectory estimation drifting along the altitude direction using the characteristics of constant robot pose relative to the ground. Our method uses tightly coupled lidar and inertial to obtain low-drift lidar odometry factors by factor graph optimization. The optimized lidar odometry factors are then added to a global factor graph, together with ground, loop closure, and GPS factors to obtain accurate robot state estimation and mapping after factor graph optimization. The experimental results show that our method has comparable results with advanced lidar SLAM methods, and even performs better in some complex and large-scale environments.

Extended Kalman Filter State Estimation for Autonomous Competition Robots

Autonomous mobile robot competitions judge based on a robot’s ability to quickly and accurately navigate the game field. This means accurate localization is crucial for creating an autonomous competition robot. Two common localization methods are odometry and computer vision landmark detection. Odometry provides frequent velocity measurements, while landmark detection provides infrequent position measurements. The state can also be predicted with a physics model. These three types of localization can be “fused” to create a more accurate state estimate using an Extended Kalman Filter (EKF). The EKF is a nonlinear full-state estimator that approximates the state estimate with the lowest covariance error when given the sensor measurements, the model prediction, and their variances. In this paper, we demonstrate the effectiveness of the EKF by implementing it on a 4-wheel mecanum-drive robot simulation. The position and velocity accuracy of fusing together various combinations of these three data sources are compared. We also discuss the assumptions and limitations of an EKF.

Extended Kalman Filter State Estimation for Autonomous Competition Robots

Autonomous mobile robot competitions judge based on a robot’s ability to quickly and accurately navigate the game field. This means accurate localization is crucial for creating an autonomous competition. Two common localization methods are odometry and computer vision landmark detection. Odometry provides frequent velocity measurements, while landmark detection provides infrequent position measurements. The state can also be predicted with a physics model. These three types of localization can be “fused” to create a more accurate state estimate using an Extended Kalman Filter (EKF). The EKF is a nonlinear full-state estimator that approximates the state estimate with the lowest covariance error when given the sensor measurements, the model prediction, and their variances. In this paper, we demonstrate the effectiveness of the EKF by implementing it on a 4-wheel mecanum-drive robot simulation. The position and velocity accuracy of fusing together various combinations of these three data sources are compared. We also discuss the assumptions and limitations of an EKF.

Invariant Cubature Kalman Filtering-Based Visual-Inertial Odometry for Robot Pose Estimation

To maintain mechanistic stability while tracking the designated walking route, a robot must be cognizant of employed posture. Generally, visual-inertial odometer (VIO) is utilized for robot state estimation; however, the traditional cubature Kalman filter VIO (CKF-VIO) cannot transfer rotational uncertainty and compensate for the system’s processing error. To effectively improve the accuracy and stability of robot rigid body pose estimation, this paper proposes a matrix Lie group representation-based CKF framework which characterizes the uncertainty prompting in robotic motion while eliminating the VIO system internalization errors. The robot state, consisting of inertial measurement unit (IMU) pose, velocity, and 3-D landmarks’ positions, is deemed to be a single element of a high-dimensional Lie group SE2+p <xref ref-type="disp-formula" rid="deqn3" xmlns:mml="http://www.w3.org/1998/Math/MathML" xmlns:xlink="http://www.w3.org/1999/xlink">(3)</xref> , while the accelerometers’ and gyrometers’ biases are appended to the state and estimated as well. The algorithm is validated by simulations with Monte Carlo and experiment. Results show that the CKF-VIO with a high-dimensional Lie group can improve the accuracy and consistency of robot pose estimation.

Variational inference as iterative projection in a Bayesian Hilbert space with application to robotic state estimation

AbstractVariational Bayesian inference is an important machine learning tool that finds application from statistics to robotics. The goal is to find an approximate probability density function (PDF) from a chosen family that is in some sense “closest” to the full Bayesian posterior. Closeness is typically defined through the selection of an appropriate loss functional such as the Kullback-Leibler (KL) divergence. In this paper, we explore a new formulation of variational inference by exploiting the fact that (most) PDFs are members of a Bayesian Hilbert space under careful definitions of vector addition, scalar multiplication, and an inner product. We show that, under the right conditions, variational inference based on KL divergence can amount to iterative projection, in the Euclidean sense, of the Bayesian posterior onto a subspace corresponding to the selected approximation family. We work through the details of this general framework for the specific case of the Gaussian approximation family and show the equivalence to another Gaussian variational inference approach. We furthermore discuss the implications for systems that exhibit sparsity, which is handled naturally in Bayesian space, and give an example of a high-dimensional robotic state estimation problem that can be handled as a result. We provide some preliminary examples of how the approach could be applied to non-Gaussian inference and discuss the limitations of the approach in detail to encourage follow-on work along these lines.

Continuum robot state estimation using Gaussian process regression on

Continuum robots have the potential to enable new applications in medicine, inspection, and countless other areas due to their unique shape, compliance, and size. Excellent progress has been made in the mechanical design and dynamic modeling of continuum robots, to the point that there are some canonical designs, although new concepts continue to be explored. In this paper, we turn to the problem of state estimation for continuum robots that can been modeled with the common Cosserat rod model. Sensing for continuum robots might comprise external camera observations, embedded tracking coils, or strain gauges. We repurpose a Gaussian process (GP) regression approach to state estimation, initially developed for continuous-time trajectory estimation in SE(3). In our case, the continuous variable is not time but arclength and we show how to estimate the continuous shape (and strain) of the robot (along with associated uncertainties) given discrete, noisy measurements of both pose and strain along the length. We demonstrate our approach quantitatively through simulations as well as through experiments. Our evaluations show that accurate and continuous estimates of a continuum robot’s shape can be achieved, resulting in average end-effector errors between the estimated and ground truth shape as low as 3.5 mm and 0.016° in simulation or 3.3 mm and 0.035° for unloaded configurations and 6.2 mm and 0.041° for loaded ones during experiments, when using discrete pose measurements.

Quadratic Pose Estimation Problems: Globally Optimal Solutions, Solvability/Observability Analysis, and Uncertainty Description

Pose estimation problems are fundamental in robotics. Most of these problems are challenging due to the nonconvex nature. This also sets up an obstacle for uncertainty description that is essential for pose integration and quality control. In this article, we show that a large class of related problems can be categorized as the quadratic pose estimation problems (QPEPs) and we propose a general quaternion-based mathematical model to unify these problems. To solve the nonconvex QPEPs, a Gröbner-basis method is investigated to derive their globally optimal and robust solutions. Furthermore, we develop the rules for characterizing the solvability and observability of these solutions. In addition, the uncertainty description, i.e., covariance matrix, as an important piece of information in robotic state estimation frameworks, is analyzed in detail. Theoretical results show that the covariance can be estimated via online optimization, in an efficient and unbiased manner. In this way, both the solution and covariance are guaranteed to be globally optimal. Through simulations and experiments, we show that the proposed QPEP-based solver is not only accurate, robust, and efficient but outperforms the representatives for covariance estimation. The designed algorithms are also assembled as a C++/MATLAB/Octave/ROS library, while these developed interfaces are built for main stream platforms and simultaneous localization and mapping schemes.

Progress in symmetry preserving robot perception and control through geometry and learning.

This article reports on recent progress in robot perception and control methods developed by taking the symmetry of the problem into account. Inspired by existing mathematical tools for studying the symmetry structures of geometric spaces, geometric sensor registration, state estimator, and control methods provide indispensable insights into the problem formulations and generalization of robotics algorithms to challenging unknown environments. When combined with computational methods for learning hard-to-measure quantities, symmetry-preserving methods unleash tremendous performance. The article supports this claim by showcasing experimental results of robot perception, state estimation, and control in real-world scenarios.