Diversity-Based Cooperative Multivehicle Path Planning for Risk Management in Costmap Environments

This paper focuses on developing new navigation algorithms for cooperative unmanned vehicles in costmap environments. The vehicles need to plan optimal paths subject to mutual diversity. We first propose three algorithms that can find optimal paths in costmap environments with improved computational efficiency. The proposed algorithms include 1) the scaled A*; 2) the A* plus-plus; and 3) the A* plus-plus with reconstruction. The heuristics of the three algorithms are proved admissible. Then, a diversity-based path planning algorithm is proposed based on the new A star variant algorithms for multiple vehicles to plan diverse paths for risk management. In particular, we propose an iterative segment planner, which generates a set of spatially diverse paths by iteratively planning segments of all paths in a sequential manner. To prevent the generation of spatially close paths, the iterative segment planner utilizes a penalty function to increase the cost of paths that lie near the route(s) assigned to other vehicles. By varying the penalty gains, different degrees of path diversity can be achieved. Some illustrative examples are provided to compare the performance of the proposed algorithms with the standard A* and Dijkstra's algorithms, and show the effectiveness of the proposed diversity-based path planning algorithm.