分布式测距下的机器人相对定位

基于分布式测距的机器人集群相对定位

摘要

本文研究了多机器人系统中的移动机器人如何保持对环境中邻近机器人的相对位置和相对方向的估计问题。该问题的研究背景是大规模的简单机器人群体，这些机器人仅能测量与邻近机器人之间的距离。我们比较了两种分布式定位算法，它们在计算复杂度和协作需求之间具有不同的权衡。第一种算法不要求机器人协调其运动，而是依赖基于非线性最小二乘的策略，使机器人能够计算邻近机器人的相对位姿。第二种算法借鉴分布式计算理论的方法，协调哪些机器人必须保持静止以及哪些机器人可以移动。这种协调使得机器人能够使用标准的三边测量技术来计算邻近机器人的相对位姿。两种算法均通过理论分析和仿真进行了验证。

1 引言

大多数由机器人集群有效执行的任务都需要机器人了解附近其他机器人的相对位置和方向信息。例如，在集群行为[1]中，机器人利用其邻居的相对方向来控制自身的航向，并利用其邻居的相对位置来确保避障和群体凝聚；在编队控制[2]中，机器人根据其邻居的相对位置调节自身位置以达到期望的构型；在地图构建[3]中，机器人利用其邻居的相对位置和相对方向来解释和融合其他机器人收集的信息。然而，现有的大多数定位研究都需要环境中具有已知位置的地标，或针对单个机器人的定位问题，或需要复杂的计算，或依赖昂贵的传感器。许多感兴趣的实际环境避免使用地标，在群体平台中，计算能力有限，且无法使用大型或昂贵的传感器。

我们研究在一个多机器人系统中，每个机器人仅依靠对附近机器人的距离估计来计算其相对位姿（位置和姿态）的问题。本文研究并比较了适用于大规模廉价且简单机器人的算法。本文所述的算法是完全分布式的，每个机器人执行的计算仅依赖于其局部邻域内的信息。该问题具有持续性，因为对于任何移动机器人而言，在执行过程中其附近的机器人集合及其相对位姿会不断变化。

我们考虑一个通用的问题描述，该描述不要求对机器人执行的运动进行显式控制。具体而言，我们考虑的第一个算法对运动完全不施加任何限制。第二个算法协调哪些机器人处于静止状态、哪些机器人处于移动状态，并以公平且分布式的方式轮换机器人角色。这使得可以将此问题的解决方案与运动控制算法组合起来，从而实现不同的高层行为。此外，我们在机器人群体环境中研究该问题，这对解决方案施加了严格的传感器和计算限制。

在典型的群体平台中，单个机器人的通信、计算和感知能力都相当有限。群体平台中单个机器人的通信限制排除了任何需要在中心节点位置收集大量数据的策略，然而单个机器人的简单性又要求某种形式的协作。此外，单个机器人的计算约束排除了存储和更新复杂的世界模型或其他机器人的可能性。因此，为了充分发掘机器人群体平台的潜力，至关重要的是采用去中心化策略，使单个机器人能够通过局部协作完成全局任务。这类似于昆虫群体中观察到的行为，它们依靠相当原始的局部通信方式，集体完成许多单个个体无法克服的复杂任务。

1.1 相关工作

目前，多机器人定位领域的现有工作大多需要环境中存在静止信标，或要求机器人能够测量除与邻居之间距离之外的其他信息。更重要的是，大多数现有的定位算法针对的是数量较少但功能较强的机器人，并且强调详细的误差模型以防止随时间发生漂移。我们在下文简要描述一些更相关的工作。相比之下，本文研究的问题是为大量仅能感知附近机器人距离的简单机器人提供相对定位服务。在这种情况下，估计值的漂移问题不那么重要，因为该信息主要用于简单的位姿控制，而不是在较长时间内执行路径积分。

仅使用距离传感器的定位问题已受到广泛关注，其中大多数研究集中在基于信标或锚点的定位。利用到具有已知位置的静止信标的连接信息[4]，可以近似估计移动节点的位置。当可获得到已知位置信标的距离测量值时（例如通过超声波），Cricket定位支持系统能够在预定义区域内对移动节点进行定位，并且通过扩展类似的设置，已经证明可以获得更精细的位置信息[5]。文献中也考虑了具有未知初始位置的固定静止信标这一更一般的情况[6, 7]。

