CN112857385A - 一种基于非均匀栅格模型的快速无人车局部路径规划方法 - Google Patents

Abstract

本发明公开了一种基于非均匀栅格模型的快速无人车局部路径规划方法，能够减少计算量，不陷入局部最优解，实现优化算法的快速收敛。该方法首先将局部路径规划的起点S到终点G的定义为规划空间的x轴，采用垂直于x轴的栅线将规划空间划分成非均匀的N个分格，从起点S到终点G所述栅线由密到疏；局部路径由栅线上的点组成，从而将二维空间的路径规划问题，转化为对栅线上点的纵坐标规划问题；构建路径评价函数；所述路径评价函数由危险度评价函数、曲率惩戒函数、长度评价函数和偏差惩戒函数组成；最后，以各栅线上点的纵坐标作为优化变量，以所述路径评价函数为适应度函数，采用协方差矩阵自适应进化策略进行栅线上点的纵坐标优化。

Description

一种基于非均匀栅格模型的快速无人车局部路径规划方法

技术领域

本发明属于智能车技术领域，具体涉及一种无人车局部路径规划方法。

背景技术

随着5G技术、人工智能、传感器等前沿学科的发展，无人驾驶技术逐步进入我们的生活。无人驾驶技术主要包括：导航定位、环境感知、路径规划和决策控制，其中路径规划技术是无人驾驶车辆的关键技术之一。路径规划问题可以分为基于地图的全局路径规划问题和基于传感器的局部路径规划问题。全局路径规划依赖于高精度的全局地图信息，求解出从起点到终点的最优路径。而局部路径规划依赖传感器获得外界环境的实时信息，对全局路径中出现的障碍物实现避障，求解出车辆从当前位置到全局路径点的可行路径，是全局路径规划的实时补偿策略。

目前的无人驾驶局部路径规划算法主要有A星搜索算法、人工势场法、神经网络法等。

申请号为201910637108.4的专利公开了一种基于改进A星搜索的无人驾驶汽车局部路径规划方法，给算法中所有拓展的节点带上方向属性，引入汽车转弯特有的机械约束；引入引导线L，引导线L由一系列的引导点组成，是无人驾驶汽车领域用于引导车辆前进的一种信息表示；提出用L来修改A星算法的评估函数，使其更符合人类驾驶员的驾驶习惯；引入关键点keyPoint的概念，用keyPoint来引导A星算法实现绕障的过程。

申请号为201910943784.4的专利公开了一种基于人工势场法的无人车辆路径规划方法，包括以下步骤：1)、构建无人车行驶的二维空间模型；2)、建立虚拟势场；3)使无人车行驶一个单位步长l，判断无人车是否陷入局部最小值点，如果是则调用步骤4)，否则进行步骤5)；4)改变斥力在X轴上的分量后回到步骤2)重新开始；5)判断无人车是否行进到目标点附近的影响距离内造成目标不可达，如果是则调用步骤6)，否则进行步骤7)；6)在斥力势场函数中引入安全距离ρs和无人车到目标点之间的距离ρt后回到步骤(2)重新开始；7)判断无人车车是否到达目标点，如果是则停止路径规划画出路径，否则回到步骤(2)重新开始。

申请号为201910800165.X的专利公开了一种基于双反向传播神经网络的无人系统智能局部路径规划方法，针对智能无人系统动态障碍规避问题，首先，建立智能无人系统运行的环境模型；其次，建立无人系统运行的运动学模型；第三，设计由两个反向传播神经网络组成拓扑结构的双反向传播神经网络，进行局部路径规划，第一个反向传播神经网络经过适当的离线训练，在动态避障中起到主导作用，第二个反向传播神经网络在线训练以获得输出的补偿。

已公开技术能够实现无人车的局部路径规划，但在实际使用中往往存在一些问题，如A星算法寻优速度慢、计算量大，路径不够平滑，传统的人工势场法易陷入局部最优解，神经网络法对计算机运算性能要求高、实时性差。

发明内容

