基于高精度地图的自主垂直泊车方法与流程

本发明涉及自动驾驶技术领域，尤其涉及一种基于高精度地图的自主垂直泊车方法。

背景技术：

城市中泊车问题主要分为道路边缘和停车场的泊车，自主垂直泊车轨迹规划是泊车系统的一个重要的技术组成部分，为基于高精度地图的智能驾驶技术提供了从车辆在泊车位附近的当前位置和姿态到泊车终点的位置和姿态提供了所需输入。

目前，垂直泊车轨迹主要考虑车辆最小转弯半径的基础上采用几何作图方式将泊车轨迹具体成数学模型表述，在考虑障碍物和车辆运动学约束条件下利用离线生成最优轨迹的方法过于单一，对于垂直泊车这种空间狭小环境复杂的自动驾驶情景无法达到实时规划，也无法适应更加复杂的泊车环境。通过神经网络和模糊控制器只通过模仿熟练驾驶员的驾驶行为作为参考轨迹的先验信息，这中轨迹方式因驾驶员不同而异，而且缺乏灵活性、不能动态调整垂直泊车路径。而随着智慧城市；智能交通；v2x的网联自动驾驶的物联网技术以及高精度地图地图引擎技术的不断发展与完善，采用基于高精度地图的自主垂直泊车，通过采样生成轨迹簇，构建代价函数，选择代价值最小的最优的垂直泊车轨迹，使的智能驾驶车辆在狭小的垂直泊车位环境中能够灵活导航。

技术实现要素：

有鉴于此，本发明提出了一种能够适应道路边缘和停车场的基于高精度地图的自主垂直泊车方法。

本发明的技术方案是这样实现的：本发明提供了一种高精度地图的自主垂直泊车方法，包括如下步骤：

步骤一、待泊车辆获取基于高精度地图的泊车位信息以及停车场和道路边缘路测传感器的环境信息，确定待泊车辆在高精度地图的位置属性，确定待泊车辆与所选垂直泊车位之间的在全局坐标系下的位置关系属性；

步骤二、通过平面直角泊车位坐标系确定待泊车辆的各个切换点；

步骤三、采用圆弧或者样条曲线对切换点进行拟合，拟合的样条曲线形成泊车轨迹；

步骤四、选取待泊车辆所在控制点为泊车起点，在满足垂直泊车路径约束和最小泊车位约束关系的条件下，通过采样点生成垂直泊车轨迹簇，对每条轨迹赋予速度信息，构建代价函数，计算垂直泊车轨迹簇中每条轨迹的代价值，选取代价值最小的一条轨迹作为最终的最优轨迹。

在以上技术方案的基础上，优选的，所述步骤一中，获取泊车位信息的方法包括，通过v2x技术注册道路边缘或者停车场的云端服务器，注册成功后，获取道路边缘或者停车场的云端服务器下发的基于高精度地图泊车位信息。

在以上技术方案的基础上，优选的，所述位置属性包括：车身控制点的id、坐标、头指向、曲率、速率、所在高精度地图车道与相邻车道的连通性、所在高精度地图车道方向和控制点的拓扑结构。

在以上技术方案的基础上，优选的，其特征在于，所述车身控制点为车辆后轴中心点，所述id包括车身控制点所在车道信息以及车身控制点位于所在车道上的具体编号信息，所述坐标为车身控制点的坐标，该坐标是相对全局坐标系的坐标，所述头指向为车身控制点处车辆的建议行驶方向与车身控制点所在车道所成的角度，所述曲率为经过该车身控制点的路径曲线在该车身控制点处的曲率，速率为车辆在该车身控制点处的建议行驶速度，所在高精度地图车道与相邻车道的连通性为该车身控制点所在车道与其左车道和右车道的连通性，所在高精度地图车道方向为该车身控制点所在位置处是否直行、左转、右转和可掉头，所在高精度地图控制点的拓扑结构表示该车身控制点与相邻车身控制点之间的连接关系。

更进一步优选的，所述位置关系属性包括：泊车终点a3坐标、泊车终点a3方向、泊车终点a3曲率和泊车位尺寸h×b，其中，h为泊车位的长度，b为泊车位的宽度。

