Dynamic Obstacle Avoidance for Mobile Robot
Github: https://github.com/Juhyung-L/GNC
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 that map, it first needs a GLOBAL path planner. The role of the global path planner is to find the shortest path from the robot's current pose to the goal pose while avoiding obstacle defined by the global map. However, the global path planner alone is not sufficient for moving in a dynamic environment as the global map is static.
A quick summary of map:
Mobile robots have two maps of the environment: global and local. The global map is a map of the entire indoor space 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.
The global map provides information about stationary obstacles, such as walls, furniture, etc, but it does not provide information about moving obstacles. 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 indoor space as sensors can only provide information about the environment near the robot.
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 defined by the local map.
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 popped 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 path found may not be the absolute shortest.
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 sample a number of trajectories achievable by the robot and score them based on a variety of different categories to choose the best trajectory.
The gray trajectories are the sampled trajectories and they are generated using the set max/min velocity and accelerations. 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 sample a number of achievable 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.
class BaseCritic
{
public:
using Ptr = std::shared_ptr<BaseCritic>;
virtual ~BaseCritic() = default;
void initialize(
nav2_util::LifecycleNode::SharedPtr nh,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
{
nh_ = nh;
costmap_ros_ = costmap_ros;
costmap_ = costmap_ros->getCostmap();
on_initialize();
}
virtual void reset() {}
/**
* @param global_traj Global trajectory inside of the local costmap
* @param goal_pose Final pose on the global trajectory (can be outside of the local costmap)
* @brief The parameters provide exhaustive information about the global trajectory
* and the derived classes might not require all of them
*/
virtual void prepare(const nav_2d_msgs::msg::Path2D& global_traj, const geometry_msgs::msg::Pose2D& goal_pose) = 0;
virtual double scoreTrajectory(const nav_2d_msgs::msg::Path2D& local_traj) = 0;
double weight_;
bool invert_score_;
protected:
virtual void on_initialize() {}
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
nav2_costmap_2d::Costmap2D* costmap_;
nav2_util::LifecycleNode::SharedPtr nh_;
std::string name_;
};
This is the class implementation for the abstract class BaseCritic which all critics inherit from. As shown by the scoreTrajectory function, the basic principle 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, I made an alignment map. The alignment map is a 2D array of values whose length and width are equal to that of the local map where each cell value represents the relative score of that pose.
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 sampled local trajectories are scored by adding the cell values of all its 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.
double GlobalTrajectoryAlignCritic::scoreTrajectory(const nav_2d_msgs::msg::Path2D& local_traj)
{
double score = 0.0;
for (auto& pose : local_traj.poses)
{
unsigned int x, y;
if (costmap_->worldToMap(pose.x, pose.y, x, y))
{
unsigned int idx = costmap_->getIndex(x, y);
score += static_cast<double>(alignment_map_[idx]);
}
}
return score;
}Results
Only the top 10 highest scoring trajectories are shown in RViz. The trajectories are colored from red to green based on their scores (red-lowest green-highest).
Comments
Post a Comment