有鉴于此，本发明提供了一种无人车局部路径规划方法，能够减小模型的复杂度，进而减小计算量；保证每一个路径点都在向前推，不陷入局部最优解；且能够实现优化算法的快速收敛，提高局部路径规划算法的实时性。

为了解决上述技术问题，本发明是这样实现的。

一种基于非均匀栅格模型的快速无人车局部路径规划方法，包括：

步骤1、将局部路径规划的起点S到终点G的定义为规划空间的x轴，采用垂直于x轴的栅线将规划空间划分成非均匀的N个分格，从起点S到终点G所述栅线由密到疏；局部路径由栅线上的点组成，从而将二维空间的路径规划问题，转化为对栅线上点的纵坐标规划问题；

步骤2、构建路径评价函数；所述路径评价函数由危险度评价函数、曲率惩戒函数、长度评价函数和偏差惩戒函数组成；危险度评价函数值表征局部路径的各路径点与障碍物之间的距离；曲率惩戒函数值表征与路径点总曲率成正向关系的惩戒值；长度评价函数值表征局部路径的总长度；偏差惩戒函数值表征与局部路径、全局路径之间的偏差呈正向关系的惩戒值；危险度评价函数和曲率惩戒函数为强评价函数，具有明显的临界值，当优化变量超过临界值时，函数值相对迅速增大；当优化变量小于临界值时，函数值相对缓慢变化；长度评价函数和偏差惩戒函数为弱评价函数，没有明显的临界值，函数值随优化变量的变化缓慢变化；

步骤3、以各栅线上点的纵坐标作为优化变量，以所述路径评价函数为适应度函数，采用协方差矩阵自适应进化策略进行栅线上点的纵坐标优化；在每一次迭代过程中，采用如下公式自适应调整种群规模：

其中，λ_g为g代种群规模，α为个体正优化比率，α₀为个体正优化比率参考值，M为种群优化指数；当前种群个体的适应度函数值若优于父代中最优个体，定义为个体正优化，反之为个体负优化，正优化个体数与种群总个体数之比定义为个体正优化比率α；若当前种群的最优个体适应度函数值优于父代最优个体，定义为种群正优化，反之为种群负优化；当种群连续正优化时，种群优化指数M为连续种群正优化的代数，当发生连续种群负优化时，种群优化指数M为负的种群负优化代数。

优选地，步骤1中，非均匀的N个分格的栅线横坐标为：

其中，L为局部路径即起点S到终点G的直线总长度，n为栅线序号，n∈[1,N]，β为栅线疏密度指数，β为1时，栅线均匀分布，β越大疏密变化越明显。

优选地，β＝2。

优选地，所述危险度评价函数的值为整条局部路径上各障碍物与各路径点的距离评价值之和；

构造表征规划空间中第h个障碍物与第n个路径点D_n之间距离的评价函数为：

其中，k_obs为系数常量，l_min为设定的最小安全距离，l_max为设定的最大危险距离，l_h→n为第h个障碍物与第n路径点D_n的距离；

则整条局部路径的危险度评价函数为：

其中，H为规划空间中障碍物总数。

优选地，所述曲率惩戒函数值为考虑无人车的转角限制情况下，整条局部路径上各路径点的曲率对应的惩戒值；

首先计算各路径点D_n对应的曲率半径，使用三分法求得最大曲率半径K_n(t)_max，进而得到路径点D_n对应的最小曲率半径：

则局部路径的曲率惩戒函数为：

其中，γ＞1为梯度系数，R_min为无人车的最小转弯半径，R₀为局部路径的起始点。

优选地，所述长度评价函数值为局部路径上各段路径的长度之和：

其中，(x_n,y_n)为局部路径上路径点D_n的横纵坐标。

优选地，所述偏差惩戒函数值为局部路径上每一个路径点与指定的全局路径的最近距离的累加和。

优选地，所述路径评价函数由危险度评价函数值、曲率惩戒函数值、长度评价函数值和偏差惩戒函数值加权计算得到。

优选地，局部路径采用Bezier曲线拟合，路径点D_n对应的曲率为：

其中，

和ψ(t)分别对应路径点D_n的(x_n,y_n)，t∈[0,1]。

