CN110838244A - 一种适用于超声波垂直泊车位的车位航向调整方法 - Google Patents

Abstract

本发明涉及一种适用于超声波垂直泊车位的车位航向调整方法，包括获取目标车位的初始坐标以及车辆位姿信息，计算车辆尾部中点坐标和车辆左右两侧的探测距离最小值，根据车辆尾部中点坐标与目标车位的初始坐标确定车辆与目标车位的深度关系；分别针对车辆位于目标车位内外情况计算目标车位的确定坐标。本方法在车辆泊车过程中，可以根据其单侧或两侧的障碍物边缘情况，判断车辆位姿，同时对目标车位精确定位，减小车辆自定位时由于超声波特性所带来的车位检测误差，提高泊车入位的成功率，避免剐蹭、碰撞的风险，同时，也可提高用户体验等。

Description

一种适用于超声波垂直泊车位的车位航向调整方法

技术领域

本发明涉及自动泊车技术领域，特别是涉及一种适用于超声波垂直泊车位的车位航向调整方法。

背景技术

目前市面上主流的自动泊车产品，都是基于纯超声波而开发的。超声波以其低成本、适用性广的特点，而得以广泛的应用。然而，面对不用的场景环境，利用超声波传感器进行车位搜索时，由于超声波传感器底层特性原因所致，产生的目标车位误差不尽相同，同时又融入车辆自定位误差，导致目标车位偏、斜，从而难以得到准确的目标车位信息，最终无法有效地的完成泊车入位。

同时，还存在仅根据目标车位两侧或单侧超声波探测的障碍物距离信息，而忽略角度信息的问题，因此，在进行车位调整时难以达到理想效果，甚至可能导致泊车失败；而目标车位的歪斜大都由于车辆自定位的角度而产生，面对不同的工况，其产生的差异性明显。因此，在目标车位偏差可接受的范围内，角度误差大，同样可能产生泊车入位的不合理性，或失败的可能。

发明内容

本发明为克服上述现有技术所述的不足，提供一种适用于超声波垂直泊车位的车位航向调整方法。

为解决上述技术问题，本发明的技术方案如下：

一种适用于超声波垂直泊车位的车位航向调整方法，包括如下步骤：

获取目标车位的初始坐标以及车辆位姿信息，并记录当前时刻为初始时刻，开始计算；

计算车辆尾部中点坐标和车辆左右两侧的探测距离最小值，根据车辆尾部中点坐标与目标车位的初始坐标确定车辆与目标车位的深度关系；

当车辆位于目标车位外时，根据车辆左右两侧的探测距离最小值确定目标车位的确定坐标；

当车辆位于目标车位内时，超声波实时获取车辆左右两侧的探测距离，并根据获取的车辆左右两侧的探测距离确定目标车位新的坐标；同时对车辆左右两侧的障碍物边缘进行定位，以判断是否定位到障碍物；

若是，获取障碍物边缘所在直线，并根据障碍物边缘所在直线结合目标车位新的坐标计算目标车位的确定坐标，否则，目标车位新的坐标则为目标车位的确定坐标。

进一步的，作为优选技术方案，车辆与目标车位的深度关系的确定具体包括以下步骤：

以初始时刻车辆位姿信息作为坐标零点建立全局坐标系；

计算车辆尾部中点在全局坐标系下的坐标，同时获取车辆左右两侧的探测距离最小值；

判断车辆尾部中点在全局坐标系下的纵坐标与目标车位上边缘中点在全局坐标系下的纵坐标之间的差值是否小于第一预设阈值，若是，则判断车辆位于目标车位外，否则，车辆位于目标车位内。

进一步的，作为优选技术方案，当车辆位于目标车位外时，根据车辆左右两侧的探测距离最小值确定目标车位的确定坐标具体包括以下步骤：

判断当前时刻车辆左右两侧的探测距离最小值是否小于第二预设阈值，若是，执行下一步，否则，记录当前时刻，触发标志位，重新开始计算；

判断上一时刻的车辆左右两侧的探测距离最小值与当前时刻的车辆左右两侧的探测距离最小值之差是否在第三预设阈值范围内，若是，则将目标车位上边缘的初始横向坐标向车辆距离障碍物相近的反方向调整第四预设阈值，得到目标车位上边缘新的横向坐标，从而得到目标车位的确定坐标，同时记录当前时刻为初始时刻，当前时刻的车辆左右两侧的探测距离最小值为上一时刻的车辆左右两侧的探测距离最小值；否则，记录当前时刻，触发标志位，重新开始计算。

