一种具有时间一致性的无人驾驶车辆局部路径规划方法与流程

本发明属于智能车辆局部路径规划相关研究领域，涉及一种具有时间一致性的无人驾驶车辆局部路径规划方法。

技术背景

据统计，2016年我国国内汽车销量达到了2802.8万辆，相比2015年增加了13.7％，连续8年蝉联全球销量第一。截止到2015年年末，全国的总里程数也达到了457.73万公里，同比增加了14.5万公里。汽车保有量和公路总里程的增加导致交通事故频发，交通拥堵情况严重。专家对城市交通数据的分析表明，人为因素是导致城市交通事故的主要原因。鉴于此，世界各国对智能交通系统(intelligenttransportationsystem)给予了充分地重视，投入大量的人力物力到其中。在这之中，智能车辆(intelligentvehicles)作为智能交通系统的关键组成部分，发挥着不容忽视的作用。所谓智能车辆，就是集合多种技术于一身(如环境感知、组合导航、人工智能、机器视觉、自动化技术)，具有感知环境和规划决策能力的智能化系统。智能车辆通过环境感知检测车辆周围环境信息，并根据决策规划系统制定相对应的控制需求，由车辆执行机构执行加减速、转向等操作，以达到车辆安全行驶的目的，避免因为驾驶员的驾驶技术、安全意识等因素造成交通事故的发生。

无人驾驶汽车控制系统按负责功能的不同可以分为规划层和执行层，规划层主要完成目标路径的生成，包括全局路径的规划和局部路径的规划。其中，局部路径规划是指在全局路径已知情况下，无人驾驶车辆在实时行驶时，由雷达和摄像头获取周围包括障碍物的尺寸、形状和位置等信息的环境信息，然后通过局部规划算法得到一条安全无碰撞的最优路径的过程。

当前的局部路径规划算法一般是基于路径生成和路径选择的方法。该方法具有实用性强，运算复杂度低，方法灵活的优点，但在曲线生成上没有结合车辆的运动学参数对生成的曲线进行约束，导致最终选择出的局部路径不能从根本上保证路径跟踪的准确度，而且由于使用的是周期优化求解的方式，当前规划周期和下一个规划周期的规划结果可能存在较大的差异，丧失时间一致性，导致可能得到阶跃的路径跟踪输出，造成车身的不稳，影响行车安全。

技术实现要素：

时间一致性是来自经济学的一个概念，时间不一致是指在t1时刻规划得到tn时刻的最优计划p，在t2时刻已不是tn时刻的最优计划，北京理工大学姜岩博士将其概念引入到智能车局部路径规划领域。

针对当前较多使用的基于路径生成和路径选择的无人驾驶车辆局部路径规划方法存在的规划出的曲线缺乏车辆运动学约束、无法保证车辆路径跟踪的准确度和安全性、以及依靠优化求解得到局部规划路径方法周期一致性较差容易导致规划结果出现阶跃影响车辆安全行驶的问题，提出了一种具有时间一致性的无人驾驶车辆局部路径规划方法。采用该方法可以提高无人驾驶汽车的环境适应性能、目标路径跟踪的准确性能以及行驶的安全性能。

为实现上述目的，本发明采用如下的技术方案：

一种具有时间一致性的无人驾驶车辆局部路径规划方法，其步骤包括如下：

步骤1，判断规划周期属性，确定多段的规划距离；

步骤2，确定无人驾驶车辆位置

对于任意一段全局路径eij，p是车辆当前位置，向量代表车辆的航向，θ1表示车辆航向与eij的夹角，θ2表示pj与eij的夹角，设定θ1＜90°并且θ2＜90°，计算步骤如下：

1)在g(n,e)中，e¹＝argmind(p,eij),(eij∈e,|θ1＜90°,|θ2＜90°|)，

d(p,eij)＝min{|pq|,q∈e}

2)在集合e¹中：e²＝argmaxf(p,eij),(eij∈e¹)

其中，

3)在搜索f(p,eij)的最大边，判断|o1o2|＞＝2ρmin满足与否。

