[发明专利]基于长距离超声波的路沿位置及角度检测方法及其系统

说明书

本发明提供了一种基于长距离超声波的路沿位置及角度检测方法及其系统。该方法和系统在长距离超声波扫描车位的同时就可以判断出路沿的准确位置及角度，不需要添加多余操作。这样在规划泊车路径的过程中就可以按照路沿的位置及角度，使泊车角度平行于路沿，且在与路沿石间距合理的位置上面。有效的解决了自动泊车姿态不正和与路沿发生刮碰的问题，提高了自动泊车的安全性，大幅增加用户的信任度及科技感。并且，可在自动泊车的全景图像中着重标记出路沿所在的位置，增加用户在自动泊车中的科技感、安全感。

技术领域

本发明涉及汽车自动泊车领域，具体涉及一种基于长距离超声波的路沿位置及角度检测方法及其系统。

背景技术

随着高级驾驶员辅助系统的迅猛发展，消费者对于辅助泊车类技术表现出明显的关注度与需求度，而典型的自动泊车系统多使用超声波对潜在的车位进行检测，然后规划路径，准确的停入到相应的位置，但在进行平行泊车时普遍存在的三个问题：1.前后车辆泊车姿态不正，导致自车的泊车角度无法确定。2.扫描车位后，只能得出车位的长度和估计出的宽度(只能用与前后车辆的距离给出深度模糊值)，所以跟踪规划的路径行驶时有撞到路沿的危险。3.平行泊车的深度判断不准确，容易出现泊车深度不足，或泊车过深的状况。

因此，亟待需要提供一种能够在进行平行泊车时，能够使泊入车辆与路沿石平行且距离适宜的自动泊车系统。

发明内容

针对上述技术问题，本发明提供一种基于长距离超声波的路沿位置及角度检测方法及其系统，能够在长距离超声波扫描车位的同时就可以判断出路沿的准确位置及角度。

本发明采用的技术方案为：

本发明实施例提供一种基于长距离超声波的路沿位置及角度检测方法，包括：

通过车辆超声波雷达在直角坐标系中的位置和超声波雷达探测的路沿距离计算路沿上的超声波反射点在直角坐标系中的初始坐标，并存储所述初始坐标，所述直角坐标系以车辆起始点为原点建立；

在存储的初始坐标中确定有预设数量的坐标为有效点，并且检测到的所有的超声波反射点的个数小于1.5倍的有效点时，确定存在路沿，其中，若初始坐标对应的路沿距离满足预设的距离范围时，则确定该初始坐标为有效点；

利用所述预设数量的坐标，使用最小二乘法进行线性拟合，得到所述路沿石在所述直角坐标系中的初始斜率和初始截距，得到相应的初始直线方程；

基于所述有效点对应的超声波雷达的坐标和对应的超声波雷达探测的路沿距离，以及所述初始直线方程，得到所述有效点的真实坐标，并利用所述真实坐标对所述初始斜率和所述初始截距进行修正，得到修正后的斜率和截距以及直线方程；

基于车辆的航向角和通过所述修正后的斜率得到的路沿的角度，确定路沿在大地坐标系中的绝对角度，以及计算车辆到修正后的直线方程的垂直距离，得到车辆到路沿的实时距离。

可选地，所述基于所述有效点对应的超声波雷达的坐标和对应的超声波雷达探测的路沿距离，以及所述初始直线方程，得到所述有效点的真实坐标，并利用所述真实坐标对所述初始斜率和所述初始截距进行修正，得到修正后的斜率和截距以及直线方程具体包括：

基于至少一个有效点对应的超声波雷达的坐标和对应的超声波雷达探测的路沿距离，以及所述初始斜率，利用两点之间的距离计算公式以及直线斜率与点之间的关系，得到所述至少一个有效点的真实坐标；

基于斜率和截距的计算方式，利用所述真实坐标进行迭代计算，最终得到修正后的斜率和截距值及相应的直线方程，其中，在每次迭代中，本次迭代利用的直线方程为上次迭代得到的直线方程。

可选地，还包括：在每次迭代计算时，如果本次迭代使用的有效点的真实坐标与上次迭代计算得到的直线方程之间的距离小于预设的阈值，则使用该真实坐标对上次迭代计算得到的直线方程的斜率和截距值进行修正，否则，不使用该真实坐标进行修正。