路口场景下基于Bezier和VFH的无人车轨迹规划方法与流程

本发明涉及智能交通系统技术领域，特别涉及无人驾驶车辆在路口这样的复杂交通场景下的实时轨迹规划方法。

背景技术：

随着汽车产业和计算机技术的飞速发展，无人驾驶车辆在机器人领域里取得了空前的进步。决策系统做为无人驾驶车辆的“大脑”，在感知系统认知周围环境后，需要做出安全可执行的决策，确定当前的有效行为及行为的目标状态，然后规划运动轨迹。路口是交通网络拓扑结构的节点，是交通事故的频发地段。所以，解决无人车在路口处的轨迹规划问题意义重大。经典的轨迹规划方法分为两大类，一类是直接有解析解，基于多项式、正弦曲线、回旋曲线、bezier曲线等方法确定参数直接生成轨迹的解析解；第二类方法是基于采样的方式生成由离散点组成的轨迹，有a*、rrt、人工势场法，vfh等多种方法解决不同环境中的轨迹规划问题。

bezier是解析法生成轨迹的典型方法。其优点是在无障碍物或者障碍物稀疏时，生成轨迹快且平滑性好。缺点在于障碍物密集时难于通过调整参数获得有效轨迹[1]。

a*算法是经典的运动规划算法，其优点是具有启发性，能够较快的得到全局最优轨迹，然而其搜索步长难以确定，在复杂并且范围较大的规划环境中效率低下[2]。

rrt算法是由美国uiuc教授s.m.lavalle提出的，该算法在扩展节点时考虑了运动系统的运动学微分方程约束，因此，生成的运动轨迹满足全局最优约束和系统自身微分约束，可见，rrt算法可以很好的解决高维度、动态环境、含有微分约束的运动规划问题，然而，其需要确定的参数数量较多，要达到比较好的规划效果，往往难度较大[3]。

人工势场法是轨迹规划算法中较成熟且高效的规划方法，它将环境信息转化为引力场和斥力场模型。人工势场法的一个缺陷就是当目标在障碍物的影响范围内时，整个势场中除目标点外还可能存在其它的局部极小点，机器人可能陷入局部极小点而不能达到目标点。此外，轨迹经过障碍物附近时由于合力方向变化产生抖动现象。

vfh算法将机器人的工作环境分解为具有二值信息的栅格单元，每个矩形栅格中有一个累积值，表示在此处存在障碍物的可信度，高的累积值表示存在障碍物的可信度高。栅格选得小，环境信息存储量大，决策速度慢；栅格选得大，环境分辨率下降，在障碍物环境中发现轨迹的能力减弱[4]。

vfh算法具有很强的避障能力，能够在复杂多障碍物环境中找到无碰撞的运动轨迹；在路口这样复杂的环境下适合使用栅格图进行环境建模，而vfh算法恰好是基于障碍物栅格表示的运动规划方法；vfh算法的规划空间是连续的，这方便将车辆的运动学约束加进来[5]。

然而，vfh算法是为移动机器人设计的，直接使用在无人驾驶车辆上往往达不到预期的目标，主要有以下三个问题：原始vfh算法是一种基于感知数据的实时运动规划方法，直接使用必然导致运动轨迹不平滑而出现“震荡”现象[6]；原始vfh算法活动区域中的部分运动状态点对无人驾驶车辆来讲是不可达的；vfh算法得到的是稀疏的位置点集，缺乏其它运动状态信息，不能引导车辆行驶[7]。

以下给出检索的相关文献：

[1]l.han,h.yashiro,h.t.n.nejad,q.h.do,ands.mita,“beziercurvebasedpathplanningforautonomousvehicleinurbanenvironment,”in2010ieeeintelligentvehiclessymposium(iv).ieee,2010,pp.1036–1042.

[2]huynn,dechterr,pearlj.probabilisticanalysisofthecomplexityofa*[j].artificialintelligence,1980.15(3):241～254.

[3]lavallesm,kuffnerjj.rapidly-exploringrandomtrees:progressandprospects[c]//4thinternationalworkshoponalgorithmicfoundationofrobotics.wellesley,usa:akpe-ters,2000:293-308.

[4]j.borensteinandy.koren,“thevectorfieldhistogram-fastobstacleavoidanceformobilerobots,”ieeetransactionsonroboticsandautomation,vol.7,no.3,pp.278–288,1991.

