MotionPlaning（二）基于采样的路径规划——RRT&RRT*

【本文仅作为本人对路径规划学习记录所用】

Preliminaries

	Definition
C-Space	机器人运动空间
Complete Planner	在一定时间内，规划器总是能找到一条路径。Exp：A*，D*。
Probabilistic Complete Planner	概率完备：若一个解存在，则用采样的方法最终定能找到该解。
Resolution Complete Planner	相较于概率完备：采样更加准确，采样过程局限于一定确定的区域内。

Probabilistic Rode Map（概率路图）

PRM
Concept	一种基于采样的路径规划方式，一种图结构：①学习阶段：随机撒点or均匀撒点，然后剔除落在障碍物区域的点，把随机撒的点相近的点用线连起来，只连collision free的点（距离准则和障碍物准则：距离太远的舍去，跨越障碍物的连线舍去）；②查询阶段：在简化的图上进行路径搜索得到路径，Exp：A*，D*。
Advantages	①概率完备；②相较于基于图搜索算法更加高效。
Limitations	①需要解决边界值问题(collision free)；②需要判断两点之间的连线是否适合机器人行进；③算法过于冗长，未将寻迹设为算法首要目标。
Improvements	Lazy collision-checking：①撒点之后不再剔除落在障碍物区域的点，将所有相近点相连；②在简图上直接进行路径规划；③对所得路径经过障碍物的部分路径进行剔除，得到一个新地图;④在新地图基础上再次进行路径规划，Loop，大幅提升PRM的效率。

有关PRM建议阅读：PRM

PRM思想示意图
Lazy collision-checking思想

Rapidly-exploring Random Tree（RRT）

RRT伪代码

RRT	快速搜索随机树
Advantages	①易于实施；②相较于PRM，目标性更强，更具针对性。
Limitations	①所求路径非最短路径；②所求路径于机器人而言亦非最优路径；③不够高效，效率较低（采样点较少进入Narrow Passage空间易使路径复杂）；④需要对整个空间进行采样。
Improvements	kd-tree：提高搜索效率；Bidirectional RRT：即起点与目标点同时构建树，当两段拥有同一节点时，则完成路径构建，能大幅改善Narrow Passage情况。

有关RRT建议阅读RRT

有关kd-tree建议阅读KD-TREE

Bidirectional RRT思想

Rapidly-exploring Random Tree Star (RRT*)

RRT*

RRT能达到渐进最优。相较于RRT算法直接把Xnear当作父节点，RRT在产生一个新节点Xnew后，会在Xnew周围一定半径之内找到相邻的节点：Xnear、Xn等，将各点都与Xnew连接，计算Xnew与各点间的Cost最小值（Cost Function：路径最短或其他规则），选取Costmin的点Xcm为父节点，将Xnew与Xcm连接。
同时加入rewire思想，比较Xn—Xnew—Xcm—Xinit与Xn—Xcm—Xinit的Cost值，若前者Cost值更小，则更新路径，将Xn的父节点更新为Xnew；同时比较Xn2—Xnew—Xcm—Xinit与Xn2—Xn1—Xcm—Xinit的Cost值，若前者Cost值更小，则更新路径，将Xn2的父节点更新为Xnew。

RRT*伪代码

RRT*搜索序示意

RRT*
Advantages	添加了Choose Parent：在产生新的Xnew时，不再直接将Xnew与Xnear连接，而是将Xnew一定半径内的所有节点列为父节点，并依次比较父节点到Xnew的cost后选取min值节点连接；添加了Rewire：即对节点进行回溯，比较节点集中的cost若相较于以Xnew为父节点的cost更高，则将当前的父节点更新为Xnew。
Limitations	在找到First路径后，依旧会对整个地图空间进行随机采样进行路径优化，效率较低，并浪费计算空间。
Improvements	RRT#：在找到First路径后，不再对整个地图随机采样，而是由First路径做出一定限制，在一定范围的椭圆内进行采样，从而优化路径，提高计算效率。

RRT*.h

以下是对项目重要宏文件的简要注释与RRT*部分补全。