基于刚体性理论的鲁棒四边形算法[8]，是少数仅依赖于机器人之间距离感知的无信标定位方法之一，且在思想上与本文工作最为接近。然而，该鲁棒四边形算法主要是为静态传感器网络设计的，无法恢复机器人的相对方向。

最近，也有研究提出了该问题的全局最优解[9]，将定位问题建模为加权最小二乘估计问题，并提出了与本文所述第一种算法思想相似的算法。

关于协同定位问题，也有大量相关研究。在协同定位方面的早期研究之一[10]要求使用方位和（可选）测距传感器，并提出了一种方法，即将机器人分为两组：一组允许移动并使用里程计，另一组则充当静止信标。[11]中描述的方法采用了类似的思路，使用测距传感器，但需要全局全对全通信以及对地标进行感知。这两种方法都忽略了选择哪些机器人作为静止信标、哪些机器人允许移动这一分布式协调问题。

在同步定位与建图的背景下，蒙特卡洛定位（MCL）方法已被证明可通过两个或多个机器人的协作来提高定位精度[12]。扩展卡尔曼滤波器（EKF）也被用于实现类似效果[13]。这两项研究均针对配备了能够测量与其他机器人之间的角度和距离的传感器，以及感知环境的传感器的机器人。

MCL 和 EKF 方法在近期的研究中被进一步扩展和改进。例如，[14]将[13]中描述的EKF方法进行了扩展，以考虑更弱形式的传感器，包括仅距离感知，而[15]描述了如何减少所需的状态和通信量。EKF的计算复杂度在[16]中进一步降低，并且[17]描述了一种通信带宽感知解决方案。类似地，也提出了减少MCL方法计算成本的新技术，例如[18]描述了一种聚类技术，以最小化所需的状态和通信量。

2 系统模型

设 $V$ 为部署在平面环境中的机器人集合。机器人 $u \in V$ 在时间 $t \in R^+$ 的位姿（即运动学状态）由元组 $\text{pose}_u^t = \langle p_u^t, \varphi_u^t \rangle$ 描述，其中 $p_u^t \in R^2$ 表示机器人 $u$ 在时间 $t$ 的位置，而 $\varphi_v^t \in [0, 2\pi)$ 表示机器人 $u$ 在时间 $t$ 的方向。机器人不知道自己的位置或方向。

每个机器人都有其自身的局部坐标系，该坐标系随其位姿变化。具体而言，在时间 $t$ 时，机器人 $u$ 的局部坐标系原点位于其自身位置 $p_u^t$，且其 x 轴与其自身方向 $\varphi_u^t$ 对齐。机器人上所有的感知信息均记录在其局部坐标系中（参见图1）。

对于 $\theta \in [0, 2\pi)$，令 $R_\theta$ 和 $\psi(\theta)$ 分别表示 $\theta$ 的旋转矩阵和角度 $\theta$ 的单位向量。机器人 $w$ 在时刻 $t’$ 相对于机器人 $u$ 在时刻 $t$ 的局部坐标系中的位置定义为 $p_{wt’} | {ut} = R {-\varphi_u^t}(p_{wt’} - p_u^t) = | wt’ - p_u^t | |\psi(\theta_{wt’} | {ut})$，机器人 $w$ 在时刻 $t’$ 相对于机器人 $u$ 在时刻 $t$ 的局部坐标系中的方向定义为 $\varphi {wt’} | {ut} = \varphi {wt’} - \varphi_u^t$。因此，机器人 $w$ 在时刻 $t’$ 相对于机器人 $u$ 在时刻 $t$ 的局部坐标系中的位姿由元组 $\text{pose} {wt’} | {ut} = \langle p_{wt’} | {ut}, \varphi {wt’} |_{ut} \rangle$ 描述。

