第五章-无人驾驶驾车路径规划课件.pptx

1、第五章 无人驾驶驾车路径规划 路径规划通常指全局的路径规划，也可以叫全局导航规划，从出发点到目标点之间的纯几何路径规划，无关时间序列，无关车辆动力学。避障规划又叫局部路径规划，又可叫动态路径规划或即时导航规划。主要是探测障碍物，并对障碍物的移动轨迹跟踪（MODAT）做出下一步可能位置的推算，最终绘制出一幅包含现存碰撞风险和潜在碰撞风险的障碍物地图，这个潜在的风险提示是100毫秒级，未来需要进一步提高，这对传感器、算法的效率和处理器的运算能力都是极大的挑战，避障规划不仅考虑空间还考虑时间序列，在复杂的市区运算量惊人，可能超过30TFLOPS，这是无人车难度最高的环节。未来还要加入V2X地图，避障

2、规划会更复杂，加入V2X地图，基本可确保无人车不会发生任何形式的主动碰撞。轨迹规划则源自机器人研究，通常是说机械臂的路径规划。在无人车领域，轨迹规划的定义感觉不统一。轨迹规划应该是在路径规划和避障规划的基础上，考虑时间序列和车辆动力学对车辆运行轨迹的规划，主要是车纵向加速度和车横向角速度的设定。将设定交给执行系统，转向、油门、刹车。如果有主动悬挂，那么轨迹规划可能还要考虑地形因素。三大规划是无人车最复杂的部分，算法多不胜数，让人眼花缭乱，这也是百度、谷歌和苹果科技巨头要切入无人车领域的主要原因，这些科技巨头最擅长的就是算法的优化整合。当然传统车厂如福特和丰田，拥有对车辆动力学的绝对优势，在此领

3、域实力并不比科技巨头要差，尤其是丰田，从开源 SLAM到KITTI，软件实力丝毫不次于谷歌。路径规划技术是无人驾驶研究领域中的一个重要分支。所谓无人驾驶的最优路径规划问题，就是依据某个或某些优化准则(如工作代价最小、行走路线最短、行走时间最短等),在其工作空间中找到一条从起始状态到目标状态的能避开障碍物的最优路径。路径规划主要包含两个步骤：建立包含障碍区域与自由区域的环境地图，以及在环境地图中选择合适的路径搜索算法，快速实时地搜索可行驶路径。路径规划结果对车辆行驶起着导航作用。它引导车辆从当前位置行驶到达目标位置。依据某种最优准则，在工作空间中寻找一条从起始状态到目标状态的避开障碍物的最优路径

4、。（1）车辆所处的环境复杂度高。因此传感器检测返回的数据复杂，障碍物个数种类多；路况信息复杂，在高速上一秒钟车辆能跑出30米，情况瞬息万变。（2）系统对规划模块的要求高。规划模块需要覆盖所有的ADAS场景，对模块计算的实时性和稳定性有着很高的要求。（3）这是一个全新的未知领域，能够从外界获得的信息有限。虽然有过去长期的积累，但是依然有多重难题需要解决。（1）静态结构化环境下的路径规划，比如说slam把图做好了之后，在已有的图上进行路径规划，已知地图进行全局规划。（2）动态已知环境下的路径规划，比如说地图已知了，但是地图中有一些可以移动的障碍物，这些障碍物在一定时间内它的信息是未知的。（3）动态

5、不确定环境下的路径规划，环境是未知的，它要求机器人自己去生成一条路径，然后主动去探索环境。A A*算法是一种在路网上中求解最短路径的直接搜索寻路算法算法是一种在路网上中求解最短路径的直接搜索寻路算法,原理是原理是引入估价函数，加快搜索速度，提高了局部择优算法搜索的精度，成为当引入估价函数，加快搜索速度，提高了局部择优算法搜索的精度，成为当前较为流行的最短路算法。前较为流行的最短路算法。从从A A移动到移动到B B，绕过障碍。，绕过障碍。划分空间划分空间,简化搜索区域。简化搜索区域。空间被划分为二维数组，数组中空间被划分为二维数组，数组中每个元素代表空间中的一个方格，每个元素代表空间中的一个方格

