CN110444044B - 基于超声波传感器的车辆位姿检测系统、终端和存储介质 - Google Patents

Abstract

本发明提供一种基于超声波传感器的车辆位姿检测系统、终端和存储介质，通过实施本方法，车载系统能够在泊入过程中计算出可泊车位附近车辆的位姿和轮廓，车载系统可以根据可泊车位附近车辆的位姿和轮廓，及时调整自身的泊车路径和泊车位姿，解决在泊车过程中由于附近车辆停车不规范而导致仅依靠视觉泊车出现无法泊入、泊车位置距离临近车位太近影响开门、或者附近车辆停车不规范而导致与附近车辆碰撞。

Description

基于超声波传感器的车辆位姿检测系统、终端和存储介质

技术领域

本发明涉及汽车电子技术领域，特别是涉及一种基于超声波传感器的车辆位姿检测系统、终端和存储介质。

背景技术

在现有技术中，“自动代客泊车”(Auto Valet Parking)成为自动驾驶领域的热门技术之一，同样也将是自动驾驶量产道路上的一个重要里程碑。作为一套完整的自动无人驾驶汽车系统，AVP系统以低速驾驶汽车或将汽车停在有限的区域内，如停车场或周围道路。此外，作为泊车辅助的一种功能扩展，也会是最早商业化的全自动驾驶功能之一。

传统的自动泊车车位检测方法主要有两种，一种是利用摄像头识别车位线，另一种是利用测距传感器检测可泊车位空间，但目前这两种方法都仅能提供泊车车位位置，无法提供泊车位两侧车辆的位姿信息。缺少可泊车位两侧车辆位姿信息会导致两个问题。一是泊入容易失败，这主要是因为可泊车位两侧的车辆停车姿势不正，导致可泊车位空间不规则。二是泊入时对可泊空间利用不够优化，可能一侧空间太多的同时另一侧空间太窄车门无法打开。

发明内容

为了解决上述的以及其他潜在的技术问题，本发明提供了一种基于超声波传感器的车辆位姿检测系统、终端和存储介质，通过实施本方法，车载系统能够在泊入过程中计算出可泊车位附近车辆的位姿和轮廓，车载系统可以根据可泊车位附近车辆的位姿和轮廓，及时调整自身的泊车路径和泊车位姿，解决在泊车过程中由于附近车辆停车不规范而导致仅依靠视觉泊车出现无法泊入、泊车位置距离临近车位太近影响开门、或者附近车辆停车不规范而导致与附近车辆碰撞。

一种基于超声波传感器的车辆位姿检测方法，包括以下步骤：

S01：车辆获得泊车车位信息以及超声波传感器采集到环境信息中各个障碍点的距离和位置数据，将采集到的表征障碍点位置的数组标记为Pvector(p1,p2,p3,p4,p5…pk)；

S02：对障碍点组成的数组Pvector(p1,p2,p3,p4,p5…pk)按照泊车车位信息进行筛选，筛选出靠近泊车车位且沿车辆泊车入库路径方向分布的障碍点，标记为感兴趣数组Proi(p1,p2,p3,p4,p5…pi)；

S03：根据第一感兴趣数组Proi(p1,p2,p3,p4,p5…pi)中每一个障碍点pi相对于目标车位的位置，将第一感兴趣数组Proi(p1,p2,p3,p4,p5…pi)分成若干子集Pl、Pr、Pf、Pb，所述Pl、Pr、Pf、Pb分别表示目标车位左侧障碍点子集、目标车位右侧障碍点子集、目标车位前方障碍点子集、目标车位后方障碍点子集；

S04：根据路径规划提取影响车辆正常泊车入库路径规划的障碍点所在的子集，并将各个子集分别做直线拟合，分别获得表征障碍物轮廓的拟合直线Ll、Lr、Lf、Lb；

S05：分别计算表征障碍物轮廓的拟合直线Ll、Lr、Lf、Lb的置信率，获得左侧置信率(confidence left)、右侧置信率(confidence right)、前方置信率(confidencefront)、后方置信率(confidence back)；

S06：将表征障碍物轮廓的拟合直线Ll、Lr、Lf、Lb以及表征拟合直线Ll、Lr、Lf、Lb的左侧置信率(confidence left)、右侧置信率(confidence right)、前方置信率(confidence front)、后方置信率(confidence back)上报至车辆控制模块。

进一步地，所述步骤S04中直线拟合的方法具体采用霍夫变换做直线拟合，霍夫变换的具体方法为：

S041：提取子集Pl、Pr、Pf、Pb中的任一个子集，将提取子集中的障碍点p，经过障碍点p做直线L，标记直线L与原点O之间的垂线r与x轴的倾斜角为θ，记录垂线r的数值；

S042：将倾斜角以m度为角度间隔，以0度到180度为遍历范围，分别经过障碍点p做直线L，标记为直线数据集(L1、L2、L3…Lj)，其中记载了180/m个直线L，分别计算每一个倾斜角θ角度下，直线数据集中直线与原点之间垂线r的数值，以矩阵M表示：

