基于RRT的路径规划器

1.介绍

快速搜索随机树算法（Rapidly-exploring Random Tree，RRT）由Lavalle提出，是一种采用增量方式增长的随机采样算法，用于解决有代数约束（障碍带来的）和微分约束（非完整性和动态环境带来的）的高维空间问题。

RRT算法的优势在于无需对系统进行建模，无需对搜索区域进行几何划分，在搜索空间的覆盖率高，搜索的范围广，可以尽可能的探索未知区域。但同时也存在算法计算代价过大的问题。

RRT* 与基本RRT算法的主要区别在于RRT* 算法引入了对新生成节点相邻节点的搜索，目的是选择低代价的父节点，除此之外还有重新布线的过程进一步减小路径代价，是解决高维的最优路径规划问题的一个突破性的方法。

RRT* 算法是渐进最优的，该算法在原有的RRT算法上，改进了父节点选择的方式，采用代价函数来选取拓展节点领域内最小代价的节点为父节点，同时，每次迭代后都会重新连接现有树上的节点，从而保证计算的复杂度和渐进最优解。

2.代码链接

GitHub - li-huanhuan/rrt_star_global_planner

3.效果

4.作为插件加入move_base

<param name="base_global_planner" value="RRTstar_planner/RRTstarPlannerROS"/>
<rosparam file="$(find 功能包名)/文件路径/base_global_planner_params.yaml" command="load" />

base_global_planner_params.yaml

RRTstarPlannerROS:
    search_radius: 2.0 #搜索附近的节点的搜索范围
    goal_radius: 0.2  #认为搜索到目标点的范围
    epsilon_min: 0.001 #节点之间的最小允许距离
    epsilon_max: 0.1 #节点之间的最大允许距离
    max_nodes_num: 2000000000.0 #节点数的最大值，最大迭代次数
    plan_time_out: 10.0 #规划超时，默认10s

5.部分代码实现过程

0.节点的定义

struct Node
{
  double x; //节点坐标x
  double y; //节点坐标y
  int node_id; //节点id
  int parent_id; //父节点id
  double cost; //节点代价，可以是栅格地图的代价值
  
  bool operator ==(const Node& node)  //重载==
  {
    return (fabs(x - node.x) < 0.0001) && (fabs(y - node.y) < 0.0001) && (node_id == node.node_id) && (parent_id == node.parent_id) && (fabs(cost - node.cost) < 0.0001) ;
  }

  bool operator !=(const Node& node) //重载！=
  {
    if((fabs(x - node.x) > 0.0001) || (fabs(y - node.y) > 0.0001) || (node_id != node.node_id) || (parent_id != node.parent_id) || (fabs(cost - node.cost) > 0.0001))
      return true;
    else
      return false;
  }
};

1.收到两个点

（一般为机器人起始点和目标点）然后规划这两个点之间的路径。函数为：

bool RRTstarPlannerROS::makePlan(const geometry_msgs::PoseStamped& start,
                                   const geometry_msgs::PoseStamped& goal,
                                   std::vector<geometry_msgs::PoseStamped>& plan)

plan即为规划出的路径。

2.定义两棵树

std::vector< Node > nodes; //第一棵树，从起始点向终点扩散
std::vector< Node > nodes_2; //第二棵树，从终点向起始点扩散

起始点转换成节点并放入树1

    Node start_node;
    start_node.x = start.pose.position.x;
    start_node.y = start.pose.position.y;
    start_node.node_id = 0;
    start_node.parent_id = -1; // None parent node
    start_node.cost = 0.0;
    nodes.push_back(start_node);

终点转换成节点并放入树2

    Node goal_node;
    goal_node.x = goal.pose.position.x;
    goal_node.y = goal.pose.position.y;
    goal_node.node_id = 0;
    goal_node.parent_id = -1; // None parent node
    goal_node.cost = 0.0;
    nodes_2.push_back(goal_node);

定义一些中间变量供计算使用

    std::pair<double, double> p_rand; //随机采样的可行点
    std::pair<double, double> p_new; //第一棵树的新节点
    std::pair<double, double> p_new_2; //第二棵树的新节点
    Node connect_node_on_tree1; //第二课树与第一课树连接到一起时第一课树上距离第二课树最近的节点
    Node connect_node_on_tree2; //第一课树与第二课树连接到一起时第二课树上距离第二课树最近的节点
    bool is_connect_to_tree1 = false; //第二课树主动连接到第一课树上标志
    bool is_connect_to_tree2 = false; //第一课树主动连接到第二课树上标志

    Node node_nearest; //用于存储距离当前节点最近的节点

