Path planning for optimal cooperative navigation

For many navigation scenarios, it is known that the accuracy of navigation state estimates depends on the path traveled, particularly when access to external navigation aids such as the Global Positioning System (GPS) is not available. In this work, we present a path planning method that attempts to minimize the navigation uncertainty of a pair of autonomous vehicles traveling from known initial locations to desired goal locations. For the scenario considered in this study, each vehicle has an onboard odometer for measuring relative changes in position and heading. The vehicles also have sensors for measuring the range between the vehicles. Navigation state estimates are obtained using a centralized batch factor graph approach implemented with the Georgia Tech Smoothing and Mapping (GTSAM) library. In previous work on this problem, candidate vehicle trajectories were chosen from a restricted class of trajectories referred to as pseudo-zigzagging. An exhaustive search of all possible trajectories in this class revealed that the final position uncertainty could be reduced by a factor of 5 when compared with the case of each vehicle traveling straight to its goal location. In the work we present here, the trajectories are no longer restricted to such a limited class. Instead, each path is constructed from a small set of waypoints that may be placed anywhere. We have devised a method that takes an arbitrary set of waypoint locations and adjusts their locations so that the resulting path meets travel time and maneuverability constraints. Using just 3 waypoints for each vehicle path, and applying a simple random search optimization algorithm to the waypoint locations, the final position uncertainty can be reduced by another factor of 3. Thus, we achieve a total factor of 15 reduction in position uncertainty when compared to the straight path (i.e. worst) case. Also, the results indicate that the final position uncertainty can be different for each vehicle. Furthermore, the navigation certainty does not necessarily improve with increased travel time. Thus, travel time should be considered a free parameter (i.e. the time constraint is not necessarily an active constraint). As is the case for most practical nonlinear optimization problems, there is no guarantee that the results obtained in this work are globally optimal. Nevertheless, it is quite clear that path planning can be used to significantly reduce navigation uncertainty for the scenario under consideration.