0 专栏介绍

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

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

1 Informed RRT*原理

传统的RRT算法存在一些局限性。在复杂的环境中，RRT算法可能会生成较长的路径，因为它主要依赖于随机采样，路径的探索性较强，而对于局部信息的利用较少，这可能导致路径搜索效率低。

在这里插入图片描述

Informed RRT*算法针对RRT*算法进行了采样优化，用椭圆采样代替全局均匀采样，避免了RRT*算法搜索树上产生过多冗余分支的缺陷，提高了搜索效率和收敛速度。在Informed RRT*算法中，以起点、终点为焦点，二者的直线距离为焦距 $c_{\min}$ ；当前规划的起点、终点最佳路径长度为 $c_{\mathrm{best}}$ ，以 $c_{\mathrm{best}}$ 为长轴， $\sqrt{c_{\mathrm{best}}^{2}-c_{\min}^{2}}$ 为短轴构造椭圆采样区域。

在这里插入图片描述

工程上一般先在标准圆内采样，再通过齐次变换

$T=\left[ \begin{matrix} \boldsymbol{R}& \boldsymbol{p}\\\end{matrix} \right] , \boldsymbol{R}=\left[ \begin{matrix} a\cos \theta& b\sin \theta\\ -a\sin \theta& b\cos \theta\\\end{matrix} \right] , \boldsymbol{p}=\left[ \begin{array}{c} x_{\mathrm{center}}\\ y_{\mathrm{center}}\\\end{array} \right]$

将采样点映射到地图中，其中 $\theta$ 是起点、终点连线与 $x$ 轴的夹角； $\boldsymbol{p}$ 是起点、终点的中点； $a$ 、 $b$ 分别是长轴、短轴， $\boldsymbol{R}$ 是伸缩变换和旋转变换的复合。我们通过代码来直观看看是如何实现椭圆采样的

Node InformedRRT::_transform(double x, double y)
{// centerdouble center_x = (start_.x_ + goal_.x_) / 2;double center_y = (start_.y_ + goal_.y_) / 2;// rotationdouble theta = -_angle(start_, goal_);// ellipsedouble a = c_best_ / 2.0;double c = c_min_ / 2.0;double b = std::sqrt(a * a - c * c);// transformint tx = (int)(a * cos(theta) * x + b * sin(theta) * y + center_x);int ty = (int)(-a * sin(theta) * x + b * cos(theta) * y + center_y);int id = grid2Index(tx, ty);return Node(tx, ty, 0, 0, id, 0);
}

2 Informed RRT*流程

Informed RRT*算法流程如下

在这里插入图片描述

3 ROS C++实现

核心代码如下所示

bool InformedRRT::plan(const unsigned char* gloal_costmap, const Node& start, const Node& goal, std::vector<Node>& path,std::vector<Node>& expand)
{// initializationc_best_ = std::numeric_limits<double>::max();c_min_ = _dist(start, goal);int best_parent = -1;sample_list_.clear();// copystart_ = start, goal_ = goal;costs_ = gloal_costmap;sample_list_.insert(start);expand.push_back(start);// main loopint iteration = 0;while (iteration < sample_num_){iteration++;// 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_, sample_node);if (new_node.id_ == -1)continue;else{sample_list_.insert(new_node);expand.push_back(new_node);}// goal foundauto dist = _dist(new_node, goal_);if (dist <= max_dist_ && !_isAnyObstacleInPath(new_node, goal_)){double cost = dist + new_node.g_;if (cost < c_best_){best_parent = new_node.id_;c_best_ = cost;}}}if (best_parent != -1){Node goal_(goal_.x_, goal_.y_, c_best_, 0, grid2Index(goal_.x_, goal_.y_),best_parent);sample_list_.insert(goal_);path = _convertClosedListToPath(sample_list_, start, goal);return true;}return false;
}

运行效果图

在这里插入图片描述

4 Python实现

核心代码如下所示

def plan(self):# generate a random node in the mapnode_rand = self.generateRandomNode()# visitedif node_rand in self.sample_list:return 0, None# generate new nodenode_new = self.getNearest(self.sample_list, node_rand)if node_new:self.sample_list.append(node_new)dist = self.dist(node_new, self.goal)# goal foundif dist <= self.max_dist and not self.isCollision(node_new, self.goal):self.goal.parent = node_new.currentself.goal.g = node_new.g + self.dist(self.goal, node_new)self.sample_list.append(self.goal)return self.extractPath(self.sample_list)return 0, None

运行效果图

在这里插入图片描述

5 Matlab实现

核心代码如下：

function [cost, flag, node_list, path] = plan(node_list, start, goal, map, param)cost = 0;flag = false;path = [];% generate a random node in the mapnode_rand = generate_node(start, goal, param);% visitedif loc_list(node_rand, node_list, [1, 2])returnend% generate new node[node_new, success] = get_nearest(node_list, node_rand, map, param);if successnode_list = [node_new; node_list];distance = dist(node_new(1:2), goal');% goal foundif distance <= param.max_dist && ~is_collision(node_new(1:2), goal, map, param)goal_ = [goal, node_new(3) + distance, node_new(1:2)];node_list = [goal_; node_list];flag = true;cost = goal_(3);path = extract_path(node_list, start);node_list(1, :) = [];returnendend
end