[发明专利]一种具有时间一致性的无人驾驶车辆局部路径规划方法

权利要求书

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

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

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

对于任意一段全局路径E_ij，P是车辆当前位置，向量代表车辆的航向，θ₁表示车辆航向与E_ij的夹角，θ₂表示Pj与E_ij的夹角，设定θ₁90°并且θ₂90°，计算步骤如下：

1)在G(N,E)中，E¹＝argmind(P,E_ij),(E_ij∈E,|θ₁90°,|θ₂90°|)，

d(P,E_ij)＝min{|PQ|,Q∈E_ij}

2)在集合E¹中：E²＝argmaxf(P,E_ij),(E_ij∈E¹)

其中，

3)在搜索f(P,E_ij)的最大边，判断|O₁O₂|＝2ρ_min满足与否；

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

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

步骤3，局部路径规划

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

步骤4，路径选择

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

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

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

Y＝a₀+a₁x+a₂x²+a₃x³+a₄x⁴+a₅x⁵

式中，a₀,a₁,a₂,a₃,a₄,a₅分别为拟合曲线纵轴坐标多项式的常数项、一次项、二次项、三次项、四次向、五次项待求系数；x为横坐标；Y为拟合出的纵坐标值；

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

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

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

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

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

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

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

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

以车辆当前周期位姿点O为圆心，以d＝V²/2a+BV+A为半径，截取上一个规划周期的局部路径规划结果作为当前规划周期的弱规划结果，Oq₁为截取的上个规划周期的规划结果，以q₁点车辆目标航向的法线做一条直线，设分别切上个规划周期规划曲线簇于q₂,q₃,q₄,q₅，上个规划周期规划曲线簇上的切点q₁,q₂,q₃,q₄,q₅便作为当前规划周期的强规划曲线起点；a为车辆制动的最大加速度；B为刹车控制子系统的延误时间；A为设定的紧急制动的安全停靠距离；

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

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

步骤3.3，求解规划曲线

步骤3.3.1，规划方案

在第一个规划周期，弱规划距离为零，只进行起点位姿为车辆当前位姿，终点位姿为O₁,O₂,O₃,O₄,O₅点对应位姿，因此需要规划出对应的OO₁,OO₂,OO₃,OO₄,OO₅五条曲线；在之后的规划周期，弱规划结果由截取前一个周期的部分规划结果得到，不需要重新进行曲线生成，而弱规划结果外的强规划区域则需要重新进行曲线生成，依靠强规划起点q₁,q₂,q₃,q₄,q₅处的位姿信息和强规划终点O₁,O₂,O₃,O₄,O₅的位姿信息通过曲线生成算法，即可得到q₁O₁,q₂O₂,q₃O₃,q₄O₄,q₅O₅强规划曲线簇；

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

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

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

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

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

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

x(u)＝x₀+x₁u+x₂u²+x₃u³

y(u)＝y₀+y₁u+y₂u²+y₃u³

对多项式方程进行参数提升，引入额外的调节参数ε₁,ε₂,ε₃,ε₄，得到参数方程如下：

x(u)＝x₀+x₁u+x₂u²+x₃u³+ε₁u⁴+ε₂u⁵

y(u)＝y₀+y₁u+y₂u²+y₃u³+ε₃u⁴+ε₄u⁵

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

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