[发明专利]基于超声波传感器的车辆位姿检测方法、系统、终端和存储介质

权利要求书

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

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的置信率，获得左侧置信率、右侧置信率、前方置信率、后方置信率；

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

2.根据权利要求1所述的基于超声波传感器的车辆位姿检测方法，其特征在于，所述步骤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的数值；

S043：上述列表中相同角度θ下，以该行中每一个垂线的位置为基准扩展为一个范围带，判断该行中其他垂线是否处于范围带中；若处于范围带中，则计数值加1；若不处于范围带中，则计数值保持原值；

以步骤SO43中上述方式遍历该行，并记载该行中的每一个垂线γn的计数值，选择该行中计数值最大的垂线并记载垂线计数值；

S044：上述列表中不同角度θ下，以每一列角度中包含一组的分别以步骤S043的方法遍历，获得每一列角度中垂线的最大计数值，并记载该最大计数值；

S045：根据步骤S043和步骤S044中计算出来的垂线计数值组成的阵列，寻找该阵列中计数值最高的垂线

3.根据权利要求2所述的基于超声波传感器的车辆位姿检测方法，其特征在于，所述m度角度间隔为0.2～1度，选择角度间隔的度数越大，计算量越小，但是相应的拟合直线的精度会下降，但是选择角度间隔的度数越小，遍历矩阵的计算量会提升，但是相应的拟合直线的精度会提高。

4.根据权利要求3所述的基于超声波传感器的车辆位姿检测方法，其特征在于，所述拟合直线的置信率的计算方法为：

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