0 专栏介绍
🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备！详细介绍全局规划(图搜索、采样法、智能算法等)；局部规划(DWA、APF等)；曲线优化(贝塞尔曲线、B样条曲线等)。
🚀详情：图解自动驾驶中的运动规划(Motion Planning)，附几十种规划算法
1 图解RRT*算法原理
RRT*算法针对传统RRT算法进行了渐进最优改进，在添加 x n e w x_{\mathrm{new}} xnew到搜索树的过程中进行重连选择(Rewire)：构造以 x n e w x_{\mathrm{new}} xnew为圆心 r r r为优化半径的邻域圆，找到与 x n e w x_{\mathrm{new}} xnew连接后路径代价最小的点 x m i n x_{\mathrm{min}} xmin作为 x n e w x_{\mathrm{new}} xnew的父节点，而非直接将 x n e a r x_{\mathrm{near}} xnear、 x n e w x_{\mathrm{new}} xnew相连；与此同时也会优化圆内其他节点 x n x_n xn的代价——若路径
< x i n i t , ⋯ , x n > > < x i n i t , ⋯ , x n e w , x n > \left< x_{\mathrm{init}},\cdots ,x_{\mathrm{n}} \right> >\left< x_{\mathrm{init}},\cdots ,x_{\mathrm{new}},x_{\mathrm{n}} \right> ⟨xinit,⋯,xn⟩>⟨xinit,⋯,xnew,xn⟩
则将 x n e w x_{\mathrm{new}} xnew重连为 x n x_n xn的父节点，如图所示。
RRT*算法流程如下所示，与RRT算法相比只是增加了一个重连优化函数，更新节点连接情况
其中碰撞检测算法 C o l l i s i o n F r e e ( ⋅ ) \mathrm{CollisionFree}\left( \cdot \right) CollisionFree(⋅)常用连线采样法，如图所示，计算概率路图中的连线 ( q , q ′ ) \left( q,q' \right) (q,q′)是否合法需要考虑两个方面
- 连线长度小于阈值 d ( q , q ′ ) < d max \mathrm{d}\left( q,q' \right) <\mathrm{d}_{\max} d(q,q′)<dmax， d max \mathrm{d}_{\max} dmax对无意义的长距离连线进行剪枝；
- 连线不穿过障碍：在连线上按一定步长采样，判断是否存在落入障碍中的采样点。
2 ROS C++算法实现
核心代码如下所示
Node RRTStar::_findNearestPoint(std::unordered_set<Node, NodeIdAsHash, compare_coordinates> list, Node& node)
{
Node nearest_node, new_node(node);
double min_dist = std::numeric_limits<double>::max();
for (const auto node_ : list)
{
// calculate distance
double new_dist = _dist(node_, new_node);
// update nearest node
if (new_dist < min_dist)
{
nearest_node = node_;
new_node.pid_ = nearest_node.id_;
new_node.g_ = new_dist + node_.g_;
min_dist = new_dist;
}
}
// distance longer than the threshold
if (min_dist > max_dist_)
{
// connect sample node and nearest node, then move the nearest node
// forward to sample node with `max_distance` as result
double theta = _angle(nearest_node, new_node);
new_node.x_ = nearest_node.x_ + (int)(max_dist_ * cos(theta));
new_node.y_ = nearest_node.y_ + (int)(max_dist_ * sin(theta));
new_node.id_ = grid2Index(new_node.x_, new_node.y_);
new_node.g_ = max_dist_ + nearest_node.g_;
}
// obstacle check
if (!_isAnyObstacleInPath(new_node, nearest_node))
{
// rewire optimization
for (auto node_ : sample_list_)
{
// inside the optimization circle
double new_dist = _dist(node_, new_node);
if (new_dist < r_)
{
double cost = node_.g_ + new_dist;
// update new sample node's cost and parent
if (new_node.g_ > cost)
{
if (!_isAnyObstacleInPath(new_node, node_))
{
new_node.pid_ = node_.id_;
new_node.g_ = cost;
}
}
else
{
// update nodes' cost inside the radius
cost = new_node.g_ + new_dist;
if (cost < node_.g_)
{
if (!_isAnyObstacleInPath(new_node, node_))
{
node_.pid_ = new_node.id_;
node_.g_ = cost;
}
}
}
}
else
continue;
}
}
else
new_node.id_ = -1;
return new_node;
}
3 Python算法实现
核心代码如下
def getNearest(self, node_list: list, node: Node) -> Node:
'''
Get the node from `node_list` that is nearest to `node` with optimization.
Parameters
----------
node_list: list
exploring list
node: Node
currently generated node
Return
----------
node: Node
nearest node
'''
node_new = super().getNearest(node_list, node)
if node_new:
# rewire optimization
for node_n in node_list:
# inside the optimization circle
new_dist = self.dist(node_n, node_new)
if new_dist < self.r:
cost = node_n.g + new_dist
# update new sample node's cost and parent
if node_new.g > cost and not self.isCollision(node_n, node_new):
node_new.parent = node_n.current
node_new.g = cost
else:
# update nodes' cost inside the radius
cost = node_new.g + new_dist
if node_n.g > cost and not self.isCollision(node_n, node_new):
node_n.parent = node_new.current
node_n.g = cost
else:
continue
return node_new
else:
return None
4 Matlab算法实现
核心代码如下所示
function [new_node, flag] = get_nearest(node_list, node, map, param)
%@breif: Get the node from `node_list` that is nearest to `node`.
flag = false;
% find nearest neighbor
dist_vector = dist(node_list(:, 1:2), node');
[~, index] = min(dist_vector);
node_near = node_list(index, :);
% regular and generate new node
distance = min(dist(node_near(1:2), node'), param.max_dist);
theta = angle(node_near, node);
new_node = [node_near(1) + distance * cos(theta), ...
node_near(2) + distance * sin(theta), ...
node_near(3) + distance, ...
node_near(1:2)];
% obstacle check
if is_collision(new_node(1:2), node_near(1:2), map, param)
return
end
% rewire optimization
[node_num, ~] = size(node_list);
for i=1:node_num
node_n = node_list(i, :);
% inside the optimization circle
new_dist = dist(node_n(1:2), new_node(1:2)');
if new_dist < param.r
cost = node_n(3) + new_dist;
% update new sample node's cost and parent
if new_node(3) > cost && ~is_collision(new_node(1:2), node_n(1:2), map, param)
new_node(4:5) = node_n(1:2);
new_node(3) = cost;
else
% update nodes' cost inside the radius
cost = new_node(3) + new_dist;
if node_n(3) > cost && ~is_collision(new_node(1:2), node_n(1:2), map, param)
node_list(i, 4:5) = new_node(1:2);
node_list(i, 3) = cost;
end
end
else
continue;
end
end
flag = true;
end
🔥 更多精彩专栏：