一种基于虚拟约束的自动驾驶车辆路径规划方法与流程

本发明涉及自动驾驶技术领域，尤其涉及一种基于虚拟约束的自动驾驶车辆路径规划方法。

背景技术：

路径规划是自动驾驶车辆的核心技术之一，其作用是保证车辆安全的完成对全局路径的跟踪。在城市道路环境或高速公路环境中，自动驾驶车辆不仅要平顺稳定的在车辆所在车道内行驶，还需要结合环境感知信息以及智能决策指令进行换道、并线等较为复杂的动作。在车道保持的工况下，为了保证车辆精确的跟随车道的中心线及保持横向稳定性，需要将车辆约束在其所在车道内；在换道工况下，需要保证车辆能够精确的换入到目标车道内，避免出现骑线、压线行驶的情况。为了达到上述目的，自动驾驶车辆的路径规划需要依赖车辆与道路相对横向位置的精确定位。有研究通过差分gps和高精度地图相结合的方式来实现车辆与道路横向位置关系的精确定位，但是这种方式受限于差分gps信号的作用范围，以及高精度地图的测绘。

现阶段，能够为自动驾驶车辆提供道路边沿、车道线等丰富的环境感知信息的三维激光雷达以及相机仍然是自动驾驶车辆上最重要的传感器。有必要利用现有传感器，设计一种融合环境感知信息与一般gps定位信息的路径规划方法。

技术实现要素：

鉴于上述的分析，本发明旨在提供一种基于虚拟约束的自动驾驶车辆路径规划方法，用以解决现有技术受限于差分gps信号的作用范围以及高精度地图的问题。

本发明的目的主要是通过以下技术方案实现的：

一种基于虚拟约束的自动驾驶车辆路径规划方法，包括以下步骤：

步骤s1：获取车体坐标系下的车道中心线航向；

步骤s2：利用步骤s1中获取的车道中心线航向对车辆航向进行修正；

步骤s3：利用步骤s2修正后的车辆航向对原始路径中的路径点进行校正；

步骤s4：基于三维激光雷达栅格地图，获取自动驾驶车辆与道路的相对横向位置关系；

步骤s5：根据步骤s4获取的自动驾驶车辆与道路的相对横向位置关系生成虚拟约束，所述虚拟约束包括所在车道的边界约束和邻车道边界约束；

步骤s6：根据步骤s5生成的虚拟约束和自动驾驶系统的决策指令，生成期望路径。

所述步骤s1还包括以下步骤：

步骤s101：判断车道线检测结果是否有效，若有效，执行步骤s102；若无效，沿用上一周期车道线检测结果；

步骤s102：获取车体坐标系下的车道中心线航向。

所述步骤s102还包括以下步骤：

步骤s1021：计算车道中心线的位置

其中，车道线检测结果(x1,y1)、(x2,y2)、(x3,y3)与(x4,y4)为左右车道线在车体坐标系下的坐标点，(x1,y1)为车辆左前方近车点，(x2,y2)为左前方远车点，(x3,y3)为右前方近车点，(x4,y4)为右前方远车点；

(xc1,yc1)与(xc2,yc2)分别代表车道中心线的近车端与远车端；

步骤s1022：计算车道中心线的航向：

其中θlane代表车道中心线在车体坐标系下的航向。

所述步骤s2还包括以下步骤：

步骤s201：初始化卡尔曼滤波器；

步骤s202：根据车辆当前的转弯半径和速度预测本周期内的大地坐标系下车辆航向预测值：

其中，θveh-pred为大地坐标系下当前时刻车辆航向的预测值，θveh-correct-old为大地坐标系下上一周期车辆航向的修正值，v为车辆当前的速度，r为车辆当前的转弯半径，t为规划周期；

步骤s203：计算车体坐标系下原始路径航向与车道中心线航向的差值：

θerror＝θroad-θlane

