来用C++写一个RRT算法求自主规划的最短路径

RRT（快速扩展随机树）算法不同于图搜索算法，它通过在全图中不断随机撒点，将路径像树一样拉伸直至终点，这样它不需要对环境进行数学建模，很适合在动态环境下的快速开拓路径。实际上，RRT算法正是维护树状结构的过程。

先写一下粗暴方法的伪代码思路：

T.init(root)
for i = 1 to N
        randnode = Sample()
        nearestnode = FindNearest(T, randnode)
        newnode = ExtendByStep(nearestnode, randnode, step)
        if IsValid(newnode)
            T.AddNode(newnode)
        if  Distance(newnode, goal) < threshold then
            return true
        else
            return false

思路很简单，先初始化一棵树的根节点，接着在N次迭代中不断撒点，与该点最近的树节点向该点延伸一个步长距离，如果新生长的树枝合法（不与障碍物碰撞）则加入到随机树中。当新生长的树枝与终点足够接近，说明成功找到终点了。

C++实现的代码如下：

在这里，首先定义树状结构的节点，为了方便与栅格地图（数组形式）进行交互，我们用数组下标x，y（而并非笛卡尔坐标xy）来表示节点所占用的体素栅格的位置，节点拥有自身位置、父节点与子节点三项数据。由于RRT节点的子节点数量不定，用一个vector表示，而父节点是为了方便随机树找到终点后回溯路径而设定的。

struct PointCell
{
  int x, y;
};

struct TreeNode
{
  PointCell cell;
  TreeNode* father;
  std::vector<TreeNode*> children;
};

 这是整个头文件，我们用ROS中的数据格式以及rviz工具进行可视化测试。值得一提的是，为了防止随机树向四周漫无目的的生长，我们可以设一个概率，在此概率下撒点撒在终点附近以加速树向终点探索。

class RRTPlanner
{
public:
  RRTPlanner();
  ~RRTPlanner();

private:
  bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, double tolerance, std::vector<geometry_msgs::PoseStamped>& plan);
  bool publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, double r, double g, double b, double a);

  TreeNode* root_;//这是整棵树的根部，即起点位置
  TreeNode* goal_node_;//终点位置
  bool buildRRT();
  bool drawSample();
  bool extendNearestNode(PointCell random);
  bool extendSingleStep(const TreeNode* rrtnode, TreeNode* &node, const PointCell random);
  void probSample();
  void addGoalOrientNode();
  void addRandomNode();
  bool goalReached(const TreeNode* nearestNode);

  void mapCB(const nav_msgs::OccupancyGrid::ConstPtr &msg);
  void initialPoseCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg);
  void goalCB(const geometry_msgs::PoseStamped::ConstPtr &msg);

  /* map information */
  double map_resolution_;
  int map_sizex_;
  int map_sizey_;
  double map_origin_x_, map_origin_y_;
  std::vector<int> map_info_;
  std::string frame_;
  std::map<int, PointCell> freespace_;//在这里，我用一个map来表示栅格地图中没有障碍物的部分，我们可以只在空闲位置随机撒点试试看

  /* rrt elements */
  double threhold_, probability_;//判断到终点的容忍度，以及向终点方向拉扯的撒点概率
  /* random number generator */
  std::default_random_engine gen_;
  std::random_device rd_;//随机数生成器
  /* sampling parameters */
  int iteration_;//最大迭代次数
  int sample_num_;//每次随机撒点的数量
  double extend_step_;//树枝扩展的步长
  std::vector<TreeNode*> rrt_nodes_;
  std::vector<PointCell> current_sampling_;//本次撒点的样本
  
  geometry_msgs::PoseStamped start_;
  int start_x_;
  int start_y_;
  geometry_msgs::PoseStamped goal_;
  int goal_x_;
  int goal_y_;
  std::vector<geometry_msgs::PoseStamped> plan_;

  /* ros publisher */
  ros::Publisher plan_pub_, tree_pub_;
  ros::Subscriber map_sub_, pose_sub_, goal_sub_;

  /* mutex */
  boost::mutex mutex;
};

 在这里我们先把每次撒点的数量设为5，生长步长设为0.2米，判断到达终点的阈值为0.1米。为了加速生长，我们使撒点在终点附近的概率设为0.5。迭代次数设为1000次，其实根据实测，这样在一般室内环境的地图100次左右的迭代就能成功。

#include <rrt_test.h>

