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();// copystart_ = 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 loopint iteration = 0;while (iteration < sample_num_){// generate a random node in the mapNode sample_node = _generateRandomNode();// obstacleif (gloal_costmap[sample_node.id_] >= lethal_cost_ * factor_)continue;// visitedif (sample_list_.find(sample_node) != sample_list_.end())continue;// regular the sample nodeNode 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 exploringNode 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 extendingwhile (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;}elsebreak;// connected -> goal foundif (new_node_b == new_node){path = _convertClosedListToPath(new_node_b);return true;}}}}// swapif (sample_list_b_.size() < sample_list_f_.size())std::swap(sample_list_f_, sample_list_b_);iteration++;}return false;
}

运行效果图

在这里插入图片描述

4 Python算法实现

核心代码如下所示

def plan(self):for _ in range(self.sample_num):# generate a random node in the mapnode_rand = self.generateRandomNode()            # generate new nodenode_new = self.getNearest(self.sample_list_f, node_rand)if node_new:self.sample_list_f.append(node_new)# backward exploringnode_new_b = self.getNearest(self.sample_list_b, node_new)if node_new_b:self.sample_list_b.append(node_new_b)# greedy extendingwhile 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_b2else:breakif 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_freturn 0, None

在这里插入图片描述

5 Matlab算法实现

核心代码如下所示

% main loop
for i=1: param.sample_num% generate a random node in the mapnode_rand = generate_node(goal, param);% generate new node[node_new, success] = get_nearest(sample_list_f, node_rand, map, param);if successsample_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_bsample_list_b = [node_new_b; sample_list_b];% greedy extendingwhile truedistance = 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;elsebreakend% goal foundif 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];returnendendendend[len_f, ~] = size(sample_list_f); [len_b, ~] = size(sample_list_b);if len_b < len_ftemp = sample_list_f;sample_list_f = sample_list_b;sample_list_b = temp;end
end