A Wireless Local Positioning System Concept and 6D Localization Approach for Cooperative Robot Swarms Based on Distance and Angle Measurements

In wireless sensor networks, spatially distributed nodes provide location-dependent sensor information. Therefore, knowledge about the 3D position of all nodes is crucial for the numerous applications that require autonomous mobility. Furthermore, to acquire the nodes' poses and the complete 6D network constellation, the 3D orientation of each node is also required. While many theoretical localization concepts exist for wireless sensor networks, there is still a lack of reliable system and localization concepts which enable robust real-time tracking in real-world scenarios. Therefore, we present a system approach based on an advanced 24 GHz wireless local positioning system, providing distance and angle measurements between pairs of nodes. Furthermore, an extended Kalman filter based localization algorithm is proposed, which evaluates these measurements to track the time varying 6D poses of all nodes in the network. Because only relative measurements are available, one node is chosen to define a joint navigation system. Hence, the proposed system works without any previously installed infrastructure or prior information of the network. The system and localization algorithm are validated by measurements performed in a mobile wireless sensor network comprising six nodes in an indoor scenario with strong multipath propagation. However, despite the challenging environment, the system allows for a stable and accurate 6D pose estimation of all robots in the network with 3D positioning root mean square errors of 6 to 15cm.