Probabilistic Visibility-Aware Trajectory Planning for Target Tracking in Cluttered Environments

TL;DR

This paper introduces a real-time, probabilistic trajectory planning method that accounts for system uncertainty to improve target visibility and safety in cluttered environments, validated through simulations and real-world tests.

Contribution

It presents a novel belief-space probability of detection (BPOD) concept and an efficient algorithm for real-time, uncertainty-aware visibility trajectory planning.

Findings

Robustly maintains target visibility under high uncertainty.

Outperforms existing methods in simulation benchmarks.

Validated effectiveness through real-world experiments.

Abstract

Target tracking has numerous significant civilian and military applications, and maintaining the visibility of the target plays a vital role in ensuring the success of the tracking task. Existing visibility-aware planners primarily focus on keeping the target within the limited field of view of an onboard sensor and avoiding obstacle occlusion. However, the negative impact of system uncertainty is often neglected, rendering the planners delicate to uncertainties in practice. To bridge the gap, this work proposes a real-time, non-myopic trajectory planner for visibility-aware and safe target tracking in the presence of system uncertainty. For more accurate target motion prediction, we introduce the concept of belief-space probability of detection (BPOD) to measure the predictive visibility of the target under stochastic robot and target states. An Extended Kalman Filter variant incorporating BPOD is developed to predict target belief state under uncertain visibility within the planning horizon. To reach real-time trajectory planning, we propose a computationally efficient algorithm to uniformly calculate both BPOD and the chance-constrained collision risk by utilizing linearized signed distance function (SDF), and then design a two-stage strategy for lightweight calculation of SDF in sequential convex programming. Extensive simulation results with benchmark comparisons show the capacity of the proposed approach to robustly maintain the visibility of the target under high system uncertainty. The practicality of the proposed trajectory planner is validated by real-world experiments.