#ifndef RRT_STAR_H
#define RRT_STAR_H

#include "occ_grid/occ_map.h"
#include "visualization/visualization.hpp"
#include "sampler.h"
#include "node.h"
#include "kdtree.h"

#include 
#include 
#include 

namespace path_plan
{
  class RRTStar
  {
  public:
    RRTStar(){};
    RRTStar(const ros::NodeHandle &nh, const env::OccMap::Ptr &mapPtr) : nh_(nh), map_ptr_(mapPtr)
    {
      nh_.param("RRT_Star/steer_length", steer_length_, 0.0);
      nh_.param("RRT_Star/search_radius", search_radius_, 0.0);
      nh_.param("RRT_Star/search_time", search_time_, 0.0);
      nh_.param("RRT_Star/max_tree_node_nums", max_tree_node_nums_, 0);
      ROS_WARN_STREAM("[RRT*] param: steer_length: " << steer_length_);
      ROS_WARN_STREAM("[RRT*] param: search_radius: " << search_radius_);
      ROS_WARN_STREAM("[RRT*] param: search_time: " << search_time_);
      ROS_WARN_STREAM("[RRT*] param: max_tree_node_nums: " << max_tree_node_nums_);

      sampler_.setSamplingRange(mapPtr->getOrigin(), mapPtr->getMapSize());

      valid_tree_node_nums_ = 0;
      nodes_pool_.resize(max_tree_node_nums_);
      for (int i = 0; i < max_tree_node_nums_; ++i)
      {
        nodes_pool_[i] = new TreeNode;
      }
    }
    ~RRTStar(){};

    bool plan(const Eigen::Vector3d &s, const Eigen::Vector3d &g)
    {
      reset();
      if (!map_ptr_->isStateValid(s))
      {
        ROS_ERROR("[RRT*]: Start pos collide or out of bound");
        return false;
      }
      if (!map_ptr_->isStateValid(g))
      {
        ROS_ERROR("[RRT*]: Goal pos collide or out of bound");
        return false;
      }
      /* construct start and goal nodes */
      start_node_ = nodes_pool_[1];
      start_node_->x = s;
      start_node_->cost_from_start = 0.0;
      goal_node_ = nodes_pool_[0];
      goal_node_->x = g;
      goal_node_->cost_from_start = DBL_MAX; // important
      valid_tree_node_nums_ = 2;             // put start and goal in tree

      ROS_INFO("[RRT*]: RRT starts planning a path");
      return rrt_star(s, g);
    }

    vector<Eigen::Vector3d> getPath()
    {
      return final_path_;
    }

    vector<vector<Eigen::Vector3d>> getAllPaths()
    {
      return path_list_;
    }

    vector<std::pair<double, double>> getSolutions()
    {
      return solution_cost_time_pair_list_;
    }

    void setVisualizer(const std::shared_ptr<visualization::Visualization> &visPtr)
    {
      vis_ptr_ = visPtr;
    };

  private:
    // nodehandle params
    ros::NodeHandle nh_;

    BiasSampler sampler_;

    double steer_length_;
    double search_radius_;
    double search_time_;
    int max_tree_node_nums_;
    int valid_tree_node_nums_;
    double first_path_use_time_;
    double final_path_use_time_;

    std::vector<TreeNode *> nodes_pool_;
    TreeNode *start_node_;
    TreeNode *goal_node_;
    vector<Eigen::Vector3d> final_path_;
    vector<vector<Eigen::Vector3d>> path_list_;
    vector<std::pair<double, double>> solution_cost_time_pair_list_;

    // environment
    env::OccMap::Ptr map_ptr_;
    std::shared_ptr<visualization::Visualization> vis_ptr_;

    void reset()
    {
      final_path_.clear();
      path_list_.clear();
      solution_cost_time_pair_list_.clear();
      for (int i = 0; i < valid_tree_node_nums_; i++)
      {
        nodes_pool_[i]->parent = nullptr;
        nodes_pool_[i]->children.clear();
      }
      valid_tree_node_nums_ = 0;
    }

