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

摘要

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

主权项

1.一种基于长距离超声波的路沿位置及角度检测方法，其特征在于，包括：通过车辆超声波雷达在直角坐标系中的位置和超声波雷达探测的路沿距离计算路沿上的超声波反射点在直角坐标系中的初始坐标，并存储所述初始坐标，所述直角坐标系以车辆起始点为原点建立；在存储的初始坐标中确定有预设数量的坐标为有效点，并且检测到的所有的超声波反射点的个数小于1.5倍的有效点时，确定存在路沿，其中，若初始坐标对应的路沿距离满足预设的距离范围时，则确定该初始坐标为有效点；利用所述预设数量的坐标，使用最小二乘法进行线性拟合，得到所述路沿石在所述直角坐标系中的初始斜率和初始截距，得到相应的初始直线方程；基于所述有效点对应的超声波雷达的坐标和对应的超声波雷达探测的路沿距离，以及所述初始直线方程，得到所述有效点的真实坐标，并利用所述真实坐标对所述初始斜率和所述初始截距进行修正，得到修正后的斜率和截距以及直线方程；基于车辆的航向角和通过所述修正后的斜率得到的路沿的角度，确定路沿在大地坐标系中的绝对角度，以及计算车辆到修正后的直线方程的垂直距离，得到车辆到路沿的实时距离。