[论文解读] Rigid Body Localization via Gaussian Belief Propagation with Quadratic Angle Approximation

Niclas Führling, Hyeon Seok Rou+3 more|arXiv (Cornell University)|Feb 4, 2026

Robotics and Sensor-Based Localization被引用 0

一句话总结

该论文提出一种基于高斯置信传播（GaBP）的刚体定位方法，使用一种新颖的二次角度近似，使在不依赖小角度先验的情况下实现准确的旋转和平移估计，同时保持较低的计算复杂度。

ABSTRACT

Gaussian belief propagation (GaBP) is a technique that relies on linearized error and input-output models to yield low-complexity solutions to complex estimation problems, which has been recently shown to be effective in the design of range-based GaBP schemes for stationary and moving rigid body localization (RBL) in three-dimensional (3D) space, as long as an accurate prior on the orientation of the target rigid body is available. In this article we present a novel range-based RBL scheme via GaBP that removes the latter limitation. To this end, the proposed method incorporates a quadratic angle approximation to linearize the relative orientation between the prior and the target rigid body, enabling high precision estimates of corresponding rotation angles even for large deviations. Leveraging the resulting linearized model, we derive the corresponding message-passing (MP) rules to obtain estimates of the translation vector and rotation matrix of the target rigid body, relative to a prior reference frame. Numerical results corroborate the good performance of the proposed angle approximation itself, as well as the consequent RBL performance in terms of root mean square errors (RMSEs) in comparison to the state-of-the-art (SotA), while maintaining a low computational complexity

研究动机与目标

• 为基于距离的刚体定位（RBL）提供无需依赖准确的初始姿态先验的动机。
• 引入二次角度近似以对旋转在较大角度偏差时进行线性化。
• 推导一个线性化的系统模型，使GaBP能够联合估计平移和旋转。
• 开发双线性GaBP算法和干扰抑制策略，鲁棒地估计RBL参数。
• 在 RMSE 和计算效率方面与最新方法进行性能对比评估。

提出的方法

• 提出用于旋转矩阵的二次角度近似，以在更大角度范围内实现线性化。
• 利用先验角参数化（α、β、γ、δ、L、D、Θ）将旋转向量化为仿射线性形式。
• 推导线性化测量模型，使测量距离通过更新的 Hθ 和 Ht 矩阵将刚体参数（旋转 θ 和平移 t）联系起来。
• 开发带软干扰消除的双线性GaBP算法，联合估计旋转和平移，并在平移出估计的 t 之后对旋转进行额外的细化。
• 给出算法1（RBL参数估计的双GaBP），概述迭代的GaBP步骤以及用于旋转和平移的两轮细化。

实验结果

研究问题

• RQ1 GaBP 是否能够在基于距离的设置中联合估计刚体平移和旋转，而不依赖小角度近似？
• RQ2 相较于传统的小角度方法，二次角度近似是否在较大姿态变化时提升旋转估计的准确性？
• RQ3 与最先进方法相比，所提出的GaBP基RBL在RMSE和计算复杂度方面的表现如何？
• RQ4 干扰消除步骤对GaBP中旋转参数估计的准确性有何影响？

主要发现

• 二次角度近似显著降低了与传统小角度近似相比的旋转模型误差，尤其对角度约在20–45度范围内效果明显。
• 线性化模型使GaBP能够在具有竞争性准确度和低计算复杂度的情况下同时估计旋转和平移。
• 带干扰消除的双GaBP程序通过解耦平移效应提高了旋转估计的准确性。
• 数值结果支持所提方法相对于依赖小角度近似的前沿GaBP基RBL方法具有良好性能。
• 该方法在准确性与复杂度之间保持有利的权衡，适用于实时或资源受限场景。