路径规划 | 图解RRT-Connect算法(附ROS C++/Python/Matlab仿真)

2023-06-07

self node rrt

目录0专栏介绍1RRT-Connect基本原理2RRT-Connectvs.RRT3ROSC++算法实现4Python算法实现5Matlab算法实现0专栏介绍🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备！详细介绍全局规划(图搜索、采样法、智能算法等)；局部规划

0 专栏介绍

🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备！详细介绍全局规划(图搜索、采样法、智能算法等)；局部规划(DWA、APF等)；曲线优化(贝塞尔曲线、B样条曲线等)。

🚀详情：图解自动驾驶中的运动规划(Motion Planning)，附几十种规划算法

1 RRT-Connect基本原理

在原始RRT算法中，终点附近的区域信息并不能得到有效利用，为了解决这个问题，可以分别以起点和终点为根节点进行双搜索树双向扩展，当两棵树建立连接时可回溯可行路径，称为RRT-Connect算法

2 RRT-Connect vs. RRT

对原始版本RRT算法不了解的同学请看：路径规划 | 图解快速随机扩展树RRT算法(附ROS C++/Python/Matlab仿真)。与RRT算法相比，RRT-Connect有什么特别的优势呢？

更高效的路径搜索：RRT-Connect算法通过引入Connect启发式算法，将传统的扩张函数替换为一种贪婪策略——允许在更长的距离上移动，而不仅限于单步扩展，从而在探索可行路径时具有更高的效率。这使得RRT-Connect能够更快地收敛到可行路径，尤其是在没有微分约束的情况下。
更好的全局规划性能：RRT-Connect算法的另一个优点是它能够同时从起始配置和目标配置开始扩展RRT。这种双向扩展可以更好地探索搜索空间，并在找到可行路径时更快地连接起始配置和目标配置。这使得RRT-Connect算法在全局路径规划方面更为有效，并且具有更好的搜索性能。

RRT-Connect算法流程如下所示

3 ROS C++算法实现

核心代码如下所示

bool RRTConnect::plan(const unsigned char* gloal_costmap, const Node& start, const Node& goal, std::vector<Node>& path,
                      std::vector<Node>& expand)
{
  sample_list_f_.clear();
  sample_list_b_.clear();
  // copy
  start_ = start, goal_ = goal;
  costs_ = gloal_costmap;
  sample_list_f_.insert(start);
  sample_list_b_.insert(goal);
  expand.push_back(start);
  expand.push_back(goal);

  // main loop
  int iteration = 0;
  while (iteration < sample_num_)
  {
    // generate a random node in the map
    Node sample_node = _generateRandomNode();

    // obstacle
    if (gloal_costmap[sample_node.id_] >= lethal_cost_ * factor_)
      continue;

    // visited
    if (sample_list_.find(sample_node) != sample_list_.end())
      continue;

    // regular the sample node
    Node new_node = _findNearestPoint(sample_list_f_, sample_node);
    if (new_node.id_ == -1)
      continue;
    else
    {
      sample_list_f_.insert(new_node);
      expand.push_back(new_node);
      // backward exploring
      Node new_node_b = _findNearestPoint(sample_list_b_, new_node);
      if (new_node_b.id_ != -1)
      {
        sample_list_b_.insert(new_node_b);
        expand.push_back(new_node_b);
        // greedy extending
        while (true)
        {
          double dist = std::min(max_dist_, _dist(new_node, new_node_b));
          double theta = _angle(new_node_b, new_node);
          Node new_node_b2;
          new_node_b2.x_ = new_node_b.x_ + (int)(dist * cos(theta));
          new_node_b2.y_ = new_node_b.y_ + (int)(dist * sin(theta));
          new_node_b2.id_ = grid2Index(new_node_b2.x_, new_node_b2.y_);
          new_node_b2.pid_ = new_node_b.id_;
          new_node_b2.g_ = dist + new_node_b.g_;

          if (!_isAnyObstacleInPath(new_node_b, new_node_b2))
          {
            sample_list_b_.insert(new_node_b2);
            expand.push_back(new_node_b2);
            new_node_b = new_node_b2;
          }
          else
            break;

          // connected -> goal found
          if (new_node_b == new_node)
          {
            path = _convertClosedListToPath(new_node_b);
            return true;
          }
        }
      }
    }

    // swap
    if (sample_list_b_.size() < sample_list_f_.size())
      std::swap(sample_list_f_, sample_list_b_);

    iteration++;
  }
  return false;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81

运行效果图

4 Python算法实现

核心代码如下所示

def plan(self):
for _ in range(self.sample_num):
    # generate a random node in the map
    node_rand = self.generateRandomNode()            
    # generate new node
    node_new = self.getNearest(self.sample_list_f, node_rand)
    if node_new:
        self.sample_list_f.append(node_new)
        # backward exploring
        node_new_b = self.getNearest(self.sample_list_b, node_new)
        if node_new_b:
            self.sample_list_b.append(node_new_b)
            # greedy extending
            while True:
                dist = min(self.max_dist, self.dist(node_new, node_new_b))
                theta = self.angle(node_new_b, node_new)
                node_new_b2 = Node((node_new_b.current[0] + dist * math.cos(theta),
                                   (node_new_b.current[1] + dist * math.sin(theta))),
                                    node_new_b.current, node_new_b.g + dist, 0)
                if not self.isCollision(node_new_b2, node_new_b):
                    self.sample_list_b.append(node_new_b2)
                    node_new_b = node_new_b2
                else:
                    break

                if node_new_b == node_new:
                    return self.extractPath(node_new)

    if len(self.sample_list_b) < len(self.sample_list_f):
        self.sample_list_f, self.sample_list_b = self.sample_list_b, self.sample_list_f

return 0, None
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

5 Matlab算法实现

核心代码如下所示

% main loop
for i=1: param.sample_num
    % generate a random node in the map
    node_rand = generate_node(goal, param);

    % generate new node
    [node_new, success] = get_nearest(sample_list_f, node_rand, map, param);
    if success
        sample_list_f = [node_new; sample_list_f];
        % backward exploring
        [node_new_b, success_b]  = get_nearest(sample_list_b, node_new(1:2), map, param);
        if success_b
             sample_list_b = [node_new_b; sample_list_b];
             % greedy extending
             while true
                 distance = min(param.max_dist, dist(node_new(1:2), node_new_b(1:2)'));
                 theta = angle(node_new_b, node_new);
                 node_new_b2 = [node_new_b(1) + distance * cos(theta), ...
                                              node_new_b(2) + distance * sin(theta), ...
                                              node_new_b(3) + distance, ...
                                              node_new_b(1:2)];
                if ~is_collision(node_new_b2(1:2), node_new_b(1:2), map, param)
                    sample_list_b = [node_new_b2; sample_list_b];
                    node_new_b = node_new_b2;
                else
                    break
                end
                % goal found
                if node_new_b(1) == node_new(1) && node_new_b(2) == node_new(2)
                    flag = true;
                    cost = sample_list_f(1, 3) + sample_list_b(1, 3);
                    path = extract_path(sample_list_f, sample_list_b, start, goal);
                    expand = [sample_list_f; sample_list_b];
                    return
                end
             end
        end
    end
    
    [len_f, ~] = size(sample_list_f); [len_b, ~] = size(sample_list_b);
    if len_b < len_f
        temp = sample_list_f;
        sample_list_f = sample_list_b;
        sample_list_b = temp;
    end
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46