CN114755662B - 一种路车融合感知的激光雷达和gps的标定方法和装置 - Google Patents
一种路车融合感知的激光雷达和gps的标定方法和装置 Download PDFInfo
- Publication number
- CN114755662B CN114755662B CN202210275064.7A CN202210275064A CN114755662B CN 114755662 B CN114755662 B CN 114755662B CN 202210275064 A CN202210275064 A CN 202210275064A CN 114755662 B CN114755662 B CN 114755662B
- Authority
- CN
- China
- Prior art keywords
- gps
- vehicle
- point
- laser radar
- target vehicle
- 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
- 238000000034 method Methods 0.000 title claims abstract description 59
- 230000004927 fusion Effects 0.000 title claims abstract description 36
- 230000008447 perception Effects 0.000 title claims description 56
- 238000013507 mapping Methods 0.000 claims abstract description 41
- 239000011159 matrix material Substances 0.000 claims description 225
- 238000013519 translation Methods 0.000 claims description 78
- 238000001514 detection method Methods 0.000 claims description 70
- 238000000605 extraction Methods 0.000 claims description 46
- 230000006870 function Effects 0.000 claims description 43
- 238000006243 chemical reaction Methods 0.000 claims description 42
- 230000000007 visual effect Effects 0.000 claims description 33
- 238000002474 experimental method Methods 0.000 claims description 23
- 238000004364 calculation method Methods 0.000 claims description 18
- 238000000354 decomposition reaction Methods 0.000 claims description 14
- 230000008030 elimination Effects 0.000 claims description 10
- 238000003379 elimination reaction Methods 0.000 claims description 10
- 230000002159 abnormal effect Effects 0.000 claims description 9
- 230000009466 transformation Effects 0.000 claims description 7
- 238000012549 training Methods 0.000 claims description 3
- 238000002372 labelling Methods 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 6
- 238000013075 data extraction Methods 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 238000013135 deep learning Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000002310 reflectometry Methods 0.000 description 1
- 238000012216 screening Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
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
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/497—Means for monitoring or calibrating
-
- 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/01—Satellite radio beacon positioning systems transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/13—Receivers
- G01S19/23—Testing, monitoring, correcting or calibrating of receiver elements
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/25—Fusion techniques
- G06F18/251—Fusion techniques of input or preprocessed data
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- General Physics & Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Computer Networks & Wireless Communication (AREA)
- Theoretical Computer Science (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Evolutionary Computation (AREA)
- General Engineering & Computer Science (AREA)
- Evolutionary Biology (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Bioinformatics & Computational Biology (AREA)
- Artificial Intelligence (AREA)
- Life Sciences & Earth Sciences (AREA)
- Optical Radar Systems And Details Thereof (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明涉及自动驾驶路车融合感知技术领域，提供了一种路车融合感知的激光雷达和GPS的标定方法和装置。该方法包括：基于靶车GPS目标参考点，获取车载激光雷达点云在GPS全局坐标系的映射和路侧激光雷达点云在GPS全局坐标系的映射，分别完成车载感知数据的标定和路侧感知数据的标定，实现车端感知数据和路侧感知数据同时到所述GPS全局坐标系下的映射，实现路车融合感知数据的标定。本发明提供了稳定可靠的空间同步方法，高精度捕捉靶车GPS的激光点云，剔除GPS高程噪声数据，有效配准GPS的激光雷达点和GPS坐标，提高标定精度，实现了车端、路侧设备的时空同步标注的自动化智能化。
Description
技术领域
本发明涉及自动驾驶路车融合感知技术领域，尤其涉及一种路车融合感知的激光雷达和GPS的标定方法和装置。
背景技术
三维激光雷达凭借其范围广、精度高、适应性好的特点被广泛应用于自动驾驶感知领域，通过扫描自动驾驶车辆周边环境为车辆提供三位环境点云信息。GPS作为高精度定位设备，同激光雷达配合，二者相互补充，为自动驾驶车辆提供准确的地理环境数据和目标位置信息。由于单车感知受限，自动驾驶车辆单独工作会出现盲区大、感知距离受限等问题，路侧感知单元常作为补盲设备同车端感知单元进行融合感知，从而达到补盲效果。而路车融合感知的前提是时空统一，即完成车端、路侧设备的时间同步和空间同步，因此，GPS和激光雷达的标定作为车端感知和路侧感知配合使用的前提，直接影响感知融合的精度和效果。
然而，传统的标定方法往往依赖于人工经验和手工测量，难以得到精确的标定结果。因此，为了减少标定工作中的人工干预过程，实现GPS、激光雷达标定的自动化、智能化，需要快速精准的标定方法。在专利CN109001711B中，提出了一种多线激光雷达与车载GPS的标定方法，该方法通过提取水平地面特征和GPS提供的转换矩阵解耦标定参数；在专利CN109901138A中，通过标定算法计算点云数据帧对和惯导数据间的转换矩阵，从而将点云数据和惯导数据转换到同一坐标系下。虽然上述方法一定程度改善了人工标定方法产生的误差问题，但是GPS坐标在高程上的波动和提取点云数据产生的误差又导致了新的问题，标定精度受到严重影响，这将导致路车感知融合精度低、误差大。
发明内容
有鉴于此，本发明提供了一种路车融合感知的激光雷达和GPS的标定方法和装置，以解决现有技术中自动驾驶车辆无法提供准确的地理环境数据和目标位置信息的问题。
第一方面，本发明提供了一种路车融合感知的激光雷达和GPS的标定方法，包括：
S1基于靶车GPS目标参考点，获取车载激光雷达点云在GPS全局坐标系的映射和路侧激光雷达点云在GPS全局坐标系的映射，分别完成车载感知数据的标定和路侧感知数据的标定；
所述车载感知数据的标定包括：在车载场景下，确保靶车GPS目标参考点被车载待标定系统中车载激光雷达和相机清晰捕捉，同时固定自动驾驶车辆，提取并存储所述车载待标定系统中车载GPS坐标及其航向角，其中，靶车GPS目标参考点是将靶车视为一个点；
采用标定算法对靶车GPS坐标高程的拟合校准、每帧靶车GPS坐标的提取和所述车载激光雷达点云坐标进行自动提取；
保存所述靶车在各实验点位的GPS坐标以及所述车载激光雷达点坐标；采用所述标定算法计算所述车载激光雷达点云到所述GPS全局坐标系下的转换矩阵，获得所述车载激光雷达点云坐标在GPS全局坐标系下的映射，完成车载感知数据的标定；
路侧感知数据的标定包括：在路侧场景下，确保靶车GPS目标参考点被路侧激光雷达和相机清晰捕捉，同时固定路侧待标定系统，提取并存储所述路侧待标定系统中路侧单元GPS坐标及其航向角；
采用所述标定算法对所述靶车GPS坐标高程的拟合校准、靶车GPS坐标的提取和路侧激光雷达点云坐标进行自动提取；
保存所述靶车在各实验点位的GPS坐标以及所述路侧激光雷达点坐标；
根据所述提取并存储的路侧单元GPS坐标及其航向角、各实验点位的GPS坐标和所述靶车GPS目标参考点在路侧激光雷达点坐标，采用所述标定算法计算所述路侧激光雷达点云到所述GPS全局坐标系下的转换矩阵，获得所述路侧激光雷达点云坐标在GPS全局坐标系下的映射，完成路侧感知数据的标定；
S2基于所述车载激光雷达点云坐标在GPS全局坐标系下的映射和路侧激光雷达点云坐标在GPS全局坐标系下的映射，实现车端感知数据和路侧感知数据同时到所述GPS全局坐标系下的映射，实现路车融合感知数据的标定。
进一步地，所述采用标定算法对靶车GPS坐标高程的拟合校准、靶车GPS坐标的提取，包括：
在标定实验前，所述车载激光雷达采集实验场地的地面点云数据，对地面点云平面进行法向量估计，得到地面的法向量所述地面的法向量求解的协方差矩阵方程如下：
其中，k是点云数量，Pci是第i个点云的坐标，是点云的平均坐标，λj是协方差矩阵C的第j个特征值，是协方差矩阵C的第j个特征向量，最小特征值λj对应的特征向量即为地面的法向量
采集并求取各所述实验点位的高程平均值作为所述实验点位的高程hi，基于各所述实验点位的所述靶车GPS坐标构成点集，对所述点集构成的平面进行法向量估计，获得点集平面的法向量，其中，点集用Xc＝{xc1，xc2，...，xci}表示，i∈{0，1，2，...，n}，n为实验点位的数量；
使得所述地面的法向量与所述点集平面的法向量保持平行，计算所述地面的法向量与所述点集平面的法向量之间的旋转矩阵，并与所述点集相乘，得到旋转后的点集，所述旋转后的点集构成理论GPS坐标点平面；
将所述旋转后的点集中的各所述实验点位高程视为理论高程，剔除各所述实验点位靶车GPS坐标的高程与所述理论高程相差1cm以上的所述靶车GPS坐标，并提取被剔除之后剩下的各所述实验点位靶车GPS坐标的平均高程作为所述实验点位的高程，完成对靶车GPS高程异常数据的剔除和对所述靶车GPS坐标的提取。
进一步地，所述采用所述标定算法对车载激光雷达点云坐标进行自动提取，包括：
在标定实验前，完成相机和车载激光雷达的时间同步和标定，采集靶车GPS的图片数据集并将所述靶车GPS的图片数据集通过视觉目标检测算法进行训练，确保所述视觉目标检测算法能够准确检测到所述靶车GPS；
在标定实验后，在各实验点位，所述车载激光雷达采集车载激光雷达点云坐标数据，同时采用所述视觉目标检测算法实时检测每帧图片上的所述靶车GPS并输出检测框位置、尺寸；
将每帧图片上靶车GPS的车载激光雷达点云投影到对应帧的图片上，基于所述视觉目标检测算法输出的靶车GPS检测框内的所述车载激光雷达点云，求取并存储所述靶车GPS检测框内车载激光雷达点云质心的坐标，并将每帧所述靶车GPS检测框内所述车载激光雷达点云质心的坐标作为当前实验点位的靶车GPS目标参考点的车载激光雷达点坐标，将各实验点所有帧中所述检测框内车载激光雷达点云质心的坐标作为所述实验点位的车载激光雷达点云坐标。
进一步地，所述采用所述标定算法计算所述车载激光雷达点到所述GPS全局坐标系下的转换矩阵，获得所述靶车GPS目标参考点的车载激光雷达点坐标在GPS全局坐标系下的映射，完成车载感知数据的标定，包括：
选择所述待标定系统中的车载GPS坐标作为参考点，完成所述靶车GPS坐标从世界坐标系到所述车载参考坐标系的转换，并记录旋转矩阵和平移矩阵，获得所述车载参考坐标系下的靶车GPS坐标点集Uc，其中，旋转矩阵和平移矩阵如下所示：
其中，(xc0，yc0，zc0)是待标定系统中的车载GPS坐标，Rco是旋转矩阵，Tco是平移矩阵，θc是车载GPS的航向角；
对所述车载参考坐标系下的靶车GPS坐标点集Uc到对应的点云点集的旋转矩阵和平移矩阵进行最小二乘法求解，目标函数如下所示：
其中，n是实验点位的数量，L(Rc，Tc)是两点集间的误差函数，Rc和Tc分别是车载参考坐标系下的靶车GPS坐标点集到对应的点云点集的旋转矩阵和平移矩阵，Uci和Vci是第i个实验点位对应的靶车GPS坐标点及其对应的车载激光雷达点；
使用奇异值分解求解所述目标函数，计算得到所述旋转矩阵Rc、所述平移矩阵Tc以及对应的所述两点集间的误差函数；
将所述旋转矩阵、所述平移矩阵的变换作用于所述点云点集；
若所述两点集间的误差函数小于给定的阈值，则保存当前旋转矩阵、平移矩阵并进入下一步，否则继续迭代，返回所述使用奇异值分解求解所述目标函数计算的步骤；
输出所述激光雷达坐标系到所述车载参考坐标系的旋转矩阵和平移矩阵；
基于所述激光雷达坐标系到所述车载参考坐标系的旋转矩阵Rc0和平移矩阵Tc0，进行所述GPS全局坐标系的二次转换，获得所述激光雷达坐标系到所述GPS全局坐标系的旋转矩阵和平移矩阵，获得所述靶车GPS目标参考点的车载激光雷达点坐标在GPS全局坐标系下的映射，完成车载感知数据的标定。
进一步地，所述采用所述标定算法对所述靶车GPS坐标高程的拟合校准、靶车GPS坐标的提取，包括：
在标定实验前，所述路侧激光雷达采集所述实验场地的地面点云数据，完成对所述地面点云平面进行所述法向量估计，得到所述地面的法向量所述地面的法向量求解的协方差矩阵方程如下：
其中，k是点云数量，Pri是第i个点云的坐标，是点云的平均坐标，λ′r是协方差矩阵C′的第j个特征值，是协方差矩阵C′的第j个特征向量，最小特征值λ′r对应的特征向量即为地面的法向量
采集并求取各所述实验点位的高程平均值作为所述实验点位的高程，基于各所述实验点位的所述靶车GPS坐标构成点集，对所述点集构成的平面进行所述法向量估计，获得所述点集平面的法向量；
使得所述地面的法向量与所述点集平面的法向量平行，计算所述地面的法向量与所述点集平面的法向量间的旋转矩阵，并与点集相乘，得到旋转后的点集，所述旋转后的点集构成所述理论GPS坐标点平面；
将所述旋转后的点集中的各所述实验点位高程视为理论高程，剔除所述各实验点位所述靶车GPS坐标的高程与所述理论高程相差1cm以上的GPS坐标，并提取被提出之后剩下的各所述实验点位靶车GPS坐标的平均高程作为所述实验点位的高程，完成对靶车GPS高程异常数据的剔除和对所述靶车GPS坐标的提取。
进一步地，所述采用所述标定算法对路侧激光雷达点云坐标进行自动提取，包括：
在标定实验前，完成相机和路侧激光雷达的时间同步和标定，采集靶车GPS的图片数据集并将所述靶车GPS的图片数据集通过所述视觉目标检测算法进行训练，确保所述视觉目标检测算法能够准确检测到靶车GPS；
在标定实验后，在所述各实验点位，所述路侧激光雷达采集路侧激光雷达点云坐标数据，同时采用所述视觉目标检测算法实时检测每帧图片上的所述靶车GPS并输出所述检测框位置、尺寸；
将每帧图片上靶车的路侧激光雷达点云坐标数据投影到对应帧的图片上，基于所述视觉目标检测算法输出的所述靶车GPS检测框内的所述路侧激光雷达点云，求取并存储检测框内路侧激光雷达点云质心的坐标，并将每帧所述检测框内靶车GPS的路侧激光雷达点云质心的坐标作为当前实验点位的靶车GPS目标参考点对应的路侧激光雷达点坐标，将各实验点所有帧中所述检测框内靶车GPS的路侧激光雷达点云质心的坐标作为所述实验点位的靶车GPS目标参考点的路侧激光雷达点云坐标。
进一步地，所述根据所述提取并存储的路侧单元GPS坐标及其航向角、各实验点位的GPS坐标和所述靶车GPS目标参考点在路侧激光雷达点坐标，采用所述标定算法计算所述路侧激光雷达点到所述GPS全局坐标系下的转换矩阵，获得所述靶车GPS目标参考点在路侧激光雷达点云坐标在GPS全局坐标系下的映射，完成路侧感知数据的标定，包括：
选择待标定系统中的路侧GPS坐标作为参考点，完成靶车GPS坐标从世界坐标系到所述路侧参考坐标系的转换，并记录旋转矩阵和平移矩阵，得到所述路侧参考坐标系下的靶车GPS坐标点集Ur，其中，旋转矩阵和平移矩阵如下所示：
其中，(xr0，yr0，zr0)是路侧GPS坐标，Rr0是旋转矩阵，Tr0是平移矩阵，θr是路侧GPS的航向角；
对所述路侧参考坐标系下的靶车GPS坐标点集Ur到对应的点云点集的旋转矩阵和平移矩阵进行最小二乘法求解，目标函数如下所示：
其中，n′是实验点位的数量，L(Rr，Tr)是两点集间的误差函数，Rr和Tr分别是路侧参考坐标系下的靶车GPS坐标点集到对应的点云点集的旋转矩阵和平移矩阵，Uri和Vri分别是第i个实验点位对应的靶车GPS坐标点及其对应的激光雷达点；
使用所述奇异值分解求解所述目标函数，计算得到所述旋转矩阵Rr、所述平移矩阵Tr以及对应的所述两点集间的误差函数；
将所述旋转矩阵、所述平移矩阵的变换作用于所述路侧激光雷达点云点集；
若所述两点集间的误差函数小于给定的阈值，则保存当前旋转矩阵、平移矩阵并进入下一步，否则，继续迭代，返回所述使用所述奇异值分解求解所述目标函数的步骤；
输出激光雷达坐标系到路侧参考坐标系的旋转矩阵和所述平移矩阵；
基于所述激光雷达坐标系到所述路侧参考坐标系的旋转矩阵和平移矩阵，进行所述GPS全局坐标系的二次转换，获得所述路侧激光雷达坐标系到所述GPS全局坐标系的旋转矩阵和平移矩阵，其中，是旋转矩阵，是平移矩阵，获得所述靶车GPS目标参考点对应的路侧激光雷达点在GPS全局坐标系下的映射，完成路侧感知数据的标定。
进一步地，所述标定算法包括靶车GPS坐标提取算法、基于相机和激光雷达融合的靶车GPS目标检测算法以及激光雷达点云和靶车GPS坐标的配准算法。
进一步地，所述实验点位之间的距离大于2m。
第二方面，本发明还提供一种路车融合感知的激光雷达和GPS的标定装置，包括：待标定系统和靶车，
所述待标定系统，包括车载待标定系统和路侧待标定系统；
车载待标定系统包括：靶车(1)、靶车计算单元(2)、靶车天线(3)、靶车GPS(4)、车载激光雷达(5)、车载相机(6)、车载GPS(7)、车载计算转换矩阵(8)、车载天线(9)和视觉辅助的GPS点云提取(10)；所述靶车(1)由靶车GPS(4)、靶车天线(3)和靶车计算单元(2)组成，用于获得靶车GPS坐标，并提供车载激光雷达和路侧激光雷达扫描到靶车GPS坐标所对应的靶车GPS目标参考点的点云；
在车载场景下，载体是自动驾驶车辆，通过车载激光雷达和车载相机(6)朝向相同，感知同一场景，所述车载相机(6)辅助所述车载激光雷达检测靶车GPS目标参考点，车载GPS(7)对所述自动驾驶车辆进行定位，所述车载计算转换矩阵8执行标定算法，通过车载激光雷达坐标系到GPS全局坐标系的转换后被标定，获得车载激光雷达点云在GPS全局坐标系下的映射，完成所述车载感知数据的标定；
路侧待标定系统包括：路侧激光雷达(11)、路侧相机(12)、路侧GPS(13)、路侧计算转换矩阵(14)、路侧天线(15)，靶车GPS(4)、靶车天线(3)、靶车计算单元(2)、靶车(1)和视觉辅助的GPS点云提取(10)；
在路侧场景下，载体是路侧感知单元支架，通过路侧激光雷达和路侧相机(12)朝向相同，感知同一场景，所述路侧相机(12)辅助所述路侧激光雷达检测所述靶车GPS目标参考点，路侧GPS13对路侧感知单元进行定位，所述路侧计算转换矩阵(14)执行所述标定算法，通过将路侧激光雷达坐标系转换到所述GPS全局坐标系(18)后被标定，获得路侧激光雷达点云在GPS全局坐标系(18)下的映射，完成所述路侧感知数据的标定，其中，标定算法包括由靶车GPS坐标提取算法、基于相机和激光雷达融合的靶车GPS目标检测算法以及激光雷达点和靶车GPS坐标的配准算法；
基于所述车载感知数据的标定和所述路侧感知数据的标定，完成在GPS全局坐标系下的路车融合感知数据的标定。
本发明与现有技术相比存在的有益效果是：
1.本发明将需要标定的系统、标定参照物和标定算法程序高度集成，整个标注流程简单有效、自动化程度高、人工干预少，为路车感知融合提供了稳定可靠的空间同步方法；
2.通过提出了一种基于视觉深度学习的靶车GPS目标参考点提取方法，自动提取靶车GPS目标参考点，该方法相较于人工提取方法或基于反射率阈值的提取方法，精度高，误差小，具智能化；
3.本发明装置提出一种GPS高程值异常剔除的方法，GPS在测量时高程误差大，通常存在厘米级误差，本发明剔除的方法可以拟合校准GPS坐标，剔除GPS高程噪声数据，保留GPS有效数据，提高标定精度。
附图说明
为了更清楚地说明本发明中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
图1是本发明提供的一种路车融合感知的激光雷达和GPS的标定方法流程图；
图2是本发明提供的车载场景下自动标定方法的流程图；
图3是本发明提供的路侧场景下自动标定方法的流程图；
图4是本发明提供的路侧场景标定的示意图，其中，路侧激光雷达(11)，路侧相机(12)，路侧GPS(13)，路侧计算转换矩阵(14)，路侧天线(15)，靶车GPS(4)，靶车天线(3)，靶车计算单元(2)，靶车(1)，视觉辅助的GPS点云提取(10)；
图5是本发明提供的车载场景标定的示意图，其中，靶车(1)，靶车计算单元(2)，靶车天线(3)，靶车GPS(4)，车载激光雷达坐标系(5)，车载相机(6)，车载GPS(7)，车载计算转换矩阵(8)，车载天线(9)，视觉辅助的GPS点云提取(10)；
图6是本发明提供的路车融合感知系统的空间统一的示意图，其中，路侧感知剧坐标系(16)，车载感知数据坐标系(17)，GPS全局坐标系(18)；
图7是本发明提供的GPS高程拟合校准及数据提取流程图；
图8是本发明提供的基于视觉辅助的靶车GPS目标参考点点云提取流程图；
图9是本发明提供的靶车GPS目标参考点点云与GPS坐标点配准算法流程图。
具体实施方式
以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本发明实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本发明。在其它情况中，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本发明的描述。
下面将结合附图详细说明根据本发明提供的一种路车融合感知的激光雷达和GPS的标定方法和装置。
图1是本发明提供的一种路车融合感知的激光雷达和GPS的标定方法流程图；
图6是本发明提供的路车融合感知系统的空间统一的示意图；
一种路车融合感知的激光雷达和GPS的标定方法。如图1所示，该自动标定方法包括：
图2是本发明提供的车载场景下自动标定方法的流程图。
S1基于靶车GPS目标参考点，获取车载激光雷达点云在GPS全局坐标系的映射和路侧激光雷达点云在GPS全局坐标系的映射，分别完成车载感知数据的标定和路侧感知数据的标定；
图5是本发明提供的车载场景标定的示意图。
所述车载感知数据的标定包括：在车载场景下，确保靶车GPS目标参考点被车载待标定系统中车载激光雷达和相机清晰捕捉，同时固定自动驾驶车辆，提取并存储所述车载待标定系统中车载GPS坐标及其航向角，其中，靶车GPS目标参考点是将靶车视为一个点；
其中，实验点位之间的距离大于2m。
采用标定算法对靶车GPS坐标高程的拟合校准、每帧靶车GPS坐标的提取和所述车载激光雷达点云坐标进行自动提取；
其中，靶车GPS坐标提取算法、基于相机和激光雷达融合的靶车GPS目标检测算法以及激光雷达点和靶车GPS坐标的配准算法。
图7是本发明提供的GPS高程拟合校准及数据提取流程图。
所述采用标定算法对靶车GPS坐标高程的拟合校准、每帧靶车GPS坐标的提取；
在标定实验前，车载激光雷达采集实验场地的地面点云数据，对地面点云平面进行法向量估计，得到地面的法向量地面的法向量求解的协方差矩阵方程如下：
其中，k是点云数量，Pci是第i个点云的坐标，是点云的平均坐标，λj是协方差矩阵C的第j个特征值，是协方差矩阵C的第j个特征向量，最小特征值λj对应的特征向量即为地面的法向量采集并求取各所述实验点位的高程平均值作为所述实验点位的高程hi，基于各所述实验点位的所述靶车GPS坐标构成点集，对所述点集构成的平面进行法向量估计，获得点集平面的法向量，其中，点集用Xc＝{xc1，xc2，...，xcj}表示，i∈{0，1，2，...，n}，n为实验点位的数量；
使得地面的法向量与点集平面的法向量保持平行，计算地面的法向量与点集平面的法向量之间的旋转矩阵，并与点集相乘，得到旋转后的点集，旋转后的点集构成理论GPS坐标点平面；
将旋转后的点集中的各实验点位高程视为理论高程，剔除各实验点位靶车GPS坐标的高程与理论高程相差1cm以上的靶车GPS坐标，并提取被剔除之后剩下的各实验点位靶车GPS坐标的平均高程作为实验点位的高程，完成对靶车GPS高程异常数据的剔除和对靶车GPS坐标的提取。
所述采用所述标定算法对车载激光雷达点云坐标进行自动提取；
包括：
在标定实验前，完成相机和车载激光雷达的时间同步和标定，采集靶车GPS的图片数据集并将所述靶车GPS的图片数据集通过视觉目标检测算法进行训练，确保所述视觉目标检测算法能够准确检测到所述靶车GPS；
在标定实验后，在各实验点位，所述车载激光雷达采集车载激光雷达点云坐标数据，同时采用所述视觉目标检测算法实时检测每帧图片上的所述靶车GPS并输出检测框位置、尺寸；
将每帧图片上靶车GPS的车载激光雷达点云投影到对应帧的图片上，基于所述视觉目标检测算法输出的靶车GPS检测框内的所述车载激光雷达点云，提取并存储所述靶车GPS检测框内车载激光雷达点云质心的坐标，并将每帧所述靶车GPS检测框内所述车载激光雷达点云质心的坐标作为当前实验点位的靶车GPS目标参考点的车载激光雷达点坐标，将各实验点所有帧中所述检测框内车载激光雷达点云质心的坐标作为所述实验点位的车载激光雷达点云坐标；
其中，将每一帧点云数据投影到对应帧的图片上，视觉目标检测算法输出的靶车GPS检测框内的点云即为靶车GPS在车载激光雷达下的点云，求取检测框内点云质心的坐标，存储该质心作为该实验点位的靶车GPS目标参考点对应的点云；
保存所述靶车在各实验点位的GPS坐标以及所述车载激光雷达点坐标；
图9是本发明提供的靶车GPS目标参考点点云与GPS坐标点配准算法流程图。
采用所述标定算法计算所述车载激光雷达点到所述GPS全局坐标系下的转换矩阵，获得所述车载激光雷达点坐标在GPS全局坐标系下的映射，完成车载感知数据的标定，包括：
选择待标定系统中的车载GPS坐标作为参考点，完成靶车GPS坐标从世界坐标系到车载参考坐标系的转换，并记录旋转矩阵和平移矩阵，获得车载参考坐标系下的靶车GPS坐标点集Uc，其中，旋转矩阵和平移矩阵如下所示：
其中，(xc0，yc0，zc0)是待标定系统中的车载GPS坐标，Rco是旋转矩阵，Tco是平移矩阵，θc是车载GPS的航向角；
对所述车载参考坐标系下的靶车GPS坐标点集Uc到对应的点云点集的旋转矩阵和平移矩阵进行最小二乘法求解，目标函数如下所示：
其中，n是实验点位的数量，L(Rc，Tc)是两点集间的误差函数，Rc和Tc分别是车载参考坐标系下的靶车GPS坐标点集到对应的点云点集的旋转矩阵和平移矩阵，Uci和Vci是第i个实验点位对应的靶车GPS坐标点及其对应的车载激光雷达点；
使用奇异值分解求解所述目标函数，计算得到所述旋转矩阵Rc、所述平移矩阵Tc以及对应的所述两点集间的误差函数；
将旋转矩阵、平移矩阵的变换作用于路侧激光雷达点云点集；
若两点集间的误差函数小于给定的阈值，则保存当前旋转矩阵、平移矩阵并进入下一步，否则继续迭代，返回所述使用奇异值分解求解所述目标函数的步骤；
输出所述激光雷达坐标系到所述车载参考坐标系的旋转矩阵和平移矩阵。
基于所述激光雷达坐标系到所述车载参考坐标系的旋转矩阵Rc0和平移矩阵Tc0，进行所述GPS全局坐标系的二次转换，获得所述激光雷达坐标系到所述GPS全局坐标系的旋转矩阵和平移矩阵，获得所述靶车GPS目标参考点的车载激光雷达点坐标在GPS全局坐标系下的映射，完成车载感知数据的标定。
图3是本发明提供的路侧场景下自动标定方法的流程图。
路侧感知数据的标定包括：在路侧场景下，确保靶车GPS目标参考点被路侧激光雷达和相机清晰捕捉，同时固定路侧待标定系统，提取并存储所述路侧待标定系统中路侧单元GPS坐标及其航向角；
采用所述标定算法对所述靶车GPS坐标高程的拟合校准、靶车GPS坐标的提取和对路侧激光雷达点云坐标进行自动提取；
所述采用所述标定算法对所述靶车GPS坐标高程的拟合校准、靶车GPS坐标的提取，包括：
在标定实验前，所述路侧激光雷达采集所述实验场地的地面点云数据，完成对所述地面点云平面进行所述法向量估计，得到所述地面的法向量所述地面的法向量求解的协方差矩阵方程如下：
其中，k是点云数量，Pri是第i个点云的坐标，是点云的平均坐标，λ′r是协方差矩阵C′的第j个特征值，是协方差矩阵C′的第j个特征向量，最小特征值λ′r对应的特征向量即为地面的法向量
采集并求取各所述实验点位的高程平均值作为所述实验点位的高程，基于各所述实验点位的所述靶车GPS坐标构成点集，对所述点集构成的平面进行所述法向量估计，获得所述点集平面的法向量；
使得所述地面的法向量与所述点集平面的法向量平行，计算所述地面的法向量与所述点集平面的法向量间的旋转矩阵，并与点集相乘，得到旋转后的点集，所述旋转后的点集构成所述理论GPS坐标点平面；
将所述旋转后的点集中的各所述实验点位高程视为理论高程，剔除所述各实验点位所述靶车GPS坐标的高程与所述理论高程相差1cm以上的GPS坐标，并提取被提出之后剩下的各所述实验点位靶车GPS坐标的平均高程作为所述实验点位的高程，完成对靶车GPS高程异常数据的剔除和对所述靶车GPS坐标的提取。
所述采用所述标定算法对路侧激光雷达点云坐标进行自动提取，包括：
在标定实验前，完成相机和路侧激光雷达的时间同步和标定，采集靶车GPS的图片数据集并将所述靶车GPS的图片数据集通过所述视觉目标检测算法进行训练，确保所述视觉目标检测算法能够准确检测到靶车GPS；
在标定实验后，在所述各实验点位，所述路侧激光雷达采集路侧激光雷达点云坐标数据，同时采用所述视觉目标检测算法实时检测每帧图片上的所述靶车GPS并输出所述检测框位置、尺寸；
将每帧图片上靶车GPS的路侧激光雷达点云坐标数据投影到对应帧的图片上，基于所述视觉目标检测算法输出的所述靶车GPS检测框内的所述路侧激光雷达点云，求取并存储检测框内路侧激光雷达点云质心的坐标，并将每帧所述检测框内靶车GPS的路侧激光雷达点云质心的坐标作为当前实验点位的靶车GPS目标参考点对应的路侧激光雷达点坐标，将各实验点所有帧中所述检测框内靶车GPS的路侧激光雷达点云质心的坐标作为所述实验点位的靶车GPS目标参考点的路侧激光雷达点云坐标。
S23保存所述靶车在各实验点位的GPS坐标以及所述路侧激光雷达点坐标；
根据所述提取并存储的路侧单元GPS坐标及其航向角、各实验点位的GPS坐标和所述靶车GPS目标参考点在路侧激光雷达点坐标，采用所述标定算法计算所述路侧激光雷达点云到所述GPS全局坐标系下的转换矩阵，获得所述路侧激光雷达点云坐标在GPS全局坐标系下的映射，完成路侧感知数据的标定；
所述根据所述提取并存储的路侧单元GPS坐标及其航向角、各实验点位的GPS坐标和所述靶车GPS目标参考点在路侧激光雷达点坐标，采用所述标定算法计算所述路侧激光雷达点到所述GPS全局坐标系下的转换矩阵，获得所述靶车GPS目标参考点在路侧激光雷达点云坐标在GPS全局坐标系下的映射，完成路侧感知数据的标定，包括：
选择待标定系统中的路侧GPS坐标作为参考点，完成靶车GPS坐标从世界坐标系到所述路侧参考坐标系的转换，并记录旋转矩阵和平移矩阵，得到所述路侧参考坐标系下的靶车GPS坐标点集Ur，其中，旋转矩阵和平移矩阵如下所示：
其中，(xr0，yr0，zr0)是路侧GPS坐标，Rr0是旋转矩阵，Tr0是平移矩阵，θr是路侧GPS的航向角；
对所述路侧参考坐标系下的靶车GPS坐标点集Ur到对应的点云点集的旋转矩阵和平移矩阵进行最小二乘法求解，目标函数如下所示：
其中，n′是实验点位的数量，L(Rr，Tr)是两点集间的误差函数，Rr和Tr分别是路侧参考坐标系下的靶车GPS坐标点集到对应的点云点集的旋转矩阵和平移矩阵，Uri和Vri分别是第i个实验点位对应的靶车GPS坐标点及其对应的激光雷达点；
使用所述奇异值分解求解所述目标函数，计算得到所述旋转矩阵Rr、所述平移矩阵Tr以及对应的所述两点集间的误差函数；
将所述旋转矩阵、所述平移矩阵的变换作用于所述路侧激光雷达点云点集；
若所述两点集间的误差函数小于给定的阈值，则保存当前旋转矩阵、平移矩阵并进入下一步，否则，继续迭代，返回所述使用所述奇异值分解求解所述目标函数的步骤；
输出激光雷达坐标系到路侧参考坐标系的旋转矩阵和所述平移矩阵；
基于所述激光雷达坐标系到所述路侧参考坐标系的旋转矩阵和平移矩阵，进行所述GPS全局坐标系的二次转换，获得所述路侧激光雷达坐标系到所述GPS全局坐标系的旋转矩阵和平移矩阵，其中，是旋转矩阵，是平移矩阵，获得所述靶车GPS目标参考点对应的路侧激光雷达点在GPS全局坐标系下的映射，完成路侧感知数据的标定。
图4是本发明提供的路侧标定场景的示意图。
在路侧场景下，确保靶车GPS目标参考点被路侧激光雷达和相机清晰捕捉，同时固定路侧待标定系统，提取并存储所述路侧待标定系统中路侧单元GPS坐标及其航向角；
采用标定算法对靶车GPS坐标高程的拟合校准、靶车GPS坐标的提取和靶车GPS目标参考点的路侧激光雷达点坐标进行自动提取；
图7是本发明提供的GPS高程拟合校准及数据提取流程图。
采用标定算法对靶车GPS坐标高程的拟合校准和靶车GPS坐标进行提取；
包括：
在标定实验前，路侧激光雷达采集实验场地的地面点云数据，完成对地面点云平面进行法向量估计，得到地面的法向量地面的法向量求解的协方差矩阵方程如下：
其中，k是点云数量，Pri是第i个点云的坐标，是点云的平均坐标，λ′r是协方差矩阵C′的第j个特征值，是协方差矩阵C′的第j个特征向量，最小特征值λ′r对应的特征向量即为地面的法向量
采集并求取各实验点位的高程平均值作为实验点位的高程，基于各实验点位的靶车GPS坐标构成点集对点集构成的平面进行法向量估计，获得点集平面的法向量，其中，点集用Xr＝{xr1，xr2，...，xri}表示，i∈{0，1，2，...，n′}，n′为实验点位的数量；
使得地面的法向量与点集平面的法向量平行，计算地面的法向量与点集平面的法向量间的旋转矩阵，并与点集Xr相乘，得到旋转后的点集X′r，旋转后的点集构成理论GPS坐标点平面；
将旋转后的点集中的各实验点位高程视为理论高程，剔除各实验点位靶车GPS坐标的高程与理论高程相差1cm以上的GPS坐标，并提取被提出之后剩下的各实验点位靶车GPS坐标的平均高程作为实验点位的高程，完成对靶车GPS高程异常数据的剔除和对靶车GPS坐标的提取。
基于提取的靶车GPS坐标，进行靶车GPS目标参考点的路侧激光雷达点坐标的自动提取；
图8是本发明提供的基于视觉辅助的靶车GPS目标参考点点云提取流程图；
包括：
在标定实验前，完成相机和路侧激光雷达的时间同步和标定，采集靶车GPS的图片数据集并将靶车GPS的图片数据集通过视觉目标检测算法进行训练，确保视觉目标检测算法能够准确检测到靶车GPS；
在标定实验后，在各实验点位，路侧激光雷达采集靶车GPS的路侧激光雷达点云坐标数据，同时采用视觉目标检测算法实时检测每帧图片上的靶车GPS并输出检测框位置、尺寸；
将每帧图片上的靶车GPS的路侧激光雷达点云坐标数据投影到对应帧的图片上，基于视觉目标检测算法输出的靶车GPS检测框内的靶车GPS的路侧激光雷达点云，求取并存储检测框内靶车GPS的路侧激光雷达点云质心的坐标，并将每帧靶车GPS检测框内路侧激光雷达点云质心的坐标作为当前实验点位的靶车GPS目标参考点对应的路侧激光雷达点坐标，将各实验点所有帧中检测框内路侧激光雷达点云质心的坐标作为实验点位的路侧激光雷达点云坐标。
保存所述靶车在各实验点位的GPS坐标以及所述靶车GPS目标参考点的路侧激光雷达点坐标；
根据提取并存储的路侧单元GPS坐标及其航向角、所有实验点位的GPS坐标所述靶车GPS目标参考点的路侧激光雷达点坐标，采用标定算法计算路侧激光雷达点到GPS全局坐标系下的转换矩阵，获得靶车GPS目标参考点的路侧激光雷达点在GPS全局坐标系下的映射，完成路侧感知数据的标定；
包括：
选择待标定系统中的路侧GPS坐标作为参考点，完成靶车GPS坐标从世界坐标系到路侧参考坐标系的转换，并记录旋转矩阵和平移矩阵，得到路侧参考坐标系下的靶车GPS坐标点集Ur，其中，旋转矩阵和平移矩阵如下所示：
其中，(xr0，yr0，zr0)是路侧GPS坐标，Rr0是旋转矩阵，Tr0是平移矩阵，θr是路侧GPS的航向角；
对路侧参考坐标系下的靶车GPS坐标点集到对应的点云点集的旋转矩阵和平移矩阵进行最小二乘法求解，目标函数如下所示：
其中，n′是实验点位的数量，L(Rr，Tr)是两点集间的误差函数，Rr和Tr分别是路侧参考坐标系下的靶车GPS坐标点集到对应点云点集的旋转矩阵和平移矩阵，Uri和Vri分别是第i个实验点位对应的靶车GPS坐标点及其对应的激光雷达点；
使用奇异值分解求解目标函数，计算得到旋转矩阵Rr、平移矩阵Tr以及对应的两点集间的误差函数；
将旋转矩阵、平移矩阵的变换作用于点云点集；
若两点集间的误差函数小于给定的阈值，则保存当前旋转矩阵、平移矩阵并进入下一步，否则，继续迭代，返回使用奇异值分解求解目标函数，计算的步骤；
输出激光雷达坐标系到路侧参考坐标系的旋转矩阵和平移矩阵；
基于所述激光雷达坐标系到所述路侧参考坐标系的旋转矩阵和平移矩阵，进行所述GPS全局坐标系的二次转换，获得所述路侧激光雷达坐标系到所述GPS全局坐标系的旋转矩阵和平移矩阵，其中，是旋转矩阵，是平移矩阵。
基于靶车GPS目标参考点的车载激光雷达点坐标在GPS全局坐标系下的映射和靶车GPS目标参考点的路侧激光雷达点坐标在GPS全局坐标系下的映射，实现车端感知数据和路侧感知数据同时映射到GPS全局坐标系下，实现路车融合感知数据的标定。
本发明完成车端感知数据和路侧感知数据的空间同步，将需要标定的车端/路侧感知系统、标定参照物和标定算法高度集成，使标定方法的自动化程度得以提高；通过对高程异常的GPS坐标进行剔除并筛选GPS坐标，减小因GPS坐标在高程上的波动产生的影响；通过一种基于视觉深度学习的靶车GPS目标参考点提取方法，提高点云数据提取的精度；本发明标定流程简单有效，自动化、智能化程度高；在完成车端和路侧感知设备坐标系统一任务的同时，保障了GPS和激光雷达的标定精度。
上述所有可选技术方案，可以采用任意结合形成本申请的可选实施例，在此不再一一赘述。
下述为本发明装置实施例，可以用于执行本发明方法实施例。对于本发明装置实施例中未披露的细节，请参照本发明方法实施例。
本发明还提供的一种路车融合感知的激光雷达和GPS的标定装置。该自动标定装置包括：
一种路车融合感知的激光雷达和GPS的标定装置，包括：待标定系统和靶车，
所述待标定系统，包括车载待标定系统和路侧待标定系统；
车载待标定系统包括：靶车(1)、靶车计算单元(2)、靶车天线(3)、靶车GPS(4)、车载激光雷达(5)、车载相机(6)、车载GPS(7)、车载计算转换矩阵(8)、车载天线(9)和视觉辅助的GPS点云提取(10)；所述靶车(1)由靶车GPS(4)、靶车天线(3)和靶车计算单元(2)组成，用于获得靶车GPS坐标，并提供车载激光雷达和路侧激光雷达扫描到靶车GPS坐标所对应的靶车GPS目标参考点的点云；
在车载场景下，载体是自动驾驶车辆，通过车载激光雷达和车载相机(6)朝向相同，感知同一场景，所述车载相机(6)辅助所述车载激光雷达检测靶车GPS目标参考点，车载GPS(7)对所述自动驾驶车辆进行定位，所述车载计算转换矩阵8执行标定算法，通过车载激光雷达坐标系到GPS全局坐标系的转换后被标定，获得车载激光雷达点云在GPS全局坐标系下的映射，完成所述车载感知数据的标定；
路侧待标定系统包括：路侧激光雷达(11)、路侧相机(12)、路侧GPS(13)、路侧计算转换矩阵(14)、路侧天线(15)，靶车GPS(4)、靶车天线(3)、靶车计算单元(2)、靶车(1)和视觉辅助的GPS点云提取(10)；
在路侧场景下，载体是路侧感知单元支架，通过路侧激光雷达和路侧相机(12)朝向相同，感知同一场景，所述路侧相机(12)辅助所述路侧激光雷达检测所述靶车GPS目标参考点，路侧GPS13对路侧感知单元进行定位，所述路侧计算转换矩阵(14)执行所述标定算法，通过将路侧激光雷达坐标系转换到所述GPS全局坐标系(18)后被标定，获得路侧激光雷达点云在GPS全局坐标系(18)下的映射，完成所述路侧感知数据的标定，其中，标定算法包括由靶车GPS坐标提取算法、基于相机和激光雷达融合的靶车GPS目标检测算法以及激光雷达点和靶车GPS坐标的配准算法；
基于所述车载感知数据的标定和所述路侧感知数据的标定，完成在GPS全局坐标系下的路车融合感知数据的标定。
应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。
以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围，均应包含在本发明的保护范围之内。
Claims (8)
1.一种路车融合感知的激光雷达和GPS的标定装置，其特征在于，包括：待标定系统和靶车，
所述待标定系统，包括车载待标定系统和路侧待标定系统；
车载待标定系统包括：靶车(1)、靶车计算单元(2)、靶车天线(3)、靶车GPS(4)、车载激光雷达(5)、车载相机(6)、车载GPS(7)、车载计算转换矩阵(8)、车载天线(9)和视觉辅助的GPS点云提取(10)；所述靶车(1)由靶车GPS(4)、靶车天线(3)和靶车计算单元(2)组成，用于获得靶车GPS坐标，并提供车载激光雷达和路侧激光雷达扫描到靶车GPS坐标所对应的靶车GPS目标参考点的点云；
在车载场景下，载体是自动驾驶车辆，通过车载激光雷达和车载相机(6)朝向相同，感知同一场景，所述车载相机(6)辅助所述车载激光雷达检测靶车GPS目标参考点，车载GPS(7)对所述自动驾驶车辆进行定位，所述车载计算转换矩阵(8)执行标定算法，通过车载激光雷达坐标系到GPS全局坐标系的转换后被标定，获得车载激光雷达点云在GPS全局坐标系下的映射，完成所述车载感知数据的标定；
路侧待标定系统包括：路侧激光雷达(11)、路侧相机(12)、路侧GPS(13)、路侧计算转换矩阵(14)、路侧天线(15)，靶车GPS(4)、靶车天线(3)、靶车计算单元(2)、靶车(1)和视觉辅助的GPS点云提取(10)；
在路侧场景下，载体是路侧感知单元支架，通过路侧激光雷达和路侧相机(12)朝向相同，感知同一场景，所述路侧相机(12)辅助所述路侧激光雷达检测所述靶车GPS目标参考点，路侧GPS(13)对路侧感知单元进行定位，所述路侧计算转换矩阵(14)执行所述标定算法，通过将路侧激光雷达坐标系转换到所述GPS全局坐标系(18)后被标定，获得路侧激光雷达点云在GPS全局坐标系(18)下的映射，完成所述路侧感知数据的标定，其中，标定算法包括由靶车GPS坐标提取算法、基于相机和激光雷达融合的靶车GPS目标检测算法以及激光雷达点和靶车GPS坐标的配准算法；
基于所述车载感知数据的标定和所述路侧感知数据的标定，完成在GPS全局坐标系下的路车融合感知数据的标定；
所述车载计算转换矩阵(8)执行标定算法，通过车载激光雷达坐标系到GPS全局坐标系的转换后被标定，获得车载激光雷达点云在GPS全局坐标系下的映射，完成所述车载感知数据的标定包括：
选择所述待标定系统中的车载GPS坐标作为参考点，完成所述靶车GPS坐标从世界坐标系到所述车载参考坐标系的转换，并记录旋转矩阵和平移矩阵，获得所述车载参考坐标系下的靶车GPS坐标点集Uc，其中，旋转矩阵和平移矩阵如下所示：
其中，(xc0，yc0，zc0)是待标定系统中的车载GPS坐标，Rco是旋转矩阵，Tco是平移矩阵，θc是车载GPS的航向角；
对所述车载参考坐标系下的靶车GPS坐标点集Uc到对应的点云点集的旋转矩阵和平移矩阵进行最小二乘法求解，目标函数如下所示：
其中，n是实验点位的数量,L(Rc，Tc)是两点集间的误差函数，Rc和Tc分别是车载参考坐标系下的靶车GPS坐标点集到对应的点云点集的旋转矩阵和平移矩阵，Uci和Vci是第i个实验点位对应的靶车GPS坐标点及其对应的车载激光雷达点；
使用奇异值分解求解所述目标函数，计算得到所述旋转矩阵Rc、所述平移矩阵Tc以及对应的所述两点集间的误差函数；
将所述旋转矩阵、所述平移矩阵的变换作用于所述点云点集；
若所述两点集间的误差函数小于给定的阈值，则保存当前旋转矩阵、平移矩阵并进入下一步，否则继续迭代，返回所述使用奇异值分解求解所述目标函数计算的步骤；
输出所述激光雷达坐标系到所述车载参考坐标系的旋转矩阵和平移矩阵；
基于所述激光雷达坐标系到所述车载参考坐标系的旋转矩阵Rc0和平移矩阵Tc0，进行所述GPS全局坐标系的二次转换，获得所述激光雷达坐标系到所述GPS全局坐标系的旋转矩阵和平移矩阵，获得所述靶车GPS目标参考点的车载激光雷达点坐标在GPS全局坐标系下的映射，完成车载感知数据的标定；
所述路侧计算转换矩阵(14)执行所述标定算法，通过将路侧激光雷达坐标系转换到所述GPS全局坐标系(18)后被标定，获得路侧激光雷达点云在GPS全局坐标系(18)下的映射，完成所述路侧感知数据的标定，包括：
选择待标定系统中的路侧GPS坐标作为参考点，完成靶车GPS坐标从世界坐标系到所述路侧参考坐标系的转换，并记录旋转矩阵和平移矩阵，得到所述路侧参考坐标系下的靶车GPS坐标点集Ur，其中，旋转矩阵和平移矩阵如下所示：
其中，(xr0,yr0,zr0)是路侧GPS坐标，Rr0是旋转矩阵，Tr0是平移矩阵，θr是路侧GPS的航向角；
对所述路侧参考坐标系下的靶车GPS坐标点集Ur到对应的点云点集的旋转矩阵和平移矩阵进行最小二乘法求解，目标函数如下所示：
其中，n'是实验点位的数量，L(Rr,Tr)是两点集间的误差函数，Rr和Tr分别是路侧参考坐标系下的靶车GPS坐标点集到对应的点云点集的旋转矩阵和平移矩阵，Uri和Vri分别是第i个实验点位对应的靶车GPS坐标点及其对应的激光雷达点；
使用所述奇异值分解求解所述目标函数，计算得到所述旋转矩阵Rr、所述平移矩阵Tr以及对应的所述两点集间的误差函数；
将所述旋转矩阵、所述平移矩阵的变换作用于所述路侧激光雷达点云点集；
若所述两点集间的误差函数小于给定的阈值，则保存当前旋转矩阵、平移矩阵并进入下一步，否则，继续迭代，返回所述使用所述奇异值分解求解所述目标函数的步骤；
输出激光雷达坐标系到路侧参考坐标系的旋转矩阵和所述平移矩阵；
基于所述激光雷达坐标系到所述路侧参考坐标系的旋转矩阵和平移矩阵，进行所述GPS全局坐标系的二次转换，获得所述路侧激光雷达坐标系到所述GPS全局坐标系的旋转矩阵和平移矩阵，其中，是旋转矩阵，是平移矩阵，获得所述靶车GPS目标参考点对应的路侧激光雷达点在GPS全局坐标系下的映射，完成路侧感知数据的标定。
2.根据权利要求1所述的标定装置的一种路车融合感知的激光雷达和GPS的标定方法，其特征在于，具体包括以下步骤：
S1基于靶车GPS目标参考点，获取车载激光雷达点云在GPS全局坐标系的映射和路侧激光雷达点云在GPS全局坐标系的映射，分别完成车载感知数据的标定和路侧感知数据的标定；
所述车载感知数据的标定包括：在车载场景下，确保靶车GPS目标参考点被车载待标定系统中车载激光雷达和相机清晰捕捉，同时固定自动驾驶车辆，提取并存储所述车载待标定系统中车载GPS坐标及其航向角，其中，靶车GPS目标参考点是将靶车视为一个点；
采用标定算法对靶车GPS坐标高程的拟合校准、每帧靶车GPS坐标的提取和所述车载激光雷达点云坐标进行自动提取；
保存所述靶车在各实验点位的GPS坐标以及所述车载激光雷达点坐标；采用所述标定算法计算所述车载激光雷达点云到所述GPS全局坐标系下的转换矩阵，获得所述车载激光雷达点云坐标在GPS全局坐标系下的映射，完成车载感知数据的标定；
路侧感知数据的标定包括：在路侧场景下，确保靶车GPS目标参考点被路侧激光雷达和相机清晰捕捉，同时固定路侧待标定系统，提取并存储所述路侧待标定系统中路侧单元GPS坐标及其航向角；
采用所述标定算法对所述靶车GPS坐标高程的拟合校准、靶车GPS坐标的提取和路侧激光雷达点云坐标进行自动提取；
保存所述靶车在各实验点位的GPS坐标以及所述路侧激光雷达点坐标；
根据所述提取并存储的路侧单元GPS坐标及其航向角、各实验点位的GPS坐标和所述靶车GPS目标参考点在路侧激光雷达点坐标，采用所述标定算法计算所述路侧激光雷达点云到所述GPS全局坐标系下的转换矩阵，获得所述路侧激光雷达点云坐标在GPS全局坐标系下的映射，完成路侧感知数据的标定；
S2基于所述车载激光雷达点云坐标在GPS全局坐标系下的映射和路侧激光雷达点云坐标在GPS全局坐标系下的映射，实现车端感知数据和路侧感知数据同时到所述GPS全局坐标系下的映射，实现路车融合感知数据的标定。
3.根据权利要求2所述的标定方法，其特征在于，所述采用标定算法对靶车GPS坐标高程的拟合校准、靶车GPS坐标的提取，包括：
在标定实验前，所述车载激光雷达采集实验场地的地面点云数据，对地面点云平面进行法向量估计，得到地面的法向量所述地面的法向量求解的协方差矩阵方程如下：
其中，k是点云数量，Pci是第i个点云的坐标，是点云的平均坐标，λj是协方差矩阵C的第j个特征值，是协方差矩阵C的第j个特征向量，最小特征值λj对应的特征向量即为地面的法向量
采集并求取各所述实验点位的高程平均值作为所述实验点位的高程hi，基于各所述实验点位的所述靶车GPS坐标构成点集，对所述点集构成的平面进行法向量估计，获得点集平面的法向量，其中，点集用Xc＝{xc1，xc2，...，xci}表示，i∈{0，1，2，...，n}，n为实验点位的数量；
使得所述地面的法向量与所述点集平面的法向量保持平行，计算所述地面的法向量与所述点集平面的法向量之间的旋转矩阵，并与所述点集相乘，得到旋转后的点集，所述旋转后的点集构成理论GPS坐标点平面；
将所述旋转后的点集中的各所述实验点位高程视为理论高程，剔除各所述实验点位靶车GPS坐标的高程与所述理论高程相差1cm以上的所述靶车GPS坐标，并提取被剔除之后剩下的各所述实验点位靶车GPS坐标的平均高程作为所述实验点位的高程，完成对靶车GPS高程异常数据的剔除和对所述靶车GPS坐标的提取。
4.根据权利要求3所述的标定方法，其特征在于，所述采用所述标定算法对车载激光雷达点云坐标进行自动提取，包括：
在标定实验前，完成相机和车载激光雷达的时间同步和标定，采集靶车GPS的图片数据集并将所述靶车GPS的图片数据集通过视觉目标检测算法进行训练，确保所述视觉目标检测算法能够准确检测到所述靶车GPS；
在标定实验后，在各实验点位，所述车载激光雷达采集车载激光雷达点云坐标数据，同时采用所述视觉目标检测算法实时检测每帧图片上的所述靶车GPS并输出检测框位置、尺寸；
将每帧图片上靶车GPS的车载激光雷达点云投影到对应帧的图片上，基于所述视觉目标检测算法输出的靶车GPS检测框内的所述车载激光雷达点云，求取并存储所述靶车GPS检测框内车载激光雷达点云质心的坐标，并将每帧所述靶车GPS检测框内所述车载激光雷达点云质心的坐标作为当前实验点位的靶车GPS目标参考点对应的车载激光雷达点坐标，将各实验点所有帧中所述检测框内车载激光雷达点云质心的坐标作为所述实验点位的车载激光雷达点云坐标。
5.根据权利要求3所述的标定方法，其特征在于，所述采用所述标定算法对所述靶车GPS坐标高程的拟合校准、靶车GPS坐标的提取，包括：
在标定实验前，所述路侧激光雷达采集所述实验场地的地面点云数据，完成对所述地面点云平面进行所述法向量估计，得到所述地面的法向量所述地面的法向量求解的协方差矩阵方程如下：
其中，k是点云数量，Pri是第i个点云的坐标，是点云的平均坐标，λ′r是协方差矩阵C'的第j个特征值，是协方差矩阵C'的第j个特征向量，最小特征值λ′r对应的特征向量即为地面的法向量
采集并求取各所述实验点位的高程平均值作为所述实验点位的高程，基于各所述实验点位的所述靶车GPS坐标构成点集，对所述点集构成的平面进行所述法向量估计，获得所述点集平面的法向量；
使得所述地面的法向量与所述点集平面的法向量平行，计算所述地面的法向量与所述点集平面的法向量间的旋转矩阵，并与点集相乘，得到旋转后的点集，所述旋转后的点集构成所述理论GPS坐标点平面；
将所述旋转后的点集中的各所述实验点位高程视为理论高程，剔除所述各实验点位所述靶车GPS坐标的高程与所述理论高程相差1cm以上的GPS坐标，并提取被提出之后剩下的各所述实验点位靶车GPS坐标的平均高程作为所述实验点位的高程，完成对靶车GPS高程异常数据的剔除和对所述靶车GPS坐标的提取。
6.根据权利要求2所述的标定方法，其特征在于，所述采用所述标定算法对路侧激光雷达点云坐标进行自动提取，包括：
在标定实验前，完成相机和路侧激光雷达的时间同步和标定，采集靶车GPS的图片数据集并将所述靶车GPS的图片数据集通过所述视觉目标检测算法进行训练，确保所述视觉目标检测算法能够准确检测到靶车GPS；
在标定实验后，在所述各实验点位，所述路侧激光雷达采集路侧激光雷达点云坐标数据，同时采用所述视觉目标检测算法实时检测每帧图片上的所述靶车GPS并输出所述检测框位置、尺寸；
将每帧图片上靶车GPS的路侧激光雷达点云坐标数据投影到对应帧的图片上，基于所述视觉目标检测算法输出的所述靶车GPS检测框内的所述路侧激光雷达点云，求取并存储检测框内路侧激光雷达点云质心的坐标，并将每帧所述检测框内靶车GPS的路侧激光雷达点云质心的坐标作为当前实验点位的靶车GPS目标参考点对应的路侧激光雷达点坐标，将各实验点所有帧中所述检测框内靶车GPS的路侧激光雷达点云质心的坐标作为所述实验点位的靶车GPS目标参考点的路侧激光雷达点云坐标。
7.根据权利要求2所述的标定方法，其特征在于，所述标定算法包括靶车GPS坐标提取算法、基于相机和激光雷达融合的靶车GPS目标检测算法以及激光雷达点云和靶车GPS坐标的配准算法。
8.根据权利要求3所述的标定方法，其特征在于，所述实验点位之间的距离大于2m。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210275064.7A CN114755662B (zh) | 2022-03-21 | 2022-03-21 | 一种路车融合感知的激光雷达和gps的标定方法和装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210275064.7A CN114755662B (zh) | 2022-03-21 | 2022-03-21 | 一种路车融合感知的激光雷达和gps的标定方法和装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114755662A CN114755662A (zh) | 2022-07-15 |
| CN114755662B true CN114755662B (zh) | 2024-04-30 |
Family
ID=82326465
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210275064.7A Active CN114755662B (zh) | 2022-03-21 | 2022-03-21 | 一种路车融合感知的激光雷达和gps的标定方法和装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114755662B (zh) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114994648B (zh) * | 2022-08-05 | 2022-11-08 | 聚时科技(深圳)有限公司 | 一种直线运动机构上2d激光雷达的外参标定方法 |
| CN115166721B (zh) * | 2022-09-05 | 2023-04-07 | 湖南众天云科技有限公司 | 路侧感知设备中雷达与gnss信息标定融合方法及装置 |
| CN116188817A (zh) * | 2023-01-06 | 2023-05-30 | 同济大学 | 一种基于图匹配的车路协同系统多传感器时空标定方法 |
| CN116449347B (zh) * | 2023-06-14 | 2023-10-03 | 蘑菇车联信息科技有限公司 | 路侧激光雷达的标定方法、装置及电子设备 |
| CN117471461B (zh) * | 2023-12-26 | 2024-03-08 | 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) | 一种用于车载辅助驾驶系统的路侧雷达服务装置和方法 |
| CN117872330B (zh) * | 2024-03-11 | 2024-05-31 | 安徽大学 | 面向复杂环境的无人驾驶多激光雷达标定与融合建图方法 |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111522043A (zh) * | 2020-04-30 | 2020-08-11 | 北京联合大学 | 一种无人车激光雷达快速重新匹配定位方法 |
| CN111694010A (zh) * | 2020-05-27 | 2020-09-22 | 东南大学 | 一种基于视觉与激光雷达融合的路侧车辆识别方法 |
| WO2020233443A1 (zh) * | 2019-05-21 | 2020-11-26 | 菜鸟智能物流控股有限公司 | 一种激光雷达与相机之间的标定方法和装置 |
-
2022
- 2022-03-21 CN CN202210275064.7A patent/CN114755662B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2020233443A1 (zh) * | 2019-05-21 | 2020-11-26 | 菜鸟智能物流控股有限公司 | 一种激光雷达与相机之间的标定方法和装置 |
| CN111522043A (zh) * | 2020-04-30 | 2020-08-11 | 北京联合大学 | 一种无人车激光雷达快速重新匹配定位方法 |
| CN111694010A (zh) * | 2020-05-27 | 2020-09-22 | 东南大学 | 一种基于视觉与激光雷达融合的路侧车辆识别方法 |
Non-Patent Citations (1)
| Title |
|---|
| 基于点云中心的激光雷达与相机联合标定方法研究;康国华;张琪;张晗;徐伟证;张文豪;;仪器仪表学报;20191230(第12期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114755662A (zh) | 2022-07-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114755662B (zh) | 一种路车融合感知的激光雷达和gps的标定方法和装置 | |
| CN107703528B (zh) | 自动驾驶中结合低精度gps的视觉定位方法及系统 | |
| CN110070615B (zh) | 一种基于多相机协同的全景视觉slam方法 | |
| CN111830953B (zh) | 车辆自定位方法、装置及系统 | |
| CN108802785B (zh) | 基于高精度矢量地图和单目视觉传感器的车辆自定位方法 | |
| US10909395B2 (en) | Object detection apparatus | |
| EP3825903A1 (en) | Method, apparatus and storage medium for detecting small obstacles | |
| CN109471096B (zh) | 多传感器目标匹配方法、装置及汽车 | |
| CN110146869A (zh) | 确定坐标系转换参数的方法、装置、电子设备和存储介质 | |
| CN111141311B (zh) | 一种高精度地图定位模块的评估方法及系统 | |
| CN104704384A (zh) | 具体用于装置的基于视觉的定位的图像处理方法 | |
| CN112740225B (zh) | 一种路面要素确定方法及装置 | |
| CN110766760B (zh) | 用于相机标定的方法、装置、设备和存储介质 | |
| CN111027381A (zh) | 利用单目相机识别障碍物的方法、装置、设备及存储介质 | |
| CN113568435A (zh) | 一种基于无人机自主飞行态势感知趋势的分析方法与系统 | |
| CN112731371A (zh) | 一种激光雷达与视觉融合的集成化目标跟踪系统及方法 | |
| CN110728720B (zh) | 用于相机标定的方法、装置、设备和存储介质 | |
| CN112017236A (zh) | 一种基于单目相机计算目标物位置的方法及装置 | |
| CN113743163A (zh) | 交通目标识别模型训练方法、交通目标定位方法、装置 | |
| CN117111085A (zh) | 一种自动驾驶汽车车路云融合感知方法 | |
| CN114485654A (zh) | 一种基于高精地图的多传感器融合定位方法及装置 | |
| KR20130034528A (ko) | 도로시설물 자동 위치측정 방법 | |
| Sun et al. | Automatic targetless calibration for LiDAR and camera based on instance segmentation | |
| CN113971697A (zh) | 一种空地协同车辆定位定向方法 | |
| CN117911271A (zh) | 一种基于yolo的动态障碍物快速点云剔除方法及系统 |
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 |