其中，若不符合，则查看e²下一个f(p,eij)，直到条件满足，最终，得到的eij便是搜索开始时的边；

上式中，g(n,e)为车辆坐标点和全局路径点可以确定的二维平面，其中n取车辆后轴中心点，e为全局目标路径点序列。

步骤3，局部路径规划

经过步骤1确定了强、弱规划部分的距离，经过步骤2确定了当前规划周期车辆相对于全局目标路径的参考位置，局部路径规划环节要完成弱规划段路径的确定，以及强规划路径的起始点的确定，然后利用曲线生成功能，得到一簇的强规划曲线；通过连接弱规划曲线和强规划曲线，最终得到一簇局部路径规划曲线。

步骤4，路径选择

经过步骤3，行驶车辆前方规划出了一簇可行驶的规划曲线。步骤4，根据环境感知系统给出的障碍物位置尺寸信息对规划曲线进行选择，得到当前规划周期的局部路径规划结果；

步骤5，局部规划路径传递

通过步骤4得到的局部规划路径曲线是一系列离散的点，对规划层得到的离散点进行处理，以实现规划层和控制层的顺利对接，使用曲线拟合的方式，利用5次多项式作为拟合曲线，形式如下：

y＝a0+a1x+a2x²+a3x³+a4x⁴+a5x⁵

式中，a0,a1,a2,a3,a4,a5分别为拟合曲线纵轴坐标多项式的常数项、一次项、二次项、三次项、四次向、五次项待求系数；x为横坐标；y为拟合出的纵坐标值。

作为优选：将局部路径规划分为弱规划部分和强规划部分，弱规划部分来自上个规划周期的局部路径规划结果，而强规划部分负责规划路径的更新，通过划分强、弱规划段达到即保持前后周期规划一致性又保证规划路径的实时性，该点对应到步骤1具体为：在每个规划周期的开始，首先确定当前规划周期是否为初次规划，如果为初次规划，则将弱规划距离d＝0，强规划距离d＝v*t，规划距离r＝d+d；如果不是初次规划，则弱规划距离d＝v²/2a+bv+a，强规划距离d＝v*t，规划距离r＝d+d。其中，v为通过车身雷达测得的障碍物接近速度；t为人为驾驶汽车时进行避障决策的安全避障时间；a为车辆制动的最大加速度；b为刹车控制子系统的延误时间；a为设定的紧急制动的安全停靠距离。

作为优选，步骤3中曲线生成部分引入了车辆运动学方程，使得到的规划曲线满足车辆的行驶特性，通过对曲线模型进行升维处理，对生成曲线进行了包括最大曲率、整体平滑度、规划曲线长度的优化，步骤3具体为：

步骤3.1，初次规划起始点确定

步骤3.1.1，初次规划起始点确定

设定弱规划距离d＝0，强规划距离d＝vt，以车辆几何中心o点作为一簇强规划曲线的起点，以车辆当前时刻的位姿作为该簇规划曲线起点的位姿；其中，v为通过车身雷达测得的障碍物接近速度；t为人为驾驶汽车时进行避障决策的安全避障时间；

步骤3.1.2，初次规划终点确定

以o点为圆心，以d＝vt为半径绘制圆，该圆曲线会与全局路径有至少一个交点，以靠近全局路径终点的交点作为强规划预瞄点o1，全局路径在o1的切线方向为o1点车辆的航向，沿航向的法线方向按照一定的横向距离在确定若干个目标位姿点o2,o3,o4,o5，位姿点航向都保持一致，使预瞄圆周与全局路径交点中靠近终点一侧交点全局路径切线的方向，该交点两侧分布位姿点之间的距离以及位姿点的数量由道路环境和障碍物尺寸确定，由此便可以确定初次规划时强路径规划的起点位姿信息和若干个终点位姿信息；

步骤3.2，非初次规划起始点确定

步骤3.2.1，强规划曲线起始点确定