S043：上述列表中相同角度θ下，以该行中每一个垂线

的位置为基准扩展为一个范围带，判断该行中其他垂线

是否处于范围带中；若处于范围带中，则计数值加1；若不处于范围带中，则计数值保持原值；以上述方式遍历该行，并记载该行中的每一个垂线γn的计数值，选择该行中计数值最大的垂线

并记载垂线

计数值；

S044：上述列表中不同角度θ下，以每一列角度中包含一组的

分别以步骤S043的方法遍历，获得每一列角度中垂线

的最大计数值，并记载该最大计数值；

S045：根据步骤S043和步骤S044中计算出来的垂线

计数值组成的阵列，寻找该阵列中计数值最高的垂线

进一步地，所述m度角度间隔，优选为0.2～1度，最优选地，选择0.5度为角度间隔。但是选择角度间隔的度数越大，计算量越小，但是相应的拟合直线的精度会下降，但是选择角度间隔的度数越小，遍历矩阵的计算量会提升，但是相应的拟合直线的精度会提高。

进一步地，步骤S04中直线拟合的方法具体采用霍夫变换做直线拟合，霍夫变换的具体方法为：

根据直线的参数方程x*cosθ+y*sinθ＝r；

对待拟合的点集{p1,p2,p3,....,pn}中的每一个点p，给定θ从0度到180度以0.5度为步长遍历，将θ和p的坐标代入参数方程，计算γ值。具体的：

给定θ＝0度，将p的坐标(x,y)代入直线参数方程，求出γ值；

给定θ＝0.5度，将p的坐标(x,y)代入直线参数方程，求出γ值；

给定θ＝1度，将p的坐标(x,y)代入直线参数方程，求出γ值；

给定θ＝1.5度，将p的坐标(x,y)代入直线参数方程，求出γ值；

......

给定θ＝179.5度，将p的坐标(x,y)代入直线参数方程，求出γ值；

对于长度为n的点集，经过上述计算，得到360行n列的矩阵M；

遍历矩阵中的每行数据，统计γ值近似相等的个数count，两个γ值近似相等的条件是：两个r值的差的绝对值小于设定值delta，delta值设定为0.02表示实际距离0.02米；对上述每行统计出的count值，寻找最大计数值，将此最大计数值对应的θ和γ就是我们要拟合的直线参数。

进一步地，所述拟合直线的置信率的计算方法为：

将遍历矩阵中所有的拟合直线即一组(θ，γ)，找到的拟合直线通过障碍点p的个数A，以及该子集中障碍点p总数C，即该拟合直线L的置信率是A/C；所述拟合直线的置信率用于评价拟合直线与障碍物轮廓的一致程度，当一致程度大于额定一致程度时，该置信度可以辅助泊车控制系统使用该拟合直线修正视觉图像识别轮廓的误差；当一致程度小于额定一致程度时，该置信度提供给辅助泊车控制系统提供给系统剩余应用自行判断是采纳该置信度或者放弃以该置信度为判断因子处理剩余应用的问题。

进一步地，所述步骤S01和步骤S02之间还包括至少一次障碍点过滤模块：

障碍点过滤模块的作用在于考虑泊车环境时，去除在障碍点获取过程中可知的存在误差的障碍点；以及车载系统计算时，去除无法给泊车环境做出贡献且增大计算量的障碍点。

进一步地，所述步骤S011还包括第一过滤模块：

所述第一过滤模块用于筛选车辆泊车过程中获取的障碍点，取得靠近目标车位的障碍点，所述靠近目标车位的障碍点为目标车位框外围距离为预设距离范围内的障碍点。

进一步地，所述步骤S012还包括第二过滤模块：

所述第二过滤模块用于过滤车辆泊车过程中由于车辆自身位姿变换使得障碍点的位置偏差的点，筛选车辆泊车过程中车辆自身姿态与目标车位相互平行或垂直时取得的障碍点。

进一步地，所述步骤S013还包括第三过滤模块：

所述第三过滤模块用于过滤由于车辆停滞或者速度过慢致使超声波传感器在同一姿态和位置或者同一姿态和极近位置情形下接收到同一位置或极近位置处障碍点的多个响应造成增加系统计算量，第三过滤模块将筛选这些障碍点的多个响应，只保留一个障碍点。

例如，当车辆进入泊车车位后，在最后车辆停止前，由于各种原因停滞或者以极其慢的速度倒车，那么在这种情况下，停滞或近似停滞时间周期内超声波传感器在车辆停滞位置接收到多个障碍物响应，这些响应会形成多个障碍点，这些障碍点重叠或者距离极其近似。所以在系统以障碍点做直线拟合获得障碍物轮廓时，这些重叠或者距离极其近似的障碍点会增加系统的计算量，因此需要根据每一个障碍点在获取时间戳对应的车辆的位置和姿态，根据相邻的障碍点的上一时刻的车辆位置和姿态与障碍点下一时刻的车辆位置和姿态计算得出车辆在相邻两个障碍点之间移动的距离，进而得到相邻两个障碍点之间的距离。获得相邻两个障碍点之间的距离之后，若该距离大于第三过滤模块过滤阈值的话，后一个障碍点被保留；若该距离小于第三过滤模块过滤阈值的话，后一个障碍点被过滤。

