当前位置：首页 > news >正文

自驾总结_Routing

news 来源：原创 2025/6/6 19:40:31

综述：

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

总体结构与功能：

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

请求检测与初始化 (A→B→C)
- 主函数循环检测新规划请求
- 解析请求中的起点、终点和途经点
坐标转换与车道匹配 (C→D→E)
- 将经纬度坐标转换为UTM坐标系统
- 根据坐标和朝向角计算加权距离
- 匹配到最近的车道中心线
路由规划 (E→F→G)
- 在车道网络有向图中构建路由
- 使用路径搜索算法（如A*或Dijkstra）
- 求解代价最小的最优路线
驾驶辅助功能 (G→H→I 和 G→O)
- 查询路线经过的路口信息
- 生成路口提醒数据
- 创建终点停车信息（位置和姿态）
车道级路径处理 (G→J→K→L→M→N)
- 查找可换道车道（左右相邻关系）
- 递归划分可选道路
- 将道路划分为不可换道的路径段
- 按左右关系排序路径
- 加载停止线和绑定红绿灯信息
实时路径优化 (N+O→P→Q→R)
- 结合车辆实时位置进行路径切片
- 定位离车身最近点
- 前后搜索保留附近区域（如前后各100米）
参考线生成 (R→S→T→U)
- 提取车道中心线作为参考线
- 计算左右车道宽度
- 获取车道限速信息
- 整合所有数据生成规划结果
结果输出 (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 - 博客园

路由图构建：

若有双向可通行的lanelet，把该lanelet的反转（inverse）加入待选lanelet列表里。
将lanelet列表里的所有lanelet作为顶点加入图中，并把lanelet的起始和末尾点加入索引。
把area作为顶点加入图中。
对于每一个lanelet，进行以下操作：A）查找后继lanelet，对所有的后继节点和本节点之间，都构建一条边，权值为本lanelet的周长/限速=travel time；B）查找左右侧车道，并加入，能换道则加入换道候选（换道产生的代价在后续处理），cost置1；C）查找有重合的车道，并加入，cost置1；D）对换道的代价进行计算；
开始构建图的边（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。

切片算法：

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

局部拓扑算法：

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