阅读说明：本技术 自动驾驶路径规划方法、系统、计算机可读存储介质及车辆 (Automatic Pilot paths planning method, system, computer readable storage medium and vehicle ) 是由 盛进源 梁锋华 万凯林 张伟方 高享久 于 2019-08-02 设计创作，主要内容包括：本发明公开了一种自动驾驶路径规划方法、系统、计算机可读存储介质及车辆,获取车辆初始约束条件和车辆终止约束条件,其中,车辆初始约束条件包括车辆的初始位姿、初始点速度、加速度和车辆距离车位线的距离,所述车辆终止约束条件包括车辆终止横向位置和姿态；基于车辆初始约束条件和车辆终止约束条件进行从起始位姿到终止位姿的路径规划,整个过程分为两个阶段,其中,第一阶段是将路径规划为一条平滑的曲线,第二阶段是将路径规划为一条直线。本发明解决了车辆在通用画线场景的自动找车位的问题,能够控制车辆沿规划路径行驶搜索车位。(The invention discloses a kind of automatic Pilot paths planning method, system, computer readable storage medium and vehicles, it obtains vehicle initializing constraint and vehicle terminates constraint condition, wherein, vehicle initializing constraint includes the distance of the initial pose of vehicle, initial point speed, acceleration and vehicle distances parking stall line, and it includes that vehicle terminates lateral position and posture that the vehicle, which terminates constraint condition,；Constraint condition is terminated based on vehicle initializing constraint and vehicle to carry out from starting pose to the path planning for terminating pose, whole process is divided into two stages, wherein, the first stage be by path planning be a smooth curve, second stage be by path planning be straight line.The present invention solves the problems, such as that vehicle looks for parking stall automatically in general setting-out scene, can control vehicle along planning path and travel search parking stall.)

自动驾驶路径规划方法、系统、计算机可读存储介质及车辆

技术领域

本发明属于泊车技术领域，具体涉及一种自动驾驶路径规划方法、系统、计算机可读存储介质及车辆。

背景技术

近年来，在低速自动泊车领域，已经可以基于超声波、影像，甚至融合探测技术实现泊车过程的全自动泊车。但是，在开始泊车之前的找车位过程，仍然需要驾驶员自己手动完成。即需要驾驶员手动按下泊车开始按键，然后驾驶车辆前行搜索车位，找到车位后根据提示，刹停车辆，挂P挡拉手刹，接着，按照提示执行一系列操作后开启泊车功能，整个过程略显繁琐。或者是基于记忆路径的路径跟随找车位过程，需要用户提前驾驶车辆模拟找车位的过程，然后控制器内部记录下之前行驶的路径，当车辆下次驶入当前场景范围内时，可以开启记忆路径的自动找车位功能。目前还没有一种路径规划方法应用于解决车辆自主找车位的问题，而要想车辆可以自动前行寻找车位，则离不开路径规划的过程。

其中，人为驾驶车辆找车位具有很大的不确定因素，路径不可控。如驾驶员在找车位过程中的车辆姿态或者车辆边缘距离车位顶点坐标连线的距离控制的不合理，则可能造成无法识别车位，或者找到车位后的路径规划不理想，造成无法一次泊入车位或增加泊车的步骤。此外，人为驾驶车辆找车位的过程，操作过程繁琐，易给用户造成不好的体验感。而基于记忆路径的自动找车位过程，只能针对提前已经采集记录好找车位路径的情况，此技术在通用场景不具有可行性。

因此，有必要开发一种自动驾驶路径规划方法、系统、计算机可读存储介质及车辆。

发明内容

本发明的目的是提供一种自动驾驶路径规划方法、系统、计算机可读存储介质及车辆，以解决车辆在通用画线场景的自动找车位的问题，能控制车辆沿规划路径行驶搜索车位。

本发明所述的自动驾驶路径规划方法，包括以下步骤：

获取车辆初始约束条件和车辆终止约束条件，其中，车辆初始约束条件包括车辆的初始位姿、初始点速度、加速度和车辆距离车位线的距离，所述车辆终止约束条件包括车辆终止横向位置和姿态；

基于车辆初始约束条件和车辆终止约束条件进行从起始位姿到终止位姿的路径规划，整个过程分为两个阶段，其中，第一阶段是将路径规划为一条平滑的曲线，第二阶段是将路径规划为一条直线。

进一步，所述将路径规划为一条平滑的曲线，具体为：

