基于长距离超声波的路沿位置及角度检测方法及其系统与流程

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

背景技术：

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

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

技术实现要素：

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

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

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

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

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

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

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

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

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

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

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

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

可选地，还包括：

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

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

可选地，还包括：将所述绝对角度和所述实时距离与车辆环视摄像头识别出的线特征做匹配分析，最终得到准确的路沿位置和角度，并进行显示。

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

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

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

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

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

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

可选地，所述修正模块具体用于：

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

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

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

可选地，所述第二计算模块还用于：

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

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

可选地，还包括分析模块和显示模块；其中，所述分析模块用于将所述绝对角度和所述实时距离与车辆环视摄像头识别出的线特征做匹配分析，最终得到准确的路沿位置和角度；所述显示模块用于将最终得到的准确的路沿位置和角度进行显示。

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

附图说明

图1为本发明实施例提供的基于长距离超声波的路沿位置及角度检测系统的硬件结构示意图；

图2为本发明实施例提供的基于长距离超声波的路沿位置及角度检测方法的流程示意图；

图3为本发明实施例提供的基于长距离超声波的路沿位置及角度检测系统的结构框图。

具体实施方式

为使本发明要解决的技术问题、技术方案和优点更加清楚，下面将结合附图及具体实施例进行详细描述。

图1为本发明实施例提供的基于长距离超声波的路沿位置及角度检测系统的硬件结构示意图。如图1所示，本发明实施例提供的基于长距离超声波的路沿位置及角度检测系统的硬件结构包括：2个安装在前保险杠两侧的侧向长距离雷达、4个环视摄像头，分别安装于前保险杠格栅处、两个侧后视镜下方及后保险杠临牌上方、安装在车轮制动盘附近的轮速脉冲传感器、安装在驾驶员座椅下方的横摆角传感器、及车内车载娱乐系统上的ivi显示。在本发明实施例中，长距离雷达能探测到5米以内的范围，环视摄像头能显示周围360°的视野范围，轮速脉冲传感器通过脉冲数能计算出各个车轮的行进距离，横摆角传感器能记录汽车纵向轴线摆动的角速度。

本发明另一实施例提供一种基于长距离超声波的路沿位置及角度检测方法，该方法通过前述系统执行。该方法首先使用泊车车辆上的轮速脉冲传感器及横摆角传感器计算出泊车车辆的实际位置(包括车辆中心的横纵向坐标和车辆的航向角度)后，再使用两个长距超声波雷达对泊车车位进行扫描得到路沿相对位置信息即可确切的计算出路沿上的点所处的坐标，一系列的坐标经过处理后即可计算出路沿大致位置及角度，再使计算出的结果与环视摄像头识别出的线特征做匹配分析，最终可准确计算出路沿的精确位置及角度，并可最终通过ivi显示在全景的界面上。以下结合图2对本发明实施例的方法进行详细介绍。

图2为本发明实施例提供的基于长距离超声波的路沿位置及角度检测方法的流程示意图，如图2所示，本发明实施例提供的基于长距离超声波的路沿位置及角度检测方法包括以下步骤：

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

在该步骤中，以车辆起始点建立直角坐标系，通过车辆超声波的位置及超声波检测出的路沿距离计算路沿上的超声波反射点所在的坐标位置(x，y)，并存储在数组当中。由于车辆超声波雷达在直角坐标系中的位置(x1，y1)可实时获得，因此，如果在以水平轴为x轴、竖直轴为y轴构建的直角坐标系时，则每个超声波反射点所在的横坐标xi等于超声波的横坐标xio加上路沿距离li：即xi＝xio+li，每个超声波反射点所在的纵坐标yi等于超声波的横坐标yio，即yi＝yio。

此外，在该步骤中，为避免杂波的干扰，检测出的路沿距离是通过滤波处理后的路沿距离。可使用现有的滤波方法对检测出的路沿距离进行滤波处理。

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

在该步骤中，当存储30组坐标对应的路沿距离在合理范围内，例如在2m～4m内，相对应的超声波反射波宽度及强度可为反映实际路沿形状及材质特点的有效点，即认为收到的信号是经过路沿石上的反射点反射回的信号，并且扫描到的所有路沿上超声波的反射点的点数小于1.5倍的有效点数时，即可认为存在路沿石。

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

在该步骤中，提取已存储的有效点的坐标(xi，yi)，有效点数记作n，这些有效点使用最小二乘法进行线性拟合，使这些点均匀的分布在一条直线两侧，通过下列公式，计算出路沿所在的直线斜率b及截距a，公式如下所示：

y＝bx+a

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

由于车辆与路沿存在初始角度不为零的情况，所以步骤s103得出的结果存在一定的误差，因此，每个有效点的坐标并不一定是真实的坐标，因此需要使用已经计算出的斜率值去推导路沿上的点所在的真实坐标，再用真实值去修正路沿所在直线的斜率及截距值，通过多个有效点的反复迭代优化即可得出比较合理的路沿斜率及截距值，具体地，可包括以下步骤：

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