其中，θerror为车体坐标系下原始路径航向与车道中心线航向的差值，θroad为车体坐标系下原始路径的航向，θlane为车道中心线航向；

步骤s204：计算大地坐标系下车辆的航向测量值：

θveh-measure＝θveh-gps+θerror，

其中θveh-measure为大地坐标系下车辆的航向测量值，θveh-gps为大地坐标系下车辆的gps航向值；

步骤s205：将步骤s202中θveh-pred与步骤s204中θveh-measure作为卡尔曼滤波器的输入得到车辆在大地坐标系下的航向修正值θveh-correct；

步骤s206：将θveh-correct赋值给θveh-correct-old并进入到下一周期。

所述步骤s3还包括以下步骤：

步骤s301：计算大地坐标系下的车辆航向修正值与车辆gps航向值的偏差：

δθ＝θveh-correct-θveh-gps；

其中，θveh-correct为车辆在大地坐标系下的航向修正值，θveh-gps为大地坐标系下车辆的gps航向值；

步骤s302：利用步骤s301中结果计算得到校正后的路径点：

其中，[xcorrectycorrectθcorrect]^t是校正后的路径在车体坐标系下的坐标，[xveh-roadyveh-roadθveh-road]^t是校正前原始路径在车体坐标系下的坐标。

所述步骤s4还包括以下步骤：

步骤s401：统计激光雷达栅格地图中，每一列被占据的栅格的个数ogni；

步骤s402：根据每一列被占据的栅格的个数ogni，计算每一列栅格的统计标志量ogsi：

其中i是栅格地图中的列号，ogsi表示栅格地图中第i列栅格的统计标志量，ot表示栅格地图的占据标志阈值；

步骤s403：根据道路模型宽度rw以及道路模型宽度偏差rwt生成卷积核：

其中kj为第j列栅格对应的卷积核，rw为道路模型宽度，rwt为道路模型宽度偏差，gs为被选中作为脉冲的栅格的列号，gs的取值范围为[0,rwt]；

步骤s404：将步骤s402中结果ogsi与步骤s403中结果kj进行卷积

其中，mw为栅格地图中栅格的列数，卷积过程中如果re＝1，那么对应栅格列的匹配值hit就加1；如果re＝-1，那么对应栅格列的未匹配值miss就加1；如果re＝0，那么对应栅格列的未知值uknow就加1；

步骤s405：根据步骤s404中结果得到某一列栅格为道路中心的概率gci：

其中gci代表第i列栅格为道路中心的概率；

步骤s406：找出gci的最大值，其对应的i就是道路中心所在的栅格的列号；

cr＝i

其中cr为道路中心所在的栅格列号；

步骤s407：计算左右路沿所在的栅格列号；

其中，lr和rr分别为左右路沿所在的栅格的列号；

步骤s408：计算车辆与道路的相对横向位置关系

其中dl和dr分别为车辆到左右路沿的距离。

所述步骤s401中，从栅格地图的正中间一列，即车辆所在的那一列，沿着与修正后的路径相垂直的方向开始向栅格地图的两边开始统计。

所述步骤s5还包括以下步骤：

步骤s501：计算车道总数cz；

步骤s502：定位车辆所在的车道c；

车辆所在的车道序号：

其中c表示车辆所在车道的左数序号，lw为车道宽度；

步骤s503：根据所在车道中心线位置(xc1,yc1)与(xc2,yc2)，计算车辆到所在车道中心线的距离dc；

步骤s504：根据车辆到所在车道中心线的距离dc和车道宽度lw分别计算车辆到左车道中心线的距离dcl和车辆到右车道中心线的距离dcr：

dcl＝lw-dc

dcr＝lw+dc

步骤s505：根据车辆距离各个车道中心线的距离生成虚拟约束，所述虚拟约束包括所在车道边界约束和邻车道边界约束。