判断车辆的初始角度是否小于等于第一预设角度，若是，则选择五次多项式路径方法进行路径规划，若否，则选择三次贝塞尔曲线方法进行路径规划；

设置曲线的纵向距离，通过五次多项式路径方法或三次贝塞尔曲线方法规划出轨迹曲线；

在轨迹曲线上间隔选取若干个采样点，计算各采样点的切线角度值是否均在第二预设角度内，若是，则此轨迹曲线为最优路径轨迹，若否，调整纵向距离，并重复以上步骤，直到规划出最优路径轨迹。

进一步，所述车辆终止横向位置的确定方法：

车辆在初始位姿下，当车辆后轴中心距车位线的距离在1.5米以内，则规划车辆在第二阶段沿1.5米的距离前行搜索车位；当车辆后轴中心距车位线的距离在1.5米-1.9米之间时，则规划车辆在第二阶段沿此距离前行搜索车位；当车辆后轴中心距车位线的距离在1米以外时，则规划车辆在第二阶段沿1米的距离前行搜索车位。

本发明所述的一种自动驾驶路径规划的系统，包括控制器，所述控制器被编程以便执行如本发明所述的自动驾驶路径规划方法的步骤。

本发明所述的一种计算机可读存储介质，其内存储有计算机程序，所述计算机程序被控制器调用执行如本发明所述的自动驾驶路径规划方法的步骤。

本发明所述的一种车辆，采用如本发明所述的自动驾驶路径规划的系统。

本发明具有以下优点：在车身初始水平时，选取五次多项式路径规划方法，能够保证车辆在第一阶段的起步以及和第二阶段的衔接阶段运动平缓，角度波动较小。当车身初始姿态与车位线有较大夹角时，则利用贝塞尔曲线规划路径，能够保证车辆姿态迅速回正的同时不会出现较大的超调量，同时解决了当车辆初始位置位于0.6m-1m区间内，且车身初始角度不为0，五次多项式法无法进行路径规定的缺点。

附图说明

图1为本发明中自动找车位场景示意图；

图2为本发明的流程图。

具体实施方式

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

如图1所示，本实施例中，一种自动驾驶路径规划方法，包括以下步骤：

获取车辆初始约束条件和车辆终止约束条件，其中，车辆初始约束条件包括车辆的初始位姿、初始点速度、加速度和车辆距离车位线的距离，所述车辆终止约束条件包括车辆终止横向位置和姿态；

基于车辆初始约束条件和车辆终止约束条件进行从起始位姿到终止位姿的路径规划，整个过程分为两个阶段，其中，第一阶段是将路径规划为一条平滑的曲线，第二阶段是将路径规划为一条直线。

(一)第一阶段的规划

本实施例中，所述将路径规划为一条平滑的曲线，具体为：

判断车辆的初始角度是否小于等于第一预设角度，若是，则选择五次多项式路径方法进行路径规划，若否，则选择三次贝塞尔曲线方法进行路径规划；设置曲线的纵向距离，通过五次多项式路径方法或三次贝塞尔曲线方法规划出轨迹曲线；在轨迹曲线上间隔选取若干个采样点，计算各采样点的切线角度值是否均在第二预设角度内，若是，则此轨迹曲线为最优路径轨迹，若否，调整纵向距离，并重复以上步骤，直到规划出最优路径轨迹。

本实施例中，预设角度为1°，判断车辆的初始角度是否小于等于1°，若是，则选择五次多项式路径方法进行路径规划，若否，则选择三次贝塞尔曲线方法进行路径规划。通过设置权重函数来实现，权重函数如下：

式中，y为最终规划的路径方程；t为权重值；y_poly为五次多项式方程；y_bes为贝塞尔路径方程；θ₀为初始的车身角度。

(1)采用五次多项式路径方法规划轨迹曲线的具体步骤如下：

采用五次多项式规划的路径方程及其关于x的一阶导数和二阶导数，如公式1-1所示。

式中，x表示车辆在参考路径上行驶方向的纵向位置坐标，y表示车辆在参考路径上的侧向位置坐标，a₀～a₅表示五次多项式的系数。

五次多项式系数的确认方法如下：

上式中，令x₀＝0，x_f＝d，y_f＝b，y₀,y'₀,y”₀,y'₁,y”₁为0，可以求得对应系数a₀～a₅如下：