    double calDist(const Eigen::Vector3d &p1, const Eigen::Vector3d &p2)
    {
      return (p1 - p2).norm();
    }

    Eigen::Vector3d steer(const Eigen::Vector3d &nearest_node_p, const Eigen::Vector3d &rand_node_p, double len)
    {
      Eigen::Vector3d diff_vec = rand_node_p - nearest_node_p;
      double dist = diff_vec.norm();
      if (diff_vec.norm() <= len)
        return rand_node_p;
      else
        return nearest_node_p + diff_vec * len / dist;
    }

    RRTNode3DPtr addTreeNode(RRTNode3DPtr &parent, const Eigen::Vector3d &state,
                             const double &cost_from_start, const double &cost_from_parent)
    {
      RRTNode3DPtr new_node_ptr = nodes_pool_[valid_tree_node_nums_];
      valid_tree_node_nums_++;
      new_node_ptr->parent = parent;
      parent->children.push_back(new_node_ptr);
      new_node_ptr->x = state;
      new_node_ptr->cost_from_start = cost_from_start;
      new_node_ptr->cost_from_parent = cost_from_parent;
      return new_node_ptr;
    }

    void changeNodeParent(RRTNode3DPtr &node, RRTNode3DPtr &parent, const double &cost_from_parent)
    {
      if (node->parent)
        node->parent->children.remove(node); 
      node->parent = parent;
      node->cost_from_parent = cost_from_parent;
      node->cost_from_start = parent->cost_from_start + cost_from_parent;
      parent->children.push_back(node);

      // for all its descedants, change the cost_from_start and tau_from_start;
      RRTNode3DPtr descendant(node);
      std::queue<RRTNode3DPtr> Q;
      Q.push(descendant);
      while (!Q.empty())
      {
        descendant = Q.front();
        Q.pop();
        for (const auto &leafptr : descendant->children)
        {
          leafptr->cost_from_start = leafptr->cost_from_parent + descendant->cost_from_start;
          Q.push(leafptr);
        }
      }
    }

    void fillPath(const RRTNode3DPtr &n, vector<Eigen::Vector3d> &path)
    {
      path.clear();
      RRTNode3DPtr node_ptr = n;
      while (node_ptr->parent)
      {
        path.push_back(node_ptr->x);
        node_ptr = node_ptr->parent;
      }
      path.push_back(start_node_->x);
      std::reverse(std::begin(path), std::end(path));
    }