namespace rrt_planner
{

RRTPlanner::RRTPlanner():
    frame_("map"), iteration_(1000), sample_num_(5), extend_step_(0.2), threhold_(0.1), probability_(0.5)
{
    ros::NodeHandle nd;
    map_sub_ = nd.subscribe<nav_msgs::OccupancyGrid>("map", 1, boost::bind(&RRTPlanner::mapCB, this, _1));
    goal_sub_ = nd.subscribe<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1, boost::bind(&RRTPlanner::goalCB, this, _1));
    pose_sub_ = nd.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 1, boost::bind(&RRTPlanner::initialPoseCB, this, _1));
    plan_pub_ = nd.advertise<nav_msgs::Path>("rrt_path", 1);
    tree_pub_ = nd.advertise<sensor_msgs::PointCloud>("rrt_tree", 1);
    root_ = new TreeNode;
    root_->father = NULL;
    root_->children.clear();
}

RRTPlanner::~RRTPlanner()
{
    for(int i = 0; i < rrt_nodes_.size(); i++)
    {
        delete rrt_nodes_[i];
    }
}

bool RRTPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, double tolerance, std::vector<geometry_msgs::PoseStamped>& plan)
{    
    if(buildRRT())
    {
        ROS_INFO("Plan found.");
        std::vector<geometry_msgs::PoseStamped> temp_plan;
        geometry_msgs::PoseStamped pose;
        pose.header.frame_id = "map";
        pose.pose.position.x = goal_node_->cell.x * map_resolution_ + map_origin_x_;
        pose.pose.position.y = goal_node_->cell.y * map_resolution_ + map_origin_y_;
        pose.pose.orientation.w = 1;
        temp_plan.push_back(pose);
        TreeNode* father = goal_node_->father;
        while(father != NULL)
        {
            pose.header.frame_id = "map";
            pose.pose.position.x = father->cell.x * map_resolution_ + map_origin_x_;
            pose.pose.position.y = father->cell.y * map_resolution_ + map_origin_y_;
            pose.pose.orientation.w = 1;
            temp_plan.push_back(pose);
            father = father->father;
        }
        plan.clear();
        for(int i = temp_plan.size() - 1; i > -1; i--)
        {
            plan.push_back(temp_plan[i]);
        }
        
        publishPlan(plan, 0.0, 1.0, 0.0, 0.0);
        
        sensor_msgs::PointCloud tree;
        tree.header.frame_id = "map";
        for(int i = 0; i < rrt_nodes_.size(); i++)
        {
            geometry_msgs::Point32 point;
            point.x = rrt_nodes_[i]->cell.x * map_resolution_ + map_origin_x_;
            point.y = rrt_nodes_[i]->cell.y * map_resolution_ + map_origin_y_;
            tree.points.push_back(point);
        }
        tree_pub_.publish(tree);
        return true;
    }
    else
    {
        ROS_ERROR("We can not find a plan in %d cycles.", iteration_);
        
        sensor_msgs::PointCloud tree;
        tree.header.frame_id = "map";
        for(int i = 0; i < rrt_nodes_.size(); i++)
        {
            geometry_msgs::Point32 point;
            point.x = rrt_nodes_[i]->cell.x * map_resolution_ + map_origin_x_;
            point.y = rrt_nodes_[i]->cell.y * map_resolution_ + map_origin_y_;
            tree.points.push_back(point);
        }
        tree_pub_.publish(tree);
        return false;
    }
    
}

bool RRTPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, double r, double g, double b, double a)
{
    nav_msgs::Path gui_path;
    gui_path.poses.resize(path.size());
    
    if(!path.empty())
    {
        gui_path.header.frame_id = path[0].header.frame_id;
        gui_path.header.stamp = path[0].header.stamp;
    }
    // Extract the plan in world co-ordinates, we assume the path is all in the same frame
    for(unsigned int i=0; i < path.size(); i++)
    {
        gui_path.poses[i] = path[i];
    }
    
    plan_pub_.publish(gui_path);
}

bool RRTPlanner::buildRRT()
{
    for(int i = 0; i < iteration_; i++)
    {
        if(drawSample())
        {
            for(int j = 0; j < current_sampling_.size(); j++)
            {
                if(extendNearestNode(current_sampling_[j]))
                    return true;
            }
        }
    }
    return false;
}

bool RRTPlanner::drawSample()
{
    if(freespace_.size() == 0)
        return false;
    else
    {
        probSample();
    }
    return true;
}

bool RRTPlanner::extendNearestNode(PointCell random)
{
    int j;
    double path_length = 10000000;
    for(int i = 0; i < rrt_nodes_.size(); i++)
    {
        if(hypot(rrt_nodes_[i]->cell.x - random.x, rrt_nodes_[i]->cell.y - random.y) <= path_length)
        {
            path_length = hypot(rrt_nodes_[i]->cell.x - random.x, rrt_nodes_[i]->cell.y - random.y);
            j = i;
        }
    }
    TreeNode* node;
    node = new TreeNode;
    if(extendSingleStep(rrt_nodes_[j], node, random))
    {
        node->father = rrt_nodes_[j];
        rrt_nodes_[j]->children.push_back(node);
        rrt_nodes_.push_back(node);
        if(goalReached(node))
        {
            goal_node_ = node;
            return true;
        }
    }
    return false;
}