一种基于超声波传感器的车辆位姿检测系统，包括以下模块：

超声波感知模块，所述超声波感知模块设置于车辆的左侧、右侧、前侧、后侧中的一种或几种，所述超声波感知模块采集到环境信息中各个障碍点的距离和位置数据，并以数组Pvector表征；

感兴趣区域提取模块，所述感兴趣区域提取模块用于自数组Pvector中筛选出靠近泊车车位且沿车辆泊车入库路径方向分布的障碍点，标记为感兴趣数组Proi(p1,p2,p3,p4,p5…pi)；

障碍点分组模块，所述障碍点分组模块根据障碍点在目标车位附近位置的信息给障碍点分组，使原始第一感兴趣数组Proi(p1,p2,p3,p4,p5…pi)分为子集Pl、Pr、Pf、Pb，所述Pl、Pr、Pf、Pb分别表示目标车位左侧障碍点子集、目标车位右侧障碍点子集、目标车位前方障碍点子集、目标车位后方障碍点子集；

直线拟合模块，所述直线拟合模块用于各个子集障碍点p分别做直线拟合，分别获得表征障碍物轮廓的拟合直线Ll、Lr、Lf、Lb。

进一步地，还包括置信度模块，所述置信度模块用于评价直线拟合模块拟合直线Ll、Lr、Lf、Lb的置信度。

进一步地，所述直线拟合模块采用霍夫变换的方式获得。

进一步地，所述置信度模块通过障碍点p的个数A，以及该子集中障碍点p总数C，即该拟合直线L的置信率是A/C；所述拟合直线的置信率用于评价拟合直线与障碍物轮廓的一致程度。

进一步地，所述置信度模块分别计算子集Pl、Pr、Pf、Pb的经过直线拟合获得拟合直线Ll、Lr、Lf、Lb的置信率，将每一个拟合直线除以其所对应的子集中障碍点的总数，获得该拟合直线的置信率数值，每一个拟合直线对应置信率以与该拟合直线辅助数据的形式记载在拟合直线的数据结构中，当某一应用获取该拟合直线数据时，由获取拟合直线数据的应用判断置信率是否应当被参考。

进一步地，所述置信度模块和直线拟合模块均与系统控制模块通过车辆控制总线协议通讯，所述置信度模块与直线拟合模块有一一对应的关系，且以时间戳标记顺序，以车辆控制系统中不同应用的请求被调用，以车辆控制总线传输这些拟合直线数据以及其对应的置信度。

进一步地，所述直线拟合模块的作用是根据第一感兴趣数组Proi(p1,p2,p3,p4,p5…pi)分成若干子集Pl、Pr、Pf、Pb，所述Pl、Pr、Pf、Pb分别表示目标车位左侧障碍点子集、目标车位右侧障碍点子集、目标车位前方障碍点子集、目标车位后方障碍点子集分别拟合出不同方位范围内障碍物的轮廓线。

进一步地，所述直线拟合模块包括：

垂线和倾斜角计算模块，所述垂线和倾斜角计算模块用于根据每一个给定位置的障碍点，通过坐标系转换为统一坐标系之后，经过直线的参数方程x*cosθ+y*sinθ＝r；计算得出每一个给定位置的障碍点经过以设定倾斜角度θ形成直线L，以坐标原点O向直线L做垂线r的长度值以及倾斜角度θ的数值；

倾斜角度遍历模块，所述倾斜角度遍历模块用于以m度角度间隔，以最初倾斜角度θ为预设值，以每一个障碍点p位置为直线L经过的特定点，遍历180度范围内所有间隔m度的角度做直线L之后，垂线

的数值；

垂线范围带扩展模块，所述垂线范围带扩展模块用于以垂线

的位置为基准扩展范围带，判断与该垂线处于同一角度θ是否处于扩展范围带中，将处于范围带中的障碍点，将该垂线的计数值加一；以此遍历每一个垂线

的计数值，选择同一角度θ中，垂线

中计数值最大的垂线；

最高计数值垂线获取模块，所述最高计数值垂线获取模块是将倾斜角度遍历模块中每一个垂线

均经过垂线范围带扩展模块计算该列中垂线

的计数值，形成一个以垂线

为行，以倾斜角度θ的遍历为列的矩阵，选择矩阵列中，计数值最大的垂线

进一步地，还包括障碍点过滤模块：

障碍点过滤模块的作用在于考虑泊车环境时，去除在障碍点获取过程中可知的存在误差的障碍点；以及车载系统计算时，去除无法给泊车环境做出贡献且增大计算量的障碍点。

进一步地，还包括第一过滤模块：

所述第一过滤模块用于筛选车辆泊车过程中获取的障碍点，取得靠近目标车位的障碍点，所述靠近目标车位的障碍点为目标车位框外围距离为预设距离范围内的障碍点。

进一步地，还包括第二过滤模块：

