智能汽车测控技术 课件 第9章-第二讲 智能汽车路径规划算法

智能汽车测控技术第2讲智能汽车路径规划算法

1.基于图搜索

有了静态地图之后，想要在出发点和目的地之间寻找到最优路径，还需要合适的算法：第2讲智能汽车路径规划算法

1.基于图搜索一、基于图搜索的路径规划1、深度/广度优先搜索算法

此类算法通常将预先构建的环境地图抽象为有向图，并基于此图使用回溯技术实施搜索从而获得最终路径图是由顶点和顶点之间边的集合组成。若顶点Vi到Vj之间的边没有方向，则这条边为无向边，反之为有向边（或弧），其中Vi称为弧尾，Vj称为弧头。若图中所有边都为无向边，则该图为无向图，反之若都为有向边，则为有向图。相邻且有边直接连接的两个顶点称为邻节点第2讲智能汽车路径规划算法

1.基于图搜索

深度优先算法和广度优先搜索算法是两种代表性图搜索算法，此类算法将有向图抽象为树，从起始状态出发遍历树的节点，直至到达目标状态或者到达一个搜索终止点。如果到达目标状态，它退出搜索并返回解路径，如果到达一个搜索终止点，将回溯到路径上含有未搜索过的节点的临近节点，并沿着这个分支继续搜索下去图搜索流程图第2讲智能汽车路径规划算法

1.基于图搜索以深度优先搜索算法为例，说明算法在有向图中进行路径搜索的步骤：（1）创建一个栈来储存所有要访问的节点。（2）初始化栈，将起始位置s放入。（3）进入循环：移除：访问初始节点s后，将s移除；扩展：获取初始节点s的所有邻节点d、e、p；存储：把未访问过的d、e、p节点放入容器中；一直循环下去直到所有点被访问。（4）结束。第2讲智能汽车路径规划算法

1.基于图搜索

广度优先搜索与深度优先搜索的区别主要在于所使用的数据结构不同，深度优先搜索使用的堆栈（图中的[1]）会返回最后进入的节点，这使整个搜索一直朝一条路走，而广度优先搜索所使用的队列（图中的[2]）返回最先进入的节点则会使整个搜索呈现一层一层的搜索。第2讲智能汽车路径规划算法

1.基于图搜索深度越大的结点在深度优先搜索中越先得到扩展，广度优先搜索中深度越小的结点越先得到扩展。深度优先搜索：能够很快找到一条路径，但其找到的第1条路径往往是较长的路径，其优点在于即使在未知全局地图的情况下也能找到一条路径。广度优先搜索：呈波状推进方式，因此其能够保证搜索到的第1条路径是一条最短路径。第2讲智能汽车路径规划算法

1.基于图搜索2、Dijkstra算法

按照路径长度逐点增长的方法构造一棵路径树，从而得出从该树的根结点（即指定结点）到其他所有结点的最短路径。算法的主要步骤包括：（1）设置两个点的集合Sn和Tn，集合Sn中存放已找到最短路径的节点，Tn集合中存放当前还未找到最短路径的节点。（2）初始状态时，集合Sn中只包含起始点。第2讲智能汽车路径规划算法

1.基于图搜索（3）不断从集合中选择到起始节点路径长度最短的节点加入集合Sn中。（4）集合Sn中每加入一个新节点，都要修改从起始点到集合Tn中剩余节点的当前最短路径长度值，集合Sn中各节点新的当前最短路径长度值为原来最短路径长度值与从起始点过新加入节点到达该节点的路径长度中的较小者。（5）重复此过程，直到集合中所有结点全部加入集合中为止。第2讲智能汽车路径规划算法

1.基于图搜索在matlab中进行仿真实验，结果如图所示：其中A为起点，B为终点，A和B之间的连线是寻找到的最优路径，灰色点是算法执行过程中遍历到的路网节点，黑色为障碍物，可以发现，算法通过遍历定量的路网节点，避过障碍物，找到一条最优的无碰路径。Dijkstra算法作为经典的路径规划算法，在路径节点数量较小情况下会得到很好的规划结果，但是当节点数量较大时则难以满足路径规划的实时性要求。第2讲智能汽车路径规划算法

1.基于图搜索3、A*算法A*算法是一种建立在Dijkstra算法基础上的启发式路径搜索算法深度优先搜索和广度优先搜索等遍历性搜索：具有盲目性，效率较低，当搜索空间很大时或者存在未知状态时，有可能在有限时间内无法搜索到目标点。启发式搜索：会在搜索过程中能够利用含有有效信息的启发函数，使搜索方向更加智能地趋向于终点，所以该算法搜索的节点数少，效率相对遍历式搜索更快。第2讲智能汽车路径规划算法

1.基于图搜索A*算法的关键：确立如下形式的启发式估价函数，此函数的合理选择决定着算法能否快速找到最优路径：

