一种未知障碍物分布的避障路径鲁棒规划方法及系统

1.本发明属于路径规划相关技术领域，尤其涉及一种未知障碍物分布的避障路径鲁棒规划方法及系统。


背景技术：

2.本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技术。
3.无人驾驶技术一直是机器人领域备受关注的研究热点，经过数十年的发展与探索，无人驾驶技术快速发展。近年来，移动机器人的操作范围从室内扩展到室外，从封闭区域扩展到半封闭甚至完全开放的区域，越来越多的实用技术被应用于低速车辆。近年来，人们对无人驾驶车辆的动态路径规划进行了大量的研究，传统的规划算法可分为四类：基于图搜索、基于采样、插值曲线和基于反应的算法。智能仿生算法包括遗传算法、粒子群算法等。
4.基于图搜索的路径规划方法是把规划问题向图搜索问题进行转化，对求解空间进行网格划分，建立成图，然后进行起始点到目标点的最优路径的搜索求解。基于图搜索的方法中常用的有a*、d*等算法。此类算法当环境复杂度较大时，算法的效率无法保证。
5.基于采样的算法对固定工作空间进行随机采样，以生成次优路径。快速探索随机树(rapidly-exploring random tree，rrt)和概率路线图(probabilistic roadmap，prm)方法是运动规划中常用的算法。这些算法通过随机选择分散点在整个空间中搜索最优路径，但是存在一些问题，例如成本高、实时性差，并且规划的路径可能不是最优路径。
6.智能算法模拟自然界中的生物进化和昆虫觅食行为，主要包括遗传算法(genetic algorithm，ga)、粒子群优化(particle swarm optimization，pso)算法等。遗传算法可以比较方便地与各类迭代算法结合，但是存在早熟、收敛路径质量低、种群多样性差、难以打破局部最优解等问题。尽管传统的粒子群算法具有较快的收敛速度，但是它也存在一些问题，例如可能陷入局部最优解并过早收敛，这些问题需要解决。
7.上述三类方法多用于全局路径的规划，但是也会与其他方法结合完成局部避障。
8.反应式算法通常被用作局部路径规划器，算法包括人工势场(artificial potential field，apf)和向量场直方图(vector field histogram，vfh)算法等。此类算法能够快速处理信息，这些信息通常来自有限的车载传感器。作为局部规划器，算法通常规划下一条即时路径或机动方向，以避开附近的障碍物，同时遵循另一个算法制定的全局规划。上述算法有着更快速、鲁棒性更高的特点，但是生成路径平滑性有所欠缺。
9.连续几何曲线插值法生成平滑的路径，可用作局部路径的生成，也可用作全局路径的平滑。几何曲线包括：多项式曲线、b样条曲线、贝塞尔曲线等。多项式曲线通过给定的起始段和结束段中的期望值或约束条件，可以计算出满足这些约束的多项式曲线的系数。在机器人轨迹规划中，这种方法被广泛应用，可以生成平滑的轨迹，并满足机器人在运动过程中的各种运动约束。b样条曲线是计算机辅助设计应用中常用的曲线，它具有更多特别适
合导航的特性，近年来在轮式机器人的路径规划中多有应用。b
é
zier曲线将曲线形状的计算转化为坐标点的求解，其曲线光滑且计算简单，被广泛用于路径规划，通过优化控制点的位置，满足曲率上限约束。
10.上述路径规划方法均保证了所规划路径平滑性和曲率连续性，但是在算法的快速性和鲁棒性上均有欠缺。


技术实现要素：

