【《仓储自动小车路径规划算法设计案例分析》8600字】

仓储自动小车路径规划算法设计案例分析目录TOC\o"1-3"\h\u27719仓储自动小车路径规划算法设计案例分析 1274471.1引言 125681.2基于A*算法的全局路径规划 145151.3基于DWA算法的局部路径规划 8270991.1.1车体运动模型建立 86491.1.2速度采样 9187331.1.3评价函数 1095061.1.4改进的DWA算法仿真 1171221.1.5分析算法实验结果 1124421.4全局规划算法与局部规划算法的融合 17140111.4.1DWA用于局部路径规划的不足 17186971.4.2A*算法与DWA算法的融合 1735031.4.3融合A*算法的DWA算法的实验 181.1引言全局规划是一种静态环境下的规划而局部路径规划是一种动态环境下的规划，通过实验的小车的每时每刻的位置来设计最佳路径路径，而DWA算法则是控制小车避开障碍物。与其他局部路径规划算法相比，DWA算法具有许多优势，比如系统稳定性和避障能力。算法有许多部分组成，但是最关键的是评价函数，而评价函数中最关键的是每个参数的权值。所以，全局路径规划A*算法算法和局部路径规划DWA算法各有优缺点。整个小车路径规划技术中，我们需要将两者结合起来取长补短，追求更好的行驶效果。1.2基于A*算法的全局路径规划A*搜索算法是全局路径规划算法是一种启发式搜索算法，依据基本代价启发函数来计算出目标最优路径，不仅计算了从行驶路径初始结点到行驶路径当前结点的代价值，同时还计算了行驶路径当前结点一直到最后行驶路径目标结点的启发值。启发式算法搜索下一刻节点时，根据启发函数搜索代价最小的下一节点。这与盲目的搜索相比，具有显著的效率优势。Dijkstra算法是将初始节点到目标节点之间的路径自由延申拓展。Dijkstra算法的第一要务就是搜索找到并给出从初始节点到最终节点的最简洁的行驶路径。但是，Dijkstra算法本身属于随机数据搜算法，而在给定的范围内进行随机搜索是一件非常困难的任务，实现该算法的成效非常的低，耗时巨大，在实际应用中也会被大大的限制。BFS(BestFirstSearch)算法基于广度优先搜索出的所有理论数据，通过启发函数估测即将被遍历的节点，然后根据最小代价节点，从而搜索到目标节点。BFS算法的主要目的是估测随机的一个节点到目标节点所消耗的代价，并选择一个最接近目标节点的节点，以此来迅速的导向目标节点。以上的两种算法都有各自的优缺点，其中BFS运行起来比较快，但却不能确保找到一条最优的路径，另一个Dijkstra算法则反之，它能找到一条最优路径，但却要消耗大量的时间。而A*算法拥有这两个算法的优点，A*算法是将两个算法互补之后诞生的具有两个算法优势的一种新算法。A*算法的一个核心组成部分就是对于代价函数的设计，其中所有代价函数的计算公式如下表示。F(n)=H(n)+G(n)(2.1)当前节点n的代价值由F(n)表示，G(n)表示从初始节点到当前节点n的实际代价值。H(n)是一个启发函数，它表示当从前节点n到目标节点最短距离的代价值。另外，将H(n)和G(n)的代价值做和时，需要使用一样的衡量路径的单位。G(n)值为定值，表示的是初始节点到当前节点的实际距离，但H(n)的取值是一个估测值，需要分两种情况讨论：①当H(n)=0时，F(n)=G(n)，此时A*的算法就相当于Dijkstra算法；②当H(n)远大于G(n)时，F(n)=H(n)，A*算法相当于BFS算法。同时，需要考虑H(n)的值与实际值的大小差距。所以，我们假设一个函数D(n)表示从当前节点到目标节点的实际代价值，然后分析以下几种情况:1)如果H(n)<D(n)，此时A*算法的搜索区域范围较大，效率较低，但最终可以获得一个的最优解;2)如果启发函数H(n)=D(n)，即启发函数的距离估算值D(n)大致就是真正的距离，该路径就会是一个距离最短的路径，此时搜索结果的效率也是最高。不过此类情况的发生概率非常小，几乎都是不可能事件。特殊的情况下如果让H(n)准确地等于实际的距离数值，A*算法将一定会使它们运行得很完美。3)如果H(n)>D(n)，则A*算法的搜索范围会缩小，从而提高搜索效率，但得到的解却不一定最优，这种情况应当避免。因此，如果使用A*算法那么最关键的是启发式函数H(n)。启发函数H(n)越合理，A*的路径规划便会更快捷、更高效。在坐标系中起始节点的坐标设为(xA,yA)，目标节点的坐标设为(xB,yB)，当前目标结点的坐标设为(xN,yN)，H(n)的代价系数设为D。下面是A*算法的四种启发函数模型:1)曼哈顿距离是指横纵方向之间的距离值的和。允许向4邻域(也就是上、下、左、右三个方向)的运动，启发函数可以用表达式定义为：H(n)=D*[abs(XB-XN)+abs(YB-YN)](2.2)2）切比雪夫距离指的是两个分量差值的最大值。用启发函数来表示：H(n)=D*max[(XB-XN)，(YB-YN)](2.3)3）欧氏的距离是最常见的。它指的是两点之间的距离，其公式为：H(n)=D*sqrt[(XB-XN)2+(YB-YN)2](2.4)对角线的距离是指可以沿着一条对角线移动的距离再加上不能沿着另一条对角线移动的距离，允许向8邻域进行移动，启发式的搜索函数可以用下列方式表示H1(n)=min[abs(XB-XN),abs(YB-YN)](2.5)H2(n)=abs(XB-XN)+abs(YB-YN)(2.6)H(n)=√2*D*H1(n)+D*[H2(n)-2*H1(n)](2.7)1.2.1改进的曼哈顿距离函数根据上文,把曼哈顿的距离做出改善,加权。在A*算法中，启发函数为曼哈顿距离时，结点距目标结点的距离有|xB-xN|>=|yB-yN|和|xB-xN|<|yB-yN|两种情况，所以设计不同的代价系数D来判断当前结点n到目标结点的横纵距离大小，具体计算如下：当|xB-xN|>=|yB-yN|时，H(n)=D1*|(xB-xN)|+D2*|(yB-yN)|(2.8)当|xB-xN|<|yB-yN|时，H(n)=D2*|(xB-xN)|+D1*|(yB-yN)|(2.9)考虑到实际的水平和垂直距离接近目标时的移动速度，实验从距离大的方向搜索，达到接近前目标节点至目标节点的实际距离，更接近设计出一条最优路径（一般曼哈顿距离通过水平和垂直方向的平移到达目的地所经过的方格数乘以10来得到H值）。可以判断此时状态为0<D2<D1<=10。如果D2>D1会出现大量结点都被搜索的情况,这种情况下,虽然也有可能让我们得到最优的路径,但是会直接导致算法搜索时间很大;如果D1,D2大于10便不能确认地保证H(n)的值远远小于其实际距离的值,不一定可以找到最优的路径;若D1=D2启发函数已经在某个程度上变成了曼哈顿的距离。利用模型障碍地理环境分析图，通过大量的科学实验和仿真，研究了各数据模型中D1和D2值对模型算法实际数据效率的直接影响。图2.5是一个改进的模型，它揭示了数据搜索中，搜索时间与D1和D2值之间的相互作用的对应关系。结合测试图2.5和仿真结果，可以清楚地发现，改进之后的曼哈顿A*算法能够有效地帮助搜索者找到最优的开发路径。但是D1的取值不能太小，否则可能会直接影响搜索函数值G(n)。代价函数F(n)更新时，函数值需要直接关注当前搜索节点和初始搜索节点之间的距离。这样，使用A*算法的每个搜索结果的持续时间可能会变长，如下图2.5所示。研究发现，在系统设计中，当代价系数D1＝10，D2＝8时，改进的路径启发式函数搜索寻找的路径最优，且过程花费时间较短。从以下两个仿真结果能看出，改进的曼哈顿距离在路径的选择以及搜索所花费的时间都是最佳的。图2.5搜索时间与代价系数关系Figure2.5TheRelationshipbetweenSearchTimeandCostCoefficient图2.6、2.7、2.8、2.9、2.10中的（a）和（b）是五种A*路径规划算法的不同寻路方式，灰色代表障碍物，绿色代表起点，红色表示终点，蓝色代表搜索点节点，边缘节点也用了色表示，数字为代价值。可以清楚地看到，使用加权算法得到曼哈顿最短路径时间的算法不能有效地保证我们找到最优路径，函数算法的搜索节点数小于其他搜索路径，搜索时间也小于其他搜索路径。实验图可知，BFS与Dijkstra搜索过多的无用节点（与目标节点距离相差过大）花费大量时间，切比雪夫和欧式距离则优于上两种算法，但是也过多的搜索了无用节点，改进的曼哈顿算法的A*寻路效效率最高，路径最优，消耗时间最少，可见，利用加权曼哈顿算法的A*寻路，可以精确地规划和设计最优时间路径。图2.6BFS路径仿真Figure2.6BFSPathSimulation图2.7Dijkstra路径仿真Figure2.7DijkstraPathSimulation图2.8切比雪夫路径仿真Figure2.8ChebyshevPathSimulation图2.9欧式距离路径仿真Figure2.9EuclideandistancePathSimulation图2.10改进的曼哈顿路径仿真Figure2.10ImprovedManhattanPathSimulation在该地图环境下,采用了四种路径规划方法和加权曼哈顿时间法距离的A*算法来寻路的性能对比如表2.2所示。表2.2路径规划算法的性能对比Table2.2Performancecomparisonofpathplanningalgorithms搜索节点数量搜索时间（s）BFS4083Dijkstra4302.2切比雪夫3561.0欧式距离3501.1改进的曼哈顿2980.9图2.6、2.7、2.8、2.9、2.10和表2.2所示的路径规划算法性能表明，改进的启发式函数可以保证找到最优路径，并且在搜索节点数和搜时间上都优于其他四个启示式函数，更是减少了的平均搜时间，说明启发式函数具有最佳的寻路性能，具有一定的优势。1.3基于DWA算法的局部路径规划1997年，DWA动态窗口的部分避障思想提出，对自动行驶小车具有重大意义。DWA算法的实验周期需要严格的把控[25]，在该周期内充分考虑小车的运动模型，初始速度，软硬件性能，以及外部环境，达到小车在当前环境下的速度采样空间。接着，每组速度的行驶轨迹需要根据评价函数来进行实验验证，自动行驶小车下一时刻的行为的决策需要根据动态窗口的最优评价值的速度参数来设计规划行驶路径[26]。1.1.1车体运动模型建立DWA算法中，最重要的是车体在一个周期内的行驶路径，我们需严格推算得出参数值。我们使用两轮差分驱动的自动行驶小车，它的动力来自底盘两侧的动力轮，为了实现车体改变方向，我们将左右轮子的速度设定为不一样的。小车行驶中会有许多误差，为了降低这些误差，我们模拟车体在特定的一段时间中的行驶路径为圆形，图1.1是行驶模型。所以得出相匹配的速度参数来描绘该轨迹，t时刻行驶路径可以用(vt,wt)来表示。公式推算参考下面：图1.1二轮差分小车行驶模型Figure1.1TwoWheelDifferentialVehicleMovingModel设小车移动速率为r，转速为w，Δt时间段内车体行驶的路径角度为θ。每个时段Δt的行驶半径为：(1.1)当转速w≠0时，下个时段小车的位移和行驶方向的角度为：.(1.2)(1.3)(1.4)1.1.2速度采样速度采样是DWA算法中较为关键的程序。我们依照图1.1的建模以及对小车的行驶路径的估测，设定小车行驶在二维空间中，因此速度空间(vt,wt)存在无穷多组移速，我们把采取到的样本移度[27]规定在一定限制中。①受根据小车最大、最小速度的限制Vm={v[vmin,vmax]，w[wmin,wmax]}(1.5)式中车体最小线速度为vmin，最大线速度为vmax；车体最小角速度为wmin，最大角速度为wmax。②电机性能对小车的影响之所以小车的极大极小加减速率会受到控制，是因为发动机性能的能力大小不同，不同行驶时间段内，有一个动态窗口[28]，该窗口可以显示现实中一定范围内小车所能加速到的最大行驶移速。动态窗口采取样本的行驶路径如图1.2，红色表示路障。其中vc，wc分别是车体当前的线性运动速度和车体的角速度。vb,wb分别是车体当前最大线速度加速度和最大角速度加速度。(1.7)图1.2采样样本的行驶路径图Figure1.2Drivingpathdiagramofsampledsamples③车体安全距离因为要保障小车安全行驶，在路障面前提前暂停行驶，所以我们要控制小车的加速度保证速度在一定限制内[29]：(1.7)假设移速为(v,w)，dist(v,w)为该速度对应的行驶路径上最近障碍物的距离。最后把速度控制在范围vr内：(1.8)1.1.3评价函数根据每组采样的时间和速度，有许多组轨迹都位于窗口vr内，这是被允许的，但小车在下一时间的行驶状态只能由一条行驶轨迹来控制。每一条行驶轨迹都用评价函数来进行评估，该函数如下：(1.9)heading(v,w)表示在当前行驶速率下小车的目标节点和估测节点之间的角度之差。根据图1.4，heading