以车辆当前周期位姿点o为圆心，以d＝v²/2a+bv+a为半径，截取上一个规划周期的局部路径规划结果作为当前规划周期的弱规划结果，oq1为截取的上个规划周期的规划结果，以q1点车辆目标航向的法线做一条直线，设分别切上个规划周期规划曲线簇于q2,q3,q4,q5，上个规划周期规划曲线簇上的切点q1,q2,q3,q4,q5便作为当前规划周期的强规划曲线起点；

步骤3.2.2，强规划曲线终点确定

以车辆当前周期位姿点o为圆心，以r＝d+d为半径画圆，设圆周与全局路径远离起点的交点为q1,以q1所在全局路径处的切线方向的法线，沿法线方向按照不同的横向位置距离设置若干个强规划终点；

步骤3.3，求解规划曲线

步骤3.3.1，规划方案

在第一个规划周期，弱规划距离为零，只进行起点位姿为车辆当前位姿，终点位姿为o1,o2,o3,o4,o5点对应位姿，因此需要规划出对应的oo1,oo2,oo3,oo4,oo5五条曲线；在之后的规划周期，弱规划结果由截取前一个周期的部分规划结果得到，不需要重新进行曲线生成，而弱规划结果外的强规划区域则需要重新进行曲线生成，依靠强规划起点q1,q2,q3,q4,q5处的位姿信息和强规划终点o1,o2,o3,o4,o5的位姿信息通过曲线生成算法，即可得到q1o1,q2o2,q3o3,q4o4,q5o5强规划曲线簇。

步骤3.3.2，规划曲线生成算法

规划曲线的目的就是要得到一条连接规划起点和终点的平滑曲线，该曲线满足起点和终点位姿条件；其中，车辆位姿信息包括车辆的在全局坐标系下的x和y轴坐标，车辆的航向角θ，车辆的前轮偏角δ，假设规划起点为a，规划终点为b，则a点位姿表示为(xa,ya,θa,δa),b点位姿表示为(xb,yb,θb,δb)；

为了使规划出的曲线能够被车辆准确地跟随，行驶曲线还要满足车辆的运动微分约束：

上式中，x为车辆的横轴坐标；y为车辆的纵轴坐标；θ为车辆航向角；δ为车辆前轮偏角；v为车辆后轴中心点速度；l为车辆轴距。

满足车辆运动微分方程的曲线可以用以无量纲参数u的多项式方程来描述：x＝x(u),y＝y(u),u∈[0,1]。

由边界条件及车辆运动微分方程可以得到确定的3次多项式方程的解，从而得到规划曲线的参数方程形式如下：

x(u)＝x0+x1u+x2u²+x3u³

y(u)＝y0+y1u+y2u²+y3u³

对多项式方程进行参数提升，引入额外的调节参数ε1,ε2,ε3,ε4，得到参数方程如下：

x(u)＝x0+x1u+x2u²+x3u³+ε1u⁴+ε2u⁵

y(u)＝y0+y1u+y2u²+y3u³+ε3u⁴+ε4u⁵

通过增加参数，可以将原方程转化为系数对调节参数的显示方程，通过对其进行优化求解可以得到满足约束条件的参数方程参数，综合考虑规划曲线的最大曲率、平滑度和长度的优化目的，设定优化求解函数如下：

其中，k为优化求解函数值，k1,k2,k3分别为规划曲线最大曲率、平滑度和长度的权重系数，k为规划曲线各离散点的曲率，s为规划曲线长度。

作为优选，步骤4具体为：对可能发生障碍物碰撞或剐蹭的生成曲线进行判定，采用滑动法对危险路径进行判断，

假定有ap1，ap2，ap3三条规划曲线，进行滑动判断前，首先需要对规划曲线进行优化排序，然后按照排序进行滑动判断；

对规划曲线进行最优排序的指标函数如下所示：

其中，k为曲线最优指标值，k1,k2,k3分别为得到的规划曲线最大曲率、平滑度和长度的权重系数，k为规划曲线各离散点的曲率，s为规划曲线长度，

