借助IMU传感器融合实现高速跟踪 | Marvelmind

0:00 各位同事好。今天我们来谈谈一种更快的定位系统，用于快节奏的体育运动、快速的工业应用和机械，或者快速飞行的无人机。当然，这仍然是精确的室内定位系统，但重点在于快速移动的物体和高更新率，以及实现的方法。问题是什么？如果典型更新率是8赫兹，对于移动非常快的物体可能根本不够，因为每段运动的位置点太少，得到的不是平滑的轨迹，而只是稀疏的点。还有什么办法？有两个选择：要么放慢运动，而这通常做不到；要么提高更新率。提高更新率的常规方法，我们已经在网页上介绍过。

1:00 请查看。基本上，就是尽可能缩小子地图的尺寸，因为我们的典型值和默认值是30米。如果只有5米，就可以大幅提高更新率——基本上能提高6倍。还可以提高无线电配置档（radio profile），因为在某些情况下，无线电可能是限制因素；还要减少各种平均处理，以及各种可能增加延迟的附加算法。这些做法或多或少不会降低更新率。但如果在我们讨论过的这些典型情况下仍然不够，就必须有新的方法。效果就是这样：这是典型的8赫兹子地图轨迹，可以看到，它非常精确。

1:57 依然一切都很好——正负两厘米。但我们特意移动得非常非常快，就是为了说明各点之间的距离可能与定位精度相当。如果轨迹是平滑的——比如椭圆——那不会有问题。但如果有急弯和急转，更新率可能就不够了，唯一的解决办法是提高定位系统的更新率。问题在哪里？超声波传播得很慢。这很好，因为正是这一点让我们的系统如此精确。请记住，声速约为每秒340米，而光速是它的一百万

2:55 倍左右。所以基本上，对于基于无线电的系统——我们测量的都是飞行时间，只不过这里是超声波的飞行时间与无线电的飞行时间相比——对于基于无线电的系统，要达到同样的精度，实际上要难上百万倍。当然，这是一种非常基础的简化，但无论如何，谈到精度，超声波的飞行时间是核心优势；同时，谈到更新率，它也是主要限制。为什么？想象一下，您有一张很大的地图——比如30米。声音传播这段距离需要时间，每秒340米除以30米，因此在没有任何其他损失的情况下，可以达到的理论最大更新率约为11赫兹，这就是上限。当然，除此之外，还会有处理、滤波、无线电等方面的损失。所以实际上，在30米时，您大致可以期望5、6、7赫兹——取决于设置，取决于您使用哪种 架构（中文） ——但大致就是这个数。也就是损失了约50%，也许40%。

3:53 这对大多数工业应用来说已经可以了，因为通常人们会说：“好，我要实时。多快？”嗯，1赫兹，也许2赫兹。那很好。所以我们通常说的是8赫兹，针对典型的子地图，也就是室内10到20米。但比如您有一张30米的大地图，理论上限约为11赫兹，实际可用的是5、6、7赫兹。如果有一辆移动非常快的小车，这就意味着点会变得稀疏。如果物体移动得太快，有些数据点可能直接丢失。也就是说，本来有一条曲线，但您得到的不是曲线——比如这里有个弯——而是一个位置和另一个位置，中间这个位置根本没有，因为移动太快了。

4:50 如果地图较小，仅靠超声波也可能达到很高的更新率。我们过去做过实验，大约是40赫兹——这还是保守的数字。正如我们在空气曲棍球——哦不，是不是空气曲棍球来着——的案例中所展示的，场地只有2到3米，所以可以获得约40赫兹的更新率，对这种简单的情况来说很好。但对于更大的场景，比如卡丁车，就不够了。为什么？我之所以提到卡丁车，是因为我们有几个案例，客户需要非常快的运动，同时需要非常精确的轨迹，主要是为了教选手或车手如何正确驾驶。我们的系统非常适合这种需求，因为它能提供高精度的轨迹，现在还能提供100赫兹的超高更新率，即使是移动得非常非常快的物体也足够了。

