Marco Polo: 仅凭距离和运动寻找朋友

# 马可·波罗 来源：https://www.jackhogan.me/blog/marco-polo 你走进一家咖啡馆，寻找你的朋友。这看起来很容易，直到你发现里面挤得根本看不清人群，大家都大声聊天，你几乎什么也听不见。你唯一知道的是自己的移动，以及你和朋友之间的距离（通过你们之间特殊的心理感应）。你们要怎么找到彼此？ 我想解决完全相同的问题，只不过是用设备代替人（所以没有心理感应），而这些设备存在于数百个其他设备的空间中。解决这个问题让我学到了许多与机器人和状态估计相关的有趣科学知识，我写下这篇文章，希望你能一起学习。 ## 正式问题：仅测距的相对定位 我有两块微控制器（蓝色大板）。每块都配有一个惯性测量单元（IMU，右上角的小板），能提供加速度和指南针航向等数据。它们还有一个超宽带模块（UWB，左侧插槽中的小板），用于测量配对设备之间的距离。 原型设备，包含 NRF54L15 开发板、IMU 和 UWB 芯片。（https://www.jackhogan.me/_astro/beacons.02DSpMF1.png） 这些信标。每个这样的套件都将封装在一个可穿戴设备中，因此可以提示佩戴者移动。这是一个重要的自由度，因为它意味着在定位过程中，我们可以利用距离随时间的变化来确定位置，而不仅仅是静态测量。 ## 可能的解决方案 为了让我们进入技术实现的思维模式，先了解一些可选方案，看看它们是否适合我们的需求。 ### 候选方案 1：多天线超宽带 超宽带定位，例如 AirTags 中的定位功能，使用一种简单的应答系统，称为**飞行时间**。UWB 测量往返信息交换所需的时间，然后将其转换为距离：$d = c \cdot (T_{loop} - T_{reply})/2$。 发起方向响应方发送一个带时间戳的脉冲，响应方在 $T_{reply}$ 时间后（由响应方报告）回复自己的时间戳包。整个交换过程耗时 $T_{loop}$，由发起方计算： 距离：15.00 米 $T_{reply}$：120.00 纳秒 $T_{loop}$：220.07 纳秒 $T_{reply}$：120.00 纳秒 推导出的飞行时间：50.03 纳秒 恢复距离：15.00 米 $\text{ToF} = \frac{T_{\text{loop}} - T_{\text{reply}}}{2} = \frac{220.07 - 120.00}{2} = 50.03 \text{ ns}$ $d = c \cdot \text{ToF} = 15.00 \text{ m}$ 拖动更新 飞行时间系统可以很好地获取与另一设备的距离，但单独使用不足以获取相对于另一设备的方位角。这就是相位差到达时间（PDoA）超宽带技术发挥作用的地方。 PDoA 利用无线电波的一种特性，称为**相位**。光和无线电波在传播过程中上下振荡；它们在振荡周期中的位置就是相位。当波经过空间中任何一个固定点时，该点的相位会循环变化： 波长 $\lambda$：80.00 $\varphi = 3.14 \text{ rad}$ 我们利用这个特性来确定相对于另一设备的航向角。通过将两根天线放置在一个已知距离 $d_a$ 上，我们可以比较每个时刻两根天线上的相位差，从而推导出另一设备消息的到达角： 拖动点来移动波源。 $\lambda$：80.00 $\Delta\varphi$：0.00 rad $\theta$：0.00 rad (0.00°) $\theta = \arcsin\!\left(\frac{\Delta\varphi \cdot \lambda}{2\pi \cdot d_a}\right) = \arcsin\!\left(\frac{0.00 \cdot 80.00}{2\pi \cdot 100}\right) = 0.00 \text{ rad}$ 拖动更新 **失败模式：** PDoA 是一个很好的系统，但它有一个局限性：需要两根天线。这种设计限制使得它很难集成到可穿戴设备中，因为皮肤会严重衰减信号。一根天线（我为 Beacons 选择的方案）也许通过巧妙的布置还能通信，但两根天线就困难得多。 ### 候选方案 2：外部三边定位系统 另一种可能的方法是引入位置已知的固定外部设备，通常称为**锚点**。至少需要三个锚点，一个 Beacon 可以测量到每个锚点的距离，然后求解自己的位置。这种策略与 GPS 的工作原理相同，利用多个距离圆（或三维球体）来推导设备的精确位置： 拖动信标。 $r_1$：130.00 $r_2$：200.00 $r_3$：200.00 $ \begin{aligned} r_1^2 &= (x - x_1)^2 + (y - y_1)^2 \\ r_2^2 &= (x - x_2)^2 + (y - y_2)^2 \\ r_3^2 &= (x - x_3)^2 + (y - y_3)^2 \end{aligned} $ $\begin{bmatrix} x_2 - x_1 & y_2 - y_1 \\ x_3 - x_1 & y_3 - y_1 \end{bmatrix} \begin{bmatrix} x \\ y \end{bmatrix} = \tfrac{1}{2}\begin{bmatrix} k_2 - k_1 \\ k_3 - k_1 \end{bmatrix}, \quad k_i = x_i^2 + y_i^2 - r_i^2$ $(x, y) = (240.00, 190.00)$ 拖动更新 **失败模式：** 一旦有了三个锚点，三边定位很容易实现。但在实践中，这意味着需要永久安装设备，让它们不断广播自己的位置。我们的定位需求是在没有特殊安装设备的情况下工作，因此我不认为三边定位是有效的解决方案。不过，如果我需要在人流量大的已知位置（如会议场地）部署这些设备，这种策略可以作为精化系统，使主系统保持稳定。 ### 候选方案 3：卡尔曼滤波器 卡尔曼滤波器是物理空间中状态空间估计的黄金标准，正是解决我们问题所需的方法。它的原理是：对不可观测的真实状态（这里指相对位置）保持一个内部估计，然后通过一个循环过程，用测量得到的新信息来更新这个估计。这个循环最终收敛到一个合理的估计值，我们可以用它来引导用户。我们的真实状态之所以不可观测，是因为仅凭传感器无法直接测量方位角；**方位角来自所有传感器的综合信息**。 我特别选择了扩展卡尔曼滤波器（EKF），它支持计算期望方位角所需的非线性函数。EKF 之所以是最佳解决方案，是因为它只需要一个 UWB 天线（不像 PDoA 需要两个），并且只需要两个设备（不像三边定位需要多个）。 ## 实现 EKF EKF 是一个循环，由两个部分组成：预测步骤和测量步骤。你有一个状态向量 $\hat{\textbf{x}}$ 和协方差矩阵 $P$，需要尽可能与实际不可观测状态 $\textbf{x}$（例如到目标的 $x$ 和 $y$ 距离）保持一致。我们使用“帽子”符号 $\hat{\cdot}$ 表示该状态向量是对真实状态的估计。$P$ 没有帽子，因为它不是对某物的估计，它只是用来辅助估计 $\hat{\textbf{x}}$。为了更新 $\hat{\textbf{x}}$ 和 $P$，你需要问两个问题： - “我认为下一个状态会是什么？”：由**状态转移函数** $f(\textbf{x}, \textbf{u})$（也称为**预测函数**）回答，该函数接受当前状态和控制输入，并将其投影到下一个时间步。对于 Beacons 来说，它问的是：“根据我当前的移动和加速度（表示为 $[a_x, a_y]$），物理定律认为我下一步会到哪里？” 加速度是控制输入而非测量值，因为它**驱动状态转移**，而不是状态转移的结果。 - “我期望收到什么测量值？”：由**测量函数** $h(\textbf{x})$ 回答，它也接受当前状态，但问的是可能的传感器读数是什么。对于 Beacons：“根据我当前的移动，我期望下一个时间步的 UWB 读数是多少？” 状态 $\textbf{x}$ 表示系统跟踪的内容，但知道各个状态值之间的*关系*也非常重要。这就是协方差矩阵 $P \in \mathbb{R}^{|\textbf{x}|\times|\textbf{x}|}$ 的作用。它是一个方阵，行/列数等于 $\textbf{x}$ 的大小，记作 $|\textbf{x}|$。矩阵中的每个元素 $P_{i,j}$ 回答的问题是：“$\textbf{x}_i$ 和 $\textbf{x}_j$ 如何相互影响各自的分布？” 对于对角线上的元素，这简化为 $\textbf{x}_i$ 的方差。我们利用 $P$ 中的元素，在预测和测量步骤中塑造我们对 $\textbf{x}$ 中变量分布的信念。 ### 附注：线性化 **线性化** 是将一个非线性函数（如抛物线）无限放大，直到曲线看起来像是直线，或者说线性的过程。这是一种近似，所以不能保证最优，但对于我们的用例（以及其他用例，包括 GPS）来说已经足够好了。 缩放 1× $f(x) = x^2$（实线） $L(x) = 2x - 1$（虚线，在 $x_0 = 1$ 处线性化） EKF 为了支持非线性函数（比如平方根，你会发现这对我们的任务必不可少），增加了额外的复杂度：它要求 $f(\textbf{x}, \textbf{u})$ 和 $h(\textbf{x})$ **可微**，以便在 $\hat{\textbf{x}}_{k+1|k}$ 处使用 $f$ 和 $h$ 的雅可比矩阵 $F$ 和 $H$ 进行线性化。这样做可以让我们复用常规的卡尔曼更新方程，使计算更容易。符号 $k+1|k$ 表示“我们在时间步 $k$ 拥有的信息条件下，对 $k+1$ 时刻的猜测”。 ### 数据流 现在我们已经有了描述数据如何在 EKF 循环中流动的所有部分。我们从当前状态 $\hat{\textbf{x}}_{k|k}$ 和协方差矩阵 $P_{k|k}$ 以及该时间步的控制输入 $\textbf{u}_k$ 开始。 第一步是预测下一个状态和协方差。 $$ \begin{align*} \hat{\textbf{x}}_{k+1|k} &= f_k(\hat{\textbf{x}}_{k|k}, \textbf{u}_k)\\ P_{k+1|k} &= F_k P_{k|k} F_k^T + Q_k \end{align*} $$ 注意 $F_k P_{k|k} F_k^T$：这就是我刚才说的线性化。$Q_k$ 项是**时间步 $k$ 的预测噪声协方差**。预测噪声越大，滤波器对其信任度越低。接下来，我们用测量值更新 $\hat{\textbf{x}}_{k+1|k}$ 和 $P_{k+1|k}$： $$ \begin{align*} K_{k+1} &= P_{k+1|k} H_{k+1}^T (H_{k+1} P_{k+1|k} H_{k+1}^T + R_{k+1})^{-1}\\ \hat{\textbf{x}}_{k+1|k+1} &= \hat{\textbf{x}}_{k+1|k} + K_{k+1} (z_{k+1} - h_{k+1}(\hat{\textbf{x}}_{k+1|k}))\\ P_{k+1|k+1} &= (I - K_{k+1} H_{k+1}) P_{k+1|k} \end{align*} $$ $z_{k+1}$ 是该时间步的测量值。$K_{k+1}$ 是**卡尔曼增益**，它根据相对方差对测量值和预测值进行加权。增益越高，说明我们越信任测量值；增益越低，说明我们越信任预测值。线性化 $H_{k+1} P_{k+1|k} H_{k+1}^T$ 再次出现，这次用于测量步骤。$R_{k+1}$ 与 $Q_k$ 作用相同，但针对的是**测量噪声协方差**。 这就是 EKF 的一个步骤！一旦理解了各部分如何配合，其实并不难。 现在我们对 EKF 的基本逻辑有了了解，是时候定义我们的变量了。 ### 第 1 步：状态向量 $\textbf{x}$ 和协方差矩阵 $P$ 设计滤波器时，可以说是最重要的一点是决定跟踪什么状态。在任何非线性系统中，尤其是在 Beacons 的情况下，都有无数的事情在发生，因此选择性存储内容可以节省处理时间，并避免将来第一次尝试不工作时可能出现的麻烦。对于 Beacons，我们将存储四个值： - $\Delta x$ 和 $\Delta y$：当前设备到另一设备在世界坐标系中 $x$ 和 $y$ 方向的估计差值（单位：米）。可以利用勾股定理将其转化为三角形，计算方位角和距离。 - $\Delta \dot{x}$ 和 $\Delta \dot{y}$：每个方向上的估计速度，对于物理计算很重要。 这四个值（打包为 $[\Delta x, \Delta y, \Delta \dot{x}, \Delta \dot{y}]^T$）使我们能够近似在二维平面内移动的两个设备的物理系统。初始值使用 UWB 距离 $d_0$，假定相对位置完全沿 $x$ 轴： $$ \hat{\textbf{x}}_0 = [d_0, 0, 0, 0]^T $$ 需要注意的是，此时无法判断另一设备的方向，因为在 $k=0$ 时我们只有一个距离测量值。滤波器将随着用户移动设备而逐渐积累方向的信念。我们使用调优后的值初始化协方差矩阵 $P_0$： $$ \text{diag}(P_0) = [0.04, d_0^2, 1, 1] $$ $0.04 \ \text{m}^2$（20 cm 标准差）告诉 EKF，径向协方差很小，受限于 UWB 相对较低的误差，并且初始假设我们正好位于 $x$ 轴上。$d_0^2$ 的方差告诉 EKF，距离越大，对 $y$ 轴越不确定（因为我们在 $k=0$ 时假设只位于 $x$ 轴上）。我们将 $\Delta\dot{x}$ 和 $\Delta\dot{y}$ 的方差初始化为 $1 \ \text{(m/s)}^2$，作为调优后的起始值。$P_0$ 的其余元素为 $0$。滤波器将在学习系统知识的过程中开始改变所有值。 现在我们知道要存储什么了，接下来需要考虑如何向前投影状态预测。 ### 第 2 步：状态转移函数 $f(\textbf{x}, \textbf{u})$ 我们已经有了状态向量的布局，现在需要利用控制向量的信息，在每个时间步向前投影。幸运的是，这很简单，使用你高中可能学过的物理公式： $$ \begin{align*} \Delta\dot{x}_{k+1|k} &= \Delta\dot{x}_{k|k} + a_{x,k} \cdot \Delta t\\ \Delta\dot{y}_{k+1|k} &= \Delta\dot{y}_{k|k} + a_{y,k} \cdot \Delta t\\ \Delta x_{k+1|k} &= \Delta x_{k|k} + \Delta\dot{x}_{k|k} \cdot \Delta t + \tfrac{1}{2}\, a_{x,k} \cdot \Delta t^2\\ \Delta y_{k+1|k} &= \Delta y_{k|k} + \Delta\dot{y}_{k|k} \cdot \Delta t + \tfrac{1}{2}\, a_{y,k} \cdot \Delta t^2 \end{align*} $$ 这些公式给出了我们的预测状态更新 $\hat{\textbf{x}}_{k+1|k}$。 > 注意：世界坐标系中的加速度值 $a_{x,k}$ 和 $a_{y,k}$ 是通过一个 AHRS 滤波器计算得出的，该滤波器结合了 IMU 的数据。