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

网络化自动驾驶轨迹规划算法设计说明作者胡评论者日期，2015/12/26玉环智星科技有限公司华为技术有限公司修订记录作者修订时间版本号修改内容胡2015/12/29V1.11.更新三种算法的原理图和相关实现2.更新2.3的接口部分。胡2016/1/3V1.31.更新2.1的轨迹规划流程。2.调整一些格式目录1概述32框架和接口32.1跟踪规划流程32.2算法选择模块42.3接口42.4算法简介43算法1:虚拟车道算法43.1算法原则43.2算法实现54算法2:曲线轨迹聚类算法54.1算法原则54.2算法实现61)基本六号线建设2)候选路径的生成73)路径选择85算法3: a *算法85.1算法原则85.2算法实现91概述图1.1整体路径规划流程示意图轨迹规划是实现智能车辆自主驾驶的关键模块。实现自动驾驶车辆控制的前提是根据车辆当前的道路环境、行驶状态和整体交通环境规划合理的轨迹，以便于下层控制器对车辆进行控制。整个轨迹规划是从南大的整体路网规划(考虑整体交通流量和交通状况)到RSU的局部交通流量规划(平衡局部交通流量)，再到OBU的具体行车轨迹规划。以下重点介绍OBU的规划算法。2框架和接口2.1跟踪规划流程图2.1.1 OBU轨迹规划跟踪规划流程：1.OBU，用户在用户界面中选择目的地，OBURSU，RSU CSU，CSU进行“全车道级路径规划”，输出结果为“车道号集，任务参数”，保存在CSU的OBU _会话中，然后CSU RSU:发布路径规划结果，RSU保存在OBU _会话中。RSU OBU:发布路径规划结果并保存在内存中。2.OBU将感知融合的数据和网络路径规划的结果投影到当前道路环境中，调整当前OBU规划的轨迹，并将其显示在用户界面上。OBU轨迹规划数据源：地图数据(包括道路边界点、道路拓扑、车道线信息)、感知融合数据(车辆线、障碍物信息等)。)。OBU轨迹规划频率和规划距离：OBU规划模块每1秒规划一次，每次规划100米。2.2算法选择模块OBU规划模块创建一个算法选择类。OBU直接调用这个类的接口来获得轨迹。算法选择类支持注册多个算法，并为轨迹规划选择一个算法。2.3接口OBU呼叫算法选择模块的界面包括：L 4线由当前车道和相邻车道组成(当前车道和左右车道共3条车道，需要4条车道线来标记)。障碍物，障碍物形状，障碍物速度。路径规划参数。2.4算法介绍OBU的轨迹规划主要包括以下三种算法1)虚拟车道算法：始终沿道路中线行驶，并通过车辆跟随(/brake)或换道来发现障碍物，这相当于虚拟车道步行至另一车道，然后继续沿中线行驶。2)曲线轨迹聚类算法：根据汽车的方向和道路的曲率构造一个曲线坐标系，然后在该坐标系中拟合曲线轨迹聚类，选择最佳曲线，然后将其转换为笛卡尔坐标系。3) A*算法：将待规划区域的地图划分为笛卡尔坐标系的网格(如20厘米边长)，将障碍物定义为可驱动的1格和0格，根据目标搜索最短路径，并平滑搜索结果。算法选择：虚拟车道法简单可靠，曲线平滑，舒适性好，可作为默认算法。然而，该算法被简化为车辆跟驰和换道两个动作，并且其处理不同车辆姿态(曲线坐标轨迹聚类算法的优点)和寻找最佳路径(A*算法的优点)的能力非常弱，因此在道路条件复杂时很容易找到出路。此时，应将其降级为“曲线坐标轨迹聚类算法”，该算法发现道路能力强，曲线平滑。如果这个算法没有得到r3.1算法原理始终沿道路中线行驶，并使用汽车跟随(/brake)或换道来发现障碍物，换道行为相当于将一条虚拟车道换到另一条车道，然后继续沿中线行驶。图3.1.1虚拟车道算法3.2算法实现1)OBU通过车道线检测实时确定当前车道线的位置，同时OBU在地图中加载车道模型(车道宽度、车道数、特征点)。2) OBU将当前检测车道与车道模型匹配，以确定当前车道和几个相邻车道。3)车辆总是沿着车道的中心线行驶。当车辆需要换道时，根据道路环境模拟与相邻车道相交的车道，完成换道动作。4算法2:曲线轨迹聚类算法4.1算法原理基于车辆方向和道路曲率构建曲线坐标系，然后在该坐标系中拟合曲线轨迹簇，选择最佳曲线，然后将其转换为笛卡尔坐标系。图4.1.1轨迹聚类算法4.2算法实现算法实现的流程如图4所示。图4.2.1曲线轨迹聚类算法流程图1)基本路线建设基本路线是通过由道路中心点组成的轨迹点的样条拟合来构建的，包括总体路径规划。参数曲线通常用于构建道路的聚合模型和定义运动路径。参数三次样条用于根据轨迹点定义基本路线的参数曲线。2)候选路径的生成行驶路线应该能够引导车辆遵循全局路线并避开障碍物。为了实现这一目标，建立了基本路线的曲线坐标系。在基本路线的曲线坐标系中设计了一种平滑的避障策略。图5示出了笛卡尔坐标系中的路径和基本路径的曲线坐标系之间的几何关系。在该图中，表示整个路径中的移动距离的基本路径的弧长S成为曲线坐标系的水平坐标，最近的偏移Q成为垂直坐标。图4.2.2笛卡尔坐标和曲线坐标之间的几何关系为了生成路径，路径的曲率由基于基本路径的曲率路径的横向偏移来确定。当计算路径的曲率时，需要横向偏移的导数和二阶导数，因此横向偏移函数必须具有一定的光滑性。横向偏移功能描述如下其间当前位置的弧长si和横向偏移qi由定位步骤给出。是当前位置的主轴箱和基本路径之间的角度。利用边界条件，我们可以得到三次多项式的系数。路径规划算法生成一些候选路径。每条路径都有一个显著的横向偏移qf。覆盖道路的所有候选路径在横向偏移上仅略有不同。路径横向偏移的变化使得自主车辆能够避开障碍物。3)路径选择我们需要使用搜索算法从生成的多条路径中找到一条使加权成本函数的线性组合最小的最优路径。成本函数应该考虑安全、平滑度和路径稳定性这三个最重要的因素。这里我是路径号，Cs是路径的安全成本，Ck是路径的平滑成本，Cc是路径的平滑成本。每个成本都有相应的权重因子。权重因子的值与车辆本身的实际情况和环境因素有关。计算公式如下：根据安全性代价、平滑性代价和一致性代价，经过编程和调试，我们可以选择一条路径，使总代价函数通过多次检查的算法得到最小值。5算法3: a *算法5.1算法原理将待规划区域的地图划分为笛卡尔坐标系的网格(如边长20厘米)，将障碍物定义为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.所有节点的子节点的搜索生成值为0。4.h(n)=V5的路径。在V0-V5的过程中，目的地点V5可以通过V1、V2、V3和V4到达。下面的问题是找到从这个起始顶点V0到目标顶点V5的路径的任何顶点V1、V2、V3、V4的最短路径。图5.2.1算法解决方案示例从上图中，我们可以看到：A*算法的核心过程是每次选择下一个当前搜索点时，从所有检测到但未搜索到的点(