进一步的，作为优选技术方案，当车辆位于目标车位内时，在对车辆左右两侧的障碍物边缘进行定位之前，需先判断上一时刻的车辆左右两侧的探测距离最小值与当前时刻的车辆左右两侧的探测距离最小值之差是否在第三预设阈值范围内，若是，根据获取的车辆左右两侧的探测距离确定目标车位新的坐标，同时通过超声波对车辆左右两侧的障碍物边缘进行定位，否则，记录当前时刻，触发标志位，重新开始计算。

进一步的，作为优选技术方案，根据超声波获取的车辆左右两侧的探测距离确定目标车位新的坐标具体包括以下步骤：

根据获取的车辆左右两侧的探测距离与第五预设阈值之间的不同关系对目标车位上边缘的横向坐标进行不同调整，得到目标车位上边缘新的横向坐标，从而得到目标车位新的坐标。

进一步的，作为优选技术方案，根据获取的车辆左右两侧的探测距离与第五预设阈值之间的不同关系对目标车位上边缘的横向坐标进行不同调整具体包括：

当车辆左右两侧的探测距离之和小于第五预设阈值的两倍时，将目标车位居中处理得到目标车位上边缘新的横向坐标；

当车辆左右两侧的探测距离之和大于等于第五预设阈值的两倍时，分别将车辆左右两侧的探测距离与第五预设阈值进行比较，当车辆左侧的探测距离小于第五预设阈值，车辆右侧的探测距离大于等于第五预设阈值时，保持车辆左侧的探测距离为第五预设阈值，得到目标车位上边缘新的横向坐标；当车辆左侧的探测距离大于等于第五预设阈值，车辆右侧的探测距离小于第五预设阈值时，保持车辆左侧的探测距离为第五预设阈值，得到目标车位上边缘新的横向坐标；当车辆左右两侧的探测距离均大于第五预设阈值，则将目标车位的初始坐标的横向坐标定义为目标车位上边缘新的横向坐标。

进一步的，作为优选技术方案，若车辆左侧和/或右侧定位到障碍物，获取障碍物边缘所在直线具体包括以下步骤：

获取障碍物边缘的定位坐标并将得到的定位坐标点进行缓存以得到定位坐标点点集；

判断所缓存的定位坐标点点集的长度是否大于第六预设阈值，若是，执行下一步，否则，记录当前时刻，触发标志位，重新开始计算；

通过最小二乘法对缓存的定位坐标点点集进行拟合处理得到车辆左侧和/或右侧的障碍物边缘所在直线；

判断车辆左侧和/或右侧的障碍物边缘所在直线的倾斜角度与目标车位的障碍物所在侧的侧边缘所在直线的倾斜角度之差的绝对值是否小于第七预设阈值，若是，根据目标车位新的坐标和车辆左侧和/或右侧的障碍物边缘所在直线计算目标车位的确定坐标，否则，记录当前时刻，触发标志位，重新开始计算。

进一步的，作为优选技术方案，获取目标车位的确定坐标具体包括以下步骤：

根据目标车位新的坐标获取目标车位上边缘的中点；

确定获取的障碍物边缘所在直线的数量；

当获取的障碍物边缘所在直线的数量为一条时，即判断车辆左侧或右侧存在障碍物，根据该中点与车辆左侧或右侧的障碍物边缘所在直线的关系，计算目标车位的确定坐标；当获取的障碍物边缘所在直线的数量为二条时，即判断车辆左侧和右侧存在障碍物，根据该中点与车辆左侧和右侧的障碍物边缘所在直线的关系，计算目标车位的确定坐标。

进一步的，作为优选技术方案，当获取的障碍物边缘所在直线的数量为一条时，计算目标车位的确定坐标具体包括：

通过目标车位上边缘的中点做障碍物边缘所在直线的垂线，其交点为目标车位上边缘的第一坐标点；

根据获取的目标车位上边缘的第一坐标点与目标车位新的坐标所对应的上边缘的第一坐标点之间的偏移向量获取目标车位上边缘的第二坐标点以及目标车位下边缘的第一坐标点和第二坐标点，从而得到目标车位的确定坐标。

进一步的，作为优选技术方案，当获取的障碍物边缘所在直线的数量为两条时，计算目标车位的确定坐标具体包括：

根据车辆左侧和右侧的障碍物边缘所在直线的斜率计算车辆左侧和右侧的障碍物边缘所在直线的角平分线斜率；