优选地，采用Bezier曲线对局部路径进行拟合的方式为：

使用二次B样条曲线对规划的轨迹点进行拟合，二次B样条曲线由多段Bezier曲线构成；

对于局部路径的起始点和结束点做单独处理：在起始点S与第一条栅线中间增加一条构造栅线，以起始点S为起点，沿无人车的朝向方向作延长线，延长线与构造栅线的交点记为D₀作为Bezier曲线的控制点P_0，1，记线段SP_0，1和P_0，1D₁的中点分别为P_0，0、P_0，2，并作为构造Bezier曲线的端点，可得到起始点的构造Bezier曲线P_0，0P_0，2；其中，D₁为第一条栅线上的轨迹点；

同理，在结束点G与最后一个规划点D_N中间添加一个构造栅线，沿结束点的朝向反向作延长线，延长线与构造栅线的交点记为D_N+1作为Bezier曲线的控制点P_N+1，1，记线段D_NP_N+1，1和P_N+1，1G的中点分别为P_N+1,0、P_N+1,2，并作为构造Bezier曲线的端点；可得到结束点的构造Bezier曲线P_N+1，0P_N+1，2。

有益效果：

(1)本发明采用非均匀栅格建模法，首先纵向栅线的划分，将二维空间的路径规划问题，转化为对栅线上点的纵坐标规划问题，这使得规划空间模型得到极大的简化；其次，使用非均匀化策略，对距离较远的模型稀疏化处理，可进一步简化模型，减小模型的复杂度，进而减小计算量。而且，这种划分方式，保证每一个路径点都在向前推进，并最终能够到达局部目标点，不会如传统的人工势场法一样在障碍物较多的地形易陷入局部最优解。

(2)路径的评价函数中，包含了危险度、运动学约束、长度和跟随性，使用较少的计算量实现了对路径性能的全面评价。

(3)使用自适应种群规模调整策略对协方差矩阵自适应进化策略(covariancematrix adaptive evolution strategy,CMA-ES)改进，当进化质量较差是通过扩大种群规模的方式增加种群的多样性；当进化质量较高时，适当减小种群规模，降低算法的计算复杂度。实现优化算法的快速收敛，提高局部路径规划算法的实时性。同时，当种群连续多次负优化时，说明出现了严重的多样性不足。通过引入种群优化指数M，可在种群规模不合适时快速调整种群规模，在保证种群多样性的前提下尽量减小种群规模。

(4)危险度评价函数与曲率惩戒函数具有强评价函数的特性，当优化变量超过对应的临界值时，函数值将迅速增大。因此，危险度评价函数能够有效避免无人车与障碍物的碰撞，曲率惩戒函数保证了规划出的路径满足无人车最小转弯半径的运动学约束。

(5)本发明采用Bezier曲线对路径点进行曲线拟合，且在拟合过程中通过增加构造栅线实现了路径曲率的平滑过渡，有利于路径点曲率的计算。

附图说明

图1为规划空间模型示意图；

图2为二次B样条曲线路径拟合示意图；

图3为起点与终点的曲线拟合示意图；

图4为运动学约束示意图；

图5为偏差惩戒示意图；

图6为CMA-ES算法流程图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

步骤1、规划空间建模，进行非均匀栅格划分。

在实现局部路径规划过程中，首先需要对规划空间进行建模。在建模的过程中若只追求规划模型的精度，往往会导致问题复杂化，影响局部路径规划算法的实时性、稳定性和鲁棒性。为了能够在模型精度与算法复杂度之间进行权衡，本发明提出一种非均匀栅格建模法。

非均匀栅格建模法通过非均匀分布的纵向栅线将规划空间进行划分。如图1所示，圆圈为障碍物，S点为局部路径规划的起点，G点为局部路径规划的终点。将S点作为规划空间的原点，

方向为规划空间的x轴方向。使用垂直于x轴的栅线将规划空间划分成N份。通过这种纵向栅线的划分，将二维空间的路径规划问题，转化为对栅线上点的纵坐标规划问题，这使得规划空间模型得到极大的简化。

