【《基于遗传算法的四轮移动小车全局路径规划实证研究》13000字（论文）】

-PAGEIII-基于遗传算法的四轮移动小车全局路径规划实证研究摘要发展科技就是提高社会生产力的大环境下，各个国家都在大力发展智能化技术，我国在智能小车技术研究方面扮演着越来越重要的角色。而在过去，人们对于车辆的研究仅限于自主式移动，从而导致了小车在现实生活中无法完成更多功能的任务，使用范围受限。为此，对路径规划算法进行研究，使小车不仅能避开障碍物，还能沿着提前预定好的路线行走。首先，利用光电编码器对小车运动学进行分析与研究，分别研究光束模型和似然场模型两种激光雷达模型，说明小车坐标系和世界坐标系的转换关系，阐述栅格地图、拓扑地图和特征地图的利弊。然后，小车系统由硬件和软件组成。硬件设计包括零部件的选取和四轮移动小车试验平台的搭建。软件设计主要通过ROS节点传送的信息实现位置控制，并且应用ROS集成体系架构，服务器端节点RVIZ，分布式ROS构架和主控制器端节点完成软件系统的设计。利用PID控制算法实现小车系统功能的可控性。最后，从全局路径规划和局部路径规划两个方面对小车路径规划进行研究。对整体路径规划仿真算法Dijkstra算法和A*算法分别进行了综合比较，分析了基于遗传算法的小车全局路径规划。结合动态窗口法对小车进行局部路径规划研究。在ROS操作系统的控制下完成路径规划试验。关键词:四轮移动小车；激光雷达；系统设计；路径规划；目录摘要 IAbstract II目录 III第一章绪论 11.1研究背景及意义 11.2研究现状及分析 11.3论文的主要内容及结构 3第二章四轮移动小车运动学分析及环境建模 42.1四轮移动小车运动学模型 42.2激光雷达模型 52.2.1激光雷达测距原理 52.2.2激光雷达测距数学模型 62.3四轮移动小车的坐标系统 92.4地图模型 102.4.1栅格地图 102.4.2拓扑地图 112.4.3特征地图 11第三章四轮移动小车系统设计 133.1硬件设计 133.2软件设计 153.3位置控制程序设计 16第四章四轮移动小车路径规划研究及试验 174.1全局路径规划研究 174.1.1Dijkstra算法与A*算法 174.1.2基于遗传算法的小车全局路径规划 204.2局部路径规划研究 234.3路径规划算法试验 26结论 28第一章绪论1.1研究背景及意义在科学技术高速发展的背景下，智能机器人在生活中应用越来越广泛，比如儿童游戏区和各种游乐场所的多功能自动化玩具、企业生产加工流水线上完成指定任务的机器人、危险环境下的工程抢修机器人等。由此可见，机器人事业的兴盛在当今社会中的影响愈来愈重要。智能交通小车是一种新型交通系统，运用导航、计算机、传感、自动导航技术实现自动控制行驶、感知周围环境信息、路径规划等多种功能。移动小车导航需要小车有准确快速感知周围环境信息和实现自主稳定移动的处理功能，其核心技术特点在于即时终点定位与移动地图系统构建（SimultaneousLocalizationAndMapping，SLAM）和及时自主移动导航（AutonomousNavigation）[1]，自主移动导航的关键技术是可行路径路线规划。路径规划需要传感器对环境信息的感知，小车传感器可分为内部传感器和外部传感器，外部传感器主要有距离传感器、感觉传感器,内部传感器主要有速度和加速度传感器、位置传感器等[2]。目前，智能移动小车的广泛应用仍受到制造成本的严重制约，小车不能使用谷歌无人车[3]中所使用的高精度成像雷达。然而，单线程的激光雷达扫描成像频率相对较低，但测量精度仍然很高。路径规划技术的应用领域较为广泛，虽然摄像机激光传感器能够带来的室内环境相关信息比较丰富，但室内地图不是太适合帮助小车在多个室内环境下同时进行路径规划。激光雷达的测距结果精度较高，这样会更好的帮助小车对室内环境相关信息的正确分析。本课题的研究意义主要在于实现一个基于移动小车系统设计和路径规划的综合研究系统。搭建小车硬件系统，利用ROS控制系统实现小车运动的可控化。从全局路径规划和局部路径规划的两个方向完成小车路径规划研究。通过系统仿真和模拟试验，验证了该研究系统是否能够在已知地图的情况下进行路径规划。1.2研究现状及分析小车行驶路径性能规划解决问题主要是根据一定的小车性能指标，从初始小车状态发展到目标小车状态，寻找最优或性能接近最优的一种无碰撞小车路径的解决方法[4]。小车路径规划能使汽车避开外界障碍物，最大限度地减小对汽车传感器感应范围的影响。根据对扫描到的环境信息掌握程度，可以划分为一个全局路径规划和一个局部路径规划[5]。对于全局路径规划算法，最常用的方法有Dijkstra算法[6]、A*算法[7-8]、D*算法[9]等，其中A*算法在目前因为其简单、成熟而适用范围最广。然而，传统的路径规划搜索方法没有直接考虑一个机器人网络本体的路径规模，而是直接采用一种启发式方法搜索最优化的路径，在大规模范围的网络环境下，由于各个节点路径数量的不断增加，会直接导致实时性能比较差。Dijkstra这个算法可以完全保证在大规模范围的地图环境中能找到一条最优化的路径，但由于该节点算法同时需要遍历地图网络中的所有节点网格，因此不能完全适用于复杂的大范围规模地图环境。为了大大提高A*存储算法的强大实时计算性能，Peng等人[10]为了有效避免遍历多个OPEN表和CLOSED表的大量时间成本开销，通过一次次的操作可以直接访问指定的一个A*存储阵列，提高了操作效率。王红卫等人[11]分析指出，为了能够使之前A*地图算法重新规划的拐点路径更加平滑，人们在之前地图信息中重新建立的拐点路径中可以判断三个拐点相邻地图节点之间的主要障碍物位置信息，并通过不断减少相邻节点间的信息数量来平滑算法规划的拐点路径，但由于拐点的信息存在仍然可能会严重限制机器人完成各种姿态上的转换。Guruji等人[12]确定了一个在碰撞前的启发式运算函数，加快了处理时间。为了有效实现复杂场地环境下的无人实时移动路径路线规划，王维等人[13]基于A*的算法，考虑到复杂场地环境和多个障碍物，对父节点的实时代价计算函数的值进行一个指数级的加权，使移动机器人快速准确的接近目标节点。传统的分析算法往往存在着实时性差等诸多缺点。基于遗传算法的搜索路径优化规划算法采用传统概率导数优化计算方法，直接对结构中的对象节点进行导数操作，无需过分考虑导数值的运算和取值函数的逻辑连续性，通过进行全局集合搜索和对空间节点区域自身的适应可以调整最优搜索路径方向，Holland等人[14]首先将遗传算法广泛应用于现代机器人最优搜索路径的规划选择，将现在机器人最优路径规划中的每个空间节点可以看作一个群体关系中的一个原始个体，并在遗传算法中通过反向交叉和集合变异两种操作可以完成迭代，最终通过一个集合全局搜索空间区域得到最优搜索路径。由于外界环境大多数都是相对动态的，小车通常需要快速避开一些已知地图中不可能存在的局部障碍物，因此我们需要定期进行一个局部避障路径的规划工作来快速完成整个小车的避障。局部空间路径移动规划主要算法包括人工动态窗口群算法（DWA）、人工移动势场法、蚁群耦合算法、粒子群耦合算法等[15]，人工势场的算法通过一个虚拟吸引力场对移动小车产生吸引力，目标点为重力，障碍物为斥力。通过计算小车所受力的合力来驱动小车运动，便于使用小车进行底层自动控制管理系统的实时自动控制。但它同时具有速度局部偏离极值的特点，在狭窄的目标通道中容易快速摆动，在发现目标锁定点附近等处有许多障碍物时无法快速找到正确路径[16]。动态窗口设计算法于1997年6月首次被提出。将基于局部避障优化问题这一描述定义为基于速度空间的局部优化避障问题。该替代算法在选择满足所有速度限制约束的矢量条件下，通过迭加替代可以选择最优化的速度约束矢量。该矢量算法已经充分考虑了局部速度路径约束的问题，在局部环境速度约束的条件下可以选择最优化的速度路径矢量，非常好的适合于进行局部高速路径矢量规划[17]。1.3论文的主要内容及结构主要研究基于激光雷达的四轮移动小车系统设计与路径规划技术。首先完成四轮移动小车系统设计，然后研究了在已知地图条件下四轮移动小车的路径规划问题，最后通过仿真和实际测试，实现了四轮移动小车的路径规划。第一章分析了研究背景与意义，如今小车在各行各业中的应用，并从小车的系统设计、路径规划算法两个方面的研究现状进行说明，最后说明文章主要内容与结构。第二章研究了小车运动学和环境建模。首先对运动学中的传感器的组成、原理和相关计算公式进行说明。然后研究了激光雷达的测距原理，比较分析了波束模型和似然场模型存在的问题和解决方法。最后分析了小车坐标系的转换和三种地图模型的优缺点，重点分析了本文采用的栅格地图。第三章详细研究了四轮小车的系统整体设计。从系统硬件的设计、软件的设计、位置控制设计三方面具体实现小车系统的设计。第四章主要研究了四轮移动小车路径规划的设计和试验。其中，路径规划分为全局路径规划和局部路径规划。第五章主要是课题总结和未来展望，总结了这篇文章的重点研究课题内容和未来发展规划中一些目前有待深入研究而不能解决的技术问题。