通过目标车位上边缘的中点做与车辆左侧和右侧的障碍物边缘所在直线的第一交点和第二交点，计算第一交点和第二交点之间的中点；

根据车辆左侧和右侧的障碍物边缘所在直线的角平分线斜率做通过第一交点和第二交点之间的中点的直线；

将通过第一交点和第二交点之间的中点的直线左右平移目标车位新的坐标所对应的上边缘的第一坐标点和第二坐标点之间的标准差，将其与车辆左侧和右侧的障碍物边缘所在直线的第三交点和第四交点确定为目标车位上边缘的第一坐标点和第二坐标点；

根据获取的目标车位上边缘的第一坐标点或第二坐标点与目标车位新的坐标所对应的上边缘的第一坐标点或第二坐标点之间的偏移向量获取目标车位下边缘的第一坐标点和第二坐标点，从而得到目标车位的确定坐标。

与现有技术相比，本发明技术方案的有益效果是：

本方法在车辆泊车过程中，可以根据其单侧或两侧的障碍物边缘情况，判断车辆位姿，同时对目标车位精确定位，减小车辆自定位时由于超声波特性所带来的车位检测误差，提高泊车入位的成功率，避免剐蹭、碰撞的风险，同时，也可提高用户体验等。

附图说明

图1为本发明方法步骤流程图。

图2为本发明车辆处于目标车位外示意图。

图3为本发明车辆处于目标车位内且车辆单侧存在障碍物示意图。

图4为本发明车辆处于目标车位内且车辆两侧存在障碍物示意图。

附图仅用于示例性说明，不能理解为对本专利的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的；相同或相似的标号对应相同或相似的部件；附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制。

具体实施方式

下面结合附图对本发明的较佳实施例进行详细阐述，以使本发明的优点和特征更易被本领域技术人员理解，从而对本发明的保护范围作出更为清楚的界定。

实施例1

一种适用于超声波垂直泊车位的车位航向调整方法，如图1所示，包括如下步骤：

S10. 获取目标车位的初始坐标以及车辆位姿信息，并记录当前时刻为初始时刻，开始计算。

本步骤具体为：

S101. 识别目标车位的坐标并对其进行初始化，如图2所示，具体为：

初始化后的目标车位的坐标记为【（Axt，Ayt），（Bxt，Byt），（Cxt，Cyt），（Dxt，Dyt）】），其中（Ax，Ay）、（Bx，By）、（Cx，Cy）和（Dx，Dy）为识别的目标车位坐标，t为识别目标车位的时刻，初始化t=0，那么， Axt=Ax，Ayt=Ay，Bxt=Bx，Byt=By，Cxt=Cx，Cyt=Cy，Dxt=Dx，Dyt=Dy，初始化后的目标车位的坐标为【（Ax，Ay），（Bx，By），（Cx，Cy），（Dx，Dy）】。

S102. 记录当前时刻，触发标志位，开始计算。

具体为：

记录当前时刻为k，并在下个周期执行k=k+1，同时，在k时刻，等待泊车系统的算法模块触发标志位，当接收到标志位后，则进入下一步，开始进行计算，否则，重复执行本步骤。

S20. 计算车辆尾部中点坐标和车辆左右两侧的探测距离最小值，根据车辆尾部中点坐标与目标车位的初始坐标确定车辆与目标车位的深度关系。

本步骤具体为：

S201. 获取车辆位姿信息，计算车辆尾部中点在全局坐标系下的坐标，同时获取车辆左右两侧的探测距离最小值。

本步骤具体为：

以初始时刻车辆位姿信息作为坐标零点建立全局坐标系，因此初始车辆位姿信息为（0，0，0），当前时刻，获取的车辆位姿信息为（x，y，yaw）。

由于全局坐标系是以初始时刻车辆尾部中点为坐标零点建立的，因此，车辆尾部中点坐标为M（x，y）；

获取车辆左右两侧的探测距离最小值；

即MIN=Min（RR，RL），其中，MIN表示车辆左右两侧的探测距离最小值，RR表示当前时刻车辆右前方的超声波探头所获取的车辆右侧的探测距离，RL表示当前时刻车辆左前方的超声波探头所获取的车辆左侧的探测距离。

S202. 根据车辆尾部中点坐标与目标车位的初始坐标确定车辆与目标车位的深度关系。

本步骤中具体为：

判断车辆尾部中点在全局坐标系下的纵坐标与目标车位上边缘中点在全局坐标系下的纵坐标之间的差值是否小于第一预设阈值，若是，则判断车辆位于目标车位外，否则，车辆位于目标车位内。

