The Most Accurate Path from Point A to Point B is Not Necessarily a Straight Line

摘要

This work studies the problem of guiding a vehicle from a known initial location to a known goal location as accurately as possible, without direct observation of the goal location (such as a bearing measurement, or line-of-sight to the goal), and without direct position measurements, such as those provided by GPS. The vehicle travels in a planar environment and has an onboard inertial measurement unit and an onboard visual system to measure bearing angles to features in the environment. Taking a zigzagging path toward the goal provides better position estimation than a straight path. For a given energy budget, there is a certain path width, or amplitude, that results in the best estimation performance, and this optimal path width depends on the sensor noise parameters. A batch estimator is derived to analyze the effect of the entire time history of the vehicle trajectory on final position estimation performance. The formulation results in a linear system of equations. The path width that minimizes the condition number of the system matrix also minimizes the final position estimation error when the feature bearing measurement noise is relatively large compared to the inertial measurement noise.