在时间 $t$ 的通信图是一个有向图 $G_t = (V, E_t)$，其中 $E_t \subseteq V \times V$ 是一组有向边，满足 $(u, v) \in E_t$ 当且仅当机器人 $u$ 在时间 $t$ 发送的消息被机器人 $v$ 接收。机器人 $u$ 在时间 $t$ 的邻居是那些在时间 $t$ 可以向其接收消息的机器人集合，记为 $N_u^t = {v | (v, u) \in E_t}$。

为简化说明，假设计算、通信和感知以同步锁步轮次 ${1, 2, …}$ 进行。实际上，可在任何部分同步系统中使用同步器[19]来模拟完全同步。如果机器人 $u$ 在轮次 $i$ 接收到来自机器人 $w$ 的消息，则机器人 $u$ 可识别该消息源自 $w$，并估计距离 $|p_{vi} - p_{wi}|$。

机器人能够使用里程计来估计其在自身局部坐标系中各轮次之间的位姿变化。具体而言，在第 $j$ 轮，机器人 $u \in V$ 可以估计其相对于第 $i$ 轮到第 $j$ 轮的平移变化 $p_{ui}| {uj}$，以及相对于第 $i$ 轮到第 $j$ 轮的方向变化 $\varphi {ui}|_{uj}$。假设里程计估计在两轮或三轮的时间间隔内（即 $i \geq j - 3$）是可靠的，但在更长的时间间隔内会出现漂移。

2.1 问题描述

形式上，问题描述要求在每一轮次 $i$ 中，每个机器人 $u$ 计算其每个邻近机器人 $w \in N_u^i$ 的相对位姿 $\text{pose} w^i | {ui}$。机器人只能通过距离感知相互感知。对于机器人 $u$ 在特定轮次计算邻近机器人 $w$ 的位姿，必须依赖之前轮次的距离测量和通信图，以及 $u$ 和 $w$ 在之前轮次的里程计估计。

所考虑的算法不需要控制每个机器人的运动；第一种算法不施加任何约束，第二种算法仅要求在机器人可以移动时进行协调，但不涉及其执行的具体运动。这使得这些算法能够与任何运动控制算法并发运行。此外，这些算法针对简单机器人的大规模群体进行了优化，因此消息的大小和计算需求不依赖于网络规模或直径等全局参数。

3 无需协调的定位

本节描述了一种无需机器人之间进行运动协调且使用最小通信的分布式定位算法。每个机器人通过求解一个非线性方程组来定位其邻居。为简化起见，本节假设距离感知和里程计估计是完美的（例如无噪声）；如果考虑零均值高斯噪声，则也可进行类似的处理。第5节将说明如何将此处提出的结果轻松扩展以处理含噪声测量。

考虑任意一对机器人 $a$ 和 $b$，在连续区间轮次 $I \subset N$ 内。为简化表示，令 $p_{aj} \rightarrow p_{bk} = p_{bk} - p_{aj}$ 表示在全局坐标系中从 $p_{aj}$ 指向 $p_{bk}$ 的向量。

观察图2很容易看出，从 $p_{ai}$（通常来说，从任意 $p_{aj}$ 开始，其中 $j < k$）至少存在两种方式到达 $p_{bk}$。例如，可以先经过一条虚线再经过一条实线，或者顺序相反。事实上，根据定义，对于所有 $j \leq k$ 均成立：
$$
p_{aj} \rightarrow p_{ak} + p_{ak} \rightarrow p_{bk} = p_{aj} \rightarrow p_{bk} = p_{aj} \rightarrow p_{bj} + p_{bj} \rightarrow p_{bk}.
\quad (1)
$$

