­ ­­ ­ ­ ­ ­ ­ ­ ­移动机器人的位姿(位置与姿态)估计功能是实现避障、路径规划等任务的基本前提。近年来，研究者们致力于运用惯性传感单元(Inertial Measurement Unit, IMU)、相机和激光雷达等机载传感器实现高性能和实时的状态估计。然而，基于机载传感器的位姿估计模块容易产生长期轨迹的漂移问题，并且在特定环境下鲁棒性较低。

ICRA Poster

­ ­­ ­ ­ ­ ­ ­ ­ ­近日，香港中文大学（深圳）数据科学学院吴均峰教授团队在机器人学顶级会议ICRA (IEEE International Conference on Robotics and Automation，机器人学领域最权威的会议之一) 发表题为 Efficient Planar Pose Estimation via UWB Measurements的文章，采用UWB超宽带无线电技术（LinkTrack UWB高精度测距、定位）的距离测量功能，实现了平面移动机器人的实时高精度位姿估计，有效避免了上述问题。在0.5m/s平均速度下的实验：GN-ULS估计器的位置估计平均误差为3.01cm，角度估计平均误差为3.97°。（文末附下载链接）

­ ­­ ­ ­ ­ ­ ­ ­ ­UWB技术被广泛应用于移动机器人的定位，但鲜有工作运用UWB技术进行移动机器人的位姿估计。团队将这一问题建模为信号处理领域的刚体定位问题(Rigid Body Localization, RBL)。通过文献调研，团队发现以往文献缺少对估计器的的统计渐进有效性分析，同时所提出算法的计算复杂度较高。

基于UWB距离测量的机器人平面位姿估计示意图

两步法估计框架

­ ­­ ­ ­ ­ ­ ­ ­ ­本工作基于两步法提出了位姿估计器GN-ULS：

• （1）我们首先通过变换观测方程、参数化旋转矩阵等步骤将最大似然问题松弛为一个线性最小二乘问题，并证明了其最小二乘解是真实位姿的一致估计
• （2）在此基础上，我们将第一步得到的位姿估计投影到特殊正交群，通过高斯-牛顿算法改进估计值。我们证明了一步高斯-牛顿算法的结果收敛于最大似然估计。

­ ­­ ­ ­ ­ ­ ­ ­ ­总结来说，本工作所提出的估计器具有以下优点：1）随着距离测量数量的增加，位姿估计以理论最快速率收敛到真实位姿(即平均均方误差达到克拉梅洛下界)；2）计算复杂度与以往研究相比有显著提升，实现了动态位姿的实时求解；3）给出了位姿估计的协方差矩阵。此外，本研究还对基于距离测量的移动机器人平面位姿估计问题的可观性等基础问题进行了讨论和研究，运用图刚性理论给出了可观性的充要条件。

在仿真实验中，我们测试了不同估计器在不同传感器数目、重复观测次数以及噪声方差下的性能。结果如下图所示。本研究所提出的GN-ULS估计器在不同仿真设定下均能达到理论最优下界，而以往方法会在特定场景中偏离理论最优下界。

仿真实验结果

实验环境及小车实验平台

­ ­­ ­ ­ ­ ­ ­ ­ ­在真实场景下，我们基于NVIDIA-TX2等硬件设备搭建了小车实验平台，运用动作捕捉系统实时提供小车位姿真值，在静态与动态场景下采集了UWB距离测量的数据，实验环境及小车平台如下图所示。

­ ­­ ­ ­ ­ ­ ­ ­ ­由于室内环境中障碍物遮挡、墙壁反射等因素，真实实验中UWB距离测量数据受离群值影响，我们在实验中通过对相邻时刻距离测量变化量设定阈值对测量离群值进行了剔除。我们在不同位置与位姿下收集了42组静态测量数据，实验结果如下图所示。在本实验中，噪声的方差为毫米级，所提GN-ULS估计器与其他方法的准确度相近，其优势体现在计算复杂度：GN-ULS可以在1毫秒内处理上万距离测量值；而以往方法的耗时在0.1s左右。

静态位姿估计实验结果

动态位姿估计实验结果

­ ­­ ­ ­ ­ ­ ­ ­ ­我们控制小车以0.5m/s，0.2m/s，0.1m/s的平均速度在室内环境中移动，收集了UWB距离测量数据与位姿真值。在0.5m/s平均速度下的实验结果如下图所示，GN-ULS估计器的位置估计平均误差为3.01cm，角度估计平均误差为3.97°；其估计轨迹的准确性与光滑性有望通过结合里程计测量与运动模型得到进一步提高。

­ ­­ ­ ­ ­ ­ ­ ­ ­本工作对基于UWB距离测量的机器人平面位姿估计问题展开了研究。我们基于两步法设计了一个渐进有效且计算复杂度低的位姿估计器，GN-ULS。仿真与实验表明，GN-ULS在计算效率、鲁棒性以及大噪声环境下的准确性等方面优于以往工作。在这项工作中，我们展示了单独使用距离测量值对机器人位姿进行实时、准确估计的可能性。

• 论文链接：https://arxiv.org/abs/2209.06779
• 江昊东博士所提供素材