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

TL;DR

This paper introduces a finite-time stable pose estimator for rigid bodies that operates directly on the Lie group SE(3), utilizing point cloud and velocity sensors, with proven stability and robustness demonstrated through simulations and experiments.

Contribution

It develops a full-state pose observer on SE(3) that guarantees finite-time stability without local coordinates, and extends it to use only point clouds and angular velocities.

Findings

The estimator is robust to measurement noise.

It outperforms existing methods like EKF and VPE in simulations.

Experimental validation confirms stability and robustness.

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.