Optimal Maneuvering for Autonomous Vehicle Self-Localization

Abstract

We consider the problem of optimal maneuvering, where an autonomous vehicle, an unmanned aerial vehicle (UAV) for example, must maneuver to maximize or minimize an objective function. We consider a vehicle navigating in a Global Navigation Satellite System (GNSS)-denied environment that self-localizes in two dimensions using angle-of-arrival (AOA) measurements from stationary beacons at known locations. The objective of the vehicle is to travel along the path that minimizes its position and heading estimation error. This article presents an informative path planning (IPP) algorithm that (i) uses the determinant of the self-localization estimation error covariance matrix of an unscented Kalman filter as the objective function; (ii) applies an l-step look-ahead (LSLA) algorithm to determine the optimal heading for a constant-speed vehicle. The novel algorithm takes into account the kinematic constraints of the vehicle and the AOA means of measurement. We evaluate the performance of the algorithm in five scenarios involving stationary and mobile beacons and we find the estimation error approaches the lower bound for the estimator. The simulations show the vehicle maneuvers to locations that allow for minimum estimation uncertainty, even when beacon placement is not conducive to accurate estimation.

UAV Path Optimization for Angle-Only Self-Localization and Target Tracking Based on the Bayesian Fisher Information Matrix

“…As our ultimate objective is to optimize the target tracking performance by means of appropriate UAV manoeuvres, the 4 × 4 submatrix of the BCRLB, Σ 11,k , will be the focus of attention. This is different from previous work, where the entire Σ k was used [16,21]. In (33), the contributions of the target AOA and beacon bearing measurements (I k and J k , respectively) to the recursive BFIM are influenced by several factors, the most obvious of which are the angle noise variances σ 2 θ and σ 2 θ i…”

“…In addition to the algorithms developed in this paper, two modifications of the A-and D-optimality-based algorithms are also simulated to illustrate the effects of using the full Kalman filter covariance matrix rather than restricting the optimization problem to the target location covariance. Full Kalman filter covariances were used in [16,21]. In Figures 11 and 12, the modified A-optimality algorithm that minimizes the trace of the filtered state estimate covariance for all state variables (UAV and target kinematic parameters, as well as UAV orientation) is labelled "min tr P k|k ", and the modified D-optimality algorithm that maximizes the determinant of the inverse covariance matrix for all state variables is labelled "max |P −1 k|k |".…”

“…In [15], an indoor self-localization algorithm for a swarm of robots was proposed using active optical beacons. Optimal UAV trajectories for stationary and mobile beacons were investigated in [16] using the covariance of predicted Kalman filter state estimates [17] and building on the D-optimality criterion [18]. Selflocalization methods for UAVs commonly use optical sensors for landmark or beacon angle measurements with respect to the UAV location and orientation.…”