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

网络化自动驾驶轨迹规划算法设计说明书作者 胡学敏评审人日期 2015/12/26环宇智行科技有限公司华为技术有限公司修订记录作者修订时间版本号修改内容胡学敏2015/12/291 .更新三种算法的原理图和相关实现2 .更新的接口部分胡学敏2016/1/31 .更新的轨迹规划流程2 .调整部分格式目录1概述 2框架和接口 轨迹规划流程 算法选择模块 接口 算法介绍 3算法1：虚拟车道算法 算法原理 算法实现 4算法2：曲线轨迹簇算法 算法原理 算法实现 1）基本路线的构建 2）候选路径的生成 3）路径的选择 5算法3： A*算法 算法原理 算法实现 错误！未定义书签错误！未定义书签错误！未定义书签错误！未定义书签错误！未定义书签错误！未定义书签错误！未定义书签错误！未定义书签错误！未定义书签错误！未定义书签错误！未定义书签错误！未定义书签错误！未定义书签错误！未定义书签错误！未定义书签错误！未定义书签错误！未定义书签错误！未定义书签1概述CSU高精度地图车道级路径规划车道级路径规划（转发自CSU）RSU局部车辆实时状态高精度地图说明：1. CSU通常只做一次 车道级路径规划2. RSU根据车辆实时 状况动态切换车辆行 驶的车道。

图整体路径规划过程示意图轨迹规划是实现智能车自主驾驶的关键模块 要实现对自动驾驶车辆的控制前提就是根据车辆的当前道路环境，行驶状态以及整体交通环境规划出一条合理的轨迹， 以便于下层控制器进行车辆控制整个轨迹规划从 CSU的整体路网规划（考虑整体车流和交通状况）到RSU的局部交通流规划（平衡局部车流），再到OBU的具体行驶轨迹规划以下着重讨论OBU中的规 划算法2框架和接口2. 1轨迹规划流程图2. 1. 1 OBU轨迹规划轨迹规划流程：1. OBU用户在UI选择目的地，OBU> RSU RSU t CSU CSU故“全程车道级路径规划”，输出结果为“车道编号集合、task参数”，保存在CSU的该OBU_session上，然后 CSU t RSU下发路径规划结果， RSU保存在该OBU_session里RSU t OBU下发路径规划结果，保存在内存2. OBU将感知融合的数据和网络路径规划的结果投影到当前道路环境中， 调整当前OBU规划的轨迹，并显示在UI上OBU轨迹规划数据源：地图数据（包括道路边界点，道路拓扑结构，车道线信息） ，感知融合数据（车线，障 碍物信息等）OBU轨迹规划频率与规划距离： OBL规划模块每1秒规划一次，每次规划100米。

2. 2算法选择模块OBU规划模块创建算法选择类， OBU直接调用该类的接口获得轨迹，算法选择类支持注册多种算法，并选择一种算法进行轨迹规划3. 3 接口OBU调用算法选择模块的接口，包括：当前车道与相邻车道构成的 4条线（当前车道与左右两条车道， 一共3条车道，需要4条车道线来标记）障碍物、障碍物形状、障碍物速度路径规划参数2. 4算法介绍OBU中轨迹规划主要包括以下三种算法1）虚拟车道算法：总是沿着道路的中线行驶，发现障碍物采用跟驰 （/刹车）或换道，换道行为相当于虚拟了一个走到另一个车道的虚拟车道，然后继续沿中线行驶2）曲线轨迹簇算法：以车的方向和道路的曲率构建曲线坐标系， 然后在此坐标系内拟合曲线轨迹簇，选择最佳曲线，然后转化为笛卡尔坐标系3） A*算法：把待规划区域的地图划分成笛卡尔坐标系的栅格（如边长 20cm）,把障碍物定义为1的栅格，可行驶的为0的栅格，按目标做最短路径搜索， 再把搜索结果做平滑处理算法选择：虚拟车道法简单可靠，且曲线平滑，舒适性好，可作为默认算法但是此算法简化为 跟驰和换道两个动作， 应对不同车姿（曲线坐标轨迹簇算法的优势） 和发现最佳路径（A*算法的优势）的能力很弱，路况复杂时容易找不到出路。

此时应降级为“曲线坐标轨 迹簇算 法”算法，此算法发现道路的能力较强，曲线也很平滑如果此算法也得不到结果，则降级为“ A*算 法”，实现“有空就钻”的路径搜索（但行驶曲线不够平滑） ，仍然没有出路时，停车（通常为完全堵车了）3算法1虚拟车道算法3. 1算法原理总是沿着道路的中线行驶，发现障碍物采用跟驰 （/刹车）或换道，换道行为相当于虚拟了一个走到另一个车道的虚拟车道，然后继续沿中线行驶虚拟车道算法4. 2算法实现1） OBU通过车道线检测实时确定当前车道线的位置，同时 OBU加载地图中的车道模 型（车道宽度、车道数目、特征点）2）OBU将当前检测车道与车道模型进行匹配，确定当前车道和邻近几条车道3）车辆总是沿着车道中心线行驶，当车辆需要换道时，根据道路环境，虚拟出一条与相邻 车道的交汇车道，完成变道动作4算法2：曲线轨迹簇算法4. 1算法原理以车的方向和道路的曲率构建曲线坐标系,然后在此坐标系内拟合曲线轨迹簇， 选择最佳曲线，然后转化为笛卡尔坐标系图4.1.1轨迹簇算法5. 2算法实现算法实现的流程如图4所示:地图载入图4. 2.1曲线轨迹簇算法流程图最优路在1）基本路线的构建基本路线是由包含整体路径规划的道路中心点所组成的航迹点的样条拟合构建的。

