无人车路径规划子系统设计（A算法）

目录TOC\o"1-3"\h\u243741绪论 绪论近年来，无人车的路径规划逐渐成为无人车研究的基础和核心方向。它为几个学科的技术研究提供了一个更广阔的平台。对于路径规划算法的研究，在理论仿真的基础上，增加了车身本身的方向性，实现了智能小车的路径规划方案。路径规划不仅是无人车的智能化的代表，而且是无人车自主导航的基础。以自行设计搭建的无人驾驶汽车为实验平台，研究了路径规划算法，逐步总结分析了Dijkstra算法等常用路径规划算法的优缺点。最后，对A*算法进行了选择、掌握和实现。在理论和仿真相结合的基础上，首先对环境进行栅格化，然后通过车身子上的电子罗盘获得更加准确的实时方向，再通过A*算法设定车辆转弯策略。最后，实现了无人车的路径规划策略。大多数人认为无人驾驶汽车是21世纪才被发现的，但工程师们从20世纪初就开始研究它们，在可行性和实用性,并取得了很大的进步,所以研究和开发开始比我们想象的更早。国外发展史通过无线电去控制三轮车20世纪初——无线电遥控根据“UnmannedSystemsofWorldWarsIandII”的作者埃弗里特（HREverett）的说法，第一辆无人车于1904年西班牙发明家莱昂纳多·托雷斯·奎韦多（LeonardoTorres-Quevedo）制造的无线电遥控三轮车。并且在第一次世界大战中，军队使用了无线电控制的各种小型车辆来运送弹药或引爆火药。到了1925年某天的美国纽约巷子里，当地市民看到了一辆无人驾驶的福特T型汽车在百老汇大街上徐徐行驶着，这个想法是用天线接收无线电信号来控制汽车的速度和方向。这是一辆由前美国陆军工程师霍迪纳开发的无线电控制汽车。无线电控制的汽车在当时引起了很多关注，被媒体称为“美国奇迹”，尽管它有几个版本的改进，但没有实际意义的进展。图1.2.1早期无线电控制的无人车到了1932年，一辆由工程师“控制”的无线电遥控汽车失控冲进人群，造成10多人受伤，这件事情使该工程师被逮捕，之后就使无人驾驶汽车退出了人们的视野。用线圈引导控制汽车1957年，美国77号公路上的一辆雪佛兰汽车通过在道路上测试自动驾驶汽车的原理下使用线圈进行导航控制，其原理是利用铺设在道路上的电线进行方向控制。当时的自动驾驶实验是由汉考克领导的，他是国家交通工程师汽车电子设备制造商RCA邀请的，电子设备制造商RCA受邀为车辆实现自动化。实现项目的自动化概念来源于诺曼•贝尔在1939年的世界博览会上向未来城市的想法与通用汽车公司的诺曼·贝尔·格鲁曼·迪斯合作提出了未来城市规划的概念，城市中的所有车辆都将由无线电控制。同时为车辆提供电力和操作。图1.2.2线圈引导控制汽车的概念蓝图为测试轮胎可靠性的“无人驾驶汽车”图1.2.3无人驾驶汽车图1.2.4控制台1968年，德国汽车制造商大陆集团（ContinentalGroup）为了测试轮胎的可靠性，提出了“无人驾驶汽车”的实验。该实验在德国康德.德罗姆（KantDerome）的测试轨道上进行，由西门子、西屋电气、慕尼黑大学和达姆施塔特大学的研究人员共同完成。当汽车转弯时，传感器会向系统发送警报信号，引导汽车回到原来的位置，而控制站可以引导汽车刹车和加速。带有传感器的汽车到了1994年，德国工程师迪克曼斯为了做实验，塔将将两辆轿车停在法国巴黎的道路上，那个轿车有一个车载电脑系统，可以改变车行驶道路、油门和刹车。而早在1986年，迪克曼斯曾经给一辆奔驰货车配带电脑、摄像头和传感器，并使其能接收道路标记和信号等信息，并且能够在实际情况的公路上驾驶。图1.2.5原视频截图电脑和机器视觉1946年，在美国宾夕法尼亚大学发明了世界上第一台电子计算机“电子数字积分计算机”。20世纪60年代，计算机科学技术开始迅速发展壮大，使计算机变得更小、更加可靠、功能更强大，它能完成很多复杂的任务：如图像处理、运算等，这为无人驾驶技术的发展创造了条件。1977年，日本筑波工程研究实验室的S.Tsugawa和他的同事开发了第一种使用摄像头测量导航信息的自动车辆——一种内置两个摄像头和模拟计算机技术进行信号处理的车辆。它能够达到每小时30公里，但是在高架轨道的帮助下，这是我们所知道的第一个使用视觉设备的无人驾驶实验，这开启自动驾驶的新篇章。图1.2.6自动控制车国内无人车发展史关于无人驾驶汽车的发展，与国外相比而言，我国起步较晚。我国自20世纪90年代初期开始研究陆地自主车。当时是以军用平台为背景，在智能移动机器人技术框架下发展。当时，国家“863”计划对智能移动机器人的主题方面研究进行遥控驾驶防核化侦察车研究。“八五”期间，在总装备部（原国防科工委）带领下，由北京理工大学、国防科技大学、南京理工大学、清华大学、浙江大学、哈尔滨工业大学组成团队，开发出我国第一辆陆地自主样车ATB-1(AutonomousTestBed-1)，也可以说是7B.8军用智能机器人平台，目前放置在北京理工大学。在“九五”时候研发出第二代陆地自主车ATB-2，然后又研制成第三代自主车，现在正在准备第四代自主车的研制工作。军用陆地自主车的研究为我国自主车的研究积累了关键技术，培养了人才队伍。贺汉根、杨静宇、何克忠、付梦印、陆际联、刘济林、朱淼良等教授为此做出了巨大贡献。从2009年开始，国家自然科学基金委为了研究具有感受自然环境与智能与决策相结合的无人驾驶汽车验证平台还专门成立了“视听觉信息的认知计算”重大研究计划组，把研究的这个平台作为其科学目标。直到2014年，已连续六年开展“中国智能车未来挑战赛”，使我国无人驾驶汽车的研究发展更上一层楼，这与李德毅院士、郑南宁院士以及专家组教授们的大力推动是分不开的。我国的一些自己创建的汽车品牌正在向无人驾驶、自动驾驶领域进军，并增强在这个领域的探索。由国家工信部允许的国内第一个国家智能网联汽车试点示范区域在2016年6月的上海嘉定正式投入建设。目前开放的封闭测试区为无人驾驶汽车提供综合性测试场地和功能要求。目前国家正在为无人驾驶相关技术规范积极推动做出努力。虽然我国涉及无人驾驶汽车领域时间相对其他国家晚了一点，不过专业人士认为，在更好的管理和消费环境下使中国可能成为无人驾驶汽车的主要市场之一。据波士顿咨询公司预计，到2035年全球无人驾驶汽车的可以卖掉将达1200万辆，其中中国出售将超过四分之一。按这样发展10年后，中国遍地都将是无人驾驶汽车并占据全球汽车领域80%的份额。近代发展方向（无人车路径规划（A*算法）方向）无人车作为研究的热点问题之一，根据我国现在的无人驾驶汽车的发展状况，分析无人驾驶汽车现在及将来还将处于研发的初级阶段，国内无人驾驶汽车量产时间最早约出现在2022年，因此2018-2021年中国无人驾驶汽车行业主要还是先搞技术，以技术研发为主要目的。估计随着国内外技术成熟度的大幅度升高，国内各车企会一直提高参与无人驾驶汽车研发的积极性。据相关权威公司估算，到2035年全球无人驾驶汽车销量将达到2100万辆左右，而我国市场的份额约占24%左右也就是约为504万辆，其市场前景非常可观。而无人车研究的路径规划本来也是为了实现无人车，它从出发点到达目的地的行车轨迹和行车过程中的路径是选择的最优或者近似最优的无碰撞路径。因为所处环境信息的不同，路径规划方案分为两种：一种是基于已知的、静态的全局无人车路径规划方案；另一种是基于未知的、动态的局部无人车路径规划方案。全局路径规划让车辆的导航在全局上是全局最优，而局部路径规划能够使得导航更具备实时性，并且能够对全局地图上不明确的状况或者无法描述的环境状况进行处理，所以通过两者的结合，让车辆导航具备更好的可用性和发展性。作为一名对无人小车有着浓厚兴趣的初学者如果直接入手英伟达的Jetbot则开发门槛较高，于是我们选择了对Jetbot做了较深度开发推出的JetbotAI智能机器人平台，本文中也将通过该平台搭配详细的教程对Jetbot机器人开发环境搭建系统进行学习。