例如：车辆尾部中点坐标为M（x，y），其纵向坐标为Mx，目标车位上边缘的初始坐标为【（Ax， Ay），Bx， By）】，其中点纵向坐标为（Ax+ Bx）/2；

即判断Mx-（Ax+ Bx）/2＜TBD1，

当Mx-（Ax+ Bx）/2＜TBD1时，则判断车辆位于目标车位外，否则，车辆位于目标车位内。其中，TBD1的取值范围为0~2m。

S30. 当车辆位于目标车位外时，根据车辆左右两侧的探测距离最小值确定目标车位的确定坐标。

本步骤具体为：

S301. 判断当前时刻车辆左右两侧的探测距离最小值是否小于第二预设阈值，若是，执行下一步，否则，记录当前时刻，触发标志位，重新开始计算.

即判断MIN<TBD2，当MIN<TBD2时，执行步骤S302，否则，返回步骤S102。

其中，TBD2表示第二预设阈值，TBD2根据超声波探头的有效测距范围和路宽设置，其取值范围为0.3-0.7m。

S301. 判断上一时刻的车辆左右两侧的探测距离最小值与当前时刻的车辆左右两侧的探测距离最小值之差是否在第三预设阈值范围内，若是，则将目标车位上边缘的初始横向坐标向车辆距离障碍物相近的反方向调整第四预设阈值，得到目标车位上边缘新的横向坐标，从而得到目标车位的确定坐标，同时记录当前时刻为初始时刻，当前时刻的车辆左右两侧的探测距离最小值为上一时刻的车辆左右两侧的探测距离最小值；否则，返回步骤S102。

即判断| PreMIN – MIN|< TBD3；其中，PreMIN 表示上一时刻k-1的车辆左右两侧的探测距离最小值，MIN表示当前时刻k的车辆左右两侧的探测距离最小值，TBD3表示第三预设阈值，其取值范围为0-0.02m。

当| PreMIN – MIN|< TBD3时，则将目标车位上边缘的初始横向坐标向车辆距离障碍物相近的反方向调整第四预设阈值，得到目标车位上边缘新的横向坐标，从而得到目标车位的确定坐标。

由于车辆位于目标车位外，因此，此时只需调整目标车位上边缘的横向坐标。

例如：

Ayt = Ay±TBD4，Byt = By±TBD4，其中，Ayt 和Byt表示目标车位上边缘新的横向坐标，TBD4表示第四预设阈值，其取值范围为0.15-0.18m，其根据车辆左右两侧所需的安全停车距离设置。

从本步骤中得到的目标车位的确定坐标为【（Ax，Ayt），（Bx，Byt），（Cx，Cy），（Dx，Dy）】。

得到目标车位的确定坐标后，记录当前时刻k为初始时刻t，当前时刻k的车辆左右两侧的探测距离最小值MIN为上一时刻k-1的车辆左右两侧的探测距离最小值PreMIN；即t=k，PreMIN=MIN。

S40. 当车辆位于目标车位内时，超声波实时获取车辆左右两侧的探测距离，并根据获取的车辆左右两侧的探测距离确定目标车位新的坐标；同时对车辆左右两侧的障碍物边缘进行定位，以判断是否定位到障碍物。

本步骤具体为：

S401. 判断上一时刻的车辆左右两侧的探测距离最小值与当前时刻的车辆左右两侧的探测距离最小值之差是否在第三预设阈值范围内，若是，执行下一步，否则，返回步骤S102。

即判断| PreMIN – MIN|< TBD3；其中，PreMIN 表示上一时刻k-1的车辆左右两侧的探测距离最小值，MIN表示当前时刻k的车辆左右两侧的探测距离最小值，TBD3表示第三预设阈值，其取值范围为0-0.5m。

档的| PreMIN – MIN|< TBD3时，执行S402，否则，返回步骤S102。

S402. 根据获取的车辆左右两侧的探测距离确定目标车位新的坐标。

本步骤具体为：

根据获取的车辆左右两侧的探测距离与第五预设阈值之间的不同关系对目标车位上边缘的横向坐标进行不同调整，得到目标车位上边缘新的横向坐标，从而得到目标车位新的坐标。

即将RR和/或RL与TBD5进行比较，根据RR和/或RL与TBD5之间的不同关系对目标车位上边缘的横向坐标进行不同调整，具体包括：

S4021. 当车辆左右两侧的探测距离之和小于第五预设阈值的两倍时，将目标车位居中处理得到目标车位上边缘新的横向坐标。