5:49 而要做到这一点，仅靠超声波几乎不可能。我们再回到基本计算：每秒340米除以100赫兹的更新率，地图的尺寸即使在理论上也只有3米，太小了。怎么办？传感器融合就是解决方案。传感器融合是我个人特别喜欢的话题。它极其复杂，但能兼得两者之长：既有室内定位系统的高精度，同时又有非常高的更新率。借助惯性测量单元（IMU），更新率甚至可以超过100赫兹。我们来深入讨论一下。但在此之前，有哪些选择？基本上有三种：一是实时播放器（Real-time Player），Dashboard中已经提供；二是IMU加超声波传感器融合后处理，也就是本视频和本演示中讨论的内容；三是IMU加超声波实时传感器融合，这个话题尤其复杂，会晚些推出——目前尚未商用。但第一种实时播放器和第二种IMU加超声波传感器融合后处理，现在就可以使用。

6:49 那么什么是实时播放器？实时播放器基本上是Dashboard内部的一项功能。它获取位置点，然后在内部利用算法、滤波等各种手段进行相对复杂的插值。所以它是在尝试推测位置点——再强调一次，这是插值、推测，而不是测量。结果相当不错，适合相对平滑的曲线。所以，当没有急转弯，或者更新率已经足够高时，实时播放器非常好，推荐使用。而且它已经可以用了——现在是免费的。所以您可以马上使用它，既可用于实时数据，也可用于已经录制的数据，比如日志文件中的数据。用起来吧，试试看，它非常非常好用。

7:46 我们来看看整体表现。这是一条典型的曲线，可以看到这些蓝点是超声波定位点。这一段我是特意、有意地移动得非常非常快的，基本上是非常快地一下跳过去。可以看到，这两条线之间的比例尺是5厘米，所以这里是5厘米。同样，这里只有几个点——这里有三个，因为已经停止移动了，然后一个、一个。好，这里大概是一个、两个，我不确定，这里一个、两个。实时播放器的轨迹是这条粉色的。好，橙色这条，我们几乎一眼就能看出IMU传感器融合

8:44 ……实时数据，也可用于已经录制的数据，比如日志文件中的数据。用起来吧，试试看，它非常非常好用。我们来看看整体表现。这是一条典型的曲线，可以看到这些蓝点是超声波定位点。这一段我是特意、有意地移动得非常非常快的，基本上是非常快地一下跳过去。可以看到，这两条线之间的比例尺是5厘米，所以这里是5厘米。同样，这里只有几个点——这里有三个，因为已经停止移动了，然后一个、一个。好，这里大概是一个、两个，我不确定，这里一个、两个。实时播放器的轨迹是这条粉色的。好，橙色这条，我们几乎一眼就能看出IMU传感器融合

9:43 的效果——作为融合结果，它显然要好得多。但我们先不谈那个，现在的主题是蓝色和粉色。可以看到，实时播放器基本上是取一些来自“未来”的位置点——这里是0个——以及5个来自过去的点，然后利用这些信息进行插值。可以看到，它已经尽力了，但并不完美。在这个例子中稍好一些，因为没有过冲之类的问题。但可以看到，这正是实时播放器的主要缺点：它没有这些信息，没有加速度信息——这一点与我们将要讨论的IMU不同。所以它基本上是在切弯，把……

10:42 ……这些拐角平滑掉了。这会带来误差，虽然还不算大。再说一次，这是5厘米，比如这里的误差大约是5厘米。但问题在于，您事先并不知道会是什么样的运动，所以无法有针对性地调整。那么用什么样的滤波呢？有些滤波更适合这种运动，有些则对另一种运动效果更好。但逻辑很简单：实时播放器很好，适合平滑运动，因为它能实现100赫兹——不是真实测量的100赫兹，而是插值得到的100赫兹。而且实际上它还有其他帮助：它不仅生成100赫兹的更新率并放置这些位置点，还进行滤波。所以如果有一些……

11:41 ……偶发的跳点，那么在超声波定位点足够多的情况下，它基本上在各方面都有帮助。一是提高了更新率；二是过滤掉一些偶发跳点，或填补超声波数据中的遗漏。比如某个位置缺失了，它仍然能工作。这就是为什么这个距离很重要。如果距离很大，那么即使超声波位置更新遗漏了两三个点，也能推测得相当好。但同时，平滑会更强，拐角会被切得更多，误差也会更大。所以您可以调整它，找到平衡。对于基本的平滑和基本的100赫兹，实时播放器是一个强大的工具。它……

