CN108909721A - 一种基于毫米波雷达的车辆偏航角计算方法 - Google Patents

Abstract

本发明涉及一种基于毫米波雷达的车辆偏航角计算方法，通过采用毫米波雷达来检测高速公路旁的静止物体，如护栏、树木，并进行直线、曲线拟合获得道路边界，利用该边界来代替车道的几何特性；采用长距毫米波雷达获得前方道路边界来预估道路为直道或弯道，采用中距毫米波雷达拟合两侧道路边界，进而计算车辆偏航角。相比于基于机器视觉的车辆偏航角计算，本方法对于天气、光照、阴影、摄像头抖动及交通标志线等因素的抗干扰能力更强，有助于车道偏离预警***的准确决策。

Description

一种基于毫米波雷达的车辆偏航角计算方法

技术领域

本发明涉及一种基于毫米波雷达的车辆偏航角计算方法，属于智能汽车主动安全技术领域。

背景技术

根据交通部门官方显示，目前我国汽车保有量位居世界第一，同时车祸数量也占据世界的50％,尤其是高速公路上的重大恶性事故居高不下。根据美国交通部门的报告指出，50％左右的事故均与车辆无意识的车道偏离有关。车道偏离预警***(Lane DepartureWarning Syetem,简称：LDWS)正是为此发展的一种主动安全技术，对驾驶员无变道意图下的车道偏离现象作出预警(如声音、震动或灯光等)，以避免汽车之间发生追尾或碰撞。

现有的车道偏离预警***主要是基于机器视觉的图像处理技术，识别车道线、计算偏离预警指标作出决策。该类方法极易受天气、光照、阴影、摄像头抖动以及交通标志线等因素干扰，应用范围受限；其中的偏离预警指标，如基于车道偏离时间(Time to LaneCrossing,简称：TLC)的决策方法、基于横向位置和方向参数的决策方法等，都依赖车辆偏航角。因此，需要提高车辆偏航角的准确度和稳定性。

发明内容

本发明的目的在于提供了一种基于毫米波雷达的车辆偏航角计算方法，解决现有车道偏离预警***中测量车辆偏航角准确度低、稳定性差的技术问题。

为了解决上述技术问题，本发明采取如下技术方案：

一种基于毫米波雷达的车辆偏航角计算方法，包括如下步骤：

步骤一、搭建车辆偏航角计算控制***：包括设置在车辆上的一个长距毫米波雷达、两个中距毫米波雷达和控制器；长距毫米波雷达和中距毫米波雷达与控制器之间电连接；其中，长距毫米波雷达安装于车辆前保险杠正中间位置，用于获取车辆前方道路的信息；两个中距毫米波雷达分别安装于车辆前保险杠两侧位置，且关于长距毫米波雷达对称设置，用于获取车辆侧方道路信息；所述控制器中包括前方弯道预估模块、侧方道路边界拟合模块和车辆偏航角计算模块。一般情况下，本方法仅需一个中距毫米波雷达，另一个毫米波雷达仅为冗余设计。长距毫米波雷达的探测距离可达到250m，中距毫米波雷达的探测距离可达到150m。

步骤二、通过长距毫米波雷达获取车辆前方道路静止物体的位置信息，并传给控制器，前方弯道预估计算模块基于有效静止物体方法选取能表征道路走向的静止物体，并进行根据这些有效静止物体的位置坐标拟合出直线l₁来预估前方道路是否为弯道，为实际道路边界拟合作准备。所述静止物体为道路两侧的护栏、树木等。

步骤三、通过中距毫米波雷达获得车辆侧方道路静止物体的位置信息，并传给控制器，侧方道路边界拟合模块基于前方弯道预估模块获得前方道路特征信息，利用中距毫米波雷达所获得车辆侧方有效道路静止物体的位置信息，进行曲线或直线拟合作为实际道路边界；

步骤四、车辆偏航角计算模块分别针对直道和弯道两种情况，通过拟合的实际道路边界，计算出的车辆偏航角。

