通过迭代地标匹配实现人形足球机器人的快速鲁棒定位

编辑：陈萍萍的公主@一点人工一点智能

入群邀请：8个专业方向交流群

论文链接：https://export.arxiv.org/pdf/2503.11020

简介

在机器人足球比赛中，特别是RoboCup成人组别中，人形机器人的自我定位是一个关键挑战。由于比赛场地的对称性和不允许使用磁力计的规定，机器人必须依靠视觉信息来确定其位置和方向。为了应对这一难题，本文提出了一种快速且鲁棒的人形足球机器人定位方法——迭代地标匹配（Iterative Landmark Matching, ILM）。该方法利用了直接线性变换（Direct Linear Transformation, DLT）和Kabsh算法进行位姿估计，并通过RANSAC（Random Sample Consensus）算法处理异常值，以提高匹配精度。此外，还结合了扩展卡尔曼滤波器（Invariant Extended Kalman Filter, InEKF），融合IMU数据与定位估计，进一步增强了定位的准确性和稳定性。

实验结果显示，ILM方法能够在1kHz频率下运行，并具有较高的初始姿态和方向容忍度。相比于传统方法，ILM不仅提高了定位的速度，还在复杂环境中展现了更高的鲁棒性。本文详细描述了ILM方法的设计思路、具体实现步骤以及实验验证结果。通过对不同场景下的性能测试，证明了该方法在实际应用中的有效性及其在RoboCup等赛事中的潜在价值。

核心创新：

· 迭代匹配与异常值去除：通过多次迭代修正地标关联，避免粒子滤波的冗余计算。

· 闭式姿态估计：采用Kabsch算法或DLT实现快速闭式解，而非耗时优化方法。

· 多传感器融合：结合IMU数据与视觉定位结果，通过不变扩展卡尔曼滤波（InEKF）提升动态稳定性。

系统概述

本文提出的系统主要解决了人形足球机器人在复杂环境中的定位问题。考虑到比赛场地的对称性和不允许使用磁力计的限制，机器人需要依赖视觉信息来进行自我定位。为此，我们设计了一个基于迭代地标匹配（ILM）的方法，该方法能够高效地计算出机器人的精确位置和方向。首先，我们将讨论系统的总体架构，包括输入输出参数以及初始化过程；然后详细介绍ILM算法的工作流程，强调其如何通过迭代方式逐步优化机器人的姿态估计。

在系统架构方面，ILM接收来自机器人体感传感器的数据，即pbody，以及初始猜测的姿态

作为输入。算法的目标是输出最终估算的姿态

。初始化阶段，将根据初始猜测构建一个世界坐标系下的初步假设

，并设置计数器counter为1。接下来，算法进入主循环，在每次迭代中更新世界坐标系下的位置pworld，再通过位姿估计函数pose_estimation(pworld, pbody)得到新的姿态参数

。如果新计算的姿态与上一次的结果相同，则停止迭代并返回最终结果。否则，继续迭代直至达到最大迭代次数或满足收敛条件。

此方法的核心在于通过多次迭代逐步逼近真实姿态，确保即使在存在噪声的情况下也能获得较为准确的位置估计。同时，通过引入阈值判断机制，可以有效过滤掉不合理的解，保证算法的稳定性和可靠性。此外，结合RANSAC算法处理异常值，进一步提升了匹配的准确性。整体而言，ILM提供了一种既快速又鲁棒的解决方案，适用于实时动态环境下的机器人定位任务。

方法详解

3.1 迭代地标匹配（ILM）流程

ILM的核心思想是通过迭代优化逐步修正地标匹配与姿态估计，其流程如图2所示：

1）初始猜测：基于多假设定位（MHL）生成候选姿态，选择匹配误差最小的假设作为起点。

2）迭代匹配：

步骤1：根据当前姿态估计，将检测到的地标（体坐标系）投影至世界坐标系。

步骤2：通过线性分配问题（LAP）将投影点与地图地标匹配，使用修改版Jonker-Volgenant算法快速求解。

步骤3：利用闭式解（Kabsch或DLT）更新姿态，公式如下：

最小化匹配误差：

步骤4：若平均误差超过阈值（0.5 m），启动RANSAC剔除异常值后重新估计。

3）收敛条件：迭代至误差稳定或达到最大迭代次数（通常4次即可收敛）。

3.2 数学推导

Kabsch算法通过奇异值分解（SVD）求解最优旋转矩阵：

1）计算体坐标系与地图地标的质心：

2）构建协方差矩阵并SVD分解：

3）最优旋转矩阵为：

平移向量由质心差计算：

3.3 全局定位与多假设处理

