COOPERATIVE LOCALIZATION TO IMPROVE CONVOY STABILITY (CLICS)

In this paper, we present CLICS, a program that optimizes convoy vehicle tracks by intelligently combining sensor updates of all vehicles in the convoy in a distributed, cooperative localization system. Currently, follower vehicles in the convoy rely either on GPS breadcrumbs from the lead vehicle, or rely on sensing the location of its predecessor and following its path. However, GPS availability and accuracy oftentimes cause the former solution to fail, and accumulated errors in tracking and control in long convoys can cause the latter solution to fail. Robotic Research’s CLICS system attempts to overcome these problems by (1) integrating multiple heterogeneous sensor outputs from multiple vehicles (2) developing a distributed, realtime non-linear estimation of inter-vehicle pose using spring network providing coordinated localization for members of a vehicle convoy, and (3) real-time robust synchronization of information amongst the convoy, and local convoy and mission level planning. Using the smooth relative navigation solution from the inertial measurement unit (IMU), CLICS inserts virtual “springs” between a vehicle’s navigation solution and other vehicles’ navigation solutions by using inter-vehicle sensing updates. An example of a common spring is a range and angle from one vehicle to another. These “springs” pull the position history of the vehicle with a strength based upon the reliability of the input data. This is done through a nonlinear optimization algorithm that computes an optimal track for each vehicle in the convoy. INTRODUCTION Robotic Research, LLC developed and implemented several technologies for Cooperative Localization to Improve Convoy Stability (CLICS) that optimizes convoy vehicle tracks by intelligently combining sensor updates of all vehicles in the convoy in a distributed, cooperative localization system. Currently, follower vehicles Proceedings of the 2017 Ground Vehicle Systems Engineering and Technology Symposium (GVSETS) Cooperative Localization to Improve Convoy Stability (CLICS), Wilhelm et al. Page 2 of 9 in the convoy rely either on GPS breadcrumbs from the lead vehicle, or rely on sensing the location of its predecessor and following its path. However, GPS availability and accuracy oftentimes cause the former solution to fail, and accumulated errors in tracking and control in long convoys can cause the latter solution to fail. The CLICS system overcomes these problems by (1) integrating multiple heterogeneous sensor outputs from multiple vehicles (2) developing a distributed, real-time non-linear estimation of inter-vehicle pose using spring network providing coordinated localization for members of a vehicle convoy, and (3) real-time robust synchronization of information amongst the convoy, and local convoy and mission level planning. CLICS has demonstrated improvement upon the state-of-the-art in autonomous vehicle convoy localization and control by leveraging its Spring Network framework. The CLICS system uses the smooth relative navigation solution from the inertial measurement unit (IMU), inserting virtual “springs” between a vehicle’s navigation solution and other vehicles’ navigation solutions by using inter-vehicle sensing updates. An example of a common spring is a range and angle from one vehicle to another. These “springs” pull the position history of the vehicle with a strength based upon the reliability of the input data. This is done through a nonlinear optimization algorithm that computes an optimal track for each vehicle in the convoy. Springs can be shared among all vehicles using vehicle-to-vehicle (V2) radios, so that the optimization algorithm runs on all sensor information in the convoy and not just local sensor data. Each vehicle can run its own instantiation of the algorithm and sensor updates can be asynchronous, allowing the CLICS system to be robust to communication losses. Open architecture principles are used to facilitate flexibility in adding new springs when a new sensor or algorithm is added. The CLICS system inserts springs using GPS, LIDAR, radar, ranging radio, and camera information. Map registration from LIDAR data also plays a large role in the CLICS system since it is a good complement to GPS. When GPS is not available, there are typically many features available for map registration. SPRING NETWORKS The Spring Network combines various position measurements to estimate the positions and tracks of all the vehicles in the convoy. The position measurements can be many different types of fundamental measurements. This can include distance from the right rear corner of a vehicle to the front left corner of a following vehicle as measured by two ranging radios at a particular time. It can include the perpendicular distance from a fence line to a vehicle as measured by LIDAR. It can include the heading of a telephone pole relative to a vehicle as measured by a camera. It also includes the change in X, Y and Heading of a vehicle from one time to another as measured by the onboard accelerometers, gyros, and wheel odometers. Each of these measurements forms a “spring” in the Spring Network. The stiffness of each spring is determined by the confidence in the corresponding measurement. A more confident measurement has a stiffer spring. Each spring can “pull” and “push” on the location estimate of a vehicle at the time the measurement was made. Depending on the type of measurement, and sometimes on the position estimate of a vehicle, a spring might only push in the X direction, only in the Y direction, or only in the Heading direction. For example, if the measurement was the range to a point 100 m away in just the X direction, then moving the position estimate in the X direction will change the force on that spring. Changing the position estimate in Y or in Heading will not change the distance and therefore will not change the force of that spring, assuming small changes in Y and Heading. Proceedings of the 2017 Ground Vehicle Systems Engineering and Technology Symposium (GVSETS) Cooperative Localization to Improve Convoy Stability (CLICS), Wilhelm et al. Page 3 of 9 Some springs can push as a combination of directions. For example, if the measurement was the range to a point both 100 m away in the X direction and 100 m away in the Y direction, then moving the position estimate in the X = Y direction will change the force in the spring, while moving the position estimate in the X = neg Y direction will not. For another example, if the measurement was that a pole was straight ahead of the vehicle, then rotating the position estimate will add force or torque to the spring. However, if the vehicle position estimate translated to the side as it rotated in such a way that the pole was always straight ahead of the rotated vehicle, then no force would be added to the spring. COOPERATIVE LOCALIZATION WITH SPRING NETWORKS The CLICS cooperative localization algorithms are based on Robotic Research’s current Spring Network framework described above. The Spring Network was initially developed on the UMAPS SBIR program for the Army. Under UMAPS, this integrated architecture was used to fuse the relative localization solutions of multiple INS systems (WarLocTM tracking devices) strapped to a set of dismounted soldiers. The information from multiple units are fused by creating a “spring” network by sharing positioning updates between units and using the springs to “pull” the relative solutions to a more accurate location. The strength of each spring is determined by the confidence of the update. The underlying mathematical model used in the nonlinear solver does not actually implement equations of springs, but the performance is analogous to a set of springs. The framework has a generic design that does not limit it to dismounted soldiers. During initial simulations, the cooperative localization algorithm that is implemented in the architecture has performed well for vehicles that are equipped with an inertial sensor and can measure range, angle, or relative position to any other vehicle or unit. In fact, Robotic Research has demonstrated this system with an RR-N-120-series navigation unit on an ATV, and a Robotic Research WarLocTM unit on several dismounts. The dismounts and ATV were also equipped with UWB ranging radios. When the dismounts came near the ATV, ranging springs were automatically added into the Spring Network. Since the ATV had a higher confidence in its localization solution due to GPS availability and a better IMU, the dismounts paths were “pulled” to an improved location. The Spring Network framework also contains the messaging system between the nodes and the database at each node to store information received from the other nodes. The current messaging system using meshing Ethernet radios similar. The Spring Network messaging system is robust regarding loss of communications. Data is transferred between nodes when both nodes are available on the network. Care is taken so that no node is overloading the network with needless messages. The Spring Network combines relative localization solutions of varying certainties with other relative localization solutions or absolute position updates. Each update is added into the spring network with an error covariance that corresponds to the strength of the spring. Each update is added with its best estimate of its relative localization solution. The Spring Network currently has the following types of springs: • Georegistered – This type of spring is an absolute update that might be entered from a GPS receiver, tagging a known georegistered point, entering a point from map data, or any other absolute position algorithm. • Relational – This type of spring is entered when two systems (e.g. dismounted soldiers) are co-located and “tag” each other. Tags can be entered manually by the soldiers or automatically by setting a threshold on a ranging device, using a Proceedings of the 2017 Ground Vehicle Systems