式中，g(n)是从起点s到候选节点n实际代价；h'(n)是从候选节点n到目标点D的估计代价。必须保证h'(n)≤h*(n)，其中h*(n)表示结点n到目标点D的实际最小代价。第7讲智能网联汽车导航定位系统4.GPS

其中OPEN表用来记录己被访问但未计算出其代价的相关节点，CLOSE表用来保存已计算出其代价的节点。第2讲智能汽车路径规划算法

1.基于图搜索

下图所示为A*仿真实验结果，与Dijkstra算法结果图相比，此图中算法搜索过的灰色路径点更少，说明A*算法能够在搜索更少路径点的情况下获得最优路径。A*算法仿真实例Dijkstra算法结果图第2讲智能汽车路径规划算法

2.基于采样二、基于采样的路径规划基本思想：是在智能汽车的路径搜索空间中生成样本点，从中寻找满足任务需求的样本点序列作为规划结果。常见的采样方法：可分为随机采样和固定采样两类。下面将介绍RRT算法和RRT*算法这两种算法。第2讲智能汽车路径规划算法

2.基于采样1、RRT

快速探索随机树（Rapidly-exploringRandomTree，RRT）算法主要用于解决复杂和动态环境中含有动力学约束的机器人路径规划问题。

此算法采用随机采样的方法构建随机扩展树，无需遍历所有路径节点。第2讲智能汽车路径规划算法

2.基于采样

第2讲智能汽车路径规划算法

2.基于采样

缺点：如果地图中存在可行路径，基于搜索的路径规划算法可以通过反复迭代计算找到最优路径，而RRT算法的目标则是快速找到一条可行路径，而这条可行路径是有单个节点连接形成的，由于冗余的节点的存在和节点连接方式造成的曲折，该路径可能不是最优路径。此问题有很多方法解决，其中一种就是RRT*算法。第2讲智能汽车路径规划算法

2.基于采样2、RRT*与RRT的比较相同点：作为基本RRT算法的改进版本，RRT*算法同样通过在智能汽车可达空间的随机采样来构建树，并在新节点通过碰撞和非完整约束检测后将新节点连接到树上。不同点：RRT*算法在生成新节点时进行重新选择父节点和重新布线。比较基本RRT算法和RRT*算法，前者生成的树具有向各个方向移动的分支，而后者生成的随机树则很少有沿父节点向后移动的分支。第2讲智能汽车路径规划算法

2.基于采样RRT*伪代码：1T←InitializeTree()2T←InsertNode(ϕ,qinit,T)3Fork←1toNdo4qrand←RandomSample(k)5qnearest←NearestNeighbor(qrand,Qnear,T)6qmin←ChooseParent(qrand,Qnear,qnearest,△q)7T←InsertNode(qmin,qrand,T)8T←Rewire(T,Qnear,qmin,qrand)9End第1-2行初始化随机树并将起始位置添加到树中；第5行选择随机采样点的最近节点；第6行是重新选择父节点过程。第2讲智能汽车路径规划算法

2.基于采样RRT*中的改进：重选父节点：保证新节点路径成本相对最小重布线：保证增加节点后随机树的结构合理优点：这种两种操作能够不断改进搜索到的路径质量，经过多次迭代后RRT*算法会收敛到渐近最优解。缺点：但是这两种操作也增加了算法计算量。下面，将着重介绍重选父节点和重布线的流程第2讲智能汽车路径规划算法

2.基于采样

随机选取父节点过程：第2讲智能汽车路径规划算法

2.基于采样

第2讲智能汽车路径规划算法

2.基于采样重新选择父节点过程伪代码：1qmin←qnearest2cmin←cost(qnearest)+c(qrand)3Forqnear∈Qneardo4qpath←Steer(qnear,qrand,△q)5IfObstacleFree←cost(qnearest)+c(qrand)6cnew←cost(qnaer)+c(qpath)7Ifcnew<cnewthen8cmin←cnew9qmin←qnear10end11end12end13Returnqmin第2讲智能汽车路径规划算法

2.基于采样第2行，使用新节点qrand邻域内的节点qnear，考虑将邻域内的节点作为重新布线的候选者，调用Steer函数来获取从根节点到新节点qrand再到qrand邻域内的节点qnear的路径；第3行，进行碰撞和路径成本检测，如果此路径上没有障碍物以及路径的总成本比目前的路径成本低，则将新节点qrand选为邻域内节点的父节点；第4行，将树重新布线，以删除qnear与之前父节点的连线，并添加qnear与新节点qrand之间的连线；第7行，返回重新布线结果。1Forqnear∈Qneardo2qpath←Steer(qrand,qnear)3IfObstacleFree(qpath)andcost(qpath)+c(qpath)<Cost(qpath)then4T←ReConnect(qrand,qnear,T)5end6end7ReturnT重新布线过程伪代码第2讲智能汽车路径规划算法

2.基于采样

