Apollo中Routing代码分析之AStar算法
本文背景
Apollo是无人驾驶相关的开源框架，GitHub地址为https://github.com/ApolloAuto/apollo，在决策部分主要具有Perception(感知),Prediction(预测),Routing(路由寻径),Planning(轨迹规划),Control(控制)。由于最近在看Routing相关的代码，所以主要针对Routing内容的总结。
本文是对Routing策略中的AStar算法的介绍。
Astar介绍
AStar算法的具体介绍在网上搜索就能知道，看到有比较好的 堪称最好的A*算法，可以先了解一下Astar的原理。主要思路就是在dijkstra的基础上增加启发式函数，往搜索目标搜索，加快搜索速度。
Apollo的Astar策略算法源代码
/******************************************************************************
* Copyright 2017 The Apollo Authors. All Rights Reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*****************************************************************************/
#include
#include
#include
#include
#include
#include
#include "modules/common/log.h"
#include "modules/routing/common/routing_gflags.h"
#include "modules/routing/graph/sub_topo_graph.h"
#include "modules/routing/graph/topo_graph.h"
#include "modules/routing/graph/topo_node.h"
#include "modules/routing/strategy/a_star_strategy.h"
namespace apollo {
namespace routing {
namespace {
struct SearchNode {
const TopoNode* topo_node = nullptr;
double f = std::numeric_limits::max();
SearchNode() = default;
explicit SearchNode(const TopoNode* node)
: topo_node(node), f(std::numeric_limits::max()) {}
SearchNode(const SearchNode& search_node) = default;
bool operator<(const SearchNode& node) const {
// in order to let the top of priority queue is the smallest one!
return f > node.f;
}
bool operator==(const SearchNode& node) const {
return topo_node == node.topo_node;
}
};
double GetCostToNeighbor(const TopoEdge* edge) {
return (edge->Cost() + edge->ToNode()->Cost());
}
const TopoNode* GetLargestNode(const std::vector& nodes) {
double max_range = 0.0;
const TopoNode* largest = nullptr;
for (const auto* node : nodes) {
const double temp_range = node->EndS() - node->StartS();
if (temp_range > max_range) {
max_range = temp_range;
largest = node;
}
}
return largest;
}
bool AdjustLaneChangeBackward(
std::vector* const result_node_vec) {
for (int i = static_cast(result_node_vec->size()) - 2; i > 0; --i) {
const auto* from_node = result_node_vec->at(i);
const auto* to_node = result_node_vec->at(i + 1);
const auto* base_node = result_node_vec->at(i - 1);
const auto* from_to_edge = from_node->GetOutEdgeTo(to_node);
if (from_to_edge == nullptr) {
// may need to recalculate edge,
// because only edge from origin node to subnode is saved
from_to_edge = to_node->GetInEdgeFrom(from_node);
}
if (from_to_edge == nullptr) {
AERROR << "Get null ptr to edge:" << from_node->LaneId() << " ("
<< from_node->StartS() << ", " << from_node->EndS() << ")"
<< " --> " << to_node->LaneId() << " (" << to_node->StartS()
<< ", " << to_node->EndS() << ")";
return false;
}
if (from_to_edge->Type() != TopoEdgeType::TET_FORWARD) {
if (base_node->EndS() - base_node->StartS() <
from_node->EndS() - from_node->StartS()) {
continue;
}
std::vector candidate_set;
candidate_set.push_back(from_node);
const auto& out_edges = base_node->OutToLeftOrRightEdge();
for (const auto* edge : out_edges) {
const auto* candidate_node = edge->ToNode();
if (candidate_node == from_node) {
continue;
}
if (candidate_node->GetOutEdgeTo(to_node) != nullptr) {
candidate_set.push_back(candidate_node);
}
}
const auto* largest_node = GetLargestNode(candidate_set);
if (largest_node == nullptr) {
return false;
}
if (largest_node != from_node) {
result_node_vec->at(i) = largest_node;
}
}
}
return true;
}
bool AdjustLaneChangeForward(
std::vector* const result_node_vec) {
for (size_t i = 1; i < result_node_vec->size() - 1; ++i) {
const auto* from_node = result_node_vec->at(i - 1);
const auto* to_node = result_node_vec->at(i);
const auto* base_node = result_node_vec->at(i + 1);
const auto* from_to_edge = from_node->GetOutEdgeTo(to_node);
if (from_to_edge == nullptr) {
// may need to recalculate edge,
// because only edge from origin node to subnode is saved
from_to_edge = to_node->GetInEdgeFrom(from_node);
}
if (from_to_edge == nullptr) {
AERROR << "Get null ptr to edge:" << from_node->LaneId() << " ("
<< from_node->StartS() << ", " << from_node->EndS() << ")"
<< " --> " << to_node->LaneId() << " (" << to_node->StartS()
<< ", " << to_node->EndS() << ")";
return false;
}
if (from_to_edge->Type() != TopoEdgeType::TET_FORWARD) {
if (base_node->EndS() - base_node->StartS() <
to_node->EndS() - to_node->StartS()) {
continue;
}
std::vector candidate_set;
candidate_set.push_back(to_node);
const auto& in_edges = base_node->InFromLeftOrRightEdge();
for (const auto* edge : in_edges) {
const auto* candidate_node = edge->FromNode();
if (candidate_node == to_node) {
continue;
}
if (candidate_node->GetInEdgeFrom(from_node) != nullptr) {
candidate_set.push_back(candidate_node);
}
}
const auto* largest_node = GetLargestNode(candidate_set);
if (largest_node == nullptr) {
return false;
}
if (largest_node != to_node) {
result_node_vec->at(i) = largest_node;
}
}
}
return true;
}
bool AdjustLaneChange(std::vector* const result_node_vec) {
if (result_node_vec->size() < 3) {
return true;
}
if (!AdjustLaneChangeBackward(result_node_vec)) {
AERROR << "Failed to adjust lane change backward";
return false;
}
if (!AdjustLaneChangeForward(result_node_vec)) {
AERROR << "Failed to adjust lane change backward";
return false;
}
return true;
}
bool Reconstruct(
const std::unordered_map& came_from,
const TopoNode* dest_node, std::vector* result_nodes) {
std::vector result_node_vec;
result_node_vec.push_back(dest_node);
auto iter = came_from.find(dest_node);
while (iter != came_from.end()) {
result_node_vec.push_back(iter->second);
iter = came_from.find(iter->second);
}
std::reverse(result_node_vec.begin(), result_node_vec.end());
if (!AdjustLaneChange(&result_node_vec)) {
AERROR << "Failed to adjust lane change";
return false;
}
result_nodes->clear();
for (const auto* node : result_node_vec) {
result_nodes->emplace_back(node->OriginNode(), node->StartS(),
node->EndS());
}
return true;
}
} // namespace
AStarStrategy::AStarStrategy(bool enable_change)
: change_lane_enabled_(enable_change) {}
void AStarStrategy::Clear() {
closed_set_.clear();
open_set_.clear();
came_from_.clear();
enter_s_.clear();
g_score_.clear();
}
double AStarStrategy::HeuristicCost(const TopoNode* src_node,
const TopoNode* dest_node) {
const auto& src_point = src_node->AnchorPoint();
const auto& dest_point = dest_node->AnchorPoint();
double distance = fabs(src_point.x() - dest_point.x()) +
fabs(src_point.y() - dest_point.y());
return distance;
}
bool AStarStrategy::Search(const TopoGraph* graph,
const SubTopoGraph* sub_graph,
const TopoNode* src_node, const TopoNode* dest_node,
std::vector* const result_nodes) {
// 参数分别为所有图节点，部分图节点，起始节点，目标节点，结果集
Clear(); // 清除所有的参数
AINFO << "Start A* search algorithm."; // 记录日志
std::priority_queue open_set_detail; //作为已经寻路过的节点
SearchNode src_search_node(src_node); // 把源节点node传入struct中，作为源SearchNode
src_search_node.f = HeuristicCost(src_node, dest_node); //启发式距离采用曼哈顿距离
open_set_detail.push(src_search_node); // 源SearchNode 传入push进寻路节点集中
open_set_.insert(src_node); // 源node节点也传入其中
g_score_[src_node] = 0.0; // g函数评分，key-value
enter_s_[src_node] = src_node->StartS(); // 进入时的S值，key-value
SearchNode current_node; // 定义SearchNode变量
std::unordered_set next_edge_set; // 定义边集
std::unordered_set sub_edge_set; // 定义边集
while (!open_set_detail.empty()) { // 当源集不为空时，继续循环
current_node = open_set_detail.top(); // 此时节点赋值为源集权重最高的，权重用f来比较
const auto* from_node = current_node.topo_node; // 保存当前SearchNode的node节点
if (current_node.topo_node == dest_node) { // 如果已经找到了目标node节点
if (!Reconstruct(came_from_, from_node, result_nodes)) { // 重构该路径是否可行
AERROR << "Failed to reconstruct route."; // 不可行说明中间错误
return false;
}
return true; // 否则返回已经正确找到节点
}
open_set_.erase(from_node); // 开集node节点删除
open_set_detail.pop(); // 开集SearchNode节点删除
if (closed_set_.count(from_node) != 0) { // 闭合集如果发现开始集曾经已经搜索过，那就不用再搜索一遍了
// if showed before, just skip...
continue;
}
closed_set_.emplace(from_node); // 闭合集增加node节点
// if residual_s is less than FLAGS_min_length_for_lane_change, only move
// forward
const auto& neighbor_edges =
(GetResidualS(from_node) > FLAGS_min_length_for_lane_change &&
change_lane_enabled_) // 如果设置可以换道并且剩余距离大于指定最小距离
? from_node->OutToAllEdge() // 可以有所有的临接edge
: from_node->OutToSucEdge(); // 只能够有前行临接edge
double tentative_g_score = 0.0; // g评分定义指定0
next_edge_set.clear(); // 边集清空，这是上一次搜索留下来的
for (const auto* edge : neighbor_edges) { // 对于所有的临接边
sub_edge_set.clear(); // 子临接边清空
sub_graph->GetSubInEdgesIntoSubGraph(edge, &sub_edge_set); // sub_edge_set赋值为edge
next_edge_set.insert(sub_edge_set.begin(), sub_edge_set.end()); // 把sub_edge_set汇入next_edge_set
}
for (const auto* edge : next_edge_set) { // 循环next_edge_set
const auto* to_node = edge->ToNode(); // 定义to_node为边的到达节点node
if (closed_set_.count(to_node) == 1) { // 如果到达节点曾经搜索过（在闭合集中出现），就再次循环
continue;
}
if (GetResidualS(edge, to_node) < FLAGS_min_length_for_lane_change) { // 如果边到到达节点node的距离小于限定值，再次循环
continue;
}
tentative_g_score =
g_score_[current_node.topo_node] + GetCostToNeighbor(edge); // g评分函数来自初始g的node节点评分加上到该边界的距离
if (edge->Type() != TopoEdgeType::TET_FORWARD) { // 如果边类型不是向前
tentative_g_score -=
(edge->FromNode()->Cost() + edge->ToNode()->Cost()) / 2; //g评分需要减去边的起始节点cost加上终止节点cost的一半,--前面加多了
}
if (open_set_.count(to_node) != 0 &&
tentative_g_score >= g_score_[to_node]) { // 如果g评分函数大于原始g的到达node节点评分并且到达节点位于开集内，重新循环
continue;
}
// if to_node is reached by forward, reset enter_s to start_s
if (edge->Type() == TopoEdgeType::TET_FORWARD) { // 如果往前
enter_s_[to_node] = to_node->StartS(); // 此时的位置在到达节点的起始点
} else {
// else, add enter_s with FLAGS_min_length_for_lane_change
double to_node_enter_s =
(enter_s_[from_node] + FLAGS_min_length_for_lane_change) /
from_node->Length() * to_node->Length();
// enter s could be larger than end_s but should be less than length
to_node_enter_s = std::min(to_node_enter_s, to_node->Length());
// if enter_s is larger than end_s and to_node is dest_node
if (to_node_enter_s > to_node->EndS() && to_node == dest_node) { // 如果满足enter_s比end_s大而且下一个节点是重点，就再次循环
continue;
}
enter_s_[to_node] = to_node_enter_s; // to_node的enter_s赋值
}
g_score_[to_node] = tentative_g_score; // to_node的g评分重新赋值
SearchNode next_node(to_node); // 定义下一个节点为to_node的SearchNode
next_node.f = tentative_g_score + HeuristicCost(to_node, dest_node); // next_node的f值为g评分加上启发式距离（曼哈顿）
open_set_detail.push(next_node); // 把下一个SearchNode放入开集SearchNode中
came_from_[to_node] = from_node; // to_node的父节点为from_node
if (open_set_.count(to_node) == 0) { // 如果开集中发现to_node没有出现过，就添加该节点
open_set_.insert(to_node);
}
}
} // 结束路径搜索
AERROR << "Failed to find goal lane with id: " << dest_node->LaneId(); // 搜索完都没有返回True，说明没找到合适的路径，输出目标节点信息
return false;
}
double AStarStrategy::GetResidualS(const TopoNode* node) {
double start_s = node->StartS();
const auto iter = enter_s_.find(node);
if (iter != enter_s_.end()) {
if (iter->second > node->EndS()) {
return 0.0;
}
start_s = iter->second;
} else {
AWARN << "lane " << node->LaneId() << "(" << node->StartS() << ", "
<< node->EndS() << "not found in enter_s map";
}
double end_s = node->EndS();
const TopoNode* succ_node = nullptr;
for (const auto* edge : node->OutToAllEdge()) {
if (edge->ToNode()->LaneId() == node->LaneId()) {
succ_node = edge->ToNode();
break;
}
}
if (succ_node != nullptr) {
end_s = succ_node->EndS();
}
return (end_s - start_s);
}
double AStarStrategy::GetResidualS(const TopoEdge* edge,
const TopoNode* to_node) {
if (edge->Type() == TopoEdgeType::TET_FORWARD) {
return std::numeric_limits::max();
}
double start_s = to_node->StartS();
const auto* from_node = edge->FromNode();
const auto iter = enter_s_.find(from_node);
if (iter != enter_s_.end()) {
double temp_s = iter->second / from_node->Length() * to_node->Length();
start_s = std::max(start_s, temp_s);
} else {
AWARN << "lane " << from_node->LaneId() << "(" << from_node->StartS()
<< ", " << from_node->EndS() << "not found in enter_s map";
}
double end_s = to_node->EndS();
const TopoNode* succ_node = nullptr;
for (const auto* edge : to_node->OutToAllEdge()) {
if (edge->ToNode()->LaneId() == to_node->LaneId()) {
succ_node = edge->ToNode();
break;
}
}
if (succ_node != nullptr) {
end_s = succ_node->EndS();
}
return (end_s - start_s);
}
} // namespace routing
} // namespace apollo
数据结构及函数介绍
struct SearchNode：定义SearchNode的结构以及比较方式。利用f的最大值作为比较基础。
GetCostToNeighbor: 获取边界间的距离
GetLargestNode：获取首尾最大距离的TopoNode节点
AdjustLaneChangeBackward: 评判由前往后时考虑左右转，如果需要转弯，选择最大的可行并且不属于i-1节点车道，最后返回是否可行。
AdjustLaneChangeForward: 评判由后往前考虑左右转，如果需要转弯，选择最大的可行并且不属于i+1节点车道，最后返回是否可行。
AdjustLaneChange：评判是否可以转换车道的函数，包含上面两个
Reconstruct：判断是否可以转换车道，包含AdjustLaneChange，如果可以就转换，不行就返回False。
AStarStrategy类相关函数：
构造函数：判断能否变换车道
Clear：清除所有参数
HeuristicCost：启发式Cost，这里用的是曼哈顿距离
Search：主要函数，路径搜索
GetResidualS：计算到end集到剩余距离。BFS的思路来直接利用node节点具有的特征s计算。
GetResidualS-重载：计算到指定点的剩余距离。
Search函数具体分析
代码及注释
bool AStarStrategy::Search(const TopoGraph* graph,
const SubTopoGraph* sub_graph,
const TopoNode* src_node, const TopoNode* dest_node,
std::vector* const result_nodes) {
// 参数分别为所有图节点，部分图节点，起始节点，目标节点，结果集
Clear(); // 清除所有的参数
AINFO << "Start A* search algorithm."; // 记录日志
std::priority_queue open_set_detail; //作为已经寻路过的节点
SearchNode src_search_node(src_node); // 把源节点node传入struct中，作为源SearchNode
src_search_node.f = HeuristicCost(src_node, dest_node); //启发式距离采用曼哈顿距离
open_set_detail.push(src_search_node); // 源SearchNode 传入push进寻路节点集中
open_set_.insert(src_node); // 源node节点也传入其中
g_score_[src_node] = 0.0; // g函数评分，key-value
enter_s_[src_node] = src_node->StartS(); // 进入时的S值，key-value
SearchNode current_node; // 定义SearchNode变量
std::unordered_set next_edge_set; // 定义边集
std::unordered_set sub_edge_set; // 定义边集
while (!open_set_detail.empty()) { // 当源集不为空时，继续循环
current_node = open_set_detail.top(); // 此时节点赋值为源集权重最高的，权重用f来比较
const auto* from_node = current_node.topo_node; // 保存当前SearchNode的node节点
if (current_node.topo_node == dest_node) { // 如果已经找到了目标node节点
if (!Reconstruct(came_from_, from_node, result_nodes)) { // 重构该路径是否可行
AERROR << "Failed to reconstruct route."; // 不可行说明中间错误
return false;
}
return true; // 否则返回已经正确找到节点
}
open_set_.erase(from_node); // 开集node节点删除
open_set_detail.pop(); // 开集SearchNode节点删除
if (closed_set_.count(from_node) != 0) { // 闭合集如果发现开始集曾经已经搜索过，那就不用再搜索一遍了
// if showed before, just skip...
continue;
}
closed_set_.emplace(from_node); // 闭合集增加node节点
// if residual_s is less than FLAGS_min_length_for_lane_change, only move
// forward
const auto& neighbor_edges =
(GetResidualS(from_node) > FLAGS_min_length_for_lane_change &&
change_lane_enabled_) // 如果设置可以换道并且剩余距离大于指定最小距离
? from_node->OutToAllEdge() // 可以有所有的临接edge
: from_node->OutToSucEdge(); // 只能够有前行临接edge
double tentative_g_score = 0.0; // g评分定义指定0
next_edge_set.clear(); // 边集清空，这是上一次搜索留下来的
for (const auto* edge : neighbor_edges) { // 对于所有的临接边
sub_edge_set.clear(); // 子临接边清空
sub_graph->GetSubInEdgesIntoSubGraph(edge, &sub_edge_set); // sub_edge_set赋值为edge
next_edge_set.insert(sub_edge_set.begin(), sub_edge_set.end()); // 把sub_edge_set汇入next_edge_set
}
for (const auto* edge : next_edge_set) { // 循环next_edge_set
const auto* to_node = edge->ToNode(); // 定义to_node为边的到达节点node
if (closed_set_.count(to_node) == 1) { // 如果到达节点曾经搜索过（在闭合集中出现），就再次循环
continue;
}
if (GetResidualS(edge, to_node) < FLAGS_min_length_for_lane_change) { // 如果边到到达节点node的距离小于限定值，再次循环
continue;
}
tentative_g_score =
g_score_[current_node.topo_node] + GetCostToNeighbor(edge); // g评分函数来自初始g的node节点评分加上到该边界的距离
if (edge->Type() != TopoEdgeType::TET_FORWARD) { // 如果边类型不是向前
tentative_g_score -=
(edge->FromNode()->Cost() + edge->ToNode()->Cost()) / 2; //g评分需要减去边的起始节点cost加上终止节点cost的一半,--前面加多了
}
if (open_set_.count(to_node) != 0 &&
tentative_g_score >= g_score_[to_node]) { // 如果g评分函数大于原始g的到达node节点评分并且到达节点位于开集内，重新循环
continue;
}
// if to_node is reached by forward, reset enter_s to start_s
if (edge->Type() == TopoEdgeType::TET_FORWARD) { // 如果往前
enter_s_[to_node] = to_node->StartS(); // 此时的位置在到达节点的起始点
} else {
// else, add enter_s with FLAGS_min_length_for_lane_change
double to_node_enter_s =
(enter_s_[from_node] + FLAGS_min_length_for_lane_change) /
from_node->Length() * to_node->Length();
// enter s could be larger than end_s but should be less than length
to_node_enter_s = std::min(to_node_enter_s, to_node->Length());
// if enter_s is larger than end_s and to_node is dest_node
if (to_node_enter_s > to_node->EndS() && to_node == dest_node) { // 如果满足enter_s比end_s大而且下一个节点是重点，就再次循环
continue;
}
enter_s_[to_node] = to_node_enter_s; // to_node的enter_s赋值
}
g_score_[to_node] = tentative_g_score; // to_node的g评分重新赋值
SearchNode next_node(to_node); // 定义下一个节点为to_node的SearchNode
next_node.f = tentative_g_score + HeuristicCost(to_node, dest_node); // next_node的f值为g评分加上启发式距离（曼哈顿）
open_set_detail.push(next_node); // 把下一个SearchNode放入开集SearchNode中
came_from_[to_node] = from_node; // to_node的父节点为from_node
if (open_set_.count(to_node) == 0) { // 如果开集中发现to_node没有出现过，就添加该节点
open_set_.insert(to_node);
}
}
} // 结束路径搜索
AERROR << "Failed to find goal lane with id: " << dest_node->LaneId(); // 搜索完都没有返回True，说明没找到合适的路径，输出目标节点信息
return false;
} 核心代码介绍
Search代码主要分为以下几个步骤：
1. 置入起点s
2. 计算s的f，利用g和h来协同计算
3. 将s加入优先队列open中
4. 找到open中最好的节点，f最小的节点now。
5. 找到与now相连的其他节点，找到最佳临接节点，加入open中。
6.重复寻找open中最好节点，重复第四步。
7.最后找到目标节点。
总结
该算法是A*算法的一个实现，也根据Apollo算法有了一定的数据改变，根据node节点新增SearchNode作为辅助搜索工具，能够考虑到转向或者变道的一个f值比较。总体能够满足f宏观搜索需求。
PS：如果对其中的一些函数或者是文章中出现一些问题，欢迎留言交流。