对于 $j = k$，方程(1)空真成立；而对于 $j < k$，它表示基于单个机器人可感知或可计算的量对机器人 $a$ 和 $b$ 的相对位姿施加的约束。接下来，我们对该前式进行变换，以将其表示为基于已知量对相对位姿的约束。
$$
p_{aj} \rightarrow p_{ak} - p_{bj} \rightarrow p_{bk} + p_{ak} \rightarrow p_{bk} = p_{aj} \rightarrow p_{bj}
$$
$$
-R_{\varphi_{ak}} p_{aj}| {ak} + R {\varphi_{bk}} p_{bj}| {bk} + R {\varphi_{ak}} p_{bk}| {ak} = R {\varphi_{aj}} p_{bj}| {aj}
$$
$$
p {aj}| {ak} + R {\varphi_{bk}| {ak}} p {bj}| {bk} + p {bk}| {ak} = R {\varphi_{aj} - \varphi_{ak}} p_{bj}| {aj}
$$
$$
|p {aj}| {ak} + R {\varphi_{bk}| {ak}} p {bj}| {bk} + p {bk}| {ak}| = |p {bj}| {aj}|
$$
$$
|-p {aj}| {ak} + R {\varphi_{bk}| {ak}} p {bj}| {bk} + d_k(a, b)\psi(\theta {bk}|_{ak})| = d_j(a, b)
\quad (2)
$$

分解公式(2)：$d_j(a, b)$ 和 $d_k(a, b)$ 是已知的，分别对应第 $j$ 轮和第 $k$ 轮时机器人 $a$ 与机器人 $b$ 之间的估计距离；$p_{aj}| {ak}$ 和 $p {bj}| {bk}$ 也是已知的，分别对应机器人 $a$ 和机器人 $b$ 从第 $j$ 轮到第 $k$ 轮的里程计估计；最后，$\varphi {bk}| {ak}$ 和 $\theta {bk}|_{ak}$ 均为未知量，分别对应第 $k$ 轮时机器人 $b$ 在机器人 $a$ 的局部坐标系中的相对位置和方向。

考虑方程（2）在多轮次中的情况，会得到一个非线性系统，如果该系统行为良好，则允许机器人估计另一个机器人的相对位姿。为了避免不确定系统，我们需要至少两个方程，因为有两个未知数。

在实践中我们观察到，即使测量是有噪声的，超约束系统提供的额外信息也无法带来足以证明增加计算成本合理的改进，即使测量是有噪声的。

以下分布式算法利用由 $\delta \geq 2$ 方程组捕获的约束，使每个机器人能够计算其邻居的相对位姿。

算法1 无需协调的定位

1: 对于每个机器人 $u \in V$ 以及每轮次 $k \in {1, …}$ 执行
2: 广播 $\langle p_u^{k-1}| {uk}, \varphi_u^{k-1}| {uk} \rangle$
3: 接收 $\langle p_w^{k-1}| {wk}, \varphi_w^{k-1}| {wk} \rangle$ for $w \in N_u^k$
4: $I = {k - \delta, …, k}$
5: for each $w \in \bigcap_{j \in I} N_u^j$ do
6: 融合里程计 $p_{uj}| {uk}, \varphi {uj}| {uk}$ for $j \in I$
7: find $\hat{\theta} {wk}| {uk}, \hat{\varphi} {wk}| {uk}$ 使得 (2) 成立 $\forall j \in I$
8: 位姿 $w_k| {uk} \leftarrow \langle d_k(u, w)\psi(\hat{\theta} {wk}| {uk}), \hat{\varphi} {wk}| {uk} \rangle$

在算法1的每一轮中，每个机器人发送固定数量的信息（该轮次的里程计测量），因此其消息复杂度为 $O(1)$。算法1的计算复杂度主要由求解非线性方程组（第7行）决定，可通过数值方法[21]在 $O(\varepsilon^{-2})$ 内完成，其中 $\varepsilon$ 是期望的精度。

无论选择 $\delta$ 为何，都存在一些运动模式，使得任何未强制执行严格运动协调的算法（包括未强制执行运动协调的算法1）都无法恢复相邻机器人之间的相对位姿。这些运动被称为退化，其描述如下（见图3）。首先，如果在 $\delta$ 轮次中两个机器人沿直线轨迹运动，则它们的相对

这些机器人之间的位姿只能恢复到存在翻转歧义的程度。其次，如果在 $\delta$ 轮次中，一个机器人沿另一个机器人轨迹的平移版本移动，则可以推断出机器人的相对方向，但旋转歧义会阻碍相对位置的恢复。退化运动可能是翻转歧义、旋转歧义，或两者组合（参见图3）。