具体为：

当RL+ RR < 2*TBD5时，得到目标车位上边缘新的横向坐标Ayt，Byt；

Ayt = Ay +（RL- RR）/2; Byt = By +（RL- RR）/2。

其中，RL表示当前时刻车辆左前方的超声波探头所获取的车辆左侧的探测距离，RR表示当前时刻车辆右前方的超声波探头所获取的车辆右侧的探测距离，TBD5表示第五预设阈值，其为居中触发阈值，其设置范围为0.3-07m。

S4022. 当车辆左右两侧的探测距离之和大于等于第五预设阈值的两倍时，分别将车辆左右两侧的探测距离与第五预设阈值进行比较，当车辆左侧的探测距离小于第五预设阈值，车辆右侧的探测距离大于等于第五预设阈值时，保持车辆左侧的探测距离为第五预设阈值，得到目标车位上边缘新的横向坐标；当车辆左侧的探测距离大于等于第五预设阈值，车辆右侧的探测距离小于第五预设阈值时，保持车辆左侧的探测距离为第五预设阈值，得到目标车位上边缘新的横向坐标；当车辆左右两侧的探测距离均大于第五预设阈值，则将目标车位的初始坐标的横向坐标定义为目标车位上边缘新的横向坐标。

具体为：

RL+ RR ≥ 2*TBD5时，

当RL < TBD5 && RR≥ TBD5时，得到目标车位上边缘新的横向坐标Ayt，Byt；

Ayt = Ay +（TBD5-RL）/2; Byt = By +（TBD5-RL）/2。

当RL≥TBD5 && RR < TBD5时，得到目标车位上边缘新的横向坐标Ayt，Byt；

Ayt = Ay +（TBD5- RR）/2; Byt = By +（TBD5- RR）/2。

当RL=TBD5 && RR = TBD5时，得到目标车位上边缘新的横向坐标Ayt，Byt；

Ayt = Ay；Byt = By。

其中，RL表示当前时刻车辆左前方的超声波探头所获取的车辆左侧的探测距离，RR表示当前时刻车辆右前方的超声波探头所获取的车辆右侧的探测距离，TBD5表示第五预设阈值，其为居中触发阈值，其设置范围为0.3-07m，其根据车辆左右两侧所需的安全停车距离设置。

由此步骤得到的目标车位上边缘新的横向坐标为Ayt ，Byt，此时得到的目标车位新的坐标为【（Ax，Ayt），（Bx，Byt），（Cx，Cy），（Dx，Dy）】。

S403. 对车辆左右两侧的障碍物边缘进行定位，以判断是否定位到障碍物。

本步骤具体为：

基于超声波探头的单发多收机制，通过设置在车辆左右两侧的超声波探头利用三角定位算法对车辆左右两侧的障碍物边缘进行定位，以判断是否定位到障碍物。

S50. 若定位到障碍物，获取障碍物边缘所在直线，并根据障碍物边缘所在直线结合目标车位新的坐标计算目标车位的确定坐标，否则，目标车位新的坐标则为目标车位的确定坐标。

本步骤具体为：

S501. 获取障碍物边缘所在直线，具体为包括：

S5011. 获取障碍物边缘的定位坐标并将得到的定位坐标点进行缓存以得到定位坐标点点集。

S5012. 判断所缓存的定位坐标点点集的长度是否大于第六预设阈值，若是，执行下一步，否则，返回步骤S102。

即判断所缓存的定位坐标点点集中的定位坐标点的数量是否大于第六预设阈值，其中第六预设阈值的设置范围为大于等于2，优选的，第六预设阈值的设置范围为5-30，在本实施例中，第六预设阈值设置范围为15-25。

S5013. 通过最小二乘法对缓存的定位坐标点点集进行拟合处理得到车辆左侧和/或右侧的障碍物边缘所在直线。

即：Ll=KL*x+b，Lr=KR*x+b；

其中，Ll表示车辆左侧的障碍物边缘所在直线，Lr表示车辆右侧的障碍物边缘所在直线，KL表示直线Ll的斜率，KR 表示直线Lr的斜率，b表示y轴截距。

S502. 根据障碍物边缘所在直线结合目标车位新的坐标计算目标车位的确定坐标，具体包括：

S5021. 判断车辆左侧和/或右侧的障碍物边缘所在直线的倾斜角度与目标车位的障碍物所在侧的侧边缘所在直线的倾斜角度之差的绝对值是否小于第七预设阈值，若是，执行下一步，否则，返回步骤S102。