[5]i.ulrichandj.borenstein,“vfh+:reliableobstacleavoidanceforfastmobilerobots,”in1998ieeeinternationalconferenceonroboticsandautomation.ieee,1998,pp.1572–1577.

[6]i.ulrichandj.borenstein,“vfh*:localobstacleavoidancewithlook-aheadverification,”in2000ieeeinternationalconferenceonroboticsandautomation.ieee,2000,pp.2505–2511.

[7]d.jiea,m.xueming,andp.kaixiang,“ivfh*:real-timedynamicobstacleavoidanceformobilerobots,”in201011thinternationalconferenceoncontrolautomationroboticsandvision(icarcv).ieee,2010,pp.844–847.

技术实现要素：

本发明的目的在于提供一种路口场景下基于bezier曲线和vfh方法的无人车实时轨迹规划方法，解决上述现有理论与技术上存在的缺陷或不足。本发明用三阶bezier曲线做为无人驾驶车辆的行驶轨迹；同时对传统vfh算法做出改进，将vfh算法和bezier曲线相结合，克服vfh算法的缺陷；并且用分层的思想将两种运动规划方法同时用于路口的轨迹规划，划分优先级。

为达到用三阶bezier曲线做轨迹规划，本发明采用了以下技术方案:

1、路口场景下基于bezier曲线和vfh方法的无人车实时轨迹规划方法，包括以下步骤：

步骤一，从行为决策层获取当前行为方式及本次轨迹规划的起始点位姿p0(x0,y0,θ0)和目标点位姿p3(x3,y3,θ3)；

步骤二，采用三阶bezier曲线模型生成从起始点p0到目标点p3的轨迹簇a1采用三阶bezier曲线模型生成从起始点p0到目标点p3的轨迹簇a1，选择4个控制点的三阶bezier曲线进行规划，根据车辆端点处的最小转弯半径求出p0p1和p2p3的下限，同时我们依据线段|p0p3|的长度确定p0p1和p2p3的上限，在得到控制点p1和p2的范围后，在范围内等间隔离散的取多个不同的p1和p2，得到多组控制点，进而得到多条满足端点曲率约束的几何轨迹，称之为轨迹簇，用a1表示。

步骤三，在得到满足曲率约束的轨迹之后，进行碰撞检测，得到无碰轨迹簇a3。若无碰轨迹簇非空，则顺序进行步骤四。若无碰轨迹簇为空，进入步骤五；

步骤四，选择最优轨迹

在得到满足曲率约束并无碰的轨迹之后，我们需要进一步选择最优轨迹。以轨迹最平滑为标准选择最优轨迹，可抽象成

步骤五，对原始vfh算法中的活动区域进行改善

rmin表示车辆的最小转弯半径，s表示车辆行进过程中的搜索步长；

步骤六，利用障碍物信息建立栅格图

由于障碍物被抽象成一个box，收到的障碍物信息是box的四个角点坐标，因此可根据四个角点建立栅格图，以栅格图正中心为原点建立坐标系，将box的四个角点坐标映射到栅格图坐标系中；

步骤七，将扇形活动区域划分成多个扇区，并判断是否有障碍物占据

对于栅格图上每一个被障碍物占据的栅格，将它们抽象成一个质点，判断该栅格是否落入扇形区域中；若落入，判断是落入到哪一个扇区中，否则认为该障碍物目前不在搜索范围内，最终对每个扇区的被障碍物占据的栅格个数做出统计；

步骤八，与bezier曲线相结合选择最优轨迹点

如果有多个扇区都是可行的，那么将选择一个最优的扇区所对应的轨迹点，这里主要有两个约束条件来筛选出最优轨迹点。一是考虑车宽，二是考虑目标点位姿。

步骤九，离散点集做控制点生成b样条曲线

b样条的数学模型描述如下：

式中，pi,n(t)表示第i+1个n阶b样条曲线片段；n表示b样条曲线的阶数；t为参数，取值为[0,1]；pi+k为控制点；fk,n(t)为b样条基函数。

b样条的数学模型描述如下：

式中，pi,n(t)表示第i+1个n阶b样条曲线片段；n表示b样条曲线的阶数；t为参数，取值为[0,1]；pi+k为控制点；fk,n(t)为b样条基函数。

所述的采用三阶bezier曲线模型生成从起始点p0到目标点p3的轨迹簇a1，选择4个控制点的三阶bezier曲线进行规划，根据车辆端点处的最小转弯半径求出p0p1和p2p3的下限，同时我们依据线段|p0p3|的长度确定p0p1和p2p3的上限，在得到控制点p1和p2的范围后，在范围内等间隔离散的取多个不同的p1和p2，得到多组控制点，进而得到多条满足端点曲率约束的几何轨迹，称之为轨迹簇，用a1表示。

