网络化自动驾驶_轨迹规划算法设计说明书_V1.3

1、.网络化自动驾驶轨迹规划算法设计说明书作者胡学敏评审人日期2015/12/26环宇智行科技有限公司华为技术有限公司.专业学习资料.修订记录作者修订时间版本号修改内容胡学敏2015/12/29V1.11.更新三种算法的原理图和相关实现2.更新 2.3的接口部分 。胡学敏2016/1/3V1.31.更新 2.1的轨迹规划流程 。2.调整部分格式目录.专业学习资料.1概述 .42框架和接口 .52.1轨迹规划流程 .52.2算法选择模块 .62.3接口 .62.4算法介绍 .63 算法 1 ：虚拟车道算法 .73.1算法原理 .73.2算法实现 .84 算法 2 ：曲线轨迹簇算法 .84.1算法原理

2、 .84.2算法实现 .91 ） 基本路线的构建 .92 ） 候选路径的生成 .93 ）路径的选择 .105算法 3 ： A* 算法 .115.1算法原理 .115.2算法实现 .12.专业学习资料.1 概述车道级路径规划CSU高精度地图车道级路径规划(转发自 CSU)RSU 局部车道调整高精度地图轨迹规划规划说明：控制控制指令1.CSU 通常只做一次车道级路径规划。车辆实时状态 OBU车辆2.RSU 根据车辆实时状况动态切换车辆行驶的高精度地图车道。图 1.1 整体路径规划过程示意图轨迹规划是实现智能车自主驾驶的关键模块。要实现对自动驾驶车辆的控制前提就是根据车辆的当前道路环境，行驶状态以及

3、整体交通环境规划出一条合理的轨迹，以便于下层控制器进行车辆控制。 整个轨迹规划从CSU 的整体路网规划（考虑整体车流和交通状况）到 RSU 的局部交通流规划（平衡局部车流 ）， 再到 OBU 的具体行驶轨迹规划。以下着重讨论 OBU 中的规划算法。.专业学习资料.2 框架和接口2.1 轨迹规划流程感知融合 &网络规划网络规划轨迹图轨迹规划轨迹规划流程：1. OBU ，用户在 UI 选择目的地 ， OBU RSU， RSU CSU， CSU 做 “全程车道级路径规划 ”，输出结果为“车道编号集合、 task参数 ”，保存在CSU 的该OBU_session上，然后 CSU RSU：下发路

4、径规划结果， RSU 保存在该OBU_session 里 。RSUOBU ：下发路径规划结果，保存在内存 。2.OBU 将感知融合的数据和网络路径规划的结果投影到当前道路环境中，调整当前OBU 规划的轨迹 ，并显示在UI 上。OBU 轨迹规划数据源：地图数据 （包括道路边界点，道路拓扑结构，车道线信息 ）， 感知融合数据（车线 ，障碍物信息等）。OBU 轨迹规划频率与规划距离： OBU 规划模块每1 秒规划一次 ，每次规划 100 米 。.专业学习资料.2.2 算法选择模块OBU 规划模块创建算法选择类， OBU 直接调用该类的接口获得轨迹，算法选择类支持注册多种算法 ，并选择一种算法进行轨迹

5、规划。2.3 接口OBU 调用算法选择模块的接口，包括：当前车道与相邻车道构成的4 条线（当前车道与左右两条车道，一共 3 条车道 ，需要 4 条车道线来标记）。障碍物 、障碍物形状 、障碍物速度 。路径规划参数 。2.4 算法介绍OBU 中轨迹规划主要包括以下三种算法1) 虚拟车道算法 ：总是沿着道路的中线行驶 ，发现障碍物采用跟驰 (/ 刹车 )或换道 ，换道行为相当于虚拟了一个走到另一个车道的虚拟车道，然后继续沿中线行驶。2) 曲线轨迹簇算法 ：以车的方向和道路的曲率构建曲线坐标系 ，然后在此坐标系内拟合曲线轨迹簇 ，选择最佳曲线 ，然后转化为笛卡尔坐标系 。3)A*算法：把待规划区域的

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

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

8、簇算法4.1 算法原理以车的方向和道路的曲率构建曲线坐标系，然后在此坐标系内拟合曲线轨迹簇，选择最佳曲线 ，然后转化为笛卡尔坐标系。图轨迹簇算法.专业学习资料.4.2 算法实现算法实现的流程如图4所示：地图载入基本路线构建候选路径生成路径选择最优路径三次样条曲线笛卡尔坐标与安全性、平滑性、连贯性代拟合曲线坐标转换价函数图曲线轨迹簇算法流程图1）基本路线的构建基本路线是由包含整体路径规划的道路中心点所组成的航迹点的样条拟合构建的。参数化曲线经常被用来建立道路的集合模型并且定义运动路径。采用参数化三次样条来定义基于航迹点构建的基本路线的参数化曲线。2）候选路径的生成行驶路线应该能够引导车辆随着全局

