一种基于毫米波车载雷达场景识别的航位修正方法与流程

1.本发明涉及航位推算技术领域，特别是涉及一种基于毫米波车载雷达场景识别的航位修正方法。


背景技术：

2.车辆定位的时候，gps可提供10hz的定位信息，但是每个gps信息来临的0.1s的间隔里面，车辆位置也有移动，尤其当车辆位于隧道等gps信号弱的环境中，车辆定位还会出现严重延时，因此，航位推算是保证车载定位系统在无gps信号或者gps信号弱的场景依旧精准定位的重要手段。
3.当前航位推算大多是在知道当前时刻的位置后，借助其他传感器估计本车航向角、车速等信息，推算下一个时刻的位置，而航向角通常是通过本车的横摆角速度随时间的积分获取。陀螺仪是车载定位系统常用的估算横摆角速度的传感器，其通过测量物体三轴的角速率估计车辆横摆角速度，但是受限于机械精度，角速度会存在微小的误差，因此横摆角速度误差导致航向角误差，且误差随时间积累越发的增大，因此在实际航位推算过程中需要对此误差进行校准和修正，当前航位推算校准模块，主要统计横摆角速度零偏，而此方法仅可校准角速度的固有偏差，对于实时误差无法进行校准，例如在本车高速变道场景，横摆角速度非常小，横摆角速度传感器的灵敏度已无法识别时，本车航向角不能准确推算，影响航位的推算。
4.由于航位推算的精度依赖于其他传感器如陀螺仪等估算的横摆角速度的精度，故此，其误差受限于机械制作精度，车辆在道路行驶过程姿态时变，在不同姿态下测量误差未知，不可实时补偿，航向角是横摆角速度时间的积分，误差也是时间的积分，因此导致航位推算的误差是随时间积累的。


技术实现要素：