其中，第七预设阈值设置为10°以下，优选的，第七预设阈值的设置范围为4°-8°，在本实施例中，第七预设阈值设置为6°.

S5022. 根据目标车位新的坐标获取目标车位上边缘的中点。

由于目标车位新的坐标为【（Ax，Ayt），（Bx，Byt），（Cx，Cy），（Dx，Dy）】，根据目标车位新的上边缘的横向坐标Ayt和Byt获取目标车位上边缘的中点M。

S5023. 确定获取的障碍物边缘所在直线的数量。

具体为：

当获取的障碍物边缘所在直线的数量为一条时，即判断车辆左侧或右侧存在障碍物，根据目标车位上边缘的中点与车辆左侧或右侧的障碍物边缘所在直线的关系，计算目标车位的确定坐标。

如图3所示，通过以下方式计算：

通过目标车位上边缘的中点做障碍物边缘所在直线的垂线，其交点为目标车位上边缘的第一坐标点。

例如：判断车辆右侧存在障碍物，那么车辆右侧的障碍物边缘所在直线为Lr，通过目标车位上边缘的中点M做Lr的垂线，并与Lr相交于点A′，定义点A′为目标车位上边缘的第一坐标点。

根据获取的目标车位上边缘的第一坐标点与目标车位新的坐标所对应的上边缘的第一坐标点之间的偏移向量获取目标车位上边缘的第二坐标点以及目标车位下边缘的第一坐标点和第二坐标点，从而得到目标车位的确定坐标。

例如：通过第一坐标点A′沿障碍物边缘所在直线Lr的法向，取得目标车位上边缘的第二坐标点B′，使目标车位上边缘的第二坐标点B′与第一坐标点A′之间的距离与目标车位上边缘AB长度相同；

沿障碍物边缘所在直线Lr取得目标车位下边缘的第一坐标点D′，使目标车位下边缘的第一坐标点D′与目标车位上边缘的第一坐标点A′之间的距离与目标车位侧边缘AD长度相同；通过目标车位上边缘的第二坐标点B′做目标车位上边缘的第一坐标点A′与第二坐标点B′所在直线的法向，取得目标车位下边缘的第二坐标点C′，使目标车位下边缘的第二坐标点C′与目标车位上边缘的第二坐标点B′之间的距离与目标车位侧边缘BC长度相同，从而得到目标车位的确定坐标【（Ax′，Ay′），（Bx′，By′），（Cx′，Cy′），（Dx′，Dy′）】。

概括为：计算A′点与A点之间的偏移向量，分别将B、C、D点加上该偏移向量得到B′、C′、D′点，从而得到目标车位的确定坐标【（Ax′，Ay′），（Bx′，By′），（Cx′，Cy′），（Dx′，Dy′）】。

当获取的障碍物边缘所在直线的数量为二条时，即判断车辆左侧和右侧存在障碍物，根据该中点与车辆左侧和右侧的障碍物边缘所在直线的关系，计算目标车位的确定坐标。

如图4所示，通过以下方式计算：

根据车辆左侧和右侧的障碍物边缘所在直线的斜率计算车辆左侧和右侧的障碍物边缘所在直线的角平分线斜率。

通过以下公式计算：

KM = tan((atand(KL) + atand(KR))/2)；

其中，KM表示直线Ll和直线Lr的角平分线斜率，KL表示直线Ll的斜率，KR 表示直线Lr的斜率。

通过目标车位上边缘的中点做与车辆左侧和右侧的障碍物边缘所在直线的第一交点和第二交点，计算第一交点和第二交点之间的中点。

具体为：做通过M点的直线，使其与Ll和Lr相交于ML和MR，然后计算ML和MR的中点Mm。

根据车辆左侧和右侧的障碍物边缘所在直线的角平分线斜率做通过第一交点和第二交点之间的中点的直线。

具体为：根据KM做通过中点Mm的直线L。

将通过第一交点和第二交点之间的中点的直线左右平移目标车位新的坐标所对应的上边缘的第一坐标点和第二坐标点之间的标准差，将其与车辆左侧和右侧的障碍物边缘所在直线的第三交点和第四交点确定为目标车位上边缘的第一坐标点和第二坐标点。

