Enhancing Feature Tracking Reliability for Visual Navigation using Real-Time Safety Filter

TL;DR

The paper addresses the challenge of sustaining reliable feature tracking for visual navigation when the robot’s task might conflict with perception needs. It introduces a real-time QP-based safety filter derived from a Nagumo-style invariance condition to constrain velocity while ensuring a minimum image-feature information score. Key contributions include an invariance-based visibility constraint, a feasible safety filter that accommodates dynamic feature observation, and successful integration with ORB-SLAM2 in texture-poor environments. The approach enhances robustness of visual localization without relying on heavy belief-space planning, enabling safer and more reliable SLAM-based navigation in challenging scenes.

Abstract

Vision sensors are extensively used for localizing a robot's pose, particularly in environments where global localization tools such as GPS or motion capture systems are unavailable. In many visual navigation systems, localization is achieved by detecting and tracking visual features or landmarks, which provide information about the sensor's relative pose. For reliable feature tracking and accurate pose estimation, it is crucial to maintain visibility of a sufficient number of features. This requirement can sometimes conflict with the robot's overall task objective. In this paper, we approach it as a constrained control problem. By leveraging the invariance properties of visibility constraints within the robot's kinematic model, we propose a real-time safety filter based on quadratic programming. This filter takes a reference velocity command as input and produces a modified velocity that minimally deviates from the reference while ensuring the information score from the currently visible features remains above a user-specified threshold. Numerical simulations demonstrate that the proposed safety filter preserves the invariance condition and ensures the visibility of more features than the required minimum. We also validated its real-world performance by integrating it into a visual simultaneous localization and mapping (SLAM) algorithm, where it maintained high estimation quality in challenging environments, outperforming a simple tracking controller.

Figures (6)

• Figure 1: The result of experiments with (a) the proposed safety filter and (b) the baseline controller. For each experiment, the robots at three timestamps (A,B,C) are visualized along with their corresponding on-board images, with features visualized in ORB-SLAM2. The detected features are represented with green dots. The proposed safety filter adaptively adjusts the control input to maintain sufficient tracking features, as demonstrated by the camera heading (arrow) and onboard image at timestamp B. In contrast, the baseline controller struggles with texture-poor surfaces, where fewer features are trackable. A detailed explanation and analysis are provided in \ref{['sec:eval']}.
• Figure 2: (a) The robot configuration and the onboard camera's field of view for the running example. The camera is mounted on the two dimensional ground robot, captures any landmark within its field of view, represented by the light green region. The field of view is defined by the angle of view, $\psi$, and the sensing range, $R$. (b) A graphical illustration of equivalence between \ref{['eq: lambda nonnegativity']} and \ref{['eq: lambda-rho']}. The gray region shows where \ref{['eq: lambda nonnegativity']} is violated. For a fixed $\mu \in [0,1]$, the feasible region of \ref{['eq: lambda-rho']} forms a half-space with the origin on its boundary. The union of all possible half-spaces aligns with the feasible set of \ref{['eq: lambda nonnegativity']}, represented by the white region.
• Figure 3: Simulation result for the running example. (left) The resulting trajectory of the robot. The position trajectory ($q_x, q_y$) is depicted as red curve, the camera poses are drawn using blue triangles. It can be seen that the robot takes a path that differs from the reference (shown in the left top corner) to maintain visibility to at least 5 landmarks. (right) The time history of $w(q)$, $\hat{w}(q,\lambda)$ values. The relation $W \leq \hat{w}(q,\lambda) \leq w(q)$ holds throughout the simulation.
• Figure 4: The diagram illustrated for the control structure for vision-based robot with the proposed safety filter.
• Figure 5: (a) The experiment scenario where the robot inspects the wall moving through the texture-poor region and (b) the hardware configuration used for the hardware experiment.