1 题目
对于本次作业，您需要完成AVP系统的规划算法。您需要完成BFS/A*/Hybrid A*算法的功能，并显示最终的规划结果。请完成src/planner.cpp中的Bfs()、AStar()和HybridAStar()函数。
秦老师课上演示点位，搜索路径示例如下图所示。其中绿色路径表示bfs搜索得到的路径，红色路径表示AStar搜索得到的路径，黄色路径表示Hybrid AStar搜索得到的路径！
2 解答
2.1 bfs
使用队列完成bfs搜索，需要注意的是：
(1) 扩展结点时，上下左右移动的优于斜对角移动。本质是因为它们的边权不一样，前者是1.0，后者是
2
\sqrt{2}
2。
(2) 访问过的结点不再访问，用unordered_set<Vector2i, Vector2iHash> visited来记录。
C++代码如下，
vector<Vector2i> Planner::Bfs(const Vector2i &start, const Vector2i &goal,
const unordered_set<Vector2i, Vector2iHash>& obstacles) {
//起点start,终点goal,障碍物obstacles
//计算最短路,并把路径存储在res中,最终返回res
std::cout << "=====start bfs planning=====" << std::endl;
if (obstacles.find(goal) != obstacles.end()) { //特判终点落在障碍物上的情况
return vector<Vector2i>();
}
unordered_set<Vector2i, Vector2iHash> visited;
queue<Vector2i> q;
q.push(start);
visited.insert(start);
unordered_map<Vector2i, Vector2i, Vector2iHash> fa; //记录父结点
while (!q.empty()) {
Vector2i t = q.front();
q.pop();
if (t == goal) {
break; //提前退出
}
//t能走到哪些结点
int x = t.x();
int y = t.y();
//注意对角线移动的代价不等于上下左右移动的代价
//bfs是等边权,只能采取4邻域
//对角线的扩展可以放在数组dirs的后面，表示不优先考虑它。但这样其实与"bfs求解等权边最短路"相矛盾。
//int dirs[4][2] = {{-1,0}, {1,0}, {0,-1}, {0,1}};
int dirs[8][2] = {{-1,0}, {1,0}, {0,-1}, {0,1}, {-1,-1}, {-1,1}, {1,-1}, {1,1}};
for (int k = 0; k < 8; ++k) {
int nx = x + dirs[k][0];
int ny = y + dirs[k][1];
Vector2i node(nx,ny);
if (obstacles.find(node) != obstacles.end()) { //是障碍物,则跳过;
continue;
}
if (visited.find(node) != visited.end()) { //访问过,则跳过;
continue;
}
visited.insert(node); //更新visited
fa[node] = t; //node的父结点是t
q.push(node);
}
}
std::cout << "bfs while loop done!" << std::endl;
//肯定会找到终点goal的
if (visited.find(goal) == visited.end()) {
return vector<Vector2i>();
}
vector<Vector2i> res; //记录最短路径
//res.emplace_back(goal);
Vector2i node = goal;
while (true) {
res.emplace_back(node);
if (fa.find(node) != fa.end()) {
node = fa[node];
} else {
break;
}
}
reverse(res.begin(), res.end()); //从起点到终点
return res;
}
2.2 AStar
使用优先队列来完成，需要注意：
(1) 扩展结点时，上下左右移动的边权为1.0，而斜对角移动的边权为
2
\sqrt{2}
2。
(2) 用unordered_map<Vector2i, double, Vector2iHash> g_values记录每个结点的最小的g函数值（即累积代价）。如果扩展的结点不在g_values中或者其g值在减小，才加入到优先队列中。
(3) 使用两点之间的欧式距离作为启发代价，即h值。
C++代码如下，
vector<Vector2i> Planner::AStar(const Vector2i &start, const Vector2i &goal,
const unordered_set<Vector2i, Vector2iHash>& obstacles) {
//使用Astar算法:同时考虑累积代价g和启发代价h;f=g+h;
//累积代价函数？？？
//启发函数h设置为两点之间的欧式距离
std::cout << "=====start AStar planning=====" << std::endl;
if (obstacles.find(goal) != obstacles.end()) { //特判终点落在障碍物上的情况
return vector<Vector2i>();
}
priority_queue<State, vector<State>, greater<State>> pq; //定义小根堆
unordered_map<Vector2i, double, Vector2iHash> g_values; //最小累积代价g
State st1(start.x(), start.y(), 0.0, 0.0, heuristic_func(start, goal), nullptr);
g_values[start] = 0.0;
pq.push(st1);
while (!pq.empty()) {
auto st = pq.top();
pq.pop();
//验证是否达到目标点
if (fabs(st.x - goal.x()) < 1e-6 && fabs(st.y - goal.y()) < 1e-6) {
//break;
//找到目标,返回路径
vector<Vector2i> res;
State* st_node = &st;
while (st_node != nullptr) {
res.emplace_back(st_node->x, st_node->y);
st_node = st_node->parent;
}
reverse(res.begin(), res.end()); //从起点到终点
return res;
}
//st可以走到哪里?
int x = st.x;
int y = st.y;
for (int dx = -1; dx < 2; ++dx) {
for (int dy = -1; dy < 2; ++dy) {
if (dx == 0 && dy == 0) {
continue;
}
int nx = x + dx;
int ny = y + dy;
Vector2i node(nx, ny);
if (obstacles.find(node) != obstacles.end()) { //落在障碍物上,则跳过
continue;
}
//计算这一步的步长
double edge = 1.0;
if (dx != 0 && dy != 0) {
edge = sqrt(2.0);
}
if (g_values.find(node) == g_values.end() || g_values[node] > st.g + edge) {
g_values[node] = st.g + edge; //更新g_values
State n_st(node.x(), node.y(), 0.0, st.g + edge, st.g + edge + heuristic_func(node, goal), new State(st));
pq.push(n_st);
}
}
}
}
return vector<Vector2i>(); //没有找到目标.返回空路径
}
2.3 Hybrid AStar
使用优先队列来完成，需要注意以下几点：
(1) 扩展结点时，角度使用
−
π
4
、
0.0
、
π
4
-\frac{\pi}{4}、0.0、\frac{\pi}{4}
−4π、0.0、4π采样值，速度使用
−
1.0
、
1.0
-1.0、1.0
−1.0、1.0采样值。
(2) 判断扩展过程中是否与障碍物发生碰撞时，需要对扩展的线段进行10等分采样，每个点转换成0.5米*0.5米分辨率下的坐标值，判断是否在unordered_set<Vector2i, Vector2iHash> obstacles中。只要有一个采样点在obstacles中，则认为是扩展过程中会与障碍物相碰，需要跳过。
C++代码如下，
vector<State*> Planner::HybridAStar(const State& start, const State& goal,
const unordered_set<Vector2i, Vector2iHash>& obstacles,
double wheelbase, double step_size, double max_steer) {
//混合AStar算法
//wheelbase表示前轮和后轮之间的距离
//max_steer表示前轮的最大转向角度
//step_size表示车辆每一步运动的距离
std::cout << "=====start Hybrid AStar planning=====" << std::endl;
Vector2i end_node(goal.x, goal.y);
if (obstacles.find(end_node) != obstacles.end()) { //特判终点落在障碍物上的情况
std::cout << "[0]hybrid astar end! goal in obstacles!" << std::endl;
return {};
}
priority_queue<State, vector<State>, greater<State>> pq; //定义小根堆
unordered_map<Vector2i, double, Vector2iHash> g_values; //最小累积代价g
//start.f = 0.0 + heuristic_func(start.x, start.y, start.theta, goal, wheelbase); //f=g+h;const变量,无法修改
pq.push(start);
g_values[Vector2i(start.x,start.y)] = 0.0; //起点的累积代价为0.0
while (!pq.empty()) {
auto st = pq.top();
pq.pop();
//cout << "hybrid astar. processing (x, y, theta) = (" << st.x << ", " << st.y << ", " << st.theta << ")" << endl;
//验证是否达到目标点
double DIS_THRESHOLD = 0.5; //单位米
//if (fabs(st.x - goal.x) < 1e-6 && fabs(st.y - goal.y) < 1e-6 && fabs(st.theta - goal.theta) < max_steer) {
if (fabs(st.x - goal.x) < DIS_THRESHOLD && fabs(st.y - goal.y) < DIS_THRESHOLD) { //不考虑角度
//break;
//找到目标,返回路径
vector<State*> res;
State* st_node = new State(st);
while (st_node != nullptr) {
res.emplace_back(st_node);
st_node = st_node->parent;
}
reverse(res.begin(), res.end()); //从起点到终点
std::cout << "[1]hybrid astar end! find path!" << std::endl;
return res;
}
//st可以走到哪里?
double x = st.x;
double y = st.y;
double theta = st.theta;
// vector<double> delta_theta_vec = {-max_steer, -max_steer * 0.8, -max_steer * 0.6, -max_steer * 0.4, -max_steer * 0.2, 0.0,
// max_steer * 0.2, max_steer * 0.4, max_steer * 0.6, max_steer * 0.8, max_steer};
//vector<double> delta_theta_vec = {-max_steer, -max_steer * 0.5, 0.0, max_steer * 0.5, max_steer};
vector<double> delta_theta_vec = {0.0, -max_steer, max_steer}; //0.0放在第一位
vector<double> next_theta_vec = {};
for (double delta_theta : delta_theta_vec) {
double next_theta = normalize_theta(theta + delta_theta); //正向;且将角度限制在-pi~pi之间
next_theta_vec.emplace_back(next_theta);
next_theta = normalize_theta(-theta + delta_theta); //负向;且将角度限制在-pi~pi之间
next_theta_vec.emplace_back(next_theta);
}
double step = 0.8; //单位米；sqrt(2) / 2.0 = 0.7071067811865476；
double resolution = 0.5; //单位米
for (double next_theta : next_theta_vec) {
double nx = x + step * cos(next_theta);
double ny = y + step * sin(next_theta);
bool is_obs = false;
int k_max = 10;
for (int k = 1; k <= k_max; ++k) {
double nx1 = x + (step / k_max) * k * cos(next_theta);
double ny1 = y + (step / k_max) * k * sin(next_theta);
int nx1_int = nx1 / resolution;
int ny1_int = ny1 / resolution;
Vector2i node1(nx1_int, ny1_int); //判断是否相交，不能四舍五入
if (obstacles.find(node1) != obstacles.end()) { //落在障碍物上,is_obs设置为true
is_obs = true;
break;
}
}
if (is_obs) { //落在障碍物上,则跳过
continue;
}
int nx_int = nx / resolution;
int ny_int = ny / resolution;
Vector2i node(nx_int, ny_int);
// if (obstacles.find(node) != obstacles.end()) { //落在障碍物上，则跳过
// continue;
// }
if (g_values.find(node) == g_values.end() || g_values[node] > st.g + step) {
g_values[node] = st.g + step; //更新g_values
State n_st(nx, ny, next_theta, st.g + step, st.g + step + heuristic_func(nx, ny, next_theta, goal, wheelbase), new State(st));
pq.push(n_st);
}
}
}
std::cout << "[2]hybrid astar end! cannot find path!" << std::endl;
return {};
}
2.4 最终效果
最终效果如下图所示。其中绿色路径表示bfs搜索得到的路径，红色路径表示AStar搜索得到的路径，黄色路径表示Hybrid AStar搜索得到的路径！
扫一扫
2239
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
