UCON: Uncertainty-aware Navigation with Historical Re-association in Dynamic Environments

2026-09-24

AI Summary

This study addresses navigation challenges in dynamic environments arising from perceptual instability and the decoupling of uncertainty from trajectory optimization. To this end, we propose the UCON algorithm. This method introduces a novel point-level historical re-association mechanism to restore target identity consistency and employs anisotropic Kalman filtering for motion state estimation. Furthermore, it constructs an uncertainty sector model that translates uncertainty into differentiable cost terms embedded within the trajectory optimization framework, thereby achieving deep coupling between perception and planning. Both simulation and real-world vehicle experiments demonstrate that, while maintaining computational efficiency, the proposed algorithm significantly outperforms existing state-of-the-art methods in perceptual stability and overall navigation performance.

Abstract

Autonomous navigation in dynamic environments is hindered by two fundamental challenges: perception instability and uncertainty-optimization mismatch. The former leads to identity switches and unreliable motion estimation, while the latter prevents principled incorporation of motion uncertainty into trajectory optimization. To address these challenges, we propose UCON, an uncertainty-aware navigation algorithm in dynamic environments. For perception instability, we present a point-level historical re-association mechanism that leverages historical point cloud fragments to recover lost targets while maintaining identity continuity. Subsequently, a Kalman filter is employed to provide anisotropic motion state estimation and covariance propagation. To resolve the uncertainty-optimization mismatch, we transform predicted states and their covariances into uncertainty sectors, which are embedded as differentiable cost terms within a trajectory optimization framework. This achieves consistent uncertainty-aware dynamic obstacle avoidance while maintaining smoothness and feasibility. Extensive simulations and real-world experiments demonstrate that, while maintaining high computational efficiency, UCON achieves superior perception stability and robust navigation performance in dynamic environments compared to state-of-the-art methods. The code will be open-sourced to facilitate further research.

Problem

• autonomous navigation
• dynamic environments
• perception instability
• uncertainty-optimization mismatch
• trajectory optimization

Innovation

• Uncertainty-aware Navigation
• Historical Re-association
• Trajectory Optimization
• Dynamic Environments
• Kalman Filter