We present a waypoint planning algorithm for an unmanned aerial vehicle (UAV) that is teamed with an unmanned ground vehicle (UGV) for the task of search and rescue in a subterranean environment. The UAV and UGV are teamed such that the localization of the UAV is conducted on the UGV via the multi-sensor fusion of a fish-eye camera, 3D LIDAR, ranging radio, and a laser altimeter. Likewise, the trajectory planning of the UAV is conducted on the UGV, which is assumed to have a 3D map of the environment (e.g., from Simultaneous Localization and Mapping). The goal of the planning algorithm is to satisfy the mission's exploration criteria while reducing the localization error of the UAV by evaluating the belief space for potential exploration routes. The presented algorithm is evaluated in a relevant simulation environment where the planning algorithm is shown to be effective at reducing the localization errors of the UAV.
View on arXiv