当所在车道左右两边均有车道时，所述虚拟约束为左车道的左边约束vll、所在车道的左边约束vcl、所在车道的右边约束vcr及右车道的右边约束vrr；当只有左侧有车道时，所述虚伪约束为左车道的左边约束vll、所在车道的左边约束vcl及所在车道的右边约束vcr；当只有右侧有车道时，所述虚拟约束为所在车道的左边约束vcl、所在车道的右边约束vcr及右车道的右边约束vrr。

所述步骤s6还包括以下步骤：

步骤s601：判断决策指令是否换道，若是，则转入步骤s603；若否，则转入步骤s602；

步骤s602：将校正后的路径平移到所在车道的中心线处作为期望路径进行跟踪，转入步骤s604；

步骤s603：将校正后的路径平移到目标车道的中心线处作为期望路径进行跟踪，转入步骤s604；

步骤s604：根据期望路径进行路径跟踪。

本发明有益效果如下：

本发明提出的基于虚拟约束的自动驾驶车辆路径规划方法，融合gps、视觉与激光雷达信息完成车辆与道路相对横向位置的精确定位，而不需要高精度定位设备和高精度地图。在车道保持行驶时，能够有效保证车辆沿着所在车道的中心线行驶，保证了车辆自动行驶时的横向稳定性；在换道时，能够保证车辆准确的换到目标车道，避免自动驾驶过程中车辆骑线或压线行驶。本发明有助于提高自动驾驶车辆在结构化的、有清晰车道线的城市或高速公路环境下的车道保持与换道能力，对于自动驾驶车辆的安全、稳定、平顺行驶具有重要意义。

本发明的其他特征和优点将在随后的说明书中阐述，并且，部分的从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。

附图说明

附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。

图1为本发明中基于虚拟约束的自动驾驶车辆路径规划方法整体流程；

图2为本发明利用卡尔曼滤波原理对车辆的航向进行修正的流程图；

图3为本发明中基于激光雷达栅格地图获取车辆与道路相对横向位置关系的流程图；

图4为本发明中在激光雷达栅格地图上统计每一列被占据的栅格数目的示意图；

图5为本发明中在获取车辆与道路相对横向位置关系时卷积核的生成方式示意图；

图6为本发明中虚拟约束生成流程图；

图7为本发明中虚拟约束生成示意图；

图8为本发明中期望路径生成示意图。

附图标记：

401-车体坐标系、402-车辆、403-栅格地图正中间的一列栅格、404-校正后的路径、405-栅格地图的左右边界、406-统计方向；

501-道路模型宽度偏差rwt、502-被选中作为脉冲的栅格、503-道路模型宽度的边界、504-道路边界、507-生成的卷积核kj；

702-虚拟约束、703-车道线；

805-车道中心线、807-左换道时的期望路径、808-车道保持时的期望路径、809-右换道时的期望路径。

具体实施方式

下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理。

根据本发明的一个优选实施例，提供了一种基于虚拟约束的自动驾驶车辆路径规划方法，如图1所示，包括以下步骤：

步骤s1：根据车道线检测结果获取车体坐标系下的车道中心线航向；

具体地，所述步骤s1还包括以下子步骤：

步骤s101：判断车道线检测结果是否有效，若有效，执行步骤s102；若无效，沿用上一周期车道线检测结果；

在本实施例中，自动驾驶车辆在结构化的有清晰车道线和道路边沿的环境下进行车道保持和换道，自动驾驶车辆的视觉传感器可以检测出车道线信息，但是考虑到十字路口处没有车道线的情况，所以要对车道线的检测结果是否有效进行判断。

步骤s102：获取车体坐标系下的车道中心线航向；

车道线检测结果(x1,y1)、(x2,y2)、(x3,y3)与(x4,y4)为左右车道线在车体坐标系下的坐标点，其中，(x1,y1)为车辆左前方近车点，(x2,y2)为左前方远车点，(x3,y3)为右前方近车点，(x4,y4)为右前方远车点，根据这四个点，求出车体坐标系下车道中心线的位置以及航向：