(v,w)的表达式为(1.10)(1.11)随着小车的行驶，每一时刻的方位随之而改变，估测方向和终点方向之间的角度为ε。当目标在车体的行驶方向上，那么这时heading(v,w)的取值为最大值。车体的方向设为θ，终点相对于小车的方向设为φ。图1.4终点与估测位置之间的角度Figure1.4AnglebetweentheFinalPointAndThePredictedPoint小车的行驶轨迹与离它最近的路障的距离为dist(v,w)，dist(v,w)能够被设置为一个较大的常数，前提是小车在行使路径不会遇到任何路障。针对小车实时行驶轨迹的移动速度和质量进行评价，设为velocity(v,w)。为了减少评价函数中因偏重某项而存在较大误差，对评价函数进行改进，把归一化之后的三个指标的参数值全部相加。(1.12)(1.13)(1.14)在这个评价函数中，采取样本的全部行驶轨迹设为n，当前行驶轨迹设为i。所以，根据路径规划算法，选取小车移动速度的标准是DWA算法的主要评价函数之一。我们要保证选定的移动速率最大可能的和路障保持最大距离，然后尽力向终点行驶，以更快的速度行驶。1.1.4改进的DWA算法仿真①参数需要初始化；②按照当前小车的参数，计算速度空间；再根据车体自身各方面性能的限定，对下一时间所有可行驶的搜索空间进行预测。③根据上述评价函数，从中选择最好的一组解。④根据最好的解从而限制下一时间的小车的行驶轨迹。⑤如果小车到达了目的地，那么终止这个算法的运算；反之如果小车没有按照设计抵达目的地，就重复①-④的算法步骤。1.1.5分析算法实验结果根据模拟的地图，首先假设出发点的位置为（0,0），接着假设小车方向角度、移动速度、角速度分别为yaw，v，w均为0，目的地的位置为（10,10）。除了上述描述，还需要设置路障半径obstacl_R采样时间dt=0.1s，轨迹向前模拟时间sim_time=3s。小车行驶的最高的移动速度max_vel=0.5m/s，小车行驶的最大转速为max_rot_vel=30rad/s，最大加速度acc_lim=0.2m/s2，最大角加速度acc_lim_th=20rad/s。解析评价函数的三个重要参数的权重α，β，γ的选择数值范围。图1.5α=0DWA算法仿真地图Figure1.5α=0SimulationMapofDWAAlgorithm在DWA算法中，小车最大限度的朝着终点行驶移动的项由heading(v,w)来表示。如果这个heading(v,w)取值为零，小车会产生原地转圈的现象，因为丢失了终点的目标，如上图1.5。首先令γ=0.2保持不变，对α，β两个参数进行试验分析。在范围α=(0.01-0.1)，β=(0.1-0.3)条件下。图1.6和1.7表示的是实验仿真的结果数据，如果当α，β的取值不同时，小车抵达目的地所消耗的时间和行驶的路程距离。图1.6参数时间折线图图1.7参数长度折线图Figure1.6TheSimulationTimeFigure1.7TheLengthofParametersParameters我们根据以上实验结果数据可以得到，如果当评价函数的两个参数γ，β的取值不变的情况下，另一个参数α值越大，实验的运行的时间越久，行驶路径距离越远，则设计的路径规划算法的效率越低。然而参数值α越大，小车则更有可能从当前所在地点向终点的连线直线行驶，假设这条路线上刚好有路障存在，那么小车一定要减速前进且一直不停地调试移动的角度，以此来避免与路障产生接触。图1.8α=0.09，γ=0.2实验结果图Figure1.8α=0.09，β=0.2Experimentresults我们注意到如上图所示选取的参数值为α=0.09，β=0.2，小车会发生停留在原地不前进的状况，这是因为这个小车距离路障的位置过于接近才选择调试移动角度，这一现象会花费过多的时间来调整小车的行驶的角度，这等同于降低该路径规划算法的运行效率，如上图1.8。DWA算法中的dist(v,w)，可以让小车尽可能的与路障保持距离，减少与路障产生接触的几率。我们根据上面实验对α的取值进行解析，把α的值设为0.01，把γ的值设为0.2，从而得出β的取值决定该路径规划算法的效率。实验中我们得出结论β的取值越大，DWA算法计算所要消耗的时间越长，设计出来的路径距离就会更远，如果当α=0.01，β=0.1，那么DWA算法可以达到最佳。我们发现在β=0的时候，DWA算法依旧可以设计出一条行驶路径，但是结果并非完美。图1.9DWA算法性能参数变化图Fig.1.9DWAAlgorithmicSmestersIndicators我们可以看到上图1.9为β变化时，DWA算法的计算时间和行驶路程长度的实验结果，上图中红色的线表示根据DWA算法运行仿真出来的路径长度，黑色的线表明的是该算法实验过程的所消耗的时间。这就告诉我们一旦β的取值过大，小车就会超出预想的远离路障，大大增加算法的计算成本，降低效率。我们发现在评价函数中能够让小车在局部行驶的过程中尽可能的保持高速由velocity(v,w)来决定，以此来提高行驶的效率。选择上述实验结果得到的最佳取值α取值为0.01，β取值为0.1设定为定值，γ可以变化越大，得出结论算法实验所消耗的时间和路程长短变化并不明显。然而当γ的取值为0.2，实验所消耗的时间和路程距离方面的性能最佳，如下图1.10。如果当γ的取值非常的小的情况下，评价函数velocity(v,w)并不能为小车运行行驶的路径速度给予更优的解。如果当γ的取值趋近为0，也就是说驱动小车行驶的速度项为0，小车会在出发点停止不动，如下图1.10。但是一旦评价函数的参数γ的取值偏大，小车行驶车速更大的路径和时间便会占据评价函数更多资源与权重，此时此刻一旦小车的行驶路线上存在路障，那就很难及时的进行小车的制动和避障，甚至会导致小车消耗极大的时间停留在路障面前，这样会导致实验算法消耗的时间越来越长，极大的影响小车行驶时算法计算的效率。所以我们得出结论γ的取值必须和每个路段的不同地图所处的环境相对比分析，然后根据实验结果取最佳值。图1.10算法性能参数变化图Fig.1.10AlgorithmicPerformanceIndicators通过实验利用TOPSIS法来选择加权系数，设这三个加权系数的向量为di{ai1,,ain}(i取1，2，3分别表示α，β，γ),决策矩阵A=(aij)3*n，规范化决策矩阵B=(bij)n*3。其中： (1.15)构成加权规范矩阵C=(cij)3*n，设给定的个属性的权重向量为：W=[w1，wn]T，则：(1.16)c*j为第j个正理想值，c0j为第j个负理想值，那么di的正理想值，负理想值的距离分别为：(1.17)(1.18)计算综合评价指标：(1.19)最后由大到小排列，即为优劣顺序。发现α取值0.01，β取值0.1，γ取值0.2时，DWA算法的运行效率最高，将初始数据仿真图（α取值0.005，β取值0.2，γ取值0.2）与最佳数据仿真图做对比，如下图1.12、图1.13。图1.12局部路径规划初始结果Figure1.12AlgorithmPathPlanningInitialResults图1.13局部路径规划优化结果Figure1.12AlgorithmPathPlanningOptimizedResults通过上述实验得出实验结论如果α的取值偏大，很有可能使小车从当前所在地点向目的地保持直线行驶，这样就会导致小车遇到路障时避开不及时，不仅出现危险，还会消耗过多的时间去运算；一旦β的取值偏大，则会导致小车在躲避路障时超出预期的远离路障，大大加长行驶的有效路径，那便不是最佳路径了；同时我们还发现γ的取值需要保持在一个均衡的水平，随着所处位置的地图的变化，必须调试变化它的取值。如果把距路障的距离和小车的行驶角度进行无边缘化，小车将变得没有任何限制会在给定的地图中自由行驶。但是我们如果单独把小车的行驶角度最大化朝着我们的目的地设定，小车便会以最快速度抵达第一个路障并且停止不前，导致路径规划失败。根据上述分析我们得出结论只有将上述三个参数在小车的算法里面达到均衡才能将路径规划的效率提升到最高。1.4全局规划算法与局部规划算法的融合1.4.1DWA用于局部路径规划的不足DWA算法是我们所熟悉的局部路径规划算法，全局的行驶路径规划不属于它的运算范畴，并且对运行速率会有最大优先级。但是，如果小车行驶到特殊的地段，例如U型地段，那么DWA算法显然就不合适了，DWA算法设定的局部最优路径和上文所阐述的全局最佳路径有区别，它不会考虑全局最优，还有很大机率停止不前，陷入局部最优的缺陷，如下图1.13。目的地的位置有红色的星号代替，路障为粉色物体，局部路径规划由蓝色的线条代替，该图就表明了小车进入了局部最优，陷入了局部路径规划的困境。图1.13DWA路径规划效果图Figure1.13DWAAlgorithminComplexEnvironment1.4.2A*算法与DWA算法的融合在我们的生活中，绝大部分地段全局路径规划是一个粗糙的不精确的规划，它只能给定一个出发点到终点的路线而已，没有考虑宽度、曲率、车身尺寸以及未知障碍物等细节。与此同时，小车会因为环境的变化与自身状态的变化行驶的状态也会逐渐发生改变，并不能一直保持，缺少了实时更新的能力。局部路径规划一般根据每一个感应器建立的实时局部地形图环境来达到最佳行驶路线。所以，在小车行驶的路线中我们需要线计划出一条全局路径再细化到局部路径，根据环境及动态障碍物设计出一条再规定地图内不会与路障发生摩擦的路线。我们把这两种路径算法结合，就可以极大的弱化他们各自的不足，融合成一个全新的更加完善的路径规划算法，能够规划出更完美的自动行驶小车行驶路线。全局路径规划方法使用加权曼哈顿距离的A*算法，根据静态路线模型设计了小车从出发点到终点的大致行驶路线。DWA算法主要采用局部路径规划方法，依照地图上的小车在行驶过程中接收到的路障信息来每时每刻设计全新的路线，以此来躲避路障。为了尽可能避免陷入局部最优，我们将改善DWA算法的部分缺陷，根据全部最佳行驶路线的长短来评价该算法的可行性。因此，DWA算法评价函数改进如下。(1.15)式中α，β，γ，δ分别为heading(v,w)、dist(v,w)、velocity(v,w)和pdist(v,w)项的权值。heading(v,w)用来评价当前估测节点和目标节点之间的角度差。pdist(v,w)可以得出小车实际轨迹和全局最优轨迹之间的距离。velocity(v,w)用来得出当前小车行驶路线的速率。dist(v,w)用来确定小车当前节点和与之最近的障碍物的距离。当然我们也要对G(v,w)函数采取优化处理方法，将其四个参数值归一化后再相加。heading(v,w)，dist(v,w)，velocity(v,w)三项的归一化公式为（1.12）、（1.13）、（1.14）。式（1.16）为pdist(v,w)的归一化公式。(1.16)上述公式中，采取样本窗口vr的全部行驶轨迹设为n，还未被评价的行驶轨迹设为i。将DWA算法的中引入一个A*算法的权重值，A*算法的权重值又为他自身的代价函数值，以此来达到两者算法的融合。1.4.3融合A*算法的DWA算法的实验我们把DWA算法与A*算法进行融合改善。算法流程图如1.14。上一章依照模拟地图上设定的出发点、终点和路障点，利用A*算法规划出一条全局路径，然后利用集成A*算法的DWA算法进行路径规划，输出速度指令控制车身行驶到目标点。开始开始初始化参数将初始节点放入openlist，计算初始化参数将初始节点放入openlist，计算F(n)取值Openlist是否为空Openlist是否为空是计算速度搜索空间是计算速度搜索空间否否将将F(n)最小的节点删除，放入closedlis