所述第二过滤模块用于过滤车辆泊车过程中由于车辆自身位姿变换使得障碍点的位置偏差的点，筛选车辆泊车过程中车辆自身姿态与目标车位相互平行或垂直时取得的障碍点。

进一步地，还包括第三过滤模块：

所述第三过滤模块用于过滤由于车辆停滞或者速度过慢致使超声波传感器在同一姿态和位置或者同一姿态和极近位置情形下接收到同一位置或极近位置处障碍点的多个响应造成增加系统计算量，第三过滤模块将筛选这些障碍点的多个响应，只保留一个障碍点。

一种基于超声波传感器的车辆位姿检测在泊车中的应用，包括以下模块：

包括用以搭载超声波传感器的硬件、用以搭载泊车控制模块的软件，以支持在车辆上实现超声波传感器感知车位环境、并对超声波传感器感知环境中提取感兴趣的区域，并对感兴趣区域中障碍点进行分组，再对分组的障碍点进行直线拟合，获得车位环境附近障碍物轮廓展示，以辅助泊车。

一种移动终端，其可以是车载终端或手机移动终端，

所述车载终端可以执行如上述基于超声波传感器的车辆位姿检测在泊车方法或搭载如上述基于超声波传感器的车辆位姿检测在泊车系统；

所述手机移动终端可以执行如上述基于超声波传感器的车辆位姿检测在泊车方法或搭载如上述基于超声波传感器的车辆位姿检测在泊车系统。

一种计算机存储介质，其是依照如上述基于超声波传感器的车辆位姿检测在泊车方法所编写的计算机程序。

如上所述，本发明的具有以下有益效果：

通过实施本方法，车载系统能够在泊入过程中计算出可泊车位附近车辆的位姿和轮廓，车载系统可以根据可泊车位附近车辆的位姿和轮廓，及时调整自身的泊车路径和泊车位姿，解决在泊车过程中由于附近车辆停车不规范而导致仅依靠视觉泊车出现无法泊入、泊车位置距离临近车位太近影响开门、或者附近车辆停车不规范而导致与附近车辆碰撞。

附图说明

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

图1显示为一实施例中超声波传感器检测垂直停车位两侧车辆的姿态和轮廓辅助线(侧边线)。

图2显示为另一实施例中超声波传感器检测垂直停车位两侧倾斜的车辆的姿态和轮廓辅助线(侧边线)。

图3显示为另一实施例中超声波传感器检测垂直停车位两侧车辆车头相向内倾斜的车辆姿态和轮廓的辅助线(侧边线)。

图4显示为背景技术中纯视觉感知寻找车位角点确定停车位时两侧车辆姿态垂直但是中间车辆姿态倾斜。

图5显示为另一实施例中中间车辆为泊车车辆，目标停车位垂直，车辆左侧车位停车垂直，车辆右侧车位停车倾斜，此时超声波传感器检测到车辆右侧的障碍物轮廓之后对视觉检测车位的修正。

其中直线框为视觉检测停车位，虚线框为根据障碍物轮廓修正后的停车位，直线b为对右侧车辆轮廓的拟合直线。

图6显示为另一实施例中中间车辆为泊车车辆，目标停车位垂直，视觉感知获得的车位为停车位C，但是如果按照视觉感知停靠停车位C时，车辆会与左侧车辆距离太近乃至摩擦划伤，根据超声波传感器对车位修正之后的车位为向右侧平移后的停车位。

图7显示为另一实施例中中间车辆为泊车车辆，目标停车位垂直，视觉感知获得的车位为停车位B，但是如果按照视觉感知停靠停车位B时，车辆会与右侧车辆距离太近乃至摩擦划伤，根据超声波传感器对车位修正之后的车位为向左侧平移后的停车位。

图8显示为另一实施例中水平停车位以超声波传感器可以感知到下方的路沿轮廓，并根据左侧和右侧的车辆获得车辆轮廓辅助停车。

图9显示为另一实施例中水平停车位的以超声波传感器的修正，视觉感知定位的停车位置靠近右侧车辆，超声波传感器修正后的车位向左侧偏移。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。

须知，本说明书所附图式所绘示的结构、比例、大小等，均仅用以配合说明书所揭示的内容，以供熟悉此技术的人士了解与阅读，并非用以限定本发明可实施的限定条件，故不具技术上的实质意义，任何结构的修饰、比例关系的改变或大小的调整，在不影响本发明所能产生的功效及所能达成的目的下，均应仍落在本发明所揭示的技术内容得能涵盖的范围内。同时，本说明书中所引用的如“上”、“下”、“左”、“右”、“中间”及“一”等的用语，亦仅为便于叙述的明了，而非用以限定本发明可实施的范围，其相对关系的改变或调整，在无实质变更技术内容下，当亦视为本发明可实施的范畴。

参见图1～图9，

一种基于超声波传感器的车辆位姿检测方法，包括以下步骤：

S01：车辆获得泊车车位信息以及超声波传感器采集到环境信息中各个障碍点的距离和位置数据，将采集到的表征障碍点位置的数组标记为Pvector(p1,p2,p3,p4,p5…pk)；