幸运的是，退化运动十分罕见。更准确地说，退化运动是一个测度为零的集合（例如，这意味着如果运动是随机的，则它们非退化的概率为1）。这可以归因于三角柱在欧几里得二维空间中的通用刚性所致，关于刚性的详细讨论参见[22]。下一个定理形式化了该算法的性质（由于篇幅限制，所有证明均被省略）。

定理1 如果在第 $i$ 轮，机器人 $u$ 和 $w$ 在一个连续区间内已互为邻居达 $\delta$ 轮或以上，并且执行了非退化运动，则在第 $i$ 轮，算法1会在 $u$ 处计算位姿 $w_i| {ui}$，并在 $w$ 处计算位姿 $u_i| {wi}$。

4 协同定位

本节描述了一种分布式定位算法，该算法采用简单的停/走运动协调方案，并且所需的通信量与邻居数量成正比。使用上述运动协调方案可使机器人通过三边测量计算邻近机器人的相对位姿。

通过收集多个距离估计，移动机器人可以使用三边测量来计算静止机器人相对于自身的位置；与之前一样，可以采用标准技术将此方法扩展到零均值噪声的情况，这在第5节中简要介绍。仅需两个这样的距离估计，移动机器人即可计算出静止机器人的相对位置，但存在翻转歧义（即沿测量位置坐标所在直线的反射对称性）。

考虑两个邻近机器人 $u$ 和 $w$，从第 $k - 1$ 轮到第 $k$ 轮，机器人 $u$ 移动而机器人 $w$ 保持静止（见图4）。机器人 $u$ 可仅依靠在第 $k$ 轮和第 $k-1$ 轮对机器人 $w$ 的距离测量以及其自身在第 $k$ 轮的里程计，计算出机器人 $w$ 在第 $k$ 轮的相对位置 $p_{wk}| {uk}$，但存在翻转歧义。具体而言，余弦定理可得出以下结论。
$$
\ell {uk} = |p_u^{k-1}| {uk}|
$$
$$
\alpha {uk} = \angle(p_u^{k-1}| {uk})
$$
$$
\beta {wk}| {uk} = \cos^{-1}\left( \frac{\ell {uk}^2 + d_k^2(u, w) - d_{k-1}^2(u, w)}{2\ell_{uk} d_k(u, w)} \right)
\quad (3)
$$
$$
\gamma_{wk}| {uk} = \cos^{-1}\left( \frac{d_k^2(u, w) + d {k-1}^2(u, w) - \ell_{uk}^2}{2d_k(u, w)d_{k-1}(u, w)} \right)
\quad (4)
$$
$$
\theta_{wk}| {uk} = \alpha {uk} \mp \beta_{wk}| {uk}
\quad (5)
$$
$$
\theta {uk}| {wk} = \theta {uk-1}| {wk} \pm \gamma {wk}|_{uk}
\quad (6)
$$

为了使机器人 $u$ 完全确定机器人 $w$ 在第 $k$ 轮的相对位姿（忽略翻转歧义），仅需计算 $\varphi_{wk}| {uk}$。注意到，若已知 $\theta {uk-1}| {wk}$，机器人 $u$ 可利用公式(6)，通过余弦定理计算得到的修正项 $\gamma {wk}| {uk}$ 来计算 $\theta {uk}| {wk}$。以下恒等式可用于借助 $\theta {wk}| {uk}$ 和 $\theta {uk}| {wk}$ 轻松恢复 $\varphi {wk}| {uk}$。
$$
\varphi {uk}| {wk} = \theta {wk}| {uk} - \theta {uk}|_{wk} + \pi \mod 2\pi
\quad (7)
$$

总之，如果机器人 $u$ 从第 $k - 1$ 轮次移动到第 $k$ 轮次，而机器人 $w$ 保持静止，则利用 $d_{k-1}(u, w)$、$d_k(u, w)$ 和 $p_u^{k-1}| {uk}$，机器人 $u$ 可以计算出机器人 $w$ 在时间 $k$ 的相对位置。如果已知 $\theta {uk-1}|_{wk}$，机器人 $u$ 还可以计算出机器人 $w$ 在时间 $k$ 的相对方向。位置和方向均正确，但存在翻转歧义。