在以上技术方案的基础上，优选的，所述全局坐标系是以泊车终点为原点，泊车位宽度方向为x轴、泊车位长度方向为y轴，所述切换点还包括泊车起点a0、转弯起点a1、转弯终点a2和泊车终点a3，待泊车辆从泊车起点a0做直线运动到转弯起点a1，从转弯起点a1在一定的转向角下做圆弧运动到转弯终点a2，最后从转弯终点a2再次做圆弧运动到达泊车终点a3，a1-a2段圆弧的圆心为o1。

在以上技术方案的基础上，优选的，所述切换点a0、a1、a2和a3的求解方法包括：

(rmin-b/2)²+d²＝(rmin-δ2-w/2)²，

所述a3[0，0，0°，0]，

所述a2[0，h-d-δ1-l，0°，1/rmin]，

所述a1[rmin，h-d-δ1-l+rmin，90°，1/rmin]，

所述a0[rmin+c，h-d-δ1-l+rmin，90°，0]，

所述o1(rmin，h-d-δ1-l)

其中，h为车库深度；b为车库宽度，c为圆心到a0的x轴方向距离，rmin为最小转弯半径，l为车长；w为车宽，l1为前后轴长度；l为后悬长；δ1为车尾部与车库底部的保护距离；δ2为车辆宽度方向左右两侧与车库宽度方向的距离；δ3为车辆泊车过程中与车道的最小距离。

在以上技术方案的基础上，优选的，所述步骤三中，拟合的方法包括，建立五次多项式进行垂直车位自动泊车系统的路径规划，其表达式如下：

y＝a0+a1(x-x0)+a2(x-x0)²+a3(x-x0)³+a4(x-x0)⁴+a5(x-x0)⁵，

求解其一阶导和二阶导：

y′＝a1+2a2(x-x0)+3a3(x-x0)²+4a4(x-x0)³+5a5(x-x0)⁴，

y″＝a1+2a2(x-x0)+3a3(x-x0)²+4a4(x-x0)³+5a5(x-x0)⁴，

已知两个控制点[x0，y0，heading0，k_s0]，[x1，y1，heading1，k_s1]，的边界条件为：

y′(x0)＝tan(heading0)

y′(x1)＝tan(heading1)

其中，第一个点的头指向是heading0以及曲率为k_s0；第二个点的头指向是heading1以及曲率为k_sl。

在以上技术方案的基础上，优选的，所述步骤四中，构建代价函数的方法包括，计算垂直泊车轨迹簇中每条备选轨迹的代价值，选取代价值最小的一条轨迹作为最终的最优轨迹，轨迹代价函数为：

c(i)＝wsafecsafe(i)+wsmocsmo(i)+wdyncdyn(i)+wkinckin(i)迹，其中i是备选轨迹的标号，csafe(i)、csmo(i)、cdyn(i)和ckin(i)分别为安全性代价函数、平滑性代价函数、动力学代价函数和运动学代价函数，wsafe、wsmo、wdyn和wkin分别为这四个代价函数所占的权重。

在以上技术方案的基础上，优选的，当轨迹贴合期望车道中心线时，安全性代价函数csafe(i)代价值为0，所述平滑性代价函数为其中csmo(i)代表第i条轨迹的平滑性代价函数，ki(s)是第i条轨迹上弧长为s位置的点的曲率，累加的上下限为路径的弧长s的起点和终点，所述动力学代价函数为其中，cdyn(i)代表第i条轨迹的动力学代价函数，是第i条轨迹上各点的纵向加速度，是第i条轨迹上各点的横向加速度，所述运动学代价函数为ckin(i)＝min(ki(s))。

本发明的基于高精度地图的自主垂直泊车方法相对于现有技术具有以下有益效果：

(1)本发明是基于云端服务器下发的基于高精度地图泊车位信息以及停车场以及道路边缘路测传感器的环境信息，垂直泊车位信息和环境信息通过云端服务器下发至待泊车辆，车辆的定位导航模块确定待泊车辆在道路边缘或者停车场高精度地图的位置属性，确定待泊车辆与所选垂直泊车位之间的在全局坐标系下的位置关系属性；