需求分析与构架设计通过前面的介绍，我们可以知道路径规划使得车辆导航具有最优的特性，并且能够实时导航，并且能够应对地图上不清楚的情况，或者不可描述的环境条件，因此通过两者的结合，可以使车辆导航具有更好的可用性。本章将介绍全局路径规划，如何利用地图信息，如何进行起点和终点之间的地图信息匹配，如何选择最优路径以及如何进行路径规划等问题进行讨论。介绍了相关的基本理论，如OSM地图介绍，A*算法介绍。提出了OSM地图的读取方法和存储方法及结构、改进的起点和终点算法、A*算法的具体实现、交叉口规划优化和道路信息补充方法。最后，介绍了系统并对实际结果进行了分析。需求分析路径规划旨在实现无人车从起始状态到目标状态的一条最优或者近似最优的无碰撞路径。在无人车路径规划问题上，首先应解决对环境的建模问题，其次应解决的是寻找合适的算法实现路径规划策略。全局路径规划与局部路径规划常使用的路径规划算法有Dijkstra算法、A*算法、Fallback算法等。本文将重点从栅格法构建的地图中用A*算法模拟无人车行驶的路径。全局和局部规划系统的综述为了给输入任务点设计一条全局最优的安全路径，使生成的路径能够更好地处理复杂的交通道路，为局部路径规划提供详细的路径信息，全局路径规划系统的流程设计如下图2.2.1：图2.2.1全局和局部规划系统示意图全局路径规划无人车和移动机器人的导航主要差异是无人车的导航是在大量的离线地图信息基础上完成的，无人车比移动机器人更能在广阔的视野中完成导航任务，现阶段主要的基于离线地图信息的全局路径规划算法有：(a)A*算法A*是一种启发式搜索算法。通过设置合适的启发式函数，从起始点开始估计每个搜索节点之间的消耗量。通过比较消耗的大小，选择最优扩展点，直到找到目标节点的位置。优点是需要扩展的节点少，稳定性好。缺点是在实际应用中忽略了自身对节点体积的限制，但有很好的改进空间。(b)Dijkstra算法Dijkstra算法是一种典型的路径最短算法，从起始点向外延伸到终点。遍历所有节点可以得到最短路径，成功率高，稳定性好。然而，遍历过多的节点会导致效率低下、大型复杂网络的应用能力低下以及无法处理负边。(c)Fallback算法Dijkstra的一种改进算法是Fallback算法，主要用于多条需要服务质量(QoS)的路径。用不同的服务质量构造目标函数，用Dijkstra搜索路径，根据服务质量因子重复算法。优缺点和Dijkstra一样。国内其他研究者也提出了许多基于Dijkstra算法、A*算法等经典算法的路径规划算法的改进算法。比如冯路、陆冬梅等人提出了交通网络有限搜索区域的最短路径算法，张连鹏、刘国林等人提出了GIS路径优化的方向优先搜索法，陈燕燕、王东柱等人提出了分布式车辆导航系统路径优化的约束A*算法。这些算法对外伤的A*算法做了很多改进。局部路径规划局部规划主要是基于综合融合无人车的传感器感知信息形成的局部感知信息来规划绕车行驶的局部路径。局部路径对环境的响应速度很快，但不会在全局范围内收敛。目前常用的局部路径规划算法有：（a）人工势场法人工势场法是模拟引力斥力的作用下的物体运动，目标位置和运动物体之间产生的是引力，运动物体和障碍物之间产生的是斥力，通过建立引力和斥力场的函数进行路径的规划，优点是规划出来的路径安全并且平滑，简单，但是存在局部最优不是全局最优的问题，引力场算法的设计是应用成功的关键。（b）RRT（快速扩展随机树）法RRT（快速扩展随机树）法是一种多维空间中有效率的规划方法。它以一个初始点作为根节点，通过随机生成一个节点，取出随机树中的离随机节点最近的节点根据步长生成树的子节点，最后生成一个随机扩展的树，当树中的叶子节点包含了终点或进入了终点所在的区域，便可以在随机树中找到一条由从初始点到标点的路径。优点是能够适应复杂环境，实现方法简单，缺点是随机产生点可能会出现局部最优情况。其他的局部路径规划算法还有CMU（卡耐基梅隆大学）基于模型预测路径生成器，利用模型预测轨迹生成初始和目标车辆状态的动态可行轨迹，适合高速地动态的环境，但是不适合非结构化的道路和复杂的环境。还有最优解算法，通过求解目标函数的最优解，二次的带有非线性不等式作为约束的非线性最优问题。适合于结构化的道路，动态特性好。构架设计A*算法是目前应用最广泛的路径规划算法，因为根据启发函数的取值不同，A*能够生成不仅仅，是路径最短的路径，还能生成时间消耗最少，转角最少等路径，拥有很强的灵活性，能够满足各种不同场景的需要。我们将利用matlab来对我们的A*算法进行实验仿真与验证：图2.3.1matlab桌面快捷图标基于栅格构图下的A*算法最短路径规划无人驾驶车辆的环境是通过网格建模方法,如下图所示,和起始点和目标点G的无人驾驶车辆设置。一个最优或近似最优无碰撞路径是通过算法选择的起点和终点之间的G，A*算法作为一种典型的启发式算法,人工智能的搜索路径问题有着广泛的应用,它将整个结构放入网格环境中，通过设置起点S、目标点G和障碍物B(灰度部分)，并通过相应的评价函数对最优路径进行分析，最终找出最优路径。图2.3.2路径规划建模A*算法实现最佳路径规划的基本思想A*算法作为一种启发式算法，通过构造栅格信息环境，每个栅格代表一个节点，并设定移动栅格大小，其中水平移动和竖直移动一个栅格的对角线=√2。其设定原理是根据勾股定理即设定直角边a，直角边b，若a=b=2，则斜边为2√2。如下图2.3.3所示：图2.3.3示意图设定起点S、移动距离V、目标点G和障碍物B（深色区域）。当把无人车看作质点，其从起点移动时，进行路径搜索。第一次搜索中VS－5=√2（VS－5表示从栅格S到栅格“5”的距离），VS－6=1，由于VS－6＜VS－5，所以将5号栅格作为下一个父节点。在第二次搜索时，由于VS－5=√2，V5－6=1，VS－6=14，即VS－6＜VS－5+V5－6，所以舍弃5号节点，选择6号节点作为最优节点，并继续重复执行其搜索过程。A*算法通过将死节点外的以及需要进行对比的节点放到一个表中，然后对所有节点进行比对，获得一个“最优节点”，实现全局范围内“最优节点”的评判，进而完成路径规划策略。文章具体内容安排第一章为文章的绪论部分，主要介绍了本文研究智能小车的意义与背景，提到了路径规划的国内外研究现状并对提到了相应的关键技术（全局路径规划与局部路径规划）。第二章为全局路径规划、局部路径规划进行了初步分析且详细说明了路径规划的常用方法和分类，以及各种算法之间的优缺点。并对于这两种规划采取了较为中间的选取栅型构图下的A*算法最短路径规划的原理和具体实现方法。第三章即开始对于前面提出的选取栅型构图下的A*算法用matlab进行实验仿真与验证。第四章从我们在matlab中完整的模拟仿真结果中的A*算法方式通过JetbotAI智能机器人平台应用在Jetbot小车中实践。第五章对全文的总结，提出了本文的A*算法在matlab以及Jetbot小车取得的结果。路径规划A*算法的构建及matlab实验仿真算法构建和分析A*算法是一种启发式搜索算法，引入了最优启发式函数，可以在搜索过程中避免盲目性，其表达式如式①:f'(n)=g*(n)+h*(n)①其中路径是由各个节点组成，可通过节点集合｛d1，d2，d3...dm...dn｝表示，dl表示起始节点，dn表示为当前节点，dm表示目标节点。f*（n）为代价函数，g*（n）是起始节点到当前节点的最短距离，如果g*(n)=0，代价函数所表示的算法是最佳优先搜索算法。h"*（n）是当前节点到目标节点的最短距离，如果h*(n)=0，代价函数所表示的算法是Dijkstra算法。目前是无法通过该节点预知该节点与目标节点的最短距离，因此使用h（n）代替h*（n），因此替换过后的代价函数表达式如式②:f(n)=g(n)+h(n)②H（n）选取原则为h（n）的值不大于h*（n），通常使用欧式距离计算公式、曼哈顿距离计算公式和切比雪夫距离计算公式，其对应表达式分别是公式③、④、⑤:③④⑤matlab程序的编写A*算法在搜索中生产两个表：“Open”表和“Close”表。Open表保存了所有已生成而未被检查的节点，Close表中记录已被检查过的节点。算法中有一步是根据估计函数重排Open表中的节点，这样，循环的每一步只考虑Open表中状态最好（代价最小）的节点，如果被发现在Open表中存在同一个节点，就应比较两个节点代价的大小，如扩展得到的新节点代价大于已有的节点代价，可以放弃扩展得到的新节点，否则就以新节点代替原来的节点。如果在Close表中存在同一个节点号，则新生成的路径的代价肯定大于原来路径的代价，可以淘汰新生成的节点。主要实现功能：确定起始点、目标点；定义一个地图，并生成一些随机障碍物；调用A算法函数，A函数返回值为路径；绘制地图、地图障碍物、起点终点、路径图3.2.1A*算法流程图图中的O为Open表的缩写，C为Close的缩写为了让大家更加直观地容易理解该流程图，下面我会举个例子假如此时障碍物以及起点S，终点G已经生成，现在我们将从S—G。如下图3.2.2：图3.2.2初始状态：设置节点S的f(x)=S，节点S进Open表，设置Close表为空。Open:S；Close：；估算节点S，考察与节点S相连的所有节点（有节点0，1）是否是目标节点，如果不是则估算这些节点的f（x）值，将其放进Open表，同时节点1被放入Close表；最后需要将open表中的节点按f（x）的值从小到大的顺序排列。Open:1；Close：S；从Open表取出f（x）值最小的节点（节点1），考察与节点1相连的所有节点（节点2，3）是否是目标节点，如果不是则估算这些节点的f（x）值，将其放进Open表，同时节点1被放入Close表。最后需要将open表中的节点按f（x）的值从小到大的顺序排列（由于节点2比节点4接近目标节点，因此节点2的f（x）值小于节点4的f（x）值，节点2排在节点4的前面）。Open:2，3；Close：S，1；从Open表取出f（x）值最小的节点（节点2），考察与节点2相连的所有节点（节点4，5）是否是目标节点，如果不是则估算这些节点的f（x）值，将其放进Open表，同时节点2被放入Close表。将open表中的节点按f（x）的值从小到大的顺序排列。Open:4，5；Close：S，1，2；从Open表取出f（x）值最小的节点（节点5），考察与节点5相连的所有节点（节点7）是否是目标节点，如果不是则估算这些节点的f（x）值，将其放进Open表，同时节点5被放入Close表。将open表中的节点按f（x）的值从小到大的顺序排列。Open:8；Close：S，1，2，5；...依次类推直到：Open:G；Close：S，1，2，5，7，8，9，11，13；从Open表取出f（x）值最小的节点（节点4），考察与节点4相连的所有节点（节点5）是否是目标节点，如果是则将节点4被放入Close表，然后将考察的节点（节点5）放入Close表，此时发现一条从开始地到目的地的路径。Open:；Close：S，1，2，5，7，8，9，11，13，G；当判断到G已经存在于Close表中后，将Close表中的点进行排序并连接起来作为最佳的路径规划如下图3.2.3：图3.2.3伪代码的编写根据以上思路，A*算法的伪代码如下：A*(){Open=[起始节点];Closed=[];while(Open表非空){从Open中取得一个节点X，并从OPEN表中删除。if(X是目标节点) { 求得路径PATH； 返回路径PATH； }for(每一个X的子节点Y){if(Y不在OPEN表和CLOSE表中) {求Y的估价值；并将Y插入OPEN表中；} //还没有排序 elseif(Y在OPEN表中) {if(Y的估价值小于OPEN表的估价值){更新OPEN表中的估价值；}else//Y在CLOSE表中{ if(Y的估价值小于CLOSE表的估价值) {更新CLOSE表中的估价值；从CLOSE表中移出节点，并放入OPEN表中；}}}将X节点插入CLOSE表中；按照估价值将OPEN表中的节点排序；}//endfor}//endwhile}//endA*()仿真结果由于程序过长，接下来就不放完整程序贴图了，完整的matlab程序放在最后的附录。当我们按章节3.2.2.中的伪代码将完整的程序写出来运行效果后如下图：图3.3.1程序运行图图3.3.2matlab完整程序运行结果1图3.3.3matlab完整程序运行结果2运行结果中的黑色部分是我们随机生成的障碍物，白色块中的绿色点是我们随机生成的起点S，红色块中的黄色小方块是我们随机生成的终点G，其他颜色的色块是我们从起点到终点所走过的地方，越靠近终点红色越深，其他接近红色方块的地方是我们从起点到终点最近的那个距离所能触及的其他地方，起点与终点之间连接地线是我们仿真出来的最佳路径。在本部分所涉及的全局变量是我们的起点S到终点G的大致规划与方向，局部变量体现在每一个父节点选择下一个子节点时的选择从程序的运行结果可验证我们前面对A*算法的选择以及通过matlab构建仿真的方向以及成果的正确，在接下来的第四章我们将会把在matlab中所取得的A*经验放入Jetbot小车中进行实体实验仿真。路径规划A*算法在Jetbot小车的应用与实验A*算法第二、三章已经介绍研究过，首先在环境信息已知的情况下利用路径规划A*算法获得一条初始路径，得到一条最优的路径。下面主要研究如何利用这A*算法在Jetbot小车上实现。路径规划算法设计首先，利用A*算法进行全局的路径规划，此时可以获得一条基于全局路径规划信息的初始路径，之后采用激光雷达传感器实时采集周围的环境信息。局部路径规划主要在障碍物周围进行，也即是在路径转向点处进行处理。在从起点到终点的路径中如果出现障碍物，这时小车被阻挡，此时利用小车的转向控制系统，也即是把障碍物模拟成一个转向点，在这个点小车所要转向角度控制便是局部路径。接下来将局部路径与全局路径结合，最终得到一条最优路径。具体的实现步骤如下：图4.1.1路径规划流程图1.智能小车通过自身搭载的IMX219摄像头采集室内周围环境，处理后利用栅格法建立环境地图模型。2.在栅格地图环境模型上确定起始点、目标点以及各个障碍物的信息后利用改进的A*算法进行全局路径规划，获得一条全局最优的初始路径。3.确定在这条初始路径中处于障碍物附近的点，即初始路径上的转向点，记录这些转向点，将这些转向点设为局部路径规划时的子目标点。4.智能小车在进行路径规划时，首先按照；初始路径趋向目标位置行进，遇到障碍物，也即是需要转向时，将上一步记录的转向点作为子目标点。然后基于改进人工势场法进；行局部路径规划，得到一条局部路径。在此过程中如果到达子目标点，则局部路径规划阶段结束，否则继续局部路径规划过程，直到到达子目标点。5.在智能小车成功绕过障碍物以后便会回归到初始路径，将局部路径与初始路径连：接起来即是A*算法规划出来的路径。6.智能小车从起始点避开障碍物，依照A*算法路径规划出来的路线准确、快速到达目标点，则算法结束。A*路径规划算法流程如图所表示。图4.1.2A*路径规划算法流程图JupyterLab软件前面的算法研究主要集中在算法理论层面，而检验一个算法是否真实有效需要应用到实际问题中去。因此，接下来主要进行真实环境下Jetbot智能小车路径规划实验来继续本课题相关研究。要进行真实环境下路径规划实验首先要有一个好的软件开发环境以及硬件平台。对于初学阶段的我们最适合使用JupyterLab交互式编程环境，所以我们的小车实际演示中将一直采用Jupyterlab进行编程，我们可以在我们的电脑上通过web浏览器来访问JupyterLab开发界面，但是我们的电脑必须要和Jetbot机器人处于同一网络下，否则无法访问，访问方式为在浏览器上输入JetbotIP.8888来进行登录，如果还没有连接WIFI，采用的是无头模式连接的话在浏览器上输入:8888进行登录访问，第一次登录的时候可能需要我们输入密码，jetbot机器人的密码为jetbot。输入后点击登录进行登录。图4.2.1JupyterLab开发界面程序编写及小车的操作实验障碍物的布置实验场地为实训楼的419实验室，在进行实验场景设置的时候，之所以选取419室内主要是，我们平常就在419室有相对理想的以太网及局域网，对小车内的编程、仿真环境较有利。并且相较于室外环境更为简单一些，由于实验室空间面积相对于室外比较小，且属于密闭空间，为了进行小车环境信息采集，建立环境地图的时候，比较容易，也相对比较快，也易于建立封闭的地图模型。我们布置了个70cm×100cm这样在进行路径规划实验的时候可以减少建立地图的时间。如图4.3.1所示为真实实验场景。图4.3.1小车障碍物布置图程序编写流程设定摄像头云台的倾斜角度