在该步骤中，可随机或者按照一定的规则选取多个有效点，基于这些有效点对应的超声波雷达的坐标和探测的距离，以及步骤s103得到的初始斜率，利用两点之间的距离计算公式以及直线斜率与点之间的关系，得到这些有效点的真实坐标。在以水平轴为x轴、竖直轴为y轴构建的直角坐标系中，真实坐标(xir，yir)可通过下述公式确定：

其中，xio和yio为每个有效点对应的超声波雷达的坐标，lio为每个有效点对应的超声波雷达探测到的距离，b为初始斜率。

需要说明的是，一般的车位扫描过程中会存在超过20个有效点，本发明实施中，通过10次以上的迭代即可得到比较合理的路沿斜率及截距值。

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

在该步骤中，首先将步骤(1)得到的真实坐标中的一个替代n组有效点中的一个，并代入求斜率b及截距a的公式中(第一次为前述得到的初始直线方程)，得到一次修正后的斜率和截距以及相应的直线方程，然后再将剩余真实坐标中的一个代入一次修正后的斜率和截距公式中，得到二次修正后的斜率和截距以及相应的直线方程，依次类推，利用剩余的真实坐标进行迭代计算，最终得到合理的路沿斜率和截距值。

此外，由于迭代优化所占权值较大，如引入不合理值会引起较大的误差，所以前述的滤波后的路沿距离将不能满足要求，因此，在每次迭代计算时，需判断计算出的有效点真实坐标(xir，yir)与上次计算的路沿所在直线方程y＝bx+a之间的距离，如果本次迭代使用的有效点的真实坐标与上次迭代计算得到的直线方程之间的距离小于预设的阈值，例如小于0.5m，则使用该真实坐标对上次迭代计算得到的直线方程的斜率和截距值进行修正，否则，不使用该真实坐标进行修正。

此外，需要说明的是，在斜率和截距的修正过程中，需考虑到超声波的检测距离及检测的横向开角有限，如在扫描车位的过程中，通过轮速脉冲及横摆角传感器计算出的车辆与车位的角度过大(例如大于10°)，即如果在每次修正后，得到的车辆与车位的角度过大，则可不对上次的斜率和截距进行修正，保持之前的计算结果。

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

在该步骤中，航向角可通过轮速脉冲传感器得到的各个车轮具体行进的距离计算得到，也可以对横摆角传感器测得的横摆角速度进行积分得到。路沿的绝对角度可等于车辆的航向角加上基于修正后的斜率得到的路沿的角度(即路沿与车辆的夹角)。在经过步骤s104的迭代计算得到最新的直线方程后，可将车辆对应的超声波雷达坐标代入到该最新的直线方程中，得到车辆与路沿之间的实时距离。

路沿的角度和实时距离可以在正式泊车时进行，但也可在每次迭代计算之后进行计算，只是最终使用的为最后一次计算得到的数据，本发明对此不作特别限定。

此外，在计算路沿的绝对角度和车辆与路沿之间的实时距离时，可执行可以下步骤：

计算所有的有效点的初始坐标与所述直线方程之间的距离累加和；如果所述距离累加和的倒数大于预设的阈值，则确定所述绝对角度和所述实时距离可信。

具体地，在本发明实施例中，路沿上的反射点坐标到已拟合出直线间距离的累加和可反映出拟合直线与实际路沿石反射点的偏差，反射点到这条直线的距离越小，说明反射点越有效，即其倒数越大，说明其越可信，即表示存在路沿，所以可将反射点到拟合直线的距离雷达和倒数作为置信度，当置信度的数值大于一定阈值时，例如1/(0.5*n)(n为有效点个数)认为计算结果可信。

进一步地，本发明的方法还可包括：将所述绝对角度和所述实时距离与车辆环视摄像头识别出的线特征做匹配分析，最终得到准确的路沿位置和角度，并进行显示。具体地，将检测出来的表征路沿的直线显示到车机上，然后与环视摄像头显示出的周围的真实环境进行比对，看真实环境中的路沿和检测到的路沿看是不是会重合。在一个示例中，以1cm为1.6个像素点，如果检测出来的路沿直线与实际环境若相差20cm以内，即12-13个像素点差距时，则认为其结果是匹配的，否则的话，就需要进行重新计算路沿直线。

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

基于同一发明构思，本发明实施例还提供了一种基于长距离超声波的路沿位置即角度检测系统，由于该系统所解决问题的原理与前述的方法相似，因此该系统的实施可以参见前述方法的实施，重复之处不再赘述。

如图3所示，本发明实施例提供的一种基于长距离超声波的路沿位置及角度检测系统，包括：

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

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

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

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

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

进一步地，所述修正模块204具体用于：

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

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

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

进一步地，所述第二计算模块205还用于：

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

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

进一步地，在本发明另一实施例中，提供的基于长距离超声波的路沿位置即角度检测系统还包括分析模块和显示模块；其中，所述分析模块用于将所述绝对角度和所述实时距离与车辆环视摄像头识别出的线特征做匹配分析，最终得到准确的路沿位置和角度；所述显示模块用于将最终得到的准确的路沿位置和角度进行显示。

上述各模块的功能可对应于前述方法中的相应处理步骤，在此不再赘述。

以上所述实施例，仅为本发明的具体实施方式，用以说明本发明的技术方案，而非对其限制，本发明的保护范围并不局限于此，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。