与bezier曲线相结合选择最优轨迹点，使其逼近目标点位姿并且轨迹足够平滑，即从a2筛选出最光滑的一条曲线作为参考轨迹，vfh算法搜索轨迹点的过程中，在满足车宽的情况下，总是寻找最贴近参考轨迹的点，即所有可行扇区中离参考轨迹距离最短的点，作为最优轨迹点。

本发明的创新点在于将解析方法(对应bezier曲线)和离散方法(对应vfh算法)结合起来，使无人车在避障的同时，又能够沿着光滑的路径以期望的位姿达到目标点，避免了无人车驶出路口后由于朝向角的偏差引起震荡。

附图说明

图1是本发明的流程示意图；

图2是三阶贝塞尔曲线的生成示意图；

图3是vfh算法中无人车的活动区域改进示意图；

图4是将障碍物信息映射到栅格图坐标系下的示意图；

图5是将活动区域分成多个扇区，并判断每个扇区是否被障碍物占据的示意图，其中，红色线对应的扇区表示被障碍物占据；

图6是基于贝塞尔曲线选择最优轨迹点的示意图，其中，粗线表示贝塞尔曲线，圆弧上距离该粗线最短的点是最优轨迹点。

具体实施方式

下面结合附图对本发明作进一步说明。

参见图1，基于分层思想的路口运动规划方法分为两部分，包括的各个步骤如下：

1)根据起始点和目标点位姿信息规划bezier曲线，具体步骤如下：

(1a)从感知处获取起始点和目标点的位姿信息x,y,θ(x坐标，y坐标，朝向角)；

(1b)寻找合适的，多种组合的控制点，生成多条bezier曲线；

bezier曲线的数学模型如下所示。

bezier曲线是一种特殊的多项式曲线，假设给定n+1个控制点pi(i＝0,1,....,n)，则n阶bezier曲线可以表示为：

其中bi,n(t)为n阶伯恩斯坦基底多项式，其数学定义为：

其中，n为bezier曲线的阶数；t是控制参数，取值范围为[0,1]；pi表示第i+1个控制点。参数t在[0,1]范围内连续变化，则得到n阶bezier曲线。依次连接控制点pi，得到的凸多边形称之为控制多边形，其中p0和pn分别为第一个和最后一个控制点，bezier曲线必然在控制多边形内部。

根据公式1‐1和1‐2，得到3阶bezier曲线的x,y坐标分别为：

x(t)＝x0(1-t)³+3x1t(1-t)²+3x2t²(1-t)+x3t³,t∈[0,1]

y(t)＝y0(1-t)³+3y1t(1-t)²+3y2t²(1-t)+y3t³,t∈[0,1]

整理成关于参数的三阶多项式有：

x(t)＝[(x3-x0)+3(x1-x2)]t³+3(x0-2x1+x2)t²+3(x1-x0)t+x0,t∈[0,1]

y(t)＝[(y3-y0)+3(y1-y2)]t³+3(y0-2y1+y2)t²+3(y1-y0)t+y0,t∈[0,1]

为了表示方便，将公式简化成如下形式：

x(t)＝a3t³+a2t²+a1t+a0,t∈[0,1](1-3)

y(t)＝b3t³+b2t²+b1t+b0,t∈[0,1](1-4)

式中，a3＝(x3-x0)+3(x1-x2)，a2＝3(x0-2x1+x2)，a1＝3(x1-x0)，a0＝x0，b3＝(y3-y0)+3(y1-y2)，b2＝3(y0-2y1+y2)，b1＝3(y1-y0)，b0＝y0。

三阶bezier曲线的斜率曲线为：

曲率曲线为：

根据图2，已知起始点位姿p0(x0,y0,θ0)和目标点位姿p3(x3,y3,θ3)，现确定另外两个控制点p1(x1,y1)和p2(x2,y2)的选取范围：由公式(1-6)可求得起始点p0处的曲率为

目标点p3处的曲率为

随着|p0p1|和|p2p3|的增大，轨迹端点处的曲率半径也会随之增大，所以车辆端点处的最小转弯半径对应着|p0p1|和|p2p3|的下限。即

其中，rmin表示车辆的最小转弯半径。同时，我们依据线段p0p3的长度确定|p0p1|和|p2p3|的上限，即|p0p1|≤|p0p3|及|p2p3|≤|p0p3|。因此我们可以确定控制点的选择有如下约束：

