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

权利要求书

1.一种基于长距离超声波的路沿位置及角度检测方法，其特征在于，包括：

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

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

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

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

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

2.根据权利要求1所述的方法，其特征在于，所述基于所述有效点对应的超声波雷达的坐标和对应的超声波雷达探测的路沿距离，以及所述初始直线方程，得到所述有效点的真实坐标，并利用所述真实坐标对所述初始斜率和所述初始截距进行修正，得到修正后的斜率和截距以及直线方程具体包括：

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

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

3.根据权利要求2所述的方法，其特征在于，还包括：在每次迭代计算时，如果本次迭代使用的有效点的真实坐标与上次迭代计算得到的直线方程之间的距离小于预设的阈值，则使用该真实坐标对上次迭代计算得到的直线方程的斜率和截距值进行修正，否则，不使用该真实坐标进行修正。

4.根据权利要求1所述的方法，其特征在于，还包括：

计算所有的有效点的初始坐标与所述直线方程之间的距离累加和；

如果所述距离累加和的倒数大于预设的阈值，则确定所述绝对角度和所述实时距离可信。

5.根据权利要求1所述的方法，其特征在于，还包括：将所述绝对角度和所述实时距离与车辆环视摄像头识别出的线特征做匹配分析，最终得到准确的路沿位置和角度，并进行显示。

6.一种基于长距离超声波的路沿位置及角度检测系统，其特征在于，包括：

初始坐标确定模块，用于通过车辆超声波雷达在直角坐标系中的位置和超声波检测出的路沿与车辆之间的路沿距离计算路沿上的超声波反射点在直角坐标系中的初始坐标，并存储所述初始坐标，所述直角坐标系以车辆起始点为原点建立；

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

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

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

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