机器人可以通过重复上述过程并检查预测的位置和姿态的一致性来消除位置和姿态中的翻转歧义。我们将保持对称性从而阻止消除翻转歧义的运动（例如共线运动）称为退化运动（参见图5）。

请注意，静止机器人与移动机器人之间的距离测量对移动机器人围绕静止机器人的旋转是不变的（参见图6）。这导致静止机器人无法利用任意数量的距离估计来恢复移动邻居的相对位置。

然而，为了让机器人 $u$ 恢复机器人 $w$ 的方向，机器人 $w$——其从第 $k−1$ 轮到第 $k$ 轮保持静止——必须计算 $\theta_{uk-1}| {wk-1} = \theta {uk-1}|_{wk}$ 并在第 $k$ 轮之前将其发送给机器人 $u$。

因此，为了利用前述三边测量过程，需要协调机器人的运动，使得每个机器人都有机会移动，并确保当某个机器人移动时，其邻居保持静止。形式上， motion-schedule 是一种在每一轮次将每个机器人分类为“移动的”或“静止的”的算法。如果在每一轮次 $i$ 被分类为“移动的”机器人集合构成了通信图 $G_i$ 的一个独立集（即任意两个移动机器人不是邻居），则该 motion-schedule 是 well-formed 的。 length 指的是任何机器人在被分类为移动之前最多需要等待的轮次数。如果一个 motion-schedule 是结构良好的且具有有限长度，则称其为 valid 。

运动调度的有效性确保了移动机器人可以使用三边测量来计算其所有邻居的相对位置，而有限长度的运动调度则保证每个机器人都有机会移动。下一小节将描述一种生成有效运动调度的分布式算法。算法2描述了一种利用有效运动调度和三边测量的分布式定位算法。

在算法2的每一轮中，每个机器人发送一条消息，其中包含其自身的里程计估计以及 $\Theta_u^{k-1}$（即其每个邻居的先前位置估计集合），因此其消息复杂度为 $O(\Delta)$。移动机器人使用三边测量来计算其邻居的相对位置和相对方向，而静止机器人则在可能的情况下利用接收到的里程计估计来更新移动机器人的相对位置和姿态。无论哪种情况，算法2用于定位每个机器人的计算量都是恒定的。

定理2 (假设存在有效的运动调度) 如果在第 $i$ 轮，机器人 $u$ 和 $w$ 在连续若干轮次中一直是邻居，并且在此期间机器人 $u$ 执行了非退化运动，则在第 $i$ 轮算法2会在机器人 $u$ 处计算出位姿 $w_i|_{ui}$。

算法2 协同定位

1: $\Theta_u^0 \leftarrow \emptyset\ \forall u \in V$
2: 对于每个机器人 $u \in V$ 以及每一轮次 $k \in {1, …}$ 执行
3: 广播 $\langle p_u^{k-1}| {uk}, \varphi_u^{k-1}| {uk}, \Theta_u^{k-1} \rangle$
4: 接收 $\langle p_w^{k-1}| {wk}, \varphi_w^{k-1}| {wk}, \Theta_u^{k-1} \rangle$ for $w \in N_u^k$
5: 如果 状态 = 移动的 那么
6: $\Theta_u^k \leftarrow {\hat{\theta} {wk}| {uk} \text{ 通过公式 }(4-5)}$
7: $\hat{\varphi} {wk}| {uk} \leftarrow \text{使用公式 }(6-7)\ \forall w \in N_u^k$
8: 使用先前的状态解析 flip 在 $\Theta_u^k$ 中
9: else
10: 更新 $\Theta_u^k$ 至 $\varphi_{wk-1}| {wk}, p {wk-1}| {wk}$
11: $\hat{\text{位姿}} {wk}| {uk} \leftarrow \langle d_k(u, w)\psi(\hat{\theta} {wk}| {uk}), \hat{\varphi} {wk}|_{uk} \rangle\ \forall w \in N_u^k$
12: 状态 $\leftarrow$ 运动调度器
13: if 状态 = 移动的 then
14: 根据运动控制器移动
15: else
16: 保持静止

