版權(quán)聲明
本文版權(quán)屬于：a15082671703。如侵權(quán)請(qǐng)聯(lián)系博主刪除。
本文背景
Apollo是無(wú)人駕駛相關(guān)的開(kāi)源框架，GitHub地址為https://github.com/ApolloAuto/apollo，在決策部分主要具有Perception(感知),Prediction(預(yù)測(cè)),Routing(路由尋徑),Planning(軌跡規(guī)劃),Control(控制)。由于最近在看Routing相關(guān)的代碼，所以主要針對(duì)Routing內(nèi)容的總結(jié)。
本文是對(duì)Routing策略中的AStar算法的介紹。
Astar介紹
AStar算法的具體介紹在網(wǎng)上搜索就能知道，看到有比較好的 堪稱(chēng)最好的A*算法，可以先了解一下Astar的原理。主要思路就是在dijkstra的基礎(chǔ)上增加啟發(fā)式函數(shù)，往搜索目標(biāo)搜索，加快搜索速度。
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 <algorithm>
#include <cmath>
#include <fstream>
#include <limits>
#include <queue>
#include <modules/perception/obstacle/camera/lane_post_process/common/util.h>
#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<double>::max();
SearchNode() = default;
explicit SearchNode(const TopoNode* node)
: topo_node(node), f(std::numeric_limits<double>::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<const TopoNode*>& 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 TopoNode*>* const result_node_vec) {
for (int i = static_cast<int>(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<const TopoNode*> 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 TopoNode*>* 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<const TopoNode*> 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 TopoNode*>* 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<const TopoNode*, const TopoNode*>& came_from,
const TopoNode* dest_node, std::vector<NodeWithRange>* result_nodes) {
std::vector<const TopoNode*> 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<NodeWithRange>* const result_nodes) {
// 參數(shù)分別為所有圖節(jié)點(diǎn)，部分圖節(jié)點(diǎn)，起始節(jié)點(diǎn)，目標(biāo)節(jié)點(diǎn)，結(jié)果集
Clear(); // 清除所有的參數(shù)
AINFO << "Start A* search algorithm."; // 記錄日志
std::priority_queue<SearchNode> open_set_detail; //作為已經(jīng)尋路過(guò)的節(jié)點(diǎn)
SearchNode src_search_node(src_node); // 把源節(jié)點(diǎn)node傳入struct中，作為源SearchNode
src_search_node.f = HeuristicCost(src_node, dest_node); //啟發(fā)式距離采用曼哈頓距離
open_set_detail.push(src_search_node); // 源SearchNode 傳入push進(jìn)尋路節(jié)點(diǎn)集中
open_set_.insert(src_node); // 源node節(jié)點(diǎn)也傳入其中
g_score_[src_node] = 0.0; // g函數(shù)評(píng)分，key-value
enter_s_[src_node] = src_node->StartS(); // 進(jìn)入時(shí)的S值，key-value
SearchNode current_node; // 定義SearchNode變量
std::unordered_set<const TopoEdge*> next_edge_set; // 定義邊集
std::unordered_set<const TopoEdge*> sub_edge_set; // 定義邊集
while (!open_set_detail.empty()) { // 當(dāng)源集不為空時(shí)，繼續(xù)循環(huán)
current_node = open_set_detail.top(); // 此時(shí)節(jié)點(diǎn)賦值為源集權(quán)重最高的，權(quán)重用f來(lái)比較
const auto* from_node = current_node.topo_node; // 保存當(dāng)前SearchNode的node節(jié)點(diǎn)
if (current_node.topo_node == dest_node) { // 如果已經(jīng)找到了目標(biāo)node節(jié)點(diǎn)
if (!Reconstruct(came_from_, from_node, result_nodes)) { // 重構(gòu)該路徑是否可行
AERROR << "Failed to reconstruct route."; // 不可行說(shuō)明中間錯(cuò)誤
return false;
}
return true; // 否則返回已經(jīng)正確找到節(jié)點(diǎn)
}
open_set_.erase(from_node); // 開(kāi)集node節(jié)點(diǎn)刪除
open_set_detail.pop(); // 開(kāi)集SearchNode節(jié)點(diǎn)刪除
if (closed_set_.count(from_node) != 0) { // 閉合集如果發(fā)現(xiàn)開(kāi)始集曾經(jīng)已經(jīng)搜索過(guò)，那就不用再搜索一遍了
// if showed before, just skip...
continue;
}
closed_set_.emplace(from_node); // 閉合集增加node節(jié)點(diǎn)
// 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_) // 如果設(shè)置可以換道并且剩余距離大于指定最小距離
? from_node->OutToAllEdge() // 可以有所有的臨接edge
: from_node->OutToSucEdge(); // 只能夠有前行臨接edge
double tentative_g_score = 0.0; // g評(píng)分定義指定0
next_edge_set.clear(); // 邊集清空，這是上一次搜索留下來(lái)的
for (const auto* edge : neighbor_edges) { // 對(duì)于所有的臨接邊
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) { // 循環(huán)next_edge_set
const auto* to_node = edge->ToNode(); // 定義to_node為邊的到達(dá)節(jié)點(diǎn)node
if (closed_set_.count(to_node) == 1) { // 如果到達(dá)節(jié)點(diǎn)曾經(jīng)搜索過(guò)（在閉合集中出現(xiàn)），就再次循環(huán)
continue;
}
if (GetResidualS(edge, to_node) < FLAGS_min_length_for_lane_change) { // 如果邊到到達(dá)節(jié)點(diǎn)node的距離小于限定值，再次循環(huán)
continue;
}
tentative_g_score =
g_score_[current_node.topo_node] + GetCostToNeighbor(edge); // g評(píng)分函數(shù)來(lái)自初始g的node節(jié)點(diǎn)評(píng)分加上到該邊界的距離
if (edge->Type() != TopoEdgeType::TET_FORWARD) { // 如果邊類(lèi)型不是向前
tentative_g_score -=
(edge->FromNode()->Cost() + edge->ToNode()->Cost()) / 2; //g評(píng)分需要減去邊的起始節(jié)點(diǎn)cost加上終止節(jié)點(diǎn)cost的一半,--前面加多了
}
if (open_set_.count(to_node) != 0 &&
tentative_g_score >= g_score_[to_node]) { // 如果g評(píng)分函數(shù)大于原始g的到達(dá)node節(jié)點(diǎn)評(píng)分并且到達(dá)節(jié)點(diǎn)位于開(kāi)集內(nèi)，重新循環(huán)
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(); // 此時(shí)的位置在到達(dá)節(jié)點(diǎn)的起始點(diǎn)
} 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) { // 如果滿(mǎn)足enter_s比end_s大而且下一個(gè)節(jié)點(diǎn)是重點(diǎn)，就再次循環(huán)
continue;
}
enter_s_[to_node] = to_node_enter_s; // to_node的enter_s賦值
}
g_score_[to_node] = tentative_g_score; // to_node的g評(píng)分重新賦值
SearchNode next_node(to_node); // 定義下一個(gè)節(jié)點(diǎn)為to_node的SearchNode
next_node.f = tentative_g_score + HeuristicCost(to_node, dest_node); // next_node的f值為g評(píng)分加上啟發(fā)式距離（曼哈頓）
open_set_detail.push(next_node); // 把下一個(gè)SearchNode放入開(kāi)集SearchNode中
came_from_[to_node] = from_node; // to_node的父節(jié)點(diǎn)為from_node
if (open_set_.count(to_node) == 0) { // 如果開(kāi)集中發(fā)現(xiàn)to_node沒(méi)有出現(xiàn)過(guò)，就添加該節(jié)點(diǎn)
open_set_.insert(to_node);
}
}
} // 結(jié)束路徑搜索
AERROR << "Failed to find goal lane with id: " << dest_node->LaneId(); // 搜索完都沒(méi)有返回True，說(shuō)明沒(méi)找到合適的路徑，輸出目標(biāo)節(jié)點(diǎn)信息
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<double>::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
數(shù)據(jù)結(jié)構(gòu)及函數(shù)介紹
- struct SearchNode：定義SearchNode的結(jié)構(gòu)以及比較方式。利用f的最大值作為比較基礎(chǔ)。
- GetCostToNeighbor: 獲取邊界間的距離
- GetLargestNode：獲取首尾最大距離的TopoNode節(jié)點(diǎn)-
AdjustLaneChangeBackward: 評(píng)判由前往后時(shí)考慮左右轉(zhuǎn)，如果需要轉(zhuǎn)彎，選擇最大的可行并且不屬于i-1節(jié)點(diǎn)車(chē)道，最后返回是否可行。 - AdjustLaneChangeForward: 評(píng)判由后往前考慮左右轉(zhuǎn)，如果需要轉(zhuǎn)彎，選擇最大的可行并且不屬于i+1節(jié)點(diǎn)車(chē)道，最后返回是否可行。
- AdjustLaneChange：評(píng)判是否可以轉(zhuǎn)換車(chē)道的函數(shù)，包含上面兩個(gè)
- Reconstruct：判斷是否可以轉(zhuǎn)換車(chē)道，包含AdjustLaneChange，如果可以就轉(zhuǎn)換，不行就返回False。
- AStarStrategy類(lèi)相關(guān)函數(shù)：
- 構(gòu)造函數(shù)：判斷能否變換車(chē)道
- Clear：清除所有參數(shù)
- HeuristicCost：?jiǎn)l(fā)式Cost，這里用的是曼哈頓距離
- Search：主要函數(shù)，路徑搜索
- GetResidualS：計(jì)算到end集到剩余距離。BFS的思路來(lái)直接利用node節(jié)點(diǎn)具有的特征s計(jì)算。
- GetResidualS-重載：計(jì)算到指定點(diǎn)的剩余距離。
Search函數(shù)具體分析
代碼及注釋
bool AStarStrategy::Search(const TopoGraph* graph,
const SubTopoGraph* sub_graph,
const TopoNode* src_node, const TopoNode* dest_node,
std::vector<NodeWithRange>* const result_nodes) {
// 參數(shù)分別為所有圖節(jié)點(diǎn)，部分圖節(jié)點(diǎn)，起始節(jié)點(diǎn)，目標(biāo)節(jié)點(diǎn)，結(jié)果集
Clear(); // 清除所有的參數(shù)
AINFO << "Start A* search algorithm."; // 記錄日志
std::priority_queue<SearchNode> open_set_detail; //作為已經(jīng)尋路過(guò)的節(jié)點(diǎn)
SearchNode src_search_node(src_node); // 把源節(jié)點(diǎn)node傳入struct中，作為源SearchNode
src_search_node.f = HeuristicCost(src_node, dest_node); //啟發(fā)式距離采用曼哈頓距離
open_set_detail.push(src_search_node); // 源SearchNode 傳入push進(jìn)尋路節(jié)點(diǎn)集中
open_set_.insert(src_node); // 源node節(jié)點(diǎn)也傳入其中
g_score_[src_node] = 0.0; // g函數(shù)評(píng)分，key-value
enter_s_[src_node] = src_node->StartS(); // 進(jìn)入時(shí)的S值，key-value
SearchNode current_node; // 定義SearchNode變量
std::unordered_set<const TopoEdge*> next_edge_set; // 定義邊集
std::unordered_set<const TopoEdge*> sub_edge_set; // 定義邊集
while (!open_set_detail.empty()) { // 當(dāng)源集不為空時(shí)，繼續(xù)循環(huán)
current_node = open_set_detail.top(); // 此時(shí)節(jié)點(diǎn)賦值為源集權(quán)重最高的，權(quán)重用f來(lái)比較
const auto* from_node = current_node.topo_node; // 保存當(dāng)前SearchNode的node節(jié)點(diǎn)
if (current_node.topo_node == dest_node) { // 如果已經(jīng)找到了目標(biāo)node節(jié)點(diǎn)
if (!Reconstruct(came_from_, from_node, result_nodes)) { // 重構(gòu)該路徑是否可行
AERROR << "Failed to reconstruct route."; // 不可行說(shuō)明中間錯(cuò)誤
return false;
}
return true; // 否則返回已經(jīng)正確找到節(jié)點(diǎn)
}
open_set_.erase(from_node); // 開(kāi)集node節(jié)點(diǎn)刪除
open_set_detail.pop(); // 開(kāi)集SearchNode節(jié)點(diǎn)刪除
if (closed_set_.count(from_node) != 0) { // 閉合集如果發(fā)現(xiàn)開(kāi)始集曾經(jīng)已經(jīng)搜索過(guò)，那就不用再搜索一遍了
// if showed before, just skip...
continue;
}
closed_set_.emplace(from_node); // 閉合集增加node節(jié)點(diǎn)
// 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_) // 如果設(shè)置可以換道并且剩余距離大于指定最小距離
? from_node->OutToAllEdge() // 可以有所有的臨接edge
: from_node->OutToSucEdge(); // 只能夠有前行臨接edge
double tentative_g_score = 0.0; // g評(píng)分定義指定0
next_edge_set.clear(); // 邊集清空，這是上一次搜索留下來(lái)的
for (const auto* edge : neighbor_edges) { // 對(duì)于所有的臨接邊
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) { // 循環(huán)next_edge_set
const auto* to_node = edge->ToNode(); // 定義to_node為邊的到達(dá)節(jié)點(diǎn)node
if (closed_set_.count(to_node) == 1) { // 如果到達(dá)節(jié)點(diǎn)曾經(jīng)搜索過(guò)（在閉合集中出現(xiàn)），就再次循環(huán)
continue;
}
if (GetResidualS(edge, to_node) < FLAGS_min_length_for_lane_change) { // 如果邊到到達(dá)節(jié)點(diǎn)node的距離小于限定值，再次循環(huán)
continue;
}
tentative_g_score =
g_score_[current_node.topo_node] + GetCostToNeighbor(edge); // g評(píng)分函數(shù)來(lái)自初始g的node節(jié)點(diǎn)評(píng)分加上到該邊界的距離
if (edge->Type() != TopoEdgeType::TET_FORWARD) { // 如果邊類(lèi)型不是向前
tentative_g_score -=
(edge->FromNode()->Cost() + edge->ToNode()->Cost()) / 2; //g評(píng)分需要減去邊的起始節(jié)點(diǎn)cost加上終止節(jié)點(diǎn)cost的一半,--前面加多了
}
if (open_set_.count(to_node) != 0 &&
tentative_g_score >= g_score_[to_node]) { // 如果g評(píng)分函數(shù)大于原始g的到達(dá)node節(jié)點(diǎn)評(píng)分并且到達(dá)節(jié)點(diǎn)位于開(kāi)集內(nèi)，重新循環(huán)
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(); // 此時(shí)的位置在到達(dá)節(jié)點(diǎn)的起始點(diǎn)
} 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) { // 如果滿(mǎn)足enter_s比end_s大而且下一個(gè)節(jié)點(diǎn)是重點(diǎn)，就再次循環(huán)
continue;
}
enter_s_[to_node] = to_node_enter_s; // to_node的enter_s賦值
}
g_score_[to_node] = tentative_g_score; // to_node的g評(píng)分重新賦值
SearchNode next_node(to_node); // 定義下一個(gè)節(jié)點(diǎn)為to_node的SearchNode
next_node.f = tentative_g_score + HeuristicCost(to_node, dest_node); // next_node的f值為g評(píng)分加上啟發(fā)式距離（曼哈頓）
open_set_detail.push(next_node); // 把下一個(gè)SearchNode放入開(kāi)集SearchNode中
came_from_[to_node] = from_node; // to_node的父節(jié)點(diǎn)為from_node
if (open_set_.count(to_node) == 0) { // 如果開(kāi)集中發(fā)現(xiàn)to_node沒(méi)有出現(xiàn)過(guò)，就添加該節(jié)點(diǎn)
open_set_.insert(to_node);
}
}
} // 結(jié)束路徑搜索
AERROR << "Failed to find goal lane with id: " << dest_node->LaneId(); // 搜索完都沒(méi)有返回True，說(shuō)明沒(méi)找到合適的路徑，輸出目標(biāo)節(jié)點(diǎn)信息
return false;
}
核心代碼介紹
Search代碼主要分為以下幾個(gè)步驟：
- 置入起點(diǎn)s
- 計(jì)算s的f，利用g和h來(lái)協(xié)同計(jì)算
- 將s加入優(yōu)先隊(duì)列open中
- 找到open中最好的節(jié)點(diǎn)，f最小的節(jié)點(diǎn)now。
- 找到與now相連的其他節(jié)點(diǎn)，找到最佳臨接節(jié)點(diǎn)，加入open中。
- 重復(fù)尋找open中最好節(jié)點(diǎn)，重復(fù)第四步。
- 最后找到目標(biāo)節(jié)點(diǎn)。
總結(jié)
該算法是A*算法的一個(gè)實(shí)現(xiàn)，也根據(jù)Apollo算法有了一定的數(shù)據(jù)改變，根據(jù)node節(jié)點(diǎn)新增SearchNode作為輔助搜索工具，能夠考慮到轉(zhuǎn)向或者變道的一個(gè)f值比較?？傮w能夠滿(mǎn)足f宏觀搜索需求。
PS：如果對(duì)其中的一些函數(shù)或者是文章中出現(xiàn)一些問(wèn)題，歡迎留言交流。