在车辆行驶过程中，首先利用长距毫米波雷达获取车辆前方道路的信息，前方弯道预估模块以此信息判断前方道路为弯道还是直道；由于长距毫米波雷达的可以检测到250米处的物体，因此在前方弯道预估模块判断的过程中，车辆继续向前运行一段距离，此时车辆距离长距毫米波雷达刚才获取的车辆前方道路两侧物体更近，然后中距毫米波雷达获得车辆侧方有效道路静止物体，并根据效道路静止物体的位置信息精确拟合出实际道路边界曲线。然后根据实际道路边界曲线，结合车辆当前前进方向，精确计算出当前车辆的偏航角，及时对车辆的前进方向作出调整，提高驾驶安全性。

随着传感器技术的飞速发展，毫米波雷达因其对天气、光照、阴影等因素抗干扰能力强，在智能汽车中有众多应用。另外，一般情况下，高速公路旁都有护栏或者树木，且跟车道方向一致，因而护栏或树木这样的静止物体能够反映车道线的几何特性,通过采用毫米波雷达来检测高速公路旁的静止物体，如护栏、树木，并进行直线、曲线拟合获得道路边界，利用该边界来代替车道的几何特性；采用长距毫米波雷达获得前方道路边界来预估道路为直道或弯道，采用中距毫米波雷达拟合两侧道路边界，进而计算车辆偏航角。相比于基于机器视觉的车辆偏航角计算，本方法对于天气、光照、阴影、摄像头抖动及交通标志线等因素的抗干扰能力更强，有助于车道偏离预警***的准确决策。

进一步改进，所述前方弯道预估模块通过如下步骤预估前方道路是否为弯道：

1)、通过长距毫米波雷达获取车辆前方静止物体的位置信息；

2)、根据前方静止物体的位置信息，获取有效静止物体群；

3)、选取静止物体数量最大的有效静止物体群作为候选目标，并根据候选目标中多个静止物体的位置信息拟合出一条表示前方道路边界的直线l₁；

4)、根据拟合直线l₁的斜率来判断前方道路特征信息，即为直道还是弯道。

进一步改进，以车辆上长距毫米波雷达的安装位置为坐标原点O，以车辆前进方向为Y轴，两个中距毫米波雷达的连线为X轴，建立自车直角坐标系XOY，所述有效静止物体群中的静止物体必须同时满足以下四个条件：(1)静止物体群中的静止物体在自车坐标系中Y轴方向的距离最大值y_max大于第一设定阈值；(2)静止物体群中的相邻静止物体在自车坐标系中Y轴方向的间距Δy_i大于第二设定阈值；(3)静止物体群中的相邻静止物体在自车坐标系中X轴方向的间距Δx_i小于第三设定阈值；(4)静止物体群中的静止物体数量n大于第四设定阈值，n为正整数，i＝1…….n；第i个静止物体在自车直角坐标系XOY中的位置坐标为(x_i，y_i)；第一设定阈值、第二设定阈值、第三设定阈值和第四设定阈值均为预先设定的数值。

进一步改进，设定车辆前方道路边界的拟合直线l₁斜率为k，第五设定阈值为k_t，则前方道路特征信息判断方法如下：

1)、若k为正，且k<k_t，则前方道路为右弯；

2)、若k为负，且abs(k)<k_t，则前方道路为左弯；

3)、其余情况均表明前方道路为为直道。

进一步改进，前方道路为弯道的情况下，车辆偏航角计算方法如下：

1)、采用回旋曲线模型拟合道路边界曲线，通过选取有效静止物体群，采用线性最小二乘法拟合出边界曲线l；

2)、过坐标原点O作曲线l的法线，法线与曲线l相交于点A，过点A作曲线l的切线l₂，过O作切线l₂的平行线l₃，则l₃与Y轴的夹角为车辆偏航角β。

进一步改进，前方道路为直道的情况下，车辆偏航角计算方法如下：通过选取有效静止物体群，采用最小二乘法拟合出边界直线l，利用直线l与Y轴相交得到夹角δ，再利用平行线定理可知，车辆偏航角β＝δ。

进一步改进，所述中距毫米波雷达根据如下步骤所获得车辆侧方有效道路静止物体的位置信息：

