Sampling functions for global motion planning using Maps of Dynamics for mobile robots

Abstract

Motion planning for mobile robots in dynamic environments shared with people is challenging. In a typical hierarchical planning framework, robots use a global planner for overall path-finding and a local planner for immediate adjustments to the path. Without considering the typical patterns of motion of dynamic entities, the global planner might generate paths that lead the robot into highly congested areas, where it is forced to wait, replan or manoeuvre around dynamic obstacles. Maps of Dynamics (MoDs) are a way to mitigate these issues. MoDs represent patterns of motion exhibited by moving entities in the environment, using probabilistic models. The use of MoDs in cost functions for motion planning enables a robot to plan motions that consider the motion patterns encoded in the MoDs. In previous work, it has been shown that the use of MoDs in the cost function helps generate more efficient paths, i.e., paths that lead to the robot and pedestrians spending less time waiting for each other. It has also been shown that using MoDs in the sampling step of sampling-based motion planning is beneficial to a mobile robot since it can result in reduced computation time by explicitly guiding the sampling process using information encoded in MoDs. However, existing work on the use of MoDs in the sampling process is limited. Correspondingly, an analysis of the performance of sampling heuristics for MoDs is also largely lacking. Since such an analysis is crucial to understand the effectiveness of MoDs in a practical setting, we ask the research question: can we obtain reasonably low-cost solutions using sampling-based motion planners that consider the flow of dynamic entities, in a reasonable amount of time . In this paper, we propose substantial improvements to two existing sampling heuristics: the Dijkstra-graph sampling (DGS), previously restricted to a specific type of MoD is extended to use any MoD; and the intensity-map (normalized number of observations of dynamic entities in each grid cell) is utilized more effectively by using importance sampling instead of rejection sampling. We show that an ellipsoidal heuristic can also be used with MoDs. We experimentally validate several sampling heuristics on two different sampling-based motion planners and present a comprehensive evaluation (52800 runs) of their performance on real-world data from densely populated environments. We conclude that reasonably cost solutions can be quickly obtained using a combination of the sampling heuristics within practically feasible time limits. Using the RRT* planner with our proposed MoD-aware, Dijkstra-graph-based heuristic yields ≈ 5%, %10 and 12% higher success rates after 2, 4 and 8 s of planning respectively, compared to uniform sampling, the baseline.