局部路径规划是一种实时的路径规划，规划出的路径未走完之前就会对路径进行重新规划，因此路径末段对当前的运动影响较小。为了在保证模型精度的情况下，进一步降低模型的复杂度，对纵向栅线进行非均化处理。从起点S到终点G，纵行栅线由密到疏，保证较近范围内拥有更高的精度。栅线的横坐标可用下式得到：

其中，L为局部路径即

的总长度，N为路径划分的份数，n为栅线序号，n∈[1,N]，β为栅线疏密度指数，β为1时，栅线均匀分布，β越大疏密变化越明显，通常取β＝2。

步骤2、建立规划路径模型。

在后续的路径规划过程中，需要根据路径曲率计算适应度。而上述的规划空间模型为了降低计算量将轨迹离散成了N个点，为了实现轨迹的平滑连接以及后续的曲率计算，需对规划的路径进行建模。

本发明使用二次B样条曲线对规划的轨迹点进行拟合，二次B样条曲线由多段Bezier曲线构成，实现了端点的平滑过渡。如图2所示，D_n-1,D_n,D_n+1为规划的路径点，P₀为D_{n- 1}D_n的中点坐标，作为Bezier曲线的起点，P₂为D_nD_n+1的中点坐标，作为Bezier曲线的终点，P₁与D_n重合，作为Bezier曲线的控制点。D_n对应的Bezier曲线方程可由下式求得

B(t)＝(1-t)²P₀+2t(1-t)P₁+t²P₂,t∈[0,1] (2)

其中，B(t)为Bezier曲线的坐标，

P₁＝D_n,

由于路径规划的起点与终点具有方向属性，起点的方向为无人车的朝向，终点的方向为全局轨迹点的方向。为了使无人车到达目标点与全局参考轨迹相同，同时在起始点满足运动学约束，需对起始点和结束点做单独处理。如图3所示，在起始点S与第一条栅线之间增加一条构造栅线，箭头为起始点S的方向，其延长线与构造栅线的交点记为D₀作为Bezier曲线的控制点P_0，1，记SP_0，1和P_0，1D₁中点分别为P_0，0，P_0，2作为Bezier曲线的端点。因此，可得到起始点的Bezier曲线。同理，在结束点与最后一个规划点中间添加一个栅线，增加一段构造Bezier曲线。通过上述方法，确保了规划的轨迹能够与规划起始点和终点的航行角平滑相连。

步骤3、构造路径评价函数。

通过构建合理的路径评价函数，可引导优化算法得到最优路径。路径评价函数应有效、全面的衡量路径的品质。本发明将路径的评价函数分成两类：强评价函数和弱评价函数。其中，强评价函数具有明显的临界值，当优化变量超过临界值时，评价函数值将迅速增大；当优化变量小于临界值时，评价函数值缓慢变化。如：无人车与障碍物的距离小于最小安全距离、规划的路径最小曲率半径小于无人车的最小转弯半径。弱评价函数的优化变量没有明显的临界值，评价函数值随优化变量的变化缓慢变化。如：规划路径的总长度、规划路径与全局路径的误差大小。下面从4个方面对路径进行评价。

(1)危险度评价函数

路径规划的前提是能够避开障碍物，使用路径点与障碍物之间的距离构造危险度评价函数。危险度评价函数的值为整条局部路径上各障碍物与各路径点的距离评价值之和。由于危险度函数是强评价函数，需要在路径点小于安全距离时产生较大的惩戒值，给出规划空间中第h个障碍物与第n路径点产生的评价值为：

其中，k_obs为系数常量，l_min设定的最小安全距离，l_max为设定的最大危险距离，l_h→n为第h个障碍物与第n路径点D_n的距离。因此，可得到整条局部路径的危险度函数为：

其中，H为规划空间中障碍物总数。

(2)曲率惩戒函数