步骤s1021：计算车道中心线的位置

其中(xc1,yc1)与(xc2,yc2)分别代表车道中心线的近车端与远车端；

步骤s1022：计算车道中心线的航向

其中θlane代表车道中心线在车体坐标系下的航向。

步骤s2：利用卡尔曼滤波器和步骤s1中获取的车道中心线航向对车辆航向进行修正；

由于从车载gps得到的车辆的航向具有时间上的滞后和空间上的偏差，所以直接将其用于把大地坐标系下的全局路径转换为车体坐标系下的局部路径时会导致较大的误差，从而使得车辆不能准确的跟踪实际路径；

车道中心线的航向具有相对稳定性，因此可以利用车道中心线的航向对车辆的航向进行修正，考虑到测量噪声的影响，修正后的车辆航向需要利用卡尔曼滤波器进行维护；

具体地，如图2所示，所述步骤s2包括如下步骤：

步骤s201：初始化卡尔曼滤波器；

步骤s202：根据车辆当前的转弯半径和速度预测本周期内的大地坐标系下车辆航向预测值θveh-pred；

其中θveh-pred为大地坐标系下当前时刻车辆航向的预测值，θveh-correct-old为大地坐标系下上一周期车辆航向的修正值，v为车辆当前的速度，r为车辆当前的转弯半径，t为规划周期。

步骤s203：计算车体坐标系下原始路径航向与车道中心线航向的差值：

路径航向和车道中心线航向都是车体坐标系下的航向，且车道中心线航向平行于路径的切线方向。由于车道中心线航向是根据车道线检测结果直接在车体坐标系下计算得到，而路径航向是以车辆的gps坐标为基点经坐标转换得到，因此，只要求得车体坐标系下的路径航向与车道中心线航向就相当于求出了车辆的gps航向与其真实航向的差值θerror：

θerror＝θroad-θlane

其中，θerror为车体坐标系下原始路径航向与车道中心线航向的差值，θroad为车体坐标系下原始路径的航向，θlane为车道中心线航向；

步骤s204：计算大地坐标系下车辆的航向测量值：

θveh-measure＝θveh-gps+θerror

其中θveh-measure为大地坐标系下车辆的航向测量值，θveh-gps为大地坐标系下车辆的gps航向值；

步骤s205：将步骤s202中θveh-pred与步骤s204中θveh-measure作为卡尔曼滤波器的输入得到车辆在大地坐标系下的航向修正值θveh-correct；

步骤s206：将θveh-correct赋值给θveh-correct-old并进入到下一周期。

步骤s3：利用步骤s2修正后的车辆航向对原始路径中的路径点进行校正；

所述原始路径为车体坐标下的路径，原始路径的起点为车体坐标系原点；

由于车辆航向的不准确，车体坐标系下的原始路径也是存在误差的，因此需要根据修正后的车辆航向，对原始路径进行校正，校正后路径的航向与车辆所在车道的航向一致。

具体地，包括以下步骤：

步骤s301：计算大地坐标系下的车辆航向修正值与车辆gps航向值的偏差：

δθ＝θveh-correct-θveh-gps

步骤s302：利用步骤s301中结果计算得到校正后的路径点：

其中[xcorrectycorrectθcorrec]t^t是校正后的路径在车体坐标系下的坐标，[xveh-roadyveh-roadθveh-road]^t是校正前原始路径在车体坐标系下的坐标。

步骤s4：基于三维激光雷达栅格地图，获取自动驾驶车辆与道路的相对横向位置关系；

所述三维激光雷达栅格地图为车体坐标系下的栅格地图，每个栅格对应一个表示该栅格是否被障碍物占据的值；

由于道路gps点坐标误差的存在，仅仅依靠车辆的gps定位、路点的gps定位以及道路模型的宽度，是无法确定车辆在道路上的横向定位的，因此也无法确定车辆在哪个车道上。本发明中提出了基于激光雷达栅格地图，利用统计学方法求得道路中心在栅格地图中的位置，然后在利用车辆与栅格地图的相对关系以及道路模型宽度，可以得到车辆相对于道路中心、道路左边沿、道路右边沿的位置。

