CN114755662A - 一种路车融合感知的激光雷达和gps的标定方法和装置 - Google Patents

一种路车融合感知的激光雷达和gps的标定方法和装置 Download PDF

Info

Publication number
CN114755662A
CN114755662A CN202210275064.7A CN202210275064A CN114755662A CN 114755662 A CN114755662 A CN 114755662A CN 202210275064 A CN202210275064 A CN 202210275064A CN 114755662 A CN114755662 A CN 114755662A
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.)
Granted
Application number
CN202210275064.7A
Other languages
English (en)
Other versions
CN114755662B (zh
Inventor
王章宇
龚子任
余贵珍
周彬
王昊
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beihang University
Original Assignee
Beihang University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beihang University filed Critical Beihang University
Priority to CN202210275064.7A priority Critical patent/CN114755662B/zh
Priority claimed from CN202210275064.7A external-priority patent/CN114755662B/zh
Publication of CN114755662A publication Critical patent/CN114755662A/zh
Application granted granted Critical
Publication of CN114755662B publication Critical patent/CN114755662B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • 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

Abstract

本发明涉及自动驾驶路车融合感知技术领域，提供了一种路车融合感知的激光雷达和GPS的标定方法和装置。该方法包括：基于靶车GPS目标参考点，获取车载激光雷达点云在GPS全局坐标系的映射和路侧激光雷达点云在GPS全局坐标系的映射，分别完成车载感知数据的标定和路侧感知数据的标定，实现车端感知数据和路侧感知数据同时到所述GPS全局坐标系下的映射，实现路车融合感知数据的标定。本发明提供了稳定可靠的空间同步方法，高精度捕捉靶车GPS的激光点云，剔除GPS高程噪声数据，有效配准GPS的激光雷达点和GPS坐标，提高标定精度，实现了车端、路侧设备的时空同步标注的自动化智能化。

Description

一种路车融合感知的激光雷达和GPS的标定方法和装置
技术领域
本发明涉及自动驾驶路车融合感知技术领域，尤其涉及一种路车融合感知的激光雷达和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坐标的提取，包括：
在标定实验前，所述车载激光雷达采集实验场地的地面点云数据，对地面点云平面进行法向量估计，得到地面的法向量
Figure BDA0003555421340000031
所述地面的法向量求解的协方差矩阵方程如下：
Figure BDA0003555421340000032
Figure BDA0003555421340000033
其中，k是点云数量，Pci是第i个点云的坐标，
Figure BDA0003555421340000034
是点云的平均坐标，λj是协方差矩阵C的第j个特征值，
Figure BDA0003555421340000035
是协方差矩阵C的第j个特征向量，最小特征值λj对应的特征向量
Figure BDA0003555421340000036
即为地面的法向量
Figure BDA0003555421340000037
采集并求取各所述实验点位的高程平均值作为所述实验点位的高程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，其中，旋转矩阵和平移矩阵如下所示：
Figure BDA0003555421340000051
Figure BDA0003555421340000052
其中，(xc0，yc0，zc0)是待标定系统中的车载GPS坐标，Rco是旋转矩阵，Tco是平移矩阵，θc是车载GPS的航向角；
对所述车载参考坐标系下的靶车GPS坐标点集Uc到对应的点云点集的旋转矩阵和平移矩阵进行最小二乘法求解，目标函数如下所示：
Figure BDA0003555421340000053
其中，n是实验点位的数量，L(Rc，Tc)是两点集间的误差函数，Rc和Tc分别是车载参考坐标系下的靶车GPS坐标点集到对应的点云点集的旋转矩阵和平移矩阵，Uci和Vci是第i个实验点位对应的靶车GPS坐标点及其对应的车载激光雷达点；
使用奇异值分解求解所述目标函数，计算得到所述旋转矩阵Rc、所述平移矩阵Tc以及对应的所述两点集间的误差函数；
将所述旋转矩阵、所述平移矩阵的变换作用于所述点云点集；
若所述两点集间的误差函数小于给定的阈值，则保存当前旋转矩阵、平移矩阵并进入下一步，否则继续迭代，返回所述使用奇异值分解求解所述目标函数计算的步骤；
输出所述激光雷达坐标系到所述车载参考坐标系的旋转矩阵和平移矩阵；
基于所述激光雷达坐标系到所述车载参考坐标系的旋转矩阵Rc0和平移矩阵Tc0，进行所述GPS全局坐标系的二次转换，获得所述激光雷达坐标系到所述GPS全局坐标系的旋转矩阵和平移矩阵，获得所述靶车GPS目标参考点的车载激光雷达点坐标在GPS全局坐标系下的映射，完成车载感知数据的标定。
进一步地，所述采用所述标定算法对所述靶车GPS坐标高程的拟合校准、靶车GPS坐标的提取，包括：
在标定实验前，所述路侧激光雷达采集所述实验场地的地面点云数据，完成对所述地面点云平面进行所述法向量估计，得到所述地面的法向量
Figure BDA0003555421340000061
所述地面的法向量求解的协方差矩阵方程如下：
Figure BDA0003555421340000062
Figure BDA0003555421340000063
其中，k是点云数量，Pri是第i个点云的坐标，
Figure BDA0003555421340000064
是点云的平均坐标，λ′r是协方差矩阵C′的第j个特征值，
Figure BDA0003555421340000065
是协方差矩阵C′的第j个特征向量，最小特征值λ′r对应的特征向量
Figure BDA0003555421340000066
即为地面的法向量
Figure BDA0003555421340000067
采集并求取各所述实验点位的高程平均值作为所述实验点位的高程，基于各所述实验点位的所述靶车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，其中，旋转矩阵和平移矩阵如下所示：
Figure BDA0003555421340000081
Figure BDA0003555421340000082
其中，(xr0，yr0，zr0)是路侧GPS坐标，Rr0是旋转矩阵，Tr0是平移矩阵，θr是路侧GPS的航向角；
对所述路侧参考坐标系下的靶车GPS坐标点集Ur到对应的点云点集的旋转矩阵和平移矩阵进行最小二乘法求解，目标函数如下所示：
Figure BDA0003555421340000083
其中，n′是实验点位的数量，L(Rr，Tr)是两点集间的误差函数，Rr和Tr分别是路侧参考坐标系下的靶车GPS坐标点集到对应的点云点集的旋转矩阵和平移矩阵，Uri和Vri分别是第i个实验点位对应的靶车GPS坐标点及其对应的激光雷达点；
使用所述奇异值分解求解所述目标函数，计算得到所述旋转矩阵Rr、所述平移矩阵Tr以及对应的所述两点集间的误差函数；
将所述旋转矩阵、所述平移矩阵的变换作用于所述路侧激光雷达点云点集；
若所述两点集间的误差函数小于给定的阈值，则保存当前旋转矩阵、平移矩阵并进入下一步，否则，继续迭代，返回所述使用所述奇异值分解求解所述目标函数的步骤；
输出激光雷达坐标系到路侧参考坐标系的旋转矩阵和所述平移矩阵；
基于所述激光雷达坐标系到所述路侧参考坐标系的旋转矩阵和平移矩阵，进行所述GPS全局坐标系的二次转换，获得所述路侧激光雷达坐标系到所述GPS全局坐标系的旋转矩阵和平移矩阵，其中，
Figure BDA0003555421340000091
是旋转矩阵，
Figure BDA0003555421340000092
是平移矩阵，获得所述靶车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坐标的提取；
在标定实验前，车载激光雷达采集实验场地的地面点云数据，对地面点云平面进行法向量估计，得到地面的法向量
Figure BDA0003555421340000121
地面的法向量求解的协方差矩阵方程如下：
Figure BDA0003555421340000131
Figure BDA0003555421340000132
其中，k是点云数量，Pci是第i个点云的坐标，
Figure BDA0003555421340000133
是点云的平均坐标，λj是协方差矩阵C的第j个特征值，
Figure BDA0003555421340000134
是协方差矩阵C的第j个特征向量，最小特征值λj对应的特征向量
Figure BDA0003555421340000135
即为地面的法向量
Figure BDA0003555421340000136
采集并求取各所述实验点位的高程平均值作为所述实验点位的高程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，其中，旋转矩阵和平移矩阵如下所示：
Figure BDA0003555421340000151
Figure BDA0003555421340000152
其中，(xc0，yc0，zc0)是待标定系统中的车载GPS坐标，Rco是旋转矩阵，Tco是平移矩阵，θc是车载GPS的航向角；
对所述车载参考坐标系下的靶车GPS坐标点集Uc到对应的点云点集的旋转矩阵和平移矩阵进行最小二乘法求解，目标函数如下所示：
Figure BDA0003555421340000153
其中，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坐标的提取，包括：
在标定实验前，所述路侧激光雷达采集所述实验场地的地面点云数据，完成对所述地面点云平面进行所述法向量估计，得到所述地面的法向量
Figure BDA0003555421340000161
所述地面的法向量求解的协方差矩阵方程如下：
Figure BDA0003555421340000162
Figure BDA0003555421340000163
其中，k是点云数量，Pri是第i个点云的坐标，
Figure BDA0003555421340000164
是点云的平均坐标，λ′r是协方差矩阵C′的第j个特征值，
Figure BDA0003555421340000165
是协方差矩阵C′的第j个特征向量，最小特征值λ′r对应的特征向量
Figure BDA0003555421340000166
即为地面的法向量
Figure BDA0003555421340000167
采集并求取各所述实验点位的高程平均值作为所述实验点位的高程，基于各所述实验点位的所述靶车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，其中，旋转矩阵和平移矩阵如下所示：
Figure BDA0003555421340000181
Figure BDA0003555421340000182
其中，(xr0，yr0，zr0)是路侧GPS坐标，Rr0是旋转矩阵，Tr0是平移矩阵，θr是路侧GPS的航向角；
对所述路侧参考坐标系下的靶车GPS坐标点集Ur到对应的点云点集的旋转矩阵和平移矩阵进行最小二乘法求解，目标函数如下所示：
Figure BDA0003555421340000183
其中，n′是实验点位的数量，L(Rr，Tr)是两点集间的误差函数，Rr和Tr分别是路侧参考坐标系下的靶车GPS坐标点集到对应的点云点集的旋转矩阵和平移矩阵，Uri和Vri分别是第i个实验点位对应的靶车GPS坐标点及其对应的激光雷达点；
使用所述奇异值分解求解所述目标函数，计算得到所述旋转矩阵Rr、所述平移矩阵Tr以及对应的所述两点集间的误差函数；
将所述旋转矩阵、所述平移矩阵的变换作用于所述路侧激光雷达点云点集；
若所述两点集间的误差函数小于给定的阈值，则保存当前旋转矩阵、平移矩阵并进入下一步，否则，继续迭代，返回所述使用所述奇异值分解求解所述目标函数的步骤；
输出激光雷达坐标系到路侧参考坐标系的旋转矩阵和所述平移矩阵；
基于所述激光雷达坐标系到所述路侧参考坐标系的旋转矩阵和平移矩阵，进行所述GPS全局坐标系的二次转换，获得所述路侧激光雷达坐标系到所述GPS全局坐标系的旋转矩阵和平移矩阵，其中，
Figure BDA0003555421340000191
是旋转矩阵，
Figure BDA0003555421340000192
是平移矩阵，获得所述靶车GPS目标参考点对应的路侧激光雷达点在GPS全局坐标系下的映射，完成路侧感知数据的标定。
图4是本发明提供的路侧标定场景的示意图。
在路侧场景下，确保靶车GPS目标参考点被路侧激光雷达和相机清晰捕捉，同时固定路侧待标定系统，提取并存储所述路侧待标定系统中路侧单元GPS坐标及其航向角；
采用标定算法对靶车GPS坐标高程的拟合校准、靶车GPS坐标的提取和靶车GPS目标参考点的路侧激光雷达点坐标进行自动提取；
图7是本发明提供的GPS高程拟合校准及数据提取流程图。
采用标定算法对靶车GPS坐标高程的拟合校准和靶车GPS坐标进行提取；
包括：
在标定实验前，路侧激光雷达采集实验场地的地面点云数据，完成对地面点云平面进行法向量估计，得到地面的法向量
Figure BDA0003555421340000193
地面的法向量求解的协方差矩阵方程如下：
Figure BDA0003555421340000194
Figure BDA0003555421340000195
其中，k是点云数量，Pri是第i个点云的坐标，
Figure BDA0003555421340000196
是点云的平均坐标，λ′r是协方差矩阵C′的第j个特征值，
Figure BDA0003555421340000197
是协方差矩阵C′的第j个特征向量，最小特征值λ′r对应的特征向量
Figure BDA0003555421340000198
即为地面的法向量
Figure BDA0003555421340000199
采集并求取各实验点位的高程平均值作为实验点位的高程，基于各实验点位的靶车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，其中，旋转矩阵和平移矩阵如下所示：
Figure BDA0003555421340000211
Figure BDA0003555421340000212
其中，(xr0，yr0，zr0)是路侧GPS坐标，Rr0是旋转矩阵，Tr0是平移矩阵，θr是路侧GPS的航向角；
对路侧参考坐标系下的靶车GPS坐标点集到对应的点云点集的旋转矩阵和平移矩阵进行最小二乘法求解，目标函数如下所示：
Figure BDA0003555421340000213
其中，n′是实验点位的数量，L(Rr，Tr)是两点集间的误差函数，Rr和Tr分别是路侧参考坐标系下的靶车GPS坐标点集到对应点云点集的旋转矩阵和平移矩阵，Uri和Vri分别是第i个实验点位对应的靶车GPS坐标点及其对应的激光雷达点；
使用奇异值分解求解目标函数，计算得到旋转矩阵Rr、平移矩阵Tr以及对应的两点集间的误差函数；
将旋转矩阵、平移矩阵的变换作用于点云点集；
若两点集间的误差函数小于给定的阈值，则保存当前旋转矩阵、平移矩阵并进入下一步，否则，继续迭代，返回使用奇异值分解求解目标函数，计算的步骤；
输出激光雷达坐标系到路侧参考坐标系的旋转矩阵和平移矩阵；
基于所述激光雷达坐标系到所述路侧参考坐标系的旋转矩阵和平移矩阵，进行所述GPS全局坐标系的二次转换，获得所述路侧激光雷达坐标系到所述GPS全局坐标系的旋转矩阵和平移矩阵，其中，
Figure BDA0003555421340000221
是旋转矩阵，
Figure BDA0003555421340000222
是平移矩阵。
基于靶车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 (10)

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目标参考点，路侧GPS13对路侧感知单元进行定位，所述路侧计算转换矩阵(14)执行所述标定算法，通过将路侧激光雷达坐标系转换到所述GPS全局坐标系(18)后被标定，获得路侧激光雷达点云在GPS全局坐标系(18)下的映射，完成所述路侧感知数据的标定，其中，标定算法包括由靶车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坐标的提取，包括：
在标定实验前，所述车载激光雷达采集实验场地的地面点云数据，对地面点云平面进行法向量估计，得到地面的法向量
Figure FDA0003555421330000031
所述地面的法向量求解的协方差矩阵方程如下：
Figure FDA0003555421330000032
Figure FDA0003555421330000033
其中，k是点云数量，Pci是第i个点云的坐标，
Figure FDA0003555421330000034
是点云的平均坐标，λj是协方差矩阵C的第j个特征值，
Figure FDA0003555421330000035
是协方差矩阵C的第j个特征向量，最小特征值λj对应的特征向量
Figure FDA0003555421330000036
即为地面的法向量
Figure FDA0003555421330000037
采集并求取各所述实验点位的高程平均值作为所述实验点位的高程hi，基于各所述实验点位的所述靶车GPS坐标构成点集，对所述点集构成的平面进行法向量估计，获得点集平面的法向量，其中，点集用Xc＝{xc1，xc2，...，xci}表示，i∈{0，1，2，...，n}，n为实验点位的数量；
使得所述地面的法向量与所述点集平面的法向量保持平行，计算所述地面的法向量与所述点集平面的法向量之间的旋转矩阵，并与所述点集相乘，得到旋转后的点集，所述旋转后的点集构成理论GPS坐标点平面；
将所述旋转后的点集中的各所述实验点位高程视为理论高程，剔除各所述实验点位靶车GPS坐标的高程与所述理论高程相差1cm以上的所述靶车GPS坐标，并提取被剔除之后剩下的各所述实验点位靶车GPS坐标的平均高程作为所述实验点位的高程，完成对靶车GPS高程异常数据的剔除和对所述靶车GPS坐标的提取。
4.根据权利要求2所述的标定方法，其特征在于，所述采用所述标定算法对车载激光雷达点云坐标进行自动提取，包括：
在标定实验前，完成相机和车载激光雷达的时间同步和标定，采集靶车GPS的图片数据集并将所述靶车GPS的图片数据集通过视觉目标检测算法进行训练，确保所述视觉目标检测算法能够准确检测到所述靶车GPS；
在标定实验后，在各实验点位，所述车载激光雷达采集车载激光雷达点云坐标数据，同时采用所述视觉目标检测算法实时检测每帧图片上的所述靶车GPS并输出检测框位置、尺寸；
将每帧图片上靶车GPS的车载激光雷达点云投影到对应帧的图片上，基于所述视觉目标检测算法输出的靶车GPS检测框内的所述车载激光雷达点云，求取并存储所述靶车GPS检测框内车载激光雷达点云质心的坐标，并将每帧所述靶车GPS检测框内所述车载激光雷达点云质心的坐标作为当前实验点位的靶车GPS目标参考点对应的车载激光雷达点坐标，将各实验点所有帧中所述检测框内车载激光雷达点云质心的坐标作为所述实验点位的车载激光雷达点云坐标。
5.根据权利要求2所述的标定方法，其特征在于，所述采用所述标定算法计算所述车载激光雷达点到所述GPS全局坐标系下的转换矩阵，获得所述靶车GPS目标参考点的车载激光雷达点坐标在GPS全局坐标系下的映射，完成车载感知数据的标定，包括：
选择所述待标定系统中的车载GPS坐标作为参考点，完成所述靶车GPS坐标从世界坐标系到所述车载参考坐标系的转换，并记录旋转矩阵和平移矩阵，获得所述车载参考坐标系下的靶车GPS坐标点集Uc，其中，旋转矩阵和平移矩阵如下所示：
Figure FDA0003555421330000051
Figure FDA0003555421330000052
其中，(xc0，yc0，zc0)是待标定系统中的车载GPS坐标，Rco是旋转矩阵，Tco是平移矩阵，θc是车载GPS的航向角；
对所述车载参考坐标系下的靶车GPS坐标点集Uc到对应的点云点集的旋转矩阵和平移矩阵进行最小二乘法求解，目标函数如下所示：
Figure FDA0003555421330000053
其中，n是实验点位的数量，L(Rc，Tc)是两点集间的误差函数，Rc和Tc分别是车载参考坐标系下的靶车GPS坐标点集到对应的点云点集的旋转矩阵和平移矩阵，Uci和Vci是第i个实验点位对应的靶车GPS坐标点及其对应的车载激光雷达点；
使用奇异值分解求解所述目标函数，计算得到所述旋转矩阵Rc、所述平移矩阵Tc以及对应的所述两点集间的误差函数；
将所述旋转矩阵、所述平移矩阵的变换作用于所述点云点集；
若所述两点集间的误差函数小于给定的阈值，则保存当前旋转矩阵、平移矩阵并进入下一步，否则继续迭代，返回所述使用奇异值分解求解所述目标函数计算的步骤；
输出所述激光雷达坐标系到所述车载参考坐标系的旋转矩阵和平移矩阵；
基于所述激光雷达坐标系到所述车载参考坐标系的旋转矩阵Rc0和平移矩阵Tc0，进行所述GPS全局坐标系的二次转换，获得所述激光雷达坐标系到所述GPS全局坐标系的旋转矩阵和平移矩阵，获得所述靶车GPS目标参考点的车载激光雷达点坐标在GPS全局坐标系下的映射，完成车载感知数据的标定。
6.根据权利要求2所述的标定方法，其特征在于，所述采用所述标定算法对所述靶车GPS坐标高程的拟合校准、靶车GPS坐标的提取，包括：
在标定实验前，所述路侧激光雷达采集所述实验场地的地面点云数据，完成对所述地面点云平面进行所述法向量估计，得到所述地面的法向量
Figure FDA0003555421330000061
所述地面的法向量求解的协方差矩阵方程如下：
Figure FDA0003555421330000062
Figure FDA0003555421330000063
其中，k是点云数量，Pri是第i个点云的坐标，
Figure FDA0003555421330000064
是点云的平均坐标，λ′r是协方差矩阵C′的第j个特征值，
Figure FDA0003555421330000065
是协方差矩阵C′的第j个特征向量，最小特征值λ′r对应的特征向量
Figure FDA0003555421330000066
即为地面的法向量
Figure FDA0003555421330000067
采集并求取各所述实验点位的高程平均值作为所述实验点位的高程，基于各所述实验点位的所述靶车GPS坐标构成点集，对所述点集构成的平面进行所述法向量估计，获得所述点集平面的法向量；
使得所述地面的法向量与所述点集平面的法向量平行，计算所述地面的法向量与所述点集平面的法向量间的旋转矩阵，并与点集相乘，得到旋转后的点集，所述旋转后的点集构成所述理论GPS坐标点平面；
将所述旋转后的点集中的各所述实验点位高程视为理论高程，剔除所述各实验点位所述靶车GPS坐标的高程与所述理论高程相差1cm以上的GPS坐标，并提取被提出之后剩下的各所述实验点位靶车GPS坐标的平均高程作为所述实验点位的高程，完成对靶车GPS高程异常数据的剔除和对所述靶车GPS坐标的提取。
7.根据权利要求2所述的标定方法，其特征在于，所述采用所述标定算法对路侧激光雷达点云坐标进行自动提取，包括：
在标定实验前，完成相机和路侧激光雷达的时间同步和标定，采集靶车GPS的图片数据集并将所述靶车GPS的图片数据集通过所述视觉目标检测算法进行训练，确保所述视觉目标检测算法能够准确检测到靶车GPS；
在标定实验后，在所述各实验点位，所述路侧激光雷达采集路侧激光雷达点云坐标数据，同时采用所述视觉目标检测算法实时检测每帧图片上的所述靶车GPS并输出所述检测框位置、尺寸；
将每帧图片上靶车GPS的路侧激光雷达点云坐标数据投影到对应帧的图片上，基于所述视觉目标检测算法输出的所述靶车GPS检测框内的所述路侧激光雷达点云，求取并存储检测框内路侧激光雷达点云质心的坐标，并将每帧所述检测框内靶车GPS的路侧激光雷达点云质心的坐标作为当前实验点位的靶车GPS目标参考点对应的路侧激光雷达点坐标，将各实验点所有帧中所述检测框内靶车GPS的路侧激光雷达点云质心的坐标作为所述实验点位的靶车GPS目标参考点的路侧激光雷达点云坐标。
8.根据权利要求2所述的标定方法，其特征在于，所述根据所述提取并存储的路侧单元GPS坐标及其航向角、各实验点位的GPS坐标和所述靶车GPS目标参考点在路侧激光雷达点坐标，采用所述标定算法计算所述路侧激光雷达点到所述GPS全局坐标系下的转换矩阵，获得所述靶车GPS目标参考点在路侧激光雷达点云坐标在GPS全局坐标系下的映射，完成路侧感知数据的标定，包括：
选择待标定系统中的路侧GPS坐标作为参考点，完成靶车GPS坐标从世界坐标系到所述路侧参考坐标系的转换，并记录旋转矩阵和平移矩阵，得到所述路侧参考坐标系下的靶车GPS坐标点集Ur，其中，旋转矩阵和平移矩阵如下所示：
Figure FDA0003555421330000071
Figure FDA0003555421330000081
其中，(xr0，yr0，zr0)是路侧GPS坐标，Rr0是旋转矩阵，Tr0是平移矩阵，θr是路侧GPS的航向角；
对所述路侧参考坐标系下的靶车GPS坐标点集Ur到对应的点云点集的旋转矩阵和平移矩阵进行最小二乘法求解，目标函数如下所示：
Figure FDA0003555421330000082
其中，n′是实验点位的数量，L(Rr，Tr)是两点集间的误差函数，Rr和Tr分别是路侧参考坐标系下的靶车GPS坐标点集到对应的点云点集的旋转矩阵和平移矩阵，Uri和Vri分别是第i个实验点位对应的靶车GPS坐标点及其对应的激光雷达点；
使用所述奇异值分解求解所述目标函数，计算得到所述旋转矩阵Rr、所述平移矩阵Tr以及对应的所述两点集间的误差函数；
将所述旋转矩阵、所述平移矩阵的变换作用于所述路侧激光雷达点云点集；
若所述两点集间的误差函数小于给定的阈值，则保存当前旋转矩阵、平移矩阵并进入下一步，否则，继续迭代，返回所述使用所述奇异值分解求解所述目标函数的步骤；
输出激光雷达坐标系到路侧参考坐标系的旋转矩阵和所述平移矩阵；
基于所述激光雷达坐标系到所述路侧参考坐标系的旋转矩阵和平移矩阵，进行所述GPS全局坐标系的二次转换，获得所述路侧激光雷达坐标系到所述GPS全局坐标系的旋转矩阵和平移矩阵，其中，
Figure FDA0003555421330000083
是旋转矩阵，
Figure FDA0003555421330000084
是平移矩阵，获得所述靶车GPS目标参考点对应的路侧激光雷达点在GPS全局坐标系下的映射，完成路侧感知数据的标定。
9.根据权利要求2所述的标定方法，其特征在于，所述标定算法包括靶车GPS坐标提取算法、基于相机和激光雷达融合的靶车GPS目标检测算法以及激光雷达点云和靶车GPS坐标的配准算法。
10.根据权利要求1所述的标定方法，其特征在于，所述实验点位之间的距离大于2m。
CN202210275064.7A 2022-03-21 一种路车融合感知的激光雷达和gps的标定方法和装置 Active CN114755662B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210275064.7A CN114755662B (zh) 2022-03-21 一种路车融合感知的激光雷达和gps的标定方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210275064.7A CN114755662B (zh) 2022-03-21 一种路车融合感知的激光雷达和gps的标定方法和装置

Publications (2)

Publication Number Publication Date
CN114755662A true CN114755662A (zh) 2022-07-15
CN114755662B CN114755662B (zh) 2024-04-30

Family

ID=

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114994648A (zh) * 2022-08-05 2022-09-02 聚时科技(深圳)有限公司 一种直线运动机构上2d激光雷达的外参标定方法
CN115166721A (zh) * 2022-09-05 2022-10-11 湖南众天云科技有限公司 路侧感知设备中雷达与gnss信息标定融合方法及装置
CN116449347A (zh) * 2023-06-14 2023-07-18 蘑菇车联信息科技有限公司 路侧激光雷达的标定方法、装置及电子设备
CN117471461A (zh) * 2023-12-26 2024-01-30 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) 一种用于车载辅助驾驶系统的路侧雷达服务装置和方法

