[发明专利]一种结构化环境下无人驾驶车辆的路径规划方法

权利要求书

1.一种结构化环境下无人驾驶车辆的路径规划方法，其特征在于包括以下步骤：

步骤S1、定位；

首先加载全局地图数据，然后读取本车状态信息，最后找出离本车距离最短的全局路径点作为全局定位点；

步骤S2、当前车道路况分析；

第一、由传感器获取交通场景信息即障碍物信息，交通标志信息，车道线信息；第二、判断所获取的交通场景信息是否需要完整重规划，如果是，转到步骤S3完整重规划，否则，转到步骤S4，更新局部规划结果；

步骤S3、完整重规划；

步骤S301、换道分析获取无人车与前后车辆的纵向距离，速度和加速度，计算得出无人车在当前交通场景下是否可以换道，具体地：

首先，以车自身为中心，将车前后划分为9个子区域，编号从左到右，从上至下分别为A1、A2…A9，其中车自身可能处于的区域有A4、A5和A6，符号表示为A_c∈{A4,A5,A6},障碍物可能处于除A_c区域外的任意区域，符号表示为A_o∈{A}and A_o≠A_c，A为全集，包括所有子区域；

向左换道分析如下，实验车处于区域A5；

①.从感知模块获取A2区域最近车辆信息，速度记为V_o2，加速度记为a_o2，如果V_o2大于实验车速度V_c，或者仿真两秒后的速度V_o2+2×a_o2大于V_c，则认为当前车道可正常行驶，选择跟随，保持当前局部路径；

②.若步骤①中的判断结果为否，则判断A4区域是否被占据，如果是，选择跟随，保持当前局部路径；

③.若步骤②中判断结果为否，则判断A1区域是否被占据，如果是，且满足条件V_o1小于V_c且V_o1+2×a_o1小于V_c，则选择跟随，保持当前局部路径；

④.若区域A1没有被占据，或者满足条件V_o1大于V_c或V_o1+2×a_o1大于V_c，则判断A7区域是否被占据；

⑤.若步骤④中A7区域没有被占据，则选择代价最小的候选路径；

⑥.若步骤④中A7区域被占据，则判断V_o7×2+2×a_o7是否大于dis+V_c×2，dis为A7区域车头到实验车尾部的纵向距离，2为仿真时间，若是，则选择跟随，保持当前局部路径，否则，选择代价最小的候选路径；

步骤S302、模板生成：采取五次样条插值算法，结合车辆运动学模型，生成31条备选路径，这31条备选路径的集合，称为模板，具体地：

满足以下公式：

根据简单自行车运动学模型建立车辆状态模型S＝[x,y,θ,δ,v]^T，其中S为车辆运动状态，x,y是车后轴中心相对于全局坐标原点的横向坐标和纵向坐标，θ为车辆朝向角，δ为车前轮转角，v为车当前速度，L为自行车前轮中心到后轮中心的距离，分别为x、y、θ对时间的一阶导数；

采用五次样条插值方法生成候选路径，候选路径上的点的x坐标应当满足以下公式：

a、b、c、d、e和f是五次样条的系数，s为车移动的距离，这里的x不是对时间t求导，而是对车移动距离s求导，公式推导如下：

dx/dt＝vcosθ

ds/dt＝v

首先，获取车辆自身状态S(x₀,y₀,θ₀,δ₀,v₀)，作为样条函数的起始条件，代入公式(*)，得方程组：

其中θ₀’＝tan(δ₀)/L；

然后，将局部路径终点信息，记为(x_se,y_se,θ_se)，作为终点条件，代入公式(*)，得方程组：

整理方程组后写成AB＝C矩阵方程形式：

其中

A＝[a b c]

C＝[sin(θ₀)θ₀'se²/2-cos(θ₀)se-x₀+x_se sin(θ₀)θ₀'se-cos(θ₀)+cos(θ_se) sin(θ₀)θ₀']

通过矩阵求逆运算A＝CB^-1求出矩阵A的值，于是求得了五次样条所有系数a、b、c、d、e和f，最后，根据五次样条函数计算所有候选路径点的x坐标；

以相同的方法，计算候选路径点的y坐标；

根据候选路径每个点的x坐标和y坐标，生成从实验车自身出发到局部路径终点的一条候选路径，然后将终点集合的其他终点分别作为终点条件生成其他的候选路径，最后产生的候选路径集合，即模板；

步骤S303、备选路径选择：计算每条备选路径的代价，采用代价最小的备选路径更新局部规划结果，具体地：

根据以下公式计算左边15条候选路径的代价，找出代价最小的备选路径：

若侯选路径遇障，则代价为无穷大，不参与上述公式计算，其中(x_c,y_c)为车自身坐标，(x_i,y_i)为候选路径终点坐标，i取值范围为1到15，I代表候选路径序号；

步骤S4、更新局部规划结果；

更新局部规划结果适应交通场景的不断变化，包括速度设定和更新当前局部路径和拓展轨迹两部分。