《Sensors》:Teardrop Risk Field-Based Spatiotemporal Path Planning Method for Mobile Robots in Dynamic Environments
编辑推荐:
超宽带(UWB)和惯性测量单元(IMU)的融合是列车及轨道导引目标定位的有效方案,因为UWB提供绝对测距结果,而IMU提供高更新率运动预测。在实际轨道交通系统中,UWB锚点通常沿轨道部署,其纵向间隔大、横向分离有限,以降低安装和维护成本。这种锚点部署导致观测矩
超宽带(UWB)和惯性测量单元(IMU)的融合是列车及轨道导引目标定位的有效方案,因为UWB提供绝对测距结果,而IMU提供高更新率运动预测。在实际轨道交通系统中,UWB锚点通常沿轨道部署,其纵向间隔大、横向分离有限,以降低安装和维护成本。这种锚点部署导致观测矩阵的条件数很大,因此由非视距(NLoS)传播引起的微小测距误差可能被放大为大的定位误差。为缓解视距(LoS)/NLoS干扰,研究人员提出了一种用于轨道约束UWB/IMU融合定位的传播可靠性估计方法。首先,将沿狭窄路径的轨道交通定位形式化为一个一维轨道约束问题,并将每个UWB测距结果转换为已知轨道中心线上的候选纵向坐标。其次,通过比较由UWB观测解算的运动增量与由IMU预测的运动,在滑动窗口内估计每个锚点-目标传播的可靠性。第三,将估计的可靠性纳入可靠性加权轨道域更新,然后进行闭环位置-速度卡尔曼修正。仿真结果表明,在混合LoS/NLoS场景下,所提方法实现了0.0090 m的NLoS区间均方根误差(RMSE)。与Track-EKF、Track-Gauss-AUKF、Track-Adaptive KF和Track-SW-FGO相比,所提方法将NLoS区间RMSE分别降低了92.9%、62.1%、92.8%和92.6%。在公开STAR-loc数据集上的补充实数据应力测试表明,在严格在线校准测距协议下,平均纵向RMSE为0.0555 m,验证了该算法在真实横向摇摆和异步传感器噪声下的实际适用性。
### 论文解读:基于运动一致性的轨道约束UWB/IMU融合定位传播可靠性估计方法
#### 研究背景与问题
在轨道交通系统及轨道导引自动导引车(AGV)中,连续可靠的定位是实现安全运行、运动控制和系统管理的基础。典型场景包括铁路、地铁隧道、地下矿井和工业导轨,这些环境下全球导航卫星系统(GNSS)信号可能不可用或不可靠。现有定位方案各有局限:轮式里程计因累积误差、打滑等问题精度下降;射频识别(RFID)等轨旁标志仅提供离散位置参考;激光雷达(LiDAR)地图匹配与多模态同步定位与建图(SLAM)需要额外硬件、高质量地图和显著计算资源。超宽带(UWB)与惯性测量单元(IMU)融合因其互补特性——UWB提供绝对测距,IMU提供高更新率运动预测——成为有前景的解决方案。然而,在轨道约束的狭长路径中,UWB锚点通常沿轨道纵向部署、横向间隔极小,导致观测矩阵条件数极大,非视距(NLoS)传播引起的微小测距误差可能被放大为大的定位误差。现有NLoS抑制方法多在测量层面进行鲁棒估计或LoS/NLoS识别,但缺乏在状态更新前对每个锚点-目标传播进行可靠性评估的机制;且现有UWB/IMU融合方法中,IMU主要作为过程输入,未充分利用其短时运动预测来评估传播可靠性。因此,研究人员提出了一种基于运动一致性的传播可靠性估计方法,用于轨道约束UWB/IMU融合定位,以有效抑制LoS/NLoS干扰。该论文发表在《Sensors》上。
#### 关键技术方法
研究人员主要采用了以下关键技术方法:首先,将轨道交通定位形式化为一维轨道约束问题,将每个UWB测距结果转换为已知轨道中心线上的候选纵向坐标,实现轨道域统一处理。其次,提出基于运动一致性的传播可靠性估计方法:在滑动窗口内,将每个UWB观测推导的纵向运动增量与IMU预测的短时运动增量进行比较,通过求解凸二次规划(投影梯度下降)获得每个锚点-目标传播的软可靠性权重。第三,设计可靠性加权轨道域更新机制,结合偏置增广、残差门控和保持抑制,产生标量伪测量;最后通过闭环位置-速度卡尔曼校正修正状态,抑制开环惯性漂移。所有方法在统一的轨道域基线(Track-EKF、Track-Gauss-AUKF、Track-Adaptive KF、Track-SW-FGO)下进行公平比较。实验基于仿真的轨道交通场景,并使用公开STAR-loc数据集进行真实数据测试。
#### 研究结果
**7.1 混合LoS/NLoS场景下的动态定位**
在默认混合LoS/NLoS场景(单锚点NLoS干扰持续40-45秒)下,所提方法在500次蒙特卡洛试验中实现整体RMSE为0.0108 m,NLoS区间RMSE为0.0090 m。相比Track-EKF、Track-Gauss-AUKF、Track-Adaptive KF、Track-SW-FGO,NLoS区间RMSE分别降低92.9%、62.1%、92.8%和92.6%。误差曲线显示基线方法在NLoS时段出现明显误差突发,而所提方法保持接近LoS水平。
**7.2 与更简单替代方法的比较**
在单链接NLoS场景下,修剪最小二乘(Trimmed LS)+IMU平滑方法NLoS区间RMSE(0.0050 m)略低于所提方法(0.0091 m),因为单残差修剪有效。但在两链接相关NLoS(Correlated-2link)场景下,多个NLoS传播共同支持错误解,所提方法NLoS区间RMSE为0.0089 m,而最接近的偏置感知EKF为0.0683 m,其他方法均超过0.36 m,证明IMU时间参考在相关多链接NLoS中的关键作用。
**7.3 LoS场景下的动态定位**
所有方法在LoS场景下均达到厘米级精度。Track-SW-FGO通过平滑窗口取得最低平均RMSE(0.005 m),所提方法为0.009 m,表明其可靠性正则化、偏置状态和门控逻辑在纯LoS下引入微小延迟,因此定位为鲁棒性导向估计器。
**7.4 IMU噪声敏感性**
在两种IMU加速度噪声方差(0.005 m
2/s
4和0.020 m
2/s
4)下,所提方法NLoS区间RMSE分别为0.0103 m和0.0115 m,变化较小,归因于短窗口位移增量而非长期惯性轨迹,以及重复速度校正。
**7.5 额外LoS/NLoS干扰模型**
在六种NLoS干扰模型(高斯、指数、对数正态、高斯混合、时间相关AR(1)、两链接相关阻塞)下,所提方法NLoS区间RMSE维持在0.0106 m至0.0148 m之间,而最近基线Track-Gauss-AUKF在五种模型中约0.022-0.024 m,在两链接相关场景下退化至0.3438 m,证明所提方法对分布形状、时间相关性和多传播同时阻塞的鲁棒性。
**7.6 超参数灵敏度、中心线误差与运行时间**
超参数分析表明,窗口长度和正则化参数主要影响可靠性序列时间平滑度;残差门控尺度直接决定从均匀UWB融合切换到NLoS抑制的阈值,RMSE范围0.010-0.041 m;先验强度平衡惯性连续与UWB校正。中心线偏移测试显示,硬轨道约束要求可靠地图,偏移几乎直接转换为定位误差。所提方法平均每步运行时间0.459 ms,仅占50 Hz采样预算20 ms的2.3%。
**7.7 多NLoS传播下的组件分析**
在两链接NLoS(C1)场景下,移除一致性学习使NLoS区间RMSE从0.377 m升至0.413 m,表明运动一致性提供有用区分;在三链接NLoS(C2)场景下,腐蚀传播占主导,完整方法(0.305 m)与无一致性学习版本接近,此时IMU先验和保持抑制成为主要稳定机制,移除IMU先验RMSE升至24.863 m,移除保持抑制升至1.065 m。
**7.8 真实数据可行性测试(STAR-loc数据集)**
使用公开STAR-loc数据集的5个近似直线子段,在严格在线协议下,所提方法平均纵向RMSE为0.0555 m,相比Track-EKF(0.1266 m)、Track-Gauss-AUKF(0.8290 m)、Track-Adaptive KF(0.1610 m)、Track-SW-FGO(0.4099 m)分别降低56.1%、93.3%、65.5%和86.4%,验证了方法在真实横向摇摆、非高斯噪声和异步测量下的有效性。
**7.9 NLoS传播下的静态验证**
静态测试中,未筛选测距RMSE为0.255 m,异常率20.9%;筛选后RMSE降至0.020 m,无样本超过0.4 m,残留P95为0.039 m。单快照静态定位中,Track-Robust LS因稀疏腐蚀可直接通过最大残差剔除而取得最低误差,所提方法信息来自时间一致性,在动态定位中更有价值。
#### 结论与讨论
研究人员提出了一种基于运动一致性传播可靠性估计的轨道约束UWB/IMU融合定位框架。首先将定位形式化为一维轨道约束问题,将UWB测距转换为候选纵向坐标以缓解几何病态;其次在滑动窗口内通过比较UWB运动增量与IMU短时运动预测动态估计每个传播的可靠性;最后将可靠性融入偏置增强轨道域更新,结合残差门控和保持抑制,再通过闭环位置-速度卡尔曼校正抑制LoS/NLoS干扰。全面评估表明,在良性LoS或孤立单链接NLoS下,简单替代方法(如修剪平滑或因子图优化)足够,但在持续或相关多链接NLoS阻塞下,所提多阶段架构表现出必要性。默认混合LoS/NLoS场景下,NLoS区间RMSE为0.0090 m,相比基线降低92.9%以上,且在六种NLoS干扰模型下保持0.0106-0.0148 m。运行时间验证了50 Hz在线可行性,STAR-loc真实数据测试平均纵向RMSE为0.0555 m,支持实际适用性。未来工作将开发定制化轨道导引UWB/IMU平台,扩展至曲线中心线,并引入概率地图约束处理横向运动与地图不确定性。