任务5.4 地图API和全局导航算法-学生手册

任务5.4地图API和全局导航算法学生手册任务5.4地图API和全局导航算法【任务导入】高精地图（HDMap，HighDefinitionMap）是专门为智能驾驶服务的地图。由于其要素丰富、精度较高，并解决部分传感器的性能边界问题，是智能驾驶车上最可靠的传感器。如何使用高精地图API，实现车道级的导航方案？【学习目标】素质目标理解高精地图的基本概念，激发学生的学习兴趣；理解导航算法的算法逻辑，培养学生细致严谨、全面分析问题的能力；培养学生精益求精、爱岗敬业的大国工匠精神。知识目标能理解高精地图的基本概念[K76]；能描述高精地图API的地图要素[K77]；能列举导航算法的步骤[K78]。能力目标能正确排列高精地图API的地图元素[A63]；能举例讲解导航算法的算法逻辑[A64]。

【知识准备】 高精地图的基本概念坐标系在高精地图中常采用的坐标系有两种：WGS84坐标系和UTM坐标系。WGS84坐标系全球定位系统使用WGS84坐标，是一种地心坐标系，以地球质心为坐标原点，采用一个十分近似、于地球自然形状的参考椭球作为描述和推算地面点位置和相互关系的基准面。三维坐标系：经度、纬度、高度。例如，东莞松山湖溪流背坡村某坐标：Lon:113.87686315°Lat:22.88044717°Altitude：12.33m。UTM坐标系UTM坐标系全称：UniversalTransverseMercatorGridSystem，一种平面直角坐标系，像橘子一样将地球分成60个区域，展开这些区域中的每个区域，通过横轴Mercator（UTM）投影。三维坐标系：x，y，z，如图5-4-1所示。例如，东莞松山湖溪流背坡村UTM坐标：zone49N794490，2533002。图5-4-1UTM坐标系格式规范常见的高精地图格式规范有两种：NDS格式规范和Opendrive格式规范。NDS格式规范NDS格式规范：NavigationDataStandardAssociation，是面向汽车生态系统的地图数据全球标准。其具有特点：数据库做了细分，细分后的产品能够独立升级：例如采用了A公司的导航算法，B公司的POI数据，让二者独立升级后依然可以正常的运行，如图5-4-2所示；定义了level的概念：类似于地图比例尺的概念，通过level将地图数据瓦片化。图5-4-2NDS格式数据库产品独立升级Opendrive格式规范Opendrive格式规范是一种开放的文件格式，用于路网结构的逻辑描述，高精地图的制作常用opendrive格式；使用xml语言描述，定义了Road、Junction、station等数据结构，如图5-4-3所示。图5-4-2NDS格式数据库产品独立升级高精地图API地图要素地图要素指的是需要地图提供功能属性的一种类型的实体，例如一段车道、一个交通灯。一个交通高精地图的要素种类通常包括：车道、道路、红绿灯、导向箭头、人行横道、停止线、路口、导流线、交通标线、功能性杆、限速标牌、路测设施、收费站、其它障碍物等。每一种特征要素都有共同点：有位置坐标、外形（点类、曲线类、多边形类）；高精地图各要素间关系主要包含：包含关系、并列关系、关联关系，如图5-4-3所示。图5-4-3高精地图各要素间关系地图要素查询方式用户在使用高精地图时，需要使用尽量少的约束条件，将地图的各要素准确的对外部进行呈现，如图5-4-4所示。图5-4-4API呈现的高精地图要素高精地图API的查询方式通常包含以下几种方式：通过ID查询要素：地图上所有要素给唯一的ID，已知ID查询对应的要素；通过位置查询要素：通过位置坐标查询对应的要素。例如：通过位置查询道路、车道等；混合条件查询：例如位置、范围查询车道；关联查询：相互关联的要素进行查询。例如：车道所关联的红绿灯；要素属性查询：例如：查询id为XX的车道宽度。高精地图API的查询结果通常包含以下几种类型：Road：一小段道路，对应一个ID，长度、左右边界，包含前后连接关系，及关联的其它要素：如车道、红绿灯、人行横道等；Lane：一段车道，车道在横向排开对应road。对应一个ID，长度、行驶参考线、左右边界范围及线型（实线、虚线、白色、黄色），限速信息、左邻、右邻、前序、后续，转向信息，以及关联的其它要素：如道路、红绿灯、人行横道；TrafficLight：一个红绿灯。对应一个ID，灯的位置坐标，以及关联的车道、停止线；Crosswalk：一个人行横道。对应一个ID，对应的多边形区域，关联的停止线。要素属性以车道要素属性为例，关于车道的要素属性见表5-4-1所示。表5-4-1车道要素属性序号名称说明1Idlane的id2Predecessor_idlane的起点位置id，与上一段lane终点相同。3successor_idlane的终点位置id，与下一段lane起点相同。4type常规车道、应急车道、公交专用车道、BRT车道等。5left_boundary_id车道左侧边界线id6Right_boundary_id车道右侧边界线id7road_id关联road的id8virtual是否为虚拟车道9speed_limit车道限速10length车道长度11width车道的宽度12sequence车道序列数，第几车道13lane_dir车道转向，直行、左转、调头等14Vehicle_type车道控制车辆的类型，如公交专用道。15Section_id关联路口的id16Section_in_id进入路口的id17Section_out_id离开路口的id如某一车道要素属性，Lane:id:2predecessor_id:nullsucessor_id:5type:HIGH_WAYspeed_limit:60road_id:1可知，该车道为车道2，无前序车道，后续车道为车道5，类型为高速路，限速为60km/h，所对应的road的id为1，如图5-4-5所示。图5-4-5车道要素属性导航算法介绍导航引擎在导航时需要输入以下条件，如图5-4-6所示：导航的起点：由车辆的位置决定，精确到车道；导航的目的的：用户选择要到达的终点；导航途径点：用户添加一个或多个途径点；导航偏好：用户选择导航偏好，如：走高速、不调头等。算法执行以下步骤：①起点、途经点、终点关联到对应的lane；②由导航偏好决定所要选择的导航配置；③导航算法建立一条连接起点、途径点、终点的行驶路径。图5-4-6高精地图输入条件导航算法-Dijkstra算法Dijkstra算法是典型的最短路径算法，计算一个节点到其它各个节点的最短路径。它的主要特点是以起始点为中心无方向性的向外层层扩展，直到扩展到终点为止，如图5-4-7所示。图5-4-7Dijkstra算法算法步骤①指定起始点s；②引进集合S和U。S记录已遍历最短路径的顶点。U记录还没有遍历到的路径的顶点；③从U中找出最短路径点，加入到S中，并更新顶点距离值；④重复步骤③，直到U中所有点都加入到了S中。算法示例以一个Dijkstra算法为例，以D为起点搜索到达各点的最短路径，如图5-4-8所示。图5-4-8算法示例①选取起点D，计算D点到各点的距离，已经到达的顶点为D，未到达的顶点为A、B、C、E、F、G，且与起点D连通的点有C、E点，如图5-4-9所示。S={D(0)}U={A(∞)，B(∞)，C(3)，E(4)，F(∞)，G(∞)}图5-4-9步骤一②选取U中最小值对应的点，即C点，并计算与C连通的B、F、E点的距离，如图5-4-10所示。注意：到达E点的途径有DEC和DE两种，算法选取距离最短的路径写入。S={D(0)，C(3)}U={A(∞)，B(13)，E(4)，F(9)，G(∞)}图5-4-10步骤二③选取U中最小值对应的点，即E点，并计算与E连通的F、G点的距离，此处EF的距离小于DCF的距离，更新对应的集合，如图5-4-11所示。S={D(0)，C(3)，E(4)}U={A(∞)，B(13)，F(6)，G(∞)}图5-4-11步骤三④重复以上步骤，选取点F，如图5-4-12所示。S={D(0)，C(3)，E(4)，F(6)}U={A(22)，B(13)，G(12)}图5-4-12步骤四⑤重复以上步骤，选取点G，如图5-4-13所示。S={D(0)，C(3)，E(4)，F(6)，G(12)}U={A(22)，B(13)}图5-4-12步骤五⑥重复以上步骤，选取点B，如图5-4-13所示。S={D(0)，C(3)，E(4)，F(6)，G(12)，B(13)}U={A(22)}图5-4-13步骤六⑦重复以上步骤，选取点A，最终计算出D点到所有点的最短距离。S={D(0)，C(3)，E(4)，F(6)，G(12)，B(13)，A(22)}导航算法-A*算法A*算法是在启发函数下，由起点向外扩散，直到扩散到终点；启发函数：f=g+h，g为起点到当前位置的代价。h为当前位置到终点的预估代价。A*特点为层层向外扩散，且优先向终点扩散，如图5-4-14所示。图5-4-14A*算法算法步骤①初始化open_set（待检查节点）和close_set（已经遍历过的节点）；②将起点加入open_set中，并设置优先级为0（优先级最高）；③从open_set中选取优先级最高的节点n（h值最小），加入到close_set中。并将n的所有邻居加入到open_set中，加入时标记其父节点为n（达到终点时回溯）。更新其代价；④重复步骤③，直到到达终点后进行回溯。算法示例以一个A*算法为例，以绿色格子为起点，红色为终点，黑色为不可到达的区域，如图5-4-15所示。图5-4-15算法示例①起点向外扩散，水平、竖直方向移动代价为10，斜向移动代价14，从起点向外扩散。扩散时标记其父节点，如图5-4-16所示。图5-4-16步骤一②每次从代价最新的点开始扩散，扩散出来的新位置加入到open_set中，已经遍历过的点从open_set中删除加入到close_set中，如图5-4-17所示。图5-4-17步骤二③到达终点后，由终点向后回溯，到达，形成导航路径，如图5-4-18所示。图5-4-18步骤三导航算法建模智能驾驶导航算法进行车道级别的导航时，遵循以下步骤建模，如图5-4-19所示：车道要素标记车道每条车道作为一个node，每个node的要素包括：ID、length（长度）、cost（本车道代价）、roadID。车道之间的连接关系相邻车道间双向连接，前后直接单向连接。连接关系用edge表示，Edge要素包括：From（上游的连接关系）、to（下游的连接关系）、cost（连接代价）、direction（方向）；cost函数构建cost函数需要满足执行代价最小，分别是：直行<左变道<右变道<右转<左转<调头，这样的代价函数才符合人类的驾驶习惯。图5-4-19导航算法建模启发函数启发函数保证朝向终点的选择代价最小，启发函数：f(n)=g(n)+h(n)，n为当前待考察的节点位置，如图5-4-20所示。选择合适的启发函数，可以使得导航计算出来的路径最优：符合人类的驾驶习惯。f(n)是节点n的总代价，在遍历节点n时，需保证n为当前open_set中代价最小的；g(n)是起点到当前节点的代价，包含节点本身的cost和上游连接的cost；h(n)是节点n到终点的预估代价。预估时可用当前节点到终点的直线距离除以默认速度。h可以保证指向终点的导航cost最小。图5-4-20启发函数黑名单机制导航时遇到某路段交通拥堵或正在施工，需要避开这些特定路段。可以将这些路段加入到黑名单中。导航算法在搜索路径时，遇到黑名单上的lane时，则选择跳过，如图5-4-21所示。图5-4-21黑名单中的不可行驶车道具体的实施方案：将黑名单对应lane的cost置为无穷大，图5-4-22所示。图5-4-22黑名单cost不将黑名单对应lane放入到open_set中，图5-4-23所示。图5-4-23黑名单判断流程分段导航用户设置了多个途径点时，需要进行分段导航。从起点开始导航到第一个途径点，途径点间进行导航，直到最后一个途径点，最后一个途径点到终点进行导航，最后用户对导航结果进行确认。（分段导航默认用户在导航过程中按分段顺序输入途径点），如图5-4-24所示。图5-4-24分段导航

【任务实施】Dijkstra路径规划导航概述本实训任务将实现Dijkstra导航算法，算法项目中包括地图信息的ros包，并可以通过Rviz可视化该算法的实现效果。实验环境准备设备检查本次实验环境包括电源线，安装Ubuntu18.04虚拟环境的PC机一台，见表5-4-2。每套实验环境适用于4~12名学生同时上机操作。表5-4-2实验清单设备名称数量备注电源线1组所有实验组共用笔记本或台式机每组1台台式机要有无线网卡项目描