自驾总结_Routing

综述：

根据规划请求完成P2P的车道级导航，并且实现路由切片与车道中心参考线下发，能够完成L2~L4自动驾驶场景下的导航规划工作，支持3D场景下的导航，能够支持感知与高精度地图元素的融合。

总体结构与功能：

Routing全局路径规划，即全局导航模块，（假设保留HDmap，当然现在更多是SDmap，或者点云地图--->OpenDrive地图）

1. 请求检测与初始化 (A→B→C)

   • 主函数循环检测新规划请求

   • 解析请求中的起点、终点和途经点

2. 坐标转换与车道匹配 (C→D→E)

   • 将经纬度坐标转换为UTM坐标系统

   • 根据坐标和朝向角计算加权距离

   • 匹配到最近的车道中心线

3. 路由规划 (E→F→G)

   • 在车道网络有向图中构建路由

   • 使用路径搜索算法（如A*或Dijkstra）

   • 求解代价最小的最优路线

4. 驾驶辅助功能 (G→H→I 和 G→O)

   • 查询路线经过的路口信息

   • 生成路口提醒数据

   • 创建终点停车信息（位置和姿态）

5. 车道级路径处理 (G→J→K→L→M→N)

   • 查找可换道车道（左右相邻关系）

   • 递归划分可选道路

   • 将道路划分为不可换道的路径段

   • 按左右关系排序路径

   • 加载停止线和绑定红绿灯信息

6. 实时路径优化 (N+O→P→Q→R)

   • 结合车辆实时位置进行路径切片

   • 定位离车身最近点

   • 前后搜索保留附近区域（如前后各100米）

7. 参考线生成 (R→S→T→U)

   • 提取车道中心线作为参考线

   • 计算左右车道宽度

   • 获取车道限速信息

   • 整合所有数据生成规划结果

8. 结果输出 (U→V)

   • 发布包含以下信息的规划结果：

     • 参考线点序列

     • 车道宽度和限速

     • 路口提醒

     • 红绿灯信息

     • 停车点信息

总结：坐标转换（输入：UTM坐标起点，终点，途径点，处理：有向图搜索（Dijistra，A*）输出：车道ID序列最优路线）-->车道匹配-->路径规划-->车道扩展（输入：最优路线车道序列，处理：递归查找左右相邻车道，输出：排序后可选路径集合）-->路径切片（输入：完整路径集合+车辆实时位置，处理：最近点搜索+窗口截取 输出：局部区域路径段）-->参考线生成（输入：切片后车道ID序列，处理：车道属性+几何计算，输出：带属性参考线点列）

路径规划

有向图概念：

1)构建有向图:以lanelet作为基本单位，转化为图G的一条有向边两条边之间添加一个顶点。
2)计算有向边权值：综合考虑lanelet的属性（如限速情况、红绿灯时长、路径长度、以及车道宽度等等因素），估算出一个表示车辆通过该lanelet所需的时长，并把该时长作为lanelet对应的边的权值。每个lanelet属性用a1表示，使用F(a1,a2,a3...,an)权值估计函数为每个lanelet计算一个唯一权值w。此外，实时交通信息表如果显示某条lanelet不通，我们可以将其设置为不连通。
比如 w = t = t1（lanlet内红灯时间）+ s/min(v,vl) （v代表平均车速，vl表示限速的速度）

Dijisktra算法：

[最短路径问题]Dijkstra算法(含还原具体路径) - MarisaMagic - 博客园

路由图构建：

1. 若有双向可通行的lanelet，把该lanelet的反转（inverse）加入待选lanelet列表里。
2. 将lanelet列表里的所有lanelet作为顶点加入图中，并把lanelet的起始和末尾点加入索引。
3. 把area作为顶点加入图中。
4. 对于每一个lanelet，进行以下操作：A） 查找后继lanelet，对所有的后继节点和本节点之间，都构建一条边，权值为本lanelet的周长/限速=travel time；B）查找左右侧车道，并加入，能换道则加入换道候选（换道产生的代价在后续处理），cost置1；C）查找有重合的车道，并加入，cost置1；D）对换道的代价进行计算；
5. 开始构建图的边（lanelets和areas）,area的构建比较简单，和lanelet的后继权值计算方法类似，也是拿周长算一个travel time 。

cost计算

计算换道的代价，声明，这里的代价是在本车道换道后，对于本车道的前后车道带来的代价变化，本车道A,左车道B，前车道C,后车道D，可以理解为原来C->A->D的代价分别是C的travel time，A的travel time ，D的travel time 。（权值为本lanelet的周长/限速=travel time）换道的代价就是C->A->B->D。分别为C的travel time ，(A的travel time+B的travel time)/2,D的travel time。

切片算法：

1. 根据定位信息定位到本车道，搜索以本车道为中心的一定区域内车道集合，并搜索本车道邻近车道。
2. 判断当前定位的车道是否能在规划的路由中匹配到（即本车道或者邻近车道是否在规划的路由中），不能则进行重规划，能则进行步骤4。
3. 判断当前定位车道到终点车道是否可以路由，不能则重规划失败，发出规划失败的状态，可以路由则更新最优规划路径。
4. 根据设定的路由长度前后找到在范围内的切片路段。
5. 把需要切片的路段扩充为左右可走的全路由，发出规划成功状态。

局部拓扑算法：

1. 根据定位信息定位到本车道，搜索以本车道为中心的一定区域内车道集合。
2. 本车道的pose值置为0，并搜索四周车道。
3. 循环以下步骤，直到四个方向搜索结果都被遍历过或者不在集合内：A)左边车道pose值为本车道pose值减一；B)右边车道pose值为本车道pose值加一；C)前后车道pose值与本车道相同；D)将这些车道分别置为本车道进入步骤2进行递归计算；
4. 搜索结束，输出车道id和pose的对应关系。

中心线算法：

地图元素响应：

路口信息：根据最短路径找到车道，去查询其绑定的路口信息，找到本次规划所有经过的路口信息，用于路口提醒功能。

红绿灯信息：对于划分好的路径，加载路径里的停止线和对应的红绿灯信息，生成与终点对应的停车信息。

正常停车逻辑：在路由规划中会将终点匹配到车道中心线上，然后车辆行驶时，实时匹配到终点的距离，同时根据车当前速度和平常的刹车减速度去计算一个停车距离，并进行判断，是否下发停车点。

红绿灯停车逻辑：在加载地图构建最短路径的过程中，将所有该路径上经过的红绿灯以及停止线绑定到相应车道，车辆行驶时，实时通过车身定位来将车定位到车道上，再通过接收到的红绿灯感知信息，进行融合后，判断是否下发停止线以及红绿灯的颜色，给到下游模块。