通过规划曲线指标函数，可以对规划出来的曲线簇按最优到次优的排序，在之后进行滑动判定时，从最优的曲线依次向次优曲线进行判断，这样判定得到的第一条安全无碰撞曲线便可以作为最后的局部规划路径进行输出，

如果通过规划曲线最优排序得到的结果为ap3优于ap2优于ap1，那么便从ap3进行滑动判定，

将车辆的几何中心首先放到ap3的第一个离散点上，然后判断车辆几何边缘与障碍物边沿离散点的情况，若障碍物边界离散点不在车辆几何边缘线内，则将车辆结合中心点放到下一个离散点位置，继续判断；若存在障碍物边界离散点在车辆几何边缘线内则判定为不安全曲线，删除该条曲线后继续下一条规划曲线判定，直到得到一条安全无碰撞曲线终止；若遍历整个规划曲线簇，无法找到一条安全无碰撞的可行驶曲线，则发送停车指令。

与现有方法相比，本发明具有以下优点：

1)提出了局部路径规划分段规划的方法，较好地解决了局部路径规划能力和规划结果时间一致性的问题；

2)使用车辆微分运动微分条件对局部路径生成曲线进行约束，并通过参数提升的方法提高规划出曲线的性能；

3)使用多项式曲线拟合的方法来进行局部路径规划层和路径跟踪层之间的离散点数据传递，保证了控制系统的模块化和规范化。

附图说明

图1为本发明所涉及的方法流程图

图2车辆在数字地图上的位置点与跟踪偏差

图3车辆位置示意图

图4判断条件示意图

图5初次规划周期局部路径规划示意图

图6非初次规划周期局部路径规划示意图

图7路径生成算法仿真图

图8路径选择方法示意图

图9局部路径规划整体功能仿真结果图

图10本发明在carsim车辆仿真环境下的3d效果图

图11时间一致性仿真效果图

具体实施方式

本发明提供一种具有时间一致性的无人驾驶车辆局部路径规划方法，包括如下步骤：

步骤1，判断规划周期属性，确定多段的规划距离

在每个规划周期的开始，首先需要确定当前规划周期是否为初次规划，如果为初次规划，则将弱规划距离d＝0，强规划距离d＝v*t，规划距离r＝d+d；如果不是初次规划，则弱规划距离d＝v²/2a+bv+a，强规划距离d＝v*t，规划距离r＝d+d。其中，v为通过车身雷达测得的障碍物接近速度；t为人为驾驶汽车时进行避障决策的安全避障时间；a为车辆制动的最大加速度；b为刹车控制子系统的延误时间；a为设定的紧急制动的安全停靠距离。一般取t＝3～5s。

步骤2，确定无人驾驶车辆位置

在路径规划的初始阶段需要首先确定无人驾驶车辆相对于全局路径点的位置。由于车辆不断处于运动状态，车辆位置信息是在不断发生变化，因此在每个规划周期进行局部路径规划时也要重新确定车辆相对于全局路径的位置。

通过车辆位姿信息获得当前车体在全局路径上的位置点，如图2所示。

其中的i和i+1代表全局离散路径点序号，(cx,cy)为车辆当前的全局坐标系坐标，(lx,ly)为车辆在全局路径上的位置点，ε为跟踪距离偏差，δ为航向角偏差。

如图3所示，对于任意一段全局路径eij，p是车辆当前位置，向量代表车辆的航向，θ1表示车辆航向与eij的夹角，θ2表示pj与eij的夹角，结合车辆实际情况，这里限定θ1＜90°并且θ2＜90°。计算步骤如下：

4)在g(n,e)中，e¹＝argmind(p,eij),(eij∈e,|θ1＜90°,|θ2＜90°|)，d(p,eij)＝min{|pq|,q∈e}

5)在集合e¹中：e²＝argmaxf(p,eij),(eij∈e¹)

其中，

6)在搜索f(p,eij)的最大边，判断|o1o2|＞＝2ρmin满足与否。