12:41 ……很有帮助。再说一次，它是后处理，所以从我们的角度看并不是实时，因为我们所说的实时基本上是15、10毫秒。但它又是实时的：您不需要先保存文件再回放。不需要，您可以实时进行，只是会有延迟。多少延迟？就是这个。实际上，在这个例子中延迟是7个位置更新点。如果是8赫兹，延迟大约是1秒，粗略地说1秒。但关键是，它现在就能用，提供100赫兹，对相对平滑的曲线效果很好，而且不贵——现在是免费的。再看另一个急转弯的例子。可以看到，它更……

13:41 ……它让曲线更加平滑，但您知道，拐角也被切得更多了。可以看到，因为它预期有三四个点，但这些点并不存在，因为当时确实没有数据。这是另一个矩形的例子。可以看到，这里的点相对多一些。这些蓝色的是超声波位置，这是真实轨迹（ground truth），这是IMU融合的结果，这是实时播放器。嗯，我认为对大多数情况来说已经足够好了。

14:40 因为再说一次，它是真实的，误差都在两厘米以内。但拐角少了一些，有点被平滑掉，误差在这里累积。但如果用得恰当，是可以用的。再说一次，这是向前1个点、向后5个点的计算。用向前3个点、向后5个点时，可以看到平滑更强，但误差已经达到5厘米左右甚至更多。这也不错，当曲线不太弯曲时，它甚至更接近真实情况。但在拐角处或方向变化非常快的地方，误差会变大。如果提高播放器的阶数，误差还会更大。

15:40 这是放大的画面。嗯，基本上这里已经是1厘米了。所以可以粗略算出实时播放器带来的误差约为5厘米。但同样，在这些区域，它帮助很大，让轨迹更加平滑，误差也已经更低。这很难精确计算，但可以看到，它基本上减小或分散了超声波的抖动，使位置更新率达到100赫兹。同时，这里的点分布小于2厘米，比原始超声波还要好。我说过，对于平滑曲线，它几乎是理想的。可以看到，我都猜不出哪个更好——是实时播放器，还是我用的融合。所以，当曲线平滑、并且可以接受后处理时，请使用实时播放器。

16:39 请记住，这都是后处理，会有大约1秒的延迟。同样，换成移动较慢的三角形，效果就已经足够好了。可以看到，那边我们只有几个点，每条边大概一、二、三个点。而这里大概有十个点，效果已经很好。再说一次，这是5厘米，所以与真实轨迹相差正负1厘米，非常好。结论还是一样：如果曲线平滑，或者超声波位置更新次数足够多，使用实时播放器甚至可以获得更好的结果。这就是为什么，例如在许多……

17:37 ……我们的 机器人 和慢速移动的视频中，我们默认启用了实时播放器，因为它能让轨迹更美观、更可靠、更平滑。但当这些都不够时，当物体移动得非常快、实时播放器效果不佳时，就需要传感器融合了。传感器融合非常强大，当然比实时播放器更好，这毫无疑问。为什么？我们先谈谈物理限制。超声波的物理限制我们已经提过：声速是系统的重要优势，因为它让室内定位系统能够达到很高的精度；但同时，它也是更新率相对较低、难以进一步提高的主要原因。这就是物理……

18:35 ……限制。传感器融合的类型我们也已经讨论过：一种是实时融合，会晚些推出；一种是IMU传感器融合后处理。今天我们只讨论后处理，因为它已经商用。本视频正是为此而做——告知所有现有和未来的客户，这项功能已经可以使用，请开始使用吧。最后，也许也是关键的一点：我们系统中的传感器融合是利用IMU完成的。因为传感器融合可以与很多东西一起实现——例如在我们的 机器人中，就有里程计、超声波和IMU之间的传感器融合。在我们的Boxie 机器人上也有，例如……

