自动驾驶（八十）---------Apollo之导航模块

没想到自动驾驶系列文章能写到80篇，因为我之前做事很浮躁，总是蜻蜓点水、走马观花，所以能写这么多我都诧异了，仔细一想支持我写到现在的原因有两点:分享的快乐、成长的乐趣。

说到成长，很多以前似懂非懂的技术问题，都在我整理资料中梳理清楚，也迫使自己接触新的技术，并且这一轮操作之后，能让我站在系统的、更高的维度来看待每一个技术和自动驾驶的现状。甚至让我能更加自信的面对工作和生活中的不确定性。

再说分享的快乐，我写博客不想挣一分钱，所以博客永远是免费分享给大家，现实中我有不错的薪水，在上海也能很好过自己的生活，已经很感恩这一切了，不需要在这来精打细算。如果博客能帮助到，同样对自动驾驶感兴趣的你，那才是对我最大的褒奖。

言归正传，说apollo的导航(Routing)模块，Routing模块给自动驾驶系统提供全局的路径导航功能，类似于百度地图/高德地图的导航。

1. Routing的概念

在收到路由请求之后，为了完成导航功能，Routing模块使用了专用的路由地图routing_map。最终输出的是自动驾驶车辆在从出发点到目的地的过程中经过的所有路段。

首先，我需要有地图，这里的地图需要的是一个拓扑结构的图，需要有车道级别的连接信息，百度高精地图就是做这个工作。然后我们定义起始点，并通过Routing找到一条最优路径。

另外，分析Routing模块之前，我们只需要能够解决以下几个问题，就算是把routing模块掌握清楚了。

如何从A点到B点？
如何规避某些点？ - 查找的时候发现是黑名单里的节点，则选择跳过
如何途径某些点？- 采用分段的形式，逐段导航（改进版的算法是不给定点的顺序，自动规划最优的线路）
如何设置固定线路，而且不会变？最后routing输出的结果是什么？

2. Routing流程

1. 加载routing_map和起始点的位置，并找到起始点最近的lian这步没什么好解释的，相信都能理解。

2. 这一步需要考虑黑名单的问题，即在全局地图中，有一些车道有一段路不能走，或者有些点不能走，一般的考虑方式是把这些点和线段删掉，重新规划全局规划，也可以按照apollo的方式把这段路切开，分成几个subline，突然遇到不能走的地方，在这一段按照subline重新规划这一条段，一般都是在黑名单一定区域内重新导航，这样就不用全局重新规划了。参考这一段apollo代码：

SubTopoGraph::SubTopoGraph(const std::unordered_map<const TopoNode*, std::vector<NodeSRange>>&black_map) {std::vector<NodeSRange> valid_range;for (const auto& map_iter : black_map) {valid_range.clear();GetSortedValidRange(map_iter.first, map_iter.second, &valid_range);// 生成子节点InitSubNodeByValidRange(map_iter.first, valid_range);}for (const auto& map_iter : black_map) {// 生成子边InitSubEdge(map_iter.first);}for (const auto& map_iter : black_map) {AddPotentialEdge(map_iter.first);}
}

3. Astar算法搜索路径。A*算法之前有文章专门介绍，传送，这里就不介绍算法的思想了，主要从代码的角度看一看apollo的实现过程：

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.";//优先级队列openlist,用priority_queue实现.std::priority_queue是一种容器适配器，//它提供常数时间的最大元素查找功能，亦即其栈顶元素top永远输出队列中的最大元素。//但SearchNode内部重载了<运算符，对小于操作作了相反的定义//因此std::priority_queue<SearchNode>的栈顶元素永远输出队列中的最小元素。std::priority_queue<SearchNode> open_set_detail;//将起点设置为检查节点SearchNode src_search_node(src_node);//起点的f值src_search_node.f = HeuristicCost(src_node, dest_node);//把起点节点存入openliopen_set_detail.push(src_search_node);//把起点加入到open_setopen_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;//进入算法主循环,只要openlist不为空,就一直循环查找while (!open_set_detail.empty()) {current_node = open_set_detail.top();const auto* from_node = current_node.topo_node;if (current_node.topo_node == dest_node) {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();//判断当前节点是否被检查过if (closed_set_.count(from_node) != 0) {// if showed before, just skip...continue;}//当前点加入openlistclosed_set_.emplace(from_node);// if residual_s is less than FLAGS_min_length_for_lane_change, only move// forward//先通过判断用不用变道,再获取当前节点的所有相邻边//GetResidualS 为当前节点到终点的剩余距离s//若s<min_length则不变道,若s > min_length 则变道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();//从上面相邻边neighbor_edges中获取其内部包含的边,将所有相邻边全部加入集合:next_edge_setfor (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();//判断是不是已经在closelist中if (closed_set_.count(to_node) == 1) {continue;}//若当前边到相邻节点的距离小于min_length,则不能通过变道到达相邻节点,就直接忽略if (GetResidualS(edge, to_node) < FLAGS_min_length_for_lane_change) {continue;}//更新当前节点的移动代价gtentative_g_score =g_score_[current_node.topo_node] + GetCostToNeighbor(edge);//如果边的类型不是向前而是向左向右,表示变道,需要更新移动代价g的计算方式if (edge->Type() != TopoEdgeType::TET_FORWARD) {tentative_g_score -=(edge->FromNode()->Cost() + edge->ToNode()->Cost()) / 2;}//总代价f = g + hdouble 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//如果是以向前的方向到达相邻节点,则更新节点的进入距离enter_sif (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 lengthto_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_nodeif (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;//把下一个待检查节点加入到openlistopen_set_detail.push(next_node);//设置当前节点的父节点came_from_[to_node] = from_node;//若相邻及节点不在openlist中,则将其加入到open_set中,便于后续考察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;
}

