PaperlayerView exact revision on arXiv

Source-linked AI summary

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

Bing Sun, Yue Lin, Yongsheng Yuan, Yang Liu, Dong Wang, Huchuan Lu

arXiv:2609.29419v1cs.RO

TL;DR

Dynamic navigation is challenged by unstable perception and difficulty incorporating motion uncertainty into trajectory optimization. UCON combines historical point-cloud re-association with covariance-aware sector costs; simulations and real-world experiments report stronger perception stability and navigation success in dense dynamic scenarios.

  • Problem

    Dynamic environments challenge navigation through identity switches and unreliable motion estimates, while existing trajectory optimization does not consistently incorporate motion uncertainty.

    Source: §I · p. 1, A. Attempts to Mitigate Perception… · p. 2

  • Method

    UCON re-associates historical point-cloud fragments to restore object identity and embeds anisotropic covariance as uncertainty-sector costs in trajectory optimization.

    Source: §I · pp. 1–2

  • Results

    85.2% accuracy with 17.78 ms average processing latency accompanies fewer identity switches than state-of-the-art methods and higher success rates up to 110 moving obstacles without noticeable computational or energy overhead.

    Source: §I · p. 2

  • Takeaways & Limitations

    UCON's unified perception-to-optimization pipeline improves navigation success in dense dynamic scenarios without significant computational overhead.

    Source: §VI · p. 7

    The reported conclusion scopes its navigation-success finding to dense, dynamic scenarios.

    Source: §VI · p. 7

Abstract

from arXiv · show

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.

I. INTRODUCTION

The introduction frames dynamic navigation as a coupled perception-and-planning problem: unstable object identity and unmodeled motion uncertainty challenge existing systems. UCON links historical point-cloud re-association with covariance-aware trajectory costs, and its reported tests show improved perception and navigation outcomes.

I. INTRODUCTION

  • Dynamic navigation faces perception instability from occlusion, clustering, and partial observation, alongside motion uncertainty that existing trajectory optimization does not consistently incorporate.Existing LiDAR methods rely predominantly on object-level association, while planning based on average states and fixed safety margins neglects uncertainty.Source: §I · p. 1
  • Historical point-level re-association uses past point-cloud fragments to recover lost objects and maintain identity continuity under occlusion and clustering.The introduction describes reconstructing motion states from temporally consistent observations as improving perception stability.Source: §I · pp. 1–2
  • UCON estimates anisotropic motion covariance and converts predicted obstacle states into uncertainty sectors embedded as differentiable trajectory costs.The sectors provide an uncertainty-aware dynamic-obstacle avoidance strategy within a tightly coupled perception-planning pipeline.Source: §I · p. 2
  • 85.2% accuracy at 17.78 ms average processing latency accompanies fewer identity switches than state-of-the-art methods and higher success rates up to 110 moving obstacles without noticeable computational or energy overhead.Real-world dynamic-environment experiments further demonstrate UCON's practicality.Source: §I · p. 2

A. Attempts to Mitigate Perception Instability

The section identifies fragile perception and mismatched uncertainty handling in dynamic planning, then presents two-stage association with historical point-level recovery to maintain identity continuity.

A. Attempts to Mitigate Perception Instability

  • Prior LiDAR methods rely predominantly on object-level association, which can become unstable with partial occlusion or sparse point clouds.The section also notes that learning-based perception can carry substantial computational overhead, while vision-based methods are susceptible to blur and illumination variation.Source: A. Attempts to Mitigate Perception… · p. 2

B. Uncertainty–optimization Mismatch in Planning

  • Planning methods often simplify uncertainty through deterministic motion, implicit uncertainty, or fixed or isotropic safety margins rather than integrating covariance consistently.This leaves a structural gap between probabilistic perception outputs and deterministic trajectory optimization.Source: B. Uncertainty–optimization Mismatch… · p. 2

III. MITIGATING PERCEPTION INSTABILITY

  • UCON combines object-level association for high-confidence state continuity with point-level historical re-association to recover lost targets.The illustration shows three dynamic objects retaining consistent identities despite proximity to each other or a static wall.Source: A. Object-level Association · p. 3, Figure 3 · p. 3, §III · p. 3

A. Object-level Association

  • Object-level association matches current detections to previously tracked objects as a bipartite graph, using projected-box overlap, motion direction, and size.The projections use the x-y plane to filter out sensor-observation noise along the z-axis.Source: A. Object-level Association · p. 3