9、路线行走并且避开障碍物。为了达到这个目的，建立了一个基本路线的曲线坐标系统。在基本路线的曲线坐标系统中设计避免障碍物的光滑策略 。 图 5 显示了在笛卡尔坐标系统中的路径和基本路径的曲线坐标系统之间的几何关系。该图中代表在整体路径中移动距离的基本路径的弧线长度s 成为了曲线坐标系统的水平坐标 ，最近的偏移q 成为了垂直坐标。.专业学习资料.图笛卡尔坐标与曲线坐标的几何关系为了生成路径，路径的曲率是由基于基本路径的曲率路径的侧向偏移所决定的。在计算路径曲率时需要侧向偏移的导数和二阶导数，因此侧向偏移函数要有一定的光滑性侧向偏移函数描述如下其中弧长 si 和当前位置的侧向偏移qi 由定位步骤给出。

10、 为当前位置车头和基本路径的夹角。 采用边界条件，我们可以得到三次多项式的系数。路径规划算法生成了一些候选路径 。 每个路径有一个明显侧向偏移qf. 覆盖道路的所有候选路径只是在侧向偏移上有些许不同 。 路径上的侧向偏移变化能使自主驾驶车辆避开障碍物。3）路径的选择我们要从生成的众多路径中，需要采用搜索算法来找到使带权重的代价函数的线性组合极小的一条最优路径。代价函数要考虑安全性，光滑性以及路径的稳定性这最为主要的三个因素 。.专业学习资料.这里 i 是路径编号 ， Cs 是路径的安全代价， Ck 是路径的光滑性代价， Cc 是路径的平稳性代价。 每个代价都有相应的权重因子。而权重因子的取值与

11、车辆自身，环境因素等实际情况有关 。 计算公式如下 ：根据安全性代价、光滑性代价、连贯性代价，经过编程调试，采用经多次检验的算法，我们可以选择出一条使得总的代价函数取得最小值的路径。5 算法 3：A* 算法5.1 算法原理把待规划区域的地图划分成笛卡尔坐标系的栅格（如边长 20cm ）， 把障碍物定义为1的栅格 ，可行驶的为0 的栅格 ，按目标做最短路径搜索，再把搜索结果做平滑处理。图地面格网和A*算法.专业学习资料.5.2 算法实现A*算法最为核心的部分，就在于它的一个估值函数的设计上： f(n)=g(n)+h(n)其中 f(n) 是每个可能试探点的估值，它有两部分组成：一部分 ，为 g(n

12、) ，它表示从起始搜索点到当前点的代价（通常用某结点在搜索树中的深度来表示 ）。另一部分 ，即 h(n) ，它表示启发式搜索中最为重要的一部分，即当前结点到目标结点的估值 ， 一种具有f(n)=g(n)+h(n)策略的启发式算法能成为A*算法的充分条件是：1 、搜索树上存在着从起始点到终了点的最优路径。2 、问题域是有限的。3 、所有结点的子结点的搜索代价值>0 。4 、 h(n)=<h*(n)（ h*(n) 为实际问题的代价值）。当此四个条件都满足时，一个具有f(n)=g(n)+h(n)策略的启发式算法能成为A* 算法，并一定能找到最优解。对于一个搜索问题，显然，条件 1,2,3

13、 都是很容易满足的，而条件 4 ： h(n)<=h*(n)是需要精心设计的，由于 h*(n) 显然是无法知道的，所以 ，一个满足条件4 的启发策略h(n) 就来的难能可贵了。不过 ，对于图的最优路径搜索和八数码问题，有些相关策略h(n) 不仅很好理解，而且已经在理论上证明是满足条件4 的，从而为这个算法的推广起到了决定性的作用。且 h(n)距离 h*(n) 的呈度不能过大，否则 h(n) 就没有过强的区分能力，算法效率并不会很高。对一个好的 h(n) 的评价是 ：h(n) 在 h*(n) 的下界之下 ，并且尽量接近h*(n) 。求 V0->V5的路径 ，V0->V5的过程中 ，可以经由V1 ， V2 ， V3 ， V4 各点达到目的点V5 。 下面的问题 ，即是求此 起始顶点 V0-> 途径任意顶点V1 、 V2 、 V3 、V4-> 目标顶点 V5.专业学习资料.的最短路径 。图算法求解示例通过上图 ，我们可以看出： A*算法最为核心的过程