CN117706530B - 一种用于实现多激光雷达与组合导航标定的方法及*** - Google Patents
一种用于实现多激光雷达与组合导航标定的方法及*** Download PDFInfo
- Publication number
- CN117706530B CN117706530B CN202410162956.5A CN202410162956A CN117706530B CN 117706530 B CN117706530 B CN 117706530B CN 202410162956 A CN202410162956 A CN 202410162956A CN 117706530 B CN117706530 B CN 117706530B
- Authority
- CN
- China
- Prior art keywords
- laser radar
- representing
- lidar
- matrix
- external parameters
- 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 83
- 239000011159 matrix material Substances 0.000 claims abstract description 167
- 238000009826 distribution Methods 0.000 claims abstract description 18
- 238000006243 chemical reaction Methods 0.000 claims abstract description 10
- 230000014509 gene expression Effects 0.000 claims description 61
- 238000005070 sampling Methods 0.000 claims description 29
- 238000005457 optimization Methods 0.000 claims description 23
- 239000013598 vector Substances 0.000 claims description 22
- 238000013519 translation Methods 0.000 claims description 19
- 208000037170 Delayed Emergence from Anesthesia Diseases 0.000 claims description 18
- 238000012545 processing Methods 0.000 claims description 15
- 238000013507 mapping Methods 0.000 claims description 13
- 230000008569 process Effects 0.000 claims description 10
- 238000003860 storage Methods 0.000 claims description 7
- 230000015556 catabolic process Effects 0.000 claims description 5
- 238000006731 degradation reaction Methods 0.000 claims description 5
- 230000002159 abnormal effect Effects 0.000 claims description 4
- 230000001360 synchronised effect Effects 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 2
- 230000000875 corresponding effect Effects 0.000 description 17
- 238000004364 calculation method Methods 0.000 description 5
- 238000004590 computer program Methods 0.000 description 5
- 238000005259 measurement Methods 0.000 description 5
- 238000002360 preparation method Methods 0.000 description 5
- 230000008901 benefit Effects 0.000 description 4
- 238000004422 calculation algorithm Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 4
- 238000011960 computer-aided design Methods 0.000 description 3
- 230000001419 dependent effect Effects 0.000 description 2
- 238000000605 extraction Methods 0.000 description 2
- 230000001105 regulatory effect Effects 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 101100379079 Emericella variicolor andA gene Proteins 0.000 description 1
- 238000004220 aggregation Methods 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000002596 correlated effect Effects 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
-
- 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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- 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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
-
- 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
-
- 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)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Manufacturing & Machinery (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Optical Radar Systems And Details Thereof (AREA)
- Radar Systems Or Details Thereof (AREA)
Abstract
本发明公开了一种用于实现多激光雷达与组合导航标定的方法及***，包括：获得多个激光雷达点云数据和组合导航数据，并计算表征每个激光雷达分别与组合导航数据间转换关系的初始外参；根据每个初始外参，考虑基于扫描的真实空间表面中的点模型服从正态分布、激光雷达时钟源差异和车身振动的因素，对当前初始外参和组合导航数据进行调节，获得各激光雷达与组合导航***间的第一类外参和优化后的组合导航数据；基于第一类外参，获得用于对各激光雷达与组合导航***的外参的垂直分量进行补偿的补偿矩阵；根据每个激光雷达的补偿矩阵和第一类外参，结合优化后的组合导航数据，获得各激光雷达间的第二类外参。
Description
技术领域
本发明涉及自动驾驶技术领域，尤其是涉及一种用于实现多激光雷达与组合导航标定的方法及***。
背景技术
激光雷达与组合导航***间的外参标定，测绘领域的研究主要分为两种方法：近似方法和严格方法。近似方法着重于在缺乏组合导航系定位数据的情况下减少激光雷达数据间的差异，但这可能无法完全消除所有的偏差。而严格方法在考虑***标定误差的情况下，利用激光雷达和组合导航***数据进行传感器标定。
在自动驾驶领域中，对激光雷达-组合导航***标定技术的探索相对较少，主要是使用手眼标定方法进行标定。但是手眼标定方法的标定精度一般，多被用作其他标定方法的初始值。此外，还提出了一种基于平面捆绑调整的激光雷达与组合导航***外参标定方法。然而，该方法受限于环境平面提取的过程，只适用于旋转式机械激光雷达。
多激光雷达***的外参标定方法，大致可分为两类：基于目标的方法和无目标的方法。基于目标的方法通过融合来自多个传感器的数据来确定空间偏移，这就要求有可识别的物体和相应的对应点。无目标的方法则进一步细分为基于运动的和基于外观的方法。基于运动的无目标方法通过分析局部坐标系中的轨迹构建运动约束来估计外参，而基于外观的无目标技术则依赖于观测环境中物体的外观来进行传感器间的标定。
综上所述，在目前激光雷达与组合导航***标定的现有技术中，大多依赖于几何特征的提取，这导致了标定方法通常局限于特定类型的激光雷达，从而限制了技术的通用性。而且，在许多现行的标定方法中，没有充分考虑组合导航***数据中可能存在的噪声问题，这会导致外参估计的准确性降低。另外，在线多激光雷达标定技术的实际应用中，或受限于激光雷达的型号，或对环境有特殊要求，这在一定程度上降低了这些标定方法的通用性。
发明内容
本发明的目的在于，需要提出一种通用性的多激光雷达与组合导航***的标定方案，以降低使用局限性并有效提高外参计算的准确性。
为了解决上述技术问题，本发明实施例提供了一种用于实现多激光雷达与组合导航标定的方法，包括：获得来自多个激光雷达的原始点云数据和来自组合导航***的组合导航数据，并计算表征每个激光雷达分别与所述组合导航数据间转换关系的初始外参；根据每个初始外参，考虑基于扫描的真实空间表面中的点模型服从正态分布、激光雷达时钟源差异和车身振动的因素，对当前初始外参和组合导航数据进行调节，获得各激光雷达与组合导航***间的第一类外参和优化后的组合导航数据；基于所述第一类外参，获得用于对各个激光雷达与组合导航***的外参的垂直分量进行补偿的补偿矩阵；根据每个激光雷达的补偿矩阵和第一类外参，结合所述优化后的组合导航数据，获得各激光雷达间的第二类外参。
优选地，通过如下步骤来计算每个初始外参：将所有激光雷达的原始点云数据从激光雷达坐标系变换到世界坐标系后进行点云拼接，获得每个激光雷达的局部点云地图模型，所述局部点云地图模型含有相应激光雷达与组合导航***间的待估计的外参参数；通过搜索所述局部点云地图模型中的每个点的最近邻点，从而将所有点与其最近邻点所形成的点对间的距离之和作为误差，以构建每个激光雷达与组合导航***之间的旋转矩阵，所述旋转矩阵为关于误差的最小化求取后而形成的矩阵；将平移矩阵赋值为零并求解每个激光雷达与组合导航***之间的旋转矩阵，获得每个激光雷达的初始旋转外参矩阵，用以表征所述初始外参。
优选地，在根据每个初始外参，考虑基于扫描的真实空间表面中的点模型服从正态分布、激光雷达时钟源差异和车身振动的因素，对当前初始外参和组合导航数据进行调节，获得各激光雷达与组合导航***间的第一类外参和优化后的组合导航数据的步骤中，包括：计算同一激光雷达采样到的任意两点所在真实空间局部表面间的第一面面距离；根据所述第一面面距离、每个激光雷达的初始旋转外参矩阵和所述组合导航数据，参考基于扫描的真实空间表面中的点模型服从正态分布的因素，构建用来对激光雷达与组合导航***间的外参进行粗调节的外参粗调模型；通过对所述外参粗调模型进行最小估计来求解所述外参粗调模型，获得每个激光雷达的估计后外参旋转矩阵；根据所述第一面面距离、每个激光雷达的估计后外参旋转矩阵和所述组合导航数据，参考基于激光雷达时钟源差异和车身振动的因素，构建用来对激光雷达与组合导航***间的外参进行精调节的外参调节模型；通过对所述外参调节模型进行最小估计来求解所述外参调节模型，获得每个激光雷达与组合导航***间的第一类外参和优化后的组合导航位姿。
优选地，在根据所述第一类外参，获得用于对各个激光雷达与组合导航***的外参的垂直分量进行补偿的补偿矩阵的步骤中，包括：以预设目标定位中心为基准，根据指定真实空间表面上的点分别在不同激光雷达坐标系下的对应表示，并将不同激光雷达的第一类外参作为对应的退化外参，构建并求解用于将任意两个激光雷达点云数据中的地平面进行对齐处理的补偿矩阵。
优选地，在根据每个激光雷达的补偿矩阵和第一类外参，结合所述组合导航数据，获得各激光雷达间的第二类外参的步骤中，包括：根据所述优化后的组合导航位姿、以及每个激光雷达的补偿矩阵和第一类外参，计算每个激光雷达所有时刻的初始位姿；计算在任意两个激光雷达在任意采样时刻下的两个分别属于不同雷达采样空间的采样点所在真实空间局部表面之间、或者同一激光雷达在任意两个采样时刻下的采样点之间的第二面面距离；根据所述第二面面距离和每个激光雷达的初始位姿，构建用来计算各个激光雷达在所有采样时刻下位姿的位姿优化模型；通过对所述位姿优化模型进行迭代计算，得到不同激光雷达在所有时刻下的位姿；根据所述不同激光雷达在所有时刻下的位姿，计算各个激光雷达相对于主激光雷达的外参，从而得到各个激光雷达间的第二类外参。
优选地，所述局部点云地图模型利用如下表达式表示：
；
其中，表示局部点云地图模型，表示激光雷达扫描的原始点云数据，
表示所有点对应时刻的集合，表示中的第k个点的模型，表示点在世界坐
标系下的表示，表示车辆中的组合导航***的定位中心在采样时刻的位姿，表示
激光雷达到组合导航***的第一类外参参数，所述旋转矩阵利用如下表达式表示：
；
其中，表示激光雷达与组合导航***间的旋转矩阵，表示基于
地图计算的误差。
优选地，所述第一面面距离利用如下表达式计算得到：
；
其中，表示在时刻由激光雷达采样到的第k个点所在的局部平面与在时
刻采样到的第l个点所在的局部平面之间的第一面面距离，表示激光雷达到组合导航
***的第一类外参，、分别表示车辆中的组合导航***的定位中心在时刻和的
位姿，、分别表示由激光雷达采样到的第k个点的模型与在时采样到的第l个点
的模型，、分别表示激光雷达在时刻和时刻下的位姿，所述外参粗调模型利用
如下表达式表示：
；
其中，表示激光雷达的外参粗调模型，表示鲁棒核函数，表示第一个
残差块的协方差矩阵，表示激光雷达到组合导航***间的初始旋转外参矩阵，表示待估计的激光雷达到组合导航***间的估计后外参旋转矩阵，Log()表示直接
将SO(3)空间映射到向量空间的符号表示，表示第二个残差块的协方差矩阵，所述外
参调节模型利用如下表达式表示：
；
其中，表示激光雷达的外参调节模型，表示第一个残差块的协方差矩阵，表示由组合导航***实时测得的组合导航数据，表示待估计的优化后的组
合导航位姿，表示第二个和第三个残差块的协方差矩阵，表示粗调节后的激光
雷达的估计后外参旋转矩阵，表示待估计的激光雷达的第一类外参，表示
直接将SE(3)空间映射到向量空间的符号。
优选地，所述补偿矩阵利用如下表达式表示：
；
其中，、分别表示作为退化外参的激光雷达和激光雷达的第一类外参，表示补偿矩阵，、分别代表同一指定真实空间表面点分别在 和坐标系下
的不同表示，在求解所述补偿矩阵的过程中，包括：先将所述补偿矩阵转换为点云配准表达
式，再将所述补偿矩阵中的补偿矩阵参量分解为坐标旋转矩阵参量和坐标平移矩阵参量，
从而通过提取当前两个激光雷达点云数据中的地平面来计算相应的坐标旋转矩阵和坐标
平移矩阵，继而获得补偿矩阵的求解结果，其中，所述点云配准表达式利用如下表达式表
示：
；
其中，表示指定真实空间表面点在雷达激光坐标系下的表示，表示指定
真实空间表面点在从坐标系到坐标系的坐标旋转矩阵，表示指定真实空间表面
点在雷达激光坐标系下的表示，表示表示指定真实空间表面点在从坐标系到坐
标系的坐标平移矩阵，所述坐标旋转矩阵和所述坐标平移矩阵利用如下表达式表示：
；
其中，、分别定义了指定真实空间表面点在雷达激光坐标系和
雷达激光坐标系下的地面平面的参数化表示，、分别表示两个地平面的法向量，
、分别表示两个地平面的截距，u分别表示与和都正交的单位向量，θ表示与之间
的角度，表示坐标旋转矩阵，t表示坐标平移矩阵，表示单位矩阵，T表示转置符号。
优选地，所述每个激光雷达的初始位姿利用如下表达式计算得到：
；
其中，表示所有激光雷达的初始位姿，表示时刻下激光雷达的初
始位姿，G0表示目标定位中心，表示时刻下的优化后的组合导航位姿，表示激光
雷达的补偿矩阵，表示激光雷达的第一类外参矩阵，表示优化后的组合导航系
统的位姿集合，表示补偿矩阵的集合，表示激光雷达到组合导航***的第一类外参集
合，表示激光雷达集合，表示时间集合，所述第二面面距离利用如下表达式计算得到：
；
其中，表示激光雷达 在采样时刻下的第k个点所在的局部平面与激
光雷达在采样时刻下的第l个点所在的局部平面之间的第二面面距离，表示时刻
时激光雷达的位姿，表示时刻时激光雷达的位姿，、分别表示由激光
雷达在时刻采样到的第k个点与激光雷达在时刻采样到的第l个点，
所述位姿优化模型利用如下表达式表示：
；
其中，表示第一个残差块的协方差矩阵，表示激光雷达在时刻下
的待估计的激光雷达位姿，表示通过激光雷达 的初始位姿而形成的单位矩
阵，表示第二个残差块的协方差矩阵，表示直接将SE(3)空间映射到向量空间
的符号，各个激光雷达间的第二类外参利用如下表达式计算得到：
；
其中，表示激光雷达与激光雷达之间的第二类外参，表示激光
雷达在时刻下所估计得到的激光雷达位姿，表示激光雷达在时刻下所待估计
的激光雷达位姿，Exp()将向量空间直接映射到SE(3)的符号表示，表示直接将SE
(3)空间映射到向量空间的符号表示，表示已同步的时间戳的集合。
优选地，在计算所述各个激光雷达间的第二类外参的步骤之前，所述方法还包括：采用3σ原则从所述不同激光雷达在所有时刻下的位姿中去除异常位姿。
另一方面，本发明实施例提供了一种计算机可读存储介质，其包含用于执行如上述所述的用于实现多激光雷达与组合导航标定的方法步骤的一系列指令。
另外，本发明实施例还提供了一种用于实现多激光雷达与组合导航标定的***，包括：初始化处理模块，其配置为获得来自多个激光雷达的原始点云数据和来自组合导航***的组合导航数据，并计算表征每个激光雷达分别与所述组合导航数据间转换关系的初始外参；第一类外参标定模块，其配置为根据每个初始外参，考虑基于扫描的真实空间表面中的点模型服从正态分布、激光雷达时钟源差异和车身振动的因素，对当前初始外参和组合导航数据进行调节，获得各激光雷达与组合导航***间的第一类外参和优化后的组合导航数据；地面对齐处理模块，其配置为基于所述第一类外参，获得用于对各个激光雷达与组合导航***的外参的垂直分量进行补偿的补偿矩阵；第二类外参标定模块，其配置为根据每个激光雷达的补偿矩阵和第一类外参，结合所述优化后的组合导航数据，获得各激光雷达间的第二类外参。
与现有技术相比，上述方案中的一个或多个实施例可以具有如下优点或有益效果：
本发明提出了一种用于实现多激光雷达与组合导航标定的方法及***。该方法及***首先无须开展例如采集用于初始化的数据、准备标定物等任何的额外输入以及标定前准备，并提供完整的自动化多激光雷达和组合导航***标定算法，最终得到每个激光雷达和组合导航***之间的外参以及多激光雷达之间的外参。而且，本发明对环境的依赖性低，且不限定不要求特定的传感器（例如激光雷达）的类型。由于是直接基于点进行构建模型并优化求解，因此本发明不要求环境具有特定的人造特征，同时适用于结构化的人造环境及非结构化的自然环境中的标定，同时，基于点的模型适用于各类激光雷达。另外，本发明考虑了不平整地面和时间同步对组合导航***定位精度的影响，提高激光雷达和组合导航***之间的外参标定精度和准确性更高，精度提高了20%左右。此外，本发明还考虑了时间同步对多激光雷达标定的影响，通过估计激光雷达位姿从而间接估计激光雷达的外参，在多激光雷达没有严格时间同步的情况下，具有更加准确的估计结果。
本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在说明书、权利要求书以及附图中所特别指出的结构来实现和获得。
附图说明
附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例共同用于解释本发明，并不构成对本发明的限制。在附图中。
图1为本申请实施例的用于实现多激光雷达与组合导航标定的方法的步骤图。
图2为本申请实施例的用于实现多激光雷达与组合导航标定的方法的具体流程图。
图3为本申请实施例的用于实现多激光雷达与组合导航标定的***的步骤图。
具体实施方式
以下将结合附图及实施例来详细说明本发明的实施方式，借此对本发明如何应用技术手段来解决技术问题，并达成技术效果的实现过程能充分理解并据以实施。需要说明的是，只要不构成冲突，本发明中的各个实施例以及各实施例中的各个特征可以相互结合，所形成的技术方案均在本发明的保护范围之内。
另外，附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机***中执行。并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。
这里所使用的术语仅仅是为了描述具体实施例而不意图限制示例性实施例。除非上下文明确地另有所指，否则这里所使用的单数形式“一个”、“一项”还意图包括复数。还应当理解的是，这里所使用的术语“包括”和/或“包含”规定所陈述的特征、整数、步骤、操作、单元和/或组件的存在，而不排除存在或添加一个或更多其他特征、整数、步骤、操作、单元、组件和/或其组合。
为了解决上述背景技术中的一个或多个技术问题，本发明提供了一种用于实现多激光雷达与组合导航标定的方法及***。该方法及***无须开展任何的额外输入以及标定前准备，能够提供完整的多激光雷达和组合导航***标定算法，最终得到多激光雷达之间的外参以及每个激光雷达和组合导航***之间的外参。本发明对环境的依赖性低，并且不要求特定的传感器类型，同时考虑了不平整地面和时间同步对组合导航***定位数据的影响，使得激光雷达和组合导航***之间的外参估计更加准确，还考虑了时间同步对多激光雷达标定的影响。另外，本发明通过优化激光雷达位姿，从而间接优化激光雷达的外参，在多激光雷达没有严格时间同步的情况下，具有更加准确的估计结果。
实施例一
图1为本申请实施例的用于实现多激光雷达与组合导航标定的方法的步骤图。下面，参考图1，对本发明实施例所述的用于实现多激光雷达与组合导航标定的方法（以下简称“组合标定实现方法”）的具体流程进行说明。
步骤S110获得自动驾驶过程中来自多个激光雷达的原始点云数据和来自组合导航***的组合导航数据，并根据多组原始点云数据和组合导航数据，计算表征每个激光雷达分别与组合导航数据间转换关系的初始外参。在步骤S110中，对于自动驾驶车辆来说，安装有多个激光雷达设备，每个激光雷达设备会实时采集相应空间视野范围下的原始点云数据，一个激光雷达设备实时采集一组动态的原始点云数据。同时，自动驾驶车辆还会安装有组合导航***，该组合导航***会实时获得一组组合导航数据。
由此，在步骤S110中，首先获得有多个激光雷达设备和组合导航***发送的多组动态的原始点云数据和组合导航数据，并根据所获得的这些数据，对激光雷达与组合导向***的外参进行初始化处理，获得表征每个激光雷达分别与组合导航数据间转换关系的初始外参。在一个实施例中，初始外参为表征一个激光雷达与组合导航数据间转换关系的旋转外参矩阵。
步骤S120根据步骤S110所确定的每个激光雷达的初始外参，考虑基于扫描的真实空间表面中的点模型服从正态分布、激光雷达时钟源差异和车身振动的因素，对当前（每个激光雷达的）初始外参和组合导航数据进行调节，获得各个激光雷达与组合导航***间的外参，记为第一类外参，以及获得优化后的组合导航数据。这样，本发明考虑了车身振动和时间同步对组合导航***定位数据的影响，获得更为准确的第一类外参并优化了组合导航位姿。其中，所标定的第一类外参不受环境、传感器类型等因素的限定，在非结构化环境中也能完成标定。
之后，步骤S130根据步骤S120所确定的每个激光雷达的第一类外参，获得用于对各个激光雷达与组合导航***的外参的垂直分量进行补偿的补偿矩阵，从而针对每个激光雷达获得相应的补偿矩阵。
最后，步骤S140根据每个激光雷达的补偿矩阵和对应的第一类外参，结合优化后的组合导航数据，获得各个激光雷达间的第二类外参。
这样，本发明通过考虑了不平整地面对组合导航***的影响以及时间同步对多激光雷达标定的影响，通过优化激光雷达位姿而间接优化激光雷达的外参，使得第二类外参的估计结果更加准确。
在对本发明的组合标定实现方法的具体流程进行说明之前，相对本发明实施例所用到的符号定义情况和关于点的概率建模情况进行说明。
符号定义
世界坐标系被表示为，而组合导航***的定位中心被标记为。设为从坐标系A到坐标系B的坐标变换。表示m个激光雷达的
集合，其中，代表主激光雷达。集合代表激光雷达之间的外参，而代表激光雷达与组合导航***之间的外参。
设为时刻时的位姿。另外，代表时刻时的位姿。激光雷达在
时刻扫描得到的点云被表示为，它位于的局部坐标系中。最后，设
代表所有激光雷达的位姿。
点的概率建模
假设一个空间点被多个激光雷达观测到，该空间点在每个激光雷达的局部坐标系中具有不同的坐标。然而，由于点云采样的稀疏性和扫描模式的不同，被多个激光雷达采样的点并不完全相同。具体来说，由于每个激光雷达所扫描得到的真实世界表面具有分段可微特性，被激光雷达扫描得到的多个扫描测量点会位于相应的局部表面上。因此，可以对每个扫描到的测量点进行建模，该点的模型为服从正态分布，
（1）；
其中，代表一个扫描点，表示正态分布，表示该分布的平均值，表示该分
布的协方差，表示将基向量进行旋转的旋转矩阵，表示该矩阵的转置，代表沿着曲面
法线方向小的不确定性。可以认为属于同一个分段表面上的所有测量点分布在一个局部平
面上。因此，本发明实施例是最大化每个点对应的局部平面共同对齐的可能性。在实际操作
中，使用例如KD树搜索来关联属于同一局部平面的测量点，从而构建出若干个点对。
图2为本申请实施例的用于实现多激光雷达与组合导航标定的方法的具体流程图。下面参考图2，对本发明实施例所述的组合标定实现方法的具体流程进行说明。
如图2所示，依次标定激光雷达-组合导航***间的外参和多激光雷达间外参
，本发明无需输入额外的初始值。首先，激光雷达数据和组合导航***数据被输入到激光雷
达-组合导航***（也称“组合惯导***”）外参初始化模块中进行初始化处理，初始化话处
理过程使用全局优化求解器求解所构建的优化问题，从而估计每个激光雷达和组合导航系
统之间的初始外参。随后，通过两阶段方法来估计激光雷达-组合导航***的外参。第一阶
段为粗标定阶段，该阶段在不考虑定位数据误差的情况下构建模型并进行优化，得到粗标
定的结果；第二阶段为精标定阶段，该阶段考虑了车身振动和时间不同步对定位精度的影
响，构建更为全面的模型进行优化，得到精标定的结果。地面对齐过程的目的是补偿激光雷
达和组合导航***的外参（由于车辆进行平面运动，垂直与地面的方向上是不可观的，即外
参中该方向的分量是不准确的），并利用补偿之后的外参，计算激光雷达的初始位姿。最后，
基于构建的模型联合优化所有激光雷达位姿，并根据所有激光雷达优化后的位姿间接计算
多激光雷达之间的外参。
在步骤S110中，本发明实施例会在获得多组原始点云数据和原始的组合导航数据之后，根据当前多组原始点云数据和原始的组合导航数据，计算表征每个激光雷达分别与原始组合导航数据间转换关系的初始外参。
具体地，首先，步骤S1101（未图示），将所有激光雷达的原始点云数据从激光雷达坐标系变换到世界坐标系后进行点云拼接，获得每个激光雷达的局部点云地图模型。其中，局部点云地图模型含有相应激光雷达与组合导航***间的第一类外参参数。而后，步骤S1102（未图示）通过搜索步骤S1101生成的局部点云地图模型中的每个点的最近邻点，从而将所有点与其最近邻点所形成的点对之间的距离之和作为误差，以构建每个激光雷达与组合导航***之间的旋转矩阵。其中，旋转矩阵为关于误差的最小化求取后而形成的矩阵。最后，步骤S1103（未图示）将平移矩阵赋值为零，并求解每个激光雷达与组合导航***之间的旋转矩阵，获得每个激光雷达的初始旋转外参矩阵，用以表征初始外参。
通常情况下，每个激光雷达与对应车辆中的组合导航***的初始外参可以通过车辆的计算机辅助设计（CAD）模型或手眼标定方法来获取。然而，在某些情况下，由于无法获得CAD模型或无法准确估计激光雷达里程计，导致无法获得初始外参。因此，本发明实施例采用全局优化求解器来求解以下建立的优化问题，从而获得激光雷达和组合导航之间的外参。
首先，将当前自动驾驶车辆内的所有激光雷达实时采集到的原始点云数据从激光雷达坐标系变换到世界坐标系后，对所有点云数据进行拼接，针对每个激光雷达均得到位于世界坐标系中的局部点云地图，其中，在每个局部点云地图中形成多个分段表面，每个分段表面中具有多个测量点。其中，局部点云地图模型利用如下表达式表示：
（2）；
其中，表示局部点云地图模型，表示激光雷达扫描的原始点云数据，
表示所有点对应时刻的集合，表示中的第k个点的模型，表示点在世界坐
标系下的表示，表示车辆中的组合导航***的定位中心在采样时刻的位姿，表示
激光雷达到组合导航***的（待估计的）外参参数。
然后，为局部点云地图模型中的每个点搜索最近邻点，并将所有点对之间的距离求和作为误差，最小化这个误差，从而求解激光雷达和组合导航***的旋转矩阵，而平移矩阵设置为0。其中，旋转矩阵利用如下表示表示：
（3）；
其中，表示激光雷达与组合导航***间的旋转矩阵，表示基
于地图计算的误差。
最后，通过nlopt库提供的线性逼近约束优化（COBYLA）方法，求解上述表达式（3）
中右式的问题，从而获得每个激光雷达的初始旋转矩阵外参，以将所有的构成
集合中。
在获得每个激光雷达的初始旋转外参后，进入到步骤S120中。在获得每个激光雷达与组合导航***间的初始旋转外参矩阵后，进行激光雷达与组合导航***间的标定。
具体地，首先，步骤S1201（未图示）计算同一激光雷达采样到的任意两点所在真实空间局部表面间的第一面面距离。而后，步骤S1202（未图示）根据步骤S1201得到的第一面面距离、每个激光雷达的初始旋转外参矩阵和当前车辆的原始组合导航数据，参考基于扫描的真实空间表面中的点模型服从正态分布的因素，构建用来对激光雷达与组合导航***间的外参进行粗调节的外参粗调模型。之后，步骤S1203（未图示）通过对外参粗调模型进行最小估计来求解外参粗调模型，获得每个激光雷达的估计后外参旋转矩阵。接下来，根据步骤S1201得到的第一面面距离、每个激光雷达的估计后外参旋转矩阵和当前车辆的原始组合导航数据，参考基于激光雷达时钟源差异和车身振动的因素，构建用来对激光雷达与组合导航***间的外参进行精调节的外参调节模型。最后，步骤S1204（未图示）通过对外参调节模型进行最小估计来求解外参调节模型，获得每个激光雷达的第一类外参和优化后的组合导航位姿。
首先，使用下述公式计算同一激光雷达中任意两个测量点所属局部平面（局部表面）之间的距离，记为第一面面距离，这些距离服从正态分布。其中，第一面面距离利用如下表达式计算得到：
（4）；
其中，表示在时刻由激光雷达采样到的第k个点所在的局部平面（表面）
与在时刻采样到的第l个点所在的局部平面（表面）之间的第一面面距离，表示激光雷
达到组合导航***的（待估计的）外参参数，、分别表示车辆中的组合导航***
的定位中心在时刻和的位姿，、分别表示由激光雷达采样到的第k个点的模型
与在时采样到的第l个点的模型，、分别表示激光雷达在时刻和时刻下的位
姿。
以下表达式（5）为外参粗调模型的表达式，该模型根据不同点对之间的面-面距离以及步骤S110得到的初始旋转外参矩阵构建而成，通过求解下述表达式（5）中右式的问题，从而获得每个激光雷达的估计后外参旋转矩阵，即为各个激光雷达和组合导航***之间的粗估计外参。
其中，外参粗调模型利用如下表达式表示：
（5）；
其中，表示激光雷达的外参粗调模型，表示鲁棒核函数，表示第一个
残差块的协方差矩阵（计算马氏距离所使用），表示激光雷达到组合导航***间
的初始旋转外参矩阵，表示待估计的激光雷达到组合导航***间的估计后外参旋转
矩阵，Log()表示直接将SO(3)空间映射到向量空间的符号表示，表示第二个残差块的
协方差矩阵（计算马氏距离所使用）。
在真实世界中，有两个因素可能影响上述粗估计外参求解结果的准确性。首先，不平整的地面条件可能导致车辆车身振动。其次，由于不同的时钟源，时间戳的微小错位可能会发生。总的来说，这两个因素导致每帧点云数据对应的组合导航***输出的位姿存在偏差，从而导致外参的估计偏差。
因此，为了改进粗标定得到的结果，通过联合优化组合导航***位姿和激光雷达-组合导航***外参来进行精标定。在本发明实施例中，根据不同点对之间的面-面距离、粗标定中得到的外参矩阵（估计后外参旋转矩阵）以及组合导航***定位数据进行构建，通过求解下述表达式（6）中右式的问题，从而获得每个激光雷达的第一类外参，即各个激光雷达和组合导航***之间的精确外参，同时获得优化后的组合导航位姿。
其中，外参调节模型利用如下表达式表示：
（6）；
其中，表示激光雷达的外参调节模型，表示第一个残差块的协方差矩阵
（计算马氏距离所使用），表示由组合导航***实时测得的组合导航数据，
表示待估计的优化后的组合导航位姿，表示第二个和第三个残差块的协方差矩阵（计算
马氏距离所使用），表示粗调节后的激光雷达的估计后外参旋转矩阵，表
示待估计的激光雷达的第一类外参，表示直接将SE(3)空间映射到向量空间的
符号。
在获得各激光雷达和组合惯导***间的精确外参后，进入到步骤S130中，以获得用于补偿各个激光雷达和组合导航***的外参的矩阵。
在步骤S130中的构建每个激光雷达的补偿矩阵的过程中，以预设目标定位中心为基准，根据指定真实空间表面上的点分别在不同雷达激光坐标系下的对应表示，并将不同激光雷达的第一类外参作为对应的退化外参，构建并求解用于将任意两个激光雷达点云数据中的地平面进行对齐处理的补偿矩阵。
在本发明实施例中，地面对齐（找齐）的目的是补偿激光雷达和组合导航***的外参。由于车辆轨迹通常是位于一个近似平面上，缺乏与地面垂直的显著运动，这种情况便引入了各个激光雷达与组合导航***之间外参的不可观测分量。
因此，在本发明实施例中，利用补偿矩阵构建如下表达式表示：
（7）；
其中，、分别表示激光雷达和激光雷达的第一类外参（退化外参），表
示补偿矩阵，、分别代表同一指定真实空间表面点分别在 和坐标系下的不同
表示。
在求解当前补偿矩阵的过程中，包括：先将公式（7）转换为点云配准表达式，再将补偿矩阵中的补偿矩阵参量分解为坐标旋转矩阵参量和坐标平移矩阵参量，从而通过提取当前两个激光雷达点云数据中的地平面来计算相应的坐标旋转矩阵和坐标平移矩阵，继而获得补偿矩阵的求解结果。
先对表达式（7）进行简化，形成点云配准表达式。其中，点云配准表达式利用如下表达式表示：
（8）；
其中，表示指定真实空间表面点在雷达激光坐标系下的表示，表示指定
真实空间表面点在从坐标系到坐标系的坐标旋转矩阵，表示指定真实空间表面
点在雷达激光坐标系下的表示，表示表示指定真实空间表面点在从坐标系到坐
标系的坐标平移矩阵。
上述表达式（8）的形式等价于点云的配准。如果初始值较差或激光雷达之间的重
叠度低，点云的配准结果可能会发散。由于不可观方向（即垂直方向）的特征是已知的，上述
表达式（8）可以进一步简化为地面对齐问题。地面对齐问题涉及将两个激光雷达点云的地
面对齐，得到坐标转换矩阵和坐标平移矩阵的近似结果。
其中，坐标旋转矩阵和坐标平移矩阵分别利用如下表达式表示：
（9）；
其中，、分别定义了指定真实空间表面点在雷达激光坐标系和
雷达激光坐标系下的地面平面的参数化表示，、分别表示两个地平面的法向量，
、分别表示两个地平面的截距，u分别表示与和都正交的单位向量，θ表示与之间
的角度，表示坐标旋转矩阵，t表示坐标平移矩阵，表示单位矩阵，T表示转置符号。
在获得了每个激光雷达的补偿矩阵后，进入到步骤S140中，对多激光雷达间的外参进行标定。
具体地，首先，步骤S1401（未图示）根据步骤S120得到的优化后的组合导航位姿、步骤S130得到的每个激光雷达的补偿矩阵和步骤S130得到的每个激光雷达的第一类外参，计算每个激光雷达在所有时刻下的初始位姿。而后，步骤S1402（未图示）基于每个激光雷达在所有时刻下的初始位姿，计算在任意两个激光雷达在任意采样时刻下的两个分别属于不同雷达采样空间的采样点所在真实空间局部表面之间、或者同一激光雷达在任意两个采样时刻下的采样点之间的第二面面距离。之后，步骤S1403（未图示）根据步骤S1402得到的第二面面距离和步骤S1401得到的每个激光雷达的初始位姿，构建用来计算各个激光雷达在所有采样时刻下位姿的位姿优化模型。然后，步骤S1404（未图示）通过对步骤S1403所构建的位姿优化模型进行迭代计算来对位姿优化模型进行求解，从而得到不同激光雷达在所有时刻下的位姿。最后，步骤S1405（未图示）根据步骤S1404得到的不同激光雷达在所有时刻下的位姿，计算各个激光雷达相对于主激光雷达的外参，从而得到各个激光雷达间的第二类外参。
在本发明实施例中，不直接估计多个激光雷达之间的外参，因为这可能会受到激光雷达间的时间同步不一致的影响。相反地，可以通过优化激光雷达的位姿来间接获得激光雷达之间的外参。
首先，使用补偿矩阵结合优化后的组合导航***定位数据，计算所有激光雷达的初始位姿。其中，每个激光雷达的初始位姿利用如下表达式计算得到：
（10）；
其中，表示所有激光雷达的初始位姿，表示时刻下激光雷达的初
始位姿，G0表示目标定位中心，表示时刻下的优化后的组合导航位姿，表示激光
雷达的补偿矩阵，表示激光雷达的第一类外参矩阵，表示优化后的组合导航系
统的位姿集合，表示补偿矩阵的集合，表示激光雷达到组合导航***的第一类外参集
合，表示激光雷达集合，表示时间集合。
接下来，利用如下表达式计算第二面面距离：
（11）；
其中，表示激光雷达 在采样时刻下的第k个点所在的局部平面（表
面）与激光雷达在采样时刻下的第l个点所在的局部平面（表面）之间的第二面面距离，表示时刻时激光雷达的初始位姿，表示时刻时激光雷达的初始位姿，、分别表示由激光雷达在时刻采样到的第k个点与激光雷达在时刻采样
到的第l个点。
在本发明实施例中，位姿优化模型根据不同点对之间的面-面距离以及t0时刻主雷达的初始位姿进行构建，通过迭代计算方法来求解表达式（12）中右式的问题，从而求解得到所有激光雷达在所有时刻的位姿。其中，位姿优化模型利用如下表达式表示：
（12）；
其中，表示第一个残差块的协方差矩阵（计算马氏距离所使用），表示
激光雷达在时刻下的待估计的激光雷达位姿，表示通过主激光雷达 的初
始位姿而形成的单位矩阵，表示第二个残差块的协方差矩阵（计算马氏距离所使用），表示直接将SE(3)空间映射到向量空间的符号。
对表达式（12）进行求解过程中是通过迭代计算来实现的，即当前计算过程是基于
上一次估计的结果进行的，直到达到特定的停止条件。由于是通过迭代算法来进行优化，所
以初始值越准确，整个优化过程的速度和优化结果的精度就会越好。而表达式（10）中的计
算结果就是作为（12）开始优化时的初始值。具体来说，在第一次计算表达式（11）时，其中的和就是表达式（10）中所计算得到的初始位姿值。
在对激光雷达的位姿优化之后，利用如下表达式来计算各个激光雷达之间的外参，记为各个激光雷达间的第二类外参：
（13）；
其中，表示激光雷达与激光雷达之间的第二类外参，表示激光
雷达在时刻下所估计得到的激光雷达位姿，表示激光雷达在时刻下所待估计
的激光雷达位姿，Exp()将向量空间直接映射到SE(3)的符号表示，表示直接将SE
(3)空间映射到向量空间的符号表示，表示已同步的时间戳的集合。
另外，为了进一步提高本发明实施例的第二类外参的精度，本发明实施例还会在利用上述表达式（12）来计算各个激光雷达间的第二类外参的步骤之前，采用3σ原则从不同激光雷达在所有时刻下的位姿中去除异常位姿。这样，在先优化位姿再去除异常位姿后所计算得到的第二类外参，可以进一步提高外参精度。
实施例二
基于上述组合标定实现方法，本发明实施例还提供了一种计算机可读存储介质，该存储介质上存储有计算机程序，执行计算机程序以运行一种用于实现多激光雷达与组合导航标定的方法。计算机程序能够运行计算机指令，计算机指令包括计算机程序代码，计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。
计算机可读取的存储介质可以包括：能够携带计算机程序代码的任何实体或装置、记录介质、U 盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器（ROM，Read-OnlyMemory）、随机存取存储器（RAM，Random Access Memory）、电载波信号、电信信号以及软件分发介质等。
需要说明的是，计算机可读取的存储介质包含的内容可以根据司法管辖区内立法和专 利实践的要求进行适当增减，例如在某些司管辖区根据立法和专利实践的要求进行适当增减，例如在某些司管辖区根据立法和专利实践的要求进行适当增减，例如在某些司管辖区根据利实践，计算机可读取的存储介质不包括电载波信号和电信信号。
实施例三
基于上述组合标定实现方法，本发明实施例还提供了一种用于实现多激光雷达与组合导航标定的***（也称“组合标定实现***”）。图3为本申请实施例的用于实现多激光雷达与组合导航标定的***的步骤图。如图3所示，本发明实施例所述的组合标定实现***包括初始化处理模块31、第一类外参标定模块32、地面对齐处理模块33和第二类外参标定模块34。
具体地，初始化处理模块31按照上述步骤S110所述的方法来实施，配置为获得来自多个激光雷达的原始点云数据和来自组合导航***的组合导航数据，并计算表征每个激光雷达分别与组合导航数据间转换关系的初始外参；第一类外参标定模块32按照上述步骤S120所述的方法来实施，配置为根据每个激光雷达的初始外参，考虑基于扫描的真实空间表面中的点模型服从正态分布、激光雷达时钟源差异和车身振动的因素，对当前初始外参和组合导航数据进行调节，获得各激光雷达与组合导航***间的第一类外参和优化后的组合导航数据；地面对齐处理模块33按照上述步骤S130所述的方法来实施，配置为基于第一类外参，获得用于对各个激光雷达与组合导航***的外参的垂直分量进行补偿的补偿矩阵；第二类外参标定模块34按照上述步骤S140所述的方法来实施，配置为根据每个激光雷达的补偿矩阵和第一类外参，结合优化后的组合导航数据，获得各激光雷达间的第二类外参。
本发明公开了一种用于实现多激光雷达与组合导航标定的方法及***。该方法及***首先无须开展例如采集用于初始化的数据、准备标定物等任何的额外输入以及标定前准备，并提供完整的自动化多激光雷达和组合导航***标定算法，最终得到每个激光雷达和组合导航***之间的外参以及多激光雷达之间的外参。而且，本发明对环境的依赖性低，且不限定不要求特定的传感器（例如激光雷达）的类型。由于是直接基于点进行构建模型并优化求解，因此本发明不要求环境具有特定的人造特征，同时适用于结构化的人造环境及非结构化的自然环境中的标定，同时，基于点的模型适用于各类激光雷达。另外，本发明考虑了不平整地面和时间同步对组合导航***定位精度的影响，提高激光雷达和组合导航***之间的外参标定精度和准确性更高，精度提高了20%左右。此外，本发明还考虑了时间同步对多激光雷达标定的影响，通过估计激光雷达位姿从而间接估计激光雷达的外参，在多激光雷达没有严格时间同步的情况下，具有更加准确的估计结果。
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉该技术的人员在本发明所揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。
在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上；术语“上”、“下”、“左”、“右”、“内”、“外”、“前端”、“后端”、“头部”、“尾部”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”、“第三”等仅用于描述目的，而不能理解为指示或暗示相对重要性。
在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。
应该理解的是，本发明所公开的实施例不限于这里所公开的特定结构、处理步骤或材料，而应当延伸到相关领域的普通技术人员所理解的这些特征的等同替代。还应当理解的是，在此使用的术语仅用于描述特定实施例的目的，而并不意味着限制。
说明书中提到的“一个实施例”或“实施例”意指结合实施例描述的特定特征、结构或特性包括在本发明的至少一个实施例中。因此，说明书通篇各个地方出现的短语“一个实施例”或“实施例”并不一定均指同一个实施例。
虽然本发明所披露的实施方式如上，但所述的内容只是为了便于理解本发明而采用的实施方式，并非用以限定本发明。任何本发明所属技术领域内的技术人员，在不脱离本发明所揭露的精神和范围的前提下，可以在实施的形式上及细节上作任何的修改与变化，但本发明的专利保护范围，仍须以所附的权利要求书所界定的范围为准。
Claims (12)
1.一种用于实现多激光雷达与组合导航标定的方法，其特征在于，包括：
获得来自多个激光雷达的原始点云数据和来自组合导航***的组合导航数据，并计算表征每个激光雷达分别与所述组合导航数据间转换关系的初始外参；
根据每个初始外参，考虑基于扫描的真实空间表面中的点模型服从正态分布、激光雷达时钟源差异和车身振动的因素，对当前初始外参和组合导航数据进行调节，获得各激光雷达与组合导航***间的第一类外参和优化后的组合导航数据；
基于所述第一类外参，获得用于对各个激光雷达与组合导航***的外参的垂直分量进行补偿的补偿矩阵；
根据每个激光雷达的补偿矩阵和第一类外参，结合所述优化后的组合导航数据，获得各激光雷达间的第二类外参。
2.根据权利要求1所述的方法，其特征在于，通过如下步骤来计算每个初始外参：
将所有激光雷达的原始点云数据从激光雷达坐标系变换到世界坐标系后进行点云拼接，获得每个激光雷达的局部点云地图模型，所述局部点云地图模型含有相应激光雷达与组合导航***间的待估计的外参参数；
通过搜索所述局部点云地图模型中的每个点的最近邻点，从而将所有点与其最近邻点所形成的点对间的距离之和作为误差，以构建每个激光雷达与组合导航***之间的旋转矩阵，所述旋转矩阵为关于误差的最小化求取后而形成的矩阵；
将平移矩阵赋值为零并求解每个激光雷达与组合导航***之间的旋转矩阵，获得每个激光雷达的初始旋转外参矩阵，用以表征所述初始外参。
3.根据权利要求2所述的方法，其特征在于，在根据每个初始外参，考虑基于扫描的真实空间表面中的点模型服从正态分布、激光雷达时钟源差异和车身振动的因素，对当前初始外参和组合导航数据进行调节，获得各激光雷达与组合导航***间的第一类外参和优化后的组合导航数据的步骤中，包括：
计算同一激光雷达采样到的任意两点所在真实空间局部表面间的第一面面距离；
根据所述第一面面距离、每个激光雷达的初始旋转外参矩阵和所述组合导航数据，参考基于扫描的真实空间表面中的点模型服从正态分布的因素，构建用来对激光雷达与组合导航***间的外参进行粗调节的外参粗调模型；
通过对所述外参粗调模型进行最小估计来求解所述外参粗调模型，获得每个激光雷达的估计后外参旋转矩阵；
根据所述第一面面距离、每个激光雷达的估计后外参旋转矩阵和所述组合导航数据，参考基于激光雷达时钟源差异和车身振动的因素，构建用来对激光雷达与组合导航***间的外参进行精调节的外参调节模型；
通过对所述外参调节模型进行最小估计来求解所述外参调节模型，获得每个激光雷达与组合导航***间的第一类外参和优化后的组合导航位姿。
4.根据权利要求3所述的方法，其特征在于，在根据所述第一类外参，获得用于对各个激光雷达与组合导航***的外参的垂直分量进行补偿的补偿矩阵的步骤中，包括：
以预设目标定位中心为基准，根据指定真实空间表面上的点分别在不同激光雷达坐标系下的对应表示，并将不同激光雷达的第一类外参作为对应的退化外参，构建并求解用于将任意两个激光雷达点云数据中的地平面进行对齐处理的补偿矩阵。
5.根据权利要求4所述的方法，其特征在于，在根据每个激光雷达的补偿矩阵和第一类外参，结合所述组合导航数据，获得各激光雷达间的第二类外参的步骤中，包括：
根据所述优化后的组合导航位姿、以及每个激光雷达的补偿矩阵和第一类外参，计算每个激光雷达所有时刻的初始位姿；
计算在任意两个激光雷达在任意采样时刻下的两个分别属于不同雷达采样空间的采样点所在真实空间局部表面之间、或者同一激光雷达在任意两个采样时刻下的采样点之间的第二面面距离；
根据所述第二面面距离和每个激光雷达的初始位姿，构建用来计算各个激光雷达在所有采样时刻下位姿的位姿优化模型；
通过对所述位姿优化模型进行迭代计算，得到不同激光雷达在所有时刻下的位姿；
根据所述不同激光雷达在所有时刻下的位姿，计算各个激光雷达相对于主激光雷达的外参，从而得到各个激光雷达间的第二类外参。
6.根据权利要求5所述的方法，其特征在于，所述局部点云地图模型利用如下表达式表示：
；
其中，表示局部点云地图模型，/>表示激光雷达/>扫描的原始点云数据，/>表示所有点对应时刻的集合，/>表示/>中的第k个点的模型，/>表示/>点在世界坐标系下的表示，/>表示车辆中的组合导航***的定位中心在采样时刻/>的位姿，/>表示激光雷达/>到组合导航***的第一类外参参数，
所述旋转矩阵利用如下表达式表示：
；
其中，表示激光雷达与组合导航***间的旋转矩阵，/>表示基于地图计算的误差。
7.根据权利要求6所述的方法，其特征在于，所述第一面面距离利用如下表达式计算得到：
；
其中，表示在/>时刻由激光雷达/>采样到的第k个点所在的局部平面与在/>时刻采样到的第l个点所在的局部平面之间的第一面面距离，/>表示激光雷达/>到组合导航***的第一类外参，/>、/>分别表示车辆中的组合导航***的定位中心在时刻/>和/>的位姿，/>、/>分别表示由激光雷达/>采样到的第k个点的模型与在/>时采样到的第l个点的模型，/>、/>分别表示激光雷达/>在/>时刻和/>时刻下的位姿，
所述外参粗调模型利用如下表达式表示：
；
其中，表示激光雷达/>的外参粗调模型，/>表示鲁棒核函数，/>表示第一个残差块的协方差矩阵，/>表示激光雷达/>到组合导航***间的初始旋转外参矩阵，/>表示待估计的激光雷达/>到组合导航***间的估计后外参旋转矩阵，Log()表示直接将SO(3)空间映射到/>向量空间的符号表示，/>表示第二个残差块的协方差矩阵，
所述外参调节模型利用如下表达式表示：
；
其中，表示激光雷达/>的外参调节模型，/>表示第一个残差块的协方差矩阵，表示由组合导航***实时测得的组合导航数据，/>表示待估计的优化后的组合导航位姿，/>表示第二个和第三个残差块的协方差矩阵，/>表示粗调节后的激光雷达/>的估计后外参旋转矩阵，/>表示待估计的激光雷达/>的第一类外参，/>表示直接将SE(3)空间映射到/>向量空间的符号。
8.根据权利要求7所述的方法，其特征在于，所述补偿矩阵利用如下表达式表示：
；
其中，、/>分别表示作为退化外参的激光雷达/>和激光雷达/>的第一类外参，/>表示补偿矩阵，/>、/>分别代表同一指定真实空间表面点分别在/> 和/>坐标系下的不同表示，
在求解所述补偿矩阵的过程中，包括：
先将所述补偿矩阵转换为点云配准表达式，再将所述补偿矩阵中的补偿矩阵参量分解为坐标旋转矩阵参量和坐标平移矩阵参量，从而通过提取当前两个激光雷达点云数据中的地平面来计算相应的坐标旋转矩阵和坐标平移矩阵，继而获得补偿矩阵的求解结果，其中，所述点云配准表达式利用如下表达式表示：
；
其中，表示指定真实空间表面点在雷达激光坐标系/>下的表示，/>表示指定真实空间表面点在从坐标系/>到坐标系/>的坐标旋转矩阵，/>表示指定真实空间表面点在雷达激光坐标系/>下的表示，/>表示指定真实空间表面点在从坐标系/>到坐标系/>的坐标平移矩阵，
所述坐标旋转矩阵和所述坐标平移矩阵利用如下表达式表示：
；
其中，、/>分别定义了指定真实空间表面点在雷达激光坐标系/>和雷达激光坐标系/>下的地面平面的参数化表示，/>、/>分别表示两个地平面的法向量，/>、/>分别表示两个地平面的截距，u分别表示与/>和/>都正交的单位向量，θ表示/>与/>之间的角度，/>表示坐标旋转矩阵，t表示坐标平移矩阵，/>表示单位矩阵，T表示转置符号。
9.根据权利要求8所述的方法，其特征在于，所述每个激光雷达的初始位姿利用如下表达式计算得到：
；
其中，表示所有激光雷达的初始位姿，/>表示/>时刻下激光雷达/>的初始位姿，G0表示目标定位中心，/>表示/>时刻下的优化后的组合导航位姿，/>表示激光雷达的补偿矩阵，/>表示激光雷达/>的第一类外参矩阵，/>表示优化后的组合导航***的位姿集合，/>表示补偿矩阵的集合，/>表示激光雷达到组合导航***的第一类外参集合，/>表示激光雷达集合，/>表示时间集合，
所述第二面面距离利用如下表达式计算得到：
；
其中，表示激光雷达 />在/>采样时刻下的第k个点所在的局部平面与激光雷达/>在/>采样时刻下的第l个点所在的局部平面之间的第二面面距离，/>表示时刻/>时激光雷达/>的位姿，/>表示时刻/>时激光雷达/>的位姿，/>、/>分别表示由激光雷达在/>时刻采样到的第k个点与激光雷达/>在/>时刻采样到的第l个点，
所述位姿优化模型利用如下表达式表示：
；
其中，表示第一个残差块的协方差矩阵，/>表示激光雷达/>在/>时刻下的待估计的激光雷达位姿，/>表示通过激光雷达 />的初始位姿而形成的单位矩阵，/>表示第二个残差块的协方差矩阵，/>表示直接将SE(3)空间映射到/>向量空间的符号，
各个激光雷达间的第二类外参利用如下表达式计算得到：
；
其中，表示激光雷达/>与激光雷达/>之间的第二类外参，/>表示激光雷达在/>时刻下所估计得到的激光雷达位姿，/>表示激光雷达/>在/>时刻下所待估计的激光雷达位姿，Exp()将/>向量空间直接映射到SE(3)的符号表示，/>表示直接将SE(3)空间映射到/>向量空间的符号表示，/>表示已同步的时间戳的集合。
10.根据权利要求5~9中任一项所述的方法，在计算所述各个激光雷达间的第二类外参的步骤之前，所述方法还包括：
采用3σ原则从所述不同激光雷达在所有时刻下的位姿中去除异常位姿。
11.一种计算机可读存储介质，其特征在于，其包含用于执行如权利要求1~10中任一项所述的用于实现多激光雷达与组合导航标定的方法步骤的一系列指令。
12.一种用于实现多激光雷达与组合导航标定的***，其特征在于，包括：
初始化处理模块，其配置为获得来自多个激光雷达的原始点云数据和来自组合导航***的组合导航数据，并计算表征每个激光雷达分别与所述组合导航数据间转换关系的初始外参；
第一类外参标定模块，其配置为根据每个初始外参，考虑基于扫描的真实空间表面中的点模型服从正态分布、激光雷达时钟源差异和车身振动的因素，对当前初始外参和组合导航数据进行调节，获得各激光雷达与组合导航***间的第一类外参和优化后的组合导航数据；
地面对齐处理模块，其配置为基于所述第一类外参，获得用于对各个激光雷达与组合导航***的外参的垂直分量进行补偿的补偿矩阵；
第二类外参标定模块，其配置为根据每个激光雷达的补偿矩阵和第一类外参，结合所述优化后的组合导航数据，获得各激光雷达间的第二类外参。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410162956.5A CN117706530B (zh) | 2024-02-05 | 2024-02-05 | 一种用于实现多激光雷达与组合导航标定的方法及*** |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410162956.5A CN117706530B (zh) | 2024-02-05 | 2024-02-05 | 一种用于实现多激光雷达与组合导航标定的方法及*** |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117706530A CN117706530A (zh) | 2024-03-15 |
| CN117706530B true CN117706530B (zh) | 2024-05-14 |
Family
ID=90148338
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410162956.5A Active CN117706530B (zh) | 2024-02-05 | 2024-02-05 | 一种用于实现多激光雷达与组合导航标定的方法及*** |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117706530B (zh) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114325667A (zh) * | 2022-01-05 | 2022-04-12 | 上海三一重机股份有限公司 | 组合导航设备与激光雷达的外参标定方法及装置 |
| WO2022127532A1 (zh) * | 2020-12-16 | 2022-06-23 | 华为技术有限公司 | 一种激光雷达与imu的外参标定方法、装置及设备 |
| CN115728753A (zh) * | 2022-11-18 | 2023-03-03 | 深圳一清创新科技有限公司 | 激光雷达与组合导航的外参标定方法、装置及智能车辆 |
| CN116106870A (zh) * | 2023-01-31 | 2023-05-12 | 新石器慧通（北京）科技有限公司 | 车辆激光雷达外参的标定方法和装置 |
| CN116182905A (zh) * | 2023-03-02 | 2023-05-30 | 潍柴动力股份有限公司 | 激光雷达和组合惯导的时空外参标定方法、装置及*** |
| CN116973891A (zh) * | 2023-06-26 | 2023-10-31 | 广州汽车集团股份有限公司 | 车辆的激光雷达标定方法、装置、电子设备及存储介质 |
| CN117388830A (zh) * | 2023-10-23 | 2024-01-12 | 中国重汽集团济南动力有限公司 | 激光雷达与惯性导航的外参标定方法、装置、设备及介质 |
-
2024
- 2024-02-05 CN CN202410162956.5A patent/CN117706530B/zh active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2022127532A1 (zh) * | 2020-12-16 | 2022-06-23 | 华为技术有限公司 | 一种激光雷达与imu的外参标定方法、装置及设备 |
| CN114325667A (zh) * | 2022-01-05 | 2022-04-12 | 上海三一重机股份有限公司 | 组合导航设备与激光雷达的外参标定方法及装置 |
| WO2023131123A1 (zh) * | 2022-01-05 | 2023-07-13 | 上海三一重机股份有限公司 | 组合导航设备与激光雷达的外参标定方法及装置 |
| CN115728753A (zh) * | 2022-11-18 | 2023-03-03 | 深圳一清创新科技有限公司 | 激光雷达与组合导航的外参标定方法、装置及智能车辆 |
| CN116106870A (zh) * | 2023-01-31 | 2023-05-12 | 新石器慧通（北京）科技有限公司 | 车辆激光雷达外参的标定方法和装置 |
| CN116182905A (zh) * | 2023-03-02 | 2023-05-30 | 潍柴动力股份有限公司 | 激光雷达和组合惯导的时空外参标定方法、装置及*** |
| CN116973891A (zh) * | 2023-06-26 | 2023-10-31 | 广州汽车集团股份有限公司 | 车辆的激光雷达标定方法、装置、电子设备及存储介质 |
| CN117388830A (zh) * | 2023-10-23 | 2024-01-12 | 中国重汽集团济南动力有限公司 | 激光雷达与惯性导航的外参标定方法、装置、设备及介质 |
Non-Patent Citations (1)
| Title |
|---|
| 基于多对点云匹配的三维激光雷达外参数标定;韩栋斌;徐友春;王任栋;齐尧;李华;;激光与光电子学进展;20170924(第02期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117706530A (zh) | 2024-03-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111929699B (zh) | 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及*** | |
| CN108765487B (zh) | 重建三维场景的方法、装置、设备和计算机可读存储介质 | |
| WO2018142900A1 (ja) | 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム | |
| WO2023131123A1 (zh) | 组合导航设备与激光雷达的外参标定方法及装置 | |
| KR20210040217A (ko) | 자율 주행 시스템을 위한 복수의 3차원 라이다 센서의 외부 파리미터 보정 방법 | |
| CN111665512B (zh) | 基于3d激光雷达和惯性测量单元的融合的测距和绘图 | |
| CN111862214B (zh) | 计算机设备定位方法、装置、计算机设备和存储介质 | |
| CN114526745A (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及*** | |
| CN115407357A (zh) | 基于大场景的低线束激光雷达-imu-rtk定位建图算法 | |
| CN111524194A (zh) | 一种激光雷达和双目视觉相互融合的定位方法及终端 | |
| KR102490521B1 (ko) | 라이다 좌표계와 카메라 좌표계의 벡터 정합을 통한 자동 캘리브레이션 방법 | |
| KR100327120B1 (ko) | 증강현실 기반의 3차원 트래커와 컴퓨터 비젼을 이용한영상 정합 방법 | |
| CN117706530B (zh) | 一种用于实现多激光雷达与组合导航标定的方法及*** | |
| EP1307705B1 (en) | Height measurement apparatus | |
| CN113959437A (zh) | 一种用于移动测量设备的测量方法及*** | |
| CN117685953A (zh) | 面向多无人机协同定位的uwb与视觉融合定位方法及*** | |
| CN112733971A (zh) | 扫描设备的位姿确定方法、装置、设备及存储介质 | |
| CN115638788B (zh) | 语义矢量地图的构建方法、计算机设备及存储介质 | |
| CN114779272A (zh) | 一种垂直约束增强的激光雷达里程计方法及装置 | |
| Ölmez et al. | Metric scale and angle estimation in monocular visual odometry with multiple distance sensors | |
| KR102155021B1 (ko) | 점군 데이터의 3차원 직육면체 모델링 방법 및 장치 | |
| CN118115597A (zh) | 双目相机、imu和激光雷达联合标定方法及*** | |
| CN117169942A (zh) | 一种基于LiDAR/GPS/IMU融合的无人车重定位方法 | |
| CN116299542A (zh) | 高精度地图的构建方法、装置、存储介质及设备 | |
| Lee et al. | Development of Data Fusion Signal Processing Algorithms for LiDAR, RADAR, and Stereo Camera Sensors |
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 |