1)、先由前方道路弯向侧对应的中距毫米波雷达检测对应侧静止物体信息，并获取有效静止物体，如果存在，则选择物体数量多的群作为候选静止物体群来拟合曲线或直线；

2)、若是该侧因有车辆或其他障碍物遮挡导致有效静止物体检测失败，则切换至另一侧中距毫米波雷达来检测并拟合道路边界；

3)、若左右两侧均检测失败，则输出前一时刻的道路边界信息来代替当前道路边界信息。适应性强，确保准确拟合出路边界曲线。

本发明的有益效果如下：

1、受天气、光照、阴影、摄像头抖动及交通标志线等因素的影响远小于机器视觉，无论是雨雪、大雾天气或者是夜间行车，本方法都能正常工作，稳定性强。

2、基于长距毫米波雷达的弯道预估策略可较好的降低道路边界拟合计算量，节约计算资源，提升计算效率。

附图说明

图1为本发明的总体结构图。

图2为本发明中的长距毫米波雷达信息直线拟合及静止物体数量计算方法示意图。

图3为本发明的静止物体拟合道路边界的流程图。

图4为本发明中的直道下车辆偏航角计算示意图。

图5为本发明中的弯道下车辆偏航角计算示意图。

具体实施方式

为使本发明的目的和技术方案更加清楚，下面将结合本发明实施例对本发明的技术方案进行清楚、完整地描述。

如图1所示的针对高速公路路况下的一种基于毫米波雷达的车辆偏航角计算方法，包括三个模块：前方弯道预估模块、侧方道路边界拟合模块以及车辆偏航角计算模块。

本发明的前方弯道预估模块通过如下步骤预估前方道路是否为弯道：(1)通过长距毫米波雷达获取前方静止物体方位信息；(2)获取有效静止物体群；(3)选取数量最大的有效静止物体群作为候选目标，并采用直线来拟合前方道路；(4)判断前方道路特征信息，即为直道还是弯道。该模块执行的示意图如图2所示。

参考图2的上半幅示意图，所述的有效静止物体群必须满足以下四个条件才能表征道路边界：(1)静止物体群中的静止物体在自车坐标系中Y轴方向距离最大值y_max大于第一设定阈值30m；(2)静止物体群中的相邻静止物体在自车坐标系中Y轴方向的间距Δy_i大于第二设定阈值5m；(3)静止物体群中的相邻静止物体在自车坐标系中X轴方向的间距Δx_i小于第三设定阈值1m；(4)静止物体群中的静止物体数量n大于第四设定阈值3个。若满足以上所有条件，则命名为有效静止物体群1；如若有多个满足条件的静止物体群，则命名为有效静止群2～j。一般情况下，有效静止群不超过3个。如图2所示，长距毫米波雷达识别前方道路两边静止物体群，分别为静止物体群Ⅰ和静止物体群Ⅱ，可以看出两个群中满足上述前3个条件的静止物体数目分别为6、5，因而取静止物体群作为候选拟合点群，进行直线拟合。

本发明的前方道路特征信息判断方法的依据是：假设前方道路边界的拟合直线l₁斜率为k，第五设定阈值为k_t，那么存在(1)若k为正，且k<k_t，则前方道路为右弯；(2)若k为负，且abs(k)<k_t，则前方道路为左弯；(3)其余情况均为直道。

基于上述的有效静止物体群获取方法，本发明的中距毫米波雷达侧方道路边界拟合的流程如图3所示，仅以前方道路为左弯或直线的情况为例阐述：(1)由预估模块将前方道路特征信息反馈给控制器，控制器控制时间延时(由于长距毫米波雷达的可以检测到250米处的物体，因此在前方弯道预估模块判断的过程中，车辆继续向前运行一段距离，此时车辆距离长距毫米波雷达刚才获取的车辆前方道路两侧物体更近)，再将预估信息赋予侧方道路边界拟合模块，这样可以有效地反映自车实际所处车道情况；(2)由右侧中距毫米波雷达检测右侧静止物体信息，并获取有效静止物体，如果存在，则选择物体数量多的群作为候选静止物体群来拟合曲线或直线；(3)若是右侧因有车辆或其他障碍物遮挡导致有效静止物体检测失败，则切换至左侧中距毫米波雷达来检测并拟合道路边界；(4)若左右两侧均检测失败，则输出前一时刻的道路边界来代替当前时刻；(5)若是由预估模块得出前方道路为右弯情况，则仅需将图3中的右侧改为左侧，左侧改为右侧即可。

