快速室内定位系统：IMU与超声波传感器融合
摘要
室内定位系统或GPS的典型位置更新率为 ~8-16Hz，足以满足大多数工业应用的需求，但并非全部。
将位置更新率提高到 100Hz 及以上的一种实用方法，是采用IMU与超声波 传感器融合 ，它结合了两种数据源的优点：IMU的极高更新率和鲁棒性，以及超声波室内定位系统的绝对坐标和无漂移特性。
基于传感器融合的快速室内定位系统的典型应用：
- 快速移动目标，例如卡丁车或快速运动的工业机械
- 高速运动：U型池滑雪跳跃、室内自行车公园、BMX小轮车赛道、滑板公园等。
简介
典型的 GPS 方案提供 ~8-16Hz 的更新率。这对普通驾驶来说足够，但对于 U型池滑雪者 的跳跃或卡丁车的快速转弯等场景则不够。
我们基于超声波的室内定位系统——室内“GPS”——也是如此：
- 其更新率约为8Hz
- 更新率首先受到超声波飞行时间的限制。例如，对于20米的子地图，即使在理论上，更新率也不可能超过(340m/s)/20m=17Hz。对于34米 => 10Hz
- 此外，还有其他耗时的环节——滤波、无线接口和其他计算——会进一步降低更新率
因此，对于10-15米的典型尺寸子地图，典型更新率为 ~8Hz （仅使用超声波定位时）。
提高更新率的限制因素
要实现尽可能高的更新率，请参阅文章： 如何提高位置更新率并降低延迟？
可以针对这一需求专门调整系统，进一步提高更新率，例如以牺牲滤波级别甚至精度为代价。但主要限制因素——超声波飞行时间——是无法消除的，除非缩小子地图的尺寸。
对于2-3米的极小子地图（例如空气曲棍球），更新率可达 40Hz 。但这是非常特殊的情况。
提高位置更新率的方案
提高位置更新率有以下几种方法：
1）使用实时播放器（Realtime Player）在Dashboard中进行后处理插值:
- 实现非常简单，但这种方法名副其实地会“切弯”：它利用现有的超声波定位数据在各点之间插值。如果相对于位置更新率而言存在急转弯和快速运动，系统无法得知，也无法还原
- 默认更新率为100Hz；更高更新率可按需提供
- 100Hz与1kHz更新率的实现难度几乎相同，因为都是对超声波位置更新的后处理
- 与100Hz相比，1kHz更新率的后处理并不能带来额外的精度
2）在Dashboard中进行传感器融合后处理:
- 这是真正的传感器融合，因为使用了不止一种数据源
- 典型的传感器融合是IMU与超声波，但也可以是里程计 + 超声波（机器人、AGV、叉车）或视觉 + 超声波（VR和AR）。还可以融合多种传感器：超声波 + IMU + 里程计 + 视觉。例如，我们的机器人就采用了这种方式： Boxie
- 这是一种相当复杂的方案，因为需要传输典型更新率为100Hz的快速IMU数据。当涉及多个移动信标时，无线电流量会增加10-100倍，并且需要在两端——卡丁车端和主计算机端——额外部署调制解调器
- 跟踪不会“切弯”，同时兼具很高的精度和速度
- 典型的真实更新率（而非上述方案1中的插值）为100Hz；如有需要，可达200Hz或400Hz
- 移动信标端的额外调制解调器，此时称为Radio Head Remote（RHR）。其硬件即Modem v5.1，配有专用软件。每个移动信标配一个
- Dashboard/计算机端的额外调制解调器，此时称为Radio Head USB（RHU）。其硬件同样是Modem v5.1，配有专用软件，同样每个移动信标配一个。RHR与RHU共同建立一条传输快速IMU数据的无线信道，与之并行的是相对较慢的超声波位置数据，后者经由以下信道发送： 常规的信标-调制解调器无线信道
- 用于启用IMU + 超声波传感器融合（后处理）的专用软件许可。每台移动载具一个许可
3）在外部计算机或机载设备上实时进行传感器融合处理:
- 与方案2类似，但处理是实时进行的
- 关键区别在于：方案2中位置数据的延迟可能达数秒，对于较长的轨迹甚至可达数分钟；而方案3在100Hz位置更新率下的延迟约为15毫秒
- 该功能仍在开发中。如果您有实际的商业应用需求，我们将加大投入，争取尽早完成
IMU + 超声波传感器融合后处理
IMU + 超声波传感器融合后处理是真正的传感器融合机制，因为它结合了两种位置数据源：
- 超声波
- IMU
惯性与超声波室内定位系统的优缺点
每种位置数据源都有各自的优缺点：
超声波室内定位系统的优点:
- 提供绝对位置数据
- 无漂移
- 噪声或非视距可能导致位置跳变
- 位置更新相对较慢——通常约8Hz
- 位置更新率高甚至非常高：默认100Hz，后续版本可按需提供200-400Hz
- 短时非视距不会导致位置跳变
- 通过对加速度计数据进行二次积分计算出的位置漂移非常快。使用现代MEMS IMU几乎不可能构建独立的惯性定位系统。此类系统必须依靠传感器融合，例如与超声波或其他能够消除漂移的数据源融合
- 只能提供相对位置数据
传感器融合的工作原理
下面通过实例看看传感器融合是如何工作的。
过程简述：
- 基于超声波的室内定位系统输出一条定位轨迹。无论是否启用传感器融合，这条轨迹的数据流传输方式都相同。详见 室内定位系统的架构
- 与超声波定位轨迹并行的，还有一路快速的IMU数据流（原始3D加速度计数据和原始3D陀螺仪数据），以100Hz的更新率从移动信标经RHR和RHU传输到中央计算机
- 超声波和IMU两路数据流都带有时间戳，因此能够精确、正确地合并
- 系统接收这两路数据流并进行数学运算——实际上就是传感器融合
传感器融合的效果
超声波定位轨迹——矩形
下方轨迹的特征：
- 位置更新率约8Hz
- 比例：点间距5厘米
- 仅超声波
- 移动信标快速运动
基于传感器融合的定位轨迹——矩形
下方轨迹的特征：
- 结果：与IMU传感器融合后，位置更新率达100 Hz——橙色点
- 原始：超声波位置更新率约8Hz——蓝色点
- 比例：点间距5厘米
- 移动信标快速运动
现在轨迹是真正原生的100Hz，同时对短时遮挡的鲁棒性也更强
融合后的IMU（蓝色）与超声波（橙色）位置点特写
可以看到，IMU点在接近拐角时减速，过了拐角后再加速。
超声波定位轨迹——三角形
条件与上例相同，只是轨迹形状不同，以便展示性能。
基于传感器融合的定位轨迹——三角形
融合后的IMU（蓝色）与超声波（橙色）位置点特写——三角形拐角
传感器融合与实时播放器（Realtime Player）对比
超声波数据不足时，实时播放器会“切角”
请注意，当超声波定位点太少时（目标移动太快或位置更新太稀疏），实时播放器会明显“切角”，从而导致更大的定位误差。
- 移动目标快速运动
- 超声波更新率约8Hz——蓝色点
- IMU传感器融合——100Hz——橙色点
- 实时播放器——100Hz——粉色点
实时播放器结果，设置：前向0、后向5
实时播放器结果，设置：前向1、后向5
实时播放器结果，设置：前向3、后向5
- 需要的前向定位点越多，实时播放器因缺少前向点而无法构建轨迹的距离就越长
- 总点数越多，轨迹越平滑，对位置点缺失的鲁棒性也越强，但同时切角也越明显
对于平滑曲线，实时播放器足以胜任
更新率足够高时，实时播放器效果良好
后处理时间
目前可用的IMU + 超声波传感器融合（后处理）功能，后处理速度约为10+0.12秒/1秒（固定约10秒，每1秒数据另需约0.12秒）。例如：
- 一次5秒的快速跳跃，后处理需要10+5*0.12 => 10.6秒
- 一次15分钟的卡丁车行驶，后处理需要10+15*60*0.12 => 2分钟
以上数据是在一台Win11、Intel i5笔记本电脑上计算得出的。性能更强的台式电脑能更快地得到后处理结果。
未来的代码优化有望显著缩短后处理时间，且无需更换硬件或购买新许可证。
勿混淆位置与方向的传感器融合
用于定位的传感器融合是一项复杂的任务，因为惯性定位系统中的位置是通过对加速度计数据二次积分计算出来的。要清楚地区分被跟踪移动目标的实际加速度与地球重力的影响并不容易。因此，不采用传感器融合的惯性室内定位系统无法工作。
相比之下，用于角度的传感器融合要容易得多。我们商业化提供这项功能已有多年。详见：
总结
传感器融合 是一项复杂的功能，但能为精确室内定位系统带来 出色的性能 ，因为它能高效地融合多个来源的数据。
例如，独立的室内定位系统典型更新率约为8Hz，而采用 IMU传感器融合 的室内定位系统，更新率可达 100Hz ，同时保持与不使用IMU传感器融合的系统相同或更高的精度，并且对短时非视距情况和外部声学噪声具有更强的鲁棒性。
The 实时播放器 足以胜任 平滑曲线 ，或更新率相对于曲线急转程度足够高的曲线。