B. Point-level Historical Re-association

  • Object-level matching can fragment motion histories and cause identity switches when occlusion or temporary detection loss disrupts aggregated, frame-by-frame matching.The described failure arises from relying on aggregated geometry without fine-grained temporal memory.Source: B. Point-level Historical Re-association · p. 3
  • Historical point-cloud fragments recover unmatched objects by matching predicted points to nearby current points, updating an object when enough points match.A point counts as matched when its predicted distance is below d0; the update requires more than γNc matched points.Source: B. Point-level Historical Re-association · p. 3

IV. UNCERTAINTY–AWARE TRAJECTORY OPTIMIZATION

This section uses a Kalman filter for recursive motion-state estimation and uncertainty-aware trajectory-planning inputs. It propagates error covariance and extracts position uncertainty for dynamic-obstacle avoidance.

A. Uncertainty Estimation

  • A Kalman filter recursively estimates object motion states to provide uncertainty-aware inputs to trajectory optimization.The state comprises position and velocity at each time step.Source: A. Uncertainty Estimation · p. 3
  • The model includes normally distributed process noise with covariance Q to characterize incremental prediction risk from deviations in motion patterns.The measurement matrix is set to the identity matrix, simplifying the Kalman gain during prediction.Source: A. Uncertainty Estimation · p. 3
  • Σt is defined as the 3 × 3 upper-left submatrix of dynamic-obstacle error covariance Pt, which describes position uncertainty and enables uncertainty-aware dynamic-obstacle avoidance.Source: A. Uncertainty Estimation · p. 4

B. Uncertainty-aware Sector Geometry Modeling

UCON models dynamic obstacles with sectors whose geometry reflects motion state and uncertainty, then uses differentiable radial and lateral costs to incorporate collision risk into trajectory optimization.

B. Uncertainty-aware Sector Geometry Modeling

  • The proposed sector releases lateral feasible space while preserving the high-confidence main motion direction, unlike the overly conservative ellipsoidal model in.Its geometry is determined adaptively from obstacle motion state and uncertainty.Source: B. Uncertainty-aware Sector Geometry… · p. 4
  • Sector radius depends on obstacle velocity magnitude and position-error covariance; the model also uses the projected bounding-box diagonal, velocity direction, and safe distance.The passage identifies h and k as balancing velocity estimation and position uncertainty.Source: B. Uncertainty-aware Sector Geometry… · p. 4

C. Sector-based Dynamic Obstacle Avoidance

  • The dynamic-obstacle cost is differentiable with respect to robot position and time and combines radial-distance and lateral-risk terms to quantify collision probability.Each summed term represents the probability of collision with a dynamic obstacle; minimizing the cost enables uncertainty-aware avoidance.Source: C. Sector-based Dynamic Obstacle… · p. 4

D. Uncertainty-aware Trajectory Optimization

UCON initializes and parameterizes a segmented trajectory, then optimizes it for smoothness, feasibility, and obstacle avoidance using gradient-based methods.

D. Uncertainty-aware Trajectory Optimization

  • The initial path is a straight line between endpoints, sampled at N + 1 points and parameterized as a third-order MINCO trajectory.Each segment is represented using coefficient vector C_n and the natural basis β(t) = (1, t, t^2, t^3, t^4, t^5)^⊤.Source: D. Uncertainty-aware Trajectory… · p. 4
  • The optimization balances uncertainty-aware dynamic-obstacle avoidance, time regularization, and smoothness while enforcing static-obstacle clearance, kinematic limits, and segment continuity.Smoothness is obtained by minimizing third-order trajectory derivatives; the constraints enforce physical segment durations, static-obstacle clearance, velocity and acceleration limits, and high-order continuity.Source: D. Uncertainty-aware Trajectory… · pp. 4–5
  • A differential homeomorphism removes the time-manifold constraint, while a time-integral penalty handles static-obstacle and kinematic constraints; MINCO inherently satisfies continuity.Higher-order trajectory integrals and derivatives are analytic, while the uncertainty-aware obstacle cost uses numerical integration.Source: D. Uncertainty-aware Trajectory… · p. 5
  • The obstacle-cost gradients are derived through the chain rule, with difference approximations for time derivatives, enabling efficient gradient-based optimization.The derivatives include radial-distance and lateral-risk costs; the complex partial derivatives of the directional term are given in Appendix B.Source: D. Uncertainty-aware Trajectory… · p. 5