19:35 还有Intel RealSense——一种视觉定位系统——它也可以参与传感器融合。这就是为什么我特别喜欢这个话题：通过在商业上、技术上巧妙地把所有这些数据结合起来，可以取长补短。因为它们没有一个是完美的，每种定位系统都有一定的局限。例如光学：光线太暗、太亮、有遮挡。里程计：可能打滑，计算出在移动，而实际上并没有移动。超声波：超声波噪声可能让系统完全无法工作。在所有情况下，如果没有无线电，就无法获取数据、无法更新，等等。但通过巧妙地结合所有这些数据并安排自检，基本上就是：

20:34 传感器融合——可以充分利用两个、甚至两个以上系统各自的优势。在我们这里，讨论的是IMU加超声波的传感器融合。什么是IMU？IMU是惯性测量单元（Inertial Measurement Unit）的缩写。在我们的系统中，它包含3D加速度计和3D陀螺仪。我们的每个 信标上都已经板载了IMU。所以如果您之前不知道，现在就可以用起来。我要强调一点：当我们谈到IMU和位置以及为什么它很复杂时——获取角度（仅仅是原始角度数据）或测量加速度，这根本不算什么，您都可以拿到，这是非常非常基础的功能。

21:31 但用IMU测量位置是一项极其复杂的任务。测量绝对角度也很复杂，但难度要小得多。不过为此，基本上需要把两个 信标 放在您的 机器人上，这样就能知道其中一个 信标 和另一个 信标的位置，也就知道了 机器人 所朝的方向——不是方向的变化（只用一个机器人、一个 信标就能测出方向变化），而是静态方向。这称为配对信标（paired beacons），我们已经有这个功能，所以不在本次讨论范围内。为了绝对讲清楚：本视频只讨论一个主题，即利用IMU与超声波之间的传感器融合，以100 Hz更新率进行快速位置测量。那么难点在哪里？难点在于：

22:31 加速度的二次积分。如果是在零重力环境下，加速度计数据会非常简单，因为只要积分就好。即使是基础的MEMS（微机电系统）——手机里或我们信标里常用的那种MEMS——也足够用，因为它们漂移很小。但在零重力下漂移小是一回事，在非零重力下——显然我们是在地球上——要从运动产生的加速度中减去地球带来的巨大加速度，就非常困难。例如，如果您只是这样移动：

23:28 您自身运动的加速度几乎为零，而同时有地球引起的1G加速度。如果您不知道信标的位置——或者说不是位置，而是信标的姿态，即加速度计的朝向——就很难区分哪些加速度是因为对准不准确、或者因为没能准确知道地球引起的加速度，哪些才是信标自身的加速度。这非常困难，误差也非常大。结果很简单：漂移会在不到一秒的时间内就超出我们正负两厘米的范围。如果校准得足够好，大约是1.5秒，有时是1秒。所以我们认为：

24:25 校准后1秒，就可能漂移开。这就是为什么纯粹基于惯性测量单元、没有任何传感器融合、没有任何漂移消除的系统——纯IMU系统——据我所知，没有一个真正能用。它们都在使用某种校准——不，不是校准——某种消除加速度计漂移的方法，因为二次积分显然会让误差增长得非常非常快。如果1秒内有2厘米的误差，那么2秒内就会翻倍，然后再翻倍、再翻倍。基本上误差增长得非常快，即使信标静止不动，一分钟后它“跑开”的距离也会达到几米甚至上百米。

25:22 这是数学决定的。所以漂移是主要问题，必须消除。当然，我们是利用超声波来消除的，因为超声波提供绝对位置。通过巧妙地结合——也就是进行传感器融合——就能做到。结果就是传感器融合后的数据，非常精确：因为IMU没有跳点，IMU提供100赫兹的快速更新率，而且没有漂移，因为漂移已经被基于超声波的系统消除了。陀螺仪当然也会漂移，但陀螺仪是较小的问题。您需要非常准确地知道角度，才能从您的加速度中减去地球引起的加速度。但陀螺仪的漂移问题较小，因为与以下因素相比，它的漂移要小：