具体地，如图3所示，基于三维激光雷达栅格地图，获取自动驾驶车辆与道路的相对横向位置关系的过程包括以下步骤：

步骤s401：统计激光雷达栅格地图中，每一列被占据的栅格的个数ogni；

优选地，如图4所示，从栅格地图的正中间一列(亦即车辆所在的那一列)沿着与修正后的路径相垂直的方向开始向栅格地图的两边开始统计。

步骤s402：根据每一列被占据的栅格的个数ogni，计算每一列栅格的统计标志量ogsi：

其中i是栅格地图中的列号，ogsi表示栅格地图中第i列栅格的统计标志量，ot表示栅格地图的占据标志阈值；

赋值统计结果标志量的原则为：如果第i列被占据的栅格个数大于ot，那么这一列栅格的统计结果标志量赋值为1；如果第i列被占据的栅格个数不大于ot，那么再考察第i-1列被占据的栅格的个数是否大于ot，如果是，那么第i列栅格的统计结果标志量赋值为0，如果否，那么第i列栅格的统计结果标志量赋值为-1。

步骤s403：根据道路模型宽度rw以及道路模型宽度偏差rwt生成卷积核：

其中kj为第j列栅格对应的卷积核，rw为道路模型宽度，rwt为道路模型宽度偏差，gs为被选中作为脉冲的栅格的列号，gs的取值范围为[0,rwt]；

如图5所示，生成卷积核的规则如下所述：选定在[0,rwt]范围内的某一列栅格(假设列号为gs)作为脉冲栅格，如果列号j小于gs，那么对应的kj赋值为0；如果列号j等于gs，那么对应的kj赋值为1；如果列号j大于gs且小于rw-gs，那么对应的kj赋值为-1；如果列号等于rw-gs，那么对应的kj赋值为1；如果列号j大于rw-gs且小于rw，那么对应的kj赋值为0。其中，gs要从0依次取到rwt。

步骤s404：将步骤s402中结果ogsi与步骤s403中结果kj进行卷积

其中，mw为栅格地图中栅格的列数，卷积过程中如果re＝1，那么对应栅格列的匹配值hit就加1；如果re＝-1，那么对应栅格列的未匹配值miss就加1；如果re＝0，那么对应栅格列的未知值uknow就加1。

步骤s405：根据步骤s404中结果得到某一列栅格为道路中心的概率gci：

其中gci代表第i列栅格为道路中心的概率；

由于ogsi和kj的取值范围都是{-1,0,1}，且j的卷积取值范围为[-rw/2,gs]，即kj在卷积过程中不取-1，卷积过程中如果卷积结果re＝1，那么对应栅格列的匹配值hit就加1；如果re＝-1，那么对应栅格列的未匹配值miss就加1；如果re＝0，那么对应栅格列的未知值uknow就加1。最后如果hit＝2，那么对应的第i列栅格为道路中心的概率赋值为0.9；如果hit＝1,miss＝1或者hit＝1,uknow＝1，那么对应的第i列栅格为道路中心的概率赋值为0.5；如果miss＝2或uknow＝2或miss＝1,uknow＝1，那么对应的第i列栅格为道路中心的概率赋值为0.3。

步骤s406：找出gci的最大值，其对应的i就是道路中心所在的栅格的列号；

cr＝i

其中cr为道路中心所在的栅格列号。

步骤s407：计算左右路沿所在的栅格列号；

其中，lr和rr分别为左右路沿所在的栅格的列号。

步骤s408：计算车辆与道路的相对横向位置关系

其中dl和dr分别为车辆到左右路沿的距离。

步骤s5：根据步骤s4获取的自动驾驶车辆与道路的相对横向位置关系生成虚拟约束，所述虚拟约束包括所在车道的边界约束和邻车道边界约束；