S02：对障碍点组成的数组Pvector(p1,p2,p3,p4,p5…pk)按照泊车车位信息进行筛选，筛选出靠近泊车车位且沿车辆泊车入库路径方向分布的障碍点，标记为感兴趣数组Proi(p1,p2,p3,p4,p5…pi)；

S03：根据第一感兴趣数组Proi(p1,p2,p3,p4,p5…pi)中每一个障碍点pi相对于目标车位的位置，将第一感兴趣数组Proi(p1,p2,p3,p4,p5…pi)分成若干子集Pl、Pr、Pf、Pb，所述Pl、Pr、Pf、Pb分别表示目标车位左侧障碍点子集、目标车位右侧障碍点子集、目标车位前方障碍点子集、目标车位后方障碍点子集；

S04：根据路径规划提取影响车辆正常泊车入库路径规划的障碍点所在的子集，并将各个子集分别做直线拟合，分别获得表征障碍物轮廓的拟合直线Ll、Lr、Lf、Lb；

S05：分别计算表征障碍物轮廓的拟合直线Ll、Lr、Lf、Lb的置信率，获得左侧置信率(confidence left)、右侧置信率(confidence right)、前方置信率(confidencefront)、后方置信率(confidence back)；

S06：将表征障碍物轮廓的拟合直线Ll、Lr、Lf、Lb以及表征拟合直线Ll、Lr、Lf、Lb的左侧置信率(confidence left)、右侧置信率(confidence right)、前方置信率(confidence front)、后方置信率(confidence back)上报至车辆控制模块。

进一步地，所述步骤S04中直线拟合的方法具体采用霍夫变换做直线拟合，霍夫变换的具体方法为：

S041：提取子集Pl、Pr、Pf、Pb中的任一个子集，将提取子集中的障碍点p，经过障碍点p做直线L，标记直线L与原点O之间的垂线r与x轴的倾斜角为θ，记录垂线r的数值；

S042：将倾斜角以m度为角度间隔，以0度到180度为遍历范围，分别经过障碍点p做直线L，标记为直线数据集(L1、L2、L3…Lj)，其中记载了180/m个直线L，分别计算每一个倾斜角θ角度下，直线数据集中直线与原点之间垂线r的数值，以矩阵M表示：

S043：上述列表中相同角度θ下，以该行中每一个垂线

的位置为基准扩展为一个范围带，判断该行中其他垂线

是否处于范围带中；若处于范围带中，则计数值加1；若不处于范围带中，则计数值保持原值；以上述方式遍历该行，并记载该行中的每一个垂线γn的计数值，选择该行中计数值最大的垂线

并记载垂线

计数值；

S044：上述列表中不同角度θ下，以每一列角度中包含一组的

分别以步骤S043的方法遍历，获得每一列角度中垂线

的最大计数值，并记载该最大计数值；

S045：根据步骤S043和步骤S044中计算出来的垂线

计数值组成的阵列，寻找该阵列中计数值最高的垂线

作为优选实施例，所述m度角度间隔，优选为0.2～1度，最优选地，选择0.5度为角度间隔。但是选择角度间隔的度数越大，计算量越小，但是相应的拟合直线的精度会下降，但是选择角度间隔的度数越小，遍历矩阵的计算量会提升，但是相应的拟合直线的精度会提高。

作为优选实施例，步骤S04中直线拟合的方法具体采用霍夫变换做直线拟合，霍夫变换的具体方法为：

根据直线的参数方程x*cosθ+y*sinθ＝r；

对待拟合的点集{p1,p2,p3,....,pn}中的每一个点p，给定θ从0度到180度以0.5度为步长遍历，将θ和p的坐标代入参数方程，计算γ值。具体的：

给定θ＝0度，将p的坐标(x,y)代入直线参数方程，求出γ值；

给定θ＝0.5度，将p的坐标(x,y)代入直线参数方程，求出γ值；

给定θ＝1度，将p的坐标(x,y)代入直线参数方程，求出γ值；

给定θ＝1.5度，将p的坐标(x,y)代入直线参数方程，求出γ值；

......

给定θ＝179.5度，将p的坐标(x,y)代入直线参数方程，求出γ值；

对于长度为n的点集，经过上述计算，得到360行n列的矩阵M；

遍历矩阵中的每行数据，统计γ值近似相等的个数count，两个γ值近似相等的条件是：

两个r值的差的绝对值小于设定值delta，delta值设定为0.02表示实际距离0.02米；对上述每行统计出的count值，寻找最大计数值，将此最大计数值对应的θ和γ就是我们要拟合的直线参数。

作为优选实施例，所述拟合直线的置信率的计算方法为：

将遍历矩阵中所有的拟合直线即一组(θ，γ)，找到的拟合直线通过障碍点p的个数A，以及该子集中障碍点p总数C，即该拟合直线L的置信率是A/C；所述拟合直线的置信率用于评价拟合直线与障碍物轮廓的一致程度，当一致程度大于额定一致程度时，该置信度可以辅助泊车控制系统使用该拟合直线修正视觉图像识别轮廓的误差；当一致程度小于额定一致程度时，该置信度提供给辅助泊车控制系统提供给系统剩余应用自行判断是采纳该置信度或者放弃以该置信度为判断因子处理剩余应用的问题。