6、，可被标记为可行或不可行。未来可被标记为可行或不可行。未来的路径就是一系列可行方块的集的路径就是一系列可行方块的集合。合。NodesNodes的概念涵盖了一系列的概念涵盖了一系列可行方块（或其他形状）。可行方块（或其他形状）。步骤：从节点A开始，搜索其临近节点，直到找到目标点 步骤A)；从节点A开始，把一系列待考虑的节点放入OpenList里面，OpenList存放着一系列需要检查的节点（方块），如图：首先检查起点周围的8个节点，给每个节点赋值F=G+H，G表示从初始点到给定待查节点的距离（可多种距离量度），H表示从给定待检查节点到目标点B的距离（可多种距离量度）（Heuristic计算时忽略

7、到达目标点会遇到的障碍）。步骤B）：找到F值最小的节点作为新的起点。将它从OpenList中删除，加入到ClosedList中的节点和不可行节点（即障碍），如果临近节点已经在OpenList里面，则对比一下是否从现节点到临近节点的G值比原G值小，若是，现节点作为父节点。若不是，则不做改动。步骤C)：上步骤中新节点未造成任何改动，我们继续在OpenList中寻找新的节点。如图重复步骤a)b),直到找到目标节点。从目标节点回溯可以找到初始点，从而确定路径。f(n)=g(n)+h(n),其中n为在路径中最后一个节点。g(n)是从起始节点到n节点的代价，h(n)是用来估计从n节点到目标节点代价的启发函

8、数。评估函数由具体问题而定。为了寻找到实际的最短路径，启发函数必须是可容许的，即启发函数永远不会对到达最近目标节点的实际代价做出过高估计。Dijkstra算法是典型的广度优先搜索算法。E.W.Dijkstra于1959年提出了Dijkstra算法。它是一个按路径长度递增的次序产生最短路径的方法，是求解最短路径的经典算法之一。Dijkstra算法是一种贪心算法。贪心算法的原则是在每一步都选择局部最优解，以期望产生一个最优解。它的突出优点在于不仅求出了起点到终点的最短路径及其长度，而且求出了起点到图中其他各节点的最短路径和长度，但由于它遍历计算的节点很多，所以效率低。Dijkstra算法的核心思想

9、为：设置两个节点的集合S和Tn，集合Sn中存放已找到最短路径的节点，集合Tn中存放当前还未找到最短路径的节点。初始状态时，集合Sn中只包含起始点，然后不断从集合Tn中选择到起始节点路径长度最短的节点加入到集合Sn中。集合Sn中每加入一个新的节点，都要修改从起始点到集合Tn中剩余节点的当前最短路径长度值。集合Tn中各节点新的当前最短路径长度值为原来最短路径长度中的较小者。不断重复此过程，直到集合Tn中所有节点全部加入到集合Sn中为止。Dijkstra算法过程包含了3个循环：第一个循环的时间复杂度为O（n）,第二、三个循环为循环嵌套，因此总的时间复杂度为O(n)。可以看出，Dijkstra最短路径

10、算法的执行时间和占用空间与图（或网）中节点数目有关。当节点数目n较大时，Dijkstra算法的时间复杂度急剧增加。当图（或网）规模较大时，直接应用该算法就会存在速度慢或空间不够的问题。所以，在大的城市交通网络图中直接应用Dijkstra最短路径算法是很困难的。路径规划作为无人驾驶汽车导航系统的重要功能模块，其算法的优劣是非常重要的。评价该算法的主要性能指标是它的实时性和准确性。Dijkstra算法作为经典的路径规划算法，在实验地图数据量较小情况下回得到很好的规划结果，但在实验地图数据量较大情况下很难满足路径规划的实时性要求。A*算法加入了启发函数，用于引导其搜索方向，A*算法会比Dijkstr

