Background
There are several things that go into play when a robot is moving from point A to point B. Assuming the robot has a map of the environment and is localized inside of that map, it first needs a GLOBAL path planner. The role of the global path planner is to find the shortest obstacle-free path from the robot's current pose to the goal pose. However, the global path planner alone is not sufficient for moving in a dynamic environment as the global map is static.
Quick Summary of Map
Mobile robots have two maps of the environment: global and local. The global map is a map of the entire environment that was generated by SLAM. The map is saved into a file after the mapping is completed and later used alongside with a localization algorithm (ex. AMCL) to localize the robot inside it. Once the map is saved, it is not updated.
Figure 1: Global map (left) of a house in Gazebo simulation (right).
The global map provides information about stationary obstacles, such as walls, furnitures, etc, but it does not provide information about dynamic obstacles, such as moving obstacles or obstacles that were not there when the environment was mapped. Hence the robot also keeps a local map. The local map constantly updates based on sensor readings (ex. LiDAR), but it does not span the entirety of the environment as sensors can only provide information about the environment near the robot. It is also computationally expensive constantly update a large map.
Figure 2: Local map of moving robot. The local map is constantly updated using LiDAR sensor readings.
In order to move in an environment with dynamic obstacles, the robot also needs a LOCAL path planner. The role of the local path planner is to find the optimal path that both follows the path generated by the global path planner and avoid dynamic obstacles detected in the local map.
Implementation
Global Path Planner
For the global path planner, I implemented the popular A* search algorithm. Given a grid-based map and two points on the map, the A* search algorithm finds the shortest path (it may not be the absolute shortest path, but it is good enough in most cases) between the two points.
The algorithm is basically a modified breath-first-search algorithm where the queue is replaced with a priority queue that sorts the inserted nodes based on their heuristics. The heuristics in this case is the sum of three components:
Distance between the current node and the goal node
Distance between the current node and the start node
Occupancy value of the current node
Every time a node is inserted into the priority queue, the node with the lowest priority comes at the top and is therefore investigated first. Intuitively, this means that nodes that are both closer to the goal and obstacle-free are searched first when looking for the shortest path. This is a greedy algorithm that results in a speedy search time, though the resulting path may not be the absolute shortest.
Figure 3: A* path planner is called at a fixed rate to find a path to the goal even when the robot is moving.
Local Path Planner
For the local path planner, I decided to implement the Dynamic Window Approach (DWA). The working principle of this planner is to generate a number of trajectories achievable by the robot and score them based on a variety of different categories to choose the best trajectory.
Figure 4: Dynamic window approach generates various trajectories achievable by the robot and picks the one that best follows the global path.
In figure 4, the gray lines are the generated trajectories and they are generated using the robot's achievable max/min velocity and accelerations to take into consideration the maneuverability constraints of the robot. For example, if the robot's current velocity in the x direction is 1 m/s and its maximum acceleration in the +x and -x directions is 0.3 m/s^2, then in the next 1 second, the robot can have a velocity ranging from 0.7 m/s to 1.3 m/s. Using this logic, you can iterate over all of the possible velocities in the x, y, and theta (rotation in z-axis) over a set duration to make a number of achievable trajectories.
Figure 5: Trajectories for a Kiwi-drive robot (holonomic).
These are the trajectories for the robot that I was working with. They are in a 360 degree around the robot because the robot had a Kiwi drive configuration (3 omni-wheels separated by 120 degrees). This drive configuration has maneuverability in x, y, and theta, where as a differential drive configuration (two wheels attached to the sides) shown in figure 4 only has maneuverability in x and theta. The difference in the maneuverability of the robot results in the difference in the generated trajectories.
Critics
The categories for scoring the trajectories are called critics. A variety of different critics can be used in combination to produce a desired robot behavior.
This is the class implementation for the abstract class BaseCritic which all critics inherit from. As shown by the scoreTrajectory function, the goal of all critics is to take in a trajectory and output a score as a double. All critics must have their own implementation of the scoreTrajectory function.
GlobalTrajectoryAlignCritic
One of the critics is named GlobalTrajectoryAlignCritic. As the name suggests, this critic gives a higher score to trajectories that align with the global trajectory. To do so, it makes an alignment map. The alignment map is a 2D array of values whose length and width are equal to that of the local map and each cell value represents the relative score of that pose.
Figure 6: Alignment map for GlobalTrajectoryAlignCritic. The trajectories that align with the global trajectory will get lower score. The score is later inverted so that the highest scoring trajectory is chosen.
The robot is at the center and the red line represents the global trajectory (generated by the A* search algorithm) inside of the local map. As shown, the lowest cell value is the pose on the global trajectory that is on the edge of the local map and all cell values increase outward from it. At each iteration, the alignment map is recomputed based on the global trajectory and all local trajectories are scored by adding the cell values of all its poses (a trajectory is a set of poses). Hence, the local trajectory that best aligns with the global trajectory will have the lowest score. For this particular critic, lowest score is considered the best fit and the score is later inverted to so that the highest score is considered best fit. The best trajectory is the trajectory with the highest total score from all of the critics.
Result
Figure 7: Dynamic window approach used to follow a global path while avoiding an obstacle. Only the top 10 highest scoring trajectories are shown. The trajectories are colored from red to green based on their scores (red-lowest green-highest).
Comments