CN115166721B - 路侧感知设备中雷达与gnss信息标定融合方法及装置 - Google Patents
路侧感知设备中雷达与gnss信息标定融合方法及装置 Download PDFInfo
- Publication number
- CN115166721B CN115166721B CN202211075825.0A CN202211075825A CN115166721B CN 115166721 B CN115166721 B CN 115166721B CN 202211075825 A CN202211075825 A CN 202211075825A CN 115166721 B CN115166721 B CN 115166721B
- Authority
- CN
- China
- Prior art keywords
- gnss
- radar
- coordinate
- coordinate sequence
- sequence
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000007500 overflow downdraw method Methods 0.000 title claims description 6
- 238000013507 mapping Methods 0.000 claims abstract description 78
- 238000000034 method Methods 0.000 claims abstract description 29
- 230000004927 fusion Effects 0.000 claims abstract description 24
- 238000001514 detection method Methods 0.000 claims description 16
- 238000004590 computer program Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 3
- 230000008447 perception Effects 0.000 claims description 2
- 230000000875 corresponding effect Effects 0.000 description 22
- 230000008859 change Effects 0.000 description 6
- 230000009466 transformation Effects 0.000 description 5
- 230000033001 locomotion Effects 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 238000009795 derivation Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 230000002596 correlated effect Effects 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 239000013598 vector Substances 0.000 description 1
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
- G01S13/86—Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/02—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S13/00
- G01S7/40—Means for monitoring or calibrating
-
- G—PHYSICS
- G08—SIGNALLING
- G08G—TRAFFIC CONTROL SYSTEMS
- G08G1/00—Traffic control systems for road vehicles
- G08G1/01—Detecting movement of traffic to be counted or controlled
- G08G1/0104—Measuring and analyzing of parameters relative to traffic conditions
- G08G1/0108—Measuring and analyzing of parameters relative to traffic conditions based on the source of data
- G08G1/0116—Measuring and analyzing of parameters relative to traffic conditions based on the source of data from roadside infrastructure, e.g. beacons
-
- G—PHYSICS
- G08—SIGNALLING
- G08G—TRAFFIC CONTROL SYSTEMS
- G08G1/00—Traffic control systems for road vehicles
- G08G1/01—Detecting movement of traffic to be counted or controlled
- G08G1/042—Detecting movement of traffic to be counted or controlled using inductive or magnetic detectors
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Chemical & Material Sciences (AREA)
- Analytical Chemistry (AREA)
- Radar Systems Or Details Thereof (AREA)
Abstract
本发明公开一种路侧感知设备中雷达与GNSS信息标定融合方法及装置,该方法步骤包括:S01.分别获取雷达、GNSS接收设备对移动目标探测到的雷达坐标序列以及GNSS坐标序列;S02.将雷达坐标序列与GNSS坐标序列进行匹配拟合,以转换为起始时间以及帧频率一致,得到匹配拟合后雷达坐标序列、GNSS坐标序列;S03.根据雷达坐标与GNSS坐标之间的映射模型以及匹配拟合后雷达坐标序列、GNSS坐标序列之间的相似度,寻找出最优映射参数值;S04.根据最优映射参数值得到雷达坐标与GNSS坐标之间的映射关系,完成标定融合。本发明具有实现方法简单、成本低、操作便捷以及效率高等优点。
Description
技术领域
本发明涉及智慧交通技术领域,尤其涉及一种路侧感知设备中雷达与GNSS(Global Navigation Satellite System,全球导航卫星坐标***)信息标定融合方法及装置。
背景技术
在路侧端,智慧交通的基础即是对多传感器数据进行收集和融合,即将多种传感器收集得到的信息汇集标定到一个统一的坐标系下。如在雷达与视频信息融合当中,将毫米波雷达检测得到的目标信息标定融合到相机成像当中的目标上,或将视频检测到的目标信息融合到毫米波坐标系当中;在激光雷达与雷达的信息融合中,通过映射关系可以将激光雷达信息标定到毫米波雷达坐标系当中。
然而无论是将视频信息标定到雷达坐标系,还是红外、激光雷达等传感器的信息融合到毫米波雷达当中,仅仅是针对智慧路上固定单点设备上的传感器进行数据融合,就不能实现不同设备之间的传感器数据融合,而一条智慧公路上通常会有大量功能相同的设备,如果设备与设备之间不能进行信息融合,则不利于对整条公路作整体信息获取和规划。要实现整条公路的智慧化,则需要将所有单点设备的信息统一映射到同一个世界坐标系当中,也即统一映射到GPS或北斗等的GNSS坐标系。
针对于雷达坐标系转换成到GNSS坐标系,现有技术中通常是采用人工逐点标定的方式,如使用高精度GPS/北斗接收设备在雷达视场内对多个点同时标定GPS/北斗坐标信息,并且获得各点在当前雷达视场内相对于雷达的坐标位置,然后对各个点对应的GPS/北斗坐标和雷达坐标进行映射关系的拟合。上述方法不仅实现复杂,且标定效率非常低,单个固定单点设备的坐标标定时间甚至可能超过3个小时,若要实现一条智慧公路上多个点的标定需要消耗大量的时间与成本,就难以实现雷达高效的标定GPS/北斗坐标信息。
发明内容
本发明要解决的技术问题就在于:针对现有技术存在的技术问题,本发明提供一种实现方法简单、成本低、操作便捷以及效率高的路侧感知设备中雷达与GNSS信息标定融合方法及装置。
为解决上述技术问题,本发明提出的技术方案为:
一种路侧感知设备中雷达与GNSS信息标定融合方法,步骤包括:
S01.分别获取雷达、GNSS接收设备对移动目标探测到的雷达坐标序列以及GNSS坐标序列;
S02.将所述雷达坐标序列与所述GNSS坐标序列进行匹配拟合,以将所述雷达坐标序列与所述GNSS坐标序列变换为起始时间以及帧频率一致,得到匹配拟合后雷达坐标序列、GNSS坐标序列;
S03.根据雷达坐标与GNSS坐标之间的映射模型以及所述匹配拟合后雷达坐标序列、GNSS坐标序列之间的相似度,寻找出所述映射模型的最优映射参数值;
S04.根据所述最优映射参数值得到雷达坐标与GNSS坐标之间的映射关系,完成雷达坐标与GNSS坐标的标定融合。
进一步的,所述步骤S02包括:
S201.对雷达坐标序列的起始点进行对齐:获取所述雷达坐标序列中的起始点的位置、速度信息,根据所述起始点的位置、速度信息以及GNSS信号的接收帧时间、GNSS坐标序列的起始点时刻,获得起始点对齐后的新轨迹点序列;
S202.对所述起始点对齐后的新轨迹点序列的点进行插值,以插值形成帧频率Fg的新轨迹序列,所述帧频率Fg为GNSS坐标序列的帧频率,得到所述匹配拟合后雷达坐标序列。
进一步的,所述步骤S201中,获得的所述起始点对齐后的新轨迹点序列为:
其中,(
xr prefix (
ti),
yr prefix (
ti))表示在
ti时刻时对应目标在雷达坐标系中的位置
,TFg为GNSS信号的接收帧时间,
tr 1 为雷达坐标序列中起始点的时刻,
tg 1 为GNSS坐标序列中起始点的时刻,
xr 0 、
yr 0 分别为雷达坐标序列中起始点的横、纵坐标,
vxr 0 、
vyr 0 分别为雷达坐标序列中起始点的横向速度和纵向速度。
进一步的,所述步骤S202包括:
在tg2到tgN的时间范围内按照
TFg的时间间隔进行遍历取值,即为:
其中,
tj为取值结果,tg2表示GNSS坐标序列中第二个点Pg2对应的时刻,tgN表示GNSS坐标序列中最后一个点PgN对应的时刻;
tj时刻对应的目标的雷达坐标和速度分别表示为:
其中,
r k
、r k+1分别表示雷达坐标序列中第
k、k+1个点,
tr k
、tr k+1分别表示雷达坐标序列中第
k、k+1个点对应的时刻,
xr k
、yr k 分别表示雷达坐标序列中第
k个点对应的横、纵坐标,
vxr k
、vyr k 分别表示雷达坐标序列中第
k个点对应的横向速度和纵向速度。
进一步的,所述雷达坐标与GNSS坐标之间的映射模型使用纬度的缩放因子、GNSS坐标旋转到雷达坐标系的旋转角度构建得到,所述步骤S03具体寻找出所述纬度的缩放因子、所述旋转角度的最优值。
进一步的,所述雷达坐标与GNSS坐标之间的映射模型具体为:
其中,
S y 为所述纬度的缩放因子,γ为所述旋转角度,
xr i
、yr i 分别为雷达坐标序列中第
i个点的横、纵坐标,
xg i
、yg i 分别为GNSS 坐标序列中第
i个点的横、纵坐标,
xg 0
、yg 0 分别为GNSS 坐标序列中起始点的横、纵坐标。
进一步的,所述步骤S03中,通过计算所述匹配拟合后雷达坐标序列、所述GNSS坐标序列之间的二维曲线的皮尔逊相关系数,并在所述纬度的缩放因子的空间集合内以及所述旋转角度的空间集合内遍历,当所述二维曲线的皮尔逊相关系数最大时,对应得到所述最优映射参数值。
进一步的,所述二维曲线的皮尔逊相关系数按照下式计算得到:
其中,Costi表示第
i次计算得到的匹配拟合后雷达坐标序列、GNSS坐标序列之间二维曲线的皮尔逊相关系数,
xr newi
、yr newi 分别为匹配拟合后雷达坐标序列中第
i个点的横、纵坐标,
xg i
、yg i 分别为GNSS 坐标序列中第
i个点的横、纵坐标,分别为匹配拟合后雷达坐标序列中横、纵坐标均值,分别为GNSS坐标序列中横、纵坐标均值,
N表示坐标点数量。
一种路侧感知设备中雷达与GNSS信息标定融合装置,包括:
获取模块,用于分别获取雷达、GNSS接收设备对移动目标探测到的雷达坐标序列以及GNSS坐标序列;
匹配拟合模块,用于将所述雷达坐标序列与所述GNSS坐标序列进行匹配拟合,以将所述雷达坐标序列转换为与所述GNSS坐标序列的起始时间以及帧频率一致的坐标序列,得到匹配拟合后雷达坐标序列;
寻优模块,用于根据雷达坐标与GNSS坐标之间的映射模型以及所述匹配拟合后雷达坐标序列、所述GNSS坐标序列之间的相似度,寻找出所述映射模型的最优映射参数值;
标定融合模块,用于根据所述最优映射参数值得到雷达坐标与GNSS坐标之间的映射关系,完成雷达坐标与GNSS坐标的标定融合。
一种计算机装置,包括处理器以及存储器,所述存储器用于存储计算机程序,所述处理器用于执行所述计算机程序以执行如上述方法。
与现有技术相比,本发明的优点在于:
1、本发明通过分别获取对移动目标探测到的雷达坐标序列以及GNSS坐标序列,然后对雷达坐标序列进行匹配拟合,以使得起始时间以及帧频率与GNSS坐标序列一致,再根据雷达坐标与GNSS坐标之间的映射模型以及匹配拟合后雷达坐标序列、GNSS坐标序列之间的相似度寻找出最优映射参数值,从而得到雷达坐标与GNSS坐标之间的映射关系,能够在保证标定准确性的前提下,快速实现对单点固定交通场景下雷达视场内的坐标位置到GNSS坐标位置的一一对应映射,高效完成固定场景中雷达坐标系到GNSS坐标系的标定,且整个标定融合过程操作便捷。
2、本发明进一步通过先建立GNSS球面坐标系到GNSS平面坐标系的映射模型,再进一步得到GNSS平面坐标系到毫米波雷达坐标系映射模型,最终得到GNSS坐标和雷达坐标之间的相互映射关系模型,可以精准的表征GNSS坐标与雷达坐标之间的映射关系,实现GNSS坐标与雷达坐标之间的映射。
3、本发明进一步通过基于二维曲线皮尔逊相关系数准则获得曲线之间的相似性,结合轨迹曲线拟合以及遍历参数空间的方式,可以快速、精准的求解最优映射参数,进而高效得到雷达坐标系与GNSS坐标系之间的映射关系。
附图说明
图1是本实施例路侧感知设备中雷达与GNSS信息标定融合方法的实现流程示意图。
图2是本实施例实现雷达与GNSS信息标定融合的原理示意图。
图3是GNSS坐标系的检测原理示意图。
图4是本实施例中雷达坐标系和GNSS直角坐标系之间关系的原理示意图。
图5是本发明在具体应用实施例中实现雷达与GNSS信息标定融合的实现流程示意图。
具体实施方式
以下结合说明书附图和具体优选的实施例对本发明作进一步描述,但并不因此而限制本发明的保护范围。
如图1所示,本实施例路侧感知设备中雷达与GNSS信息标定融合方法的步骤包括:
S01.分别获取雷达(具体为毫米波雷达)、GNSS接收设备对移动目标探测到的雷达坐标序列以及GNSS坐标序列;
S02.将雷达坐标序列与所述GNSS坐标序列进行匹配拟合,以将雷达坐标序列与GNSS坐标序列变换为起始时间以及帧频率一致,得到匹配拟合后雷达坐标序列、GNSS坐标序列;
S03.根据雷达坐标与GNSS坐标之间的映射模型以及匹配拟合后雷达坐标序列、GNSS坐标序列之间的相似度,寻找出映射模型的最优映射参数值;
S04.根据最优映射参数值得到雷达坐标与GNSS坐标之间的映射关系,完成雷达坐标与GNSS坐标的标定融合。
本实施例通过分别获取对移动目标探测到的雷达坐标序列以及GNSS坐标序列,然后对雷达坐标序列进行匹配拟合,以使得起始时间以及帧频率与GNSS坐标序列一致,再根据雷达坐标与GNSS坐标之间的映射模型以及匹配拟合后雷达坐标序列、GNSS坐标序列之间的相似度寻找出最优映射参数值,从而得到雷达坐标与GNSS坐标之间的映射关系,基于该映射关系即可快速实现将雷达探测的目标位置信息快速映射为GNSS信息(GPS/北斗坐标信息),即将雷达提取到的交通路面上各类目标的相对坐标映射为GNSS坐标系下的绝对位置坐标。
如图2所示,本实施例在路侧感知设备中的单点固定交通场景环境中,由雷达传感器设备实时获取GNSS卫星授时,同时在GNSS坐标接收视场范围内,移动目标本身携带GNSS信号接收器实时接收GNSS信号,在视场范围内毫米波雷达实时获取移动目标的位置和速度信息,以同时获取目标的雷达坐标信息和GNSS坐标信息。上述移动目标包括但不限于人、汽车等场景移动目标。仅需要通过一个移动的目标在雷达视场内迅速跑动穿过即可完成上述标定融合。
本实施例步骤S01中获取雷达坐标信息的详细步骤包括:
S101.雷达按照预设的帧时间间隔获取得到移动目标的位置和速度信息,得到多个雷达目标轨迹点。
如图2所示,移动目标在雷达视场范围内进行“S”形线路运动,由毫米波雷达按照预设的帧时间间隔获取得到目标的位置和速度信息,经一系列运动之后获取得到移动目标在雷达坐标系当中的轨迹PR,PR由一系列点组成,即为:
(1)
其中,
Pr i 表示第
i个点,
Pr i 包含该移动目标当前的位置、速度和当前时刻,表示如下:
(2)
其中,
tr i 表示当前雷达接收该目标的时刻,即为雷达目标序列中第
i个点对应的时刻,
xr i
、yr i 分别表示当前时刻移动目标在雷达坐标系中的横向坐标、纵向坐标,即为雷达目标序列中第
i个点对应的横向坐标、纵向坐标,
vxr i
、vyr i 分别表示当前时刻移动目标在雷达坐标中的横向速度和纵向速度,即为雷达目标序列中第
i个点对应的横向速度和纵向速度。
S102.根据步骤S101获取得到的雷达目标轨迹点进行目标跟踪,最终得到雷达目标序列。
由于雷达视场内任何植物可能收到风催动等干扰,也可能产生杂乱的目标,上述目标的出现会影响雷达对真实目标移动轨迹的提取,因此需要进行进一步的目标跟踪。本实施例基于目标当前的
Pr i 信息,结合卡尔曼滤波算法进一步跟踪以滤除虚警,最终得到多个跟踪之后的目标轨迹点为
PR 1
,
PR 2
, … ,
PR N。
根据移动目标和杂波目标对应的特点可知,移动目标移动过程连续,连续检测的时间最长,且检测得到的次数最多,即轨迹最长,本实施例进一步配置移动目标的轨迹索引序号满足如下条件:
(3)
其中,
length()表示求取轨迹的长度,表示求取最大值对应的索引。
按照上述步骤即求取得到移动目标在雷达坐标系下的对应移动轨迹
PR 1
,
PR 2
, … ,
PR N。
本实施例步骤S01中具体采用高精度RTK(real-timekinematic,实时动态)设备进行GNSS坐标信息的获取,可以获取厘米级精度的GNSS坐标。具体地,当目标在移动的时候,携带RTK设备,在移动的同时实时接收GNSS信号,从而确定目标在当前时刻移动到的对应位置的GNSS坐标。具体获取得到的GNSS坐标轨迹如下:
(4)
其中,
Pg i 表示GNSS目标序列中第
i个点,
Pg i 包含该移动目标当前的GNSS的经纬度位置、当前时刻,表示如下:
(5)
其中,
tg i 表示当前移动目标接收到的GNSS时刻,
xg i 和
yg i 分别表示当前时刻移动目标的经度和纬度坐标。
雷达坐标轨迹PR和GNSS坐标轨迹PG之间的坐标并不是完全的一一对应的关系,包括:
1)雷达视场区域和GNSS信号接收区域是不一致的,GNSS在任何位置上都能接收得到,而雷达的视场区域显然会存在信号接收盲区的,导致雷达坐标轨迹与GNSS坐标轨迹的起始点并不是对齐的。如图2所示,在雷达安装设备的垂直下方临近部分为雷达接收盲区,移动目标如果处在此部分盲区内,则无法接收到位置信息,此时只能接收到GNSS坐标信息;
2)由于雷达信号接收的帧频率和GNSS信号接收的帧频率不一致的,会导致雷达信号与GNSS信号在位置上也不是一一对应的关系。
本实施例考虑上述雷达坐标轨迹和GNSS坐标轨迹之间并不对应的特性,进一步对步骤S01获取的雷达坐标轨迹进行匹配拟合,以使得雷达坐标轨迹与GNSS坐标轨迹的起始点对齐且帧频率一致,进而使得可以通过匹配的雷达坐标轨迹与GNSS坐标轨迹之间的相似度得到雷达坐标系与GNSS坐标系之间的映射关系。
本实施例中,步骤S02具体包括:
S201.对雷达坐标序列的起始点进行对齐:获取雷达坐标序列中的起始点的位置、速度信息,根据起始点的位置、速度信息以及GNSS信号的接收帧时间、GNSS坐标序列的起始点时刻,获得起始点对齐后的新轨迹点序列;
S202.对起始点对齐后的新轨迹点序列的点进行插值,以插值形成帧频率Fg的新轨迹序列,帧频率Fg为GNSS坐标序列的帧频率,得到匹配拟合后雷达坐标序列。
由于获取的雷达目标的轨迹点包含了时间、位置、速度等信息,因此可以将雷达轨迹插值成和GNSS轨迹起始时间一致、帧频率一致的新轨迹。在具体应用实施例中,假定雷达信号接收的帧时间为TFr,帧频率为Fr,则有TFr=1/Fr,GNSS信号的接收帧时间为TFg,帧频率为Fg,则有TFg=1/Fg。首先对雷达坐标轨迹的起始点进行对齐,未对齐前的雷达轨迹PR的起始点时刻为PR.Pr1.tr1,表示PR轨迹的第一个点Pr1的时刻值为tr1,GNSS轨迹PG的起始点时刻为PG.Pg1.tg1,表示PG轨迹的第一个点Pg1的时刻值为tg1。利用雷达的起始点的位置和速度,往前推导得到起始点之前的雷达坐标位置,关系如下:
(6)
其中,(
xr prefix (
ti),
yr prefix (
ti))表示在
ti时刻时对应目标在雷达坐标系中的位置
,TFg为GNSS信号的接收帧时间,
tr 1 为雷达坐标序列中起始点的时刻,
tg 1 为GNSS坐标序列中起始点的时刻,
xr 0 、
yr 0 分别为雷达坐标序列中起始点的横、纵坐标,
vxr 0 、
vyr 0 分别为雷达坐标序列中起始点的横向速度和纵向速度。
通过上式(6)即得到起始点对齐后的新轨迹点序列为:
。
然后对雷达轨迹内的点重新进行插值,以将原来的帧频率Fr重新插值为帧频率Fg的新轨迹,具体在tg2到tgN的时间范围内按照TFg的时间间隔进行遍历取值,即为:
其中,
tj为取值结果,tg2表示GNSS坐标序列中第二个点Pg2对应的时刻,tgN表示GNSS坐标序列中最后一个点PgN对应的时刻。
当
tj处于雷达轨迹前后两个坐标时刻之间时,如处于
tr k
、tr k+1之间,则
tj时刻对应的目标的雷达坐标和速度分别表示为:
(7)
(8)
(9)
(10)
其中,
r k
、r k+1分别表示雷达坐标序列中第
k、k+1个点,
tr k
、tr k+1分别表示雷达坐标序列中第
k、k+1个点对应的时刻,
xr k
、yr k 分别表示雷达坐标序列中第
k个点对应的横、纵坐标,
vxr k
、vyr k 分别表示雷达坐标序列中第
k个点对应的横向速度和纵向速度。
通过上述步骤即可以得到
tj时刻下对应的雷达轨迹点,也即经重建之后得到的新的匹配拟合后雷达坐标轨迹PRnew,该匹配拟合后雷达坐标轨迹PRnew不仅起始点与GNSS坐标轨迹一致,而且帧频率也是一致的,以使得后续可以根据轨迹相似度拟合求解出映射关系。
本实施例上述通过以GNSS坐标序列为基准,将雷达坐标序列与GNSS坐标序列进行匹配拟合,以使得雷达坐标序列与GNSS坐标序列的起始点对齐以及帧频率一致,实现雷达与GNSS坐标序列之间的匹配。可以理解的是,当然也可以采用其他基准进行匹配拟合,以使得雷达坐标序列与GNSS坐标序列变换为起始点与帧频率一致的序列。
本实施例构建映射模型的原理详细为:
GNSS坐标系是一种球面坐标系,检测区域在GNSS坐标系当中如图3所示,其中检测区域与地轴之间的夹角为θ,与地心之间的夹角为δ,地球半径为R,检测区域横截圆面的半径为L,由经纬度关系可知,L与R的关系如下:
(11)
其中,
θ lat 表示检测区域内检测点的纬度值;可知纬度越高,L越小;
如图3所示,依据扇形公式有:
(12)
雷达视场检测区域最远距离通常在1500m以内,即h1和h2的范围在1.5km以内,因此,当纬度变化范围约为30-530时,的值极其小(万分之一量级),依据泰勒展开公式,可以认为
θ≈sin
θ,即有:
(13)
同理,有:
(14)
即雷达视场内的检测区域弧线长度h1和h2可以近似等于对应圆心角的弦线段,角度的线性变换对应于距离的线性变换,也即可以认为在雷达视场范围内,GNSS坐标中经度和纬度都是线性变化的,可以表示为平面直角坐标系,仅是纵轴(纬度)和横轴(经度)的线性因子不相同。
由上推导可知,经度的线性变换因子
S x 与纬度的线性变换因子(缩放因子)
S y 关系如下:
(15)
因此雷达坐标的距离最小变化单元对应GNSS坐标的变化单元,即有:,其中,∆h表示前后帧时间内目标移动的距离变化,∆δ表示前后帧时间内目标移动的经纬度变化值,因此缩放因子
S y 满足如下关系:
(16)
由于实际求解过程并不能获取具体的∆h值,一般跟雷达的距离分辨率有关,因此引入k系数,根据实际测试范围k一般在0.2m~2m的范围。
以毫米波雷达设备所在位置为原点,法线方向为纵轴方向的雷达坐标系和GNSS直角坐标系之间的关系如图4所示,由图4可知,雷达坐标系中的P点坐标可以看成是由GNSS坐标系旋转之后得到的,轨迹点的位置没有变化,但表达的坐标系发生变化,则根据该坐标的变换,可以得到P点雷达坐标与GNSS坐标之间的关系如下:
(17)
其中,(
xr i
、yr i )表示雷达视场内的第
i个点坐标,(
xg i
、yg i )表示对应的GNSS经度和纬度坐标(横纵坐标),(
xg 0
、yg 0 )表示雷达设备安装点对应的GNSS经度和纬度坐标(横纵坐标),(
S x ,
S y )分别表示经度方向和纬度方向上的缩放因子,γ表示GNSS坐标系旋转到雷达坐标系的旋转角度。
进一步可以得到:
(18)
其中,。
进一步也可以由雷达坐标映射到GNSS坐标系当中,即为:
(19)
由上述式(18)、(19)即可得到雷达坐标系与GNSS坐标系之间的映射模型。由式(18)、(19)可知,只要获得精确的缩放因子
S y 和旋转角度γ,就可以将GNSS坐标映射到雷达坐标以及将雷达坐标映射到GNSS坐标。
本实施例通过先建立GNSS球面坐标系到GNSS平面坐标系的映射模型,再进一步得到GNSS平面坐标系到毫米波雷达坐标系映射模型,最终得到GNSS坐标和雷达坐标之间的相互映射关系模型,可以精准的表征GNSS坐标与雷达坐标之间的映射关系,实现GNSS坐标与雷达坐标之间的映射。
由上述可知,只要求得精确的缩放因子
S y 和旋转角度γ,便可以得到雷达坐标与GNSS坐标之间的映射关系,其中当
S y 和γ越接近真实值时,GNSS坐标的映射就越接近对应的雷达坐标,即匹配拟合后GNSS坐标序列PGnew与雷达坐标序列PRnew就会越相似,当
S y 和γ为理想值时,此时PGnew和PRnew则完全相同。采用皮尔逊相关系数或者余弦相关系数法等可以衡量两个序列之间的相似性,但通常只适用于一维向量,对于二维曲线则无法适用。本实施例步骤S03中,通过在经典皮尔逊相关系数法的基础上,使用二维曲线的皮尔逊相关系数,以作为衡量PGnew和PRnew相似性准则,得到匹配拟合后雷达坐标序列、GNSS坐标序列之间的相似度。
本实施例中,二维曲线的皮尔逊相关系数具体按照下式计算得到:
(20)
其中,Costi表示第
i次计算得到的匹配拟合后雷达坐标序列、GNSS坐标序列之间二维曲线的皮尔逊相关系数,
xr newi
、yr newi 分别为匹配拟合后雷达坐标序列中第
i个点的横、纵坐标,
xg i
、yg i 分别为GNSS 坐标序列中第
i个点的横、纵坐标,分别为匹配拟合后雷达坐标序列中横、纵坐标均值,分别为GNSS坐标序列中横、纵坐标均值,
N表示坐标点数量。
上述得到的相关系数Costi的取值范围为-1~1,其中0表示两条曲线PGnew和PRnew没有相关性,-1 表示两者完全负相关,+1则表示两者完全相同。由相关系数Costi的大小即可以表征的匹配拟合后雷达坐标序列、GNSS坐标序列之间的相似程度,进而可以求取出映射模型中最优的参数值。
本实施例步骤S03中,具体通过在纬度的缩放因子的空间集合内以及旋转角度的空间集合内遍历,当二维曲线的皮尔逊相关系数最大时,对应得到纬度的缩放因子、旋转角度的最优值。旋转角度γ遍历的空间范围为[-π,π],缩放因子
S y 遍历的空间范围为,在具体应用实施例中,将旋转角度γ i 和缩放因子
S yi 在各自空间内等间隔分布,然后遍历所有参数,对应每一次遍历参数(
S yi ,γ i ),可以求得一个相似度值数Cost(
S yi ,γ i ),最终求得的最优映射参数即为:
(21)
本实施例通过基于二维曲线皮尔逊相关系数准则获得曲线之间的相似性,结合轨迹曲线拟合以及遍历参数空间的方式,可以快速、精准的寻找最优映射参数,进而高效得到雷达坐标系与GNSS坐标系之间的映射关系。
基于上述得到的雷达坐标系与GNSS坐标系之间的映射关系,即可以实现雷达坐标与GNSS坐标系之间的映射。本实施例具体以GNSS坐标为基准,将获取到的雷达坐标序列按照式(6)~(10)进行匹配拟合,以使得与GNSS坐标的起始点对齐以及帧频率一致,得到匹配后的雷达坐标序列PRnew,然后按照式(20)、(21)求解出最优映射参数(
S yopt ,γ opt )后,按照式(18)将GNSS坐标转换至雷达坐标系,实现GNSS坐标与雷达坐标的融合。
如图5所示,本发明在具体应用实施例中,先按照上述建立坐标信息获取模型以分别获取雷达、GNSS接收设备对移动目标探测到的雷达坐标轨迹序列以及GNSS坐标轨迹序列,然后分别对雷达坐标轨迹序列以及GNSS坐标轨迹序列进行跟踪检测,最终得到雷达坐标序列以及GNSS坐标序列,然后将雷达坐标序列与GNSS坐标序列进行匹配拟合,以使得起始点以及帧频率一致,然后再基于二维曲线皮尔逊相关系数准则在纬度的缩放因子、旋转角度的空间集合内进行遍历,求取出最优映射参数值,得到雷达坐标系与GNSS坐标系之间的映射关系,无需人工参与即可快速完成雷达与GNSS的标定融合。
本实施例路侧感知设备中雷达与GNSS信息标定融合装置包括:
获取模块,用于分别获取雷达、GNSS接收设备对移动目标探测到的雷达坐标序列以及GNSS坐标序列;
匹配拟合模块,用于将雷达坐标序列与所述GNSS坐标序列进行匹配拟合,以将雷达坐标序列与GNSS坐标序列变换为起始时间以及帧频率一致,得到匹配拟合后雷达坐标序列、GNSS坐标序列;
寻优模块,用于根据雷达坐标与GNSS坐标之间的映射模型以及匹配拟合后雷达坐标序列、GNSS坐标序列之间的相似度,寻找出映射模型的最优映射参数值;
标定融合模块,用于根据最优映射参数值得到雷达坐标与GNSS坐标之间的映射关系,完成雷达坐标与GNSS坐标的标定融合。
本实施例路侧感知设备中雷达与GNSS信息标定融合装置与上述路侧感知设备中雷达与GNSS信息标定融合方法为一一对应,在此不再一一赘述。
本实施例还提供计算机装置,包括处理器以及存储器,存储器用于存储计算机程序,处理器用于执行计算机程序以执行如上述路侧感知设备中雷达与GNSS信息标定融合方法。上述计算机装置可以直接配置在路侧感知设备中,也可以设置在于路侧感知设备连接的控制端,以实现路侧感知设备中雷达传感器与GNSS接收设备的信息融合。
上述只是本发明的较佳实施例,并非对本发明作任何形式上的限制。虽然本发明已以较佳实施例揭露如上,然而并非用以限定本发明。因此,凡是未脱离本发明技术方案的内容,依据本发明技术实质对以上实施例所做的任何简单修改、等同变化及修饰,均应落在本发明技术方案保护的范围内。
Claims (8)
1.一种路侧感知设备中雷达与GNSS信息标定融合方法,其特征在于,步骤包括:
S01.分别获取雷达、GNSS接收设备对移动目标探测到的雷达坐标序列以及GNSS坐标序列;
S02.将所述雷达坐标序列与所述GNSS坐标序列进行匹配拟合,以将所述雷达坐标序列与所述GNSS坐标序列变换为起始时间以及帧频率一致,得到匹配拟合后雷达坐标序列、GNSS坐标序列;
S03.根据雷达坐标与GNSS坐标之间的映射模型以及所述匹配拟合后雷达坐标序列、GNSS坐标序列之间的相似度,寻找出所述映射模型的最优映射参数值,所述雷达坐标与GNSS坐标之间的映射模型使用纬度的缩放因子、GNSS坐标旋转到雷达坐标系的旋转角度构建得到,所述步骤S03具体寻找出所述纬度的缩放因子、所述旋转角度的最优值;
所述雷达坐标与GNSS坐标之间的映射模型具体为:
xg′i=(xgi-xg0)cosθlat,yg′i=(ygi-yg0)
ygi=yg′i+yg0
其中,Sy为所述纬度的缩放因子,γ为所述旋转角度,xri、yri分别为雷达坐标序列中第i个点的横、纵坐标,xgi、ygi分别为GNSS坐标序列中第i个点的横、纵坐标,xg0、yg0分别为GNSS坐标序列中起始点的横、纵坐标,θlat表示检测区域内检测点的纬度值;
S04.根据所述最优映射参数值得到雷达坐标与GNSS坐标之间的映射关系,完成雷达坐标与GNSS坐标的标定融合。
2.根据权利要求1所述的路侧感知设备中雷达与GNSS信息标定融合方法,其特征在于,所述步骤S02包括:
S201.对雷达坐标序列的起始点进行对齐:获取所述雷达坐标序列中的起始点的位置、速度信息,根据所述起始点的位置、速度信息以及GNSS信号的接收帧时间、GNSS坐标序列的起始点时刻,获得起始点对齐后的新轨迹点序列;
S202.对所述起始点对齐后的新轨迹点序列的点进行插值,以插值形成帧频率Fg的新轨迹序列,所述帧频率Fg为GNSS坐标序列的帧频率,得到所述匹配拟合后雷达坐标序列。
3.根据权利要求2所述的路侧感知设备中雷达与GNSS信息标定融合方法,其特征在于,所述步骤S201中,获得的所述起始点对齐后的新轨迹点序列为:
xrprefix(ti)=xr0+(ti-tr1)×vxr0
yrprefix(ti)=yr0+(ti-tr1)×vyr0
ti=tr1,tr1-TFg1,tr1-2×TFg1,...,tg1
其中,xrprefix(ti),yrprefix(ti)分别表示在ti时刻时对应目标在雷达坐标系中的横坐标位置、纵坐标位置,TFg1为GNSS信号的接收帧时间,tr1为雷达坐标序列中起始点的时刻,tg1为GNSS坐标序列中起始点的时刻,xr0、yr0分别为雷达坐标序列中起始点的横、纵坐标,vxr0、vyr0分别为雷达坐标序列中起始点的横向速度和纵向速度。
4.根据权利要求2所述的路侧感知设备中雷达与GNSS信息标定融合方法,其特征在于,所述步骤S202包括:
在tg2到tgN的时间范围内按照TFg的时间间隔进行遍历取值,即为:
tj=tg2,tg2+TFg1,tg2+2×TFg1,...,tgN
其中,tj为取值结果,tg2表示GNSS坐标序列中第二个点Pg2对应的时刻,TFg1为GNSS信号的接收帧时间,tgN表示GNSS坐标序列中最后一个点PgN对应的时刻;
tj时刻对应的目标的雷达坐标和速度分别表示为:
xrnew(tj)=xrk+(tj-trk)×xvrnew(tj)
yrnew(tj)=yrk+(tj-trk)×yvrnew(tj)
其中,rk、rk+1分别表示雷达坐标序列中第k、k+1个点,trk、trk+1分别表示雷达坐标序列中第k、k+1个点对应的时刻,xrk、yrk分别表示雷达坐标序列中第k个点对应的横、纵坐标,vxrk、vyrk分别表示雷达坐标序列中第k个点对应的横向速度和纵向速度。
5.根据权利要求1所述的路侧感知设备中雷达与GNSS信息标定融合方法,其特征在于,所述步骤S03中,通过计算所述匹配拟合后雷达坐标序列、GNSS坐标序列之间的二维曲线的皮尔逊相关系数,并在所述纬度的缩放因子的空间集合内以及所述旋转角度的空间集合内遍历,当所述二维曲线的皮尔逊相关系数最大时,对应得到所述最优映射参数值。
7.一种路侧感知设备中雷达与GNSS信息标定融合装置,其特征在于,包括:
获取模块,用于分别获取雷达、GNSS接收设备对移动目标探测到的雷达坐标序列以及GNSS坐标序列;
匹配拟合模块,用于将所述雷达坐标序列与所述GNSS坐标序列进行匹配拟合,以将所述雷达坐标序列与所述GNSS坐标序列变换为起始时间以及帧频率一致,得到匹配拟合后雷达坐标序列、GNSS坐标序列;
寻优模块,用于根据雷达坐标与GNSS坐标之间的映射模型以及所述匹配拟合后雷达坐标序列、所述GNSS坐标序列之间的相似度,寻找出所述映射模型的最优映射参数值,所述雷达坐标与GNSS坐标之间的映射模型使用纬度的缩放因子、GNSS坐标旋转到雷达坐标系的旋转角度构建得到,所述寻优模块具体寻找出所述纬度的缩放因子、所述旋转角度的最优值;所述雷达坐标与GNSS坐标之间的映射模型具体为:
xg′i=(xgi-xg0)cosθlat,yg′i=(ygi-yg0)
ygi=ygi+yg0
其中,Sy为所述纬度的缩放因子,γ为所述旋转角度,xri、yri分别为雷达坐标序列中第i个点的横、纵坐标,xgi、ygi分别为GNSS坐标序列中第i个点的横、纵坐标,xg0、yg0分别为GNSS坐标序列中起始点的横、纵坐标,θlat表示检测区域内检测点的纬度值;
标定融合模块,用于根据所述最优映射参数值得到雷达坐标与GNSS坐标之间的映射关系,完成雷达坐标与GNSS坐标的标定融合。
8.一种计算机装置,包括处理器以及存储器,所述存储器用于存储计算机程序,其特征在于,所述处理器用于执行所述计算机程序以执行如权利要求1~6中任意一项所述方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211075825.0A CN115166721B (zh) | 2022-09-05 | 2022-09-05 | 路侧感知设备中雷达与gnss信息标定融合方法及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211075825.0A CN115166721B (zh) | 2022-09-05 | 2022-09-05 | 路侧感知设备中雷达与gnss信息标定融合方法及装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN115166721A CN115166721A (zh) | 2022-10-11 |
CN115166721B true CN115166721B (zh) | 2023-04-07 |
Family
ID=83481370
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211075825.0A Active CN115166721B (zh) | 2022-09-05 | 2022-09-05 | 路侧感知设备中雷达与gnss信息标定融合方法及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115166721B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116450711B (zh) * | 2023-06-20 | 2023-08-18 | 山东科技大学 | Gnss坐标时间序列数据流匹配方法 |
Family Cites Families (12)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102567991B (zh) * | 2011-12-09 | 2015-10-21 | 北京航空航天大学 | 一种基于同心圆合成图像匹配的双目视觉标定方法和*** |
CN111340134B (zh) * | 2020-03-11 | 2022-09-06 | 南京理工大学 | 一种基于局部动态规整的快速模板匹配方法 |
CN111491367B (zh) * | 2020-04-20 | 2021-03-30 | 电子科技大学 | 一种基于群智感知和多融合技术的室内定位方法 |
CN111612760B (zh) * | 2020-05-20 | 2023-11-17 | 阿波罗智联(北京)科技有限公司 | 用于检测障碍物的方法和装置 |
CN112346103A (zh) * | 2020-10-29 | 2021-02-09 | 清华大学 | 基于v2x的智能网联汽车动态协同定位方法与装置 |
GB2620877A (en) * | 2021-01-01 | 2024-01-24 | Du Yuchuan | On-board positioning device-based roadside millimeter-wave radar calibration method |
CN112836737A (zh) * | 2021-01-29 | 2021-05-25 | 同济大学 | 一种基于车路数据融合的路侧组合感知设备在线标定方法 |
CN112800351B (zh) * | 2021-02-05 | 2024-02-09 | 北京中交兴路信息科技有限公司 | 轨迹相似度判断方法、***及计算机介质 |
CN114357019A (zh) * | 2021-12-03 | 2022-04-15 | 同济大学 | 一种智能网联环境下路侧感知单元数据质量监测方法 |
CN114444284A (zh) * | 2022-01-18 | 2022-05-06 | 武汉光庭信息技术股份有限公司 | 一种车端实时仿真场景道路底图生成方法及*** |
CN114608589B (zh) * | 2022-03-04 | 2024-06-25 | 西安邮电大学 | 一种多传感器信息融合方法及*** |
CN114755662B (zh) * | 2022-03-21 | 2024-04-30 | 北京航空航天大学 | 一种路车融合感知的激光雷达和gps的标定方法和装置 |
-
2022
- 2022-09-05 CN CN202211075825.0A patent/CN115166721B/zh active Active
Non-Patent Citations (3)
Title |
---|
"Cross calibration Thaichote and Landsat 8 over Libya4 using pseudo invariant calibration sites (PICS)";Alongkot Pakat等;《2017 International Conference on Applied System Innovation (ICASI)》;834-837 * |
"Design and Performance of a GPS Constellation Power Monitor System for Improved CYGNSS L1B Calibration";Tianlin Wang等;《 IEEE Journal of Selected Topics in Applied Earth Observations and Remote Sensing》;26-36 * |
"基于多源信息融合的智能汽车环境感知算法开发及测试评价";白林;《中国优秀硕士学位论文全文数据库工程科技Ⅱ辑》;C035-199 * |
Also Published As
Publication number | Publication date |
---|---|
CN115166721A (zh) | 2022-10-11 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112525162B (zh) | 一种无人机测量输电线路影像距离的***及方法 | |
CN108983248A (zh) | 一种基于3d激光雷达及v2x的网联车定位方法 | |
EP3137850B1 (en) | Method and system for determining a position relative to a digital map | |
CN105547305B (zh) | 一种基于无线定位和激光地图匹配的位姿解算方法 | |
Li | Mobile mapping: An emerging technology for spatial data acquisition | |
CN110873570B (zh) | 用于位置信息的定源、生成并且更新表示位置的地图的方法和装置 | |
CN109813335B (zh) | 数据采集***的标定方法、装置、***及存储介质 | |
CN105184776A (zh) | 目标跟踪方法 | |
CN112558023A (zh) | 传感器的标定方法和装置 | |
CN113405560B (zh) | 车辆定位和路径规划统一建模方法 | |
CN111796315A (zh) | 无人机室内外的定位方法及装置 | |
CN114413909A (zh) | 一种室内移动机器人定位方法及*** | |
CN115166721B (zh) | 路侧感知设备中雷达与gnss信息标定融合方法及装置 | |
CN111398980A (zh) | 一种机载LiDAR数据处理的方法及装置 | |
CN105301621A (zh) | 一种车辆定位装置及一种智能驾考*** | |
CN114488094A (zh) | 一种车载多线激光雷达与imu外参数自动标定方法及装置 | |
CN115760999A (zh) | 一种基于gis辅助的单目相机校准和目标地理位置提取方法 | |
CN113447908B (zh) | 一种固定式雷达探测目标的gps北斗坐标获取方法以及装置 | |
CN114063127A (zh) | 一种多焦距视觉slam与gps融合的方法及存储介质 | |
CN105403886A (zh) | 一种机载sar定标器图像位置自动提取方法 | |
CN110865367B (zh) | 一种雷达视频数据智能融合方法 | |
CN103777196A (zh) | 基于地理信息的地面目标距离单站测量方法及其测量*** | |
CN108332749B (zh) | 一种室内动态追踪定位方法 | |
Iannucci et al. | Cross-Modal Localization: Using automotive radar for absolute geolocation within a map produced with visible-light imagery | |
WO2024069760A1 (ja) | 環境地図製作装置、環境地図製作方法及びプログラム |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |