做过室内定位的人应该都有同感GPS在室外是王者进了室内就彻底抓瞎。我最早接触行人室内定位是在做商场导航Demo的时候第一反应是上蓝牙信标方案真正测完之后才发现很多场景其实更适合用惯导——也就是IMU惯性测量单元配合航位推算的思想来做。这套方案业内一般叫PDRPedestrian Dead Reckoning行人航位推算不依赖Wi-Fi、蓝牙甚至地图完全靠传感器自己“走一步算一步”。这篇文章就把行人惯导定位的原理、实现、误差来源和实测调参经验完整梳理一遍适合刚接触室内定位、正在对比技术方案的开发者参考也适合想从零搭一个定位原型的同学照着动手。1. GPS失效之后惯导为什么是绕不开的答案1.1 室内场景对定位技术有一套“反直觉”的要求很多从室外定位转来做室内的朋友第一反应是把GPS的思维搬进来多搞几个基站测距离、算坐标。但室内环境完全不是这么回事。卫星信号在钢筋混凝土里衰减到几乎不可用就算偶尔收到一点反射后的多径信号也会让定位结果在十几米的范围内乱跳。另一条路是布设无线基础设施比如Wi-Fi指纹、蓝牙信标、UWB基站。这类方案精度不错但有个前提你得先把网络铺好而且环境一变家具挪了、AP换了、人流密度变了指纹库就要重新采。在应急场景里比如消防员进火场、救援人员下矿井、安保人员在陌生楼宇寻人根本没有时间去布基站这时候唯一还能用的就是惯导。这里有个反直觉的事实无线方案看起来“高科技”但它的定位精度取决于环境先验知识惯导方案看起来“很笨”只知道加速度和角速度但它不依赖任何外部设施走到哪里都能算。两类方案的本质差别不是精度而是“自主性”。1.2 惯导方案的核心优势与天然短板惯导在室内定位里的定位是这样的短时间高精度、长时间漂移。刚起步的几十秒内如果你做了准确的初始化和零速修正误差通常能控制在行走距离的1%到2%但走十几分钟之后航向漂移和步长误差会不断累积轨迹可能从“大致正确”变成“面目全非”。所以行业内很少把惯导作为唯一的定位手段用更常见的姿势是惯导负责“短时递推”提供高频、平滑的相对位移蓝牙/UWB/Wi-Fi负责“绝对校正”每隔一段时间把惯导拉回真实位置地图约束负责“兜底”让轨迹不至于穿墙。但理解惯导仍然很重要因为它是整个融合方案里唯一“持续在线”的定位源。无线信号会丢、指纹库会过期IMU却从开机那一刻起就在输出数据。这也是为什么各大手机厂商的地图导航、室内步行导航里都藏着惯导的影子。1.3 一个关键的认知转变惯导是“推算”而非“测量”做惯导最容易犯的错是把它当成GPS那样的绝对定位工具。GPS是“我在哪里”直接读出来惯导是“上一次我在哪里我走了多远、朝哪个方向所以我现在大概在哪里”。每一个新位置都依赖上一个位置误差像滚雪球一样往前传。这个认知非常重要它会决定你怎么设计系统。比如你在起步点是A走了十步到B惯导告诉你的是“B相对于A的位移是8.5米、方向是北偏东20度”而不是“B的经纬度是多少”。如果你不事先知道A点的绝对坐标惯导就只能在“相对坐标系”里画轨迹。理解了这一点下面所有环节——步频怎么数、步长怎么估、航向怎么判——都是在回答同一个问题怎么让“推算”这一步算得准。2. PDR航位推算的四块基石步频、步长、航向、坐标更新2.1 步频检测的本质是识别一个“准周期信号”人走路的时候躯干和腿部的加速度会呈现明显的周期性波动脚落地瞬间垂直方向加速度会有一个冲击峰值脚抬起来再往前迈加速度又会变化。这个波形不一定完美正弦但它是“准周期”的——每走一步波形就会重复一次。步频检测最朴素的做法是波峰检测import numpy as np def detect_steps(acc_norm, fs, peak_threshold12.0): step_times [] min_interval int(fs * 0.3) # 最小步间隔防止同一波峰被数两次 last_peak -min_interval for i in range(1, len(acc_norm) - 1): if acc_norm[i] peak_threshold and \ acc_norm[i] acc_norm[i-1] and acc_norm[i] acc_norm[i1]: if i - last_peak min_interval: step_times.append(i) last_peak i return np.array(step_times) / fs这里有几个细节很容易踩坑。一是加速度波形会有大量高频抖动直接做峰值检测会把一次振动数成好几步所以通常要先做低通滤波把10Hz以上的噪声洗掉二是阈值不能拍脑袋定不同人的走路冲击力度差别很大固定阈值会导致有人被漏步、有人被多步三是“最小步间隔”必须有否则一步里的两个小波峰会被算成两步。更鲁棒的做法是检测“波峰和波谷的落差”峰值-谷值或者用自相关分析找到周期长度但作为入门波峰检测已经够用了。2.2 步长估计固定值不行得用“动态模型”很多初学者以为步长就是一个常数0.7米。实测走下来你会发现不同人的步幅差很多同一个人在不同速度下步幅也差很多。快走和慢走步幅能差出30%。业界常用的经验模型是Weinberg模型它利用每一步垂直方向的加速度波动幅度来估算步长步长 k × (acc_max - acc_min) 的1/4次方其中acc_max和acc_min是同一“步”内垂直加速度的最大值和最小值k是个人标定系数。这个模型的直觉是步幅越大身体起伏越剧烈垂直加速度的波动幅度自然就大。如果你的传感器固定在鞋上还可以用“摆腿角度积分”的方法通过陀螺仪积分出腿摆过的角度估算步长。这个方法更准但只适用于足部安装。步长估计还有一个简单实用的变体用“步频”映射步长走得快就步子大但模型需要大量数据去拟合。我建议从Weinberg起步因为参数少、解释性强实测精度通常比固定步长高出一大截。2.3 航向估计PDR里最大的坑没有之一位置推算的公式其实很简单每一步算一个位移矢量再累加。但这个矢量有两个分量步长是模长航向是方向。步长估偏了轨迹只是长短问题航向估偏了轨迹会整条转向走100米偏5度终点就偏了近9米。航向的来源有三个陀螺仪积分短期很准但有漂移每秒0.1度的零偏在10分钟里就累积出60度的偏差磁力计提供绝对航向相对地磁北极但它对环境中铁磁性物质极度敏感室内钢筋、金属门框、电子设备都会让它狂跳加速度计可以用来计算俯仰和横滚但无法直接得到航向。行业内普遍的做法是互补陀螺仪负责短时积分磁力计负责长时纠偏。具体实现上可以做一个简单的补偿计算当前窗口内磁力计航向与陀螺仪积分航向的差值按一个很小的比例系数把陀螺仪的漂移拉回来。系数不能太大否则磁力计的噪声会污染整个航向也不能太小否则漂移完全压不住。这个“度”要实测去调。2.4 坐标更新把空间想象成一张方格纸把行走过程离散化成“每走一步更新一次”的链式结构位置更新的原型非常简单x 0.0 y 0.0 heading 0.0 # 相对初始方向的角度 for step_i in range(num_steps): length estimate_step_length(step_i) yaw get_current_heading(step_i) x length * math.sin(yaw) y length * math.cos(yaw)这里要特别注意坐标约定。有的系统以正北为0度、顺时针增大有的以正东为0度、逆时针增大sin和cos哪个对应x、哪个对应y不同资料里写法都不一样。保持一致就行但一定要在代码旁边注释清楚约定不然项目一搁两周再看连自己都容易绕晕。实际工程里很少用“每步更新”而是用“每帧更新”——IMU数据一般50Hz到200Hz每次拿到新数据就积分一次位置。这样做的好处是轨迹更平滑也更方便和其它传感器按时间戳对齐。但“按帧积分”的代价是误差累积更快因为每一帧都在做积分所以零速修正ZUPT就变得必不可少。3. 器件选型与误差来源IMU不是越贵越好3.1 消费级IMU与工业级模块的真实差距做行人惯导第一步是选器件。这里的陷阱是很多人以为精度越高越好上来就买昂贵的高精度惯性导航模块。实际上行人惯导对IMU性能的要求并不极端——因为我们不是在解导弹弹道而是利用“走路过程”中断续出现的静止时刻做修正。消费级的IMU芯片常见于手机的六轴传感器零偏稳定性通常在每小时几十到上百度的范围内看起来不太好但配上零速修正之后在100米的直线行走测试里依然能做出8米以内的误差。工业级模块的零偏稳定性高一个数量级价格也高一个数量级但优势主要体现在长时间无修正的积分场景纯PDR里边际收益没那么大。初学阶段不要纠结器件先用手头任意一部手机或者一个几十块钱的九轴模块跑通算法等你确认问题出在“数据处理”而不是“传感器噪声”的时候再考虑换更好的IMU。3.2 加速度计、陀螺仪、磁力计各有各的“性格”传感器擅长的事糟糕的事在PDR里的角色加速度计感知直线加速度、重力方向高频振动噪声大步频检测、步长估计、姿态初值陀螺仪感知转动角速度瞬时响应快积分后漂移不收敛短时航向递推磁力计提供绝对航向参照受铁磁物质和电子干扰长时航向纠偏这张表的核心信息是没有哪个传感器能单独扛起航向估计。陀螺仪是“短跑冠军”起步几秒极其精准但跑长距离会跑偏磁力计是“方向感好的老人”大方向记得准但遇到金属门就犯迷糊。PDR的航向模块本质上是这两个冤家的调停者。3.3 安装位置决定了算法的上限同一个IMU放的位置不同PDR的效果天差地别。我在测试中总结过三类安装方式足部安装绑在鞋面或鞋跟最容易做零速修正因为脚着地时的静止区间非常明显还可以用脚摆动的角速度估计步长。缺点是传感器撞击感强数据噪声大而且用户穿戴不舒适。腰部或躯干安装手机放口袋算一种舒适适合长时间佩戴但波形幅度小步频检测的鲁棒性下降零速检测也没有那么清晰。手持安装手机拿在手上边走边玩手机的场景最自然但手臂摆动会叠加大量额外运动是三种方式里最难处理的。我的建议是如果你做的是工程产品优先足部或腰部如果你只是拿手机那套数据做实验也是可以的但要预期到“手机放口袋”和“手机拿手上”的算法参数完全不同。3.4 温漂与零偏最不起眼却最致命的细节IMU的零偏不是恒定不变的。温度变了它会漂电压变了它会漂甚至电路板受潮它也会漂。很多新手的误区是在系统启动时静止采集了几秒钟求个平均当成零偏后面就再也不管了——这是不够的。一个简单有效的补救是“滑动零偏校正”每隔一段固定时间如果检测到系统处于静止状态加速度方差很小且角速度幅值很小就用这段静止数据的均值更新一次零偏。这样就能把温漂导致的长期误差压住代价只是多几行代码。4. 零速修正与姿态解算把累计误差按在地上摩擦4.1 为什么纯积分会“飞”如果你把加速度计的数据直接积分成速度、再积分成位移会发现轨迹在几秒之内就开始飞。原因很简单加速度计的零偏哪怕只有0.01m/s²积分十秒后速度就会多出0.1m/s再过十秒位移误差就增长到米级。误差随时间的平方增长指数级等会儿可能有点夸张二次方增长已经足够可怕了。在行人PDR哈欠的场景里有个天赐的好条件人的脚落地时速度会短暂归零。如果我能检测出“脚已经停了”那我就能趁机把积分出来的速度强制归零——这个动作就叫零速修正ZUPT。4.2 零速修正的直觉与实现零速修正的思路非常朴素你走一步、停一瞬再迈下一步。当脚接触地面、完全静止的那几十毫秒里加速度计的读数理论上应该只有重力陀螺仪读数理论上应该接近零。利用这个信息可以做两件事检测到零速区间时把速度误差清零用零速区间内的加速度和角速度数据重新校正姿态。零速检测最常见的判据是“滑窗统计”在一个滑窗比如0.1秒内如果加速度幅值接近重力9.8m/s²、加速度的方差很小、角速度幅值也很小就判定为零速。三个条件缺一个误判率就会明显上升。如果这三个条件都用上零速检测的可靠性会高很多。实现框架如下def detect_zero_velocity(acc, gyro, fs, window10, g9.8): n len(acc) prob np.zeros(n) for i in range(window, n - window): acc_win acc[i-window:iwindow] gyro_win gyro[i-window:iwindow] acc_var np.var(np.linalg.norm(acc_win, axis1)) gyro_avg np.mean(np.linalg.norm(gyro_win, axis1)) acc_mean np.mean(np.linalg.norm(acc_win, axis1)) if acc_var 0.8 and gyro_avg 0.2 and abs(acc_mean - g) 1.0: prob[i] 1.0 return prob 0.5这里阈值怎么取值要看你的安装位置和行走习惯后面我专门讲调参。4.3 姿态解算为什么不能把传感器数据直接用一个必须解决的问题是“坐标系”。IMU输出的加速度和角速度是“机体系”传感器自己的坐标系而我们要算的位置是“导航系”水平的东西南北坐标系。两者之间有旋转关系而这个旋转关系是由姿态决定的——传感器是歪的、斜的、翻过来的测量出来的“前后左右”方向就完全没法直接用。所以姿态解算是一切的前提。常用的做法有两种互补滤波把加速度计得到的长期姿态和陀螺仪积分的短期姿态按比例融合计算量小、实现简单适合单片机。卡尔曼滤波用状态空间模型估计姿态参数多、调参复杂但效果上限更高。入门阶段互补滤波就足够了。先把姿态算准再把机体系的加速度投影到导航系得到“水平方向的加速度”才能进一步做步频和步长分析。很多新手调不通PDR问题不在步频也不在步长而是姿态根本没有解对全是在拿错误坐标系的加速度硬算。4.4 不绑在脚上时用“步态相位检测”替代ZUPT足部ZUPT效果好但穿戴场景受限。如果你用腰部的IMU脚落地时的“零速”只体现为躯干垂直加速度的一个平稳谷段不能直接套用速度归零。替代方案是“步态相位检测”把每一步分为“摆动相”和“支撑相”在支撑相把向下的速度累加值覆盖掉间接压制垂直方向上的漂移。或者用更轻量级的“峰值谷值对消”——因为每一步的垂直速度波动是正负交替的只要保证波峰波谷被完整捕捉到积分误差在一个周期里会被大部分抵消。这个技巧能让腰部安装的PDR从“没法用”变成“可用”但精度会打折扣。工程上如果你有选择权我还是推荐优先足部。如果只能用腰部那就要接受大概2%-5%的距离误差然后在后续的融合阶段靠无线校正去补。5. 从零搭一个可用的行人惯导原型流程与代码骨架5.1 数据采集端的设置第一步是把IMU数据采出来。注意几个关键设置采样率50Hz是最低可接受值200Hz是推荐值。太低会漏掉脚步冲击波形中的峰值太高则计算量变大但收益有限。量程加速度计量程±8g以上因为走路时的冲击力可能超过1g陀螺仪量程±500dps以上。静止初始化设备上电后保持静止5到10秒。这段时间用来估计零偏、确定重力方向同时建立导航系的初始姿态。如果跳过静止初始化后面所有数据在坐标系里都是歪的整个轨迹会以肉眼可见的速度发散。采集数据的格式建议是CSV每行一个时间戳、三轴加速度、三轴角速度、三轴磁力计。先离线调试等算法跑通了再考虑实时传输和在线解算。5.2 预处理滤波的“度”在哪里原始加速度数据直接进算法之前一定要做低通滤波。原因前面说过高频振动会产生大量伪波峰步频检测会被彻底扰乱。最简单的做法是滑动窗口平均窗口长度通常取采样率的十分之一到五分之一比如100Hz采样下用10到20个点的窗口。也可以用一阶低通滤波alpha 1 / (1 fs * tau) # tau约为0.05到0.1秒 filtered alpha * raw_prev (1 - alpha) * raw这里有个反直觉的经验滤波太狠和完全不滤波一样糟糕。如果窗口开得过大脚步的冲击峰会被削平步频检测会漏步如果太短高频噪声又会让检测器多步。我第一次用100Hz数据时窗口选了50个点去“保平滑”结果步数直接少了三成。后来一步步往下试发现10到15个点最合适。5.3 从滤波后的数据里数出每一步步频检测的核心流程是计算三轴加速度的模长并归一化低通滤波去除高频抖动用滑动窗口去找波峰波峰必须高于动态阈值通过最小间隔抑制同一步内的重复峰值每检测到一个有效波峰步数加一同时记录该“步”内的加速度最大最小值用于步长估计。动态阈值怎么定最简单的方案是取过往N步中峰值均值的一定比例比如“当前步峰值 最近10步平均峰值 × 0.7”。这样快走和慢走都能适配比固定阈值稳得多。我实测下来动态阈值方案在快慢交替行走时步数误差能控制在5%以内。5.4 每一步都更新步长、航向然后画轨迹步长用Weinberg模型航向用陀螺仪积分 磁力计纠偏最后把每一步的位移累加进坐标轨迹就出来了。这个原型非常简短核心逻辑也就几十行。但你真正开始跑起来之后会发现三个问题立刻浮现轨迹会慢慢往一个方向偏转航向漂移、总距离偏短或偏长步长模型没标定、起步方向和真实方向对不上初始化没做好。这三个问题分别对应下一章的三类调参经验。5.5 离线评估怎么知道自己做得好不好评估PDR的标准不是“看起来像不像”而是数值。我的建议是准备一条已知长度的路线比如走廊里从A点走到B点直线距离50米中间转两个直角弯。然后算三个指标终点绝对误差算法推算出终点与实际终点的平面距离。相对误差百分比终止误差除以轨迹总长度通常以2%-5%为可接受区间。航向误差整条轨迹的端到端朝向与真实朝向的夹角。这三个指标能帮你定位问题出在哪一环。如果终点误差很大但总距离基本对那是航向漂移如果总距离差得远那是步长或步频如果转弯后轨迹形态完全错乱优先检查姿态解算。6. 实测中的坑与调参心得这些经验文档里没有6.1 步频阈值固定值就是给自己埋雷我一开始做步频检测时用的是固定阈值取的是“自己走路的平均冲击峰值”的一半。但换了一个更瘦更矮的同事去测他的冲击峰值比我小很多每一步都触发不了阈值整条轨迹步数少了一半。固定阈值的问题在于它假设所有人的步态特征是一样的。解决办法前面提过——动态阈值。但因为很多人第一次接触我再给一个更具体的建议用“滑动百分位”取过去2到3秒加速度模长的95%百分位作为当前峰值参考再乘以0.6到0.8作为检测阈值。这样无论走路快慢阈值会跟着波形自动缩放。6.2 磁力计在室内几乎是个“反复无常”的传感器磁力计的原理是测量地磁场方向。问题在于室内环境到处是人造的磁场钢筋、钢结构、电梯井、音箱磁铁甚至旁边路过的手机电池都能让磁力计读数发生跳变。怎么判断当前磁力计可信一个简单办法是算“磁场模长”。正常情况下地磁模长在某地是相对稳定的比如45到55微特斯拉之间如果你发现模长在短时间内波动超过10%说明周围有强干扰环境这时候必须降低磁力计在航向融合里的权重。比数值本身更重要的是它的波动幅度——我见过在金属货架旁边走磁力计航向在几秒内跳了90度的场景。遇到这种环境宁可让陀螺仪多扛一会儿也不能让磁力计把航向带飞。6.3 初始化方向错了整条轨迹都是白算某个版本的Demo里我发现一个奇怪现象明明算法逻辑没问题但画出来的轨迹总是整体旋转了一个角度。排查了半天才发现是因为设备放置时没有对准正北方向而我的代码里默认初始化航向为0度正北。用户把设备往西南方向一装整条轨迹就跟着偏到西南去了。解决办法有两个一是增加“初始朝向校准”步骤让用户先朝正北站好或者用磁力计读取初始朝向二是做轨迹后处理时用终点和起点的真实方位去反推初始朝向误差。不管用哪种都要在启动阶段就把绝对朝向定准否则后面所有算法优化都白费。6.4 长时间运行的漂移处理PDR的“保质期”有上限刚做完定位时我满心欢喜地跑了个1小时测试结果轨迹图让我怀疑人生走了20分钟以后整条轨迹开始像一个慢慢膨胀的气球越走越圆越走越偏。这就是PDR的宿命——它在短时间内的相对精度极好但误差会慢慢累积。1分钟内的定位误差可以控制在2米内10分钟就可能到20米以上。要想做长时间稳定定位必须引入外部的绝对位置修正。最简单的融合方式当收到蓝牙/Wi-Fi/UWB定位结果且与惯导推算位置的距离差在合理范围内直接将惯导位置“拉”到无线定位结果附近同时重置航向和位置估计的不确定度让惯导带着新的起点继续推如果超过一定时间没有外部修正就降低惯导的可信度输出并在界面上提示“定位精度下降”。我在做商场导航项目时的经验是无线修正信号每隔30到60秒来一次融合的轨迹就可以长期保持稳定。单纯指望惯导撑全场是对器件和算法的不切实际期待。6.5 我没写进原型的那些“调味料”原型代码之外还有几个细节能让定位质量再上一个台阶步数平滑连续几步的步长可以互相平滑防止单步突变造成轨迹抖动转向处理检测到角速度峰值时把航向变化单独切出来避免转弯过程里的步长模型失真地图约束如果已知室内平面图可以把轨迹点约束到可通行区域内。这是性价比最高的“后处理优化”哪怕只做简单的“穿墙拉回”轨迹观感都会好非常多。做惯导定位这几年我最大的感受是它不像地图匹配或指纹库那样“开箱即用”但恰恰是这种需要自己调教每一个环节的体验让人真正理解定位的本质。最后再分享一个小技巧如果你刚开始试验别急着上卡尔曼滤波那些花活先用最朴素的PDR跑通一条直线再把航向误差一点一点压下来。每一步误差砍一半整条轨迹就会给你惊喜。