11.为克服上述现有技术的不足，本发明提供了一种未知障碍物分布的避障路径鲁棒规划方法及系统，所提方法在保证避障轨迹可执行性的前提下，轨迹长度更小且轨迹稳定性较好，在保证避障轨迹安全性的前提下，规划成本时间更小。
12.为实现上述目的，本发明的第一个方面提供一种未知障碍物分布的避障路径鲁棒规划方法，包括：
13.步骤1：获取车辆所处的环境信息，结合车辆可安全通过的最小通行宽度和已知的全局路径采用vfh算法确定最优可通行区域；
14.步骤2：根据所确定的最优可通行区域，确定局部避障路径目标点；
15.步骤3：从所确定的局部避障路径目标点与车辆的初始点之间确定控制点，通过引入未知参数拟合成分段贝塞尔曲线；
16.步骤4：以行驶路径最短和局部避障路径目标点方向变化量最小建立目标函数，在局部避障路径目标点方向约束条件下，求解所述未知参数，得到最优避障路径。
17.本发明的第二个方面提供一种未知障碍物分布的避障路径鲁棒规划系统，包括：
18.最优可通行区域确定模块：获取车辆所处的环境信息，结合车辆可安全通过的最小通行宽度和已知的全局路径采用vfh算法确定最优可通行区域；
19.目标点确定模块：根据所确定的最优可通行区域，确定局部避障路径目标点；
20.拟合曲线模块：从所确定的局部避障路径目标点与车辆的初始点之间确定控制点，通过引入未知参数拟合成分段贝塞尔曲线；
21.最优路径确定模块：以行驶路径最短和局部避障路径目标点方向变化量最小建立目标函数，在局部避障路径目标点方向约束条件下，求解所述未知参数，得到最优避障路径
22.本发明的第三个方面提供一种计算机设备，包括：处理器、存储器和总线，所述存储器存储有所述处理器可执行的机器可读指令，当计算机设备运行时，所述处理器与所述存储器之间通过总线通信，所述机器可读指令被所述处理器执行时执行一种未知障碍物分布的避障路径鲁棒规划方法。
23.本发明的第四个方面提供一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器运行时执行一种未知障碍物分布的避障路径鲁棒规划方法。
24.以上一个或多个技术方案存在以下有益效果：
25.在本发明中，结合车辆可安全通过的最小通行宽度和已知的全局路径来改进vfh算法确定最优可通行区域，提高了最优可通行区域的安全性、有效性；通过局部避障路径目标点与车辆的初始点之间确定控制点，通过引入未知参数拟合成分段贝塞尔曲线描述的最优规划路径，在保证路径平滑的前提下，实现了路径的最短，且优化所需时间较小，降低避
障能耗，满足了路径规划的实时性；通过建立行驶路径最短和局部避障路径目标点方向变化量最小建立目标函数以及局部避障路径目标点方向约束条件求解未知参数，从而得到长度最短、目标方向变化最小，满足车辆运动学约束的最优避障路径。
26.本发明附加方面的优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
27.构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。
28.图1为本发明实施例一中避障过程示意图；
29.图2为本发明实施例一中避障路径规划功能框图；
30.图3为本发明实施例一中鲁棒路径规划算法示意图；
31.图4为本发明实施例一中不规则边界约束示意图；
32.图5为本发明实施例一中向量场直方图；
33.图6为本发明实施例一中向量场二值化直方图；
34.图7为本发明实施例一中车辆最小通过宽度示意图；
35.图8(a)为本发明实施例一中最优内可通行区域与全局路径没有交集下，双侧均有障碍物情况下的可通行区域示意图；
36.图8(b)为本发明实施例一中最优内可通行区域与全局路径没有交集下，单侧有障碍物情况下的可通行区域一示意图；
37.图8(c)为本发明实施例一中最优内可通行区域与全局路径没有交集下，单侧有障碍物情况下的可通行区域另一示意图；
38.图9为本发明实施例一中最优内可通行区域与全局路径有交集下，可通行区域示意图；
39.图10为本发明实施例一中分段二次贝塞尔曲线示意图；
40.图11为本发明实施例一中局部路径规划流程图；
41.图12为本发明实施例一中避障实验场景；
42.图13为本发明实施例一中避障轨迹示意图；
43.图14为本发明实施例一中避障规划的成本时间对比图；
44.图15为本发明实施例一中避障轨迹长度对比图；
45.图16为本发明实施例一中避障路径航向角变化均方差对比图；
46.图17(a)为本发明实施例一中园区内道路边缘崎岖不平的场景；
47.图17(b)为本发明实施例一中为园区由宽敞环境进入狭窄的场景；
48.图17(c)为本发明实施例一中为园区内由狭窄环境进入宽敞的场景；
49.图17(d)为本发明实施例一中为园区内“s”型道路的场景；
50.图18(a)为本发明实施例一中无人车跟踪全局路径期间，无人车遇到避障场景；
51.图18(b)为本发明实施例一中为在实车实验中，无人车跟踪全局路径期间遇到障碍物时的实时运行显示图；
52.图19(a)为本发明实施例一中无人车跟踪全局路径期间，无人车绕过障碍物回归
到全局路径场；
53.图19(b)为本发明实施例一中无人车避过障碍回归到全局路径时的算法的实时运行显示图。
具体实施方式
54.应该指出，以下详细说明都是示例性的，旨在对本发明提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。
55.需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。
56.在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
57.实施例一
58.本实施例研究在园区内已知全局路径上行驶车辆遇到未知障碍物的避障路径规划，园区内行驶环境可能是有边界约束结构性道路，如单行车道或多车道，和非结构性环境，如广场，调度场站，道路边界不明等。本发明研究路径规划算法旨在使无人驾驶车辆可以遵从已知环境约束的前提下安全、快速、平滑地避开障碍物，回归到全局路径。
59.本实施例公开了一种未知障碍物分布的避障路径鲁棒规划方法，包括：
60.步骤1：获取车辆所处的环境信息，结合车辆可安全通过的最小通行宽度和已知的全局路径采用vfh算法确定最优可通行区域；
61.步骤2：根据所确定的最优可通行区域，确定局部避障路径目标点；
62.步骤3：从所确定的局部避障路径目标点与车辆的初始点之间确定控制点，通过引入未知参数拟合成分段贝塞尔曲线；
63.步骤4：以行驶路径最短和局部避障路径目标点方向变化量最小建立目标函数，在局部避障路径目标点方向约束条件下，求解所述未知参数，得到最优避障路径。
64.如图1所示，整个避障过程根据无人驾驶车辆与全局路径的位置关系可以分为三个阶段：全局路径偏离，障碍物规避，全局路径回归。如图2所示，本实施例所提方法主要由三部分组成，即最优可通行区域的确定、基于分段二次贝塞尔曲线的路径构造、路径的鲁棒规划。
65.针对园区内的半结构化道路的避障问题，首先需要在已知障碍物信息与环境信息的情况下，确定避障的可行性方向，如图3所示，即中间灰色区域；然后确定局部避障路径的目标点p4及其梯度约束，并结合起始p0及其梯度约束，研究分段二次贝塞尔曲线的控制点p1,p3及分段点p2的确定方法，进而实现局部避障路径的贝塞尔曲线的构造；在车辆运行曲率及目标点方向约束下，优化贝塞尔曲线参数，使避障路径最短且目标点方向变化最小。
66.本实施例研究园区内无人车未知障碍物分布的路径鲁棒规划，在园区内行驶场景较为复杂，道路边界模糊不清且极其不规则。因此本实施例在可通行区域确定过程中，既要考虑感知到的障碍物信息，还要考虑车道边缘和非结构化场景的边界约束。
67.园区内结构道路采用传感器难以确定其道路边界，因此可沿着车道边缘采集坐标点，采用曲线拟合方法建立车道的边界线，进而构成车道信息的电子地图。
68.同理，对非结构化场景，通过采集该场景的边界点，构造该类场景的边界。本实施
例将位于车辆探测范围内的道路边缘采集点设置为虚拟障碍物，虚拟障碍物与当前感知到的障碍物信息做相同处理。如图4所示，两侧线为当前行驶场景边界，穿过黑色块的线为已规划好的全局参考路径，车辆所在的线为无人车避障路线，黑色块为障碍物，黑色边界为探测范围内的道路边界，d
max
为车辆最大环境感知距离。
69.在本实施例步骤1中，结合车辆最小通行宽度改进了传统vfh算法候选通行区域筛选策略；同时，通过修改候选方向损失函数，将传统vfh系列算法以终点牵引改进为以全局路径为牵引，从而获得最优可通行区域。使得改进的vfh算法可以应对已知全局路径时园区内复杂环境的避障问题。
70.vfh算法是将车辆周围的环境以栅格的形式进行划分，通过车载传感器实时采样，不停更新二维栅格，并将智能车周围一定范围定为一个活动窗口进行分析。每一个栅格作为一个活动单元，然后将活动窗口划分为扇区计算每个扇区的障碍物强度值mn，每个扇区的障碍物强度表示为：
71.mn＝c
v2
(a-bd
n2
)
ꢀꢀꢀ
(1)
72.其中，dn为障碍物到无人车的距离；cv为概率函数，不同于传统的超声波传感器需要对锥形区域内的障碍物网格置信度进行概率评估，激光雷达能够准确返回某一方位的距离测量值，因此，当障碍物网格被障碍物覆盖时cv为固定值，固定值的大小可以看情况而定，并无特殊要求；d为活动单元到无人车的距离；a,b均为常数，a，b满足d
max
为激光雷达最大探索距离。所得到的极坐标直方图如图5所示。
73.由公式(1)可知每个距离值dn都有对应的障碍物强度值，设置某一合适的距离为距离阈值d
th
用其对应障碍物强度值对向量场直方图进行二值化，由此可得到可通行扇区直方图。
74.可通行扇区直方图是方向选择的基础与前提，而可通行扇区直方图的构造与阈值d
th
的取值密切相关，因此阈值d
th
的选取对vfh系列算法至关重要。考虑了无人车硬件特性、运动特性和目标环境，设置最大距离阈值和最小距离阈值将区间以一定步长δd
th
切割，可得距离阈值集合。表示为：则障碍物强度阈值集合可表示为：障碍物强度阈值集合某个元素可表示为二值化规则如下式所示：
[0075][0076]
其中，mn为障碍物强度值，障碍物强度阈值。
[0077]
依据上二值化规则可建立当前阈值下的二值化直方图，如图6所示，根据当前距离阈值对应的障碍物强度阈值将无人车环境划分为安全扇区和危险扇区。扇区的范围如图6横坐标所示用角度划定，在安全扇区内筛选可通行区域。为确定可通行区域的安全性，本实施基于阈值距离确定角度阈值给出了可通行区域的角度范围，如果角度范围上
下限的差值大于则确定为候选可通行区域
[0078]
阈值由下式获得：
[0079][0080]
其中，w
min
为确保车辆可以安全通过的最小通行宽度，计算公式如下：
[0081]wmin
＝w
p
+r
min
ꢀꢀꢀꢀ
(4)
[0082][0083]wp
＝w+2rmin(1-cosθ
max
)
ꢀꢀꢀ
(6)
[0084]
如图7所示其中，r
min
是车辆最小转弯半径，w
p
是车辆最窄行驶通道宽度，lw为车辆轴距；lf为前悬长度；lk为主销中心距，w为整车宽度，θ
max
为转向轮最大转角。
[0085]
候选可通行区域中的候选可行性方向为可通行区域的中线方向如下式所示：
[0086][0087]
将所获得的候选方向c
ij
代入下式，得到当前障碍物强度阈值下最优可行性方向：
[0088][0089]
传统vfh算法使用终点作为运动方向的牵引，为适应园区内复杂路线如：环形路线、曲线等，改进了传统vfh算法使用全局路径牵引运动方向。公式(8)为改进的候选方向损失函数，公式(8)中δθi为候选方向c
ij
与最近全局路径点切线方向夹角；为候选方向c
ij
与无人车当前运动方向的夹角；δφi表示候选方向c
ij
与上一次运动方向的夹角；μ1＞0,μ2＞0,μ3＞0，μ1≥μ2+μ3。
[0090]
假设障碍物强度阈值集合中有n个元素，则可知候选可通行方向有n个，每个代表着一个可通行区域，利用代价函数公式(9)，求得最小值，其对应和记为最优通行方向c
*
和最优距离阈值其目的为获得安全的距离阈值和接近全局路径前进方向的通行方向。根据c
*
结合公式(7)可得到了最优的可通行性区域角度上下限将用于在本实施例步骤2局部避障路径目标点的确定。代价公式如下：
[0091][0092]
其中，η(η＞0)为阈值权重系数，为与终点方向夹角。
[0093]
在本实施例步骤2中，采用分段二次贝塞尔曲线描述局部避障路径，根据不同的避障阶段，给出曲线控制点与分段点的确定方法。另外，较高的贝塞尔曲线计算量较大，不能满足算法的实时性要求。另一方面，为使得规划出局部路径最短，本实施例将路径长度最短为优化目标，而高阶贝塞尔曲线的长度难以精确求解。
[0094]
根据步骤1中所确定的最优可通行区域如图7所示，为保证轨迹规划的安全性对最优可通行区域边沿处的障碍物端点进一步膨胀，膨胀半径为车辆最小转弯半径，得到了更加安全的最优可通行区域，称之为最优内可通行区域。
[0095]
根据外部环境的不同最优内可通行区域角度范围可表示为：根据外部环境的不同最优内可通行区域角度范围可表示为：为向量方向，为向量方向，为车辆右侧障碍物左端点膨胀圆从p0点所作切线切点，为车辆左侧障碍物右端点膨胀圆从p0点所作切线切点。
[0096]
需要说明的是，为描述方便，上述的“左”“右”是以车辆行驶方向上的左右进行描述的。
[0097]
本实施例分两种情况确定目标状态，情况1：最优内可通行区域与全局路径没有交集；情况2：最优内可通行区域与全局路径存在交集。
[0098]
具体的，对于情况1，如图8(a)所示，全局路径与最优内可通行区域没有交集，内可通行区域角度范围为则局部路径目标点和中距离全局路径最小的点为局部路径目标点，即：
[0099][0100]
其中，d
l
、dr分别为到全局路径最短距离。
[0101]
如图8(b)、图8(c)所示，为最优内可通行区域单侧有障碍时全局路径与内可通行区域没有交集的情况，内可通行区域角度范围为p
*
为最优可行方向c
*
上与车辆距离为的一点，此时局部路径目标点取和p
*
中距离路径最小的点为局部路径目标点，即：
[0102][0103]
其中，d
l
、d
*
分别为p
*
到全局路径最短距离。
[0104]
在情况1中，局部路径目标点处的方向为向量p
4-p0的方向，可表示为
[0105][0106]
对于情况2，如图9所示，若在内可通行区域内有全局路径点，内可通行区域的检测
距离为最大探测距离d
max
，取内可通行区域最远边界与全局路径交点为p4，此时p4也称作回归点。
[0107]
当局部路径目标点取全局路径点时，其方向为该点的切线方向，设该点的切线方向向量为g
tan
，g
tan
与向量p
4-p0夹角为θ
tan
，则：
[0108][0109]
在本实施例步骤3中，基于分段贝塞尔曲线描述的局部避障路径如图10所示，p0第一段二次贝塞尔曲线初始点，p4第二段二次贝塞尔曲线终止点，p1，p3分别为两条贝塞尔曲线的控制点，p2为两段曲线的分段点，通过移动控制点可以得到一条安全、舒适的避障路径曲线。
[0110]
该路径可描述为：
[0111]
bz1(t)＝(1-t2)p0+2t(1-t)p1+t2p2,0≤t≤1
[0112]
bz2(t)＝(1-t2)p2+2t(1-t)p3+t2p4,0≤t≤1
ꢀꢀꢀꢀ
(14)
[0113]
其中，bz1，bz2分别对应第一、二段二次贝塞尔曲线，p0第一段二次贝塞尔曲线初始点，p4第二段二次贝塞尔曲线终止点，p1，p3分别为两条贝塞尔曲线的控制点，p2为两段曲线的分段点。
[0114]
具体的，初始状态的位置约束可以简单地通过设置第1个控制点p0来满足，p0即车载激光雷达在全局坐标系中的坐标点。g1为当前无人车航向单位向量，在进行路径规划时，一般要求贝塞尔曲线起始点的切线方向与无人车起点航向相同，终止点的切线方向与终点航向相同。设p0与p1距离为l，可以得到第1个控制点的坐标为：
[0115]
p1＝[p
01
+g
11
l,p
02
+g
12
l]
t
ꢀꢀꢀ
(15)
[0116]
上述步骤2中已求得目标点p4及其方向g4，根据障碍物的与车辆相对关系给出的未来运行方向，并不一定满足车辆行进过程中的所有约束。因此，为提高轨迹规划成功率，引入两参数α，β，对g4进行微调，具体方法如下：
[0117][0118]
p3求解方法如下：
[0119][0120]
通过调节α，β可以实现调节向量p4～p3的方向与长度，增加了终点方向的自由度，提高了路径规划的成功率，增强了规划算法的鲁棒性。
[0121]
为保证分段二次贝塞尔曲线的平滑性，p2在p1、p3连线上取点，即：
[0122][0123]
其中，g1＝[g
11
,g
12
]
t
为当前无人车航向单位向量，g
11
、g
12
分别为g1横坐标、纵坐标，p
01
、p
02
分别为p0的横坐标、纵坐标；p
11
、p
12
分别为p1的横坐标、纵坐标；p
31
、p
32
分别为p3的横
坐标、纵坐标；p
41
、p
42
分别为p4的横坐标、纵坐标；为
[0124]
基于分段贝塞尔曲线描述的局部避障路径连续且光滑，已知，
[0125]
bz1(t)＝(1-t)2p0+2t(1-t)p1+t2p2,0≤t≤1
[0126]
bz2(t)＝(1-t)2p2+2t(1-t)p3+t2p4,0≤t≤1
[0127]
因此，
[0128][0129]
因为，
[0130]
bz'1(t)＝2(t-1)2p0+2(1-2t)p1+2tp2,0≤t≤1
[0131]
bz'1(t)＝2(t-1)2p2+2(1-2t)p3+2tp4,0≤t≤1
[0132]
所以，
[0133][0134]
综上所示，上述方法构造分段二次贝塞尔曲线连续且平滑。
[0135]
在本实施例步骤4中，构建了路径最短与目标方向变化量最小的目标函数并给出了目标点方向可变裕量约束和曲率约束，采用序列二次规划(sqp)算法来求解最优路径参数。
[0136]
对于目标点方向可变裕量约束，为了提高路径规划的鲁棒性，引入α，β让原有目标方向可以有一定可变裕量，其约束如下式：
[0137][0138]
当时，
[0139][0140]
当时，
[0141][0142]
化简求得对目标方向可变裕量约束：
[0143][0144]
其中，α，β为微调参数，δ为可变裕量约束。
[0145]
需要说明的是，为避免除零，在实际的应用中，令g
21
＝g
21
+ε，ε为以极
小值。
[0146]
对于曲率有界约束，曲线上任一点的曲率公式为：
[0147][0148]
通过不等式缩放可以得到一个新的曲线曲率的上限值，化简缩放过程如下式：
[0149][0150]
由(24)式可知其最右侧公式分母为关于t的二次函数，其最小值可表示为：d(l,α,β)＞0，分子为关于待优化参数l,α,β的常数，记为f(l,α,β)，因此||k||的上限值为：
[0151][0152]
进而可以得到曲率上限的约束，即：
[0153][0154]
其中，k
max
为曲率上限，r
min
为最小转弯半径。
[0155]
为求解贝塞尔曲线中的未知参数，给出了目标函数，其不仅要求长度最短，而且要求路径目标方向变化最小，如公式(27)所示，并在相应的约束条件下，优化描述路径的曲线参数：
[0156][0157]
该优化目标是以参数l,α,β,δ为变量的函数，ω为权重参数。该目标函数的主要目的是鼓励将自动车辆的转向操作和驾驶风险降至最低。公式(28)给出了带约束的目标优化问题，即：
[0158]
min imize:j(l,α,β,δ)
[0159][0160]
l≥0
[0161]
α≥0
[0162]
β≥0
[0163][0164]
0≤δ≤1
ꢀꢀꢀ
(28)
[0165]
本实施例采用序列二次规划(sqp)算法解决最优路径参数求解问题，最终得到一条长度最短、目标方向变化最小，满足车辆运动学约束的最优避障路径。
[0166]
无人驾驶车辆行驶过程中实时检测外部环境，当检测到当前行驶的全局或局部避障路径前方不可通行时，则进行局部路径规划。本实施例针对路径规划算法提出一种路径监控策略，根据当前探测已知环境判断是否需要重新规划避障路径，该策略在保证避障过程安全性的同时，大大节省计算避障整体过程的时间成本。
[0167]
路径规划监控策略如图11所示，包括规划机制、安全检测机制。为保证局部路径的安全性，需保证局部路径始终在可通行区域内。由于每个控制周期所计算可通行区域以车辆为中心，随着车辆的行驶即使障碍物没有发生变化，最优可通行区域角度范围也会随之改变。因此，通过实时监测路径是否一直处于最优可通行区域，无法准确判定路径的安全性。综上，本实施例通过实时监测每一个时刻算得的局部路径目标点与已规划路径目标点的距离变化量δd
t
与角度变化量δθ
t
，设置角度变化阈值θ
t
和距离变化阈值d
t
，若距离变化量与角度变化量均大于对应阈值则重新规划路径，如下式所示：
[0168][0169][0170][0171]
p
ti
为每个控制周期生成的路径规划目标点，具体是否需要优化得出新的路径由上一次规划目标点与当前时刻目标点距离与角度变化程度决定。当变化量大于设置阈值时则代表可通行区域发生改变，当前局部路径已经不再合适，此时进行局部路径再次规划生成。
[0172]
设置相同的起始点，设置不同类型的障碍物场景，对比本实施例提出路径鲁棒规划算法与基于frenet坐标系的多项式规划算法和面向无人车运动规划问题的vfh算法的避障效果，分别对比了三种算法在10种场景的规划成功率，成本时间，路径长度，航向角稳定性。除此之外，本实施例还构造不规则结构化场景验证本实施例所提算法，最后，本实施例在校园中进行了实车实验验证。
[0173]
仿真实验所采用的电脑cpu为intel core i5-5200u,内存为8gb。算法在windows 10(64位)系统下matlab2018 b中进行。三种规划算法的在matlab进行运行，具体参数如下表1所示：
[0174]
表1轨迹规划算法参数设置
[0175][0176][0177]
1、规划成功率对比
[0178]
为验证本实施例所提算法在复杂环境中的避障效果，本实施例设置仿真实验验证。本实验设置起始点为(0,20)，目标点为(90,40)，设置不同障碍物共10种场景，如图12所示。
[0179]
分别用三种算法作避障规划，规划成功率如表2所示：
[0180]
表2轨迹规划算法成功率
[0181][0182]
如图13所示，取四个场景展示规划轨迹效果，本实施例所提方法为蓝色轨迹，基于frenet坐标系的无人驾驶轨迹规划算法为粉红色轨迹，面向无人车运动规划问题的vfh算法为红色轨迹。
[0183]
基于frenet坐标系的无人驾驶轨迹规划算法是以全局路径为参考生成轨迹，所生成轨迹受全局路径约束，在面对复杂环境时避障规划表现不佳10个场景中规划成功7次；面向无人车运动规划问题的vfh算法采用一种基于感知数据的“双阈值”策略，该策略一定程度缓解vfh阈值敏感问题。但是面对更为复杂的障碍物场景时，固定的阈值无法适应所有的避障场景，因此会有规划失败情况，在场景4中规划失败，成功规划9次；由于本发明所提方法在目标点处自由度的设置，且采用一种基于自适应阈值与车辆最窄通行通道约束的候选
可通行区域筛选方法，使得可通行方向的选择和归集的规划更加灵活，面对较为复杂的外部环境有着更高的规划成功率，10个场景都避障成功。
[0184]
2、时间成本对比
[0185]
在10种不同障碍物环境的场景中，本实施例所提路径规划算法、基于frenet的多项式曲线规划算法和改进的vfh算法，相同起止点的路径规划成本时间对比如图14所示。此处成本时间为，从起点到终点用于路径规划所花费的整体时间，单位为秒。如下图13所示，本实施例所提算法与另外两种算法对比时间成本最小。
[0186]
面向无人车vhf算法以一定步长在每一个控制周期内根据最优方向生成轨迹点，最终得到避障轨迹，因此需要在每一个控制周期都需要执行全部的规划流程。与之相同的是，基于frenet的多项式规划算法在每个控制周期都会重新规划避障轨迹。因此这两种算法的成本时间都比较长。本发明所提路径鲁邦规划算法提出一种轨迹监控决策，在判断到上一次检测轨迹不安全时才重新规划下一条轨迹，在整个避障周期内大部分时间都进行轨迹监控，因此本发明所提算法的成本时间较小。
[0187]
3、路径长度对比
[0188]
在起止点、障碍物环境相同的条件下统计了10个场景中本发明所提路径规划算法、基于frenet的多项式规划算法和改进的vfh算法的路径长度。基于frenet的多项式规划算法和改进的vfh算法获得的路径点都为控制周期车辆速度确定间隔的路径点。本发明仿真环境为低速匀速行驶，因此规划出路径为间隔固定的路径点。而本发明所提算法非实时规划，因此将生成分段二次贝塞为曲线进行匀速插值最终得到与另外两种算法相同路径点固定间隔的避障路径，固定控制周期为0.3s，车辆速度为10km/h。因此，可统一采用折线法计算路径长度进行对比。
[0189]
结果如下图15所示，本发明所提路径鲁棒规划算法路径长度与另外两种算法相比路径长度最短。由于本发明所提算法在轨迹生成时优化目标为轨迹最小，依据最小转弯半径设置曲率约束，因此在保证路径可行的同时，路径最短。
[0190]
4、路径航向角对比
[0191]
从起点到避障终点，避障路径规划结束，可以将避障路径按照一定时间刻度离散，求得每个路径点的航向角，以整条路径的航向角均方差反映整条避障路径的稳定性与平滑性。下图分别为三种算法在10个场景中避障规划的轨迹航向角的均方差，如图16所示，三种算法都规划成功的场景有7个，有4个场景本发明所提算法的轨迹航向角变化的均方差和幅值都为最小。因此本发明所提算法规划轨迹较为稳定。
[0192]
5、复杂边界场景仿真实验
[0193]
本发明模拟无人车在园区内的典型场景，仿真运行本发明所提路径鲁棒规划算法。避障过程如图17(a)-17(d)所示，绿色曲线为已知全局路径，红色为当前行驶场景的边缘。二者数据由构造的电子地图信息所得，蓝色曲线为无人车行驶轨迹，黑色表示障碍物。
[0194]
图17(a)为园区内道路边缘崎岖不平的场景，图17(b)为园区由宽敞环境进入狭窄的场景，图17(c)为园区内由狭窄环境进入宽敞的场景，图17(d)为园区内“s”型道路的场景，如图所示四种典型园区内环场景中，无人车均成功避过障碍物，到达终点。实验证明，本发明所提算法，可以适应园区内各种复杂的边界约束，有较高的鲁棒性。
[0195]
6、实车实验
[0196]
实车实验场景为校内广场，绕巨型花坛采集路径点生成全局路径进行实车试验。实车试验采用阿克曼转向平台jd-01，车载中海达gnss接收机用于车辆的定位与定向，sick16线激光雷达用障碍信息的检测。
[0197]
首先，在花坛周围采集路径点，运用三次多项式拟合得到全局路径，为图中绿色曲线。图中蓝色轨迹为无人车行驶轨迹，粉色曲线为无人车行驶过程中规划局部路径，无人车绕行花坛效果如图17，18所示。
[0198]
图18(a)中，无人车跟踪全局路径期间，无人车遇到避障场景。图18(b)中为在实车实验中，无人车跟踪全局路径期间遇到障碍物时的实时运行显示图，如图中所示，无人车感知到前方行人，成功规划出了局部路径，避开了障碍物。
[0199]
图19(a)中，无人车跟踪全局路径期间，无人车绕过障碍物回归到全局路径场景。图19(b)中为在实车实验中，无人车避过障碍回归到全局路径时的算法的实时运行显示图，如图中所示无人车成功规划出了归回到全局路径的局部路径。
[0200]
经实车测试，本发明所提算法可使无人车成功避开出现在全局路径上的障碍物。
[0201]
实施例二
[0202]
本实施例的目的是提供一种未知障碍物分布的避障路径鲁棒规划系统，包括：
[0203]
最优可通行区域确定模块：获取车辆所处的环境信息，结合车辆可安全通过的最小通行宽度和已知的全局路径采用vfh算法确定最优可通行区域；
[0204]
目标点确定模块：根据所确定的最优可通行区域，确定局部避障路径目标点；
[0205]
拟合曲线模块：从所确定的局部避障路径目标点与车辆的初始点之间确定控制点，通过引入未知参数拟合成分段贝塞尔曲线；
[0206]
最优路径确定模块：以行驶路径最短和局部避障路径目标点方向变化量最小建立目标函数，在局部避障路径目标点方向约束条件下，求解所述未知参数，得到最优避障路径。
[0207]
实施例三
[0208]
本实施例的目的是提供一种计算装置，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现上述方法的步骤。
[0209]
实施例四
[0210]
本实施例的目的是提供一种计算机可读存储介质。
[0211]
一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时执行上述方法的步骤。
[0212]
以上实施例二、三和四的装置中涉及的各步骤与方法实施例一相对应，具体实施方式可参见实施例一的相关说明部分。术语“计算机可读存储介质”应该理解为包括一个或多个指令集的单个介质或多个介质；还应当被理解为包括任何介质，所述任何介质能够存储、编码或承载用于由处理器执行的指令集并使处理器执行本发明中的任一方法。
[0213]
本领域技术人员应该明白，上述本发明的各模块或各步骤可以用通用的计算机装置来实现，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。本发明不限制于任何特定的硬件和软件的结合。
[0214]
上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