l1≤|p0p1|≤l3，l2≤|p2p3|≤l3

现对|p0p1|和|p2p3|的取值范围三等分，可以得到4个控制点，即p1(x1,y1)和p2(x2,y2)各有4种取值，用公式表示如下：

对于p1，有：

对于p2，有：

由于p1和p2各有4种取值，因此有4x4＝16种组合的控制点，对应的有16条bezier曲线。若将|p0p1|和|p2p3|的取值范围n等分，可以得到n+1个控制点，对应的有(n+1)²条bezier曲线。

得到每组控制点后，可以计算出参数方程(1-3)、(1-4)的系数a0,a1,a2,a3以及b0,b1,b2,b3，从而得到bezier曲线方程，即无人车辆的轨迹方程。

(1c)通过曲率约束对多条bezier曲线进行筛选；

得到候选轨迹簇之后，需要依据最大曲率约束对轨迹簇进行筛选，因为在某一条轨迹上存在最小转弯半径小于车辆的固有转弯半径(低速下是个定值)的点，那么该轨迹对车辆来讲是不可行的。由于每一条轨迹都是用位置点集表示的，可根据公式

计算出每一个点的曲率，从而得到每个点的曲率半径

然后判断曲线是否存在某点的曲率半径满足

r≤rmin

若满足，则该曲线是不可行的，舍弃该曲线；否则，保留该曲线。

(1d)通过碰撞检测对多条bezier曲线再一次进行筛选；

通过步骤(1c)筛选后的轨迹，考虑到车辆行进过程中需要避开障碍物，因此还需要对轨迹进一步做碰撞检测，选出无碰的轨迹。

(1e)若经过(1c)、(1d)步骤筛选后已无剩下bezier曲线，则认为bezier曲线规划路口可行轨迹失败，转用vfh与bezier曲线相结合的方法；否则，通过轨迹最平滑准则在剩余的曲线里筛选出最优的轨迹。

若步骤(1d)筛选后还有满足要求的曲线，那么我们需要选择一条最优的曲线做为车辆最终的轨迹；现我们以轨迹最平滑为准则，选出最优轨迹。

首先，根据公式

计算出每一个点的曲率κ，然后根据公式

可以得到曲率平方和最小的那条曲线，该曲线就是最平滑的曲线，同时也是满足曲率要求，无碰的可行轨迹。

2)若bezier曲线簇都不满足以上约束，导致不存在期望的轨迹，则用vfh算法与bezier曲线相结合的算法规划轨迹，具体步骤如下：

(2a)根据车辆运动学模型确定车辆的活动区域；

由图3可知，根据车辆的最小转弯半径rmin以及搜索步长s，可以得到活动区域在车体当前位姿下的极坐标表示：

p表示以车辆后轴中心为坐标原点，航向角方向为零度的极坐标系；

ρ表示活动区域的距离范围，并且有ρ∈[0,ρmax]，ρmax表示最大活动距离；

表示活动区域的角度范围，并且有：

则搜索到的轨迹点必然在弧线上。

(2b)从传感器获取障碍物信息建立栅格图；

由于vfh算法是用栅格图来表征环境的，因此获取障碍物信息后需要以栅格图的形式表现出来。

障碍物可以看成是一个box，因此从传感器处收到的是该box的四个角点坐标，现需要根据这4个坐标点将障碍物映射到栅格图坐标系中。

如图4所示，以车体的朝向为x轴正方向，车的后轴中心点为坐标系原点建立右手坐标系做为栅格图坐标系。假设该栅格图的尺寸是gridm*gridn，即有gridm行，gridn列；且分辨率是ratio，即每个栅格的长宽都是ratio；4个角点坐标分别是(x1,y1)、(x2,y2)、(x3,y3)、(x4,y4)。首先获得包围box的虚线框，步骤如下：令

xmin＝min{x1,x2,x3,x4}，xmax＝{x1,x2,x3,x4}

ymin＝min{y1,y2,y3,y4}，ymax＝max{y1,y2,y3,y4}

可以通过如下公式

计算出虚线框在栅格图上的位置。其中，rowmin表示虚线框所占据的行的最小值，rowmax表示所占据的行的最大值；colmin表示虚线框所占据的列的最小值，colmax表示所占据的列的最大值。[]符号表示向下取整，比如[1.5]＝1。

