Waypoint generation and route planning for terrain relative navigation
Keywords: Aeronautics and Astronautics.

Navigation is a key capability for autonomous systems, requiring both determination of the system's position relative to the target and development of a plan to get there. In some applications, the amount of information incorporated into the position estimate depends on the location of the autonomous system. When the information is sparse, the route to the target site can impact the system's ability to localize. Planning a route to increase localization accuracy can greatly increase the likelihood of successfully navigating to the site. Many robotic systems rely on GPS for localization; however, this option is unavailable underwater. Terrain Relative Navigation (TRN) is a localization technique that was previously developed to enable Autonomous Underwater Vehicles (AUVs) to perform missions to sites on the seafloor. Accurate localization requires observation of varying, informative terrain. Many underwater missions are over flat terrain with only sparse areas of high information. A well planned route that guides the AUV over the informative terrain can increase localization accuracy when arriving at the target. Route planning for TRN presents several challenges that are not addressed by current planning techniques. Belief space motion planning techniques have been applied to similar problems; however, these techniques rely on the use of a linearized Gaussian position estimate. This assumption is not valid for TRN. TRN uses a particle filter, a Monte Carlo localization technique, to handle the multi-modal distribution that may arise from the information sparsity. The planning techniques for linearized Gaussian estimates extend the state space with a compact representation of the positional uncertainty and solve for a path using traditional search methods. These methods are not easily adaptable to use with a particle filter. Several planning techniques for partially observable Markov decision processes (POMDPs) are adapted for particle filter estimation; however, POMDP solution techniques solve for an approximate optimal closed-loop policy. Since the AUV routes are determined prior to starting the mission, the policy search techniques for particle filters do not apply for this application. This thesis presents a planning technique called the TRN Route Planning Method (TRN-RPM), which generates the intermediate waypoints of the route. TRN-RPM formulates route planning as an optimization to maximize the probability of arriving within a given threshold from a target site. A simulation of the AUV was developed to generate Monte Carlo samples of the target error distribution from different vehicle trajectories along a given route. These samples are used to approximate a route's success probability. Monte Carlo estimation greatly increases the computation required. Therefore a two-step method was developed to limit the time needed for route selection. First, the number of potential waypoint locations is limited by using a heuristic based on the posterior Cramer--Rao lower bound to place waypoints in informative areas. The waypoints are used to form a finite number of high value candidate routes for the optimization. Second, the optimization speed is increased by using a specialized version of Thompson sampling. Thompson sampling is a search strategy that balances exploration and exploitation. Traditional Thompson sampling is modified to incorporate an upper bound on the success probability, reducing the time until convergence. Additionally, the simulation is structured to reuse data from previously-computed routes. TRN-RPM accurately estimates the success probability and greatly reduces the time required to plan routes for TRN missions. TRN-RPM was demonstrated using simulation and experimental dive results. The method was used to plan routes for two missions in Monterey Bay, California. The simulation results demonstrate convergence to routes with a high success probability in 1/100th of the time required for an exhaustive search of all candidate routes. One of these missions was experimentally demonstrated on an AUV operated by the Monterey Bay Aquarium Research Institute (MBARI). These demonstrations included several runs in which the AUV followed the optimized route as well as runs using several other non-optimized routes for comparison. They demonstrated that using TRN-RPM can effectively increase mission success probability in real-world TRN missions.