本发明的道路边界曲线拟合是采用回旋曲线模型，模型描述为：

y＝ax³+bx²+c

其中a、b、c为待求系数，将至少3个静止物体坐标代入上式，采用线性最小二乘法进行曲线拟合，具体如下：假设静止物体群的各点到道路曲线的距离偏差平方和为

为了使Q最小，对上式分别求a、b、c的偏导得到三式如下：

将上述三式转化为矩阵形式，则有

通过上式即可获得系数a、b、c，那么道路边界曲线方程即可得到。

本发明的车辆偏航角计算模块包括直道计算、弯道计算。

如图4所示为直道下的车辆偏航角计算示意图，通过选取有效静止物体群，采用最小二乘法拟合出边界直线l₄，利用直线l₄与Y轴相交得到夹角δ，再利用平行线定理可知，车辆偏航角β＝δ。

如图5所示为弯道下的车辆偏航角计算示意图，通过选取有效静止物体群，采用线性最小二乘法拟合出边界曲线l，利用坐标原点O作出曲线l的法线相交于点A，过点A作曲线l的切线l₂，将切线l₂沿法线OA平移到原点O，即可算出车辆偏航角β。

本发明是仅适用于高速公路上，因而可通过车速来自动开启本发明功能。本发明结构简单，计算量小，可以有效地弥补基于机器视觉的车辆偏航角计算方法对于天气、光照、阴影、摄像头抖动及交通标志线等因素的不稳定性。

本发明中未做特别说明的均为现有技术或者通过现有技术即可实现，而且本发明中所述具体实施案例仅为本发明的较佳实施案例而已，并非用来限定本发明的实施范围。即凡依本发明申请专利范围的内容所作的等效变化与修饰，都应作为本发明的技术范畴。

Claims (8)

1.一种基于毫米波雷达的车辆偏航角计算方法，其特征在于，包括如下步骤：

步骤一、搭建车辆偏航角计算控制***：包括设置在车辆上的一个长距毫米波雷达、两个中距毫米波雷达和控制器；长距毫米波雷达和中距毫米波雷达与控制器之间电连接；其中，长距毫米波雷达安装于车辆前保险杠正中间位置，用于获取车辆前方道路的信息；两个中距毫米波雷达分别安装于车辆前保险杠两侧位置，且关于长距毫米波雷达对称设置，用于获取车辆侧方道路信息；所述控制器中包括前方弯道预估模块、侧方道路边界拟合模块和车辆偏航角计算模块；

步骤二、通过长距毫米波雷达获取车辆前方道路静止物体的位置信息，并传给控制器，前方弯道预估计算模块基于有效静止物体方法选取能表征道路走向的静止物体，并进行根据这些有效静止物体的位置坐标拟合出直线l₁来预估前方道路是否为弯道；

步骤三、通过中距毫米波雷达获得车辆侧方道路静止物体的位置信息，并传给控制器，侧方道路边界拟合模块基于前方弯道预估模块获得前方道路特征信息，利用中距毫米波雷达所获得车辆侧方有效道路静止物体的位置信息，进行曲线或直线拟合作为实际道路边界；

步骤四、车辆偏航角计算模块分别针对直道和弯道两种情况，通过拟合出的实际道路边界，计算出的车辆偏航角。

2.如权利要求1所述的基于毫米波雷达的车辆偏航角计算方法，其特征在于，所述前方弯道预估计算模块通过如下步骤预估前方道路是否为弯道：

1)、通过长距毫米波雷达获取车辆前方静止物体的位置信息；

2)、根据前方静止物体的位置信息，获取有效静止物体群；