参数化曲线经常被用来建立道路的集合模型并且定义运动路径 采用参数化三次样条来定义基于航迹点构建的基本路线的参数化曲线2）候选路径的生成行驶路线应该能够引导车辆随着全局路线行走并且避开障碍物 为了达到这个目的，建立了一个基本路线的曲线坐标系统在基本路线的曲线坐标系统中设计避免障碍物的光滑策略图5 显示了在笛卡尔坐标系统中的路径和基本路径的曲线坐标系统之间的几何关系图中代表在整体路径中移动距离的基本路径的弧线长度 S成为了曲线坐标系统的水平坐标，最近的偏移q成为了垂直坐标图4. 2.2笛卡尔坐标与曲线坐标的几何关系为了生成路径，路径的曲率是由基于基本路径的曲率路径的侧向偏移所决定的在计算路径 曲率时需要侧向偏移的导数和二阶导数，因此侧向偏移函数要有一定的光滑性侧向偏移函数描述如 下其中弧长si和当前位置的侧向偏移qi由定位步骤给出0为当前位置车头和基本路径的夹角采 用边界条件，我们可以得到三次多项式的系数路径规划算法生成了一些候选路径每个路径有一 个明显侧向偏移q£覆盖道路的所有候选路径只是在侧向偏移上有些许不同路径上的侧向偏移变 化能使自主驾驶车辆避开障碍物3）路径的选择我们要从生成的众多路径中，需要采用搜索算法来找到使带权重的代价函数的线性组合极小的 一条最优路径。

代价函数要考虑安全性，光滑性以及路径的稳定性这最为主要的三个因素这里i是路径编号，Cs是路径的安全代价，Ck是路径的光滑性代价，Cc是路径的平稳性代 价每个 代价都有相应的权重因子而权重因子的取值与车辆自身，环境因素等实际情况有 关计算公式如 下：根据安全性代价、光滑性代价、连贯性代价，经过编程调试，采用经多次检验的算法，我们可 以选择出一条使得总的代价函数取得最小值的路径5算法3： A*算法6. 1算法原理把待规划区域的地图划分成笛卡尔坐标系的栅格（如边长 20cm）,把障碍物定义为1的栅格，可行驶的为0的栅格，按目标做最短路径搜索，再把搜索结果做平滑处理图5. 1. 1地面格网和A*算法5. 2算法实现A*算法最为核心的部分，就在于它的一个估值函数的设计上： f(n )=g( n)+h( n)其中f(n)是每个可能试探点的估值，它有两部分组成：一部分，为g(n),它表示从起始搜索点到当前点的代价(通常用某结点在搜索树中的深度来另一部分，即h(n),它表示启发式搜索中最为重要的一部分，即当前结点到目标结点的估 值， 一种具有f (n) =g(n) +h(n) 策略的启发式算法能成为 A*算法的充分条件是：1、搜索树上存在着从起始点到终了点的最优路径。

2、问题域是有限的3、所有结点的子结点的搜索代价值 >04、h(n)=〈h*(n) (h*(n)为实际问题的代价值)当此四个条件都满足时，一个具有f (n) =g(n) +h(n) 策略的启发式算法能成为 A*算法，并一定能找到最优解对于一个搜索问题，显然，条件1,2, 3都是很容易满足的，而条件4： h(n)«h*(n)是需要精心 设计的，由于h*(n)显然是无法知道的，所以，一个满足条件 4的启发策略h(n)就来的难能可贵了不过，对于图的最优路径搜索和八数码问题，有些相关策略 h(n)不仅很好理解，而且距离h*(n)的呈度不能过大，否则h(n)就没有过强的区分能力，算法效率并不会很高对一个好的 h(n)的评价是：h(n)在h*(n)的卜界之下，并且尽量接近h*(n)求V0->V5的路径，V0->V5的过程中，可以经由Vlf V2f V3f %各点达到目的点V5下面的问题，即是求此 起始顶点V0->途径任意顶点VI V2. V3 V4->目标顶点V5的最短路径图5. 2.1 A*算法求解示例通过上图，我们可以看出：：A*算法最为核心的过程，就在每次选择下一个当前搜索点时，是 从所有已探知的但未搜索过点中（可能是不同层，亦可不在同一条支路上），选取f值最小的结点 进行展开。

而所有“已探知的但未搜索过点”可以通过一个按f值升序的队列（即优先队歹IJ）进 行排列这样，在整体的搜索过程中，只要按照类似广度优先的算法框架，从优先队列中弹出队 首元素（f值），对其可能子结点计算g、h和f值，直到优先队列为空（无解）或找到终止点为 止。