其中，若不符合，则查看e²下一个f(p,eij)，直到条件满足，最终，得到的eij便是搜索开始时的边，判断的条件如图4所示。

步骤3，局部路径规划

步骤3.1，初次规划起始点确定

步骤3.1.1，初次规划起始点确定

如图1中所示，在初次规划局部路径时，由于不存在与前一个规划周期结果的连接性问题，因此只需得到强规划结果即可，因此设定弱规划距离d＝0，强规划距离d＝vt。如图5所示，此时以车辆几中心o点作为一簇强规划曲线的起点，以车辆当前时刻的位姿作为该簇规划曲线起点的位姿。

步骤3.1.2，初次规划终点确定

以o点为圆心，以d＝vt为半径绘制圆，该圆曲线会与全局路径有至少一个交点，以靠近全局路径终点的交点作为强规划预瞄点o1，全局路径在o1的切线方向为o1点车辆的航向，沿航向的法线方向按照一定的横向距离在确定若干个目标位姿点，如图5中o2,o3,o4,o5位姿点所示，位姿点航向都保持一致，为预瞄圆周与全局路径交点中靠近终点一侧交点全局路径切线的方向。该交点两侧分布位姿点之间的距离以及位姿点的数量由道路环境和障碍物尺寸确定，当障碍物尺寸较大时，横向位姿点间距较大，全局路径两侧可行驶区域较宽时，位姿点可以设置的较多一些。

由此便可以确定初次规划时强路径规划的起点位姿信息和若干个终点位姿信息。

步骤3.2，非初次规划起始点确定

步骤3.2.1，强规划曲线起始点确定

除去首个规划周期，每个新的规划周期到来时，上个规划周期的局部规划曲线车辆仅仅只行驶了一小部分，大部分的规划结果车辆并没有跑完。因此，为了提高规划结果的时间一致性，提高规划结果的利用率，降低算法消耗，以车辆当前周期位姿点o为圆心，以d＝v²/2a+bv+a为半径，截取上一个规划周期的局部路径规划结果作为当前规划周期的弱规划结果。具体操作如图6所示，oq1为截取的上个规划周期的规划结果，以q1点车辆目标航向的法线做一条直线，设分别切上个规划周期规划曲线簇于q2,q3,q4,q5，上个规划周期规划曲线簇上的切点q1,q2,q3,q4,q5便作为当前规划周期的强规划曲线起点。

步骤3.2.2，强规划曲线终点确定

以车辆当前周期位姿点o为圆心，以r＝d+d为半径画圆，设圆周与全局路径远离起点的交点为q1,以q1所在全局路径处的切线方向的法线，沿法线方向按照不同的横向位置距离设置若干个强规划终点，如图6中o1,o2,o3,o4,o5所示。

步骤3.3，求解规划曲线

步骤3.3.1，规划方案

如图5所示，在第一个规划周期，弱规划距离为零，只进行起点位姿为车辆当前位姿，终点位姿为o1,o2,o3,o4,o5点对应位姿。因此需要规划出对应的oo1,oo2,oo3,oo4,oo5五条曲线；如图6所示，在之后的规划周期，弱规划结果由截取前一个周期的部分规划结果得到，不需要重新进行曲线生成，而弱规划结果外的强规划区域则需要重新进行曲线生成，依靠强规划起点q1,q2,q3,q4,q5处的位姿信息和强规划终点o1,o2,o3,o4,o5的位姿信息通过曲线生成算法，即可得到q1o1,q2o2,q3o3,q4o4,q5o5强规划曲线簇。

步骤3.3.2，规划曲线生成算法

规划曲线的目的就是要得到一条连接规划起点和终点的平滑曲线，该曲线满足起点和终点位姿条件。其中，车辆位姿信息包括车辆的在全局坐标系下的x和y轴坐标，车辆的航向角θ，车辆的前轮偏角δ。假设规划起点为a，规划终点为b，则a点位姿表示为(xa,ya,θa,δa),b点位姿表示为(xb,yb,θb,δb)。

