CN109866762B - 基于高精度地图的自主平行泊车方法 - Google Patents

Abstract

本发明提出了一种基于高精度地图的自主平行泊车方法，包括：车辆获取来自服务器的基于高精度地图的泊车位信息，通过泊车位信息选择泊车位，车辆在泊车位附近获取基于高精度地图的全局坐标系以及位置关系属性，通过确定切换点，对切换点用圆弧或者五次样条曲线拟合，不仅能保证航向角的连续性，还能够确保曲率连续，通过最优路径进行自动泊车；能够确定平行泊车入库最优平行泊车轨迹。

Description

基于高精度地图的自主平行泊车方法

技术领域

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

背景技术

由于乘用车的大量普及，泊车位日趋紧张，再加之泊车位的划分上越来越狭窄，导致泊车无法快速准确入位或稍有不慎就可能发生碰撞事故，因此人工泊车导致的汽车事故的发生比例很高，而驾驶盲区使得驾驶员很难对后方以及前侧方的障碍车辆的位置和距离做出准确的估计，而且倒车时驾驶者同时要兼顾车辆前方以及两侧的情况，让人高度紧张过程更易产生不安全因素，即使是经验丰富的司机也认为泊车是件让人头疼的事。

城市中泊车问题主要分为道路边缘的平行泊车(也可以称为侧向泊车)和停车场的平行泊车，目前主流的自动泊车***大致思路是，待泊车辆沿道路缓慢行驶，依据车载传感器自动对车辆行驶过程中左右两侧的区域进行分析建模，搜索目标停车区；当找到目标停车区后，示意驾驶员停车，驾驶员停车后，确认泊车目标位置，提示驾驶员档位操作，并进行泊车运动控制；最终自主驾驶***泊车入位。而随着智慧城市、智能交通、V2X的网联自动驾驶的物联网技术以及高精度地图地图引擎技术的不断发展与完善，传统的自动泊车***将发生极大的变化。

发明内容

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

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

步骤一、待泊车辆获取基于高精度地图的泊车位信息，选取泊车位；

步骤二、待泊车辆到达选取的泊车位附近后，待泊车辆获取道路边缘或者停车场高精度地图的位置属性，确定待泊车辆与所选泊车位之间的在全局坐标系下的位置关系属性；

步骤三、在位置关系属性前提下，根据阿克曼转向机构的原理，在泊车位平面直角坐标系下确定平行泊车的多个切换点；

步骤四、确定车辆在泊车位附近一定范围内的泊车起点，在满足平行泊车路径约束、最小泊车位约束关系的前提下，确定平行泊车入库最优平行泊车轨迹，控制模块控制车辆泊车。

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

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

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

更进一步优选的，步骤二中，所述位置关系属性包括：泊车控制点rw′坐标(x₀，y₀)、泊车控制点rw′方向heading0、泊车控制点rw′曲率k_s0＝1/R_min和泊车位尺寸L_S*H_S，其中，L_S为泊车位的长度，H_S为泊车位的宽度，R_min为车辆的最小转弯半径。

在以上技术方案的基础上，优选的，所述全局坐标系是以泊车位控制点为原点，泊车位长度方向为x轴、泊车位宽度方向为y轴，所述切换点包括第一切换点f₁(x₃，y₃)和第二切换点f₂(x₄，y₄)，待泊车辆从泊车起点rw到第一切换点f₁的路径为圆弧，由第一切换点f₁到第二切换点f₂的路径为直线或样条曲线，由第二切换点f₂到泊车控制点rw′的路径为圆弧。

在以上技术方案的基础上，优选的，泊车起点rw到第一切换点f₁的路径圆弧的圆心为O₁，O₁坐标为(S，H-R_min)；第二切换点f₂到泊车控制点rw′的路径圆弧的圆心为O₂，O₂坐标为(0，R_min)；f₁和f₂的坐标分别为：

其中，rw的坐标为(S，H)，S为横向泊车距离，H为纵向泊车距离；

在以上技术方案的基础上，优选的，所述第一切换点f₁到第二切换点f₂的路径的五次多项式为：

y＝a₀+a₁(x-x₀)+a₂(x-x₀)²+a₃(x-x₀)³+a₄(x-x₀)⁴+a₅(x-x₀)⁵；