11、a算法规划速度快不少。人工势场法路径规划是由Khatib提出的一种虚拟力法（Oussama Khatib，Real-Time obstacle Avoidance for Manipulators and Mobile Robots.Proc of The 1994 IEEE.）。它的基本思想是将机器人在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求合力来控制移动机器人的运动。应用势场法规划出来的路径一般是比较平滑并且安全，但是这种方法存在局部最优点问题。为了解决这个问题,许多的学者进行了研究，如Rimon,Sh

12、ahid和Khosla等。他们期望通过建立统一的势能函数来解决这一问题。但这就要求障碍物最好是规则的，否则算法的计算量将很大，有时甚至是无法计算的。但是，从另一个方面来看，由于人工势场法在数学描述上简洁、美观，这种方法仍然具有很大的吸引力的。其内在的局限性主要表现在，即当目标附近有障碍物时，移动机器人将永远也到达不了目的地。在以前的许多研究中，目标和障碍物都离的很远，当机器人逼近目标时，障碍物的斥力变的很小，甚至可以忽略，机器人将只受到吸引力的作用而直达目标。但在许多实际环境中，往往至少有一个障碍物与目标点离的很近，在这种情况下，当移动机器人逼近目标的同时，它也将向障碍物靠近，如果利用以前对引

13、力场函数和斥力场函数的定义，斥力将比引力大的多，这样目标点将不是整个势场的全局最小点，因此移动机器人将不可能到达目标。这样就存在局部最优解的问题，因此如何设计“引力场”问题就成为该方法的关键。常用的引力函数：这里的是尺度因子.(q,q_goal)表示物体当前状态与目标的距离。引力场有了，那么引力就是引力场对距离的导数（类比物理里面W=FX）：）（goalattqqqU,21)(2)()()(qqqUqFgoalattatt上式是传统的斥力场公式。公式中是斥力尺度因子，（q,q_obs）代表物体和障碍物之间的距离。_0代表每个障碍物的影响半径。换言之，离开一定的距离，障碍物就对物体没有斥力影响。

14、斥力就是斥力场的梯度：0020),(.0),(.1121)(obsobsobsrepqqifqqifqqqU），（0020),(.0),().,(),(1)1),(1()()(obsobsobsobsobsreprepqqifqqifqqqqqqqUqF 总的场就是斥力场合引力场的叠加，也就是U=U_att+U_rep,总的力也是对对应的分力的叠加，如下图所示：优点：简单实用，有良好的实时性；结构简单，便于底层的实时控制，在实时壁障和平滑的轨迹控制方面得到广泛的应用。缺点：当目标点距离较远的话，引力将变得特别大，相对较小的斥力下，物体路径可能会碰到障碍物；当目标点附近有障碍物时，斥力将非常大，

15、引力相对较小，物体很难到达目标点；在某个点，引力斥力刚好相等，方向相反，物体容易陷入局部最优解或震荡；容易陷入局部最优解。快速搜索随机树（RRT-Rapidly-Exploring Random Trees），是一种多维空间中有效率的规划方法，它本质上是一种随机生成的数据结构树。RRT算法是从起始点开始向外拓展一个树状结构，而树状结构的拓展方向是通过在规划空间内随机采点确定的。它以一个初始点作为根节点，通过随机采样增加叶子节点的方式，生成一个随机扩展树，当随机树中的叶子节点包含了目标点或进入了目标区域，便可以在随机树中找到一条由从初始点到目标点的路径。主体部分是节点扩展过程：步骤：首先，在空间

16、中随机采样一个节点qrand，然后在当前的树中找到距离该随机点最近的节点qnear，以固定的步长从节点qnear 向qrand的方向拓展一步，得到新节点qnew，接下来判断扩展边qnearqnew是否会和障碍物发生碰撞，如果不会，则新节点被添加到树中，如果会发生碰撞，则该节点会被舍弃。重复上述步骤，直到搜索到终点。则从起点到终点的一条路径就得到了。它的优势：快。因为它是随机采样，不需要对空间中所有的点都进行搜索。不需要对障碍物空间进行精确建模。因为在碰撞检测时它只需要知道给定节点是否为障碍物就可以，而不需要知道障碍物的范围、形状、大小和精确的模型表示。在多维空间中更具优势。维数越高，节点状态就

17、越多，对于传统路径规划方法，很容易导致维数灾难，而RRT为随机采样，只需要随机更多的维数，而不需要采样空间中的每一个状态，因此维数的升高对RRT的效率并不会有太大的影响。它的不足：最显著的一点就是，因为RRT采样的随机性，其得到的路径有很多冗余的节点，增加了路径的长度。当然，现在也有许多的优化方法去得到次优和最优的路径。最常用也是最简单的优化方法就是剪枝，裁剪掉不必要的节点。如下图所示，a,b两段就可以用c来替换，由三角不等式可知，替换后的路径更短且中间的冗余节点可以被裁剪。剪枝的方法非常实用，虽然已经有很多对RRT的改进方法，但在实时性上，这个简单的优化方法无疑是最快的，这里给出一个示意图。

18、随机路图法PRM是基于图搜索的方法，随机路图（Probabilistic Road Maps，PRM）就是在规划空间内随机选取N个节点，之后连接各节点，并去除与障碍物接触的连线，由此得到一个随机路图。优点：（1）克服了以往一些路径规划方法易于陷入局部极小的缺点；（2）可应用于多自由度的机器人的路径规划中；（3）计算量小；缺点：当采样点太少，或者分布不合理时，PRM算法是不完备的，但是随着采用点的增加，也可以达到完备。所以PRM是概率完备且不最优的。主要的应用背景有：（1）核反应工厂冷却管的维护；（2）汽车装配时点对点的焊接；（3）飞机机身的清理；（4）飞机引擎维护时拆解行为规划；用随机路径图（

19、PRM）法寻找给定地图中两点之间的路径，PRM进行路径规划的步骤：学习阶段：在给定图的自由空间里随机撒点（自定义个数），构建一个路径网络图。查询阶段：查询从一个起点到一个终点的路径。PRM学习阶段包含两部分内容：（1）构造无向路径网络图R=(N,E)，其中N代表随机点集，E代表所有可能的两点之间的路径集。（2）扩充难于连线区域的点，该部分内容旨在找出那些在“困难”区域的点，并且扩充这一区域点的个数。通过给每个点引入权重系数w(c)来决定那些区域需要增加点。学习阶段已经构造了无向路径网络图R=（N,E），进入查询阶段时只需根据设定的起点s和终点g，选择合适的路径，具体过程如下：（1）将s和g点与

20、路径网络中的两个点x,y分别连接 （2）寻找无向路径网络图中x与y连接的路径，这样就可以将起点和终点连接起来，构成全局路径。（3）得到全局路径后，可以使用平滑的方法寻找捷径，优化路径。主要的难点在于寻找s到x的路径，g到y点的路径。采用局部规划器和距离函数D结合的方法寻找。如果失败了，就采用random-bounce行走的方法寻找连接路径。基于Frenet坐标系的动作规划方法由于是由BMW的Moritz Werling提出的，该方法在高速情况下的高级车道保持和无人驾驶都具有很强的实用性，是目前普遍采用的一种动作规划算法。对于横向控制而言，假定由于车辆因为之前躲避障碍物或者变道或者其他制动原因而

21、偏离了期望的车道线，那么此时最优的动作序列（或者说轨迹）是在车辆制动能力的限制下，相对最安全，舒适，简单和高效的轨迹。同样的，纵向的最优轨迹也可以这么定义：如果车辆此时过快，或者太接近前方车辆，那么就必须做减速，那么具体什么是“舒适而又简单的”减速呢？我们可以使用 Jerk 这个物理量来描述，Jerk即加速度的变化率，也即加加速度，通常来说，过高的加加速度会会引起乘坐者的不适，所以，从乘坐舒适性而言，应当优化Jerk这个量，同时，引入轨迹的制动周期 T,即一个制动的操作时间：T=tendtstart 在Frenet坐标系中，我们使用道路的中心线作为参考线，使用参考线的切线向量 t和法线向量 n

22、建立一个坐标系，如下图的右图所示，这个坐标系即为Frenet坐标系，它以车辆自身为原点，坐标轴相互垂直，分为 s方向（即沿着参考线的方向，通常被称为纵向，Longitudinal）和 d 方向（即参考线当前的法向，被称为横向，Lateral），相比于笛卡尔坐标系（下图的作图），Frenet坐标系明显地简化了问题，因为在公路行驶中，我们总是能够简单的找到道路的参考线（即道路的中心线），那么基于参考线的位置的表示就可以简单的使用纵向距离（即沿着道路方向的距离）和横向距离（即偏离参考线的距离）来描述，同样的，两个方向的速度（和）的计算也相对简单。那么现在我们的动作规划问题中的配置空间就一共有三个维度

23、：(s,d,t),t是我们规划出来的每一个动作的时间点，轨迹和路径的本质区别就是轨迹考虑了时间这一维度。Werling的动作规划方法一个很关键的理念就是将动作规划这一高维度的优化问题分割成横向和纵向两个方向上的彼此独立的优化问题，具体来看下面的图：假设我们的上层（行为规划层）要求当前车辆在t8越过虚线完成一次变道，即车辆在横向上需要完成一个d以及纵向上完成一个s的移动，则可以将 s和d分别表示为关于t的函数：s(t)和d(t)，那么d,s关于时间t的最优轨迹应该选择哪一条呢？通过这种转换原来的动作规划问题被分割成了两个独立的优化问题，对于横向和纵向的轨迹优化，我们选取损失函数C，将使得C最小的

24、轨迹作为最终规划的动作序列。而Werling方法中损失函数的定义，则与我们前面提到的加加速度Jerk相关。由于我们将轨迹优化问题分割成了s和d 两个方向，所以Jerk最小化可以分别从横向和纵向进行，令p为我们考量的配置（即s或d），加加速度Jt关于配置p在时间段t1t0内累计的Jerk的表达式为：现在我们的任务是找出能够使得 Jt(p(t)最小的p(t)，Takahashi的文章Local path planning and motion control for AGV in positioning中已经证明，任何Jerk最优化问题中的解都可以使用一个5次多项式来表示：dptpJttt210)

25、()(5544332210)(ttttttp 要解这个方程组需要一些初始配置和目标配置，以横向路径规划为例，初始配置为 ，即t0时刻车辆的横向偏移，横向速度和横向加速度为 ，即可得方程组：305204032040530420302105054043032020100201262)(5432)()(ttttdtttttdttttttdddddddddddddddd 为了区分横向和纵向，我们使用（di）和（si）来分别表示d和s方向的多项式系数，同理，根据横向的目标配置D1 ，可得方程组：我们通过t0=0来简化这个六元方程组的求解，可直接求得（d0），（d1）和（d2）为：315214132141

26、531421312115154143132121101201262)(5432)()(ttttdtttttdttttttdddddddddddddddd 2)()()(020100tdtdtdddd 令T=t1t0，剩余的三个系数（d3）,（d4）,（d5），可通过解如下矩阵方程得到：该方程的解可以通过Python的Numpy中的 np.linalg.solve 简单求得。至此，我们在给定任意的初始配置D0=，目标配置D1=，制动时间T的情况下，可以求的对应的d方向关于时间t的五次多项式的系数，同理，可以使用相同的方法来求解纵向（即s方向）的五次多项式系数。)()()()()()(21)()(

27、)(20126543010012000154332432543tdtdTtdtdtdTtdTtdtdtdTTTTTTTTTddd 那么问题来了，我们如何去确定最优的轨迹呢？Werling方法的思路是通过一组目标配置来求得轨迹的备选集合，然后在备选集合中基于Jerk最小化的原则选择最优轨迹，我们仍然以d方向的优化轨迹为例讲解：我们可以取如下目标配置集合来计算出一组备选的多项式集合：对于优化问题而言，我们实际上希望车辆最终沿着参考线（道路中心线）平行的方向行驶，所以我们令di=di=0，那么目标配置只涉及di和Tj两个变量的组合，而这两个变量在无人驾驶的应用场景中实际上是受限的，我们可以通过定义(

28、dmin,dmax)和(Tmin,Tmax)来约束目标配置的取值范围，通过d和T来限制采样密度，从而在每一个制动周期获得一个有限的备选轨迹集合。jiijTdTddd,0,0,111 要在备选集合中选择最优轨迹（即上图中的绿色轨迹），我们需要设计损失函数，对于不同的场景，损失函数也不相同，以横向轨迹为例，在较高速度的情况下，损失函数为：该损失函数包含三个惩罚项：（1）惩罚Jerk大的备选轨迹；（2）制动应当迅速，时间短；（3）目标状态不应偏离道路中心线太远。其中，和这三个惩罚项的系数，它们的比值大小决定了我们的损失函数更加注重哪一个方面的优化，由此我们可以算出所有备选轨迹的损失，取损失最小的备选

29、轨迹作为我们最终的横向轨迹。21)(dkTktdJkCdttjd 值得注意的是，以上的损失函数仅适用于相对高速度的场景，在极端低速的情况下，车辆的制动能力是不完整的，我们不再将d表示为关于时间t的五次多项式，损失函数也会略有不同，但是这种基于有限采样轨迹，通过优化损失函数搜索最优轨迹的方法仍然是一样的，在此不再赘述。讨论完横向的轨迹优化问题，我们再来看看纵向的轨迹优化，在不同的场景下纵向轨迹的优化的损失函数也各不相同，Werling方法中将纵向轨迹的优化场景大致分成如下三类：*跟车 *汇流和停车 *车速保持 我们详细了解车速保持场景下的纵向轨迹优化，在高速公路等应用场景中，目标配置中并不需要考

30、虑目标位置（即 s1），所以在该场景下，目标配置仍然是(s0,s0,s0)，目标配置变成了(s1,s1)，损失函数为：其中Sc是我们想要保持的纵向速度，第三个惩罚项的引入实际上是为了让目标配置中的纵向速度尽可能接近设定速度，该情景下的目标配置集为：即优化过程中的可变参数为 Si 和 Tj，同样，也可以通过设置 T 和 Si 来设置轨迹采样的密度，从而获得一个有限的纵向轨迹集合。21)()(csttjssskTktsJkC jicijTssTss,0,11 其中，绿线即为纵向最优轨迹。以上我们分别讨论了横向和纵向的最优轨迹搜索方法，在应用中，我们将两个方向的损失函数合并为一个，即：这样，我们就可

31、以通过最小化C（total）得到优化轨迹集合（我们不能得到“最优”的轨迹多项式参数，还可以得到“次优”，“次次优”轨迹等等）。slondlattotalCkCkC 显然，我们上面的轨迹优化损失函数中并没有包含关于障碍物躲避的相关惩罚，并且我们的损失函数中也没有包含最大速度，最大加速度和最大曲率等制动限制，也就是说我们的优化轨迹集合并没有考虑障碍物规避和制动限制因素，不将障碍物避免加入到损失函数中的一个重要的原因在于碰撞惩罚项的引入将代入大量需要人工调整的参数（即权重），是的损失函数的设计变得复杂，Werling方法将这些因素的考量独立出来，在完成优化轨迹以后进行。具体来说，我们会在完成所有备选轨迹的损失计算以后进行一次轨迹检查，过滤掉不符合制动限制的，可能碰撞障碍物的轨迹，检查内容包括：（1）s方向上的速度是否超过设定的最大限速；（2）s方向的加速度是否超过设定的最大加速度；（3）轨迹的曲率是否超过最大曲率；（4）轨迹是否会引起碰撞（事故）；通常来说，障碍物规避又和目标行为预测等有关联，本身即使一个复杂的课题，高级自动驾驶系统通常具备对目标行为的预测能力，从而确定轨迹是否会发生事故。