总结，导航在apollo中不是特别重要的一环，但是百度地图在导航领域有很深的积累，难得apollo开源，我们才可以有幸看到这部分代码，希望大家能珍惜~~

自动驾驶（八十）---------Apollo之导航模块相关推荐

自动驾驶：2022 apollo day 观后感（二）
自动驾驶:2022 apollo day 观后感(二) TOPIC TWO:自动驾驶地图:Apollo的实践和思考(黄际洲) 地图需求升级自动驾驶规划方向轻成本,重体验轻成本重体验安全: 舒 ...
智行者王肖：自动驾驶本质在于对待数据的态度、获取及应用方式 | 自动驾驶这十年...
雷锋网(公众号:雷锋网)按:回头看,第一届DARPA自动驾驶挑战赛已经是十年前的事了.对比互联网产业,十年前手机还没有支付业务,而现在支付宝.微信已经全面改变了人们的消费习惯.反观汽车产业,尤其是自动 ...
智行者王肖：自动驾驶本质在于对待数据的态度、获取及应用方式 | 自动驾驶这十年... 1
雷锋网(公众号:雷锋网)按:回头看,第一届DARPA自动驾驶挑战赛已经是十年前的事了.对比互联网产业,十年前手机还没有支付业务,而现在支付宝.微信已经全面改变了人们的消费习惯.反观汽车产业,尤其是自动 ...
李彦宏亲测“自动驾驶汽车”，Apollo（阿波罗）坐镇
今天百度AI开发者大会又掀起一个小高潮.百度董事会副主席.集团总裁兼首席运营官现场连线正赶来主题演讲的百度创始人.董事长兼首席执行官李彦宏,李彦宏正坐在一辆自动驾驶汽车的副驾,主驾好似坐了一位&quo ...
吴甘沙创业照见自动驾驶十年变迁，产业爆发在下一个春天里 | 自动驾驶这十年...
个人创业照见自动驾驶十年变迁. 效力英特尔16年.三发创业之心最终"出走"人工智能,吴甘沙的创业故事已经被说得太多.而他所领衔创立的驭势科技,继2017年完成国内首个自动驾驶商业运 ...
自动驾驶学习过程（Apollo）
文章目录前言一.自动驾驶基础概念 1. 自动驾驶等级 2. 无人驾驶车的核心 3. 自动驾驶的研发流程 4. 自动驾驶汽车的硬件系统 4.1 核心传感器: 4.2 传感器单元(Sensor box ...
基于Unity的自动驾驶模拟总结(Apollo)
什么是Apollo平台阿波罗是百度的全栈开放开源的自动驾驶软件平台. 上图是Apollo 3.5版本的架构图,我们可以看到阿波罗是从下往上的全栈式的平台,从硬件到各种感知器,到我们自主开发的Cybe ...
自动驾驶：2022 apollo day 观后感（一）
2022 apollo day 观后感(一) 注: ppt来自apollo day,结合ppt,讲述一些自己的想法,欢迎批评指正! Topic One -- 打造安全.智能.高效的自动驾驶技术体系(陈 ...
眼图matlab仿真_MATLAB开发自动驾驶第二十课-自动驾驶场景设计器中的预建驾驶场景...
自动驾驶场景设计应用提供了一个代表常见驾驶动作的预建场景库.该应用还包括代表欧洲新车评估计划(Euro NCAP)测试协议的场景,以及3D仿真环境中使用的预建场景的3D版本. 选择一个预建场景要开始 ...
自动驾驶（十四）---------车道线检测
目前车道线检测主要有两种方法:传统方法,深度学习,本文主要从传统方法出发来介绍车道线检测,毕竟Mobileye也没有用到太多的深度学习(以前和Moblieye有交流). 再者,一直以来我们传统汽车行业 ...

自动驾驶（八十）---------Apollo之导航模块

自动驾驶（八十）---------Apollo之导航模块相关推荐

最新文章

热门文章