任务5.5 路径规划算法-学生手册

任务5.5路径规划算法学生手册任务5.5路径规划算法【任务导入】路径规划是整个智能驾驶系统中最核心的部分之一，前面的课程已经讲解全局路径规划的内容，本任务将着重讲解局部路径规划。局部规划和全局规划有什么异同点呢？【学习目标】素质目标通过路径规划算法的流程，培养学生的逻辑思维和创新思维；了解环境处理模块的逻辑，提高学生在技能上的职业素养。知识目标能理解路径规划的基本概念[K79]；能描述拟人式和场景编排式路径规划算法的流程[K80]；能理解环境处理模块的工作流程[K81]。能力目标能列举路径规划常见算法[A65]；能举例讲解环境预处理模块的逻辑[A66]。

【知识准备】路径规划算法背景整个智能驾驶系统细分为感知融合、地图定位、规划和控制等多个模块，如果说感知像是人的眼睛等五官，控制像是人的手脚，那么规划（Planning）更像是人的大脑，是整个智能驾驶系统中最核心的部分之一。在本任务中所描述的Planning更侧重于局部路径规划，Planning会考虑所有当前信息和历史信息，生成一条最终车辆要行驶的路径交给控制模块去控制车辆行驶。自动驾驶中的路径规划是指根据车辆的起始点和目标点生成一条无碰撞且满足车辆自身约束的轨迹，一般情况下路径规划不包含时间信息。路径规划算法的作用整个智能驾驶系统可以划分为两大块：广义感知和广义规控，如REF_Ref15332\h图5-5-1所示。在规控部分可划分为地图定位、全局导航、路径规划、安全边界、运动控制等，其中路径规划是整个规控的核心模块。路径规划会接收到反馈真实外部世界的模型数据、导航数据、车辆及驾驶人状态数据后，在车辆中心点前后累加300米内，输出一条符合规则（导航和交规）、安全、平滑的路径（包含路径和速度）。图5-5-SEQ图5-5-\*ARABIC1广义感知和规控常见路径规划算法业界常用的路径规划算法可以分为四大类，见REF_Ref14601\h表5-5-1：基于图形搜索的规划器：将地图、车辆的信息三格化，然后在三格化的图形中搜索出一条无碰撞且代价很小的路径，缺点在于搜索算法在大范围和三格不停细化的场景下，时间复杂度会提高。基于采样的规划器：相对图算法更加迅速，但在特定一些复杂的空间下，采样会导致效率下降。插值曲线规划器：通过少数的控制点去生成光滑的插值曲线，但是这些插值曲线未必满足约束条件。数值优化方法：设定曲率、位移速度、光滑可导等约束条件，通过数值优化的方法去求解轨迹点，能在一系列约束下得到最优轨迹，但在一些极端的约束条件下可能会导致求解失败。表5-5-SEQ表5-5-\*ARABIC1各路径规划算法比较算法分类算法名字优点缺点基于图形搜索的规划器Dijkstras不仅求出起点到终点的最短路径以及长度，而且还求解出了起点到图中各个点的的最短路径，适用于结构化和非结构化环境中的全局规划。当节点数目n比较大的时候，该算法的时间复杂度急剧增加，直接运用该算法存在速度慢或者空间不足的问题，很难满足路径规划实时性的要求。A*系列基于Dijkstra算法，搜索中增加了启发函数，减少了搜索节点，从而提高了搜索效率，目前全局路径规划运用最为广泛的算法。启发函数大多情况下不好确定，其空间增长是指数级别的。状态格算法能够处理多个维度（位置、速度、加速度、时间），适用于局部规划和动态环境下的规划。由于对数据库中每个可能的解决方案进行了评估，因此计算非常耗时。基于采样的规划器RRT系列算法能够在多维系统中提供快速解决方案。如果有一个解，并且有足够的时间，总是能收敛到这个解，适用于全局和局部规划。产生的轨迹不连续，当空间中包含大量障碍物或者狭窄通道约束时，算法的收敛速度极慢，效率会大幅下降。插值曲线规划器多项式曲线计算量较小，可用于多条曲线的串联，规划结果舒适性较高。曲率连续最低要求4次，关键参数不容易获取。贝塞尔曲线计算量较小，可用于多条曲线的串联，规划结果舒适性较高。随着曲线阶数的增加，曲线的延展性下降，计算时间也随之增加。样条曲线计算量较小，得出由不同控制点拟合出的连续曲线。从道路适应度和曲率最小化的角度来看，该解决方案可能不是最优，因为它的结果更侧重于实现节点之间的连续性，而不是适应道路约束的延展性。数值优化方法功能优化可以将道路的约束、车辆状态的约束以及其他的交通参与者直接表示出来并通过数学方法进行求解。在每个运动状态下都会进行实时的计算，计算量大且非常耗时，依赖于全局导航的路点信息。路径规划算法概述常用的路径规划算法流程框架有拟人式和场景编排式。拟人式规划算法（状态迁移式）拟人式规划算法框架是一种基于状态迁移的规划算法，可以理解为该算法是从一个正常人类司机开车的角度出发，设定一系列横向、纵向的行为以及一系列的目标，并根据这些信息生成相应的决策行为和轨迹规划，其算法框架如REF_Ref15492\h图5-5-2所示。行为决策通过时间、空间范围内的安全性、效率性、舒适性、利他性等因素，进行行为的状态迁移。输出信息如下：横向行为[车道保持/避障、（取消/强制）变道、泊车、掉头、路口左/右转、汇入汇出]；纵向行为[free、跟车、汇入、（V/P/E）停车、泊车、倒车]；速度目标[速度限制，虚拟墙、减速度、P-stop距离]；跟踪目标[ID，速度，距离，需要保持的距离]；变道gap（旁路）目标[目标gap（前后车速度、距离）、bypass旁路距离]。轨迹规划根据不同行为，确定路点序列。输出信息如下：路点序列；速度信息[目标速度（限制）]；提前的灯控信息图5-5-SEQ图5-5-\*ARABIC2拟人式规划算法框架场景编排式规划算法场景编排式规划算法的核心是把规划过程根据功能需求进行job（单一场景的路径规划策略）化的拆分，按照模块化的思路，通过不同驾驶场景的需求来进行不同job的组合，来实现对应的功能，如REF_Ref15414\h图5-5-3所示。例如在跟车场景中，算法就只需要考虑跟车相关的job以及功能函数，其他诸如超车、变道等job就不需要考虑，这样的结构使得规划算法更加灵活，复用程度高、易于拓展。图5-5-SEQ图5-5-\*ARABIC3场景编排式规划算法框架场景编排式规划算法的整体流程类似一个流水线，如REF_Ref15381\h图5-5-4所示，环境处理模块提供的planningBUS(数据流)相当于“原材料”，每一个job对数据总线上的“原材料”进行获取和修改，最终获得轨迹信息。图5-5-SEQ图5-5-\*ARABIC4场景编排式规划算法流程路径规划算法环境处理模块从路径规划算法中流程框架中可以发现，路径规划的必要步骤是对环境信息进行提取和处理，下面将展开介绍路径规划算法环境预处理的相关内容。路径规划环境预处理流程，如所示REF_Ref16201\h图5-5-5所示。输入：预处理流程会结合多种环境信息输入如定位信息、全局导航信息、车辆底盘信息、车辆车身信息、障碍物信息等，进行环境的处理；处理：环境预处理可分为两大部分，第一是结合地图信息，生成局部导航路径，也就基准的参考线；第二是根据参考线，结合障碍物的各种信息、交通规则等抽象投影到参考线上，为后续的规划提供帮助；输出：具有车辆、道路、障碍物详细信息的数据流。图5-5-SEQ图5-5-\*ARABIC5环境预处理流程地图导航信息处理地图导航信息处理的目标是生成参考线，参考线的作用是提供环境状态的一个坐标参考，通过实现坐标转换的方法，便于后续在统一的坐标系下进行算法和内部逻辑的处理。参考线的生成即基于车道线中心线进行生成，但是自车行驶的车道信息需要地图全局导航结果，因此首先要进行地图全局导航信息的处理，根据全局导航信息的结果以及自车当前的位置，找出自车即将要走的一段路线上的车道信息，一般包括主车前后两三百米的车道信息，从而生成一条局部的导航路径，如REF_Ref23565\h图5-5-6所示。图5-5-SEQ图5-5-\*ARABIC6参考线生成Lane：表征道路中的每一个可行驶的车道；Road：表征道路的信息，诸如“环湖路”等；SubLane：表征局部导航中的Lane的组成形式，可以是完整的Lane，也可以是Lane的其中一段。此时得到的参考线，实质上是一个基于车道信息提取的车道中心线的一系列采样点，但是初始的参考线还是离散的，需要对其进行平滑处理，对这些离散的点进行曲线的拟合，如REF_Ref27624\h图5-5-7所示。图5-5-SEQ图5-5-\*ARABIC7参考线拟合障碍物投影有了一条平滑的参考线后，如REF_Ref30129\h图5-5-8所示中的红色曲线，还需要根据各种信息做投影，从左图上弯曲的道路信息，障碍物信息、预测轨迹都投影到右图上的统一直角坐标系中，最终得到一条包含障碍物的参考线，用于后续的规划算法。图5-5-SEQ图5-5-\*ARABIC8障碍物投影【任务实施】路径规划导航概述本实训任务的将使用自动驾驶一体化框架AutoWare，通过加载场景及视像bag包以及自动驾驶算法，构建虚拟仿真场景，复现路径规划导航。实验环境准备设备检查本次实验环境包括电源线，安装Ubuntu18.04虚拟环境的PC机一台（最低CPU配置为I510代），见REF_Ref14601\h表5-5-2。每套实验环境适用于4~12名学生同时上机操作。表5-5-SEQ表5-5-\*ARABIC2实验清单设备名称数量备注电源线1组所有实验组共用笔记本或台式机每组1台台式机要有无线网卡项目描述视觉里程计项目文件：5-5aw_ws文件夹，主要用到的文件如图5-3-9所示。图5-5-SEQ图5-5-\*ARABIC9项目描述进入工程目录打开主目录5-5aw_ws文件夹，右键鼠标打开终端框，如图5-5-10所示。图5-5-SEQ图5-5-\*ARABIC10进入工程目录构建环境变量打开终端框后，必须先将工作空间的路径加入到环境变量中，才能使用ros相关的命令，输入以下命令：sourceinstall/setup.bash执行命令后，如REF_Ref9357\h图5-5-11所示。图5-5-SEQ图5-5-\*ARABIC11构建环境变量运行项目脚本使用ros命令运行aw_ws项目脚本，输入以下命令：roslaunchruntime_managerruntime_manager.launch执行命令后，终端框会输出进程启动信息以及软件启动信息，如REF_Ref10618\h图5-5-12项目启动命令所示。图5-5-SEQ图5-5-\*ARABIC12项目启动命令将自启动RuntimeManager软件，显示图像化界面，如图5-5-13所示。图5-5-SEQ图5-5-\*ARABIC13RuntimeManager界面加载bag包点击Simulation选项卡，选择aw_ws项目文件夹中的sample_moriyama_150324.bag包，如REF_Ref12384\h图5-5-14所示。图5-5-SEQ图5-5-\*ARABIC14加载bag包设置倍率Rate：0.5，开始时间StartTime(s)：140，点击“Play”按钮播放bag包，当播放1秒后，点击“Pause”按钮暂停播放。（由于bag包的画面渲染非常占用内存，所以提前播放bag包，加载画面信息，以免后续运行时卡顿，如REF_Ref13789\h图5-5-15所示。图5-5-SEQ图5-5-\*ARABIC15加载播放bag包读取算法文件点击QuickStart选项卡，依次点击Map（地图）、Sensing（传感器）、Localization（定位）、Detection（检测信息）、MissionPlanning（任务规划）、MotionPlanning（路径规划）按钮，加载相关算法程序文件，如REF_Ref15294\h图5-5-16所示。图5-5-SEQ图5-5-\*ARABIC16读取资源文件若终端框中出现如REF_Ref16731\h图5-5-17所示提示，则表示加载成功。（WARN警告是因为算法加载已经完成，但是有bag包暂停播放，算法没有接收到相关数据处于阻塞状态。）图5-5-SEQ图5-5-\*ARABIC17文件加载成功启动RViz读取完所有资源文件后，点击Rviz按钮，启动RViz软件，如REF_Ref17368\h图5-5-18所示。图5-5-SEQ图5-5-\*ARABIC18启动Rviz软件播放bag包在“RuntimeManager”软件界面中，重新点击“Simulation”选项卡，再次点击“Pause”按钮重新播放bag包，如REF_Ref18014\h图5-5-19所示。图5-5-SEQ图5-5-\*ARABIC19重新播放bag包加载配置文件点击左上角File->OpenConfig选项，如REF_Ref18014\h图5-5-20所示。图5-5-SEQ图5-5-\*ARABIC20打开配