Citations (3)

* Cited by examiner, † Cited by third party
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 菜鸟智能物流控股有限公司 一种激光雷达与相机之间的标定方法和装置

Patent Citations (3)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
Title
康国华;张琪;张晗;徐伟证;张文豪;: "基于点云中心的激光雷达与相机联合标定方法研究", 仪器仪表学报, no. 12, 30 December 2019 (2019-12-30) *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114994648A (zh) * 2022-08-05 2022-09-02 聚时科技(深圳)有限公司 一种直线运动机构上2d激光雷达的外参标定方法
CN114994648B (zh) * 2022-08-05 2022-11-08 聚时科技(深圳)有限公司 一种直线运动机构上2d激光雷达的外参标定方法
CN115166721A (zh) * 2022-09-05 2022-10-11 湖南众天云科技有限公司 路侧感知设备中雷达与gnss信息标定融合方法及装置
CN116449347A (zh) * 2023-06-14 2023-07-18 蘑菇车联信息科技有限公司 路侧激光雷达的标定方法、装置及电子设备
CN116449347B (zh) * 2023-06-14 2023-10-03 蘑菇车联信息科技有限公司 路侧激光雷达的标定方法、装置及电子设备
CN117471461A (zh) * 2023-12-26 2024-01-30 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) 一种用于车载辅助驾驶系统的路侧雷达服务装置和方法
CN117471461B (zh) * 2023-12-26 2024-03-08 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) 一种用于车载辅助驾驶系统的路侧雷达服务装置和方法

