前言
今天学习移植一下混合A*(Hybrid a)。想要工程代码的看我这一篇:
Apollo6.0规划代码ros移植-路径规划可跑工程分享
原理基础
1.A start算法
使用A Star 算法实现自动寻路详解
2.Rs曲线
Reeds-Shepp和Dubins曲线简介
[运动规划算法]Dubins曲线和Reeds-Shepp曲线
流程
Hybrid A star是一种带有半径约束的路径平滑规划算法,算法思想来自A算法,但A是没有考虑平滑和半径约束的路径规划算法,且基于栅格地图的网格搜索算法,它们的目标代价函数是为了形成一条路径最短的无碰撞路径。而HybirdAstar是在二者的基础上加入半径约束,进行路径不同曲率方向采样,同时采用reedsheep曲线,进行目标点的衔接,来加速路径的生成效率。
plan代码:
// Hybrid A star的核心过程
//输入:起点(sx, sy, sphi)、终点(ex, ey, ephi)、地图边界(XYbounds,好像要4个:xmin, xmax, ymin, ymax)、障碍物顶点信息(obstacles_vertices_vec)
//输出:*result
bool HybridAStar::Plan(
double sx, double sy, double sphi, double ex, double ey, double ephi,
const std::vector<double> &XYbounds,
const std::vector<std::vector<Vec2d>> &obstacles_vertices_vec,
HybridAStartResult *result)
{
// clear containers
//每次规划,清空之前的缓存数据
open_set_.clear();
close_set_.clear();
open_pq_ = decltype(open_pq_)();
final_node_ = nullptr;
std::vector<std::vector<LineSegment2d>> obstacles_linesegments_vec;
//构造障碍物轮廓线段容器
for (const auto &obstacle_vertices : obstacles_vertices_vec)
{
size_t vertices_num = obstacle_vertices.size(); // 拿到每个障碍物的边
std::vector<LineSegment2d> obstacle_linesegments;
//我认为这里有错,少构造了一条线段
for (size_t i = 0; i < vertices_num - 1; ++i) // 依次对该障碍物的每个边读取
{
LineSegment2d line_segment = LineSegment2d(obstacle_vertices[i], obstacle_vertices[i + 1]); // 遍历各个顶点得到边,这里应该少了end至start的情况
obstacle_linesegments.emplace_back(line_segment);
}
//因为我觉得有问题,所以补充了一条线段
LineSegment2d line_segment = LineSegment2d(obstacle_vertices[0], obstacle_vertices[vertices_num - 1]);
obstacle_linesegments.emplace_back(line_segment);
obstacles_linesegments_vec.emplace_back(obstacle_linesegments); // 该障碍物的所有边得到
}
obstacles_linesegments_vec_ = std::move(obstacles_linesegments_vec);
// load XYbounds
XYbounds_ = XYbounds;
// load nodes and obstacles
//构造规划的起点和终点,并检查其有效性
start_node_.reset(new Node3d({sx}, {sy}, {sphi}, XYbounds_));
end_node_.reset(new Node3d({ex}, {ey}, {ephi}, XYbounds_));
if (!ValidityCheck(start_node_))
{
std::cout << "start_node in collision with obstacles";
return false;
}
if (!ValidityCheck(end_node_))
{
std::cout << "end_node in collision with obstacles";
return false;
}
//使用动态规划DP来计算目标点到某点的启发代价(以目标点为DP的起点)
//生成graph的同时获得了目标点到图中任一点的cost,后续只需要查表,空间换时间
grid_a_star_heuristic_generator_->GenerateDpMap(ex, ey, XYbounds_,
obstacles_linesegments_vec_);
// std::cout << "map time " << Clock::NowInSeconds() - map_time;
// load open set, pq
open_set_.emplace(start_node_->GetIndex(), start_node_);
open_pq_.emplace(start_node_->GetIndex(), start_node_->GetCost());
// Hybrid A* begins
size_t explored_node_num = 0;
double heuristic_time = 0.0;
double rs_time = 0.0;
while (!open_pq_.empty())
{
// take out the lowest cost neighboring node
const std::string current_id = open_pq_.top().first;
open_pq_.pop();
std::shared_ptr<Node3d> current_node = open_set_[current_id];
// check if an analystic curve could be connected from current
// configuration to the end configuration without collision. if so, search
// ends.
// true:如果生成了一条从当前点到目标点的ReedShepp曲线,就找到了最短路径
// false:否则,继续Hybrid A*扩展节点
if (AnalyticExpansion(current_node)) // 用RS曲线试试运气,运气爆棚可以到达终点,则搜索结束
{
break;
}
close_set_.emplace(current_node->GetIndex(), current_node);
// 从current_node出发,依次以不同steering,前进arc(对角线长度)
for (size_t i = 0; i < next_node_num_; ++i)
{
//一个grid内的最后一个路径点叫node,该grid内可以有多个路径点,
//该node的next_node一定在相邻的其他grid内
std::shared_ptr<Node3d> next_node = Next_node_generator(current_node, i);
// boundary check failure handle
if (next_node == nullptr)
{
continue;
}
// check if the node is already in the close set
if (close_set_.find(next_node->GetIndex()) != close_set_.end())
{
continue;
}
// collision check, 负责碰撞检测,输入参数节点所连接的、在同一grid内的其他路径点也一起检测了
if (!ValidityCheck(next_node))
{
continue;
}
// 从未被探索,进行初始化
// open_set_其实是close_set_和 open_pq_的合集
// 每个栅格的index由 x_grid_、y_grid_、phi_grid_共同决定,而不只是x_grid_、y_grid,
// 不过这里phi_grid_根据 phi_grid_resolution 做了离散化,所以重叠程度还是有的
if (open_set_.find(next_node->GetIndex()) == open_set_.end())
{
explored_node_num++;
CalculateNodeCost(current_node, next_node);
open_set_.emplace(next_node->GetIndex(), next_node);
open_pq_.emplace(next_node->GetIndex(), next_node->GetCost());
}
}
}
if (final_node_ == nullptr)
{
std::cout << "Hybrid A searching return null ptr(open_set ran out)";
return false;
}
if (!GetResult(result))
{
std::cout << "GetResult failed";
return false;
}
return true;
}
输入:
起点(sx, sy, sphi)、终点(ex, ey, ephi)、地图边界(XYbounds,好像要4个:xmin, xmax, ymin, ymax)、障碍物顶点信息(obstacles_vertices_vec)。
输出:
HybridAStartResult *result
struct HybridAStartResult
{
std::vector<double> x;
std::vector<double> y;
std::vector<double> phi;
std::vector<double> v;
std::vector<double> a;
std::vector<double> steer;
std::vector<double> accumulated_s;
};
流程代码注释挺清楚的。
测试效果
打开Map的话题,关闭地图的话题,视角为TopDowmOrtho,这时可以出现open_sapce的地图:
open_sapce的地图可以在src/planning/dynamic_routing/maps/map.yaml里面修改图片路径。图片出自https://github.com/karlkurzer/path_planner,感谢大佬。
这里提一下,图里面的黑色原本是障碍物,但是我没有去读里面障碍物的信息,而是用map_empty自己去加障碍物。于是有了下面:
顺便订阅混合A星专用障碍物测试。障碍物发布在src/planning/dynamic_routing/src/Obstacle/Obstacle.cpp文件中。
可看课程系列:
https://www.bilibili.com/video/BV1j94y1B7Ai/?buvid=XY4BB7D48AA71766FB7E422428AB2162768E4&is_story_h5=false&mid=vLcmcVJZwLVM5M3cErNrKw%3D%3D&plat_id=240&share_from=ugc&share_medium=android&share_plat=android&share_source=QQ&share_tag=s_i×tamp=1689339850&unique_k=CP6TAse&up_id=300556577
需要代码可私信。
参考链接
感谢下面的大佬们,大家原理和代码可以参考这些一起学习。
Apollo 6.0 的 Hybrid A star planner