保证路径曲率连续性，对五次多项式求一阶导和二阶导：

y′＝a₁+2a₂(x-x₀)+3a₃(x-x₀)²+4a₄(x-x₀)³+5a₅(x-x₀)⁴；

y″＝2a₂+6a₃(x-x₀)+12a₄(x-x₀)²+20a₅(x-x₀)³；

所述泊车控制点[x₀，y₀，heading0，k_s0]和泊车起点rw到第一切换点f₁的路径圆弧的圆心为O₁[x₁，y₁，heading1，k_s1]的边界条件应与五次多项式的一阶导和二阶导相同，其中泊车控制点的头指向是heading0以及曲率为k_s0；O₁的头指向是heading1以及曲率为k_s1，两个控制点坐标以及四个边界条件，六个独立方程六个未知系数，可以确定出唯一的五次样条曲线。

在以上技术方案的基础上，优选的，所述平行泊车路径约束包括：

R_min为常数，H∈[3，5]

在以上技术方案的基础上，优选的，所述最小停车位约束包括：

其中，所述L_b为车辆后悬长度，所述L_f为车辆前悬长度，dd为车辆实际宽度。

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

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

(2)本发明的自主平行泊车轨迹可以看成是由连续的多段圆弧和曲线组成的轨迹，在平面直角泊车位坐标系下确定平行泊车的各个圆弧和曲线切换点，对切换点用圆弧或者五次样条曲线拟合，不仅能保证航向角的连续性而且还能确保曲率的连续；

(3)本发明根据车辆在待泊车位附近一定范围内任意泊车起点，在满足平行泊车路径约束，最小泊车位约束关系条件下，确定平行泊车入库最优平行泊车轨迹，控制模块控制车辆安全泊车。

附图说明

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

图1为本发明车辆泊车路径图；

图2为本发明的横向停车与纵向停车距离约束关系图；

图3为本发明的最小泊车位数学几何模型图；

图4为本发明的泊车位长度和宽度的约束关系图；

图5为本发明的最小转弯半径与泊车位宽度的约束关系图；

图6为本发明的两端圆弧及样条曲线组成的完整的倒车轨迹图。

具体实施方式

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

本发明的基于高精度地图的自主平行泊车方法，其包括：

平行泊车路径的最短路径优化：

根据泊车路径要求可知，路径需要由导数连续的曲线构成，简单说明了由以车辆最小转弯半径为半径的圆弧和直线组成的路径为泊车的最短路径。

如图1所示，若路径为圆和直线组成，根据车辆的运动学的限制可知，路径的连接应由初始起点和终点的两段圆弧及与两段圆弧相切的直线组成，车辆的倒车过车过程可以描述为：车辆先以某一固定转弯半径向右侧转弯倒车，到达f₁(x₃，y₃)后，将方向盘回正，沿直线倒车到f₂(x₄，y₄)点后，将方向盘左打到某一固定角度后，逐渐将车身回正，当车身回正至与道路平行，且车辆rw点到达理想停车点rw¹后，将方向盘回正即可完成此次倒车。

假设当车辆以最小转弯半径转弯时，车辆后轮中心rw的转弯半径为R_min，则可知初始转弯时的转弯圆的圆心O₁坐标(x₁，y₁)为(s，H-R_min)，最终点回转时转弯圆的圆心O₂坐标(x₂，y₂)为(0，R_min)，切线方程为y＝ax+b，即

ax-y+b＝0；

路径可以描述为

由点到直线距离公式：点(x，y)到直线Ax+By+C＝0距离为

则圆心O₁，圆心O₂到直线ax-y+b＝0距离为

(x₁，y₁)＝(S，H-R_min)，(x₂，y₂)＝(0，R_min)

因为圆心O₁在直线上方，圆心O₂在直线上方可得

可以解的切线参数a，b

由以上关系可得

θ＝arctan a

则交点f₁和f₂的坐标方程为

路径的连接应由初始和终点的两段圆弧及用五次样条曲线代替与两段圆弧相切的直线组成，数学表达式如下：

泊车路径约束分析

横向泊车距离S以及纵向泊车距离H约束关系

由参数a、b可知，

R_min为常数H∈[3，5]