另外，基于不同的纵向距离(本实施例中，一般选择6m，8m，10m)，可以规划出不同的路径方程。比如：设定的纵向距离为6m，通过五次多项式路径方法规划的轨迹曲线后，在轨迹曲线上间隔(比如：每5cm取一个采用点)选取若干个采样点，然后对轨迹曲线上各采样点的切线角做约束；具体为：要保证规划的每个采样点的切线角度值小于等于10°，若所有采样点的切线角度值均小于等于10°，则认为此轨迹曲线为最优路径轨迹，否则，调整纵向距离(比如：将纵向距离调整为8m)，再重复以上步骤，直到规划出最优路径轨迹。同时，为了保证车辆尽快跟踪到指定位置，规定纵向距离最大为10米。

通过此方法即将路径规划为一条平滑的曲线，实现了车辆从初始位姿到直线运动起点的运动。

(2)采用三次贝塞尔曲线方法规划轨迹曲线的具体步骤如下：

基于车辆初始约束条件(主要指初始点的位置坐标)和车辆终止约束条件(主要指终止点的位置坐标)，可以求出P₀点和P₃点的横向位置坐标，即P_0Y＝0，P_3Y＝b。

同样的，x表示车辆在参考路径上行驶方向的纵向位置坐标(坐标系为以车辆初始位置的后轴中心为原点，平行于车道线的参考坐标系)，令x∈[0～x_f]。

规划如公式1-4、1-5的方程，可以求得对应任意纵向距离x下的P₁和P₂点的横向位置坐标P_1Y、P_2Y。

其中，P₀、P₁、P₂、P₃为贝塞尔曲线上的四个控制点。

式中，Bes_t＝x/x_f,为纵向距离的归一化处理，Bes_t∈[0～1]。

P_1Y＝Bes_t*x_f*sin(Initial_Angle) (1-4)

P_2Y＝y_f-(Bes_t*x_f*sin(End_Angle)) (1-5)

上式中，Initial_Angle为初始点对应的姿态角，End_Angle为终止点处对应的姿态角。

因此，最终的三次横向贝塞尔曲线方程可表示为：

本方法是基于贝塞尔曲线的改良，通过简化纵向x为非时间参考量，可以获得横向位移相对于纵向距离的路径方程(即

y＝y(x)＝P_0Y*(1-Bes_t)³+3*(1-Bes_t)²*Bes_t*P_1Y+3*(1-Bes_t)*Bes_t²*P_2Y+Bes_t³*P_3Y)。

(二)第二阶段的规划

如图2所示，基于泊车轨迹路径规划的知识，车辆的后轴中心距离车位线约1.5米-1.9m范围内是最佳停车位置，此时的路径规划可以很好地保证车辆一次进入车位，且减少后续调整的次数。而车辆自动找车位的过程，我们基于以上理论规则，能够保证车辆运动在车辆后轴中心距离车位线约1.5米-1.9m的范围内前行搜索车位，让原本不可控的因素变为简单可控的问题。

本实施例中，所述车辆终止横向位置的确定方法：

车辆在初始位姿下，当车辆后轴中心距车位线的距离在1.5米以内，则规划车辆在第二阶段沿1.5米的距离前行搜索车位；当车辆后轴中心距车位线的距离在1.5米-1.9米之间时，则规划车辆在第二阶段沿此距离前行搜索车位；当车辆后轴中心距车位线的距离在1米以外时，则规划车辆在第二阶段沿1米的距离前行搜索车位。

本实施例中，第二阶段的路径方程为Y＝y_f，y_f为车辆终止横向位置。

车辆基于最优的轨迹曲线能够迅速跟踪上指定距离的直线，实现直线前行搜索车位功能。在路径规划算法的基础上，先基于dSPACE快速原型在实车上实现基本功能，然后再转化为C代码，在控制器里优化此功能。

对于车位空间较小的情况，驾驶员可以提前下车，然后通过手机操控车辆自动前行找车位并泊车，免去了车辆泊好后下车难的问题。

本实施例中，通过全景摄像头对车位线进行识别，得出车辆相对于车位线的实时位姿信息。基于轮脉冲和方向盘转角信息预估车辆后轴中心的位姿，对车辆进行定位估计。

本实施例中，一种自动驾驶路径规划的系统，包括控制器，所述控制器被编程以便执行如本实施例所述的自动驾驶路径规划方法的步骤。

本实施例中，一种计算机可读存储介质，其内存储有计算机程序，所述计算机程序被控制器调用执行如本实施例所述的自动驾驶路径规划方法的步骤。

本实施例中，一种车辆，采用如本实施例所述的自动驾驶路径规划的系统。