Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Finite-time Stable Pose Estimation on TSE(3) using Point Cloud and Velocity Sensors

Nazanin S. Hashkavaei, Abhijit Dongare, Neon Srinivasu, Amit K. Sanyal

TL;DR

This work develops a finite-time stable pose estimator (FTS-PE) for rigid bodies on the Lie group $SE(3)$ using 3D point-cloud measurements and velocity data. It derives a full-state observer directly on $SE(3)$, backed by a Morse-Lyapunov analysis that guarantees almost global finite-time convergence in the noise-free case and bounded convergence under bounded noise, with a geometric variational integration discretization. A robustness analysis shows the estimator remains convergent to a neighborhood under velocity-noise bounds, and a translation-velocity-free variant is provided via a finite-time stable filter. Numerical simulations and a ZED 2i experiment validate fast, robust convergence and favorable comparison to existing estimators (VPE, DQ-MEKF), highlighting practical applicability for autonomous vehicles without GNSS.

Abstract

This work presents a finite-time stable pose estimator (FTS-PE) for rigid bodies undergoing rotational and translational motion in three dimensions, using measurements from onboard sensors that provide position vectors to inertially-fixed points and body velocities. The FTS-PE is a full-state observer for the pose (position and orientation) and velocities and is obtained through a Lyapunov analysis that shows its stability in finite time and its robustness to bounded measurement noise. Further, this observer is designed directly on the state space, the tangent bundle of the Lie group of rigid body motions, SE(3), without using local coordinates or (dual) quaternion representations. Therefore, it can estimate arbitrary rigid body motions without encountering singularities or the unwinding phenomenon and be readily applied to autonomous vehicles. A version of this observer that does not need translational velocity measurements and uses only point clouds and angular velocity measurements from rate gyros, is also obtained. It is discretized using the framework of geometric mechanics for numerical and experimental implementations. The numerical simulations compare the FTS-PE with a dual-quaternion extended Kalman filter and our previously developed variational pose estimator (VPE). The experimental results are obtained using point cloud images and rate gyro measurements obtained from a Zed 2i stereo depth camera sensor. These results validate the stability and robustness of the FTS-PE.

Finite-time Stable Pose Estimation on TSE(3) using Point Cloud and Velocity Sensors

TL;DR

This work develops a finite-time stable pose estimator (FTS-PE) for rigid bodies on the Lie group using 3D point-cloud measurements and velocity data. It derives a full-state observer directly on , backed by a Morse-Lyapunov analysis that guarantees almost global finite-time convergence in the noise-free case and bounded convergence under bounded noise, with a geometric variational integration discretization. A robustness analysis shows the estimator remains convergent to a neighborhood under velocity-noise bounds, and a translation-velocity-free variant is provided via a finite-time stable filter. Numerical simulations and a ZED 2i experiment validate fast, robust convergence and favorable comparison to existing estimators (VPE, DQ-MEKF), highlighting practical applicability for autonomous vehicles without GNSS.

Abstract

This work presents a finite-time stable pose estimator (FTS-PE) for rigid bodies undergoing rotational and translational motion in three dimensions, using measurements from onboard sensors that provide position vectors to inertially-fixed points and body velocities. The FTS-PE is a full-state observer for the pose (position and orientation) and velocities and is obtained through a Lyapunov analysis that shows its stability in finite time and its robustness to bounded measurement noise. Further, this observer is designed directly on the state space, the tangent bundle of the Lie group of rigid body motions, SE(3), without using local coordinates or (dual) quaternion representations. Therefore, it can estimate arbitrary rigid body motions without encountering singularities or the unwinding phenomenon and be readily applied to autonomous vehicles. A version of this observer that does not need translational velocity measurements and uses only point clouds and angular velocity measurements from rate gyros, is also obtained. It is discretized using the framework of geometric mechanics for numerical and experimental implementations. The numerical simulations compare the FTS-PE with a dual-quaternion extended Kalman filter and our previously developed variational pose estimator (VPE). The experimental results are obtained using point cloud images and rate gyro measurements obtained from a Zed 2i stereo depth camera sensor. These results validate the stability and robustness of the FTS-PE.
Paper Structure (20 sections, 8 theorems, 81 equations, 8 figures, 2 tables)

This paper contains 20 sections, 8 theorems, 81 equations, 8 figures, 2 tables.

Table of Contents

  1. Introduction
  2. Mathematical Preliminaries
  3. Real-time Navigation using 3D Point Cloud Data
  4. Pose Measurement Model
  5. Velocities Measurement Model
  6. Preliminary Concepts for Pose Estimation on $\SE$
  7. Potential Functions for Pose Estimation Errors
  8. Useful Prior Results
  9. Finite-time Stable Pose Estimation
  10. Robustness and Pose Estimation without Translational Velocity Measurements
  11. Robustness Analysis
  12. FTS Pose Estimator Implementation without Translational Velocity Measurements
  13. Selected Pose Estimation Schemes for Comparison
  14. Variational Pose Estimatior
  15. DQ-MEKF estimator
  16. ...and 5 more sections

Key Result

Lemma 1

Define $Q = R\widehat{R}\T$ as the attitude estimation error. Let $D \in \bR^{3 \times n}$ be as defined as in D_def with $rank(D) =3$. Let the gain matrix $W$ of the generalization of Wahba's cost function be given by, where $K = \diag([k_1, k_2, k_3])$ and $k_1 > k_2 > k_3 \geq 1$. Then, in the absence of measurement errors, which is a Morse function on $\SO$ whose critical points are elements

Figures (8)

  • Figure 1: True and Estimated Trajectories.
  • Figure 2: Plots of attitude and position estimation errors in the absence of measurement noise.
  • Figure 3: Plots of attitude and position estimation errors in the presence of measurement noise in velocities.
  • Figure 4: Plots of estimation errors in velocities in the presence of measurement noise in velocities.
  • Figure 5: Plots of bounded measurement noise in velocities.
  • ...and 3 more figures

Theorems & Definitions (9)

  • Lemma 1
  • Lemma 2
  • Lemma 3
  • Lemma 4
  • Theorem 1
  • Corollary 1
  • Remark 1
  • Proposition 2
  • Proposition 3