基于点云与速度传感器,实现三维刚体姿态的有限时间稳定估计。
Finite-time Stable Pose Estimation on TSE(3) using Point Cloud and Velocity Sensors
- 直接在SE(3)流形上设计观测器,避免奇点与展开问题。
- 在有限时间内稳定,对测量噪声具有鲁棒性,可处理任意运动。
- 仅需点云与角速度即可工作,适用于无人机等自主系统。
本文提出一种用于三维刚体旋转与平动运动的有限时间稳定姿态估计算法(FTS-PE),利用机载传感器提供的惯性固定点位置矢量和体速度测量值。该算法为完整状态观测器,可同时估计姿态(位置与方向)及速度,通过李雅普诺夫分析证明其在有限时间内稳定且对有界测量噪声具有鲁棒性。观测器直接构建于刚体运动的李群切丛SE(3)上,不依赖局部坐标或双四元数表示,因此可无奇点地估计任意刚体运动,避免展开现象,适用于自主车辆。进一步,还设计了无需平动速度测量、仅依赖点云与陀螺仪角速度的版本,并采用几何力学框架进行离散化,便于数值与实验实现。数值仿真对比了FTS-PE与双四元数扩展卡尔曼滤波器及先前提出的变分姿态估计算法(VPE)。实验使用Zed 2i立体深度相机获取的点云图像与陀螺仪数据,验证了FTS-PE的稳定性与鲁棒性。
原文摘要 · Abstract (English)
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.
Thank you to arXiv for use of its open access interoperability. PaperDance 不是 arXiv 官方产品;中文卡片由大模型生成,请以原文为准。