足球场的对称性可能导致初始姿态多义性（如机器人面朝相反方向）。为此，论文采用多假设定位（MHL）：

• 初始假设生成：在己方半场边界预设6个候选姿态（图4），优先选择匹配地标数≥5且误差<0.5 m的假设。

• 快速收敛性：ILM对初始误差容忍度高（图6），仅需少量迭代即可覆盖86.67%场地范围，降低对初始假设数量的依赖。

3.4 异常值剔除与滤波融合

• RANSAC机制：当地标匹配平均误差超过阈值时，随机采样最小子集（如2个地标）估计姿态，统计内点数量，剔除偏离模型的地标。

• InEKF设计：状态向量包含位置(tx,ty)与航向角θ，运动模型为：

其中控制输入Uk=[vf,vs,ω]T包含前向速度、侧向速度与角速度。观测模型直接融合ILM输出的姿态，通过卡尔曼增益平衡预测与测量值。

综上所述，ILM方法通过结合多种先进的数学工具和技术手段，实现了高效且可靠的机器人定位。无论是地标匹配还是姿态估计，每一个环节都经过精心设计，以确保算法在各种条件下都能表现出色。此外，借助RANSAC算法的强大能力，系统能够有效地抵御异常值带来的干扰，进一步提升了整体性能。

实验验证

为了验证ILM方法的有效性和优越性，我们在多个模拟和实际场景中进行了广泛的实验测试。首先，我们评估了ILM方法的计算效率，特别是在最大允许迭代次数为4的情况下，采用DLT算法的平均求解时间为0.901毫秒，而使用Kabsh算法则为1.132毫秒。这意味着ILM能够在大约1kHz的频率下运行，这对于实时动态环境下的机器人定位至关重要。此外，我们还考察了初始姿态估计的准确性及其对最终定位结果的影响。结果显示，ILM方法对初始位置和方向具有很高的容错能力，即使在较大的误差范围内也能迅速收敛至正确的姿态。

接下来，我们针对多假设定位策略进行了详细的分析。根据RoboCup规则，机器人应在己方半场边缘开始比赛，并面向场地。因此，我们采用多假设定位策略，在比赛开始时尝试识别六个可能的起始姿态。每个假设均需接收到超过5个地标点，并设定匹配误差的最大阈值为0.5米。通过比较各个假设的姿态估计误差，选择误差最小的那个作为初始姿态，其余假设则被舍弃。实验表明，ILM方法的高容错特性意味着不需要过多的初始假设即可获得满意的定位效果，从而简化了计算流程并提高了效率。

最后，我们探讨了异常值剔除的重要性及其在ILM中的具体应用。由于错误匹配会对姿态估计产生负面影响，因此我们引入了RANSAC算法来自动检测并排除异常值。具体来说，当平均匹配误差大于0.5米时，系统会检查当前帧内是否存在足够的地标点（至少5个）。如果有，则利用RANSAC算法找出不符合模型的异常点，并重新计算仅包含内点的姿态。实验结果证实，这种异常值处理机制显著增强了ILM在复杂环境下的鲁棒性，使得定位结果更加准确可靠。

综上所述，通过一系列严格的实验验证，我们证明了ILM方法不仅具备高效的计算性能，还能在复杂的现实环境中保持良好的定位精度和稳定性。这为未来的研究提供了坚实的基础，并展示了其在RoboCup及其他类似竞赛中的巨大潜力。

总结与展望

本文提出的迭代地标匹配（ILM）方法为人形足球机器人在复杂环境中的精确定位提供了一种快速且鲁棒的解决方案。通过结合直接线性变换（DLT）和Kabsh算法进行位姿估计，并利用RANSAC算法处理异常值，ILM显著提高了定位的准确性和稳定性。实验结果表明，该方法不仅在计算效率上表现出色，而且在面对较大初始误差时也展现出高度的容错能力。此外，多假设定位策略的应用进一步增强了系统的灵活性和适应性。

然而，尽管取得了显著进展，ILM方法仍面临一些挑战和改进空间。例如，如何在更为动态和不确定的环境中保持持续稳定的定位性能，仍然是一个值得深入研究的问题。未来工作可以探索将深度学习技术融入现有框架，以提升地标识别的准确性和鲁棒性。同时，开发更加智能的自适应算法，使系统能够根据环境变化自动调整参数，也是提升整体性能的重要方向之一。

总之，ILM方法为解决人形足球机器人定位难题提供了一个有效的途径，并为进一步研究奠定了基础。随着技术的不断进步和完善，相信该方法将在更多实际应用中发挥重要作用，推动机器人足球领域的持续发展。