无基站多目标协同定位系统

适应步态变化的零速检测#

针对步态变化导致零速判定不稳定的问题，检测器融合两类信息：

• 残差能量：利用短时加速度、角速度残差，捕捉足部触地前后的状态变化。
• 低频角运动包络：保留步态的周期性特征，辅助判断支撑阶段。

通过软归一化压缩，降低大动态范围对判定的影响，避免引入较长的检测延迟。

一帧输入，一次状态输出#

系统采用六状态无迹卡尔曼滤波器（UKF），直接在导航坐标系中建模复合加性加速度误差，结合条件触发的零速更新（ZUPT）抑制累积漂移。

每接收一帧 IMU 数据，就在同一处理周期内发布对应时刻的状态；摆动阶段也持续输出当前状态，无需等待下一次支撑确认。这使运动数据能够及时参与后续协同定位，减少状态陈旧与时间对齐问题。

从距离恢复相对拓扑#

在启动时刻，使用多维尺度分析（MDS），仅依据 UWB 节点间距离恢复相对空间拓扑，无需预设节点位置。

利用运动完成方位对齐#

节点开始运动后，通过时空联合优化，求解整体旋转与各节点相对方位，将不同初始朝向下得到的局部惯导轨迹映射到统一坐标系。

针对距离重建存在的镜像歧义，采用双分支并行求解，比较两种构型的观测残差。原文实验中，正确构型的残差比镜像构型低数个数量级。

单节点惯导验证#

足载惯导实验使用 ESP32-C6（160 MHz）与 BMI088。室内实验在约 15 m × 14 m 的半篮球场进行，两名参与者分别以 90、100、120 步/分钟完成重复闭环行走，并进行了 90 至 120 步/分钟连续变化的步频实验。

室外长距离行走实验进一步观察了连续轨迹传播与周期性误差抑制。与等待支撑确认后才发布状态的延迟基线相比，一帧输入、一次输出的机制提供了更连续的当前状态输出。

多节点协同验证#

协同实验在 28 m × 15 m 的室内篮球场进行，5 名参与者从分散位置、随机初始朝向出发，执行直线、直角转弯、环形与不规则运动，全程不使用预部署基站或预设位置。

UWB 模块使用双边双向测距，典型测距精度为 5–20 cm。这一数值描述的是节点间测距精度，并不等同于完整系统的定位误差。

真实硬件实验表明，在传感器噪声、同步误差和复杂运动条件下，算法能够恢复连续多节点轨迹，为无 GNSS 环境中的协同导航提供自主初始化方案。