如图2所示，取值最小转弯半径R_min＝8m，横向停车距离H＝3m到5m；纵向停车距离S与横向停车距离H取值关系为如图与y轴正方形的所围非闭合区域。

最小泊车位约束关系

如图3所示，假设车辆以固定转弯半径进入泊车位，当车辆前端远离横向泊车位出口的一端端点B到转弯半径的圆心距离为R_B，泊车位的长度为L_S，泊车位的宽度为H_S，dd为车辆实际宽度，车辆前悬的长度为L_f，车辆后悬的长度为L_b，车辆的轴距为l，存在关系：

根据2R_min＞H_S，可以得到

如图4所示，若取值L_S为7m到9m，R_min＝8m，可以得到泊车位的长度与泊车位宽度的关系。

如图5所示，若取值L_S为7.85m，R_min取值5m到10m，可以得到泊车位的长度与泊车位宽度的关系。

泊车路径仿真分析

长安CS555基本尺寸

长*宽*高(单位mm)	4500*1855*1690
		轴距(单位mm)	2650

纵向停车距离S＝10.5m；横向停车距离H＝3.6m；最小转弯半径R_min＝8m；可以计算出航向角heading0和heading1，曲率k_s0和k_s1；

从泊车点开始平行泊车过程包括：

泊车点(S，H)到第一个切换点(x₄，y₄)；

第一个切换点(x₄，y₄)到第二个切换点(x₃，y₃)；

第二个切换点(x₃，y₃)到泊车终点(0，0)；

选择的最小转弯半径R_min＝8m，所以假设第一段的曲率为正即为1/8＝0.125，则第三段曲率则为-1/8＝-0.125，第二段曲率虽有一定的波动但是上下波动没有超过1/8，是完全满足我们的曲率的连续要求的。

车库长度L_S为8.5m；车辆后悬的距离加上保护距离delta_S为1.2m；车库宽度为2.5m。

基于任意起点内的泊车路径仿真分析

泊车前车辆当前位置和姿态在区间x(-5，0)，y(4，6)以及头指向和曲率也可以任意给定值，本实施例中我们取头指向为0，曲率为0.0005。考虑到泊车稳定性以及可靠性，在泊车之前需要往前行走一段直线距离以保证车辆倒车前的状态稳定，这段距离为了保证曲率的连续性，我们也选用五次多项式曲线拟合泊车点(S，H)和起始泊车点(S+Extended_line，H)以代替直线。同理我们也用五次多项式曲线拟合车辆当前位置(x，y)和起始泊车点(S+Extended_line，H)。其中Extended_line选择值为长安cs555车长。

如图6所示，完整的平行泊车过程包括：

1、车辆当前位置(x，y)到起始泊车点(S+Extended_line，H)；

2、起始泊车点(S+Extended_line，H)到泊车点(S，H)；

3、泊车点(S，H)到第一个切换点(x₄，y₄)；

4、第一个切换点(x₄，y₄)到第二个切换点(x₃，y₃)；

5、第二个切换点(x₃，y₃)到泊车终点(0，0)；

6、如果考虑到车辆到达泊车终点(0，0)后还要在车库中进行微调，可以根据车辆相对与车库的相对位置向前或者向后运动以调整车辆在车库中居中的位置。

选择的最小转弯半径R_min＝8m，所以假设第三段的曲率为负即为-1/8＝-0.125，则第五段曲率则为1/8＝0.125，第一段，第二段以及第四段曲率虽有一定的波动但是上下波动没有超过1/8，完全满足曲率的连续要求。

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

Claims (3)

1.一种基于高精度地图的自主平行泊车方法，其特征在于，包括以下步骤：

步骤一、待泊车辆获取基于高精度地图的泊车位信息，选取泊车位；