为了使规划出的曲线能够被车辆准确地跟随，行驶曲线还要满足车辆的运动微分约束：

满足车辆运动微分方程的曲线可以用以无量纲参数u的多项式方程来描述：x＝x(u),y＝y(u),u∈[0,1]。

由边界条件及车辆运动微分方程可以得到确定的3次多项式方程的解。从而得到规划曲线的参数方程形式如下：

x(u)＝x0+x1u+x2u²+x3u³

y(u)＝y0+y1u+y2u²+y3u³

然而，这样得到的参数唯一确定的3次多项式方程并不能很好地解决规划曲线优化的问题，无法对规划曲线的最大曲率、平滑度和长度进行约束。因此，这里对多项式方程进行参数提升，引入额外的调节参数ε1,ε2,ε3,ε4，得到参数方程如下：

x(u)＝x0+x1u+x2u²+x3u³+ε1u⁴+ε2u⁵

y(u)＝y0+y1u+y2u²+y3u³+ε3u⁴+ε4u⁵

通过增加参数，可以将原方程转化为系数对调节参数的显示方程，通过对其进行优化求解可以得到满足约束条件的参数方程参数，综合考虑规划曲线的最大曲率、平滑度和长度的优化目的，这里设定优化求解函数如下：

其中，k为优化求解函数值，k1,k2,k3分别为规划曲线最大曲率、平滑度和长度的权重系数，k为规划曲线各离散点的曲率，s为规划曲线长度。

仿真环境下，曲线生成效果如图7所示。

步骤4，路径选择

如图7所示，经过步骤3，行驶车辆前方规划出了一簇可行驶的规划曲线，为了保证最终得到的局部规划路径车辆在行驶时能够达到安全无碰撞的目的，还需要根据环境感知系统给出的障碍物位置尺寸信息对规划曲线进行选择，从而得到当前规划周期的局部路径规划结果。步骤4.1，危险路径判定

基于栅格的局部路径规划算法往往依靠腐蚀膨胀的方法对可能发生障碍物碰撞或剐蹭的生成曲线进行判定，本发明采用滑动法对危险路径进行判断。

如图8所示，我们假定有ap1，ap2，ap3三条规划曲线，进行滑动判断前，首先需要对规划曲线进行优化排序，然后按照排序进行滑动判断。

步骤4.1.1，规划曲线最优排序

不同于机器人等缓慢移动的运动对象，车辆具有高速运动的属性，因此必须考虑尽量降低规划算法的计算复杂度。因此在对规划曲线进行判断前必须进行曲线最优排序，这样在判断得到最优的规划曲线之后，无需再对剩余的规划曲线进行判定，提高局部路径规划效率。

对规划曲线进行最优排序的指标函数如下所示：

其中，k为曲线最优指标值，k1,k2,k3分别为得到的规划曲线最大曲率、平滑度和长度的权重系数，k为规划曲线各离散点的曲率，s为规划曲线长度。

曲线对应的最优指标值越小，表示曲线的性能越高。

通过规划曲线指标函数，可以对规划出来的曲线簇按最优到次优的排序，在之后进行滑动判定时，从最优的曲线依次向次优曲线进行判断，这样判定得到的第一条安全无碰撞曲线便可以作为最后的局部规划路径进行输出，降低判断操作的次数。

步骤4.1.2，滑动判定

如图8所示，如果通过规划曲线最优排序得到的结果为ap3优于ap2优于ap1，那么便从ap3进行滑动判定。

将车辆的几何中心首先放到ap3的第一个离散点上，然后判断车辆几何边缘与障碍物边沿离散点的情况，若障碍物边界离散点不在车辆几何边缘线内，则将车辆结合中心点放到下一个离散点位置，继续判断；若存在障碍物边界离散点在车辆几何边缘线内则判定为不安全曲线，删除该条曲线后继续下一条规划曲线判定，直到得到一条安全无碰撞曲线终止；若遍历整个规划曲线簇，无法找到一条安全无碰撞的可行驶曲线，则发送停车指令。