在局部路径规划过程中还需要关注路径曲率，过多的转弯将影响行驶的稳定性，尤其在高速行驶情况下，可能发生侧翻，因此路径应尽可能平直。同时，在避让障碍物过程中，应考虑无人车的转角限制。如图4所示，箭头为无人车的当前航向角，曲线1、曲线2为无人车在最小转弯半径下向两侧行驶的轨迹，因此无人车的轨迹将被限制在阴影区域。

Bezier曲线方程可改写为如下形式

其中，t∈[0,1]。则路径点D_n对应的Bezier曲线曲率可通过下式计算：

由于上式的曲率函数是一个单峰函数，可使用三分法求得最大曲率半径K_n(t)_max，进而得到路径点D_n对应的最小曲率半径：

轨迹曲率半径应不小于无人车的最小转弯半径。通过上述公式，可以计算出各规划路径点的曲率半径R₁,R₂…R_N和起始点R₀的构造曲线的曲率半径R₀。则路径的曲率惩戒函数为：

其中，γ＞1为梯度系数，R_min为无人车的最小转弯半径。可见，曲率惩戒函数值是与路径点总曲率成正向关系的惩戒值。

(3)长度评价函数

在局部路径规划中，希望从起点到终点的路径尽可能短，路径长度是路径规划的基本指标。由于规划出的路径采用二次B样条曲线拟合，并不适合计算长度，这里给出路径长度评价函数的简化算法：

其中，(x_n,y_n)为局部路径上路径点D_n的横纵坐标。

(4)偏差惩戒函数

局部路径规划的起点为无人车的当前位置，目标点为全局轨迹上的预瞄点。如图5所示，曲线为规划的局部路径，虚线为指定的全局路径T_r，该全局路径一般都来自全局规划阶段。局部规划的轨迹应尽可能接近全局路径。对每一个规划路径点与全局路径的最近距离累加，得到偏差惩戒函数如下：

其中，dis(D_n,T_r)为规划轨迹的D_n点与参考轨迹T_r的最近距离。

通过以上四个方面对规划路径的不同性能进行评价，得到如下的路径总体评价函数：

F_cost＝k₁f_dist+k₂f_obs+k₃f_ang+k₄f_path (11)

其中，k₁，k₂，k₃，k₄分别为各自的权重。可根据不同的需求对权进行调整。

步骤4、以各栅线上点的纵坐标作为优化变量，以所述路径评价函数为适应度函数，采用协方差矩阵自适应进化策略进行栅线上点的纵坐标优化。

本发明采用协方差矩阵自适应进化策略(covariance matrix adaptiveevolution strategy,CMA-ES)实现优化，并对其进行改进。

协方差矩阵自适应进化策略是由法国科学家Hansen提出的一种应用于非线性非凸连续优化问题的数值优化算法，通过更新协方差矩阵对个体的突变方向进行调整，不断迭代更新，最终得到最优解。CMA-ES算法的优化过程主要包括：初始化、突变、竞争与选择、重组。CMA-ES算法的流程如图6所示，其详细实现步骤如下：

(1)参数设置与初始化

根据上述的路径规划问题，需要对算法的参数进行初始化，包括优化变量的维数(列栅线数)、最大迭代次数、种群规模、优化终止条件、初始协方差矩阵等。

(2)种群突变

为了实现种群个体的多样化，CMA-ES算法以均值为中心，突变产生一定数量的个体。生成下一代的公式如下：

其中，

为g+1代的第k个种群个体，m^g为g代种群的均值，σ^g为g代的全局步长，C^g为g代种群的协方差矩阵，λ为种群规模大小。

(3)竞争与选择

CMA-ES算法采用(μ,λ)策略，利用路径总体评价函数式(11)评价个体的好坏。λ个个体按评价值从小到达排列，选择前μ个个体作为下一代的父本，用于优化参数的更新。一般取μ≈λ/2。

(4)参数更新

使用上一步产生的μ个个体更新种群参数，主要有均值、协方差矩阵和步长。均值的更新通过μ个最优个体加权平均得到：

其中，

ω_1,...,μ为个体在均值中所占的权重。

为

中排序为i的个体，且

f(·)为路径总体评价函数式(11)。