5.本发明为克服上述现有技术中航位推算存在一定误差的问题，提供一种基于毫米波车载雷达场景识别的航位修正方法。
6.为解决上述技术问题，本发明的技术方案如下：
7.一种基于毫米波车载雷达场景识别的航位修正方法，包括步骤：
8.s10.获取本车车身信息，并根据本车车身信息推算下一帧本车的航位信息；同时，获取车身坐标系下雷达探测的目标点迹特征，并对其进行预处理得到绝对坐标系下的目标点迹；
9.s20.对获取的单帧目标点迹进行聚类，将聚类后的目标点迹集合边缘形成矩形框；
10.s30.将上一帧获得的矩形框与当前帧获得的矩形框进行类匹配，将匹配的目标点迹聚为一类，并更新当前帧的矩形框得到类矩形框；
11.s40.根据类矩形框进行特征提取，获取满足栏杆或者路沿特征的类矩形框，并对
其进行处理得到栏杆或者路沿目标；
12.s50.获取识别的栏杆或路沿目标与参考的栏杆或路沿目标之间的角度差，将其补偿至推算的下一帧本车的航位信息中，得到下一帧本车确定的航位信息。
13.进一步的，作为优选技术方案，步骤s10具体包括：
14.获取本车车身信息，以本车中心为原点建立车身坐标，同时设定参考原点，以参考原点为坐标原点建立绝对坐标系；
15.根据本车车身信息，获取绝对坐标系下当前帧本车的航位信息，并根据当前帧本车的航位信息推算绝对坐标系下下一帧本车的航位信息；
16.获取当前帧车身坐标系下雷达探测的目标点迹特征，根据绝对坐标系下当前帧本车的航位信息，获得绝对坐标系下的目标点迹。
17.进一步的，作为优选技术方案，获取的车身信息包括车辆位置、车速和横摆角速度；
18.下一帧本车的航位信息的推算通过以下公式获取：
19.θ＝θ0 θ
yaw
*t；
20.x＝x0 v*t*cos(θ0)；
21.y＝y0 v*t*sin(θ0)；
22.其中，[x0，y0，θ0]为当前帧本车的航位信息，[x，y，θ]为下一帧本车的航位信息，v为车速，θ
yaw
为横摆角速度，t为雷达刷新时间。
[0023]
进一步的，作为优选技术方案，绝对坐标系下的目标点迹通过以下公式获取：
[0024]
x
1-x*cos(o0) y*sin(o0) x0；
[0025]y1-x*cos(o0 90) y*cos(o0) y0；
[0026]
其中，[x0.y0，θ0]为当前帧本车的航位信息，[x，y]为当前帧车身坐标系下雷达探测的目标点迹特征，[x1，y1]为绝对坐标系下的目标点迹坐标。
[0027]
进一步的，作为优选技术方案，步骤s20具体包括：
[0028]
采用dbscan算法，根据栏杆和路沿聚类的动态波门完成对获取的单帧目标点迹进行聚类，将聚类后的目标点迹集合边缘形成矩形框。
[0029]
进一步的，作为优选技术方案，单帧目标点迹通过以下公式进行聚类：
[0030]
deltx1＝|delt x1*cos(θ)| |delt y1*sin(θ))|；
[0031]
delty1＝|delt x1*cos(θ 90)| |d delty1*cos(θ))|；
[0032]
其中，[x1，x1][y1，y1]表示聚类后的目标点迹集合边缘形成的矩形框，delt x1，delt y1，表示设置的栏杆、路沿聚类动态波门，delt x1＝1，delt y1＝3。
[0033]
进一步的，作为优选技术方案，步骤s30中，相匹配的目标点迹满足以下条件：
[0034]
x_cur1∈[x
1-delt x1，x1 delt x1]或者x_cur1∈[x
1-delt x1，x1 delt x1]；
[0035]
同时，或者y_cur1∈|y
1-delt y1，y1 delt y1；
[0036]
其中，delt x1，delt y1，表示栏杆、路沿聚类动态波门，[x1，x1] [y1，y1]表示上一帧的矩形框，[x_cur1，x_cur1] [y_cur1，y_cur1]表示当前帧的矩形框。
[0037]
进一步的，作为优选技术方案，步骤s40中具体包括：
[0038]
从类矩形框中获取类点的个数以及类长度，将类点个数满足第一预设阈值，且类长度满足第二预设阈值的类矩形框识别为栏杆或者路沿的边缘特征；
[0039]
对识别为栏杆或者路沿的边缘特征的类矩形框进行拟合，获取栏杆或者路沿目标的特征参数。
[0040]
进一步的，作为优选技术方案，获取的栏杆或者路沿目标的特征参数为栏杆或者路沿目标在绝对坐标系下所在的直线。
[0041]
进一步的，作为优选技术方案，步骤s50中识别的栏杆或路沿目标与参考的栏杆或路沿目标之间的角度差为栏杆或者路沿目标在绝对坐标系下所在的直线与参考的栏杆或路沿目标在绝对坐标系下所在的直线之间的夹角。
[0042]
与现有技术相比，本发明技术方案的有益效果是：
[0043]
本发明通过雷达识别的场景信息实现对车辆航位的修正，直接实时获取航向角偏差并进行补偿，与现有校准横摆角速度零偏从而达到航位修正的技术相比，不仅仅可以校准横摆角速度零偏导致的航向角偏差，而且可以校准横摆角速度精度误差导致的航向角偏差且校准难度有所下降，且与时间解耦合，不随着时间积累，进而提高航位推算精度。
附图说明
[0044]
图1为本发明流程框图。
[0045]
图2为本发明帧间类匹配示意图。
[0046]
图3为本发明特征提取示意图。
[0047]
图4为本发明航向角补偿示意图。
[0048]
附图仅用于示例性说明，不能理解为对本专利的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的；相同或相似的标号对应相同或相似的部件；附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制。
具体实施方式
[0049]
下面结合附图对本发明的较佳实施例进行详细阐述，以使本发明的优点和特征更易被本领域技术人员理解，从而对本发明的保护范围作出更为清楚的界定。。
[0050]
本发明实施例的附图中相同或相似的标号对应相同或相似的部件；在本发明的描述中，需要理解的是，若有术语“上”、“下”、“左”、“右”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制。
[0051]
此外，若有“第一”、“第二”等术语仅用于描述目的，主要是用于区分不同的装置、元件或组成部分(具体的种类和构造可能相同也可能不同)，并非用于表明或暗示所指示装置、元件或组成部分的相对重要性和数量，而不能理解为指示或者暗示相对重要性。
[0052]
实施例1
[0053]
本实施例公开一种基于毫米波车载雷达场景识别的航位修正方法，如图1所示，包括步骤：
[0054]
s10.获取本车车身信息，并根据本车车身信息推算下一帧本车的航位信息；同时，获取车身坐标系下雷达探测的目标点迹特征，并对其进行预处理得到绝对坐标系下的目标
点迹。
[0055]
本步骤具体包括：
[0056]
s101.获取本车车身信息，以本车中心为原点建立车身坐标，同时设定参考原点，以参考原点为坐标原点建立绝对坐标系。
[0057]
在本步骤中，获取的车身信息包括车辆位置[x0，y1，θ0]、车速v和横摆角速度θ
yaw
，其车辆位置包括车辆坐标[x0，y0]以及航向角θ0。
[0058]
假设当前帧获取的本车车身信息为初始信息，即，[x0，y0，θ0]＝[0，0，0]，那么，在车身坐标以及绝对坐标系下的当前帧本车的航位信息均为[x0，y0，θ0]。
[0059]
s102.根据本车车身信息，获取绝对坐标系下当前帧本车的航位信息，并根据当前帧本车的航位信息推算绝对坐标系下下一帧本车的航位信息。
[0060]
参见步骤s101，假设绝对坐标系下当前帧本车的航位信息为[x0，y0，θ0]，
[0061]
下一帧本车的航位信息的推算通过以下公式获取：
[0062]
θ＝θ0 θ
yaw
*t；
[0063]
x＝x0 v*t*cos(θ0)；
[0064]
y＝y0 v*t*sin(θ0)；
[0065]
其中，[x0，y0，θ0]为当前帧本车的航位信息，[x，y，θ]为下一帧本车的航位信息，v为车速，θ
yaw
为横摆角速度，t为雷达刷新时间。
[0066]
s103.获取当前帧车身坐标系下雷达探测的目标点迹特征，根据绝对坐标系下当前帧本车的航位信息，获得绝对坐标系下的目标点迹。
[0067]
在本步骤中，绝对坐标系下的目标点迹通过以下公式获取：
[0068]
x1＝x*cos(θ0) y*sin(θ0) x0；
[0069]
y1＝x*cos(θ0 90) y*cos(θ0) y0；
[0070]
其中，[x0，y0，θ0]为当前帧本车的航位信息，[x，y]为当前帧车身坐标系下雷达探测的目标点迹特征，[x1，y1]为绝对坐标系下的目标点迹坐标。
[0071]
s20.对获取的单帧目标点迹进行聚类，将聚类后的目标点迹集合边缘形成矩形框。
[0072]
本步骤具体为：采用dbscan算法，根据栏杆和路沿聚类的动态波门完成对获取的单帧目标点迹进行聚类，将聚类后的目标点迹集合边缘形成矩形框。
[0073]
例如，在本步骤中，设置的栏杆或路沿聚类动态波门为delt x1，delt y1，其中，delt x1＝1，delt y1＝3；
[0074]
单帧目标点迹通过以下公式进行聚类：
[0075]
deltx1＝|delt x1*cos((θ)| |delt y1*sin(θ))|；
[0076]
delty1＝|delt x1*cos(θ 90)| |d delt y1*cos(θ))|；
[0077]
聚类后的目标点迹集合边缘形成的矩形框，记录为x1，y1，其范围为[x1，x1][y1，y1]。
[0078]
s30.将上一帧获得的矩形框与当前帧获得的矩形框进行类匹配，将匹配的目标点迹聚为一类，并更新当前帧的矩形框得到类矩形框。
[0079]
在本步骤中，假设上一帧获得的矩形框为[x1，x1] [y1，y1]，当前帧获得的矩形框为[x_cur1，x_cur1][y_cur1y_cur1]，将[x1，x1] [y1，y1]与[x_cur1，x_cur1][y_cur1，y_cur1]
进行匹配，将满足以下条件的目标点迹视为相匹配，聚为一类：
[0080]
x_cur1∈[x
1-delt x1，x1 delt x1]或者x_cur1∈[x
1-delt x1，x1 delt x1]；
[0081]
同时，y_cur1∈[y
1-delt y1，y1 delt y1]或者y_cur1∈[y
1-delt y1，y1 delt y1；
[0082]
其中，delt x1，delt y1，表示栏杆、路沿聚类动态波门。
[0083]
如图2所示：将上一帧矩形框中与当前帧矩形框中相匹配的目标点迹更新至当前帧矩形框中，同步更新当前帧矩形框参数，进而得到类矩形框。
[0084]
s40.根据类矩形框进行特征提取，获取满足栏杆或者路沿特征的类矩形框，并对其进行处理得到栏杆或者路沿目标。
[0085]
本步骤具体包括：
[0086]
s401.从类矩形框中获取类点的个数以及类长度，将类点个数满足第一预设阈值，且类长度满足第二预设阈值的类矩形框识别为栏杆或者路沿的边缘特征。
[0087]
在本步骤中，假设类矩形框为[x1，x1][y1，y1]，其类点的个数为n，那么类长度l通过以下公式获得：
[0088]
将类点个数n满足第一预设阈值，且类长度l满足第二预设阈值的类矩形框识别为栏杆或者路沿的边缘特征。
[0089]
在本实施例中，第一预设阈值设置范围为8-12，第二预设阈值设置范围为4-6m，作为优选实施例，第一预设阈值设置为10，第二预设阈值设置为5m，故此，将满足n》10且l》5m的类矩形框识别为栏杆或者路沿的边缘特征。
[0090]
s402.对识别为栏杆或者路沿的边缘特征的类矩形框进行拟合，获取栏杆或者路沿目标的特征参数。
[0091]
在本步骤中，获取的栏杆或者路沿目标的特征参数为栏杆或者路沿目标在绝对坐标系下所在的直线。
[0092]
如图3所示，采用直线拟合方程，对识别为栏杆或者路沿的边缘特征的类矩形框进行直线拟合，如下：
[0093]
x＝ky b
[0094]
其中，x表示栏杆或者路沿目标在绝对坐标系下所在的直线，k表示当前栏杆的斜率,b表示栏杆在x轴截距。
[0095]
s50.获取识别的栏杆或路沿目标与参考的栏杆或路沿目标之间的角度差，将其补偿至推算的下一帧本车的航位信息中，得到下一帧本车确定的航位信息。
[0096]
本步骤中识别的栏杆或路沿目标与参考的栏杆或路沿目标之间的角度差为栏杆或者路沿目标在绝对坐标系下所在的直线与参考的栏杆或路沿目标在绝对坐标系下所在的直线之间的夹角。
[0097]
即，识别的栏杆目标与参考的栏杆目标之间的角度差为识别的栏杆目标在绝对坐标系下所在的直线与参考的栏杆目标在绝对坐标系下所在的直线之间的夹角；或者，识别的路沿目标与参考的路沿目标之间的角度差为识别的路沿目标在绝对坐标系下所在的直线与参考的路沿目标在绝对坐标系下所在的直线之间的夹角。
[0098]
如图4所示，假设，识别的栏杆或路沿目标与参考的栏杆或路沿目标之间的角度差为α，步骤s101中推算的下一帧本车的航位信息为[x，y，θ]，即，推算的下一帧本车的航向角为θ，那么，对本车的航向角进行补偿，即，下一帧本车确定的航向角为θ1＝θ α，故此，下一
帧本车确定的航位信息为[x，y，θ1]。
[0099]
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。