在采集数据时，为了能够采集到实质的数据和保持和测试数据在同一个角度，保证我们训练的数据是有效的，我们需要设置一个统一的摄像头倾斜角度。

from

jetbot

import

Robot

import

time

robot

=

Robot()

#

设置视觉云台的倾斜角度

robot.makerobo_servo(0，90,1,70)#根据自己的实际角度设置

2.连接摄像头

当设置好倾斜角度之后，我们需要初始化摄像头，并显示所看到的画面。

我们的神经网络采用224x224像素的图像作为输入。因此我们将摄像头设置为该大小，以最小化文件大小，而最小化数据集。（我们已经通过测试此像素适用于此任务）在某些情况下，收集数据时最好用较大的图像尺寸，然后做处理的时候缩小到需要的大小。

import

traitlets

import

ipywidgets.widgets

as

widgets

from

IPython.display

import

display

from

jetbot

import

Camera,

bgr8_to_jpeg

camera

=

Camera.instance(width=224,

height=224)​

image

=

widgets.Image(format='jpeg',

width=224,

height=224)

#

这个宽度和高度不一定必须与相机匹配

camera_link

=

traitlets.dlink((camera,

'value'),

(image,

'value'),

transform=bgr8_to_jpeg)​

display(image)

Error

displaying

widget:

