Dynamic Obstacle Avoidance
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.
Figure 1: Global map of the indoor space. It does not get updated once SLAM is completed.
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.
Figure 2: Local map of the region surrounding the robot. It is constantly updated by sensor data.
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.
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 sample 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 used to follow a path.
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.
Figure 5: Sampled trajectories for a Kiwi-drive robot (holonomic).
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.
Figure 6: Alignment map for GlobalTrajectoryAlignCritic. Sampled trajectories that align with the global trajectory will get lower score. The score is later inverted.
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