Similar Documents

Publication Publication Date Title
AU2018282302B2 (en) Integrated sensor calibration in natural scenes
CN110148185B (zh) 确定成像设备坐标系转换参数的方法、装置和电子设备
CN107704821B (zh) 一种弯道的车辆位姿计算方法
Alonso et al. Accurate global localization using visual odometry and digital maps on urban environments
CN111830953B (zh) 车辆自定位方法、装置及系统
US10909395B2 (en) Object detection apparatus
CN109471096B (zh) 多传感器目标匹配方法、装置及汽车
CN110766760B (zh) 用于相机标定的方法、装置、设备和存储介质
CN111065043A (zh) 一种基于车路通信的隧道内车辆融合定位系统及方法
Dawood et al. Harris, SIFT and SURF features comparison for vehicle localization based on virtual 3D model and camera
WO2020133415A1 (en) Systems and methods for constructing a high-definition map based on landmarks
CN110766761A (zh) 用于相机标定的方法、装置、设备和存储介质
CN112740225A (zh) 一种路面要素确定方法及装置
CN111238490A (zh) 视觉定位方法、装置以及电子设备
CN111191596B (zh) 一种封闭区域制图方法、装置及存储介质
CN112255604A (zh) 一种雷达数据准确性的判断方法、装置及计算机设备
CN115100290B (zh) 交通场景下的单目视觉定位方法及装置、设备、存储介质
US20220404170A1 (en) Apparatus, method, and computer program for updating map
WO2020113425A1 (en) Systems and methods for constructing high-definition map
CN114755662A (zh) 一种路车融合感知的激光雷达和gps的标定方法和装置
CN112258391B (zh) 一种基于道路交通标线的碎片化地图拼接方法
CN114755662B (zh) 一种路车融合感知的激光雷达和gps的标定方法和装置
CN112798020A (zh) 一种用于评估智能汽车定位精度的系统及方法
CN113155121B (zh) 一种车辆定位方法、装置及电子设备
CN117611762B (zh) 一种多层级地图构建方法、系统及电子设备

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