A. Implementation Details

The implementation uses an omnidirectional LiDAR-equipped robot and onboard Jetson AGX Orin computing; the supplied visuals include a qualitative perception comparison and a perception-stability table.

A. Implementation Details

  • The real-world robot uses an upside-down top-mounted LiDAR for surrounding perception, with computations on an onboard Jetson AGX Orin.The experiments use a Livox Mid-360 LiDAR with a 360°×59° field of view and 50 Hz point-cloud updates; simulations and benchmarks also run on the Orin platform.Source: Figure 5 · p. 5, A. Implementation Details · p. 5
  • The perception-stability keyframes contrast UCON, which maintains target identity, with FAPP, which loses targets and switches identities.The caption attributes UCON's outcome to point-level historical re-association and FAPP's instability to single-metric object-level association.Source: Figure 6 · p. 6
  • Table I is scoped as a comparison of perception stability; the described perception-accuracy metric is MOTA.The table caption does not provide its values.Source: Table I · p. 6

B. Evaluation of Perception Stability

The real-world perception evaluation reports stable tracking with reduced identity switching, while trajectory-optimization tests favor complete UCON on success rate and show safe, smooth paths in the 80-obstacle visualization.

B. Evaluation of Perception Stability

  • In the real-world test with three pedestrians, UCON reports 85.2% MOTA, 17.78 ms average perception time, and fewer identity switches than the compared methods.The 243-second experiments used an average of 4,922 LiDAR points per scan.Source: B. Evaluation of Perception Stability · p. 6

C. Evaluation of Uncertainty-aware Trajectory Optimization

  • With 80 dynamic obstacles, the visualization shows safe, smooth UCON trajectories, while FAPP collides and the ablation is unsafe without motion-uncertainty modeling.The Intent-MPC trajectory is described as not ideal due to low generalization ability.Source: Figure 7 · p. 6, C. Evaluation of Uncertainty-aware… · p. 6
  • UCON achieves the highest planning success rate with less degradation as obstacle counts increase, while planning efficiency and energy cost remain comparable to the best method.The table summarizes average trajectory jerk, planning time, and the proportion of trajectories without collisions.Source: C. Evaluation of Uncertainty-aware… · pp. 6–7, Table II · p. 7

D. Navigation in Real-world Dynamic Environments

Real-world tests show UCON navigating varied static and moving obstacles with safe trajectories, including pedestrians intentionally blocking its path. The paper concludes that it improves navigation success in dense dynamic scenarios, while noting a limitation from omitted semantic information.

D. Navigation in Real-world Dynamic Environments

  • In two real-world experiments, UCON generates safe, smooth trajectories among varied obstacles and avoids pedestrians who intentionally block its path.The robot uses Faster-LIO for localization without prior maps; the pedestrian experiment reports a collision-free trajectory.Source: D. Navigation in Real-world Dynamic… · p. 7, Figure 8 · p. 7

VI. CONCLUSIONS AND LIMITATIONS

  • The paper concludes that UCON improves navigation success rates in dense, dynamic scenarios without significant computational overhead.The conclusion presents UCON as a unified framework addressing perception instability and the uncertainty-optimization mismatch.Source: §VI · p. 7
  • UCON’s limitation is its omission of semantic-level information, which occasionally causes it to underperform learning-based methods.The authors identify incorporating dynamic objects’ motion intent as future work.Source: §VI · p. 7

APPENDIX

The appendix outlines the point-level historical re-association procedure, including its inputs, output, and initialization.

APPENDIX

B. Detailed Expression of Partial Derivative

This subsection derives partial derivatives used to form the lateral risk cost, beginning with a cosine expression and applying the chain rule.

B. Detailed Expression of Partial Derivative

  • The subsection introduces a cosine expression for the angle between δ_i(t) and v_i(t), with u_i(t) defined as the norm of v_i(t).The displayed expression is identified as equation (23).Source: B. Detailed Expression of Partial… · p. 8
  • The partial derivative of c_i(p, t) with respect to position p is obtained using the chain rule for fractional differentiation.Source: B. Detailed Expression of Partial… · p. 8
  • The subsection similarly derives the derivative with respect to t_n and states that the results yield derivatives of the lateral risk cost l_i.It also introduces components p_i(t) and q_i(t).Source: B. Detailed Expression of Partial… · p. 8
Loading 2609.29419v1…