一种毫米波雷达安装角偏差在线标定方法及装置

1.本发明涉及本发明涉及自动驾驶传感器标定领域，尤其涉及一种毫米波雷达安装角偏差在线标定方法及装置。


背景技术：

2.毫米波前向雷达作为fcw、aeb、acc系统中的关键传感器，在汽车驾驶过程中可有效降低事故发生概率。前向毫米波雷达一般安装在车头中央，要求雷达法线与车辆中线重合。但是雷达在安装及后续使用过程中可能会出现一定的方位角偏差，导致目标测量角度偏差，严重的会导致目标的漏报、误报。为了不影响目标检测准确性，在使用过程中需要进行雷达安装方位角偏差的自动在线标定。
3.现有在线方位角偏差标定方法主要利用雷达检测长直护栏拟合直线方程，或人工设置角反等静态标志物利用测量的径向速度与方位角关系完成雷达安装方位角偏差的标定。
4.长直护栏拟合直线方程的方法需要假定自车行驶方向与护栏严格平行；
5.径向速度与方位角关系方法需要保证自车直线行驶，均对标定场景有较高要求，极易因为条件不能严格满足而引入误差。


技术实现要素：

6.本发明实施例提供一种毫米波雷达安装角偏差在线标定方法及装置，能有提效高安装角偏差在线标定结果的准确性。
7.一种毫米波雷达安装角偏差在线标定方法，包括以下步骤：
8.s1、从车身底盘获取自车运动信息，根据雷达传感器安装位置参数计算当前时刻雷达传感器自身横、纵向速度；
9.s2、获取当前时刻的雷达量测数据集；
10.s3、利用步骤s2获得的所述雷达量测数据集中的每个量测数据求雷达安装方位角偏差，得到第一雷达安装方位角偏差集合；
11.s4、对第一雷达安装方位角偏差集合进行随机抽取一致算法去除外点，得到由静止目标量测计算得到的第二雷达安装方位角偏差集合；
12.s5、对第二雷达安装方位角偏差集合中的偏差求算术平均，得到第一雷达安装方位角偏差标定值；
13.s6、重复步骤s1～s5三次，对三次得到的第一雷达安装方位角偏差标定值进行有效性判断，取中值作为第二雷达安装方位角偏差标定值，完成当前时刻所述雷达安装方位角偏差标定。
14.进一步的，所述自车运动信息包括自车速度与转向角速度；所述传感器安装位置参数包括雷达安装位置在自车坐标系中的横、纵向坐标；所述计算当前时刻雷达传感器自身横纵向速度具体包括：
15.通过以下公式计算雷达传感器自身纵向速度
16.通过以下公式计算雷达传感器自身横向速度
17.其中，v
ego
为自车速度；
18.ω为自车转向角速度；
19.a为雷达传感器安装位置纵坐标；
20.b为雷达传感器安装位置横坐标；
21.为雷达传感器纵向速度；
22.为雷达传感器横向速度。
23.进一步的，所述雷达量测数据包括目标检测点距离雷达距离、方位角以及相对径向速度；所述雷达量测数据集包括当前时刻检测到的多个雷达量测数据。
24.进一步的，步骤s3中，所述利用雷达量测数据集中的每个量测数据求雷达安装方位角偏差，具体包括：
25.通过以下公式计算安装方位角偏差：
26.所述第一雷达安装方位角偏差集合表示为
27.其中，δθi为利用量测i计算得到的方位角偏差；
28.为雷达传感器纵向速度；
29.为雷达传感器横向速度；
30.θi量测i的方位角；
31.为量测i的径向相对速度；
32.n
t
为量测个数。
33.进一步的，步骤s4中，所述对第一雷达安装方位角偏差集合进行随机抽样一致操作去除外点，得到由静止目标量测计算得到的第二雷达安装方位角偏差集合，具体包括：
34.所述随机抽样一致算法为一种通用的外点去除算法，可以从一组包含外点的观测数据集中，通过迭代方式估计数学模型的参数，它是一种不确定的算法，有一定的概率得出一个合理的结果，为了提高概率必须提高迭代次数；
35.所述第二雷达安装方位角偏差集合表示为
36.其中，n
ts
为通过随机抽取一致算法筛选出的由静止目标量测计算得到的雷达安装偏差角个数。
37.进一步的，步骤s6具体包括：
38.对三次重复得到的第一雷达安装方位角偏差标定值的集合表示为
39.所述有效性判断为如果条件成立则判断标定
成功，输出中值作为第二雷达安装方位角标定值；
40.其中th为判定门限，根据经验选取为0.02rad。
41.本发明进一步公开了一种雷达安装方位角偏差在线标定装置，包括：
42.第一计算模块、雷达数据获取模块、第二计算模块、第三计算模块、第四计算模块、判定输出模块，其中，
43.所述第一计算模块，用于根据从车身底盘获取自车运动信息，雷达传感器安装位置参数计算当前时刻雷达传感器自身横纵向速度；
44.所述雷达数据获取模块，用于获取当前时刻的雷达量测数据的集合；
45.所述第二计算模块，用于根据雷达量测数据集中的每个量测数据求雷达安装方位角偏差，得到第一雷达安装方位角偏差集合；
46.所述第三计算模块，用于对第一雷达安装方位角偏差集合进行随机抽取一致算法去除外点，得到由静止目标量测计算得到的第二雷达安装方位角偏差集合；
47.所述第四计算模块，对第二雷达安装方位角偏差集合中的偏差求算术平均，得到第一雷达安装方位角偏差标定值；
48.所述判定输出模块，用于对三次得到的第一雷达安装方位角偏差标定值进行有效性判断，取中值作为第二雷达安装方位角偏差标定值，完成当前时刻所述雷达安装方位角偏差标定。
49.通过实施本发明实施例具有如下有益效果：
50.本发明实施例提供了一种毫米波雷达安装角偏差在线标定方法及装置，所述方法在对道路模型进行更新时，首先从车身底盘获取自车运动信息，根据雷达传感器安装位置参数计算当前时刻雷达传感器自身横纵向速度，然后获取当前时刻的雷达量测数据的集合，利用雷达量测数据集中的每个量测数据求雷达安装方位角偏差，得到第一雷达安装方位角偏差集合，紧接着对第一雷达安装方位角偏差集合进行随机抽取一致算法去除外点，得到由静止目标量测计算得到的第二雷达安装方位角偏差集合，并求算术平均，得到第一雷达安装方位角偏差标定值，然后重复以上操作三次，对三次得到的第一雷达安装方位角偏差标定值进行有效性判断，最后取中值作为第二雷达安装方位角偏差标定值，完成当前时刻所述雷达安装方位角偏差标定。与现有技术相比，本发明在安装角偏差在线标定过程中，利用自车的速度和转向角速度信息得到雷达传感器的横纵向速度，在计算相对径向速度时考虑传感器横向速度，解决了目前基于径向相对速度和方位角的在线标定方法需要自车直线行驶，对标定场景要求较高，易因为标定实施时不能满足该假设引入较大误差；或者迟迟不能满足标定场景要求而造成在线标定时间较长的技术问题。
附图说明
51.图1是本发明一实施例提供的一种毫米波雷达安装角偏差在线标定方法的流程示意图。
52.图2是本发明一实施例提供的一种毫米波雷达安装角偏差在线标定装置的结构示意图。
具体实施方式
53.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
54.如图1所示，本发明一实施例提供了一种毫米波雷达安装角偏差在线标定方法，包括以下步骤：
55.步骤101:从车身底盘获取自车速度与转向角速度信息，根据雷达传感器安装位置参数计算当前时刻雷达横纵向速度。
56.步骤102:获取当前时刻的雷达量测数据的集合，所述雷达量测数据包括雷达量测的距离，方位及径向相对速度信息。
57.步骤103:利用雷达量测数据集中的每个量测数据求雷达安装方位角偏差，得到第一雷达安装方位角偏差集合。
58.步骤104:对第一雷达安装方位角偏差集合进行ransac操作去除外点，得到由静止目标检测计算得到的第二雷达安装方位角偏差集合。
59.步骤105:对第二雷达安装方位角偏差集合中的偏差求算术平均，得到第一雷达安装方位角偏差标定值。
60.步骤106:重复101～105步骤三次，对得到的第一雷达安装方位角偏差标定值进行有效性判断，取中值作为第二雷达安装方位角偏差标定值，完成安装方位角误差在线标定。
61.对于步骤101，具体的，从车身底盘获取自车运动信息包括车辆坐标系原点处(自车后轴中心)自车的速度v
ego
及转向角速度ω；雷达传感器在车辆坐标系下的安装位置坐包括雷达传感器距离车辆坐标系的横向距离a及纵向距离b；雷达传感的纵向速度由计算得到，横向速度由计算得到；
62.对于步骤102，具体的，获取当前时刻的雷达量测集i表示第i个雷达量测，n
t
为雷达量测个数，z＝[r,θ,vr]表示单个雷达量测数据，包括量测的距离r,方位角θ，径向速度vr需要说明的是，上述雷达量测集为雷达上报的当前时刻所有检测，包括雷达覆盖范围内的所有运动或静止目标的检测结果以及少量虚假检测。
[0063]
对于步骤103、在一个优选的实施例中，所述利用雷达量测数据集中的每个量测数据求雷达安装方位角偏差，具体包括：
[0064]
静止目标检测的相对径向速度可以表示为：
[0065][0066]
其中，为雷达传感器纵向速度；为雷达传感器横向速度；θi量测i的方位角；δθ为雷达传感器方位偏差真实值，为一个较小的角度值；为量测i的径向相对速度。
[0067]
利用cos(δθ)≈1,sin(δθ)≈δθ将上式简化
[0068]
[0069]
因此通过以下公式计算安装方位角偏差估计值：
[0070]
对所有n
t
个量测分别计算，得到第一雷达安装方位角偏差集合可以表示为其中，δθi为利用量测i计算得到的方位角偏差。
[0071]
对于步骤s104、在一个优选的实施例中，所述对第一雷达安装方位角偏差集合进行ransac操作去除外点，具体包括：
[0072]
第一雷达安装方位角偏差集是利用所有雷达量测速度计算得到的雷达安装方位角偏差集合，一般来说，其中静止目标占大多数。对于静止目标检测数据，得到的安装方位角偏差估计值δθi偏差估可以表示为δθ为偏差真值，为0均值的估计误差；对于运动目标检测或虚假检测数据，得到的安装方位角偏差估计值为为无规律的杂乱分布，因此可以采用随机抽取一致算法滤除这类外点，得到由静止目标检测计算得到的第二雷达安装方位角偏差集合其中，n
ts
为静止目标检测个数。
[0073]
对于步骤s105、在一个优选的实施例中，对第二雷达安装方位角偏差集合中的偏差求算术平均，得到第一雷达安装方位角偏差标定值
[0074]
对于步骤s106、在一个优选的实施例中，所述重复101～105步骤三次，对得到的第一雷达安装方位角偏差标定值进行有效性判断，取中值作为第二雷达安装方位角偏差标定值，具体包括：
[0075]
重复101～105步骤三次，将三次重复得到的第一雷达安装方位角偏差标定值的集合表示为
[0076]
判断三次结果中的最大值与最小值的差值绝对值是否小于固定门限0.02弧度，如果不小于该门限，则认为三次标定结果不一致，需要重新进行标定；如果小于该门限，则标定成功，选取三个结果中的中间值作为第二雷达安装方位角偏差标定值。
[0077]
通过本发明上述实施例所公开的雷达安装方位角偏差在线标定方法，在对当前时刻的雷达安装角偏差进行标定，引入了自车偏移角速度及雷达安装坐标计算得到的雷达传感器横向速度信息，即使在本车非直线行驶的情况下，也能准确的对静止目标检测的相对径向速度进行建模，从而利用雷达检测信息进行雷达安装方位角偏差的在线标定。
[0078]
在上述方法项实施例的基础上本发明对应提供了装置项实施例；
[0079]
如图2所示，本发明一实施例提供了一种雷达安装方位角偏差在线标定装置，包括：第一计算模块、雷达数据获取模块、第二计算模块、第三计算模块、第四计算模块、判定输出模块；
[0080]
所述第一计算模块，用于根据从车身底盘获取自车运动信息，雷达传感器安装位置参数计算当前时刻雷达传感器自身横纵向速度；
[0081]
所述雷达数据获取模块，用于获取当前时刻的雷达量测数据的集合；
[0082]
所述第二计算模块，用于根据雷达量测数据集中的每个量测数据求雷达安装方位
角偏差，得到第一雷达安装方位角偏差集合；
[0083]
所述第三计算模块，用于对第一雷达安装方位角偏差集合进行随机抽取一致算法去除外点，得到由静止目标量测计算得到的第二雷达安装方位角偏差集合；
[0084]
所述第四计算模块，对第二雷达安装方位角偏差集合中的偏差求算术平均，得到第一雷达安装方位角偏差标定值；
[0085]
所述判定输出模块，用于对三次得到的第一雷达安装方位角偏差标定值进行有效性判断，取中值作为第二雷达安装方位角偏差标定值，完成当前时刻所述雷达安装方位角偏差标定。
[0086]
需说明的是，上述装置项实施例是与本发明方法项实施例相对应的，其可以实现本发明任意一实施例所述的道路模型的更新方法，此外以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。另外，本发明提供的装置实施例附图中，模块之间的连接关系表示它们之间具有通信连接，具体可以实现为一条或多条通信总线或信号线。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
[0087]
以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。