计算出rowmin、rowmax、colmin、colmax后，我们就知道了虚线框所包含的每一个栅格的位置。接下来要做的是判断虚线框里的每一个栅格是否落在box内，具体方法是判断每一个栅格的四个角点是否存在一个或多个点落入到box内；若存在，则认为该栅格是被障碍物占据的，相应的值用1表示；若不存在，则该栅格未被障碍物占据，相应的值用0表示。

现介绍如何判断一个点是否落入到凸四边形内。假设该凸四边形为abcd，且abcd为顺时针，待判断的点为m，则需要满足：

abxam>0,bcxbm>0,cdxcm>0,daxdm>0

即可证明点m在凸四边形内部。

计算出每个栅格对应的0‐1值，相当于建立起了栅格图。

(2c)将活动区域分成多个扇区，并判断每个扇区是否有障碍物占据；

假设活动区域的角度为ω，将活动区域分成k个扇区，如图5所示，则每个扇区的角度为w/k。

现判断每个被障碍物占据的栅格是否在某个扇区内，具体步骤为：将每个栅格抽象成质点，可以是每个栅格的中心点，假设该栅格处于第row行，第col列，则该栅格的坐标点(x,y)为：

从而可以计算出该栅格与车体的距离，若该距离小于搜索步长s，则用角度继续判断是否在扇区内：计算该栅格坐标点与车体坐标点连线所形成的与x轴的夹角，假设为θ，若则证明该栅格确实在活动区域内，然后通过下式

计算出具体位于哪个扇区(n表示第n个扇区，n＝0,1,2,.....,k)。最后对每个扇区的被障碍物占据的栅格个数做统计，若某个扇区的该数值大于0，则认为有障碍物占据，是不可行的；否则认为是可行的扇区。

(2d)考虑车宽对可行扇区进行筛选；

一般每个扇区对应的弧长只有0.42m左右，而车宽有2m，这意味着若车选择了该扇区，虽然该扇区没有障碍物，但邻近扇区有障碍物依旧有可能产生碰撞，因此选择扇区时应考虑车宽。根据计算可知，5个扇区对应的弧长大于2m，又由于车体的对称性，选择的可行扇区需满足其左边相邻的两个扇区与右边相邻的两个扇区也是可行的。

(2e)用bezier曲线做为参考曲线选择最优扇区；

经过(2d)步骤后，选出的满足上述条件的扇区可能有多个，因此我们选择用bezier曲线做为参考曲线选择最优扇区。首先根据起始点和目标点的位姿以及轨迹最平滑准则选出一条最平滑的bezier曲线做为参考曲线，如图6所示，具体过程可参照(1)中的步骤；由于每个扇区都对应一个轨迹点，对于每一个满足(2d)条件的扇区，都可以通过其对应的轨迹点算出该点距离bezier曲线的最短距离(由于bezier曲线是用点集表示，计算最短距离时可先计算轨迹点距离曲线上每一个点的距离，然后在这些距离里取最短距离)。假设bezier曲线由n个点表示，满足(2d)条件的扇区有m个，对于每一个扇区，可以计算出该扇区对于参考线的最短距离

di,min＝min{di,1,di,2,....,di,n},i＝1,2,...,m

其中，i表示第i个满足(2d)条件的扇区，di,n表示第i个扇区对参考线上第n个点的距离，di,min表示第i个扇区距离参考线的最短距离。最后，我们可以通过公式

k＝argmin(di,min),k＝1,2,3,...,m

计算出哪个扇区是距离参考线最短，并认为该扇区是最优扇区，该扇区对应的轨迹点是最优轨迹点。

(2f)离散位置点集做控制点生成b样条曲线；

由(2e)步骤得到的轨迹点属于位置点集，无朝向角、速度等信息，而且点比较稀疏，无法做为车辆的可行轨迹。这里依据位置点集生成b样条曲线的方式生成可行驶路径。根据上述的b样条数学模型，取n＝3，k＝3带入公式(2-1)和(2-2)中得到三阶b样条曲线的基函数为：

现将位置点集做为b样条曲线的控制点即可得到最终轨迹，该轨迹含有位置信息，航向角信息和曲率信息，可以做为无人驾驶车辆的可行路径。

我们分别验证了路口存在单障碍物以及多障碍物时该路径规划算法的有效性。实验结果显示，无人车在遇到障碍物时能够有效避障并保持安全距离，绕过障碍物后在bezier曲线的约束下能够按照期望的目标位姿回到目标点上，该算法实时有效。