《Machines》:Motion-State-Aware Adaptive Multi-Sensor Fusion Localization Using Sliding-Window Incremental Factor Graph Optimization
编辑推荐:
复杂运动环境下无人车辆准确实时的定位面临异步多传感器测量、时变测量质量以及长期因子图优化计算负担增长的挑战。为解决这些问题,研究人员提出AVFGO,一种用于运动状态感知自适应多传感器融合定位的滑动窗口增量因子图优化框架。研究人员采用IMU预积分作为主状态传播主
复杂运动环境下无人车辆准确实时的定位面临异步多传感器测量、时变测量质量以及长期因子图优化计算负担增长的挑战。为解决这些问题,研究人员提出AVFGO,一种用于运动状态感知自适应多传感器融合定位的滑动窗口增量因子图优化框架。研究人员采用IMU预积分作为主状态传播主干,将异步激光雷达里程计、姿态航向参考系统(AHRS)、轮式里程计和气压计测量统一表示为因子图约束。滑动窗口边缘化机制通过将历史信息保留为先验因子来限制优化规模。运动状态感知融合速率策略控制外部因子的插入密度,残差驱动的向量式自适应加权模型调整不同测量维度下异构传感器的协方差。在GNSS拒止轮式无人车数据集上的实地实验表明,AVFGO的3D位置均方根误差(RMSE)为0.380 m,相对于IFGO降低65.36%,相对于SWIFGO降低52.71%。平均单步优化时间为0.0389 s,相对于IFG加速11.9倍,相对于SWIFGO减少75.81%。这些结果表明所提框架在提升精度和实时性能的同时,受限于单平台实地测试范围,这在文中被明确讨论为未来验证的方向。
论文发表在《Machines》。研究背景方面,复杂环境中连续可靠无缝的高精度定位对无人巡检、救援等智能移动系统至关重要,对导航定位技术的鲁棒性、实时性及环境适应性提出严格要求。单一传感器难以在复杂场景下长期提供稳定位姿信息,惯性导航系统自主且输出率高、短期运动信息准确但误差随时间累积,全球导航卫星系统(GNSS)在开阔天空可提供稳定绝对定位,但在城市峡谷、隧道、室内地下及强电磁干扰环境中易受多径、信号遮挡与中断影响。多传感器融合成为移动机器人与智能巡检平台常用方案,但导航传感器在采样率、测量精度、可观性及失效模式上存在差异,测量通常异步到达,且环境与载体运动状态导致传感器可靠性时变,因此多传感器融合定位系统需支持灵活接入与异步处理,并根据当前运行状态自适应选择融合策略。传统滤波方法常将延迟测量传播至当前融合历元引入外推误差,且递归滤波边缘化历史状态主要依赖当前新息,难以充分利用历史观测相关性获得全局一致估计。因子图优化(Factor Graph Optimization, FGO)以概率图模型表示多传感器约束并按时间戳关联测量因子与状态节点,适合异步多速率即插即用定位问题,已广泛用于同步定位与建图(SLAM)及激光惯性、视觉惯性等组合导航系统。但传统全图方法中状态节点与因子随运行时间持续累积,计算复杂度大幅增加,限制其在无人平台实时部署。现有滑动窗口因子图方法在退化环境中异常观测仍会降低估计精度,且常依据经验传感器参数赋予固定测量协方差与预设权重,无法捕捉不同环境、运动状态与测量维度下传感器时变可靠性,需在因子图框架内建立传感器状态与残差驱动的可靠性评估机制以实现跨传感器与维度的自适应权重分配,提升复杂退化环境鲁棒性。
研究人员开展的研究为提出AVFGO框架并在GNSS拒止轮式无人车实地数据集验证其精度与实时性。结论为该框架在测试平台上3D位置均方根误差(RMSE)达0.380 m,较IFGO降低65.36%、较SWIFGO降低52.71%,平均单步优化时间0.0389 s,较IFGO加速11.9倍、较SWIFGO减少75.81%,在提升精度与实时性的同时受限于单平台实地测试范围。意义在于通过运动状态感知因子调度与残差驱动向量式自适应加权在受限滑动窗口因子图内的集成,改善复杂运动环境下多传感器融合定位的性能与计算效率。
主要关键技术方法包括:构建滑动窗口增量因子图优化框架,以IMU预积分作为相邻状态间高率运动传播主干,将LiDAR里程计、AHRS、轮式里程计(ODO)、气压计异步测量构造为统一因子图约束,通过边缘化机制将窗口外历史信息与关联因子压缩为先验因子作用于保留窗口状态以限制优化变量数为窗口长度W而非全轨迹长度N;设计基于IMU辅助的运动状态识别指标,包括比力幅值与重力偏差Δa、角速度幅值ωmag、偏航率ψ?,通过二值运动状态指示器调度外部因子插入密度与融合频率调整系数αs以在稳态减小冗余插入、在动态或异常区间提高约束密度;提出残差驱动的向量式自适应加权模型,对各传感器不同测量维度独立计算归一化残差度量r?j,k(s),依据正常阈值τnorm与异常阈值τanom及调整斜率kadj计算自适应调整系数λj,k(s)并约束于[1, λmax],以有效协方差Σ′k(s)=Λk(s)Σk(s)Λk(s)实现分量级协方差膨胀与因子权重下调,其中LiDAR水平分量结合运动状态指示Mk调整,AHRS横滚俯仰依据Δa调整、偏航结合偏航残差与偏航率调整,ODO前向强约束调整前向系数、侧向与垂向作弱固定约束,气压计依据高度跳变Δhk与垂直一致性误差ev,k调整。实验在搭载NVIDIA Jetson AGX ORIN、STIM300 IMU、C16 LiDAR、AHRS、ODO、气压计的地面轮式无人车平台开展,GNSS拒止环境下以NovAtel SPAN-KVH1750组合导航系统高精度解算作为参考真值但不参与因子图融合,于Ubuntu 20.04与ROS Noetic部署采集预处理与融合算法,轨迹含直行、转弯、加减速及局部环境变化。
研究结果部分保留小标题并说明如下。
- 2.
滑动窗口增量因子图优化框架:研究人员定义离散时刻k载体导航状态向量Xk=[Rk, pk, vk, bg,k, ba,k]T包含姿态旋转矩阵Rk、位置pk、速度vk及陀螺仪偏置bg,k与加速度计偏置ba,k,在高斯噪声假设下将最大后验估计转化为非线性最小二乘问题。IMU预积分将i至j时刻高率测量压缩为单一约束,构建IMU因子残差rIMU(Xi, Xj, Zij)。传感器因子构建中,LiDAR里程计位置因子提供局部三维位置约束以抑制短期惯性漂移,ODO速度因子依据前向位移与机体坐标系位移构建残差,气压计高度因子约束垂直分量,AHRS姿态因子将欧拉角转为单位四元数并在局部李代数上最小化马氏范数残差。滑动窗口与边缘化机制设定窗口长度W,新增状态Xk+1后移除最早状态Xk-W+1,通过舒尔补将移除状态及其关联因子线性化后消除,得到作用于保留边界状态的先验信息矩阵Λprior与先验信息向量bprior,以平方根信息形式存为附着于保留状态的线性残差,边缘化雅可比固定于边缘化点而窗口内活跃因子重新线性化,使优化状态节点数受限为W并将目标函数加入历史先验因子与当前窗口有效因子集合,避免长期运行因子图持续增长同时通过先验保留窗外历史约束影响以减少精度退化。
- 3.
自适应优化算法:研究人员在滑动窗口增量因子图基础上提出AVFGO,联合运动状态感知自适应融合速率策略与残差驱动多源自适应向量式加权因子。自适应融合速率基于运动状态识别,以短时窗平滑加速度偏差Δa与角速度幅值ωmag、偏航率ψ?计算运动状态指示Mk∈{0,1}区分稳态与转向高动态或振动,依Mk、测量残差rk(s)与质量指示qk(s)调整融合频率f′k(s)=αk(s)fnom(s),其中αk(s)在稳态取较小值、在动态或异常增大以提高关键约束密度,通过自适应插入间隔Δt′ins(s)=Δtnom(s)/αk(s)控制因子插入并限定上下界避免图无界增长与更新间隔振荡。残差驱动自适应加权因子对传感器s在时刻k的名义协方差Σk(s)构造对角调整矩阵Λk(s)=diag(λ1,k(s), …, λns,k(s)),有效协方差Σ′k(s)=Λk(s)Σk(s)Λk(s),依据各维度归一化残差r?j,k(s)处于正常、中等、异常区分别赋予名义、渐增膨胀、有界最大值系数λj,k(s)∈[1, λmax]。LiDAR位置自适应加权区分水平与垂直残差ehoriz,kL、evert,kL,水平调整系数结合Mk;AHRS姿态自适应加权中横滚俯仰系数λr,kA、λp,kA依据Δa调整,偏航系数λy,kA结合偏航残差与偏航率;ODO速度自适应加权设前向调整系数λf,kO、侧向与垂向作弱约束λlat,kO=λvert,kO=λweak;气压计垂直自适应加权依据高度跳变Δhk与垂直一致性误差ev,k计算λh,kB以调整有效方差σ′k2,B=λh,kBσk2,B。
- 4.
实验验证:实验设置在GNSS拒止评估下使用地面轮式无人车平台,比较AVFGO与固定协方差FGO、增量全因子图优化(IFGO)、固定滑动窗口因子图优化(SWIFGO)及标量自适应加权变体的效果,隔离全图累积、固定滑动窗口、标量自适应与向量式自适应加权影响。结果显示IFGO因持续累积历史状态单步优化时间随运行增长至最大8.18 s,SWIFGO以固定窗口将均值降至0.16 s,AVFGO结合受限窗口与自适应因子管理进一步降至0.0389 s,高动态区间均值增至约0.06 s但仍保实时。轨迹对比中3D位置RMSE由IFGO的1.098 m、SWIFGO的0.804 m降至AVFGO的0.380 m,相对降低65.36%与52.71%;水平定位误差在转弯与局部退化区间AVFGO峰值更低且恢复更快,因向量式自适应加权抑制不可靠测量维度而非对整个传感器因子施加单一标量权重;标量自适应变体相对SWIFGO改善姿态精度但未持续改善3D位置RMSE,表明单一传感器级缩放因子不足以应对异质测量维度,AVFGO在该数据集中取得位置、横滚、俯仰、偏航与计算时间的最佳平衡。诊断敏感性测试显示名义参数设置给出最佳平衡,过于激进的AHRS/ODO高频插入或过强LiDAR降采样可能降低位置精度;范围比较表明AVFGO面向受限窗口GNSS拒止异构车载传感器融合,公开SLAM基线侧重视觉惯性或激光惯性建图管线,当前实验显示AVFGO在报告数据集中优于固定协方差与标量自适应变体但未证明优于所有鲁棒FGO方法。
总结讨论部分,研究人员指出AVFGO主要新颖性在于将运动状态感知因子调度与残差驱动向量式自适应加权集成于受限滑动窗口因子图中,而非引入新因子图估计原理。在240 s GNSS拒止实地实验中AVFGO实现3D位置RMSE 0.380 m与平均单步优化时间0.0389 s,在测试平台上优于IFGO与SWIFGO的定位精度与运行时间。局限在于当前验证基于单一室外轨迹与单一轮式平台,结果泛化性受限,未来将开展重复试验、跨平台异构机器人实验与跨场景验证以量化统计变异性、鲁棒性与适应性,并引入丢包、通信延迟、传感器偏置注入、异常测量与非线性扰动的受控退化测试以评估AVFGO在挑战性现实条件下的韧性;方法上学态运动状态识别规则与有界协方差膨胀策略将通过概率或基于学习的运动模式识别及更全面的鲁棒加权模型进一步改进。翻译研究结论部分为:本研究提出AVFGO,一种基于滑动窗口增量因子图的复杂运动环境下自适应多传感器融合定位方法。修订后的文稿阐明AVFGO的主要新颖性在于运动状态感知因子调度与残差驱动向量式自适应加权在受限滑动窗口因子图中的集成,而非引入新的因子图估计原理。在240 s GNSS拒止实地实验中,AVFGO实现3D位置均方根误差(RMSE)0.380 m与平均单步优化时间0.0389 s,在测试平台上于定位精度与运行时间两方面均优于IFGO与SWIFGO。若干局限仍有待未来工作解决。当前验证基于单一室外轨迹与单一轮式平台,限制了结果的普适性。因此将开展重复试验、使用异构机器人系统的跨平台实验以及跨场景验证,以量化统计变异性、鲁棒性与适应性。此外将引入涉及丢包、通信延迟、传感器偏置注入、异常测量与非线性扰动的受控退化测试,以进一步评估AVFGO在挑战性现实条件下的韧性。在方法论上,当前运动状态识别规则与有界协方差膨胀策略将通过概率或基于学习的运动模式识别及更全面的鲁棒加权模型加以改进。