步骤二、待泊车辆到达选取的泊车位附近后，待泊车辆获取道路边缘或者停车场高精度地图的位置属性，确定待泊车辆与所选泊车位之间的在全局坐标系下的位置关系属性，所述位置属性包括：车身控制点的ID、坐标、头指向、曲率、速率、所在高精度地图车道与相邻车道的连通性、所在高精度地图车道方向和控制点的拓扑结构，所述车身控制点为车辆后轴中心点，所述ID包括车身控制点所在车道信息以及车身控制点位于所在车道上的具体编号信息，所述坐标为车身控制点的坐标，该坐标是相对全局坐标系的坐标，所述头指向为车身控制点处车辆的建议行驶方向与车身控制点所在车道所成的角度，所述曲率为经过该车身控制点的路径曲线在该车身控制点处的曲率，速率为车辆在该车身控制点处的建议行驶速度，所在高精度地图车道与相邻车道的连通性为该车身控制点所在车道与其左车道和右车道的连通性，所在高精度地图车道方向为该车身控制点所在位置处是否直行、左转、右转和可掉头，所在高精度地图控制点的拓扑结构表示该控制点与相邻控制点之间的连接关系

步骤三、在位置关系属性前提下，根据阿克曼转向机构的原理，在泊车位平面直角坐标系下确定平行泊车的多个切换点；所述位置关系属性包括：泊车控制点rw'坐标(x₀,y₀)、泊车控制点rw'方向heading0、泊车控制点rw'曲率k_s0＝1/R_min和泊车位尺寸L_S*H_S，其中，L_S为泊车位的长度，H_S为泊车位的宽度，R_min为车辆的最小转弯半径，所述全局坐标系是以泊车位控制点为原点，泊车位长度方向为x轴、泊车位宽度方向为y轴，所述切换点包括第一切换点f₁(x₃,y₃)和第二切换点f₂(x₄,y₄)，待泊车辆从泊车起点rw到第一切换点f₁的路径为圆弧，由第一切换点f₁到第二切换点f₂的路径为直线或样条曲线，由第二切换点f₂到泊车控制点rw'的路径为圆弧，泊车起点rw到第一切换点f₁的路径圆弧的圆心为O₁，O₁坐标为(S,H-R_min)；第二切换点f₂到泊车控制点rw'的路径圆弧的圆心为O₂，O₂坐标为(0,R_min)；车辆先以某一固定转弯半径向右侧转弯倒车，到达f₁(x₃,y₃)后，将方向盘回正，沿直线倒车到f₂(x₄,y₄)点，切线方程为y＝ax+b，即ax-y+b＝0；f₁和f₂的坐标分别为：

其中，rw的坐标为(S,H)，S为横向泊车距离，H为纵向泊车距离；

所述第一切换点f₁到第二切换点f₂的路径的五次多项式为：

y＝a₀+a₁(x-x₀)+a₂(x-x₀)²+a₃(x-x₀)³+a₄(x-x₀)⁴+a₅(x-x₀)⁵；

保证路径曲率连续性，对五次多项式求一阶导和二阶导：

y'＝a₁+2a₂(x-x₀)+3a₃(x-x₀)²+4a₄(x-x₀)³+5a₅(x-x₀)⁴；

y”＝2a₂+6a₃(x-x₀)+12a₄(x-x₀)²+20a₅(x-x₀)³；

所述泊车控制点[x₀,y₀,heading0,k_s0]和泊车起点rw到第一切换点f₁的路径圆弧的圆心为O₁[x₁,y₁,heading1,k_s1]的边界条件应与五次多项式的一阶导和二阶导相同，其中泊车控制点的头指向是heading0以及曲率为k_s0；O₁的头指向是heading1以及曲率为k_s1，两个控制点坐标以及四个边界条件，六个独立方程六个未知系数，可以确定出唯一的五次样条曲线；

所述平行泊车路径约束包括：

R_min为常数，H∈[3，5]

步骤四、确定车辆在泊车位附近一定范围内的泊车起点，在满足平行泊车路径约束、最小泊车位约束关系的前提下，确定平行泊车入库最优平行泊车轨迹，控制模块控制车辆泊车。

2.如权利要求1所述的一种基于高精度地图的自主平行泊车方法，其特征在于，所述步骤一中，获取泊车位信息的方法包括，车辆通过V2X技术注册道路边缘或者停车场的云端服务器，注册成功后获取基于高精度地图的泊车位信息。

3.如权利要求1所述的一种基于高精度地图的自主平行泊车方法，其特征在于，所述最小泊 车位约束包括：

其中，所述L_b为车辆后悬长度，所述L_f为车辆前悬长度，dd为车辆实际宽度，车辆的轴距为l。

Images