具体为：将直线L左右平移(sqrt((Ax - Bx)^2 + (Ay - By)^2))/2，分别与Ll和Lr相交于B′和A′，得到目标车位上边缘的第一坐标点A′和第二坐标点B′，沿直线L左右平移(sqrt((Ax - Bx)^2 + (Ay - By)^2))/2后的直线，取A′D′=AD，B′C′=BC，得到目标车位的确定坐标【（Ax′，Ay′），（Bx′，By′），（Cx′，Cy′），（Dx′，Dy′）】。

S503. 若没有定位到障碍物，目标车位的确定坐标为计算的目标车位新的坐标。

即【（Ax′，Ay′），（Bx′，By′），（Cx′，Cy′），（Dx′，Dy′）】为【（Ax，Ayt），Bx，Byt），（Cx，Cy），（Dx，Dy）】。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (10)

1.一种适用于超声波垂直泊车位的车位航向调整方法，其特征在于，包括如下步骤：

获取目标车位的初始坐标以及车辆位姿信息，并记录当前时刻为初始时刻，开始计算；

计算车辆尾部中点坐标和车辆左右两侧的探测距离最小值，根据车辆尾部中点坐标与目标车位的初始坐标确定车辆与目标车位的深度关系；

当车辆位于目标车位外时，根据车辆左右两侧的探测距离最小值确定目标车位的确定坐标；

当车辆位于目标车位内时，超声波实时获取车辆左右两侧的探测距离，并根据获取的车辆左右两侧的探测距离确定目标车位新的坐标；同时对车辆左右两侧的障碍物边缘进行定位，以判断是否定位到障碍物；

若是，获取障碍物边缘所在直线，并根据障碍物边缘所在直线结合目标车位新的坐标计算目标车位的确定坐标，否则，目标车位新的坐标则为目标车位的确定坐标。

2.根据权利要求1所述的适用于超声波垂直泊车位的车位航向调整方法，其特征在于，车辆与目标车位的深度关系的确定具体包括以下步骤：

以初始时刻车辆位姿信息作为坐标零点建立全局坐标系；

计算车辆尾部中点在全局坐标系下的坐标，同时获取车辆左右两侧的探测距离最小值；

判断车辆尾部中点在全局坐标系下的纵坐标与目标车位上边缘中点在全局坐标系下的纵坐标之间的差值是否小于第一预设阈值，若是，则判断车辆位于目标车位外，否则，车辆位于目标车位内。

3.根据权利要求1所述的适用于超声波垂直泊车位的车位航向调整方法，其特征在于，当车辆位于目标车位外时，根据车辆左右两侧的探测距离最小值确定目标车位的确定坐标具体包括以下步骤：

判断当前时刻车辆左右两侧的探测距离最小值是否小于第二预设阈值，若是，执行下一步，否则，记录当前时刻，触发标志位，重新开始计算；

判断上一时刻的车辆左右两侧的探测距离最小值与当前时刻的车辆左右两侧的探测距离最小值之差是否在第三预设阈值范围内，若是，则将目标车位上边缘的初始横向坐标向车辆距离障碍物相近的反方向调整第四预设阈值，得到目标车位上边缘新的横向坐标，从而得到目标车位的确定坐标，同时记录当前时刻为初始时刻，当前时刻的车辆左右两侧的探测距离最小值为上一时刻的车辆左右两侧的探测距离最小值；否则，记录当前时刻，触发标志位，重新开始计算。

4.根据权利要求1所述的适用于超声波垂直泊车位的车位航向调整方法，其特征在于，当车辆位于目标车位内时，在对车辆左右两侧的障碍物边缘进行定位之前，需先判断上一时刻的车辆左右两侧的探测距离最小值与当前时刻的车辆左右两侧的探测距离最小值之差是否在第三预设阈值范围内，若是，根据获取的车辆左右两侧的探测距离确定目标车位新的坐标，同时通过超声波对车辆左右两侧的障碍物边缘进行定位，否则，记录当前时刻，触发标志位，重新开始计算。

5.根据权利要求4所述的适用于超声波垂直泊车位的车位航向调整方法，其特征在于，根据超声波获取的车辆左右两侧的探测距离确定目标车位新的坐标具体包括以下步骤：

根据获取的车辆左右两侧的探测距离与第五预设阈值之间的不同关系对目标车位上边缘的横向坐标进行不同调整，得到目标车位上边缘新的横向坐标，从而得到目标车位新的坐标。

6.根据权利要求5所述的适用于超声波垂直泊车位的车位航向调整方法，其特征在于，根据获取的车辆左右两侧的探测距离与第五预设阈值之间的不同关系对目标车位上边缘的横向坐标进行不同调整具体包括：

