Abstract
This paper introduces a real-time safety filter designed to prevent localization failure in visual navigation systems. By framing the maintenance of visual features as a constrained control problem rather than a multi-objective optimization task, the authors develop a Quadratic Programming (QP) based filter. This filter modifies robot movements just enough to keep landmarks in sight without abandoning the primary mission. Experimental results on a differential wheeled robot demonstrate that the filter successfully maintains feature tracking in texture-poor environments where standard SLAM systems typically fail.
Introduction
Visual navigation systems, such as Simultaneous Localization and Mapping (SLAM), are fundamental to autonomous robotics. These systems rely on tracking "features"—distinct visual landmarks like corners or textures—to estimate a robot's pose. However, these systems are vulnerable to "feature-poor" environments, such as blank walls or poorly lit corridors, where a lack of visual information can lead to localization failure and mission termination.
Traditional approaches often frame this as "perception-aware planning," treating visibility as an objective to be maximized. This frequently results in a computational "tug-of-war" between the robot's primary task and its need to maintain visibility, leading to complex, hard-to-tune multi-objective optimizations. This paper proposes a shift in perspective: visibility should be treated as a hard safety constraint rather than an optimization goal.
Methodology: The QP-based Safety Filter
The authors introduce a real-time safety filter based on Quadratic Programming (QP). This filter acts as an intermediary between a high-level mission controller and the robot's actuators. It modifies the reference velocity commands only when necessary to satisfy visibility constraints, ensuring the robot maintains a minimum threshold of features required for localization.
Technical Innovations
- Differentiable Visibility Score: To overcome the discrete nature of feature counts, the researchers introduced auxiliary state variables ($\lambda$ and $\mu$) to create a continuous and differentiable "smoothened" version of the feature count.
- Control Barrier Functions (CBFs): The system adapts CBF theory—typically used for obstacle avoidance—to define a "safe" region in the state space where feature counts remain sufficient.
- Real-Time Execution: By formulating the problem as a QP with a kinematic model, the filter can be solved in milliseconds, allowing for high-frequency control loops.
Experimental Validation
The approach was validated using a differential wheeled robot equipped with a ZED2i stereo camera and a SICK 2D LiDAR. The test scenario involved a wall inspection task where the robot had to traverse a region with a large, texture-less white curtain.
Key Results
The experiments demonstrated that the safety filter significantly enhances SLAM reliability. While the baseline controller (ignoring visibility) experienced a total loss of features and subsequent localization failure when facing the blank curtain, the proposed filter automatically adjusted the robot's heading to keep textured areas in view.
- Adaptive Heading: As shown in the experimental results, the filter proactively rotated the camera toward feature-rich zones as the robot approached the "dead zone."
- Minimal Mission Impact: The robot deviated from its reference path only as much as required to maintain safety, returning to its original mission parameters once sufficient features were again available.
- Multi-Constraint Handling: The system successfully integrated visual safety constraints with LiDAR-based obstacle avoidance, proving its robustness in complex environments.
Conclusion and Future Work
By treating perception as a safety constraint, this research provides a computationally efficient method to prevent "visual blindness" in autonomous robots. Future work will focus on extending this framework to higher-order systems like aerial vehicles and incorporating more sophisticated information metrics to ensure that tracked features are not just numerous, but also high-quality for localization.
Summary updated