由于本发明中将车辆的行为分为了车道保持与换道两种，在车道保持过程中，仅需考虑车辆所在车道的边界约束、避撞等问题；在换道过程中，只考虑所在车道与目标车道内的障碍物情况即可。因此，根据车辆与道路的相对定位关系、车道宽度以及道路宽度，虚拟出车辆当前所在车道的边界约束以及邻车道边界约束，从而四条虚拟约束可以确定三条车道，因此能够在车道保持时保证车辆横向运动的稳定性，在换道时保证准确的换到目标车道的中心线位置，避免骑线或压线行驶；

具体地，如图6所示，虚拟出所在车道的边界约束和邻车道边界约束的过程包括以下步骤：

步骤s501：计算车道总数cz；

步骤s502：定位车辆所在的车道c；

车辆所在的车道序号：

其中c表示车辆所在车道的左数序号，lw为车道宽度。

步骤s503：根据所在车道中心线位置(xc1,yc1)与(xc2,yc2)，计算车辆到所在车道中心线的距离dc；

步骤s504：根据车辆到所在车道中心线的距离dc和车道宽度lw分别计算车辆到左车道中心线的距离dcl和车辆到右车道中心线的距离dcr：

dcl＝lw-dc

dcr＝lw+dc

步骤s505：根据车辆距离各个车道中心线的距离生成虚拟约束，所述虚拟约束包括所在车道边界约束和邻车道边界约束；

具体地，当所在车道左右两边均有车道时，如图7(a)所示，所述虚拟约束为左车道的左边约束vll、所在车道的左边约束vcl、所在车道的右边约束vcr及右车道的右边约束vrr；当只有左侧有车道时，如图7(b)所示，所述虚伪约束为左车道的左边约束vll、所在车道的左边约束vcl及所在车道的右边约束vcr；当只有右侧有车道时，如图7(b)所示，所述虚拟约束为所在车道的左边约束vcl、所在车道的右边约束vcr及右车道的右边约束vrr；

靠近道路边界的虚拟约束与道路边界之间有一定的安全距离。

步骤s6：根据步骤s5生成的虚拟约束和自动驾驶系统的决策指令，生成期望路径。

校正后的路径可以保证航向与车道线方向一致，但并不能保证该路径位于当前所在车道的中心位置，因此经校正后的路径首先需要平移到当前所在车道的中心线位置，得到在进行车道保持时的期望路径，而在换道时，则将期望路径平移到目标车道的中心线位置；

图8为期望路径生成示意图；

具体地，包括以下步骤：

步骤s601：判断决策指令是否换道，若是，则转入步骤s603；若否，则转入步骤s602；

步骤s602：将校正后的路径平移到所在车道的中心线处作为期望路径进行跟踪，转入步骤s604；

步骤s603：将校正后的路径平移到目标车道的中心线处作为期望路径进行跟踪，转入步骤s604；

步骤s604：根据期望路径进行路径跟踪。

综上所述，本发明实施例提供了一种基于虚拟约束的自动驾驶车辆路径规划方法，融合gps、视觉与激光雷达信息完成车辆与道路相对横向位置的精确定位，而不需要高精度定位设备和高精度地图。在车道保持行驶时，能够有效保证车辆沿着所在车道的中心线行驶，保证了车辆自动行驶时的横向稳定性；在换道时，能够保证车辆准确的换到目标车道，避免自动驾驶过程中车辆骑线或压线行驶。本发明有助于提高自动驾驶车辆在结构化的、有清晰车道线的城市或高速公路环境下的车道保持与换道能力，对于自动驾驶车辆的安全、稳定、平顺行驶具有重要意义。

本领域技术人员可以理解，实现上述实施例方法的全部或部分流程，可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于计算机可读存储介质中。其中，所述计算机可读存储介质为磁盘、光盘、只读存储记忆体或随机存储记忆体等。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。