当车辆左右两侧的探测距离之和小于第五预设阈值的两倍时，将目标车位居中处理得到目标车位上边缘新的横向坐标；

当车辆左右两侧的探测距离之和大于等于第五预设阈值的两倍时，分别将车辆左右两侧的探测距离与第五预设阈值进行比较，当车辆左侧的探测距离小于第五预设阈值，车辆右侧的探测距离大于等于第五预设阈值时，保持车辆左侧的探测距离为第五预设阈值，得到目标车位上边缘新的横向坐标；当车辆左侧的探测距离大于等于第五预设阈值，车辆右侧的探测距离小于第五预设阈值时，保持车辆左侧的探测距离为第五预设阈值，得到目标车位上边缘新的横向坐标；当车辆左右两侧的探测距离均大于第五预设阈值，则将目标车位的初始坐标的横向坐标定义为目标车位上边缘新的横向坐标。

7.根据权利要求1所述的适用于超声波垂直泊车位的车位航向调整方法，其特征在于，若车辆左侧和/或右侧定位到障碍物，获取障碍物边缘所在直线具体包括以下步骤：

获取障碍物边缘的定位坐标并将得到的定位坐标点进行缓存以得到定位坐标点点集；

判断所缓存的定位坐标点点集的长度是否大于第六预设阈值，若是，执行下一步，否则，记录当前时刻，触发标志位，重新开始计算；

通过最小二乘法对缓存的定位坐标点点集进行拟合处理得到车辆左侧和/或右侧的障碍物边缘所在直线；

判断车辆左侧和/或右侧的障碍物边缘所在直线的倾斜角度与目标车位的障碍物所在侧的侧边缘所在直线的倾斜角度之差的绝对值是否小于第七预设阈值，若是，根据目标车位新的坐标和车辆左侧和/或右侧的障碍物边缘所在直线计算目标车位的确定坐标，否则，记录当前时刻，触发标志位，重新开始计算。

8.根据权利要求7所述的适用于超声波垂直泊车位的车位航向调整方法，其特征在于，获取目标车位的确定坐标具体包括以下步骤：

根据目标车位新的坐标获取目标车位上边缘的中点；

确定获取的障碍物边缘所在直线的数量；

当获取的障碍物边缘所在直线的数量为一条时，即判断车辆左侧或右侧存在障碍物，根据该中点与车辆左侧或右侧的障碍物边缘所在直线的关系，计算目标车位的确定坐标；当获取的障碍物边缘所在直线的数量为二条时，即判断车辆左侧和右侧存在障碍物，根据该中点与车辆左侧和右侧的障碍物边缘所在直线的关系，计算目标车位的确定坐标。

9.根据权利要求8所述的适用于超声波垂直泊车位的车位航向调整方法，其特征在于，当获取的障碍物边缘所在直线的数量为一条时，计算目标车位的确定坐标具体包括：

通过目标车位上边缘的中点做障碍物边缘所在直线的垂线，其交点为目标车位上边缘的第一坐标点；

根据获取的目标车位上边缘的第一坐标点与目标车位新的坐标所对应的上边缘的第一坐标点之间的偏移向量获取目标车位上边缘的第二坐标点以及目标车位下边缘的第一坐标点和第二坐标点，从而得到目标车位的确定坐标。

10.根据权利要求8所述的适用于超声波垂直泊车位的车位航向调整方法，其特征在于，当获取的障碍物边缘所在直线的数量为两条时，计算目标车位的确定坐标具体包括：

根据车辆左侧和右侧的障碍物边缘所在直线的斜率计算车辆左侧和右侧的障碍物边缘所在直线的角平分线斜率；

通过目标车位上边缘的中点做与车辆左侧和右侧的障碍物边缘所在直线的第一交点和第二交点，计算第一交点和第二交点之间的中点；

根据车辆左侧和右侧的障碍物边缘所在直线的角平分线斜率做通过第一交点和第二交点之间的中点的直线；

将通过第一交点和第二交点之间的中点的直线左右平移目标车位新的坐标所对应的上边缘的第一坐标点和第二坐标点之间的标准差，将其与车辆左侧和右侧的障碍物边缘所在直线的第三交点和第四交点确定为目标车位上边缘的第一坐标点和第二坐标点；

根据获取的目标车位上边缘的第一坐标点或第二坐标点与目标车位新的坐标所对应的上边缘的第一坐标点或第二坐标点之间的偏移向量获取目标车位下边缘的第一坐标点和第二坐标点，从而得到目标车位的确定坐标。

Images