    bool rrt_star(const Eigen::Vector3d &s, const Eigen::Vector3d &g)
    {
      ros::Time rrt_start_time = ros::Time::now();
      bool goal_found = false;

      /* kd tree init */
      kdtree *kd_tree = kd_create(3);
      //Add start and goal nodes to kd tree
      kd_insert3(kd_tree, start_node_->x[0], start_node_->x[1], start_node_->x[2], start_node_);

      /* main loop */
      int idx = 0;
      for (idx = 0; (ros::Time::now() - rrt_start_time).toSec() < search_time_ && valid_tree_node_nums_ < max_tree_node_nums_; ++idx)
      {
        /* biased random sampling */
        Eigen::Vector3d x_rand;
        sampler_.samplingOnce(x_rand);
        // samplingOnce(x_rand);
        if (!map_ptr_->isStateValid(x_rand))
        {
          continue;
        }

        struct kdres *p_nearest = kd_nearest3(kd_tree, x_rand[0], x_rand[1], x_rand[2]);
        if (p_nearest == nullptr)
        {
          ROS_ERROR("nearest query error");
          continue;
        }
        RRTNode3DPtr nearest_node = (RRTNode3DPtr)kd_res_item_data(p_nearest);
        kd_res_free(p_nearest);

        Eigen::Vector3d x_new = steer(nearest_node->x, x_rand, steer_length_);
        if (!map_ptr_->isSegmentValid(nearest_node->x, x_new))
        {
          continue;
        }

 
        /* kd_tree bounds search for parent */
        vector<RRTNode3DPtr> neighbour_nodes;
        struct kdres *nbr_set;
        nbr_set = kd_nearest_range3(kd_tree, x_new[0], x_new[1], x_new[2], search_radius_);
        if (nbr_set == nullptr)
        {
          ROS_ERROR("bkwd kd range query error");
          break;
        }
        while (!kd_res_end(nbr_set))
        {
          RRTNode3DPtr curr_node = (RRTNode3DPtr)kd_res_item_data(nbr_set);
          neighbour_nodes.emplace_back(curr_node);
          // store range query result so that we dont need to query again for rewire;
          kd_res_next(nbr_set); //go to next in kd tree range query result
        }
        kd_res_free(nbr_set); //reset kd tree range query

        /* choose parent from kd tree range query result*/
        double dist2nearest = calDist(nearest_node->x, x_new);
        double min_dist_from_start(nearest_node->cost_from_start + dist2nearest);
        double cost_from_p(dist2nearest);
        RRTNode3DPtr min_node(nearest_node); 

     
        for (auto &curr_node : neighbour_nodes)
        {
            if(map_ptr_->isSegmentValid(x_new, curr_node->x))
            {
                if(min_dist_from_start > curr_node->cost_from_start + calDist(curr_node->x, x_new))
                {
                    dist2nearest = calDist(curr_node->x, x_new);
                    min_dist_from_start = curr_node->cost_from_start + dist2nearest;
                    cost_from_p = dist2nearest;
                    min_node = curr_node;
                }
            }
        }

        RRTNode3DPtr new_node(nullptr);
        new_node = addTreeNode(min_node, x_new, min_dist_from_start, cost_from_p);

       
        kd_insert3(kd_tree, x_new[0], x_new[1], x_new[2], new_node);
   

   
        double dist_to_goal = calDist(x_new, goal_node_->x);
        if (dist_to_goal <= search_radius_)
        {
          bool is_connected2goal = map_ptr_->isSegmentValid(x_new, goal_node_->x);
      
          bool is_better_path = goal_node_->cost_from_start > dist_to_goal + new_node->cost_from_start;
          if (is_connected2goal && is_better_path)
          {
            if (!goal_found)
            {
              first_path_use_time_ = (ros::Time::now() - rrt_start_time).toSec();
            }
            goal_found = true;
            changeNodeParent(goal_node_, new_node, dist_to_goal);
            vector<Eigen::Vector3d> curr_best_path;
            fillPath(goal_node_, curr_best_path);
            path_list_.emplace_back(curr_best_path);
            solution_cost_time_pair_list_.emplace_back(goal_node_->cost_from_start, (ros::Time::now() - rrt_start_time).toSec());
          }
        }

        /* rewire */

        for (auto &curr_node : neighbour_nodes)
        {
            double best_cost_before_rewire = goal_node_->cost_from_start;
            // ! -------------------------------------

            if(curr_node->x != start_node_->x && map_ptr_->isSegmentValid(x_new, curr_node->x))
            {
                if(curr_node->cost_from_start > new_node->cost_from_start + calDist(new_node->x, curr_node->x))
                {
                    double cost_from_parent = calDist(curr_node->x, new_node->x);
                    changeNodeParent(curr_node, new_node, cost_from_parent);
                }
            }

            // ! -------------------------------------
            if (best_cost_before_rewire > goal_node_->cost_from_start)
            {
              vector<Eigen::Vector3d> curr_best_path;
              fillPath(goal_node_, curr_best_path);
              path_list_.emplace_back(curr_best_path);
              solution_cost_time_pair_list_.emplace_back(goal_node_->cost_from_start, (ros::Time::now() - rrt_start_time).toSec());
            }
        }
     
      }


      vector<Eigen::Vector3d> vertice;
      vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> edges;
      sampleWholeTree(start_node_, vertice, edges);
      std::vector<visualization::BALL> balls;
      balls.reserve(vertice.size());
      visualization::BALL node_p;
      node_p.radius = 0.06;
      for (size_t i = 0; i < vertice.size(); ++i)
      {
        node_p.center = vertice[i];
        balls.push_back(node_p);
      }
      vis_ptr_->visualize_balls(balls, "tree_vertice", visualization::Color::blue, 1.0);
      vis_ptr_->visualize_pairline(edges, "tree_edges", visualization::Color::red, 0.04);

      if (goal_found)
      {
        final_path_use_time_ = (ros::Time::now() - rrt_start_time).toSec();
        fillPath(goal_node_, final_path_);
        ROS_INFO_STREAM("[RRT*]: first path length: " << solution_cost_time_pair_list_.front().first << ", use_time: " << first_path_use_time_);
      }
      else if (valid_tree_node_nums_ == max_tree_node_nums_)
      {
        ROS_ERROR_STREAM("[RRT*]: NOT CONNECTED TO GOAL after " << max_tree_node_nums_ << " nodes added to rrt-tree");
      }
      else
      {
        ROS_ERROR_STREAM("[RRT*]: NOT CONNECTED TO GOAL after " << (ros::Time::now() - rrt_start_time).toSec() << " seconds");
      }
      return goal_found;
    }