4.1 运动调度

作为一种无需通信且能输出有效运动调度的初步分布式算法，可以考虑一种以循环方式在每轮中仅分配一个移动机器人的算法（即在第 $i$ 轮，令机器人 $k = i \mod n$ 为移动的，其余的 $n - 1$ 个机器人为静止的）。尽管该算法生成的运动调度是有效的，但它并不适用于群体环境，因为它不具备并行性，且每个机器人移动所需的时间与机器人数量呈线性关系。

在任意特定轮次中，寻找一个能最大化移动机器人数量的运动调度等价于寻找通信图的最大独立集（即最大独立集 MaxIS），而这是 NP 难问题。类似地，寻找最短长度的运动调度意味着要找到通信图的最少颜色数的顶点着色，这也是 NP 难问题。

算法3描述了一种运动调度，其较为温和的性质在于：每轮次中移动机器人的集合构成了通信图的一个极大独立集（即 MIS）。一旦某个机器人被归类为移动的，它将不再参与后续的 MIS 计算，直到其所有邻居均被归类为移动的为止。基于这些性质，不难证明，对于任意机器人 $u$ 和轮次 $k$，机器人 $u$ 被归类为移动状态所需的轮次数受限于机器人 $u$ 在轮次 $k$ 时的邻居数量。

定理3 算法3定义了一个长度为 $\Delta + 1$ 的有效运动调度。

算法3的描述使用了一个分布式极大独立集算法作为子程序（伪代码中的第4行）。然而，需要注意的是，通过分布式算法寻找极大独立集的问题是一个基本的对称性打破问题，远非平凡。幸运的是，该问题已被分布式计算领域广泛研究，并在多种通信模型下提出了极为高效的解决方案[23–25]。经典解法[23]需要 $O(\log n)$ 轮次的通信，且每个节点总共使用 $O(\log n)$[26] 比特的通信量。在无线网络环境中，已知[24] 可以通过最多交换 $O((\log^* n)^2)$ 比特来找到一个极大独立集。由于篇幅限制，对于本文而言，只需知道可以在底层通信层实现分布式极大独立集协议而不会带来显著开销即可。

算法3 运动调度器

1: 如果 $\forall w \in N_u\ \text{state}_w =$ 非活动 则
2: $\text{state}_u \leftarrow$ 竞争
3: 如果 $\text{state}_u =$ 竞争 则
4: 如果 $u$ 在分布式 MIS 中被选中 则
5: $\text{state}_u \leftarrow$ 非活动
6: 输出 移动的
7: 输出 静止的

迭代对数函数 用于计算在参数上应用对数的次数，直到结果小于或等于1。它是一个增长极慢的函数，例如，宇宙中原子数量的迭代对数函数值小于5。

5 算法评估

本节评估所提出的定位算法的性能，考虑了距离测量和里程计估计均受噪声影响的情况。我们使用一个仿真环境，该环境经过专门设计，以高度模拟 Kilobot 群体平台的物理特性。

具体而言，我们假设每个机器人的距离测量受到独立的零均值高斯噪声影响，其方差为 $\sigma_d$，而里程计估计受到两个独立噪声源的影响：方向分量受到方差为 $\sigma_\varphi$ 的零均值高斯噪声影响，平移分量受到方差为 $\sigma_{xy}$ 的二维对称零均值高斯噪声影响。我们并未采用里程计模型的标准噪声假设，因为我们的里程计模型旨在描述在 Kilobot 群体平台上用于提供里程计信息的外部顶置计算机视觉系统中存在的噪声（Kilobot 群体平台所使用的粘滑式移动会导致机器人运动依赖于各机器人下方表面的不规则性，因此无法内置里程计）。

算法1依赖于在使用与该机器人相关的距离估计和里程计估计构建的非线性方程组中寻找零点。当这些估计存在噪声时，相应的非线性系统不再保证存在零点。为了应对含噪声测量，只需寻找使均方误差最小的点即可。这不会带来额外的计算开销，因为可以使用与无噪声情况下相同的数值方法来实现。

