毕业设计（论文）-无人驾驶车辆路径规划方法研究与仿真.docx

无人驾驶车辆路径规划方法研究与仿真SHANGHAI JIAO TONG UNIVERSITY学士学位论文BACHELORS THESIS论文题目：无人驾驶车辆路径规划方法研究与仿真学生姓名: 学生学号: 专 业: 指导教师: 学院(系): 电子信息与电气工程学院无人驾驶车辆路径规划方法研究与仿真摘要随着无人驾驶技术的飞速发展，无人驾驶汽车的路径规划也变得也来越重要。在不同任务和环境下的路径规划成为了主要的研究方向。为了满足无人驾驶汽车导航的全局最优和实时性，将路径规划方式分为了全局路径规划和局部路径规划。针对全局路径规划问题，本文利用优化的A*算法进行路径规划，该方法采用A*算法选择全局最优路径，对规划出的路径采用贝塞尔曲线进行路口等特殊路段优化，最后对路径信息进行封装。避免了全局路径规划在特殊路段的不合理性，在关键路段能够有效的给出指引。针对局部路径规划问题，本文提出了四阶贝塞尔曲线路径规划方法和改进的快速搜索随机树（RRT）方法。四阶贝塞尔曲线路径规划方法采用了四阶贝塞尔曲线进行路径规划，提高了规划效率，并且生成的路径具有良好的曲率连续特性。改进的RRT方法提高了传统RRT算法的搜索效率，并避免其随机性对路径生成的影响，对生成的路径采用B样条和贝塞尔曲线进行后处理和平滑，提高了路径的平滑程度。最后实验结果表明，本文提出的方法在全局路径规划中能够为车辆选择最优路径且能够在关键路段进行有效指引，局部规划算法具有良好的实时性且更好的满足了车辆运动学约束与环境约束。关键词：全局路径规划，局部路径规划，A*，RRT，四阶贝塞尔曲线，路径优化全套设计加扣 3012250582UNMANNED VEHICLES PATH PLANNING METHOD AND SIMULATIONABSTRACTWith the rapid development of self-driving technology, path planning of autonomous vehicles have also become more and more important. Path planning in different tasks and environment has become the main directions for research on path planning. In order to satisfy the real-time and the global optimal of driverless car navigation, path planning methods can be divided into the global path planning and local path planning. For global path planning problem, this article put forward put forward the optimization of the A * path planning method. This method adopts the A * algorithm for global path planning, then encapsulate path information after using Bezier curve to optimize intersections and other special road section. This method improves the readability and the quality of guidance of the path, and avoids the irrationality of global path planning in the special section. For local path planning problem this article put forward the fourth-order Bessel curve and the optimization method of RRT. The fourth-order Bessel curve path planning methods uses the fourth-order Bessel curve to plan path. This method avoids the subsequent operation of path planning, improves the efficiency and generates paths that has well continuous features in curvature. The optimization method of RRT uses the optimized RRT method to plan path, then uses b-spline and Bessel curve to reprocess and smooth the path. This method increases smoothness of the path. Experimental results show that the path of the global path planning method has great global optimality and quality of guidance, local path planning method has great real-time performance and availability.Key words: global path planning, local path planning, A*, RRT, fourth-order-Bezier, Path Optimization目 录 第一章 绪论11.1 背景与意义11.2 国内外研究现状21.2.1国外研究现状21.2.2国内研究现状21.4 路径规划关键技术31.4.1 全局路径规划31.4.2 局部路径规划41.5 主要研究内容4第二章 全局路径规划62.1 引言62.2 全局路径规划方法综述62.3 基于A*的全局路径规划72.3.1地图信息的读取和处理72.3.2 起始点匹配92.3.3 最优路径选取102.3.4 路径信息输出112.4 基于贝塞尔曲线的路径优化122.5 实验结果与分析142.6 本章小结19第三章 基于四阶贝塞尔曲线的局部规划203.1 引言203.2 方法综述203.3 局部状态栅格图223.4 基于四阶贝塞尔曲线的路径规划233.5 实验结果与分析263.6 本章小结26第四章 基于改进RRT算法的局部规划274.1 引言274.2 RRT算法274.2.1 RRT算法的优化274.2.2 路径的后处理和平滑344.3实验结果与分析384.4 本章小结40第五章 总结与展望41参考文献43谢辞45第一章 绪论1.1 背景与意义随着科学技术的日益发展，传统的汽车行业已经进一步发展成为能够自主驾驶的无人驾驶汽车行业。无人驾驶汽车是根据车载传感系统感知的道路环境，智能规划行车路线并控制车辆到达预定位置的智能汽车。它不仅能够使用各种车载传感器，例如车载摄像头、激光雷达、声呐传感器、GPS、里程计、陀螺仪等，来感应和识别汽车周围的环境，还能够接收其他智能系统，例如GRRS定位信息等，获得周围环境的辅助信息，最后通过汽车内部的计算机对获得的道路，车辆姿态和障碍物信息的处理，控制车辆的前行方向和速度，从而确保车辆能够安全可靠的在各种道路上行驶。无人驾驶汽车技术是集合了人工智能，计算机视觉，自动控制技术，机械学，自主导航，传感器网络，信息综合和机械设计制造等多种理论和技术于一体的多学科、多行业的综合技术。这种技术的研究对现代交通系统的发展有重要意义，并且有着军事工业等应用前景，它代表了国家计算机科学，模式识别和智能控制技术的发展水平，已经成为世界范围内机器人和人工智能领域的研究热点之一12。从而，无人驾驶汽车的推广和运用,毫无疑问将会减少至少一半的交通伤亡等事故，将会为快速发展的汽车社会带来很多好处，并为汽车驾驶司机减少很多后顾之忧。拥有“交通识别能力”的汽车，不仅有益于驾驶者的安全，对交通参与者的安全都是一种有效的保护。美国国防部先进项目研究局（DARPA）举行了三次无人驾驶汽车挑战赛，第一届挑战赛于2004年3月举办，在106个申请车队中只有15只参加了最终的角逐，成绩最好的车辆行驶了全程的百分之五。第二届比赛于2005年10月，在197个申请DARPA中选出了23支队伍参加，之后被证实比2004比赛中的最优秀车辆更加优秀，2005年的赛道穿越内华达的132英里艰难的沙漠道路，包含了复杂的非结构化地形，尘土、急转弯、狭窄道路、桥梁等，虽然地形很复杂，但相对是一条单通道的道路，没有涉及到道路规划问题。第三届DARPA挑战赛为DARPA城市挑战赛（Urban Challenge）,于2007年11月3日在美国西部加利福尼亚周维克多镇已关闭的乔治空军基地举行3。城市挑战赛需要自动地面车辆沿着路网中的一些列检查点前进。道路网络作为预知信息以道路网络定义文件（Road Network Definition File，RNDF）给出，任务定义文件（Mission Definition File，MDF）提供了车辆必须通过的检查点。DARPA挑战赛从第二阶段的沙漠到城市，对车辆提出了更高的要求，确实为无人驾驶汽车的发展起到了很好的推动作用；图1-1 DARPA无人驾驶汽车示意图为了赶上发达国家在无人驾驶汽车的研究水平，2009年6月，在浐灞生态区举行了中国首届“智能车未来挑战”比赛。这项比赛是国家自然科学基金会进行主办，由西安交通大学进行承办，得到了多个研究机构的共同支持，其中包括“视听觉信息的认知计算”重大研究计划指导专家组，IEEE智能交通协会（IEEE ITSS）和中国科学院自动化所。比赛参照2007年的DARPA比赛过程，经过角逐，湖南大学，北京理工大学和上海交通大学分获冠亚季军。2010年10月16日至18日在西安市长安大学举行了第二届“智能车未来挑战赛”。本次比赛由国家自然科学基金委员会主办，长安大学承办，广汽丰田汽车有限公司进行赞助。国家自然科学基金委员会“视听觉信息的认知计算”重大研究计划的总体目标之一就是研制具有能够感知自然环境感知并做出相应的智能决策的无人驾驶车辆，本届智能车比赛的目的是以比赛为载体，交流和验证视听觉信息认知和计算的研究工作成果。2011年10月20日至21日，第三届广汽丰田杯“中国智能车未来挑战赛”在内蒙古自治区鄂尔多斯市举行。本次比赛目的在于将主要研究成果体现在研制能够感知自然环境感知并做出相应的智能决策的无人驾驶车辆上，并以比赛的方式聚焦科学问题，检验相关研究的进展情况，推动该重大研究计划产生具有原创性重大研究成果。第四届“中国智能车未来挑战赛”于2014年11月15日至16日在江苏省常熟市举行。来自国内不同研发单位的22辆无人驾驶智能车辆参加了为期两天的比赛。诸多赛事的举办和参与机构的不断增多，都在说明无人驾驶汽车注定会成为未来发展的主要潮流，而对无人驾驶汽车能够顺利完成所规定的任务的路径规划研究毫无疑问也是今后的一个主流研究方向，能够根据给出的地图信息和目标点能够规划出安全可行的最优路径，将会对无人驾驶汽车的发展提供很大的帮助。1.2 国内外研究现状1.2.1国外研究现状在2007年Darpa Urban Challenge中各个单位针对城市环境下的无人驾驶的的路径规划提出了很多解决方案。CMU的BOSS采用全局规划局部规划的框架，利用A*算法在数字地图上进行全局规划。根据不同的航向前轮偏角和目标位置通过离线查表生成所有可能的最优路径。Standford的Junior将全局规划采用的是动态规划算法，局部规划把无人驾驶分为车道行驶和非结构化道路两类，车道行驶采用车道中心线作为最优路径，在非结构化道路上使用混合A*算法。MIT 的Talos车队 也采用了全局规划局部规划构架的规划算法。局部规划中使用快速扩展随机树 (Rapidly-exploring Random Trees，RRTs)算法在线生成可行路径。AnnieWay 使用触须算法进行局部规划，触须算法用圆弧模型描述车辆期望行驶轨迹, 一旦确定待执行路径，期望前轮偏角也就确定了。2014年，奔驰重走Bertha之路，在高精细地图上进行大范围的全局路径规划。然后局部路径规划通过求解路径代价函数的方法得到最优路经，最终生成了连续且满足车辆约束和环境约束的最优路径。1.2.2国内研究现状我国从2010年开始也每年举行无人车“未来挑战赛”，是各家单位展示与交流的平台，展现出很多优秀的解决方案。中科院合肥物质研究所采用A*算法进行全局优化，利用RRT进行局部路径规划再用贝塞尔曲线进行路径平滑。国防科技大学在局部规划中使用极坐标栅格图，利用A*算法进行局部最优路径搜索。北京理工大学则是基于模型预测原理（Model Predictive Control，MPC）进行局部路径规划。通过对车辆运动微分方程进行滚动优化的方法完成路径规划。其核心是求解多约束下的最优化问题，使得该方法能够在考虑环境约束和车辆约束的情况下生成一定范围内的最优路径。这里所提到的大部分全局路径规划算法，都是在数字地图及高精细地图下完成，并不适用于大范围的全局路径规划。而其中局部路径规划的方法都依赖于精确的车辆定位信息，而且时间效率较低，不具备鲁棒性和实时性的特点。1.4 路径规划关键技术路径规划给出了无人驾驶汽车从出发点到达目的地的行车轨迹和行车过程中的路径选择，是无人驾驶汽车能够安全快速的到达目的地和完成一系列的行车动作所必不可少的一个环节。路径规划既有使用离线地图进行路径选择的全局规划，又有从车辆自身周围环境出发的局部规划。全局路径规划能够使得车辆的导航在全局上具有全局最优的特性，而局部路径规划能够使得导航能够具备实时性，并且能够应对全局地图上不明确或者无法描述的环境情况，所以通过两者的结合能够使得车辆导航具备更好的可用性。图1-2 全局和局部规划系统示意图1.4.1 全局路径规划无人驾驶汽车的导航和移动机器人的导航重要的区别在于无人驾驶汽车的导航是基于大量的离线地图信息完成的，所以相比移动机器人，无人驾驶汽车能够在更加广泛的区域中完成导航任务，现阶段主要的基于离线地图信息的全局路径规划算法有：（a）A*算法A*是一种启发式搜索算法，通过设定合适的启发函数，从起点开始扩散性估计各个搜索节点之间的消耗，通过比较消耗的大小，选择最优的扩展点，直到找到目标节点位置4。优点在于所需扩展的节点较少，具有良好的鲁棒性，缺点在于实际运用中忽略了自身的体积对节点限制，但具有良好的改进空间。（b）Dijkstra算法Dijkstra算法是一种典型的路径最短算法，它以初始点为中心向外扩展直到终点为止。通过遍历所有节点得到最短路径，所以成功率高，鲁棒性好，但是遍历节点太多，效率低，大型复杂网络运用能力低，而且不能处理负边。（c）Fallback算法对Dijkstra的一种改进算法就是Fallback算法，主要用于多QoS（Quality of Service）要求的路径，目标函数以不同的QoS进行构造，运用Dijkstra进行路径搜索，根据QoS因素满足的情况重复算法5。优缺点和Dijkstra相同。（d）Floyd算法（弗洛伊德算法）需要寻找特定权值的拓扑路径网络中顶点之间的最短路径时，提出了Floyd算法，首先把路径网络简化成权值矩阵，之后进行两点间的最短路径寻找4。优点在于权值可正可负，并且起始点和终点之间的变化对算法的影响不大，效率高于Dijkstra，但是存在时间复杂度高，不适合大量数据的缺点。（e）混合A*（Hybrid A）混合A*是美国stanford大学在DARPA比赛中使用的A*算法的改进算法。主要改进在于将A*算法中的离散的坐标点改进为一系列汽车能够识别的连续坐标点，相比传统的A*算法混合A*有着更光滑的路径，能够被车辆更好的识别。另外还有最佳邻点启发式搜索（Best Neighbor Heuristic Search）算法，简称为B*算法，卡耐基梅隆机器人中心的Stentz在1994年提出的动态A*（Dynamic A*，D*）算法等。A*算法在搜索的工程中引入了一致的全局信息，对当前的节点到终点的代价进行了判断，作为评价节点是最有利路径的可能性，使得算法能够优先扩展节点，提高效率，如果没有全局信息，A*就退化成了Dijkatra算法。国内其他研究者也提出了很多机遇Dijkstra算法，A*算法等经典算法的路径规划算法的改进算法。如陆峰，卢冬梅等人提出的交通网络限制搜索区域时间最短路径算法，张连蓬，刘国林等人提出的GIS路径寻优的方向优先搜索法，陈艳艳，王东柱等人提出的分布式车载导航系统路线优化的有约束A*算法等。这些算法都对创痛的A*算法有了很多的改进。1.4.2 局部路径规划局部规划则主要是根据无人驾驶汽车的传感器感知信息综合的融合形成的局部感知信息，进行车辆周围行进的局部路径的规划。局部路径对环境反应迅速，但是不能全局收敛。现阶段常用的局部路径规划算法有：（a）人工势场法人工势场法是模拟引力斥力的作用下的物体运动，目标位置和运动物体之间产生的是引力，运动物体和障碍物之间产生的是斥力，通过建立引力和斥力场的函数进行路径的规划，优点是规划出来的路径安全并且平滑，简单，但是存在局部最优不是全局最优的问题，引力场算法的设计是应用成功的关键。（b）RRT（快速扩展随机树）法RRT（快速扩展随机树）法是是一种多维空间中有效率的规划方法。它以一个初始点作为根节点，通过随机生成一个节点，取出随机树中的离随机节点最近的节点根据步长生成树的子节点，最后生成一个随机扩展的树，当树中的叶子节点包含了终点或进入了终点所在的区域，便可以在随机树中找到一条由从初始点到目标点的路径6。优点是能够适应复杂环境，实现方法简单，缺点是随机产生点可能会出现局部最优情况。其他的局部路径规划算法还有CMU（卡耐基梅隆大学）基于模型预测路径生成器，利用模型预测轨迹生成初始和目标车辆状态的动态可行轨迹，适合高速的动态的环境，但是不适合非结构化的道路和复杂的环境。还有最优解算法，通过求解目标函数的最优解，二次的带有非线性不等式作为约束的非线性最优问题。适合于结构化的道路，动态特性好。1.5 主要研究内容由上述介绍可以知道路径规划在无人驾驶汽车中的重要作用，本文主要针对无人驾驶汽车路径规划问题进行研究，主要讨论和实现全局路径规划和局部路径规划，并作出相应的优化。其中在全局路径规划中主要研究OSM地图信息的输入和解码，使用A*算法对输入的初始点匹配最优路径，之后使用贝塞尔曲线进行路径的优化。对于局部路径规划部分，主要有两个方向的研究：一是如何使用改进的RRT算法实现路径规划，并对生成的路径进行后处理，最后使用贝塞尔和B样条进行路径优化。二是研究如何使用四阶贝塞尔曲线方法进行路径规划，以得到无需后处理的优良特性路径。文章具体的内容安排如下：第一章为文章的绪论部分，主要介绍了本文的研究背景和意义，讨论了路径规划的国内外研究现状并对相应的关键技术做了分析和说明，详细说明了路径规划的常用方法和分类，以及各种算法之间的优缺点。最后指出本文的主要研究内容。第二章为全局路径规划部分的原理和具体实现方法。针对实际的运用提出了使用OSM信息对起始点进行匹配方法，之后使用A*算法提供全局最优路径。然后对生成的路径在路口和U形弯道进行进一步的优化处理，并对路径信息进行补充以便之后使用，最后进行了实际实验。第三章为局部规划的四阶贝塞尔曲线规划的说明和实现。主要针对部分具有特殊要求的局部路径规划。并通过贝塞尔曲线自身的优良属性省去局部路径的后续处理和平滑，从而提高局部路径规划效率。第四章为局部路径规划的优化RRT算法的介绍和实现，主要对RRT算法进行了优化并做出相应的说明，并且对规划出的路径提出了进一步后处理和平滑方式的设计方案和实现，最后对两种局部路径的规划算法进行了仿真和实际的实验，并对比了两种规划方式的结果。第五章为全文的总结，提出了本文的创新点和取得的结果，并对将来的研究进行了展望。第二章 全局路径规划2.1 引言通过之前的介绍可以知道，本文将通过两个部分进行说明，一个是全局经规划，一个是局部路径规划。本章将对全局路径规划部分进行说明，针对如何使用地图信息，如何进行起点和终点的地图信息匹配，如何完成最优路径的选取和如何进行路径规划等问题进行了相应的讨论和研究。介绍了相关的基础知识，如OSM地图介绍，A*算法介绍等。提出了OSM地图的读取和存储方法和结构，改进的起始和终止点算法，A*算法的具体实现，路口规划的优化方法和道路信息补充方法。最后对实现的系统进行了介绍和实际结果分析。2.2 全局路径规划方法综述2.2.1全局路径规方法流程为了根据输入的任务点规划出一条全局最优的安全路径，使生成的路径能够应对复杂的交通道路并且能够提供详细的路径信息给局部路径进行路径规划，对全局路径规划系统的流程作如下设计：图2-1 全局路径规划系统流程图根据流程图可以看出系统的总体结构分为四个部分：地图信息的读取和处理，外部信息输入，路径规划和规划路径信息输出。地图信息的读取和处理主要是从外部读取记录了地图信息的OSM地图文件（.osm文件）之后根据地图信息文件进行相应的地图信息解码，包括了对地图信息的不同模块的信息读取和相应节点信息的存储。首先根据OSM原本自带的属性得出节点的GPS坐标和节点所在道路是单行道还是双行道的信息，之后读取出节点之间的连接关系并存储到列表中，确保之后的路径规划能够使用地图信息。初步信息存储之后，根据节点之间的连接信息和节点自身的坐标信息计算处理出节点的属性（直道点，路口点，起点，终点等）。根据连接信息还能提取出节点链接其他的节点数目等其他信息。地图信息读取完毕之后等待系统外部信息的输入，这个时候有两个模式的外部信息输入，一种是任务书的形式，一种是直接在地图上点击起点和终点，然后系统识别生成初步的地图信息。当外部信息输入完毕之后就能根据A*算法实际规划路径。路径规划完毕之后会进行初步的路径信息输出，这里系统将会以两种方式进行生成路径的展示，第一种为在实际的地图图像上进行路径的绘制，第二种是将详细的路径信息以TXT文件的形式详细的输出出来，并且对生成的路径进行相应的路口部分的优化。2.2.2 全局路径规划的算法选择首先在算法的选取方面，对于现在常用的几个路径规划算法：Dijkstra算法，贪心最好优先算法和A*算法中进行对比选取一个相应的算法。 Dijkstra算法简单解释就是从起点开始对其他节点进行查询，并将该节点放入到等待检测节点的集合中去，然后使用拉格朗日松弛算法更新等待检测的节点的路径长度值。一旦图中不存在负的权值的边，该算法就能够确保找到最短的路径。贪心最好优先搜索算法总体上和Dijkstra算法类似，不同点在于该算法对目标点距离有一个估算值（启发值）。相比Dijkstra算法在选择后续计算节点是在待检测集合中选择离起始点最近的节点，该算法使用的是距离终点最近的节点。该算法虽然不能保证寻找的路径是最优的路径，却能够大大提高路径搜寻的速度，因为该算法使用了启发式的方法引导了搜寻的走向。事实证明在没有障碍物的情况下，贪心最好优先算法能够更加快速的寻找到路径7。但是在有障碍物的情况下，Dijkstra依旧能够找到最优的路径，贪心最好优先算法不能。1968年提出的A*算法结合了贪心最好优先搜索算法和Dijkstra算法的优点。A*算法不仅有启发式算法的快速性，同时还能克服启发值无法保证最优的情况下依旧能够生成一条最优的路径。A*算法是目前应用最广泛的路径规划算法，因为根据启发函数的取值不同，A*能够生成不仅仅是路径最短的路径，还能生成时间消耗最少，转角最少等路径，拥有很强的灵活性，能够满足各种不同场景的需要。综合了Dijkstra算法所使用的偏向于距离起点较近的节点信息，和贪心最好优先搜索算法的启发信息（离终点较近的节点）是A*算法的成功之处。在讨论A*算法的时候，使用g（n）标示从起点到任意节点的路径消耗，h（n）标示从节点n到终点的路径消耗的估计值（启发值）。fn=gn+hn (2-1)f（n）称之为启发函数，A*算法将每次检测拥有最小的f（n）值的节点。2.3 基于A*的全局路径规划2.3.1地图信息的读取和处理开放街道地图（OpenStreetMap，简称OSM）是一个基于网络的地图协作的计划，目标是能够创造一个自由并且能够让所有人编辑使用的世界地图。OSM地图的灵感来自于维基百科，所以能够对注册用户提供对地图的编辑功能。OSM的地图信息的主要来源是用户提供的GPS信息，卫星照片和航拍等，甚至可以根据用户对区域的熟知程度和自身知识进行地图补充。网站中的地图图像和向量的数据全都根据共享创意姓名标示-相同方式同享2.0授权。图2-2 OSM网站实际效果图2004年7月史蒂夫克斯特创建了OSM，2008年8月后不久，第二届国家地图的会议举行，有超过五万的注册贡献者，2009年底有20万人，到2015年3月为止OSM已经有了超过200万的注册用户，根据OSM最新的帮助文档中的OSM活跃用户比例图表中能够看出OSM在2007到2008年之间达到了最高，之后活跃用户比例逐渐减少，这是由于OSM使用的用户逐渐增多。截止2015年2月23日，活跃用户维持在1%，大概2万人左右，而08年的比例最高峰活跃用户大概只有5500人。可以看出OSM地图的影响力正在增加。OSM为开发者提供了能够自己进行搭建的地图服务器，并且能够嵌入到开发者自己的移动设备，或者网站中等。OSM提供了地图的导出服务，可以导出的格式有：（1）OpenStreetMap XML：OpenStreetMap的xml格式（扩展名.osm）（2）Mapnik图像：片格式包含：JEPG、PNG、SVG、PDF、Postscript（3）Osmarender图像：片格式(png)本方案主要使用了OpenStreetMap的XML作为全局路径规划的地图信息。因为可扩展标记语言（Extensible Markup Language，XML）是一种用于标记电子文件让它具有结构性的标记语言，这些标记语言将文档分成许多部件并对这些部件加以标识，相比传统的图像信息，XML能够更加精准明确的声明地图内容，方便跨越多种平台实现更有意义的搜索。它提供了一种描述地图数据的格式，简化网络中的数据交换和标示，使得代码、数据和标示分离，并作为数据交换的标准格式。XML兼容现有的协议，能够很容易的通过HTTP协议传输。并且由于数据的结构固定，所以能够较为容易的编写地图信息的解调程序，成功准确的读取地图信息并且结构化存储以便之后的全局路径规划。首先，系统使用pyhton语言编写，使用python自带的XML解码函数解码OSM地图。根据解码分类提取“node”的行可以提取到地图信息中的节点信息，其中“id”作为节点的标示信息，作为查询表的编号信息，“lat”，“lon”作为节点的经纬度信息存储。即nodesid=（lat，lon）进行nodes表格建立。本文所使用的是上海交通大学的OSM地图信息，该地图信息包括了1282个可用路径节点，即不包含建筑物等节点，共有1488个路径关联。图2-3 OSM地图节点信息之后解码节点之间的链接关系，提取关键字“way”之后就能够提取到道路包含的节点的编号信息，根据不同的道路将道路分别存储到“highway”，“railway”和“oneway”中，并根据相应的属性提取出可循环和可逆道路两种属性，之后根据属性将两个节点以父亲和孩子节点的形式存储到检查表中，并且在添加链接信息中间根据道路的可逆性对nodes表格进行单双道路的信息补充。图2-4 链接信息表routing结构示意图存储完节点的链接信息之后，为了获取节点的自然属性，还要对routing表进行遍历以添加相关信息。对routing表遍历过程中根据node的子节点个数可以将节点能够连接几个其他节点的属性进行存储，并且把node的自然属性区别出来：当node 有三个及以上的linked node的话，即可确定这个节点为一个路口节点；当没有linked node 的话一般作为缺失，在存储的时候以-1标示；特殊情况在于有两个linked node情况，这种情况可能为直道的节点，也可能为路口节点（单向转弯路口）。所以对这种节点要采取计算节点转角来判断。以两个linked node作为角的两边点，node作为角的顶点计算它们的角度，作为是直道还是路口的判断依据。node=x0,y0 (2-2)node1=x1,y1 (2-3)node2=x2,y2 (2-4)cos=x1-x0)(x2-x0+(y1-y0)(y2-y0)(x1-x0)2+(y1-y0)2(x2-x0)2+(y2-y0)2 (2-5)angle=cos-1cos (2-6)其中angle既是三个点的夹角，根据夹角判断，当夹角大于100度的时候即为直道的节点，其他的角度的就是单向转弯的路口节点。截止到现在，对地图的初步信息处理就能够完成节点的经纬度，连接节点数，节点在单双道上和节点的自然属性进行存储，并且为路径规划提供了所需的查询列表。2.3.2 起始点匹配任务点的信息输入就是向系统提供所需的起点和终点的经纬度坐标，以及部分属性信息（部分信息可缺省），之后进行再OSM地图中选取相应的目标节点。传统的目标节点的选取方式是通过计算选取点到各个节点的距离，距离最短的作为路径规划的起点和终点。但是遇到如图所示情况就会出现路径规划的不合理。图2-5 规划路径不合理情况示意图当一条路是完全直道的时候，存储的节点就只有道路开头和结尾部分，中间没有其他的节点，如果按照最短距离搜索，就会出现图中所示蓝色虚线所示的路径，有一段不合理的规划，而不是按照合理的绿色实线的规划，为了避免这种情况的出现，本系统采用了更加合理的搜寻方式：在选取规划的初始节点的时候，改进的算法就是把单纯的计算节点到选取点最短距离变成计算选取点到两个链接的节点构成的道路之间的距离，这样就能避免之前出现的规划不合理的情况。改进的方法处理的点分别为：选取点，遍历routing表的节点1和它的子节点（节点2）。根据图2-6可以看出能通过计算三个点构成的三角形的最长斜边在两个节点构成的边上的投影长度和节点间的距离之差来判断垂足是否在节点之间的连线上，若投影长度小于节点之间的距离说明垂足在节点之间，则在两个节点之间插入新的节点作为起始终止点，并将选取点到两个节点构成的道路之间的距离作为判断距离；若投影大于节点之间的距离，则垂足在节点之外，这时候不新添加节点，将两个节点中距离选取点最近的节点作为待定节点，并将他们之间的距离作为判断距离。对于垂足在节点之间的情况，由于要在所建立的nodes表和routing表中新添加一个节点信息，并添加相应的连接信息才能够在路径规划算法中作为起始点和终点被使用。所以在这种情况中，建立一个新的node之后，根据相邻的两个节点的属性对其添加相应的单双道路属性，直道属性，链接关系根据单双道属性分别连接到两个节点。（a） （b）图2-6 改进的起始终点选取示意图2.3.3 最优路径选取采用A*算法对给出的起始点和终止点规划出相应的最优路径，A*算法的具体实现为：表2-1 A*算法伪代码编号 伪代码1： A_star_search（）2： 3： queue=，CLOSED=起点ID，searchEnd=终点ID,4： blank_node=“end”：-1，“distance”：0，“nodes”：起始点的ID 5：for i 为起点的子节点6：计算i的启发函数f（i）的值7：queue.insert（i）8：对queue按照f值从小到大排序 9：while(未超时)5： 6： 从queue表中取出第一个节点X，并把X从queue表中去除7： if（X是终点）8； 9： 从终点回溯到起点生成路径path，返回path列表10：11：for（取出X的每个子节点Y）12：13：if（Y不在queue表也不在CLOSED表中）14：15：求出Y的f（n）值，并将Y加到queue表中/未进行排序16：17：else if（Y在queue表中）18：19：if（Y的f（n）小于在queque表中的值）更新queque表中Y的f（n）值20：21：else/Y在CLOSED表中22：23：if（Y的f（n）小于在CLOSED表中的值）24：25：更新CLOSED表中Y的f（n）值26：把Y从CLOSED中去除并加到queque表中27：28：把X放入CLOSED表中29：把queque表中的节点按照f（n）从小到大排序按照伪代码就可完成对地图信息进行搜索的A*算法。其中，要是出现任务书的模式，则在程序外新建一个存储任务书的总路径的查询列表。2.3.4 路径信息输出作为系统较为关键的一个部分，规划路径的详细信息输出的实现将在这个部分做出说明。（a）文档信息输出:经过路径规划部分之后规划的路径存储在一个查询表中，现在要将该部分信息输出到外部的TXT文档中。表2-2 路径信息输出格式num lon lat oneway/cyclecharacter linked nodes anglespeed1121.43747531.027602 N 0 N 0 N由表中可以看出，输出的道路信息主要包括：路径点编号（num），经纬度坐标（lat/lon），单双道属性（oneway/cycle），自然属性（character），链接节点个数（linked nodes），转角（angle）和理论速度（speed）。其中缺失部分信息使用“N”显示，信息缺失主要是由于节点不是OSM地图上的点，无法计算相应属性。单双道属性主要显示该节点是在单行道上还是双行道上，节点属性值为：1标示单行道，2标示双行道，-1信息出错。自然属性包括：0表示未知(包括行驶方向和交通标志两者均未知)，1表示直行，2表示右转，3表示左转，4表示掉头，5表示有交通标志。链接节点个数主要表示节点和几个节点相连接，结合节点的自然属性就能知道路口节点链接了几条道路等相关信息。路径转角是根据公式2-2到2-6可以计算出路径点之间的夹角，用180度减去这个夹角之后就是沿着一边转向到另一边的夹角。对于理论速度的计算，根据已有的研究8推算出的转弯速度和转弯半径之间的关系的公式对数拟合公式如下：V=2.562+8.740lnR （2-7）根据之前算出的道路夹角和三个路径点信息再根据正玄定理可以计算出理论的转弯半径，计算公式如下：R=d2*sin （2-8）其中d为夹角所对的对边长度，为三个节点之间的夹角。根据公式2-7和公式2-8即可计算出理论的速度取值。 (b) 图片信息输出根据存储好的路径信息文件，并根据地图和上实际坐标之间的关系，就可以得到实际的GPS坐标点在给出的地图上的相对坐标点，之后根据规划的道路信息在图片上进行路径绘画，最后进行图片的存储和显示。2.4 基于贝塞尔曲线的路径优化本部分将讨论如何在路口和U形弯道对规划的路径进行相应的优化处理。图2-7 传统路口道路规划模型可以看出，传统的模型在路口转弯的规划上有很生硬的直角，往往这种直角对于无人驾驶车辆是无法完成转弯的，并且弯道在很深入路口的时候才进行转弯，对无人驾驶汽车的后续工作，包括道路识别，交通标志识别等都会造成很大的影响。为了解决这个问题，本系统采用贝塞尔曲线平滑的处理方式来对路口道路的规划进行优化。根据之前存储的节点的自然属性，可以很轻易的看出节点中存在的路口点，基于这个节点往前和往后推算一个节点就是路口的进入和出口点。根据这两个节点和他们之前之后的点就能够对路口规划进行相应的优化。图2-8 路口平滑原理示意图如图所示，最终用来进行贝塞尔平滑的节点是路口的出入点和相应的补充点，采用有理贝塞尔曲线进行平滑处理。优化路口的路径规划，并且因为使用有理贝塞尔曲线进行优化，所以可以根据实际情况对有理贝塞尔曲线的权值进行调整，对路口转弯进行不同转弯半径的路径规划，从而更加符合无人驾驶汽车的运动学特性。对U形弯道也可以采用类似的方式进行优化：图2-9 U形弯平滑原理示意图如图可以看出对U形弯道也有良好的优化性能。2.5 实验结果与分析首先是路径规划系统实现的简单的用户界面（User Interface，UI）:图2-10 全局路径规划系统UI本系统提供了较为简单的用户交互界面，主要体现了整个系统的基础功能，由图可以看出系统包含了任务书规划模式和单独输入起点和终点模式。（a）任务书形式输入：任务文件采用“中国智能车未来挑战赛”的任务书格式。任务文件是以点序列的形式提供，用于引导车辆形式的检查口的关键点，数据用ASCII码的格式保存，具体形式如下表所示：表2-3 试验用任务书内容编号 纬度 经度 高度 属性1 属性21121.43747531.0276020002121.43855331.0265290303121.44030431.0264490304121.44128231.0294770305121.44080331.0298450306121.43763031.0288290307121.43747531.027602070（1）编号序列从1开始（2）属性1中的编号主要表示了这个节点的自然属性，包括了：1表示起始点，2表示普通点，3表示路口点，7表示终点（3）属性2表示了该节点行驶方向或有无交通标志，0表示未知(包括行驶方向和交通标志两者均未知)，1表示直行，2表示右转，3表示左转，4表示掉头，5表示有交通标志。（4）经、维度单位为度，数值精确到小数点后6位；高度的单位为米，数值精确到小数点后三位。对任务书进行读取之后进行以下步骤：步骤1：判断任务书中的节点的属性1，若为起点则进行起点的关键点信息存储，并进行起始点的节点搜寻，最终存储路径规划的起始点ID；若为终点则发出结束程序信号；若为途中点，测执行步骤2；步骤2：对途中点进行节点搜寻，最终存储路径规划的终点ID，然后进行一次路径规划，之后吧起始点ID换成现在的终点ID；步骤3；重复以上步骤。按下UI上的taskfile route按钮，即可开始对任务书进行路径规划，任务书的基础路径在程序内部进行输入，之后根据实际运用会进行进一步修改。图2-11 任务书模式运行结果点击按钮之后，系统开始对任务书的内容进行读取，并进行相应的路径规划，路径规划完成之后会在原本的地图上对规划完成的路径进行绘制并给出新的窗口进行显示。同时会对生成路径的OSM地图ID标号进行输出：在规划过程中由新的搜寻节点创建生成的点，即编号为66666666和附近编号的节点的周围会出现重复情况，这是由于对任务书分段进行规划造成的，这个错误在路径的详细信息输出过程中进行了相应的优化处理。表2-4 规划路径的详细信息num lon lat oneway/cycle character linked nodes angle speed1121.43747531.027602N 0 N 0N2121.43747331.0275802 3 234.75690532.9514023121.43764631.0272752 3 28.86069749.3533654121.43783431.0270382 3 29.20323347.2620965121.43800531.0268822 3 25.34596651.8060216121.43823731.0267072 3 28.73873748.9858247121.43852531.0265522 3 29.31848343.7482268121.43855731.0265412 5 289.4645417.7565879121.43855331.026529N 3 N91.31943927.05371310121.43886831.0264322 5 22.26900657.87994611121.43897631.0263942 3 210.5871054643939631.0263292 3 26.18298152.37459213121.43961531.0263192 5 20.02092399.98645114121.43990231.0263062 3 321.02845537.12293115121.43998931.0263352 5 20.98776054.302207由表格可以看出，规划出的路径信息，完整包含了路径的顺序，GPS坐标是否是单双道，节点属性，链接的结点个数，预计转角和理论最大速度都进行了计算和输出。为无人驾驶汽车使用提供了充分的信息，并且根据进一步需求可以进行更多信息的计算和显示。由结果可以看出，部分节点的属性中会出现“N”的缺省值，一个是由于该节点是起点或者终点，不具备相应属性，第二个是该节点是任务书内的节点，不是OSM地图上节点，无法对其链接性和单双道等信息做出判断所以缺省。其中在任务书节点周围会出现两次较大的转角，之所以会出现这种情况是因为任务书的节点和地图中原本的节点存在偏差并且任务书中的点必须被经过。具体说明如图：图2-12 转角情况说明对于这个问题采取的解决方式是对任务书节点的附近去除第一个出现大转角的节点，就能避免这种情况的出现。（b）单独输入起点和终点形式：图2-14 单独输入起点和终点的选取图片(分辨率：L*H)通过用户在显示出来的图片上选取一个起点和一个终止点，然后系统获取相对这个图片本身的坐标x1,y1和x2，y2。然后根据选取地图图片的左上角GPS坐标（LON1，LAT1）和右下角GPS坐标（LON2，LAT2）通过以下计算就能将实际选取的起点和终点的GPS坐标计算出来：LON=LON1+x(LON2-LON1)L (2-15)LAT=LAT1-y(LAT1-LAT2)H (2-16)最终用户所选取的点的GPS坐标为（LON,LAT）得出所需的起点终点之后就是确定在OSM地图的节点中选取合适的开始进行A*路径规划的起点和终点。要运行直接输入起点和终点模式则是在UI上显示的实际地图上使用鼠标左键选取起点，鼠标右键选取终点，选取完毕后即可开始路径规划。图2-15 单独输入起始终止点模式运行结果表2-5 规划路径的详细信息num lon lat oneway/cyclecharacter linked nodes angle speed1121.43452431.029601N N N0N2121.43451731.0295682 5 260.09594817.2434623121.43444931.0295462 3 496.69564516.7148794121.43448231.0294742 5 20.13925093.5089045121.43511531.0280842 5 20.10784695.6937676121.43514431.02