协方差矩阵决定了个体突变的方向，在CMA-ES算法中起着决定性作用。CMA-ES算法采用了秩μ更新和秩1更新。秩μ更新使用当前代的信息更新协方差矩阵C，其算法如下：

其中

c_μ为C的学习律。

秩1更新结合了历代信息更新协方差矩阵C,其算法如下：

其中

p_c为学习律，μ_eff为方差有效选择质量，c₁为协方差矩阵学习律。

将两种更新策略结合最终得到下式：

为了实现算法快速收敛，使用如下的步长更新策略：

其中

c_σ为步长学习律，d_σ为阻尼参数。

(5)停止判断

判断是否达到最大迭代次数或达到目标精度。符合停止条件则输出最优解，否则回到步骤(2)。

根据本发明的局部路径规划算法特点，对CMA-ES算法进行改进。在CMA-ES算法寻优过程中，种群的大小将直接影响着优化的结果。若种群规模太小，将降低种群的多样性，易陷入局部最优解。若种群规模太大，增加了计算量影响算法的实时性，且由于存在大量相同个体，对优化结果的提升并无明显作用。考虑到优化算法的实时性，种群规模还应与优化变量的维数匹配，给出种群规模的参考值为：

λ₀＝η+4lg N (18)

其中，η为初始种群规模，N为优化变量的维数。

为了表示种群进化的质量，给出如下定义。当前种群个体的适应度函数值若小于(优于)父代中最优个体，定义为个体正优化，反之为个体负优化，正优化个体数与种群总个体数之比定义为个体正优化比率。若当前种群的最优个体适应度函数值小于(优于)父代最优个体，定义为种群正优化，反之为种群负优化。定义种群优化指数，当种群连续正优化时，种群优化指数为连续种群正优化的代数，当发生连续种群负优化时，种群优化指数为负的种群负优化代数。由此，提出如下的自适应种群规模算法：

其中，λ_g为g代种群规模，α为个体正优化比率，α₀为个体正优化比率参考值，M为种群优化指数。通过上式，实现当进化质量较差(α较小)是通过扩大种群规模的方式增加种群的多样性；当进化质量较高(α较大)时，适当减小种群规模，降低算法的计算复杂度。当种群连续多次正优化时，说明出现了严重的种群规模过大；当种群连续多次负优化时，说明出现了严重的多样性不足。通过引入种群优化指数M，可在种群规模不合适时快速调整种群规模，在保证种群多样性的前提下尽量减小种群规模。

以上的具体实施例仅描述了本发明的设计原理，该描述中的部件形状，名称可以不同，不受限制。所以，本发明领域的技术人员可以对前述实施例记载的技术方案进行修改或等同替换；而这些修改和替换未脱离本发明创造宗旨和技术方案，均应属于本发明的保护范围。

Claims (10)

1.一种基于非均匀栅格模型的快速无人车局部路径规划方法，其特征在于，包括：

步骤1、将局部路径规划的起点S到终点G的定义为规划空间的x轴，采用垂直于x轴的栅线将规划空间划分成非均匀的N个分格，从起点S到终点G所述栅线由密到疏；局部路径由栅线上的点组成，从而将二维空间的路径规划问题，转化为对栅线上点的纵坐标规划问题；

步骤2、构建路径评价函数；所述路径评价函数由危险度评价函数、曲率惩戒函数、长度评价函数和偏差惩戒函数组成；危险度评价函数值表征局部路径的各路径点与障碍物之间的距离；曲率惩戒函数值表征与路径点总曲率成正向关系的惩戒值；长度评价函数值表征局部路径的总长度；偏差惩戒函数值表征与局部路径、全局路径之间的偏差呈正向关系的惩戒值；危险度评价函数和曲率惩戒函数为强评价函数，具有明显的临界值，当优化变量超过临界值时，函数值相对迅速增大；当优化变量小于临界值时，函数值相对缓慢变化；长度评价函数和偏差惩戒函数为弱评价函数，没有明显的临界值，函数值随优化变量的变化缓慢变化；

步骤3、以各栅线上点的纵坐标作为优化变量，以所述路径评价函数为适应度函数，采用协方差矩阵自适应进化策略进行栅线上点的纵坐标优化；在每一次迭代过程中，采用如下公式自适应调整种群规模：