每次仿真试验的长度为20轮，每轮6秒（2分钟）。对于每种不同的噪声参数组合，共进行了50次试验。在每次试验中，20个机器人被随机部署在一个10米 × 10米的区域内，每轮每个机器人允许执行一次运动，其方向变化为随机值，介于 $[- \pi/4, \pi/4]$ 之间，平移变化则服从均值3米、方差0.5米的正态分布。每次试验的长度为20轮，每轮6秒（2分钟）。下图（参见图7和图8）显示了在50次随机试验中，不同噪声参数下计算位置（蓝色）和方向（红色）的均方误差（MSE）。由于算法2在初始化位置和姿态估计时至少需要三轮数据，因此每次试验的前三轮数据被舍弃。

不出所料，算法1产生的结果对各轴上的误差都很敏感，尽管其对平移里程计误差的鲁棒性略高于对距离感知误差的鲁棒性。此外，相对姿态估计对噪声的容忍度始终高于位置估计。正如预期的那样，在所有不同的噪声设置下，将参数 $\delta$ 从2增加到3， consistently reduced 算法1在位置和方向上产生的均方误差。然而，增加 $\delta$ 也会增加算法的计算成本，并且仅带来递减回报。

算法2 使用与 算法1 相同的参数进行评估，但有一个例外；为了使 算法1 和 算法2 每次试验的动作次数大致相同，试验长度被加倍至 40轮，因为在每个轮次中，对于每一对节点，只有一个节点是移动的，而另一个将保持静止。

算法2产生的位姿估计在大多数情况下在任一维度上受到的噪声影响均等。与算法1的情况一样，相对方向估计始终比位置估计对噪声更具容忍度。总体而言，与算法1相比，结果表明算法2在各个方面对噪声的敏感性更低。这一点，加上其计算简单性，使其更适合在 Kilobot 平台上实现。

5.1 运动控制与定位

在这里，我们探讨将现有运动控制算法与所提出的定位算法进行组合的可行性。出于简洁性考虑，我们研究了集群行为的经典问题[1]。非正式地说，集群行为描述了一组智能体在没有中央协调的情况下表现出的一种涌现行为，尽管这些智能体事先没有共同的方向感知，但仍能协同移动。

集群行为在科学界受到了广泛关注。Vicsek 等人[27]通过仿真从物理学角度研究了集群行为，并重点关注自驱动粒子系统中对齐现象的出现。集群行为也从控制理论视角进行了研究，例如在[28, 29]中，重点在于尽管通信具有局部性和不可预测性，最终对齐过程的鲁棒性仍得以保持。

我们研究一种集群行为，其中每个机器人将其航向与邻居对齐，并避免与附近的邻居发生碰撞。具体而言，在每一轮中，每个机器人将其自身方向调整为其邻居平均方向，并调节其速度以避免过于接近任何邻居。研究表明{v3}，在非常宽松的假设条件下，该过程会收敛到所有机器人具有相同方向的状态。

图9展示了将基于平均的集群行为算法与算法1结合以提供相对姿态估计的结果。在初始轮次中，机器人由于位置和姿态估计尚未初始化而移动不稳定，随后所有机器人的方向迅速收敛。距离感知和里程计测量中的误差增大会导致生成的相对朝向估计精度降低，从而影响最终的集群行为状态。

在群体达到稳态之前，可以利用距离测量进行定位；而当不再需要调整且群体处于稳态时，定位将变得不可能。

6 结论与未来工作

我们考虑了两种分布式算法来解决针对简单机器人群体的相对定位问题。这些算法在通信和计算需求方面有所不同，且对感知误差的鲁棒性也不同。具体而言，更强的通信与协作能力能够降低所需的计算复杂度，并提高对感知误差的鲁棒性。在未来的工作中，我们希望进一步研究这种权衡是否是该问题固有的特性。

我们目前正在 Kilobot 群体平台上实现所描述的算法。Kilobot 平台没有浮点运算单元，程序内存有限（30k），并且带宽非常有限（每秒24字节）。因此，即使简单的算法也需要对所有参数进行仔细调整和优化，才能在 Kilobots 上实现。我们还在研究通信需求更少的算法。