(2)本发明的自主垂直泊车轨迹可以看成是由连续的多段圆弧和曲线组成的轨迹，在平面直角泊车位坐标系下确定垂直泊车的各切换点，对切换点之间可以选择用圆弧或者样条曲线拟合，拟合方式选择恰当不仅能保证航向角的连续性而且还能确保曲率的连续与平滑，从而保证车辆运动控制的平顺性，可以消除车辆在泊车过程中方向盘的抖动，实现安全泊车；

(3)本发明根据车辆在待泊车位附近一定范围内任意泊车起点，在满足垂直泊车路径约束，最小泊车位约束关系条件下，通过采样点生成垂直泊车轨迹簇，对轨迹簇每条轨迹赋予速度信息，构建代价函数，计算垂直泊车轨迹簇中每条备选轨迹的代价值，选取代价值最小的一条轨迹作为最终的最优轨迹。确定垂直泊车入库最优泊车轨迹，控制模块控制车辆安全泊车。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明的基于高精度地图的自主垂直泊车方法的流程框图。

具体实施方式

下面将结合本发明实施方式，对本发明实施方式中的技术方案进行清楚、完整地描述，显然，所描述的实施方式仅仅是本发明一部分实施方式，而不是全部的实施方式。基于本发明中的实施方式，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施方式，都属于本发明保护的范围。

如图1所示，本发明的基于高精度地图的自主垂直泊车方法，其包括：

步骤一、待泊车辆获取基于高精度地图的泊车位信息以及停车场和道路边缘路测传感器的环境信息，确定待泊车辆在高精度地图的位置属性，确定待泊车辆与所选垂直泊车位之间的在全局坐标系下的位置关系属性；

步骤二、通过平面直角泊车位坐标系确定待泊车辆的各个切换点；

步骤三、采用圆弧或者样条曲线对切换点进行拟合，拟合的样条曲线形成泊车轨迹；

步骤四、选取待泊车辆所在控制点为泊车起点，在满足垂直泊车路径约束和最小泊车位约束关系的条件下，通过采样点生成垂直泊车轨迹簇，对每条轨迹赋予速度信息，构建代价函数，计算垂直泊车轨迹簇中每条轨迹的代价值，选取代价值最小的一条轨迹作为最终的最优轨迹。

在具体实施方式中，所述步骤一中，获取泊车位信息的方法包括，通过v2x技术注册道路边缘或者停车场的云端服务器，注册成功后，获取道路边缘或者停车场的云端服务器下发的基于高精度地图泊车位信息。

以上实施方式中，云端服务器通过v2x技术可以实时补充和更新道路边缘或者停车场的基于高精度地图停车位信息，已经有车辆占用的停车位不可选择，根据时间权重，已经选择的停车位后来者不可选择。

由于道路边缘或者停车场的云端服务器的网络覆盖面积极广，因此智能驾驶车辆需要道路边缘或者停车场停车时，启动自主泊车功能，智能驾驶车辆获取最近的云端服务器，规划一条离最近云端服务器对应的道路边缘或者停车场泊车位，当智能驾驶车辆到达道路边缘或者停车场附近，可以选择想要泊车的车位。

以上实施方式中，步骤一中所述的道路边缘或者停车场的云端服务器获取高精度地图，高精度地图包括泊车位信息、道路边缘或者停车场内道路信息；一方面智能驾驶车辆安装的传感器可以获取车辆周边的环境信息，以便车辆做出正确的，安全的决策，另一方面云端服务器会实时更新道路边缘或者停车场内的泊车位以及障碍物信息，特别是车辆行驶盲区的环境信息，以弥补智能驾驶车辆传感器的不足。

在具体实施方式中，所述位置属性包括：车身控制点的id、坐标、头指向、曲率、速率、所在高精度地图车道与相邻车道的连通性、所在高精度地图车道方向和车身控制点的拓扑结构。

在具体实施方式中，所述车身的控制点为车辆后轴中心点，所述id包括车身控制点所在车道信息以及车身控制点位于所在车道上的具体编号信息，所述坐标为车身控制点的坐标，该坐标是相对全局坐标系的坐标，所述头指向为车身控制点处车辆的建议行驶方向与车身控制点所在车道所成的角度，所述曲率为经过该车身控制点的路径曲线在该车身控制点处的曲率，速率为车辆在该车身控制点处的建议行驶速度，所在高精度地图车道与相邻车道的连通性为该控制点所在车道与其左车道和右车道的连通性，所在高精度地图车道方向为该车身控制点所在位置处是否直行、左转、右转和可掉头，所在高精度地图车身控制点的拓扑结构表示该车身控制点与相邻车身控制点之间的连接关系。