作为优选实施例，所述步骤S01和步骤S02之间还包括至少一次障碍点过滤模块：

障碍点过滤模块的作用在于考虑泊车环境时，去除在障碍点获取过程中可知的存在误差的障碍点；以及车载系统计算时，去除无法给泊车环境做出贡献且增大计算量的障碍点。

作为优选实施例，所述步骤S011还包括第一过滤模块：

所述第一过滤模块用于筛选车辆泊车过程中获取的障碍点，取得靠近目标车位的障碍点，所述靠近目标车位的障碍点为目标车位框外围距离为预设距离范围内的障碍点。

作为优选实施例，所述步骤S012还包括第二过滤模块：

所述第二过滤模块用于过滤车辆泊车过程中由于车辆自身位姿变换使得障碍点的位置偏差的点，筛选车辆泊车过程中车辆自身姿态与目标车位相互平行或垂直时取得的障碍点。

作为优选实施例，所述步骤S013还包括第三过滤模块：

所述第三过滤模块用于过滤由于车辆停滞或者速度过慢致使超声波传感器在同一姿态和位置或者同一姿态和极近位置情形下接收到同一位置或极近位置处障碍点的多个响应造成增加系统计算量，第三过滤模块将筛选这些障碍点的多个响应，只保留一个障碍点。

例如，当车辆进入泊车车位后，在最后车辆停止前，由于各种原因停滞或者以极其慢的速度倒车，那么在这种情况下，停滞或近似停滞时间周期内超声波传感器在车辆停滞位置接收到多个障碍物响应，这些响应会形成多个障碍点，这些障碍点重叠或者距离极其近似。所以在系统以障碍点做直线拟合获得障碍物轮廓时，这些重叠或者距离极其近似的障碍点会增加系统的计算量，因此需要根据每一个障碍点在获取时间戳对应的车辆的位置和姿态，根据相邻的障碍点的上一时刻的车辆位置和姿态与障碍点下一时刻的车辆位置和姿态计算得出车辆在相邻两个障碍点之间移动的距离，进而得到相邻两个障碍点之间的距离。获得相邻两个障碍点之间的距离之后，若该距离大于第三过滤模块过滤阈值的话，后一个障碍点被保留；若该距离小于第三过滤模块过滤阈值的话，后一个障碍点被过滤。

一种基于超声波传感器的车辆位姿检测系统，包括以下模块：

超声波感知模块，所述超声波感知模块设置于车辆的左侧、右侧、前侧、后侧中的一种或几种，所述超声波感知模块采集到环境信息中各个障碍点的距离和位置数据，并以数组Pvector表征；

感兴趣区域提取模块，所述感兴趣区域提取模块用于自数组Pvector中筛选出靠近泊车车位且沿车辆泊车入库路径方向分布的障碍点，标记为感兴趣数组Proi(p1,p2,p3,p4,p5…pi)；

障碍点分组模块，所述障碍点分组模块根据障碍点在目标车位附近位置的信息给障碍点分组，使原始第一感兴趣数组Proi(p1,p2,p3,p4,p5…pi)分为子集Pl、Pr、Pf、Pb，所述Pl、Pr、Pf、Pb分别表示目标车位左侧障碍点子集、目标车位右侧障碍点子集、目标车位前方障碍点子集、目标车位后方障碍点子集；

直线拟合模块，所述直线拟合模块用于各个子集障碍点p分别做直线拟合，分别获得表征障碍物轮廓的拟合直线Ll、Lr、Lf、Lb；

置信度模块，所述置信度模块用于评价直线拟合模块拟合直线Ll、Lr、Lf、Lb的置信度。

作为优选实施例，所述直线拟合模块采用霍夫变换的方式获得。

作为优选实施例，所述置信度模块通过障碍点p的个数A，以及该子集中障碍点p总数C，即该拟合直线L的置信率是A/C；所述拟合直线的置信率用于评价拟合直线与障碍物轮廓的一致程度。

进一步地，所述直线拟合模块包括：

垂线和倾斜角计算模块，所述垂线和倾斜角计算模块用于根据每一个给定位置的障碍点，通过坐标系转换为统一坐标系之后，经过直线的参数方程x*cosθ+y*sinθ＝r；计算得出每一个给定位置的障碍点经过以设定倾斜角度θ形成直线L，以坐标原点O向直线L做垂线r的长度值以及倾斜角度θ的数值；

倾斜角度遍历模块，所述倾斜角度遍历模块用于以m度角度间隔，以最初倾斜角度θ为预设值，以每一个障碍点p位置为直线L经过的特定点，遍历180度范围内所有间隔m度的角度做直线L之后，垂线

的数值；

垂线范围带扩展模块，所述垂线范围带扩展模块用于以垂线

的位置为基准扩展范围带，判断与该垂线处于同一角度θ是否处于扩展范围带中，将处于范围带中的障碍点，将该垂线的计数值加一；以此遍历每一个垂线