3.开始扩展树一

首先用随机数判断是随机扩展还是启发式扩展，0.8的概率使用随机采样扩展，0.2的概率使用启发扩展。随机采样意思是随机在地图范围内生成xy坐标且该xy坐标点为可行区域。启发扩展意思是本次的随机节点 = 目标点然后进行后面的计算。

        srand(ros::Time::now().toNSec() + seed++);//修改种子
        unsigned int rand_nu = rand()%10;
        if(rand_nu > 1) // 0.8的概率使用随机采样扩展
        {
          p_rand = sampleFree(); // random point in the free space
        }
        else // 0.2的概率使用启发扩展
        {
          p_rand.first = goal.pose.position.x;
          p_rand.second = goal.pose.position.y;
        }

p_rand为一个新的随机采样点，从树一中找到距离p_rand最近的一个节点

node_nearest = getNearest(nodes, p_rand);

从node_nearest（距离本次随机节点最近的节点）朝向本次随机节点扩展出一个新节点p_new，扩展的长度有距离限制，epsilon_min_为最小距离限制，epsilon_max_为最大距离限制。

p_new = steer(node_nearest.x, node_nearest.y, p_rand.first, p_rand.second);

p_new为扩展出来的新节点。

判断新扩展出来的节点（p_new）与最近的节点（node_nearest）之间是否有障碍物，若无障碍物则执行以下操作否则返回步骤1。

        if (obstacleFree(node_nearest, p_new.first, p_new.second))
        {//树枝无碰撞
          Node newnode;
          newnode.x = p_new.first;
          newnode.y = p_new.second;
          newnode.node_id = nodes.size(); // index of the last element after the push_bask below
          newnode.parent_id = node_nearest.node_id;
          newnode.cost = 0.0;

          // 优化
          newnode = chooseParent(node_nearest, newnode, nodes); // Select the best parent
          nodes.push_back(newnode);
          rewire(nodes, newnode); 优化新节点附近的节点，选择最优的父节点

          geometry_msgs::Point point_tem;
          point_tem.x = nodes[newnode.parent_id].x;
          point_tem.y = nodes[newnode.parent_id].y;
          point_tem.z = 0;
          this->marker_tree_.points.push_back(point_tem);

          if(this->isConnect(newnode,nodes_2,nodes, connect_node_on_tree2)) //判断树一是否与树二连接到一起
          {
            is_connect_to_tree2 = true;
          }

          break;
        }

如果树一与树二连接到了一起则得出路径

      if(is_connect_to_tree2)
      {
        std::cout << "两棵树连接在了一起 1->2 耗时：" << ros::Time::now().toSec() - start_time << "秒" << std::endl;

        getPathFromTree(nodes,nodes_2,connect_node_on_tree2,plan,GetPlanMode::CONNECT1TO2);

        plan[0].pose.orientation = start.pose.orientation;
        plan[plan.size()-1].pose.orientation = goal.pose.orientation;

        nav_msgs::Path path_pose;
        path_pose.header.frame_id = this->frame_id_;
        path_pose.header.stamp = ros::Time::now();
        path_pose.poses = plan;
        plan_pub_.publish(path_pose);
        return true;
      }

如果树一扩展到了目标点足够近则得出路径。

      // 第一棵树搜索到目标点
      if (pointCircleCollision(p_new.first, p_new.second, goal.pose.position.x , goal.pose.position.y, goal_radius_) )
      {
        std::cout << "第一棵树搜索到目标点,耗时：" << ros::Time::now().toSec() - start_time << "秒" << std::endl;

        getPathFromTree(nodes,nodes_2,nodes.back(),plan,GetPlanMode::TREE1);

        plan[0].pose.orientation = start.pose.orientation;
        plan[plan.size()-1].pose.orientation = goal.pose.orientation;

        nav_msgs::Path path_pose;
        path_pose.header.frame_id = this->frame_id_;
        path_pose.header.stamp = ros::Time::now();
        path_pose.poses = plan;
        plan_pub_.publish(path_pose);
        return true;
      }

4.开始扩展树二

与第一课树的扩展区别：第二棵树的随机节点 = 第一棵树的新节点，其他步骤与树一的扩展步骤类似。

// 第二棵树
      p_rand.first = p_new.first;
      p_rand.second = p_new.second;

基于RRT的路径规划器

猜你喜欢