26:22 即因加速度计朝向不够精确、以及我们所说的重力“溢出”到加速度计各轴上而导致的漂移。再强调一次：IMU传感器融合是一项复杂的功能，真的非常复杂。这就是为什么它需要一些时间，但成果非常理想，因为您真正获得了两者的优势。再强调一次：IMU的优点在于它不像超声波那样会跳点。即使暂时没有视线，也没关系。也就是说，如果有东西遮挡了超声波，那么IMU会：

27:21 毫不在意。在这段时间里，系统仍然在输出位置。如果在零点几秒或一秒后重新恢复视线，超声波就会再次进行校正。超声波校正漂移，因为IMU——包括加速度计和陀螺仪——始终都在漂移，而超声波每秒校正几次。位置数据由加速度计数据二次积分得到，并且一直由所有超声波数据进行校正。简而言之，这是一个非常强大的方案，您基本上能得到理想的一切：100赫兹、更高的稳健性，以及更高的精度。因为超声波显然可能会跳动：

28:18 正负两厘米，但融合结果不会跳。它的噪声只有零点几毫米。所以实际上，如果专注于精度，可以达到亚毫米级精度。这也许会在将来实现，不在本次讨论范围内。但再说一次，两者之长是可以兼得的，唯一的缺点是复杂性，而这个问题我们已经为您解决了。这还是我们那条可爱的轨迹：位置更新非常少，然后您启用这项功能，应用IMU传感器融合后处理，它就会变成这样。您可以仔细看看，非常漂亮。可以看到它是怎样停下的，然后有很多很多点——因为有动量，点更多了——然后又一次加速。看到了吗？

29:17 先加速，然后又急停，非常漂亮。可以看到超声波的噪声，这是5厘米，所以我估计大约是正负1.5厘米，有时正负2厘米。但融合后的误差甚至比这还小。所以再说一次，您兼得两者之长：高更新率、对遮挡更强的稳健性，以及更高的精度。IMU传感器融合非常好。再看看拐角——等您用上这项功能时，可以自己试试，功能就在您手中。我们来看，这是其中一个例子。看，这是一个非常快的动作，基本上我们就这样非常非常快地移动，而

30:17 更新率约为8赫兹。也就是说只有一、二、三个点——我估计这个动作是在四分之一秒或三分之一秒内完成的——非常快的移动。没错，您能猜出这是个三角形，但只是一个非常粗糙的三角形。采用IMU传感器融合后，您会得到相当不错的轨迹，这与实时播放器不同：它显示的就是真实轨迹，没有实时播放器的那些局限。所以，当您面对要求极高、移动极快的场景时，IMU传感器融合就是您的解决方案。再说一次，我们已经讨论过：对于平滑运动，甚至实时播放器也足够好。所以对于非常平滑的运动，如果愿意，完全可以使用实时播放器。如果您想要

31:16 用于位置更新；但如果您希望对遮挡等情况有更强的稳健性，那么IMU传感器融合同样是您的选择。现在来谈谈后处理时间。正如我在开头提到的，我们目前的传感器融合是后处理。位置数据通过两种方式传输到计算机。一种是常规方式：由移动信标计算，或者由 调制解调器 计算。不过，这部分是超声波位置，照旧不变。但与此同时，移动信标上的IMU会产生大量数据，而且来得非常快：100赫兹的更新率。通过常规无线电信道不可能发送这些数据，因为带宽不够。

32:15 尤其是当有很多移动物体时——比如赛道上有很多车在跑。所以，每个移动信标都会对应一个 调制解调器 和另一个 调制解调器。专用 调制解调器——它们与普通 调制解调器 在硬件上相同，但软件是专用的，基本上只为一个目的服务：获取非常快的数据，即IMU数据，并把它发送给对应的调制解调器，然后由计算机收集。这样就有两路数据流，这些数据流当然都带有时间戳。因此，传感器融合算法能够在正确的时间对齐下，把超声波数据和IMU数据结合起来。经过传感器融合处理后，它就能输出