    void sampleWholeTree(const RRTNode3DPtr &root, vector<Eigen::Vector3d> &vertice, vector<std::pair<Eigen::Vector3d, Eigen::Vector3d>> &edges)
    {
      if (root == nullptr)
        return;


      RRTNode3DPtr node = root;
      std::queue<RRTNode3DPtr> Q;
      Q.push(node);
      while (!Q.empty())
      {
        node = Q.front();
        Q.pop();
        for (const auto &leafptr : node->children)
        {
          vertice.push_back(leafptr->x);
          edges.emplace_back(std::make_pair(node->x, leafptr->x));
          Q.push(leafptr);
        }
      }
    }
  };

} 
#endif

kdtree.h

#ifndef _KDTREE_H_
#define _KDTREE_H_

#ifdef __cplusplus
extern "C" {
#endif

struct kdtree;
struct kdres;


/* create a kd-tree for "k"-dimensional data */
struct kdtree *kd_create(int k);

/* free the struct kdtree */
void kd_free(struct kdtree *tree);

/* remove all the elements from the tree */
void kd_clear(struct kdtree *tree);

void kd_data_destructor(struct kdtree *tree, void (*destr)(void*));

/* insert a node, specifying its position, and optional data */
int kd_insert(struct kdtree *tree, const double *pos, void *data);
int kd_insertf(struct kdtree *tree, const float *pos, void *data);
int kd_insert3(struct kdtree *tree, double x, double y, double z, void *data);
int kd_insert3f(struct kdtree *tree, float x, float y, float z, void *data);

/* Find the nearest node from a given point. */
struct kdres *kd_nearest(struct kdtree *tree, const double *pos);
struct kdres *kd_nearestf(struct kdtree *tree, const float *pos);
struct kdres *kd_nearest3(struct kdtree *tree, double x, double y, double z);
struct kdres *kd_nearest3f(struct kdtree *tree, float x, float y, float z);



/* frees a result set returned by kd_nearest_range() */
void kd_res_free(struct kdres *set);

/* returns the size of the result set (in elements) */
int kd_res_size(struct kdres *set);

/* rewinds the result set iterator */
void kd_res_rewind(struct kdres *set);

/* returns non-zero if the set iterator reached the end after the last element */
int kd_res_end(struct kdres *set);

/* advances the result set iterator, returns non-zero on success, zero if there are no more elements in the result set. */
int kd_res_next(struct kdres *set);

/* returns the data pointer (can be null) of the current result set item and optionally sets its position to the pointers(s) if not null.*/
void *kd_res_item(struct kdres *set, double *pos);
void *kd_res_itemf(struct kdres *set, float *pos);
void *kd_res_item3(struct kdres *set, double *x, double *y, double *z);
void *kd_res_item3f(struct kdres *set, float *x, float *y, float *z);

/* equivalent to kd_res_item(set, 0) */
void *kd_res_item_data(struct kdres *set);


#ifdef __cplusplus
}
#endif

#endif  /* _KDTREE_H_ */