的计数值，选择同一角度θ中，垂线

中计数值最大的垂线；

最高计数值垂线获取模块，所述最高计数值垂线获取模块是将倾斜角度遍历模块中每一个垂线

均经过垂线范围带扩展模块计算该列中垂线

的计数值，形成一个以垂线

为行，以倾斜角度θ的遍历为列的矩阵，选择矩阵列中，计数值最大的垂线

进一步地，还包括障碍点过滤模块：

障碍点过滤模块的作用在于考虑泊车环境时，去除在障碍点获取过程中可知的存在误差的障碍点；以及车载系统计算时，去除无法给泊车环境做出贡献且增大计算量的障碍点。

进一步地，还包括第一过滤模块：

所述第一过滤模块用于筛选车辆泊车过程中获取的障碍点，取得靠近目标车位的障碍点，所述靠近目标车位的障碍点为目标车位框外围距离为预设距离范围内的障碍点。

进一步地，还包括第二过滤模块：

所述第二过滤模块用于过滤车辆泊车过程中由于车辆自身位姿变换使得障碍点的位置偏差的点，筛选车辆泊车过程中车辆自身姿态与目标车位相互平行或垂直时取得的障碍点。

进一步地，还包括第三过滤模块：

所述第三过滤模块用于过滤由于车辆停滞或者速度过慢致使超声波传感器在同一姿态和位置或者同一姿态和极近位置情形下接收到同一位置或极近位置处障碍点的多个响应造成增加系统计算量，第三过滤模块将筛选这些障碍点的多个响应，只保留一个障碍点。

一种基于超声波传感器的车辆位姿检测在泊车中的应用，包括以下模块：

包括用以搭载超声波传感器的硬件、用以搭载泊车控制模块的软件，以支持在车辆上实现超声波传感器感知车位环境、并对超声波传感器感知环境中提取感兴趣的区域，并对感兴趣区域中障碍点进行分组，再对分组的障碍点进行直线拟合，获得车位环境附近障碍物轮廓展示，以辅助泊车。

一种移动终端，其可以是车载终端或手机移动终端，

所述车载终端可以执行如上述基于超声波传感器的车辆位姿检测在泊车方法或搭载如上述基于超声波传感器的车辆位姿检测在泊车系统；

所述手机移动终端可以执行如上述基于超声波传感器的车辆位姿检测在泊车方法或搭载如上述基于超声波传感器的车辆位姿检测在泊车系统。

一种计算机存储介质，其是依照如上述基于超声波传感器的车辆位姿检测在泊车方法所编写的计算机程序。

作为优选实施例，本实施例还提供一种终端设备，如可以执行程序的智能手机、平板电脑、笔记本电脑、台式计算机、机架式云端、刀片式云端、塔式云端或机柜式云端(包括独立的云端，或者多个云端所组成的云端集群)等。本实施例的终端设备至少包括但不限于：可通过系统总线相互通信连接的存储器、处理器。需要指出的是，具有组件存储器、处理器的终端设备，但是应理解的是，并不要求实施所有示出的组件，可以替代的基于超声波传感器的车辆位姿检测在泊车方法实施更多或者更少的组件。