其中，λ_g为g代种群规模，α为个体正优化比率，α₀为个体正优化比率参考值，M为种群优化指数；当前种群个体的适应度函数值若优于父代中最优个体，定义为个体正优化，反之为个体负优化，正优化个体数与种群总个体数之比定义为个体正优化比率α；若当前种群的最优个体适应度函数值优于父代最优个体，定义为种群正优化，反之为种群负优化；当种群连续正优化时，种群优化指数M为连续种群正优化的代数，当发生连续种群负优化时，种群优化指数M为负的种群负优化代数。

2.如权利要求1所述的方法，其特征在于，步骤1中，非均匀的N个分格的栅线横坐标为：

其中，L为局部路径即起点S到终点G的直线总长度，n为栅线序号，n∈[1,N]，β为栅线疏密度指数，β为1时，栅线均匀分布，β越大疏密变化越明显。

3.如权利要求2所述的方法，其特征在于，β＝2。

4.如权利要求1所述的方法，其特征在于，所述危险度评价函数的值为整条局部路径上各障碍物与各路径点的距离评价值之和；

构造表征规划空间中第h个障碍物与第n个路径点D_n之间距离的评价函数为：

其中，k_obs为系数常量，l_min为设定的最小安全距离，l_max为设定的最大危险距离，l_h→n为第h个障碍物与第n路径点D_n的距离；

则整条局部路径的危险度评价函数为：

其中，H为规划空间中障碍物总数。

5.如权利要求1所述的方法，其特征在于，所述曲率惩戒函数值为考虑无人车的转角限制情况下，整条局部路径上各路径点的曲率对应的惩戒值；

首先计算各路径点D_n对应的曲率半径，使用三分法求得最大曲率半径K_n(t)_max，进而得到路径点D_n对应的最小曲率半径：

则局部路径的曲率惩戒函数为：

其中，γ＞1为梯度系数，R_min为无人车的最小转弯半径，R₀为局部路径的起始点。

6.如权利要求1所述的方法，其特征在于，所述长度评价函数值为局部路径上各段路径的长度之和：

其中，(x_n,y_n)为局部路径上路径点D_n的横纵坐标。

7.如权利要求1所述的方法，其特征在于，所述偏差惩戒函数值为局部路径上每一个路径点与指定的全局路径的最近距离的累加和。

8.如权利要求1所述的方法，其特征在于，所述路径评价函数由危险度评价函数值、曲率惩戒函数值、长度评价函数值和偏差惩戒函数值加权计算得到。

9.如权利要求5所述的方法，其特征在于，局部路径采用Bezier曲线拟合，路径点D_n对应的曲率为：

其中，

和ψ(t)分别对应路径点D_n的(x_n,y_n)，t∈[0,1]。

10.如权利要求1所述的方法，其特征在于，采用Bezier曲线对局部路径进行拟合的方式为：

使用二次B样条曲线对规划的轨迹点进行拟合，二次B样条曲线由多段Bezier曲线构成；

对于局部路径的起始点和结束点做单独处理：在起始点S与第一条栅线中间增加一条构造栅线，以起始点S为起点，沿无人车的朝向方向作延长线，延长线与构造栅线的交点记为D₀作为Bezier曲线的控制点P_0，1，记线段SP_0，1和P_0，1D₁的中点分别为P_0，0、P_0，2，并作为构造Bezier曲线的端点，可得到起始点的构造Bezier曲线P_0，0P_0，2；其中，D₁为第一条栅线上的轨迹点；

同理，在结束点G与最后一个规划点D_N中间添加一个构造栅线，沿结束点的朝向反向作延长线，延长线与构造栅线的交点记为D_N+1作为Bezier曲线的控制点P_N+1，1，记线段D_NP_N+1，1和P_N+1，1G的中点分别为P_N+1,0、P_N+1,2，并作为构造Bezier曲线的端点；可得到结束点的构造Bezier曲线P_N+1，0P_N+1，2。

Images