技术特征：
1.一种未知障碍物分布的避障路径鲁棒规划方法，其特征在于，包括：步骤1：获取车辆所处的环境信息，结合车辆可安全通过的最小通行宽度和已知的全局路径采用vfh算法确定最优可通行区域；步骤2：根据所确定的最优可通行区域，确定局部避障路径目标点；步骤3：从所确定的局部避障路径目标点与车辆的初始点之间确定控制点，通过引入未知参数拟合成分段贝塞尔曲线；步骤4：以行驶路径最短和局部避障路径目标点方向变化量最小建立目标函数，在局部避障路径目标点方向约束条件下，求解所述未知参数，得到最优避障路径。2.如权利要求1所述的一种未知障碍物分布的避障路径鲁棒规划方法，其特征在于，在所述步骤1中，具体包括：将车辆所处的环境以栅格形式进行划分，根据车载传感器所采集的信息实时更新栅格，将栅格划分为扇区，计算每个扇区的障碍物强度，确定可通行区域；以车辆可安全通过的最小通行宽度和车辆距离阈值集合中一点确定角度阈值；若可通行区域的角度范围差值大于所述角度阈值，则该可通行区域为候选可通行区域；根据候选可通行区域的角度范围确定候选可通行区域的候选方向；基于候选方向与最近的已知全局路径点切线方向夹角、候选方向与车辆当前运动方向的夹角、候选方向与车辆上一次运动方向的夹角，确定最优可行性方向；基于最优可行性方向和所述车辆距离阈值集合中一点通过代价函数确定最优通行方向和最优距离阈值；基于最优通行方向得到最优可通行性区域角度上下限。3.如权利要求1所述的一种未知障碍物分布的避障路径鲁棒规划方法，其特征在于，在所述步骤2中，具体为：以车辆所检测到的障碍物端点为原点，以车辆最小转弯半径为膨胀半径，确定障碍物所处范围；根据所确定的障碍物所处范围以及最优可通行区域，确定最优内可通行区域；根据最优内可通行区域与已知的全局路径有无交集，确定局部避障路径目标点。4.如权利要求3所述的一种未知障碍物分布的避障路径鲁棒规划方法，其特征在于，若最优内可通行区域与已知的全局路径没有交集，且最优内可通行区域左右两侧均有障碍物的情况下，将左侧障碍物的右端点、右侧障碍物的左端点分别到已知的全局路径距离最小的点作为局部避障路径目标点；或，若最优内可通行区域与已知的全局路径没有交集，且最优内可通行区域存在单侧有障碍物的情况下，将障碍物的端点、最优可行性方向上距离最优距离阈值范围最远的点分别到全局路径距离最小的点作为局部避障路径目标点。或，若最优内可通行区域与已知的全局路径有交集，将最优内可通行区域的边界与全局路径的交点作为局部避障路径目标点。5.如权利要求1所述的一种未知障碍物分布的避障路径鲁棒规划方法，其特征在于，在所述步骤3中，具体为：将车辆的当前初始位置作为第一段二次贝塞尔曲线初始点，将局部避障路径目标点作
为第二段二次贝塞尔曲线终止点；根据贝塞尔曲线起始点的切线方向与车辆起点航向相同，终止点的切线方向与车辆终点航向相同的原则，确定第一段二次贝塞尔曲线上的控制点p1；根据所述局部避障路径目标点及其方向，引入未知参数对局部避障路径目标点方向进行微调；根据微调后的局部避障路径目标点方向以及局部避障路径目标点确定第二段二次贝塞尔曲线控制点p3；根据控制点p1和控制点p3基于曲线的平滑性确定p1、p3连线上的分段点，进而得到分段贝塞尔曲线。6.如权利要求1所述的一种未知障碍物分布的避障路径鲁棒规划方法，其特征在于，在所述步骤4中，所述局部避障路径目标点方向约束条件包括目标点方向可变裕量约束和曲率有界约束，采用序列二次规划在所述目标点方向可变裕量约束和曲率有界约束下，求解含有未知参数的目标函数，得到最优避障路径。7.如权利要求1所述的一种未知障碍物分布的避障路径鲁棒规划方法，其特征在于，还包括：根据每一时刻得到的局部避障路径目标点与已规划路径目标点的距离变化量、角度变化量与所设定的距离变化阈值、角度变化阈值的大小，判断是否需要重新规划路径。8.一种未知障碍物分布的避障路径鲁棒规划系统，其特征在于，包括：最优可通行区域确定模块：获取车辆所处的环境信息，结合车辆可安全通过的最小通行宽度和已知的全局路径采用vfh算法确定最优可通行区域；目标点确定模块：根据所确定的最优可通行区域，确定局部避障路径目标点；拟合曲线模块：从所确定的局部避障路径目标点与车辆的初始点之间确定控制点，通过引入未知参数拟合成分段贝塞尔曲线；最优路径确定模块：以行驶路径最短和局部避障路径目标点方向变化量最小建立目标函数，在局部避障路径目标点方向约束条件下，求解所述未知参数，得到最优避障路径。9.一种计算机设备，其特征在于，包括：处理器、存储器和总线，所述存储器存储有所述处理器可执行的机器可读指令，当计算机设备运行时，所述处理器与所述存储器之间通过总线通信，所述机器可读指令被所述处理器执行时执行如权利要求1至7任一项所述的一种未知障碍物分布的避障路径鲁棒规划方法。10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器运行时执行如权利要求1至7任一项所述的一种未知障碍物分布的避障路径鲁棒规划方法。

技术总结
本发明提出了一种未知障碍物分布的避障路径鲁棒规划方法及系统，结合车辆可安全通过的最小通行宽度和已知的全局路径来改进VFH算法确定最优可通行区域，提高了最优可通行区域的安全性、有效性；通过局部避障路径目标点与车辆的初始点之间确定控制点，通过引入未知参数拟合成分段贝塞尔曲线描述的最优规划路径，在保证路径平滑的前提下，实现了路径的最短，且优化所需时间较小，降低避障能耗，满足了路径规划的实时性；通过建立行驶路径最短和局部避障路径目标点方向变化量最小建立目标函数以及局部避障路径目标点方向约束条件求解未知参数，从而得到长度最短、目标方向变化最小，满足车辆运动学约束的最优避障路径。满足车辆运动学约束的最优避障路径。满足车辆运动学约束的最优避障路径。


技术研发人员：王目树 潘为刚 董兴学 秦石铭 唐相猛 王书新 高珅琦 刘子阳 李正磊
受保护的技术使用者：山东交通学院
技术研发日：2023.04.07
技术公布日：2023/6/28