ルーティングモジュールの機能
Apollo のルーティング モジュールは、高精度地図の元の情報を読み取り、入力RoutingRequest
情報に従ってbase_map
ナビゲーション トラックの始点と終点として最も一致する点を選択し、その根拠に従ってbase_map
生成されたrouting_map
アクションを読み取ります。topo_graph
次に、Astar アルゴリズムを使用してトポロジ マップ内で検索および接続します。開始点への最適なパスがRoutingResponse
出力として送信されます。
ルーティングモジュールの主な手順
地図上の 2 点間の最短距離を求める場合、道路の交差点 (ジャンクション) をノード (Node)、道路の長さをエッジ (Edge) として使用し、最短経路探索アルゴリズム (Astar) を使用できます。 /BFS/DFS... )、2点間の最短経路を求めて出力します。
一般に、取得した地図形式は最短経路の探索に適した位相形式ではないため、地図を変換する必要があります。したがって、ルーティング モジュールは次の手順に要約できます。
- 地図、ノード、道路情報の生データを取得します。
- 上記の情報を使用して、有向グラフを作成します。
- 最短経路アルゴリズムを使用して、2 点間の最短距離を見つけます
TopoCreater マップ構築
Apollo のマッピングによって構築されたトポロジカル グラフのノードは、上記の従来のノードと矛盾します。自動運転の場合、道路の建設は車線の境界レベルに達する必要がありますが、道路をエッジとして使用し、交差点をノードとして使用する従来の方法は適用できません。
Apollo では、下図に示すように、ポイントが車線を表し、エッジが車線間の相対関係となる定義方法を採用しています。
以下の図は、Apollo における道路と車線の概念を示しています。
Apollo では、Node と Edge は にある protobuf で定義されますmodules/routing/proto/topo_graph.proto
。
出口: 車線の点線に相当する部分、または自分で定義した車線変更が可能な道路区間の区間 道路区間コスト
:制限速度や右折道路区間によりコストが増加し、コスト係数は中心線routing_config.pb.txt
で定義されます: 仮想、基準線の生成に使用されます
マッピングプロセス
マップを構築するコードrouting/topo_creator
は以下のとおりで、ファイル構造は次のとおりです。
.
├── BUILD
├── edge_creator.cc // 建边
├── edge_creator.h
├── graph_creator.cc // 建图
├── graph_creator.h
├── graph_creator_test.cc
├── node_creator.cc // 建节点
├── node_creator.h
└── topo_creator.cc // main函数
具体的な処理やコードの解説はこちらのブログ(https://zhuanlan.zhihu.com/p/65533164)で詳しく解説されています。
PS1 : apollo マップの読み取りには 2 つの形式があります。1 つは OpendriveAdapter によって読み取られる Opendrive 形式で、もう 1 つは apollo 自体によって定義される形式です。
PS2 : routing_map ファイル。txt と bin の 2 つの形式があります。
PS3 : 作成したグラフの流れをまとめます。まず、base_map から道路情報を読み取り、次に道路を横断し、最初にノードを作成し、次にノードのエッジを作成します。グラフの変換(点や辺の情報)は routing_map に保存されるので、 routing_map は grap_protobuf 形式を固めたもので、後からルーティングモジュールは作成された routing_map を読み込み、Astar アルゴリズムを使って経路計画を行います。
ノードを作成する
ノードを作成するプロセスは主に次のGetPbNode()
関数で実装されます。
void GetPbNode(const hdmap::Lane& lane, const std::string& road_id,
const RoutingConfig& routingconfig, Node* const node) {
// 1. 初始化节点信息
InitNodeInfo(lane, road_id, node);
// 2. 初始化节点代价
InitNodeCost(lane, routingconfig, node);
}
ノード情報の初期化
void InitNodeInfo(const Lane& lane, const std::string& road_id,
Node* const node) {
double lane_length = GetLaneLength(lane); // 长度
node->set_lane_id(lane.id().id()); // 车道ID
node->set_road_id(road_id); // 道路ID
// 根据lane的边界,添加能够变道的路段
AddOutBoundary(lane.left_boundary(), lane_length, node->mutable_left_out());
AddOutBoundary(lane.right_boundary(), lane_length, node->mutable_right_out());
node->set_length(lane_length);
node->mutable_central_curve()->CopyFrom(lane.central_curve());
node->set_is_virtual(true);
if (!lane.has_junction_id() ||
lane.left_neighbor_forward_lane_id_size() > 0 ||
lane.right_neighbor_forward_lane_id_size() > 0) {
node->set_is_virtual(false);
}
}
ノードコストの初期化:道路の長さと制限速度に基づいてコストを計算します
void InitNodeCost(const Lane& lane, const RoutingConfig& routing_config,
Node* const node) {
double lane_length = GetLaneLength(lane);
double speed_limit =
lane.has_speed_limit() ? lane.speed_limit() : routing_config.base_speed();
double ratio = speed_limit >= routing_config.base_speed()
? std::sqrt(routing_config.base_speed() / speed_limit)
: 1.0;
// 1. 根据道路长度和速度限制来计算代价
double cost = lane_length * ratio;
if (lane.has_turn()) {
// left_turn_penalty: 50.0
// right_turn_penalty: 20.0
// uturn_penalty: 100.0
if (lane.turn() == Lane::LEFT_TURN) {
cost += routing_config.left_turn_penalty();
} else if (lane.turn() == Lane::RIGHT_TURN) {
cost += routing_config.right_turn_penalty();
} else if (lane.turn() == Lane::U_TURN) {
cost += routing_config.uturn_penalty();
}
}
node->set_cost(cost);
}
エッジを作成する
エッジを作成するプロセスは関数内にありますGetPbEdge()
。
void GetPbEdge(const Node& node_from, const Node& node_to,
const Edge::DirectionType& type,
const RoutingConfig& routing_config, Edge* edge) {
// 设置起始,终止车道和类型
edge->set_from_lane_id(node_from.lane_id());
edge->set_to_lane_id(node_to.lane_id());
edge->set_direction_type(type);
// 默认代价为0,即直接向前开的代价
edge->set_cost(0.0);
if (type == Edge::LEFT || type == Edge::RIGHT) {
const auto& target_range =
(type == Edge::LEFT) ? node_from.left_out() : node_from.right_out();
double changing_area_length = 0.0;
for (const auto& range : target_range) {
changing_area_length += range.end().s() - range.start().s();
}
// 计算代价
double ratio = 1.0;
// base_changing_length: 50.0
if (changing_area_length < routing_config.base_changing_length()) {
ratio = std::pow(
changing_area_length / routing_config.base_changing_length(), -1.5);
}
edge->set_cost(routing_config.change_penalty() * ratio);
}
}
ルーティングコンポーネント
Apollo の各モジュールはサイバーに依存して関連情報が登録されており、各モジュールを参照するにはコンポーネントファイルから開始する必要があります。
ルーティング_コンポーネント.h
#pragma once
#include <memory>
#include "modules/routing/routing.h"
namespace apollo {
namespace routing {
class RoutingComponent final
: public ::apollo::cyber::Component<RoutingRequest> {
public:
// default用来控制默认构造函数的生成。显式地指示编译器生成该函数的默认版本。
RoutingComponent() = default;
~RoutingComponent() = default;
public:
bool Init() override;
// 收到routing request的时候触发
bool Proc(const std::shared_ptr<RoutingRequest>& request) override;
private:
// routing消息发布handle
std::shared_ptr<::apollo::cyber::Writer<RoutingResponse>> response_writer_ =
nullptr;
std::shared_ptr<::apollo::cyber::Writer<RoutingResponse>>
response_history_writer_ = nullptr;
// Routing类
Routing routing_;
std::shared_ptr<RoutingResponse> response_ = nullptr;
// 定时器
std::unique_ptr<::apollo::cyber::Timer> timer_;
// 锁
std::mutex mutex_;
};
// 在cyber框架中注册routing模块
CYBER_REGISTER_COMPONENT(RoutingComponent)
} // namespace routing
} // namespace apollo
ルーティング_コンポーネント.cc
#include "modules/routing/routing_component.h"
#include <utility>
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/routing/common/routing_gflags.h"
DECLARE_string(flagfile);
namespace apollo {
namespace routing {
bool RoutingComponent::Init() {
// 加载并检查config配置文件
// 消息类型定义在modules/routing/proto/routing_config.proto中
RoutingConfig routing_conf;
ACHECK(cyber::ComponentBase::GetProtoConfig(&routing_conf))
<< "Unable to load routing conf file: "
<< cyber::ComponentBase::ConfigFilePath();
AINFO << "Config file: " << cyber::ComponentBase::ConfigFilePath()
<< " is loaded.";
// 设置消息qos,控制流量,创建消息发布response_writer_
apollo::cyber::proto::RoleAttributes attr;
attr.set_channel_name(routing_conf.topic_config().routing_response_topic());
auto qos = attr.mutable_qos_profile();
qos->set_history(apollo::cyber::proto::QosHistoryPolicy::HISTORY_KEEP_LAST);
qos->set_reliability(
apollo::cyber::proto::QosReliabilityPolicy::RELIABILITY_RELIABLE);
qos->set_durability(
apollo::cyber::proto::QosDurabilityPolicy::DURABILITY_TRANSIENT_LOCAL);
response_writer_ = node_->CreateWriter<RoutingResponse>(attr);
// 历史消息发布,和response_writer_类似
apollo::cyber::proto::RoleAttributes attr_history;
attr_history.set_channel_name(
routing_conf.topic_config().routing_response_history_topic());
auto qos_history = attr_history.mutable_qos_profile();
qos_history->set_history(
apollo::cyber::proto::QosHistoryPolicy::HISTORY_KEEP_LAST);
qos_history->set_reliability(
apollo::cyber::proto::QosReliabilityPolicy::RELIABILITY_RELIABLE);
qos_history->set_durability(
apollo::cyber::proto::QosDurabilityPolicy::DURABILITY_TRANSIENT_LOCAL);
response_history_writer_ = node_->CreateWriter<RoutingResponse>(attr_history);
// 创建定时器
std::weak_ptr<RoutingComponent> self =
std::dynamic_pointer_cast<RoutingComponent>(shared_from_this());
timer_.reset(new ::apollo::cyber::Timer(
FLAGS_routing_response_history_interval_ms,
[self, this]() {
auto ptr = self.lock();
if (ptr) {
std::lock_guard<std::mutex> guard(this->mutex_);
if (this->response_ != nullptr) {
auto timestamp = apollo::cyber::Clock::NowInSeconds();
response_->mutable_header()->set_timestamp_sec(timestamp);
this->response_history_writer_->Write(*response_);
}
}
},
false));
timer_->Start();
// 执行Routing类
return routing_.Init().ok() && routing_.Start().ok();
}
// 接收"RoutingRequest"消息,输出"RoutingResponse"响应。
bool RoutingComponent::Proc(const std::shared_ptr<RoutingRequest>& request) {
auto response = std::make_shared<RoutingResponse>();
// 响应routing_请求
if (!routing_.Process(request, response.get())) {
return false;
}
// 填充响应头部信息,并且发布
common::util::FillHeader(node_->Name(), response.get());
response_writer_->Write(response);
{
std::lock_guard<std::mutex> guard(mutex_);
response_ = std::move(response);
}
return true;
}
} // namespace routing
} // namespace apollo
routing-component の一般的な機能を理解したら、Routing の具体的な実装を見てみましょう。
ルーティング固有の実装
ルーティングの具体的な実装はrouting.h
とにありますrouting.cc
。
ルーティングモジュールの入力情報は、元の高精度地図情報(base_map)と生成されたトポロジマップ(routing_map)の2つの固定情報と、外部から入力される起点要求情報(RoutingRequest)からなる。ルートナビゲーションの結果を出力します。
ルーティングモジュールの初期化
まず、Routing の初期化関数を見てみましょう。
// 初始化函数
apollo::common::Status Routing::Init() {
// 读取拓扑地图routing_map的文件位置信息
const auto routing_map_file = apollo::hdmap::RoutingMapFile();
AINFO << "Use routing topology graph path: " << routing_map_file;
// 在Navigator类加载指定的graph图
navigator_ptr_.reset(new Navigator(routing_map_file));
// 通过map模块提供的功能包,读取原始地图信息,即包括点和边的信息
// 据此查找routing request请求的点距离最近的lane,并且返回对应的lane id.
hdmap_ = apollo::hdmap::HDMapUtil::BaseMapPtr();
ACHECK(hdmap_) << "Failed to load map file:" << apollo::hdmap::BaseMapFile();
return apollo::common::Status::OK();
}
ナビゲーターの初期化
ルーティングはNavigator
内部でパスを検索します。検索パスが必要なため、Navigator
完全なトポロジ マップが必要です。
Navigator::Navigator(const std::string& topo_file_path) {
Graph graph;
if (!cyber::common::GetProtoFromFile(topo_file_path, &graph)) {
AERROR << "Failed to read topology graph from " << topo_file_path;
return;
}
graph_.reset(new TopoGraph());
if (!graph_->LoadGraph(graph)) {
AINFO << "Failed to init navigator graph failed! File path: "
<< topo_file_path;
return;
}
black_list_generator_.reset(new BlackListRangeGenerator);
result_generator_.reset(new ResultGenerator);
is_ready_ = true;
AINFO << "The navigator is ready.";
}
コンストラクターではNavigator
、トポロジ マップがロードされます。BlackListRangeGenerator
同時に、2 つのクラス(ブラックリスト セクション生成) とResultGenerator
(結果生成)のオブジェクトが初期化されます。
bool Navigator::Init(const RoutingRequest& request, const TopoGraph* graph,
std::vector<const TopoNode*>* const way_nodes,
std::vector<double>* const way_s) {
Clear();
// 获取routing请求,对应图中的节点
if (!GetWayNodes(request, graph_.get(), way_nodes, way_s)) {
AERROR << "Failed to find search terminal point in graph!";
return false;
}
// 根据请求生成对应的黑名单lane
black_list_generator_->GenerateBlackMapFromRequest(request, graph_.get(),
&topo_range_manager_);
return true;
}
ブラックリストの道路と車線をルーティング リクエストで指定して、ルーティング リクエストがこれらの車線をカウントしないようにすることができます。
ルーティングプロセスのメインプロセス
Processのメインプロセスを実行するプロセスは次のとおりです。
bool Routing::Process(const std::shared_ptr<RoutingRequest>& routing_request,
RoutingResponse* const routing_response) {
CHECK_NOTNULL(routing_response);
AINFO << "Get new routing request:" << routing_request->DebugString();
// 找到routing_request节点最近的路
// FillLaneInfoIfMissing从地图中选取最佳匹配点
const auto& fixed_requests = FillLaneInfoIfMissing(*routing_request);
double min_routing_length = std::numeric_limits<double>::max();
for (const auto& fixed_request : fixed_requests) {
RoutingResponse routing_response_temp;
// 是否能够找到规划路径
if (navigator_ptr_->SearchRoute(fixed_request, &routing_response_temp)) {
const double routing_length =
routing_response_temp.measurement().distance();
if (routing_length < min_routing_length) {
routing_response->CopyFrom(routing_response_temp);
min_routing_length = routing_length;
}
}
FillParkingID(routing_response);
}
if (min_routing_length < std::numeric_limits<double>::max() &&
SupplementParkingRequest(routing_response)) {
monitor_logger_buffer_.INFO("Routing success!");
return true;
}
AERROR << "Failed to search route with navigator.";
monitor_logger_buffer_.WARN("Routing failed! " +
routing_response->status().msg());
return false;
}
FillLaneInfoIfMissing
詳細な説明は、このブログ投稿https://zhuanlan.zhihu.com/p/459954010で説明されています。
ナビゲーターのメイン機能
Navigator 自体はパス検索アルゴリズムを実装しません。他のクラスを使用してルーティング パスの検索プロセスを完了するだけです。その主な機能は にありますSearchRoute
(このブログで詳しく説明していますhttps://paul.pub/apollo-routing/#id-routing_configproto )。
主に次のタスクを実行します:
1. リクエストのパラメータを確認する;
2. 準備完了状態であるかどうかを判断する;
3. リクエストに必要なパラメータを初期化する;
4. 検索アルゴリズムを実行する;
5. 検索結果を組み立てる; 5. リクエストに必要なパラメータを初期化する
SearchRoute
核心は、SearchRouteByStrategy
このブログに詳細な説明があるということですhttps://zhuanlan.zhihu.com/p/65533164 は
主に次のタスクを完了します:
1. パス検索アルゴリズム (AStar) を設定します。
2. 各routing_request
ノードを走査し、設定しますTopoRangeManager
( NodeSRange。検索、追加、並べ替え、マージが可能です)、AddBlackMapFromTerminal
ブラックリストの追加、始点と終点に従ってレーンを分割、サブグラフの作成SubTopoGraph
(検索アルゴリズムで使用)、始点と終点の取得、 AStar パスを使用するのが最適です。結果をnode_vec
3 に保存します。ルート結果をマージします。
Aスター戦略
Navigator::SearchRoute
メソッドはSearchRouteByStrategy
クラス自体のメソッドを呼び出します。この方法では、AStarStrategy
パスの検索は の助けを借りて行われます。
AStarStrategy
このクラスは抽象クラスのStrategy
サブクラスです。つまり、AStar は別のアルゴリズムを実装することで置き換えることができます。
AStar の基本原理については、このブログを参照してください: https://blog.csdn.net/sinat_52032317/article/details/127077625
AStar の具体的な実装プロセスについては、このブログhttps://zhuanlan.zhihu.com/p/459954010
およびこの
https://www.jianshu.com/p/2ee0d6b19b5fを参照してください。
// 输入:起点、终点、读取的拓扑地图以及根据起点终点生成的子拓扑图、到起点到达终点的点集:
bool AStarStrategy::Search(const TopoGraph* graph,
const SubTopoGraph* sub_graph,
const TopoNode* src_node, const TopoNode* dest_node,
std::vector<NodeWithRange>* const result_nodes) {
Clear();
AINFO << "Start A* search algorithm.";
// 该优先列表将f最小的值作为最优点
std::priority_queue<SearchNode> open_set_detail;
// 将地图选取的起点作为搜索的第一个点,计算起点到终点的代价值(曼哈顿距离)并作为f
SearchNode src_search_node(src_node);
src_search_node.f = HeuristicCost(src_node, dest_node);
// 将该点加入到开放列表之中
open_set_detail.push(src_search_node);
open_set_.insert(src_node);
g_score_[src_node] = 0.0;
enter_s_[src_node] = src_node->StartS();
// 定义当前节点
SearchNode current_node;
std::unordered_set<const TopoEdge*> next_edge_set;
std::unordered_set<const TopoEdge*> sub_edge_set;
while (!open_set_detail.empty()) {
// 若open集非空,选取最优的为当前节点,开始搜索
current_node = open_set_detail.top();
const auto* from_node = current_node.topo_node;
// 若当前点等于目标点,结束搜索
if (current_node.topo_node == dest_node) {
// ReconstructL:从终点到起点进行反向搜索,获取轨迹点
if (!Reconstruct(came_from_, from_node, result_nodes)) {
AERROR << "Failed to reconstruct route.";
return false;
}
return true;
}
// 将当前点从搜索点集中清除
open_set_.erase(from_node);
open_set_detail.pop();
// 跳过closed集中添加的点
if (closed_set_.count(from_node) != 0) {
// if showed before, just skip...
continue;
}
// 将当前点添加到closed集中
closed_set_.emplace(from_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()
: from_node->OutToSucEdge();
double tentative_g_score = 0.0;
next_edge_set.clear();
for (const auto* edge : neighbor_edges) {
sub_edge_set.clear();
sub_graph->GetSubInEdgesIntoSubGraph(edge, &sub_edge_set);
next_edge_set.insert(sub_edge_set.begin(), sub_edge_set.end());
}
for (const auto* edge : next_edge_set) {
const auto* to_node = edge->ToNode();
// 跳过closed集中的点
if (closed_set_.count(to_node) == 1) {
continue;
}
// 跳过不能换到到达的点
if (GetResidualS(edge, to_node) < FLAGS_min_length_for_lane_change) {
continue;
}
// 将当前节点的g值和能够达到的点的cost值相加
// GetCostToNeighbor返回相邻节点的cost和边edge的cost
tentative_g_score =
g_score_[current_node.topo_node] + GetCostToNeighbor(edge);
// 若下一点需要换道才能到达,cost会减少部分值
if (edge->Type() != TopoEdgeType::TET_FORWARD) {
tentative_g_score -=
(edge->FromNode()->Cost() + edge->ToNode()->Cost()) / 2;
}
// 计算能够达到点的f值
double f = tentative_g_score + HeuristicCost(to_node, dest_node);
if (open_set_.count(to_node) != 0 && f >= g_score_[to_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) {
continue;
}
enter_s_[to_node] = to_node_enter_s;
}
g_score_[to_node] = f;
// 初始化下一节点
SearchNode next_node(to_node);
next_node.f = f;
open_set_detail.push(next_node);
came_from_[to_node] = from_node;
// 将能够达到的点加入open集
if (open_set_.count(to_node) == 0) {
open_set_.insert(to_node);
}
}
}
AERROR << "Failed to find goal lane with id: " << dest_node->LaneId();
return false;
}
ルーティングモジュールの全体構造
概要: ルーティング モジュールは、まず高精度マップの元の情報を読み取り、TopoCreator
作成された点とエッジを使用してトポロジ マップを生成します。RoutingRequest
メッセージ (一連のポイント)を読み取った後、Navigator
クラスはトポロジ マップを使用し、TopoRangeManager
AStar アルゴリズムを使用してトポロジ マップの開始点を接続する最適なパスを検索しRoutingResponse
、それを出力として送信します。
参考
[1] apollo が導入したルーティング モジュール (6) https://zhuanlan.zhihu.com/p/65533164
[2] Apollo Spark プロジェクトの学習ノート―第 7 回自動運転計画技術の原理 1 https://blog.csdn .net/sinat_52032317/article/details/128300053#t6
[3] Apollo ナビゲーションモジュール記録(ルーティングモジュール)https://zhuanlan.zhihu.com/p/459954010
[4] Cyber RT 基礎入門と実践https:/ /apollo .baidu.com/community/article/1093
[5] 入門者必見丨Baidu Apollo のルーティング モジュールの分析https://mp.weixin.qq.com/s?__biz=MzI1NjkxOTMyNQ== &mid=2247490369&idx=1&sn=3a3f1dafc46782da311a2fc910e609 5a&scene= 21#wechat_redirect
[6] Baidu Apollo のルーティング モジュールの分析https://paul.pub/apollo-routing/