第二章四轮移动小车运动学分析及环境建模如今汽车的很多高端功能都依赖于传感器的感知能力，就比如汽车的自动导航、车自身温度的检测、时速表等信息的显示都离不开传感器的作用。传感器在小车行驶过程中就像是人的大脑一样，赋予了汽车视觉、触觉、力的感知等多种功能，实现智能化处理。每个功能的实现都源自于传感器，受传感器采集到的数据所控制。对小车的运动学进行分析有助于我们把握小车的运动状态，激光雷达技术可用于探测距离、方位等有效信息，研究小车坐标系来描述小车运动情况，地图的构建用以实现小车路径规划。2.1四轮移动小车运动学模型光电编码器一般会安装在驱动轮子的电机上。主要作用是记录轮子转轴转动一圈所输出的位移量，也就是光电编码器的分辨率。再通过记录轮子转轴在某一时间段内转动的转数，应用相应的计算公式就可算出这一时间段内小车的行驶里程，也就是里程计的工作原理。然而，由于路面不平滑导致车轮抖动和噪声对光电编码器的影响，对搜集到的里程信息造成很大的误差，随着小车的行驶，误差不断累加。还有安装车轮时形成的误差，会导致计算精确度不够高。光电编码器通过测量驱动电机上码盘的转速μ、轮子的直径d（单位：mm）和光电编码器的精度ρ，推算出机器人位姿的变化量δ。采样频率可以设定，一般在QUOTE30Hz30Hz(2.1)里程计是通过编码器输出的左右驱动轮脉冲增量QUOTENLNL和QUOTENRNR，和编码器的分辨率δ来分别计算左右驱动轮在采样周期的位移增量QUOTE∆SL∆SL和∆SRQUOTE∆SR：(2.2)当小车在转弯时，由于直线模型无法直接描述其运动轨迹，故采用圆弧模型计算小车在转弯时的运动情况。图2.1轮式里程计圆弧模型小车的位姿变化可用t−1时刻的姿态QUOTEXt-1(xt-1,yt-1,θt-1)Xt-1(x(2.3)小车里程计的输入模型为QUOTEut=(∆Dt,∆θ(2.4)里程计的误差来源有很多，系统误差有测量仪器精确度不够高、安装车轮时位置控制不够准确形成的误差等，无法消除。非系统误差有路面不平滑导致车轮抖动和噪声对光电编码器的影响，对搜集到的里程信息造成很大的误差，随着小车的行驶，误差不断累加，小车停止后误差不再增加。2.2激光雷达模型2.2.1激光雷达测距原理本次课题所使用的雷达是型号名称为A1M1的新型激光雷达RPLIDAR，由美国思岚激光科技在2013年自主研发，目前国内市场价格相对较低。360°扫描，扫描距离为8米，可用于机器人路径规划研究等领域。图2.2激光雷达RPLIDARA1RPLIDARA1激光雷达系统的稳定性较好，雷达转速不会随着光源电压的变化而改变，因此选择该型号激光雷达不会对路径规划造成很大误差。扫描方向都是顺时针方向。一般情况下，激光雷达都是安装在小车中心位置，以激光雷达为坐标原点建立直角坐标系，其中QUOTEρi和diρi和di是当前状态下激光雷达到障碍物的远近，QUOTEθiQUOTEzi=ρi，θ式中，N是扫描点数，可自行设定。2.2.2激光雷达测距数学模型通过对比分析光束模型和似然场模型来研究激光雷达在进行路径规划时采用的数学模型，着重分析了本设计用到的似然场模型。相较于光束模型，似然场模型适用于各种环境，而且计算量小。（1）光束模型预期的光束模型QUOTEpztxt,m因此当移动车辆处于动态环境中时，用QUOTEphit表示phit表示第一个高斯测距模型，由于地图是静态的，所以不包括在地图中的物体，如行人，应该从静态地图中删除，并用指数分布QUOTEpshortpshort表示，处理这类问题的简单方法是将其作为传感器噪声直接去除，此时自动激光雷达将自动返回到最大精度测量区域范围QUOTEzmaxzmax，即能以QUOTEzmaxzmax为中心的点群分布QUOTEpmaxpmax表示，即最大精度测量区域范围已经明确建立。当一个激光测距系统产生各自无法解释的外力测量时，例如激光发射的多束激光被多个壁反弹或激光受到多个传感器的干扰时，为了有效简化这种测量情况，用QUOTEprandprand在激光雷达天线测距测量范围内的完全均匀分布曲线来精确表示。图2.3激光雷达测距模型分布考虑到几率和密度为1，加权平均求和时通过四个分布对应的参数QUOTEzhitzhit、QUOTEzshortzshort、QUOTEzmaxzmax、QUOTEzrandzrand进行的，假如四个参数和为1，则有：(2.6)从计算的角度看，由于每次扫描返回值都有上百个，而且扫描频率也很高，导致计算传感器全部返回值是很难实现的。又因为每次扫描后都要进行射线投射操作，从而不能总是实时地将扫描整合到当前的置信度中。为了解决这一问题，仅综合激光雷达扫描过的一部分等间隔测量结果。并且不会轻易受相邻测量的噪声影响。基于波束的模型存在算法的局限性和光滑性不够。当小车身处一个复杂的障碍物运动环境中时，其在不同障碍位置的能量分布不连续。当会议室里放置有许多小型桌椅和沙腿时，因为小车的一个细微角度变化就会导致小车局部坐标值在空间中x-y的巨大局部偏移，也就是说，在这样一个不光滑的模型中，存在着更多的小车局部极大值偏移值。由于利用光线的反射原理，激光雷达的每一个光线扫描工作周期有数百个光线数值，这些数值都可能需要进行一个光线投影的运算，计算量很大。（2）似然场模型似然场模型计算是一种在特定计算环境下的数学假设设计算法，相似于高斯平滑的计算环境，它不要求计算是在激光雷达感应器上数学模型的基本条件概率，其主要数学思想是通过全局三角变换将一个激光雷达的扫描坐标点直接投影到位于地图中心的一个全局坐标系中。与其它波束模型方法相比，似然场模型通过一个三角形的映射将预先观测到的结果通过投影映射到一个全局坐标系，只有在电子探测器遇到障碍物时才会具有意义，该模型因而放弃了最大观测距离下对观测结果值的自动读取。为达到模型的平滑度，测量到的结果由一个模型平均值为0的高斯函数进行估计，测量的坐标由欧氏模型距离间的dist来表示，即使所测量到的结果在一个相应的小车坐标系下与欧氏模型距离间的速度地图上模拟重合：(2.7)给出了似然场模型的几率分配：(2.8)测量过程中的波束噪声由高斯概率值的估计值表示；对于一个测定未成功时，即在第二种测量情况下讨论波束模型时，它由一个函数点阵组合表示；对于不能正确解释的随机函数测量，它是由一个统一的分布表示。表2.1似然场模型概率计算伪代码似然场概率估计算法：1:q=12:forallkto3:if4:5:6:7:8:endif9:endfor10:returnq从表2.1可以看出，似然场模型中，第三行是在检测激光雷达读数是否为最大值，如果答案是，则可以丢弃。第4-7行主要测量将障碍物高斯模糊后的数据。第6行用欧氏距离表示测量距离。第7行整合均匀分布和点群分布。将障碍物高斯核处理后，用全局坐标X-Y来表示测距的似然估计。采用的似然场模型优点是对障碍物进行高斯处理后，使观测结果更加平滑，适用于结构化和非结构化环境。并且通过对地图进行光栅化来加快运行速度，本代码最耗时的步骤是第6步，通过查表可以得到光栅化的地图。2.3四轮移动小车的坐标系统在小车研究领域中，有多个坐标系可用来描述小车的位置和姿态信息，并且在一定条件下各坐标系的数据可以相互转换。系统中所有点的坐标在任何时候都可以用世界坐标系来描述，世界坐标系是绝对的。通过将小车坐标系QUOTEORXRYRORXRYR中的环境信息转换为世界坐标系图2.4小车坐标系图2.4显示了小车坐标系与世界坐标系的转换关系。可通过公式（2.9）将m点的小车坐标QUOTEX0=[x0，y0]TX0=（2.9）2.4地图模型2.4.1栅格地图栅格地图（Gridmap）分为矩形栅格地图和扇形栅格地图。主要研究矩形栅格地图，它是由一系列大小一致的矩形栅格组成，栅格大小可人为设定。如下图2.4表示一个简单的矩形栅格地图，由直线分割得到。白色表示可行区域，黑色表示有障碍物，在矩形栅格地图上可形象直观的进行路径规划研究，有利于数据的存储和计算。图2.5栅格地图2.4.2拓扑地图拓扑地图（Toplogicalmap）就是用简单的点线连接表示两点之间的互通性。在室内，节点代表门、桌子和拐角等位置，之间的连线表示可以到达。拓扑地图分为星型、网状型和簇状型拓扑地图，如图2.6所示。由于空间复杂度比较低，可进行有效的路径规划，但不容易识别指定位置，也有可能造成规划出的路径不是最佳的。图2.6拓扑地图2.4.3特征地图特征地图（Featuremap）是以简单几何形状中的点、线段和圆弧来表示环境信息的一种地图，如图2.7所示。首先对环境中各自的特征信息进行识别与提取，收集环境中的目标特征，然后获取物体的地理坐标，计算出目标特征的地理坐标，生成特征地图。然而，在重新定位时如果数据匹配错误，将会导致路径规划错误。图2.7特征地图

第三章四轮移动小车系统设计3.1硬件设计首先，建立四轮移动汽车的实验平台。完成移动结构、驱动系统、感觉系统和承载平台的组装。四轮车的驱动机构和运动是轮式汽车的基本运动结构。图3.1（a）显示了由两层组成的车身。采用轮式移动机构。前轮是控制轮，后轮是驱动轮，可差速转动。驱动系统是为机械结构提供动力的装置。驱动系统采用ArduinoTeensy3.1底层驱动板如图3.1（b）搭配直流电源如图3.1（c）所示。传感系统由外部传感器和内部传感器组成。内部传感器用于检测汽车自身的状态，外部传感器用于感知外部世界，检测工作对象和工作环境的状态，如视觉、听觉、触觉等。本文选用图3.1（d）所示的RplidarA1激光雷达传感器。控制系统的任务是根据汽车的操作指令和传感器的信号反馈，控制汽车的执行机构完成规定的动作和功能。为实现四轮小车灵活、易扩展、模块化的优点，控制系统采用主控板如图3.1(e)所示，和开源卡片式电脑RaspberryPi3b如图3.1（f）所示。(a)移动小车(b)底层驱动板(c)直流电源(d)激光传感器(e)主控板(f)卡片式电脑图3.1各部件及四轮小车实物四轮移动小车硬件系统主要由小车控制系统和服务器两部分组成，其结构框图如图3.2所示。控制系统由RplidarA1激光传感器、电源模块、主控板惯性测量、位置控制模块，每部分将各自传感器接收到的信息通过WIFI传给服务器从而远程控制小车运动和RVIZ的可视化接受。位置控制模块主要由GY-85九轴陀螺仪传感器、底层驱动板ArduinoTeensy3.1、差速PID调速驱动器、360线AB编码器、12V366PRM直流有刷电机等形成闭环系统，选择PID(ProportionalIntegralDerivative)算法实行控制，采用360线AB编码器进行小车的车轮线速度计算，小车的角速度由GY-85陀螺仪控制，完成小车的线速度、角速度等控制信息准确回发到服务器端，对小车精准定位。电源选择12V2800mAh可充电大电流锂电池为小车充电（市面上的电池盒和连接器有无数种，其中一种见图2），3节18650型锂电池组成电芯。利用RplidarA1激光雷达，主控制器把获取的环境二维激光数据连接ROS之间的节点并将数据通过无线传输给服务器，实现二维环境下的地图构建。主控制器选用的BCM2837处理器为1.2GHz64位，搭载四核ARMCortexA53的开源卡片式电脑RaspberryPi3B，内置10/100自适应网卡，配备多种接口和1G内存，可通过802.11nWIFI无线USB网卡与服务器进行无线网络通信。戴尔笔记本电脑安装开源操作系统ROS和Ubuntu16.04操作系统作为服务器，主控制器传送来的里程信息、速度、位姿的接收主要由服务器端的PC机承担，用于RVIZ可视化的整个区域地图的构建和远程操控。图3.2四轮小车系统设计框架3.2软件设计ROS（RobotOperatingSystem）作为目前面向机器人的主流开源操作系统，其提供相应的工具和库便于完成整个机器人操作系统框架搭建。ROS支持Ubuntu16.04操作系统和树莓派的Raspbian操作系统，在本节中简要概述移动机器人的ROS集成体系结构及通过ROS节点发布的消息进行位置控制。通过地图建设的图形化处理，解决了树莓派地区数据处理量过大的问题。采用ROS的分布式处理框架来完成不同节点之间的通信，保证树莓派和虚拟机在同一网段中。ROS是一种基于图状架构，不同节点的进程能接受，发布，整合小车信息。在服务器端，我们可以接收移动汽车的角速度和线速度修正、IMU修正、键盘调试和连续传输信息，并合并里程信息、激光雷达等节点。小车结构如图3.3所示。RVIZ被配置为显示构建的地图，用来估计和校正汽车的姿态和最新的激光扫描数据。服务器节点RVIZ主要负责消息的可视化：(1)键盘调试。使用键盘上的指令，控制汽车行驶并接收反馈的里程信息。(2)线速度和角速度的校正。为了获得更高的精度，对线速度和速度校正码进行了修正。(3)SLAM。获得最新的激光扫描和估计测距姿态，从而校正汽车姿态和更新地图。图3.3小车分布式ROS架构移动小车主控制器的终端节点主要负责传感器数据的采集和小车运动的控制：(1)激光雷达。从RPLIDARA1激光传感器接收激光数据信息；(2)里程信息。通过读取编码器数据并使用该算法，可以估计出汽车的位置和姿态；(3)基本控制器。Arduino控制器被配置为远程节点，订阅主题动作，接收汽车的角速度和线速度，控制汽车精确移动；(4)Arduino。与树莓派上位机建立通信，Arduino节点发布和订阅控制信息。3.3位置控制程序设计通过小车与服务器操作系统的连接来完成小车位置的实时控制。在初始化小车角速度和线速度的前提下，服务器接收和分析小车位置。接收到当前位置的参数后，计算误差，并用PID算法调节电机转速。选择GY－85陀螺仪控制角速度，根据电机上的编码器计算线速度。服务器发送指令来控制输出量，小车通过RplidarA1激光传感器做出相应的移动和避障，并将当前姿态信息传送到服务器，如图3.4所示。图3.4位置控制程序流程图

第四章四轮移动小车路径规划研究及试验小车路径路线规划主要是根据最短行驶距离，寻找规划出一条从起始点到目标点之间的无碰撞行驶路径。基于上一章移动小车系统的整体设计，结合二维激光雷达和栅格地图的相关知识，很容易实现小车的路径规划。本章重点研究了A*算法和遗传算法两种小车全局路径规划算法，DWA动态窗口规划算法用以完成局部路径规划。4.1全局路径规划研究4.1.1Dijkstra算法与A*算法Dijkstra算法是用于计算一个节点到另一个节点的最短路径的典型算法。对于单源最短路径，首先指定图中的一个顶点为起点，以起点为中心向外扩展，记录下从起点到各个点的距离。然后在剩余顶点中找出与源点距离最近的一个可行顶点作为第二个点向外扩展，依此类推，直到所有的点都被选完。每个被选定的点组成一条可行路径，该算法为贪心算法的策略。如图4.1所示，这是模拟Dijkstra算法的路径规划。绿色网格是起点，黄色网格是终点，红色网格是基于源点的可行范围，黑色网格是障碍物，蓝色表示区域边界。这种遍历搜索方式的优点是能以最短路径找到最优解，但没有考虑到搜索时间、搜索效率等问题。图4.1Dijkstra算法路径规划A*算法结合了最佳优先级搜索策略，在Dijkstra算法的基础上添加了启发式搜索算法，如图4.2中的算法路径搜索。使用以下函数f(n)来计算每个节点的优先级： (4.1)图4.2A*算法路径搜索f(n)是节点的综合优先级。f(n)由g(n)和h(n)组成，优先级越高，f(n)越小；其中g(n)是当前节点从起点的开销，h(n)是当前节点从终点的开销；每次从优先级队列中选择f(n)值最小的节点作为下一个要遍历的节点。正如栅格地图模型，下列距离模型可用于h(n)：图4.3A*算法流程图曼哈顿距离：，如果图形中只允许四个运动方向，则可以使用此方法表示当前位置的距离。对角距离：，如果每个节点都是正方形，且允许向相邻节点倾斜运动，那么函数可以对角测量。欧几里得距离：QUOTEhn=xn-x0采用的汽车四轮运动底盘，基本上可以实现任意方向的运动，所以选用曼哈顿距离。4.1.2基于遗传算法的小车全局路径规划遗传算法通过全局搜索空间区域，自适应地调整搜索方向直接对结构对象进行操作。路径优化过程如图4.4所示：图4.4路径规划遗传算法流程图提出了一种基于栅格地图构建的移动车辆路径规划方法。单个网格的区域需要综合考虑环境地图的障碍物信息，每个网格都按照建立的坐标系进行编号，其中白网格为可行区域，黑网格为障碍物。对于填充初始化步骤，主要目的是生成多条可达路径。首先在每一行按顺序取出一个无障碍网格，然后用公式4.2判断网格是否连续：（4.2）abs函数是绝对值函数。当且仅当D=1时，这两个网格可以判断为连续的，而其他网格则是不连续的，网格不连续时取中间网格：QUOTExnew=int(xi+1+新网格的坐标由公式(4.3)获得，初始化路径流程如图4.5所示。图4.5初始化路径流程图通过初始化路径过程，可以得到无数条连续的路径，最优路径由适应度函数确定。为了实际考虑，我们考虑了两个部分。首先，我们需要实现最短路径。其次，对于研究的四轮移动小车而言，道路的转弯与其他运动方式相比会导致一个巨大的定位误差，因此对路径规划的平滑度要求很高。路径长度计算如下：QUOTEd总=i=1end-1遗传算法首先需要经过选择操作，而轮盘赌中的选择方法比基于订单的选择方法更为简单，易于实现。轮盘赌的过程就是选择高概率而放弃低概率。因此，基于轮盘赌选择法，随机选取适应度值较大的第一部分的概率较高，适应度函数的第一部分为：（4.5）对于移动小车路径的平滑度考虑，此时考虑邻近三点间的距离：（4.6）小车的行走方式：直角、钝角、锐角和直角。路径规划的最佳方法是直线，其次是钝角和直角，最不利的方法是锐角。此时，汽车转弯过多，很容易导致汽车姿态的巨大误差。而且，在考虑相邻三点时，直线距离最大，平滑度最高，因此，在路径规划中给惩罚函数剩下的路一个较低的分数。此时，适应度函数的第二部分是：（4.7）完整的适应度函数如下：（4.8）此时，我们需要给适应度函数的第一部分和适应度函数的第二部分赋值，因为有些路径是最短的，但不能考虑平滑度，所以我们需要给a和b赋值，其和为1。在轮盘赌算法的基础上，每个个体出现的概率为：（4.9）在选择过程完成之后，即在个体的数目通过连续复制达到预定数目之后，执行交叉操作。交叉运算符的值设置为0.8以生成0到1的值。当小于0.8时，执行交叉操作，并且确定两条路径中的同一点作为插入操作的交点。完成交叉操作后，判断随机值的大小和交叉算子的0.06，如果小于0.06，则完成变异操作。为了避免突变点的值出现在操作区域之外，必须定义一个新的标记点来替换先前的突变点，并过滤掉不相关的点。MATLAB用于模拟分析。首先，建立了包含400个障碍物单元的栅格地图。起点是左下角的网格，终点是右上角，如图4.6（a）所示。此时，只考虑汽车的最短距离。从图4.6（b）可以看出，算法经过约10次迭代后收敛，网格图的单位长度为1，规划路径中有许多直角和锐角，车辆行走困难。对于图4.6（c），考虑到车辆的路径长度和平滑度，并赋予平滑度更大的权重，规划路径更平滑，两种情况下的路径长度相等。图4.6基于遗传算法的路径规划仿真通过考虑路径的平滑度和最短距离，增加了迭代次数，使路径更为平滑，适合汽车行走。在a=1的情况下，汽车只考虑最短路径。此时，虽然得到了一条较好的路径，但没有考虑汽车的运动学模型。直角或锐角的巨大转角容易导致车辆的定位偏差，因此需要综合考虑车辆路径的平顺性。4.2局部路径规划研究在地图已知的条件下，车辆可以得到全局路径规划，但不能避免遇到临时障碍物。此时需要进行局部路径规划操作并做出避障反应。在完成局部路径规划以避开障碍物后，它可以按照系统规划的一个全局路径进行移动。目前对于局部窗口路径规划主要方法有动态窗口势场法和静态人工窗口势场法。在静态人工势场法中，目的地是引力源。通过虚拟化，目标点将吸引汽车在环境中的位置，环境中的障碍物将击退汽车。因此，可以同时计算汽车在环境中的位置和姿态。人工势场法通过设置相应的参数，可以有效地避开障碍物，具有良好的实时性。然而，不完全的局部信息可能导致错误的路径规划。因此，结合汽车运动学模型，采用动态窗口法进行局部路径规划。动态窗口算法对不同速度空间条件中的多组不同数据速度进行自动采样，并在这些不同速度下自动模拟汽车在特定行驶时间和速度空间条件中的行驶轨迹。动态窗口的主要意义是根据汽车的运动学模型，通过控制增减运动速度，在可行的范围内可以获得有限的速度空间的采样。四轮移动汽车只能向前移动和旋转，轨道是直线或圆弧，所以在短时间内，车速可以看作是一个常数。车辆限速：（4.10）四轮小车的运动是由电机驱动的，电机的转矩是有限的，所以小车的加速度也是有限的，所以单位时间内小车速度的变化也是有限的：（4.11）在式(4.10)中，计算小车的最大加速度和减速度。如果环境中有障碍物，则需要在障碍物前方一定距离处停车。考虑到车辆的加速度，速度范围为：（4.12）式中，表示在采样空间中，当前采样值下距障碍物最近的距离。这种情况需要模拟汽车的运动轨迹。当观察到障碍物时，只有当前的采样速度才能允许车辆停在障碍物前面，此速度将被接受。小车速度的空间范围如式(4.12)所示：（4.13）确定最优轨道是必要的：(4.14)在公式(4.14)中，指的是方位角评价函数，用来评价四轮移动小车以当前速度到达模拟轨道末端时方向与目标之间的角度误差。是指到评价函数的距离，并计算出与障碍物之间的距离。如果此时道路上没有障碍物，这是一个常数。指的是评价函数的速度，在局部导航的过程中，小车不仅能避开障碍物，还能以更高的速度到达目标点。为了使评估