3)、选取静止物体数量最大的有效静止物体群作为候选目标，并根据候选目标中多个静止物体的位置信息拟合出一条表示前方道路边界的直线l₁；

4)、根据拟合直线l₁的斜率来判断前方道路特征信息，即为直道还是弯道。

3.如权利要求1或2所述的基于毫米波雷达的车辆偏航角计算方法，其特征在于，以车辆上长距毫米波雷达的安装位置为坐标原点O，以车辆前进方向为Y轴，两个中距毫米波雷达的连线为X轴，建立自车直角坐标系XOY，所述有效静止物体群中的静止物体必须同时满足以下四个条件：(1)静止物体群中的静止物体在自车坐标系中Y轴方向的距离最大值y_max大于第一设定阈值；(2)静止物体群中的相邻静止物体在自车坐标系中Y轴方向的间距Δy_i大于第二设定阈值；(3)静止物体群中的相邻静止物体在自车坐标系中X轴方向的间距Δx_i小于第三设定阈值；(4)静止物体群中的静止物体数量n大于第四设定阈值，n为正整数，i＝1…….n；第i个静止物体在自车直角坐标系XOY中的位置坐标为(x_i，y_i)；第一设定阈值、第二设定阈值、第三设定阈值和第四设定阈值均为预先设定的数值。

4.如权利要求3所述的基于毫米波雷达的车辆偏航角计算方法，其特征在于，设定车辆前方道路边界的拟合直线l₁斜率为k，第五设定阈值为k_t，则前方道路特征信息判断方法如下：1)、若k为正，且k<k_t，则前方道路为右弯；

2)、若k为负，且abs(k)<k_t，则前方道路为左弯；

3)、其余情况均表明前方道路为直道。

5.如权利要求4所述的基于毫米波雷达的车辆偏航角计算方法，其特征在于，前方道路为弯道的情况下，车辆偏航角计算方法如下：

1)、采用回旋曲线模型拟合道路边界曲线，通过选取有效静止物体群，采用线性最小二乘法拟合出边界曲线l；

2)、过坐标原点O作曲线l的法线，法线与曲线l相交于点A，过点A作曲线l的切线l₂，过O作切线l₂的平行线l₃，则l₃与Y轴的夹角为车辆偏航角β。

6.如权利要求4所述的基于毫米波雷达的车辆偏航角计算方法，其特征在于，前方道路为直道的情况下，车辆偏航角计算方法如下：通过选取有效静止物体群，采用最小二乘法拟合出边界直线l₄，利用直线l₄与Y轴相交得到夹角δ，再利用平行线定理可知，车辆偏航角β＝δ。

7.如权利要求6所述的基于毫米波雷达的车辆偏航角计算方法，其特征在于，所述中距毫米波雷达根据如下步骤所获得车辆侧方有效道路静止物体的位置信息：

1)、先由前方道路弯向侧对应的中距毫米波雷达检测对应侧静止物体信息，并获取有效静止物体，如果存在，则选择物体数量多的群作为候选静止物体群来拟合曲线或直线；

2)、若是该侧因有车辆或其他障碍物遮挡导致有效静止物体检测失败，则切换至另一侧中距毫米波雷达来检测并拟合道路边界；

3)、若左右两侧均检测失败，则输出前一时刻的道路边界信息来代替当前道路边界信息。

8.如权利要求7所述的基于毫米波雷达的车辆偏航角计算方法，其特征在于，所述侧方道路边界拟合模块拟合道路边界曲线采用回旋曲线模型，模型描述为：

y＝ax³+bx²+c；

其中a、b、c为待求系数，第i个静止物体在自车直角坐标系XOY中的位置坐标为(x_i，y_i)；

将至少3个静止物体坐标代入上式，采用线性最小二乘法进行曲线拟合，具体如下：假设静止物体群的各点到道路曲线的距离偏差平方和为：

为了使Q最小，对上式分别求a、b、c的偏导得到三式如下：

将上述三式转化为矩阵形式，则有：

通过上式即可获得系数a、b、c，那么道路边界曲线方程即可得到。