The A* search algorithm is nearly ubiquitous in path planning applications due to its simplicity and impressive performance. Before planning a route using A*, the map is usually discretized into cells where each has a cost associated with it (see Chatper 3 for a detailed treatment of costmaps). The cost of each cell can be used to constrain the search space or alternatively to penalize paths. In the constraint formulation, cells with costs above a threshold are marked as unnavigable and excluded from the planning. For the penalty formulation, the cost of each cell is a component of the overall cost of a route; this means that longer paths may have an overall lower cost if they traverse lower cost cells.
A* requires a heuristic cost function that is admissible, which means that the heuristic function must always produce an optimistic estimate of the expected cost from the current location to the goal. When the cost of each cell is used as a penalty, creating an admissible and useful heuristic cost function is much more difficult than the constraint case. In this work, A* will be used on a costmap where the robot is constrained to some cells while others cells are deemed impassable. The A* search performed for this project departs from the standard use-case because it must consider dynamic elements in the environment.
The proposed extension to A* is Dynamic Horizon A* where dynamic objects are incorporated into the costmap only when they are near the robot. The rationale for this choice is that agents that are far away will likely move before the robot is able to reach their current location, yet it is important for the robot to plan a route that avoids nearby agents. The horizon must be specified according the specifications of the robot. The requirement of defining a horizon may appear antithetical to the previous work on eliminating hyper-parameters in the trajectory prediction models, but the horizon value is much different. The horizon is based on the kinematic constraints of the robot - how far it can move in a single timestep. For Social-LSTM, the hyper-parameters are chosen to best predict human trajectories where it is not known a-priori which neighbors may affect the trajectories. For a robot, the influence of neighboring pedestrians is known since the robot is merely planning a route that is free of obstructions. The horizon value is specified as a radius from the current location of the robot. All cells within this horizon circle (defined by the radius) are checked to ensure that they are not likely to be occupied in the next timestep by another agent and that there are no static obstacles in that cell. Cells outside of the circle are considered valid if there is no static object in the cell.
of the distribution of their likely next location is specified by the output of the neural network. Samples are drawn from these distributions and the distance between the sampled points and the coordinates of the cell are compared. If the distance between the cell and the sample is less than two times the radius of a pedestrian, then the sample indicates a potential collision. By taking many samples (in the experiments, 1000 samples were taken), the proportion of collisions with the samples indicates the overall probability of a collision if the robot moves to that cell in the next timestep. The developer of the robot can then define a threshold where a probability greater than the threshold means that the cell is not safe since the possibility of collision is too high, while probabilities lower than the threshold mark valid cells.
The Dynamic Horizon A* approach is convenient because it can be readily incor- porated in path planning systems that only handle static obstacles. Additionally, it is fast and will converge to the shortest possible path for the current costmap. How- ever, there are two drawbacks. The first issue is that it only considers one timestep into the future and thus it cannot consider the long-range influences of other agents potentially walking into its path in the future. The second issue is that it has an inflexible cost function. A* planners are used to calculate the shortest safe path to the destination, but there are other criteria that may affect the choice of a path. For example, the robot may wish to minimize the amount of times that it forces pedes- trians to walk around it. There is no clear way to incorporate such a goal into the A* search procedure.
While not immediately apparent, a well-chosen horizon value should enable the Dynamic Horizon A* method to minimize the effects of the frozen robot problem. The frozen robot problem occurs when the robot cannot plan a viable route - no safe path exists. If static objects block all paths to the destination, then there is obviously no planning algorithm that can overcome the issue, but there are other cases where dynamic agents (pedestrians) may currently block all paths to the destination. Yet,
Figure 4.11: Dynamic Horizon A* at Each Timestep
people are likely to make room for a robot or other pedestrian trying to go forward. Since the robot only considers cells invalid when a neighbor is likely to be in the cell and the cell is within the horizon, the robot can still plan a path towards the destination that is blocked by people as long as the people are outside of the horizon. A horizon that is too large may cause the robot to plan circuitous routes or fail to find any route (frozen robot problem). A horizon that is too small may allow the robot to collide with pedestrians.
Figure 4.11 is an example of the plan created by the Dynamic Horizon A* algo- rithm at each timestep. After constructing the path (shown in black), the subsequent position of the robot (at the next timestep) is the location on the planned path that is closest to the destination but still possible for the robot to reach within a single timestep. The dashed line in the figure indicates the dynamic horizon. The white dots are the positions of other pedestrians, and the background is a heatmap showing the probability of agents occupying the space in the next timestep. The path clearly avoids the red regions (areas that are highly likely to be occupied) within the circle.
However, the path goes directly through the red regions outside of the horizon be- cause it is expected that the pedestrians will have moved past these areas by the time the robot reaches them.