bool RRTPlanner::extendSingleStep(const TreeNode* rrtnode, TreeNode* &node, const PointCell random)
{
    double sintheta, costheta;
    
    sintheta = (random.y - rrtnode->cell.y) / sqrt(pow((random.x - rrtnode->cell.x), 2) + pow((random.y - rrtnode->cell.y), 2));
    costheta = (random.x - rrtnode->cell.x) / sqrt(pow((random.x - rrtnode->cell.x), 2) + pow((random.y - rrtnode->cell.y), 2));
    node->cell.x = rrtnode->cell.x + (extend_step_ / map_resolution_) * costheta;
    node->cell.y = rrtnode->cell.y + (extend_step_ / map_resolution_) * sintheta;
    if(map_info_[node->cell.y * map_sizex_ + node->cell.x] == 0)
        return true;
    else
        return false;
}

void RRTPlanner::probSample()
{    
    double prob = rd_() % 10 * 0.1;
    
    if(prob > probability_)
    {
        addGoalOrientNode();
    }
    else
        addRandomNode();
}

void RRTPlanner::addGoalOrientNode()
{
    current_sampling_.clear();
    for(int i = 0;i < sample_num_;i++)
    {
        PointCell p;
        //这里我们可以将撒的点直接设在终点，也可以在终点附近有一个高斯分布
        p.x = goal_x_;// + (rd_() % 200 - 100) * 0.2;
        p.y = goal_y_;// + (rd_() % 200 - 100) * 0.2;
        current_sampling_.push_back(p);
    }
}

void RRTPlanner::addRandomNode()
{
    current_sampling_.clear();
    for(int i = 0;i < sample_num_;i++)
    {
        int x = rd_() % map_sizex_;
        int y = rd_() % map_sizey_;
        
        PointCell p;
        
        p.x = x;
        p.y = y;
        
        current_sampling_.push_back(p);
    }
}

bool RRTPlanner::goalReached(const TreeNode* nearestNode)
{
    if(sqrt(pow(nearestNode->cell.x - goal_x_, 2) + pow(nearestNode->cell.y - goal_y_, 2)) * map_resolution_ < threhold_)
        return true;
    else
        return false;
}

void RRTPlanner::mapCB(const nav_msgs::OccupancyGrid::ConstPtr &msg)
{
    mutex.lock();
        
    map_resolution_ = msg->info.resolution;
    map_sizex_ = msg->info.width;
    map_sizey_ = msg->info.height;
    map_origin_x_ = msg->info.origin.position.x;
    map_origin_y_ = msg->info.origin.position.y;
    freespace_.clear();
    int j = 0;
    int index;
    for(int i = 0; i < map_sizex_ * map_sizey_; i++)
    {
        map_info_.push_back(msg->data[i]);
        
        if(msg->data[i] == 0)
        {
            index = i + 1;
            ++j;
            
            PointCell cell;
            cell.x = index % map_sizex_;
            cell.y = index / map_sizex_;
            freespace_[j] = cell;
        }
        
    }
    mutex.unlock();
}

void RRTPlanner::initialPoseCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg)
{
    mutex.lock();
    if(!map_info_.empty())
    {
        start_x_ = (msg->pose.pose.position.x - map_origin_x_) / map_resolution_;
        start_y_ = (msg->pose.pose.position.y - map_origin_y_) / map_resolution_;
        start_.pose = msg->pose.pose;
        
        root_->cell.x = start_x_;
        root_->cell.y = start_y_;
        rrt_nodes_.push_back(root_);
    }
    mutex.unlock();
}

void RRTPlanner::goalCB(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    mutex.lock();
    if(!map_info_.empty())
    {        
        goal_x_ = (msg->pose.position.x - map_origin_x_) / map_resolution_;
        goal_y_ = (msg->pose.position.y - map_origin_y_) / map_resolution_;
        
        PointCell goal;
        goal.x = goal_x_;
        goal.y = goal_y_;
        
        PointCell start;
        start.x = start_x_;
        start.y = start_y_;
        
        goal_.pose = msg->pose;
    }
    
    mutex.unlock();
    
    makePlan(start_, goal_, threhold_, plan_);
    
}

} /* end of namespace */

int main(int argc, char** argv)
{
    ros::init(argc, argv, "sampling");
    
    rrt_planner::RRTPlanner planner;
    
    ros::spin();
    return 0;
};

这样得到的路径，它的美观程度只能是一般，由于生长的不确定性，可能会长出一些奇怪的转角，这个我们可以用一些美化方法例如贝赛尔曲线等将路径变得更加平滑，等会再来填坑。当然，RRT比起图搜索（A*，Dijkstra）来说确实是快，而且无需对地图进行图论意义上的转化，很适合无人驾驶、复杂机械臂等领域的动态避障问题。