Improving Ranging-Based Location Estimation with Rigidity-Constrained CRLB-Based Motion Planning

Ranging systems can provide inexpensive, accurate, energy- and computationally-efficient navigation solutions for mobile robots. This work focuses on location and pose estimation in ranging networks composed of anchors with known positions as well as mobile robots modeled as rigid bodies, each carrying multiple tags to localize. Noisy distance measurements can be obtained between a subset of the nodes (anchors and tags), and the robots can move in order to improve the accuracy of the localization process, which depends on the geometry of the network. We propose a method to find trajectories for the robots leading to configurations that locally optimize this localization accuracy. These trajectories minimize a cost function based on the constrained Cramér-Rao Lower Bound (CRLB), where the constraints capture the information about the known distances between tags carried by the same robot. A primal-dual optimization scheme aims to enforce these distance constraints between tags in the motion planner as well. An important feature of the approach is that the gradient terms necessary to plan the motion can be computed essentially in closed form, thereby simplifying the implementation. We compare the proposed method to a naive two-stage algorithm that optimizes the positions and orientations of the robots independently. Simulation results illustrate the benefits of using the constrained optimization approach.