节点9是基于随机采样新生成的节点，其邻域内的节点有4、6、8的三个节点，它们的父节点分别是节点0、4、5。考虑新节点邻域内的节点，将邻域内节点的父节点变为新生成的节点9，如果这种更改能够降低当前的路径成本，则进行更改，若路径成本增加或不变，则保持原来的随机树的结构。将节点6的父节点改为新节点后路径成本降低，因此进行重新布线，更改了随机树的结构。下面来看一个重新布线的案例：第2讲智能汽车路径规划算法

3.基于人工智能三、基于人工智能的路径规划于人工智能的路径规划算法将诸如进化计算、人工神经网络、模糊逻辑与信息融合等现代人工智能技术应用于智能汽车的路径规划中，以克服传统路径规划方法的不足第2讲智能汽车路径规划算法

3.基于人工智能1、蚁群算法寻找食物的蚁群能够通过分泌一种称为信息素的生物激素交流觅食信息，从而快速找到目标使用网格离散化的方法对带有障碍物的环境建模，使用邻接矩阵存储该环境，使得问题转化为蚁群算法寻找最短路径基于信息正反馈原理第2讲智能汽车路径规划算法

3.基于人工智能蚁群算法流程图：蚁群算法路径规划技术通常与其他路径规划方法结合在一起使用，主要利用进化机制进行优化求解，单独完成路径规划任务的情况较少。第2讲智能汽车路径规划算法

3.基于人工智能2、遗传算法遗传算法是一种主要用来解决优化问题的智能算法，主要步骤包括种群初始化、适应度函数计算、选择、交叉和变异。算法流程图：第2讲智能汽车路径规划算法

3.基于人工智能（1）初始化主要分为两个步骤：（a）生成间断路径。

由于车辆每次行走一个栅格，每一行至少有一个栅格在可行路径中，因此初始化时按顺序在每一行随机取出一个无障碍栅格，形成一条间断路径，其中第一个和最后一个栅格分别为起点和终点。（b）将间断路径连接为连续路径。

第一个栅格开始判断相邻的两个栅格是否为连续栅格，判断方法为：第2讲智能汽车路径规划算法

3.基于人工智能（1）初始化主要分为两个步骤：（a）生成间断路径。

由于车辆每次行走一个栅格，每一行至少有一个栅格在可行路径中，因此初始化时按顺序在每一行随机取出一个无障碍栅格，形成一条间断路径，其中第一个和最后一个栅格分别为起点和终点。（b）将间断路径连接为连续路径。

从第一个栅格开始判断相邻的两个栅格是否为连续栅格，判断方法为：第2讲智能汽车路径规划算法

3.基于人工智能

式中，xi和yi分别为第i步所在栅格的横坐标和纵坐标。若D等于1则说明两个相邻栅格连续，反之不连续。对于不连续的栅格，取两个栅格的中点栅格，中点栅格坐标为：

若新栅格为障碍物栅格，则以上下左右顺序取新栅格的相邻栅格，并判断此栅格是否已经在路径中（防止陷入死循环）；第2讲智能汽车路径规划算法

3.基于人工智能若新栅格为无障碍栅格且不在路径中，则插入路径中；如果遍历上下左右四个栅格后没有满足条件的栅格，则删除这条路径；若新栅格为无障碍物栅格，则插入两个不连续栅格中间。继续判断新插入的栅格和新插入的栅格的前一个栅格是否连续，若不连续则循环以上步骤，直到两个栅格连续。当两个栅格连续后取下一个栅格，循环以上步骤，直到整条路径连续。第2讲智能汽车路径规划算法

3.基于人工智能初始化流程图：第2讲智能汽车路径规划算法

3.基于人工智能2.适应度函数计算适应度函数分成两部分，分别用来判断路径的长短和平滑程度第一部分：路径长度的计算公式为：第2讲智能汽车路径规划算法

3.基于人工智能第二部分：

计算路径平滑度时，由于车辆运动学和动力学约束，车辆行进时拐弯不宜过大，并且相对平滑的路径有利于车辆行驶，因此产生的路径有平滑度的要求。路径越平滑，相邻三点形成的角度越大，角度越大相邻三点之间的距离越大，因此适应度函数的第二部分需要计算路径中所有相邻三点的距离，计算公式为：第2讲智能汽车路径规划算法

3.基于人工智能

路径中连续三点的夹角可分为180度、钝角、直角和锐角四种情况，其中呈180度的三点在一条直线上，平滑度最好，钝角、直角次之，由于车辆动力学约束，不允许出现锐角，因此对除直线外的其他三种情况设置惩罚系数，可以根据实际情况改动惩罚的大小。惩罚之和的倒数即为适应度函数的第二部分：最后通过下式确定路径长度和平滑度在适应度函数中的权重：

第2讲智能汽车路径规划算法

3.基于人工智能3、选择方法

采用基于概率的轮盘赌方法