33:13 近乎完美的位置和100赫兹的更新率。但这是后处理，所以需要时间。需要多少时间？我举几个例子。目前我们还没有对此进行优化，但以后会优化，而且纯粹通过软件实现，硬件方面不需要做任何改动。但就目前而言，一台普通的笔记本电脑——不是台式机，不是什么特殊设备，也不在云端——一台非常普通的笔记本电脑的计算时间可用以下公式估算：10秒加上T（记录时长）乘以0.12，即日志记录的秒数乘以0.12。比如我有一段半管跳跃或BMX跳跃，总共持续5秒，那么处理时间就是10秒加5×0.12，即10.6

34:13 秒。所以跳跃结束后，10.6秒内就能得到理想的3D轨迹。如果是15分钟的赛车行驶，那就是10秒加15分钟（900秒）×0.12，结果约为2分钟。所以15分钟的轨迹，两分钟后就能得到精度优于2厘米、更新率100赫兹的理想轨迹，可以回放、查看弯道曲线等一切细节。正如我所说，这是在普通笔记本电脑上完成的，在台式机上显然会快得多。我们相信，将来能把后处理速度提高好几倍。但这是目前的情况。为了让大家有正确的预期，我再强调一次我们说的是哪种传感器融合，因为我们现在有好几种

35:12 融合，将来还会更多。有用于方向的传感器融合，也有用于位置的传感器融合。方向传感器融合是我们配对信标配置的一部分，至少已经推出两年左右了。这是一项复杂的任务，但没那么复杂。为什么？因为它基本上是利用位置，再加上内部的IMU，也就是陀螺仪。它不使用加速度计，所以不必担心加速度计造成的漂移，而陀螺仪的漂移很小。但和其他所有传感器融合一样，它兼得两者之长。首先，方向传感器融合即使在今天也能输出100赫兹。所以您已经有了它。如果您还不知道，其实您已经在用了。比如，即使没有位置传感器融合，您也已经有了：您有

36:10 8赫兹的位置更新，但方向的更新率是100赫兹。它从哪里来？来自方向传感器融合。再说一次，您有两个信标，信标之间用两根线连接，数据在它们之间传输。其中一个信标是主信标（Master）。主信标计算角位置，它知道各自的位置，并能精确地对方向进行传感器融合：方向基于超声波计算——基本上是基于这两个点各自的位置——再结合IMU，这里是陀螺仪。结果是得到非常可靠的内部方向。它主要来自陀螺仪，所以不会跳动；同时也不会漂移，因为它一直在被修正。

37:08 漂移一直依据超声波被消除。所以再说一次，两者之长兼得：没有漂移、100赫兹、非常高的精度和极高的分辨率——可达零点几度。但今天我们讨论的是位置传感器融合，这是一项新功能，复杂得多，但非常非常强大。因为正如我所说，它带来了理想的结果：100赫兹、更强的稳健性，同时精度更高，因为它减小了超声波位置测量的抖动。未来会怎样？未来是“圣杯”，即实时传感器融合。实时处理是更复杂的任务，因为正如您所见，目前我们需要几秒钟来做后处理。我们需要设法把它压缩，做到实时。这并不是

38:06 不可能完成的任务，但我们还没有做到。它的主要好处将是低延迟。因为现在我们为您提供100赫兹、非常高的精度和稳健性，但延迟是以秒计的。所以如果您需要，比如控制机器人、需要非常快地停下，那我们还做不到，那需要非常低的延迟。延迟是难度极高的问题。它是可以解决的，将通过传感器融合来解决，这是我们未来的计划，但还没有实现。只是让您了解我们的方向，以及将来可以期待什么。总结一下，我再强调一遍：现在有几种选择：一是实时播放器（Real-time Player），目前免费；二是基于IMU的传感器融合，即IMU传感器融合。

39:04 它现在就可以使用，提供100 Hz。这新的100 Hz与基于插值的实时播放器不同，它同时带来更强的稳健性和更高的精度。它适用于快速机械、快节奏的体育运动，比如非常快的无人机等。我相信您还有很多我们不知道的其他应用。顺便说一句，这个项目主要是由前沿案例推动的。本视频与网页上已发布的文章相关，请查看，那里以文字形式介绍了同样的内容。如果您还有疑问，请随时提出，我们乐意解答。请发邮件至info@marvelmind.com。非常感谢。