为了能够降低规划算法计算复杂度，这里将车辆几何外形设定为以车辆几何中心为圆心，以车辆对角线距离一半作为半径的圆周。

步骤5，局部规划路径传递

通过步骤4，得到的局部规划路径曲线是一系列离散的点，当规划距离较长时，规划曲线离散点数量是较为可观的。如果将这些离散点直接传递至控制层，会导致占用大量的传递接口，不利于控制系统的规范化设计。除此之外，规划周期和控制周期并不一致，控制层使用规划层的离散点进行控制并不能得到最佳的效果。因此有必要对规划层得到的离散点进行处理，以实现规划层和控制层的顺利对接。这里使用曲线拟合的方式，利用5次多项式作为拟合曲线，形式如下：

y＝a0+a1x+a2x²+a3x³+a4x⁴+a5x⁵

式中，a＝[a0,a1,a2,a3,a4,a5]为待求参数。

通过拟合的方法，将需要传递的离散规划路径点转变为传递多项式参数的问题，减少了规划模块和控制模块之间传递接口的数量。

图9为该方法在matlab/simulink与汽车动力学仿真软件carsim搭建的仿真系统中运行的情况。其中，横轴为x运动方向，纵轴为y运动方向，o点为车辆起始点，p1p2红色曲线为全局规划路径，蓝色曲线为各个规划周期的局部路径规划结果，绿色为车辆实际行驶轨迹，abcd为设置的障碍物。由图9可知，在开始时刻，车辆偏离全局路径，因此局部路径规划单元引导车辆跟踪全局路径；当检测到障碍物时，局部路径规划单元规划处安全无碰撞的局部路径，绕开障碍物；当车辆经过障碍物后，局部路径规划单元引导车辆继续跟踪全局路径。

图10为本发明所提方法在carsim车辆仿真环境下实现的3d效果图。无人驾驶车辆原本的目标路径是位于右侧车道的一条直线，但行驶过程总遇到了一辆停在道路中间的故障车，无人驾驶车辆的局部路径规划单元利用本发明所提方法，规划出了安全无碰撞的可行驶路径，由图10可以看出，车辆能够安全无碰撞地驶过障碍物并继续跟踪全局路径。

图11为本发明所使用方法中时间一致性仿真效果图，其中横轴为控制周期，每个控制周期t＝0.05s，纵轴为解算出来的方向盘转角值。由图11可知，起始时刻车辆偏离目标路径，两种路径规划方法都能够调整车辆跟踪目标路径，而无时间一致性规划方法的车辆在140个规划周期左右开始检测到障碍物，虽然做出了可以躲避障碍物的规划，但由于每个周期规划出来的局部路径曲线和其前后周期规划结果没有时间一致性，导致整个避障过程中，控制输出的方向盘转角值出现剧烈变化引起造成车身抖动。而使用了本文所提时间一致性方法的避障规划过程由于各个相邻规划周期的规划结果具有时间一致性，因此最终路径跟踪时输出的方向盘转角值具有平滑性，保证了无人驾驶车辆行驶的稳定性和安全性。

综合图9、图10和图11可知，本发明所提出的无人驾驶汽车局部路径规划方法具有较好的路径规划效果效果和较高的算法可行性。

本发明所提的具有时间一致性的无人驾驶汽车局部路径规划方法，首先为规划路径曲线增加车辆运动微分约束，使得车辆能够准确地对生成的轨迹进行跟踪；其次，为了避免车辆在较高速度行驶时规划出的路径曲线曲率过大，对路径规划轨迹曲线模型进行了模型参数提升，为规划路径增加了更大的自由度，便于将对曲率的约束问题转化为数学的求解最优化问题；最后，对于利用周期优化求解获得局部路径方法前后周期优化结果有较大差异，导致车辆进行实时路径跟踪时出现抖动的问题，本发明将局部路径规划段分为弱规划和强规划两部分，弱规划段由上个规划周期的强规划结果得到，通过规划的分段，即保证规划结果的时间一致性，又可使局部路径规划具有较高的避障能力。