在具体实施方式中，所述位置关系属性包括：泊车终点a3坐标、泊车终点a3方向、泊车终点a3曲率和泊车位尺寸h×b，其中，h为泊车位的长度，b为泊车位的宽度。

在具体实施方式中，所述全局坐标系是以泊车终点为原点，泊车位宽度方向为x轴、泊车位长度方向为y轴，所述切换点还包括泊车起点a0、转弯起点a1、转弯终点a2和泊车终点a3，待泊车辆从泊车起点a0做直线运动到转弯起点a1，从转弯起点a1在一定的转向角下做圆弧运动到转弯终点a2，最后从转弯终点a2再次做圆弧运动到达泊车终点a3，a1-a2段圆弧的圆心为o1。

在具体实施方式中，所述切换点a0、a1、a2和a3的求解方法包括：

(rmin-b/2)²+d²＝(rmin-δ2-w/2)²，

所述a3[0，0，0°，0]，

所述a2[0，h-d-δ1-l，0°，1/rmin]，

所述a1[rmin，h-d-δ1-l+rmin，90°，1/rmin]，

所述a0[rmin+c，h-d-δ1-l+rmin，90°，0]，

所述o1(rmin，h-d-δ1-l)。

在具体实施方式中，所述步骤三中，拟合的方法包括，建立三次、四次、五次样条曲线或者三次、四次、五次b样条曲线拟合，本实施方式中建立五次多项式进行垂直车位自动泊车系统的路径规划，其表达式如下：

y＝a0+a1(x-x0)+a2(x-x0)²+a3(x-x0)³+a4(x-x0)⁴+a5(x-x0)⁵，

求解其一阶导和二阶导：

y′＝a1+2a2(x-x0)+3a3(x-x0)²+4a4(x-x0)³+5a5(x-x0)⁴，

y″＝a1+2a2(x-x0)+3a3(x-x0)²+4a4(x-x0)³+5a5(x-x0)⁴，

已知两个控制点[x0，y0，heading0，k_s0]、[x1，y1，heading1，k_s1]，的边界条件为：

y′(x0)＝tan(heading0)

y′(x1)＝tan(heading1)

其中，第一个点的头指向是heading0以及曲率为k_s0；第二个点的头指向是heading1以及曲率为k_s1。

在具体实施方式中，所述步骤四中，构建代价函数的方法包括，计算垂直泊车轨迹簇中每条备选轨迹的代价值，选取代价值最小的一条轨迹作为最终的最优轨迹，轨迹代价函数为：

c(i)＝wsafecsafe(i)+wsmocsmo(i)+wdyncdyn(i)+wkinckin(i)，

其中i是备选轨迹的标号，csafe(i)、csmo(i)、cdyn(i)和ckin(i)分别为安全性代价函数、平滑性代价函数、动力学代价函数和运动学代价函数，wsafe、wsmo、wdyn和wkin分别为这四个代价函数所占的权重。

在具体实施方式中，当轨迹贴合期望车道中心线时，安全性代价函数csafe(i)代价值为0，所述平滑性代价函数为

其中csmo(i)代表第i条轨迹的平滑性代价函数，ki(s)是第i条轨迹上弧长为s位置的点的曲率，累加的上下限为路径的弧长s的起点和终点，所述动力学代价函数为

其中，cdyn(i)代表第i条轨迹的动力学代价函数，是第i条轨迹上各点的纵向加速度，是第i条轨迹上各点的横向加速度，所述运动学代价函数为ckin(i)＝min(ki(s))。

在具体实施方式中，自动垂直泊车过程是：

首先，将车辆驶入待停车区域，通常在路边或停车场，驾驶员换开启自动泊车系统，自动泊车系统首先检测车辆周围的环境信息，直到搜索到空闲车位信息，并提示驾驶员有空闲车位可以停车，驾驶员选择空闲车位，泊车系统则计算最优轨迹；