model

not

found

运行完上面的代码块后，就可以实时地看到摄像头拍摄到的画面。

3.创建数据采集文件夹

接下来让我们创建一些目录存储数据。我们将会建立一个叫dataset的文件夹，里面有两个子文件夹，分别是free和blocked，用于分类放置每一个场景的图片。

import

os

blocked_dir

=

'dataset/blocked'

free_dir

=

'dataset/free'​

#我们有这个“try/except”语句，因为如果目录已经存在，这些下一个函数可能会引发错误

try:

os.makedirs(free_dir)

os.makedirs(blocked_dir)

except

FileExistsError:

print('Directories

not

created

becasue

they

already

exist')

Directories

not

created

becasue

they

already

exist

运行完上面的代码块后，你现在刷新左侧的Jupyter文件浏览器，你应该会见到这些目录。

4.创建采集数据按钮

接下来，我们将创建一些按钮用来保存不同标签的快照。我们还将创建一些文本框，用于显示到目前位置我们每个标签收集到的图像数量。这很重要，因为我们要确保采集到的free图像要和blocked图像一样多。还有助于我们了解整体收集了多少图像。

button_layout

=

widgets.Layout(width='128px',

height='64px')

free_button

=

widgets.Button(description='add

free',

button_style='success',

layout=button_layout)

blocked_button

=

widgets.Button(description='add

blocked',

button_style='danger',

layout=button_layout)

free_count

=

widgets.IntText(layout=button_layout,

value=len(os.listdir(free_dir)))

blocked_count

=

widgets.IntText(layout=button_layout,

value=len(os.listdir(blocked_dir)))​

display(widgets.HBox([free_count,

free_button]))

display(widgets.HBox([blocked_count,

blocked_button]))

Error

displaying

widget:

model

not

found

Error

displaying

widget:

model

not

found

到此为止，那些按钮是不会做任何事的。我们需要把保存图像的函数附加到每一个按钮的on_click事件中。我们会通过Image部件，把这些经过压缩处理的JPEG格式图像保存到对应的分类文件夹里。

为了确保我们不重复的任何文件名，我们将用python中的uuid包，它定义了uuid1方法从当前时间和机器的MAC地址生成唯一标识符。

from

uuid

import

uuid1​

def

save_snapshot(directory):

image_path

=

os.path.join(directory,

str(uuid1())

+

'.jpg')

with

open(image_path,

'wb')

as

f:

f.write(image.value)​

def

save_free():

global

free_dir,

free_count

save_snapshot(free_dir)

free_count.value

=

len(os.listdir(free_dir))

def

save_blocked():

global

blocked_dir,

blocked_count

save_snapshot(blocked_dir)

blocked_count.value

=

len(os.listdir(blocked_dir))

#在回调函数中使用了lambda函数，以忽略on_click事件传递参数

free_button.on_click(lambda

x:

save_free())

blocked_button.on_click(lambda

x:

save_blocked())

现在上面的按钮应该可以把图片保存在free或者blocked文件夹里。你可以使用Jupyter

Lab的文件浏览器去查看这些文件。

现在开始动手收集一些数据1.把

JetBot

放在阻挡的情况下，并按下

add

blocked

按钮

2.把

JetBot

放在通畅的情况下，并按下

add

free

按钮

重复

1，2

（提示：我们可以把那些按钮部件输出在新的窗口，这样方便操作。我们也将执行下面的代码把它们显示在一起。）

以下是一些数据标记的提示

尝试不同方向、尝试不同的照明、尝试不同的对象/碰撞类型：例如墙壁，物体等、尝试不同纹理的平面/物体：例如图案，不同光滑度，玻璃等

最终，JetBot在现实世界中越多的场景数据其防撞行为就越好，所以得到各种各样的数据很重要，而不仅仅是大量的数据。可能每个分类都需要100个图像（这不一定是一个科学的做法，仅仅是一个有用的提示）。收集这么多数据其实不用担心，当你开始收集的时候，就会变得很快完成。

display(image)

display(widgets.HBox([free_count,

free_button]))

display(widgets.HBox([blocked_count,

blocked_button]))

Error

displaying

widget:

model

not

found

Error

displaying

widget:

model

not

found

Error

displaying

widget:

model

not

found

注意：如果是在JetsonNano上直接训练数据，下面压缩文件我们可以不用执行下一步。当我们收集足够的数据的时候，我们需要把这些数据复制到我们的GPU平台上进行训练。首先，我们可以调用terminal*（命令行模式又或者叫终端）命令，进行数据打包压缩成一个‘.zip’文件。

！表示我们要将使用shell命令运行－r表示包含所有包含子文件夹文件。-q表示zip命令不输出任何信息

!zip-r-qdataset.zipdataset

我们应该在Jupyter

Lab文件浏览器中看到名为dataset.zip的文件，右键点击该文件进行下载操作。

接下来，我们需要把这些数据上传到我们的GPU平台或者云计算机（我们将其称为host主机）来训练我们的防撞神经网络。训练好模型之后，我们将返回到机器人JupyterLab环境，以使用该模型进行实时演示！图4.3.2图4.3.3图4.3.4图4.3.5图4.3.6图4.3.7（图4.3.2—图4.3.7为程序仿真动作过程时的转向抓拍照片）总结本文主要基于jetbot智能小车路径规划的A*算法。首先通过各种路径规划算法的优劣对比选择了全局路径规划和局部路径规划针对两种算法各自存在的不足之处，分别进行了一定的改进A*算法，并进行matlab仿真验证。同时在jetbot智能小车硬件平台上进行了真实环境下的路径规划实际应用，效果较好。第一章为文章的绪论部分，主要介绍了本文小车的研究背景和意义，讨论了路径规划的国内外研究现状并对提到了相应的关键技术（全局路径规划与局部路径规划）。第二章为全局路径规划、局部路径规划进行了初步分析且详细说明了路径规划的常用方法和分类，以及各种算法之间的优缺点。并对于这两种规划采取了较为中间的选取栅型构图下的A*算法最短路径规划的原理和具体实现方法。第三章即开始对于前面提出的选取栅型构图下的A*算法用matlab进行实验仿真与验证。第四章从我们在matlab中完整的模拟仿真结果中的A*算法方式通过JetbotAI智能机器人平台应用在Jetbot小车中实践。第五章对全文的总结，提出了本文的A*算法在matlab以及Jetbot小车取得的结果，说明了我们的理论不仅在理论上有效，在实际Jetbot小车试验上也有效。随着三个多月毕业设计的结束，在大学的时光也接近了尾声，我的毕业设计也终究完成了。在没有做毕业设计前我觉得毕业设计仅仅是对这三年来所学知识的简单总结，但是通过这次做毕业设计发现自我的看法有点太片面了。毕业设计不仅仅是对以往知识的检验，更是对自身潜能的提升。通过这次毕业设计使我明白了我原先的知识还比较欠缺，我需要学习的东西还有很多。鸣谢在此要感谢我的指导老师刘运松老师对我的悉心指导和帮助，本文的理论及仿真试验工作是在我的导师刘运送老师的精心指导和照顾下完成的。从开篇绪论到总结，我学到了很多知识，经历了很多困难，但通过查阅大量相关资料，与同学、老师咨询交流及自学也收获很大。我所取得的每一个进展和我编写的每一个程序都致力于导师的辛勤工作和艰苦的努力。刘运送老师严谨的学术态度，对各个学科的深刻了解以及无私的奉献精神给我带来了深深的启发。从我受人尊敬的导师那里，我不仅学到了扎实而广泛的专业知识，而且学到了做人的真相。在以后的学习和工作中，我会牢记导师的指导和鼓励，并尽最大努力取得更好的成绩。在此，我要再次对导老师表示衷心的感谢和深切的敬意！并且在大学的三年学习中，每位老师都对我的学习，生活和学习给予了热烈的关怀和帮助，这大大提高了我的水平，取得了长足的进步。在此，我向所有关心和帮助我的老师，同学和朋友们表示衷心的感谢！最后，我要衷心感谢所有忙于审阅论文并参加答辩的老师、专家和教授。

参考文献[1]马静；王佳斌；张雪.A*算法在无人车路径规划中的应用

计算机技术与发展2016年第11期[2]“每天分享一点冷知识”.5个“无人驾驶汽车”的发展历史，从无线电控制到车载传感器网页2020.[3]上海交通大学电子信息与电气工程学院.毕业设计（论文－无人驾驶车辆路径规划方法研究与仿真

2019.[4]张广林；胡小梅；柴剑飞；赵磊；俞涛.路径规划算法及其应用综述现代机械2011年第5期[5]赵德群；段建英；陈鹏宇；苏晋海.基于A*算法的三维地图最优路径规划北京工业大学信息学部，北京2019.[6]ape_spring.A*searchalgorithm浅谈CSDN博客2016.[7]宣仁虎.基于改进A_算法和人工势场法智能小车路径规划研究西安电子科技大学2020.

附录matlab完整程序functionastardemon=20;%生成一个20*20的界面wallpercent=0.45;%45%的界面作为阻碍物（墙）[field,startposind,goalposind,costchart,fieldpointers]=...initializeField(n,wallpercent);%初始化界面setOpen=[startposind];setOpenCosts=[0];setOpenHeuristics=[Inf];setClosed=[];setClosedCosts=[];movementdirections={'R','L','D','U'};counterIterations=1;axishandle=createFigure(field,costchart,startposind,goalposind);while~max(ismember(setOpen,goalposind))&&~isempty(setOpen)%返回与A同大小的矩阵，其中元素1表示A中相应位置的元素在B中也出现，0则是没有出现[temp,ii]=min(setOpenCosts+setOpenHeuristics);%从OPEN表中选择花费最低的点temp,ii是其下标(也就是标号索引)[costs,heuristics,posinds]=findFValue(setOpen(ii),setOpenCosts(ii),...field,goalposind,'euclidean');%扩展temp的四个方向点，获得其坐标posinds，各个方向点的实际代价costs，启发代价heuristicssetClosed=[setClosed;setOpen(ii)];%将temp插入CLOSE表中setClosedCosts=[setClosedCosts;setOpenCosts(ii)];%将temp的花费计入ClosedCosts%更新OPEN表分为三种情况if(ii>1&&ii<length(setOpen))%temp在OPEN表的中间，删除tempsetOpen=[setOpen(1:ii-1);setOpen(ii+1:end)];setOpenCosts=[setOpenCosts(1:ii-1);setOpenCosts(ii+1:end)];setOpenHeuristics=[setOpenHeuristics(1:ii-1);setOpenHeuristics(ii+1:end)];elseif(ii==1)setOpen=setOpen(2:end);%temp是OPEN表的第一个元素，删除tempsetOpenCosts=setOpenCosts(2:end);setOpenHeuristics=setOpenHeuristics(2:end);else%temp是OPEN表的最后一个元素，删除tempsetOpen=setOpen(1:end-1);setOpenCosts=setOpenCosts(1:end-1);setOpenHeuristics=setOpenHeuristics(1:end-1);endforjj=1:length(posinds)%对于扩展的四个方向的坐标if~isinf(costs(jj))%如果此点的实际代价不为Inf，也就是没有遇到墙if~max([setClosed;setOpen]==posinds(jj))%如果此点不在OPEN表和CLOSE表中fieldpointers(posinds(jj))=movementdirections(jj);%将此点的方向存在对应的fieldpointers中costchart(posinds(jj))=costs(jj);%将实际代价值存入对应的costchart中setOpen=[setOpen;posinds(jj)];%将此点加入OPEN表中setOpenCosts=[setOpenCosts;costs(jj)];%更新OPEN表实际代价setOpenHeuristics=[setOpenHeuristics;heuristics(jj)];%更新OPEN表启发代价elseifmax(setOpen==posinds(jj))%如果此点在OPEN表中I=find(setOpen==posinds(jj));%找到此点在OPEN表中的位置ifsetOpenCosts(I)>costs(jj)%如果在OPEN表中的此点实际代价比现在所得的大costchart(setOpen(I))=costs(jj);%将当前的代价存入costchart中，注意此点在costchart中的坐标与其自身坐标是一致的（setOpen(I)其实就是posinds(jj)），下同fieldpointerssetOpenCosts(I)=costs(jj);%更新OPEN表中的此点代价，注意此点在setOpenCosts中的坐标与在setOpen中是一致的，下同setOpenHeuristicssetOpenHeuristics(I)=heuristics(jj);%更新OPEN表中的此点启发代价fieldpointers(setOpen(I))=movementdirections(jj);%更新此点的方向endelse%如果此点在CLOSE表中，说明已经扩展过此点I=find(setClosed==posinds(jj));ifsetClosedCosts(I)>costs(jj)%如果在CLOSE表中的此点实际代价比现在所得的大costchart(setClosed(I))=costs(jj);%将当前的代价存入costchart中setClosedCosts(I)=costs(jj);%更新CLOSE表中的此点代价fieldpointers(setClosed(I))=movementdirections(jj);%更新此点的方向endendendendfunctionp=findWayBack(goalposind,fieldpointers)n=length(fieldpointers);%场的长度posind=goalposind;[py,px]=ind2sub([n,n],posind);%storeinitialpositionp=[pypx];while~strcmp(fieldpointers{posind},'S')%当查询到的点不是'S'起点时switchfieldpointers{posind}case'L' %如果获得该点的来源点方向为左时px=px-1;case'R'%如果获得该点的来源点方向为右时px=px+1;case'U'%如果获得该点的来源点方向为上时py=py-1;case'D'%如果获得该点的来源点方向为前时py=py+1;endp=[p;pypx];posind=sub2ind([nn],py,px);endfunction[cost,heuristic,posinds]=findFValue(posind,costsofar,field,...goalind,heuristicmethod)n=length(field);%场的长度[currentpos(1)currentpos(2)]=ind2sub([nn],posind);%获得当前点的行列坐标，注意currentpos(1)是列坐标，currentpos(2)是行坐标[goalpos(1)goalpos(2)]=ind2sub([nn],goalind);%获得目标点的行列坐标cost=Inf*ones(4,1);heuristic=Inf*ones(4,1);pos=ones(4,2);%向左查询，那么就是从右边来newx=currentpos(2)-1;newy=currentpos(1);ifnewx>0%如果没有到边界pos(1,:)=[newynewx];%获得新的坐标switchlower(heuristicmethod)case'euclidean'heuristic(1)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);%heuristic(1)为启发函数计算的距离代价case'taxicab'heuristic(1)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);endcost(1)=costsofar+field(newy,newx);%costsofar为之前花费的代价，field(newy,newx)为环境威胁代价，cost(1)为经过此方向点的真实代价endnewx=currentpos(2)+1;newy=currentpos(1);ifnewx<=npos(2,:)=[newynewx];switchlower(heuristicmethod)case'euclidean'heuristic(2)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);case'taxicab'heuristic(2)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);endcost(2)=costsofar+field(newy,newx);endnewx=currentpos(2);newy=currentpos(1)-1;ifnewy>0pos(3,:)=[newynewx];switchlower(heuristicmethod)case'euclidean'heuristic(3)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);case'taxicab'heuristic(3)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);endcost(3)=costsofar+field(newy,newx);endnewx=currentpos(2);newy=currentpos(1)+1;ifnewy<=npos(4,:)=[newynewx];switchlower(heuristicmethod)case'euclidean'heuristic(4)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);case'taxicab'heuristic(4)=abs(goalpos(2)-newx)+abs(goalpos(1)-newy);endcost(4)=costsofar+field(newy,newx);endposinds=sub2ind([nn],pos(:,1),pos(:,2));%posinds为此点扩展的四个方向上的坐标function[field,startposind,goalposind,costchart,fieldpointers]=...initializeField(n,wallpercent)%Thisfunctionwillcreateafieldwithmovementcostsandwalls,astart%andgoalpositionatrandom,amatrixinwhichthealgorithmwillstore%fvalues,andacellmatrixinwhichitwillstorepointers%createthefieldandplacewallswithinfinitecost初始化界面和墙field=ones(n,n)+10*rand(n,n);