一种基于RRT的路径规划器

最近写了一个基于RRT的全局路径规划器在此记录下来。

环境

ubuntu18.04

ros-melodic

实现方式

作为move_base插件。

介绍

包括RRT* ，RRT-connect，启发式扩展，路径优化等内容。

最终效果

代码连接

https://github.com/li-huanhuan/rrt_star_global_planner

说明

相关算法网上有很多，可以先看看算法的过程，这里不再做介绍。实现过程与最初的算法可能有些出入但核心思想是一致的，仅供参考学习。

实现过程

0、节点的定义（node）

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、开始扩展树一

1、首先用随机数判断是随机扩展还是启发式扩展，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;
        }

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

node_nearest = getNearest(nodes, p_rand);

3、从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为扩展出来的新节点。

4、判断新扩展出来的节点（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;
        }

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

      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;
      }

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

      // 第一棵树搜索到目标点
      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;

测试

首先安装turtlebot3_fake和turtlebot3_bringup

sudo apt-get install ros-melodic-turtlebot3-fake
sudo apt-get install ros-melodic-turtlebot3-bringup

然后启动rrt节点的launch文件

roslaunch rrt_star_global_planner rrt_node.launch

在rviz上发送目标点即可。

效果

作为movebase的插件使用与其他插件一样

设置参数：base_global_planner: RRTstar_planner/RRTstarPlannerROS ,

然后movebase加载/params/rrt_star_planner.yaml参数即可。