其次，车辆按照最优轨迹进行泊车，首先到达合适的倒车点、开始倒车、倒车完毕并到达合适的拐弯点，开始拐弯，并使车身与泊车位长度方向相互平行，车身方向盘自动回正，开始倒车，最后进入泊车位，完成泊车。

以上实施方式中，定义泊车成功的标准是，在泊车的过程中，车辆不与两侧车辆发生碰撞；泊车结束时，车辆与两侧车辆的车身方向平行，车辆的对称面与车位的中心线重合，车辆尾部与车位底部的距离在0.2-0.3m。

在具体实施方式中，基于双圆弧的三段路径垂直泊车方法的核心是确定三段路径的a1、a2、a3三个切换点分别是车辆进行圆弧运动的起点、圆弧运动的终点和泊车终点，车辆由其他点运动至a1点，并在a1点开始做圆弧运动，直至运动到a2点，车辆在a2点的方向应与泊车位的长度方向一致，且a2点位于泊车位中心线上，进入a2点后车辆开始回正方向盘并倒车，直至运动到a3点并停车完成泊车。

以a3点作为原点，泊车位长度方向建立y轴，泊车位宽度方向建立x轴，形成全局直角坐标系，其中s为行车道宽度h为车库深度b为车库宽度；p1、p2、p3、p4为车库四个顶点；a0、a1、a2、a3是泊车的四个切换点；o1为圆弧圆心；c为圆心到a0的x轴方向距离；rmin为最小转弯半径；l为车长；w为车宽；l1为前后轴长度；l为后悬长；δ1为车尾部与车库底部的保护距离；δ2为车辆宽度方向左右两侧与车库宽度方向的距离；δ3为车辆泊车过程中与车道的最小距离，行车道宽度根据垂直泊车的泊车过程以及建立的几何模型可知：

(rmin-b/2)²+d²＝(rmin-δ2-w/2)²

在全局直角坐标系中求解切换点a0、a1、a2、a3；(x，y，heading，k_s)

a3[0，0，0°，0]

a2[0，h-d-δ1-l，0°，1/rmin]

a1[rmin，h-d-δ1-l+rmin，90°，1/rmin]

a0[rmin+c，h-d-δ1-l+rmin，90°，0]

其中圆弧段对应的圆心o1坐标为：

o1(rmin，h-d-δ1-l)

车辆在泊车过程中防撞的约束条件为：

以上实施方式中，为保证泊车过程轨迹平顺以及方向盘的平顺，需要确定泊车轨迹在a1和a2两点的坐标值零阶导连续、头指向一阶导连续以及曲率二阶导连续，在车辆驶入待泊车区域开始泊车时，设置任意已知切换点a4，a4的属性为(x4，y4，heading4，k_s4)，以选择的最优一条轨迹为例说明：

从待泊车区域任意切换点a4到泊车起始点a0的曲线采用五次多项式拟合；

从泊车起始点a0到第一个转折点a1的曲线采用五次多项式拟合；

从第一个转折点a1到第二个转折点a2的圆弧的圆形为o1，半径为最小转弯半径rmin的圆弧；

从第二个转折点a2到泊车终点a3的曲线采用五次多项式拟合。

具体实施例中，长安cs555，基本尺寸可知，车库纵向保护距离δ1＝0.3；车库横向保护距离δ2＝0.2；上边界保护距离δ3＝0.5；最小转弯半径rmin＝8；车库宽度b＝2.6；车库深h＝5.3；行车道宽度s＝5.5；车宽w＝2；车长l＝5；车后轮轴中心到车后端的距离l＝1.0；前轴中心与后轴的位置l1＝2.650；倒车之前需要往前行走一段直线距离以保证车辆倒车前的状态正确c＝5。

同理选择的最小转弯半径rmin＝8m，所以第三段的曲率为-1/8＝-0.125，第一段，第二段以及第四段曲率虽有一定的波动但是上下波动没有超过1/8，满足我们的曲率的连续要求。根据以往实际测试发现，自动驾驶时横向控制方向方向盘转角变化是与曲率有关的，曲率连续可以保证方向盘在转动过程中更加稳定和平滑。

以上所述仅为本发明的较佳实施方式而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。