网络化自动驾驶-轨迹规划算法设计说明书-V1

1、网络化自动驾驶轨迹规划算法设计说明书作者胡学敏评审人日期2015/12/26环宇智行科技有限公司华为技术有限公司修订记录作者修订时间版本号修改内容胡学敏2015/12/29V1.11. 更新三种算法的原理图和相关实现2. 更新2.3的接口部分。胡学敏2016/1/3V1.31. 更新2.1的轨迹规划流程。2. 调整部分格式目录1 概述 32 框架和接口 32.1 轨迹规划流程 322算法选择模块 42.3 接口 42.4 算法介绍 43 算法1:虚拟车道算法 53.1 算法原理 53.2 算法实现 54 算法2:曲线轨迹簇算法 54.1 算法原理 54.2 算法实现 61) 基本路线的构建 6

2、2) 候选路径的生成 63) 路径的选择 75 算法3: A*算法 85.1 算法原理 85.2 算法实现 81概述图1.1整体路径规划过程示意图轨迹规划是实现智能车自主驾驶的关键模块。要实现对自动驾驶车辆的控制前提就是根据车辆的当前道路环境，行驶状态以及整体交通环境规划出一条合理的轨迹，以便于下层控制器进行车辆控制。整个轨迹规划从CSU的整体路网规划（考虑整体车流和交通状况）到RSU的局部交通流规划（平衡局部车流），再到OBU的具体行驶轨迹规划。以下着重讨论OBU中的规划算法。2框架和接口2.1轨迹规划流程感知融合&网络规划网络规划轨迹图2.1.1 OBU轨迹规划轨迹规划流程：1. OBU,

3、用户在UI选择目的地，OBUtRSU, RSU宀CSU, CSU做“全程车道级路径规戈”输出结果为“车道编号集合、task参数”，保存在CSU的该OBU_session上，然后CSU trsu：下发路径规划结果，RSU保存在该 OBU_session里。RSU tOBU:下发路径规划结果，保存在内存。2. OBU将感知融合的数据和网络路径规划的结果投影到当前道路环境中，调整当前OBU规划的轨迹，并显示在 UI上。OBU轨迹规划数据源：地图数据（包括道路边界点，道路拓扑结构，车道线信息），感知融合数据（车线，障碍物信息等）。OBU轨迹规划频率与规划距离：OBU规划模块每1秒规划一次，每次规划 1

4、00米。2.2算法选择模块OBU规划模块创建算法选择类，OBU直接调用该类的接口获得轨迹，算法选择类支持注册多种算法，并选择一种算法进行轨迹规划。2.3 接口OBU调用算法选择模块的接口，包括：当前车道与相邻车道构成的4条线（当前车道与左右两条车道，一共3条车道，需要4条车道线来标记）。障碍物、障碍物形状、障碍物速度。路径规划参数。2.4算法介绍OBU中轨迹规划主要包括以下三种算法1）虚拟车道算法：总是沿着道路的中线行驶，发现障碍物采用跟驰（/刹车）或换道，换道行为相当于虚拟了一个走到另一个车道的虚拟车道，然后继续沿中线行驶。2）曲线轨迹簇算法：以车的方向和道路的曲率构建曲线坐标系，然后在此坐

5、标系内拟合曲线轨迹簇，选择最佳曲线，然后转化为笛卡尔坐标系。3）A*算法：把待规划区域的地图划分成笛卡尔坐标系的栅格（如边长20cm ）,把障碍物定义为1的栅格，可行驶的为0的栅格，按目标做最短路径搜索， 再把搜索结 果做平滑处理。算法选择：虚拟车道法简单可靠，且曲线平滑，舒适性好，可作为默认算法。但是此算 法简化为跟驰和换道两个动作，应对不同车姿（曲线坐标轨迹簇算法的优势）和发现最佳路径（A*算法的优势）的能力很弱，路况复杂时容易找不到出路。此时应降级为“曲线坐标 轨迹簇算法”算法，此算法发现道路的能力较强，曲线也很平滑。如果此算法也得不到结果，则降级为“ A*算法”，实现“有空就钻”的路径

6、搜索（但行驶曲线不够平滑），仍然没有出路时，停车（通常为完全堵车了）。3算法1虚拟车道算法3.1算法原理总是沿着道路的中线行驶，发现障碍物采用跟驰（/刹车）或换道，换道行为相当于虚拟了一个走到另一个车道的虚拟车道，然后继续沿中线行驶。3.2算法实现1） OBU通过车道线检测实时确定当前车道线的位置，同时OBU加载地图中的车道 模型（车道宽度、车道数目、特征点）。2）OBU将当前检测车道与车道模型进行匹配，确定当前车道和邻近几条车道。3）车辆总是沿着车道中心线行驶，当车辆需要换道时，根据道路环境，虚拟出一条 与相邻车道的交汇车道，完成变道动作。4算法2:曲线轨迹簇算法4.1算法原理以车的方向和道

7、路的曲率构建曲线坐标系，然后在此坐标系内拟合曲线轨迹簇，选择最佳曲线，然后转化为笛卡尔坐标系。图4.1.1轨迹簇算法4.2算法实现算法实现的流程如图 4所示:图421曲线轨迹簇算法流程图1）基本路线的构建基本路线是由包含整体路径规划的道路中心点所组成的航迹点的样条拟合构建的。参数化曲线经常被用来建立道路的集合模型并且定义运动路径。采用参数化三次样条来定义基于航迹点构建的基本路线的参数化曲线。2）候选路径的生成行驶路线应该能够引导车辆随着全局路线行走并且避开障碍物。为了达到这个目的， 建立了一个基本路线的曲线坐标系统。在基本路线的曲线坐标系统中设计避免障碍物的光滑策 略。图5显示了在笛卡尔坐标系

8、统中的路径和基本路径的曲线坐标系统之间的几何关系。图中代表在整体路径中移动距离的基本路径的弧线长度s成为了曲线坐标系统的水平坐标,最近的偏移图422笛卡尔坐标与曲线坐标的几何关系为了生成路径，路径的曲率是由基于基本路径的曲率路径的侧向偏移所决定的。在计算路径曲率时需要侧向偏移的导数和二阶导数，因此侧向偏移函数要有一定的光滑性侧向偏移函数描述如下J 昭十2A用十c.其中 ? 0。4、 h(n)=h*(n)(h*(n)为实际问题的代价值)。当此四个条件都满足时，一个具有f(n )=g( n)+h( n)策略的启发式算法能成为A*算法，并一定能找到最优解。对于一个搜索问题，显然，条件 1,2,3都是

9、很容易满足的，而条件 4： h(n)V5的路径，V0-V5的过程中，可以经由 V1, V2, V3，V4各点达到目的点 V5。 下面的问题，即是求此 起始顶点V0-途径任意顶点 V1、V2、V3、V4-目标顶点V5的最短 路径。1OPESCLOSE隔 m, 3d)(V5, iqoOPEN 浮芥却：(n6Q)CUB,100)CLOSE (TO.DH fOJOPEKCLOSE L n w 30?佩 6W 件区 00)OFEJiCLOSE图5.2.1 A*算法求解示例通过上图，我们可以看出：A*算法最为核心的过程，就在每次选择下一个当前搜索点时，是从所有已探知的但未搜索过点中（可能是不同层，亦可不在同一条支路