作为优选实施例，存储器(即可读存储介质)包括闪存、硬盘、多媒体卡、卡型存储器(例如，SD或DX存储器等)、随机访问存储器(RAM)、静态随机访问存储器(SRAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、可编程只读存储器(PROM)、磁性存储器、磁盘、光盘等。在一些实施例中，存储器可以是计算机设备的内部存储单元，例如该计算机设备的硬盘或内存。在另一些实施例中，存储器也可以是计算机设备的外部存储设备，例如该计算机设备上配备的插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(SecureDigital,SD)卡，闪存卡(Flash Card)等。当然，存储器还可以既包括计算机设备的内部存储单元也包括其外部存储设备。本实施例中，存储器通常用于存储安装于计算机设备的操作系统和各类应用软件，例如实施例中的基于超声波传感器的车辆位姿检测在泊车方法程序代码等。此外，存储器还可以用于暂时地存储已经输出或者将要输出的各类数据。

本实施例还提供一种计算机可读存储介质，如闪存、硬盘、多媒体卡、卡型存储器(例如，SD或DX存储器等)、随机访问存储器(RAM)、静态随机访问存储器(SRAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、可编程只读存储器(PROM)、磁性存储器、磁盘、光盘、云端、App应用商城等等，其上存储有计算机程序，程序被处理器执行时实现相应功能。本实施例的计算机可读存储介质用于基于超声波传感器的车辆位姿检测在泊车方法程序，被处理器执行时实现基于超声波传感器的车辆位姿检测在泊车方法程序实施例中的镜头付着物的检测方法。

上述实施例仅例示性说明本发明的原理及其功效，而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中包括通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。

Claims (9)

1.一种基于超声波传感器的车辆位姿检测系统，其特征在于，包括以下模块：

超声波感知模块，所述超声波感知模块设置于车辆的左侧、右侧、前侧、后侧中的一种或几种，所述超声波感知模块采集到环境信息中各个障碍点的距离和位置数据，并以数组Pvector表征；

感兴趣区域提取模块，所述感兴趣区域提取模块用于自数组Pvector中筛选出靠近泊车车位且沿车辆泊车入库路径方向分布的障碍点，标记为感兴趣数组Proi(p1,p2,p3,p4,p5…pi)；

障碍点分组模块，所述障碍点分组模块根据障碍点在目标车位附近位置的信息给障碍点分组，使原始第一感兴趣数组Proi(p1,p2,p3,p4,p5…pi)分为子集Pl、Pr、Pf、Pb，所述Pl、Pr、Pf、Pb分别表示目标车位左侧障碍点子集、目标车位右侧障碍点子集、目标车位前方障碍点子集、目标车位后方障碍点子集；

直线拟合模块，所述直线拟合模块用于各个子集障碍点p分别做直线拟合，分别获得表征障碍物轮廓的拟合直线Ll、Lr、Lf、Lb；

置信度模块，所述置信度模块用于评价直线拟合模块拟合直线Ll、Lr、Lf、Lb的置信度，所述置信度模块通过障碍点p的个数A，以及该子集中障碍点p总数C，即该拟合直线L的置信率是A/C；所述拟合直线的置信率用于评价拟合直线与障碍物轮廓的一致程度。

2.根据权利要求1所述的基于超声波传感器的车辆位姿检测系统，其特征在于，所述直线拟合模块采用霍夫变换的方式获得。

3.根据权利要求2所述的基于超声波传感器的车辆位姿检测系统，其特征在于，所述直线拟合模块包括：

垂线和倾斜角计算模块，所述垂线和倾斜角计算模块用于根据每一个给定位置的障碍点p，通过坐标系转换为统一坐标系之后，经过直线的参数方程x*cosθ+y*sinθ＝r；计算得出每一个给定位置的障碍点p经过以设定倾斜角度θ形成直线L，以坐标原点O向直线L做垂线r的长度值以及倾斜角度θ的数值；

倾斜角度遍历模块，所述倾斜角度遍历模块用于以m度角度间隔，以最初倾斜角度θ为 预设值，以每一个障碍点p位置为直线L经过的特定点，遍历180度范围内所有间隔m度的角 度做直线L之后，垂线

的数值；

垂线范围带扩展模块，所述垂线范围带扩展模块用于以垂线

的位置为基准扩 展范围带，判断与该垂线r处于同一角度θ是否处于扩展范围带中，将处于范围带中的障碍 点p，将该垂线r的计数值加一；以此遍历每一个垂线

的计数值，选择同一角度θ中， 垂线

中计数值最大的垂线；

最高计数值垂线获取模块，所述最高计数值垂线获取模块是将倾斜角度遍历模块中每 一个垂线

均经过垂线范围带扩展模块计算垂线的计数值，形成一个以垂线

为行，以倾斜角度θ的遍历为列的矩阵，选择矩阵列中，计数值最大的垂线

。

4.根据权利要求1所述的基于超声波传感器的车辆位姿检测系统，其特征在于，还包括障碍点过滤模块：

障碍点过滤模块的作用在于考虑泊车环境时，去除在障碍点获取过程中可知的存在误差的障碍点p；以及车载系统计算时，去除无法给泊车环境做出贡献且增大计算量的障碍点p。

5.根据权利要求4所述的基于超声波传感器的车辆位姿检测系统，其特征在于，还包括第一过滤模块：

所述第一过滤模块用于筛选车辆泊车过程中获取的障碍点p，取得靠近目标车位的障碍点p，所述靠近目标车位的障碍点p为目标车位框外围距离为预设距离范围内的障碍点p。

6.根据权利要求5所述的基于超声波传感器的车辆位姿检测系统，其特征在于，还包括第二过滤模块：所述第二过滤模块用于过滤车辆泊车过程中由于车辆自身位姿变换使得障碍点p的位置偏差的障碍点p，筛选车辆泊车过程中车辆自身姿态与目标车位相互平行或垂直时取得的障碍点p。

7.根据权利要求5所述的基于超声波传感器的车辆位姿检测系统，其特征在于，还包括第三过滤模块：

所述第三过滤模块用于过滤由于车辆停滞或者速度过慢致使超声波传感器在同一姿态和位置或者同一姿态和极近位置情形下接收到同一位置或极近位置处障碍点p的多个响应造成增加系统计算量，第三过滤模块将筛选这些障碍点p的多个响应，只保留一个障碍点p。

8.一种移动终端，其可以是车载终端或手机移动终端，其特征在于，

所述车载终端可以执行如上述权利要求1-7任一项所述的基于超声波传感器的车辆位姿检测系统；

所述手机移动终端可以执行如上述权利要求1-7任一项所述的基于超声波传感器的车辆位姿检测系统。

9.一种基于超声波传感器的车辆位姿检测系统在泊车中的应用，其特征在于，所述基于超声波传感器的车辆位姿检测系统是权利要求1-7任意一项所述的基于超声波传感器的车辆位姿检测系统，用于获得车位环境附近障碍物轮廓展示，以辅助泊车。

Images