本发明涉及自动驾驶，尤其是涉及一种用于实现多激光雷达与组合导航标定的方法及系统。
背景技术：
：：1、激光雷达与组合导航系统间的外参标定，测绘领域的研究主要分为两种方法：近似方法和严格方法。近似方法着重于在缺乏组合导航系定位数据的情况下减少激光雷达数据间的差异，但这可能无法完全消除所有的偏差。而严格方法在考虑系统标定误差的情况下，利用激光雷达和组合导航系统数据进行传感器标定。2、在自动驾驶领域中，对激光雷达-组合导航系统标定技术的探索相对较少，主要是使用手眼标定方法进行标定。但是手眼标定方法的标定精度一般，多被用作其他标定方法的初始值。此外，还提出了一种基于平面捆绑调整的激光雷达与组合导航系统外参标定方法。然而，该方法受限于环境平面提取的过程，只适用于旋转式机械激光雷达。3、多激光雷达系统的外参标定方法，大致可分为两类：基于目标的方法和无目标的方法。基于目标的方法通过融合来自多个传感器的数据来确定空间偏移，这就要求有可识别的物体和相应的对应点。无目标的方法则进一步细分为基于运动的和基于外观的方法。基于运动的无目标方法通过分析局部坐标系中的轨迹构建运动约束来估计外参，而基于外观的无目标技术则依赖于观测环境中物体的外观来进行传感器间的标定。4、综上所述，在目前激光雷达与组合导航系统标定的现有技术中，大多依赖于几何特征的提取，这导致了标定方法通常局限于特定类型的激光雷达，从而限制了技术的通用性。而且，在许多现行的标定方法中，没有充分考虑组合导航系统数据中可能存在的噪声问题，这会导致外参估计的准确性降低。另外，在线多激光雷达标定技术的实际应用中，或受限于激光雷达的型号，或对环境有特殊要求，这在一定程度上降低了这些标定方法的通用性。技术实现思路1、本发明的目的在于，需要提出一种通用性的多激光雷达与组合导航系统的标定方案，以降低使用局限性并有效提高外参计算的准确性。2、为了解决上述技术问题，本发明实施例提供了一种用于实现多激光雷达与组合导航标定的方法，包括：获得来自多个激光雷达的原始点云数据和来自组合导航系统的组合导航数据，并计算表征每个激光雷达分别与所述组合导航数据间转换关系的初始外参；根据每个初始外参，考虑基于扫描的真实空间表面中的点模型服从正态分布、激光雷达时钟源差异和车身振动的因素，对当前初始外参和组合导航数据进行调节，获得各激光雷达与组合导航系统间的第一类外参和优化后的组合导航数据；基于所述第一类外参，获得用于对各个激光雷达与组合导航系统的外参的垂直分量进行补偿的补偿矩阵；根据每个激光雷达的补偿矩阵和第一类外参，结合所述优化后的组合导航数据，获得各激光雷达间的第二类外参。3、优选地，通过如下步骤来计算每个初始外参：将所有激光雷达的原始点云数据从激光雷达坐标系变换到世界坐标系后进行点云拼接，获得每个激光雷达的局部点云地图模型，所述局部点云地图模型含有相应激光雷达与组合导航系统间的待估计的外参参数；通过搜索所述局部点云地图模型中的每个点的最近邻点，从而将所有点与其最近邻点所形成的点对间的距离之和作为误差，以构建每个激光雷达与组合导航系统之间的旋转矩阵，所述旋转矩阵为关于误差的最小化求取后而形成的矩阵；将平移矩阵赋值为零并求解每个激光雷达与组合导航系统之间的旋转矩阵，获得每个激光雷达的初始旋转外参矩阵，用以表征所述初始外参。4、优选地，在根据每个初始外参，考虑基于扫描的真实空间表面中的点模型服从正态分布、激光雷达时钟源差异和车身振动的因素，对当前初始外参和组合导航数据进行调节，获得各激光雷达与组合导航系统间的第一类外参和优化后的组合导航数据的步骤中，包括：计算同一激光雷达采样到的任意两点所在真实空间局部表面间的第一面面距离；根据所述第一面面距离、每个激光雷达的初始旋转外参矩阵和所述组合导航数据，参考基于扫描的真实空间表面中的点模型服从正态分布的因素，构建用来对激光雷达与组合导航系统间的外参进行粗调节的外参粗调模型；通过对所述外参粗调模型进行最小估计来求解所述外参粗调模型，获得每个激光雷达的估计后外参旋转矩阵；根据所述第一面面距离、每个激光雷达的估计后外参旋转矩阵和所述组合导航数据，参考基于激光雷达时钟源差异和车身振动的因素，构建用来对激光雷达与组合导航系统间的外参进行精调节的外参调节模型；通过对所述外参调节模型进行最小估计来求解所述外参调节模型，获得每个激光雷达与组合导航系统间的第一类外参和优化后的组合导航位姿。5、优选地，在根据所述第一类外参，获得用于对各个激光雷达与组合导航系统的外参的垂直分量进行补偿的补偿矩阵的步骤中，包括：以预设目标定位中心为基准，根据指定真实空间表面上的点分别在不同激光雷达坐标系下的对应表示，并将不同激光雷达的第一类外参作为对应的退化外参，构建并求解用于将任意两个激光雷达点云数据中的地平面进行对齐处理的补偿矩阵。6、优选地，在根据每个激光雷达的补偿矩阵和第一类外参，结合所述组合导航数据，获得各激光雷达间的第二类外参的步骤中，包括：根据所述优化后的组合导航位姿、以及每个激光雷达的补偿矩阵和第一类外参，计算每个激光雷达所有时刻的初始位姿；计算在任意两个激光雷达在任意采样时刻下的两个分别属于不同雷达采样空间的采样点所在真实空间局部表面之间、或者同一激光雷达在任意两个采样时刻下的采样点之间的第二面面距离；根据所述第二面面距离和每个激光雷达的初始位姿，构建用来计算各个激光雷达在所有采样时刻下位姿的位姿优化模型；通过对所述位姿优化模型进行迭代计算，得到不同激光雷达在所有时刻下的位姿；根据所述不同激光雷达在所有时刻下的位姿，计算各个激光雷达相对于主激光雷达的外参，从而得到各个激光雷达间的第二类外参。7、优选地，所述局部点云地图模型利用如下表达式表示：8、；9、其中，表示局部点云地图模型，表示激光雷达扫描的原始点云数据，表示所有点对应时刻的集合，表示中的第k个点的模型，表示点在世界坐标系下的表示，表示车辆中的组合导航系统的定位中心在采样时刻的位姿，表示激光雷达到组合导航系统的第一类外参参数，所述旋转矩阵利用如下表达式表示：10、；11、其中，表示激光雷达与组合导航系统间的旋转矩阵，表示基于地图计算的误差。12、优选地，所述第一面面距离利用如下表达式计算得到：13、；14、其中，表示在时刻由激光雷达采样到的第k个点所在的局部平面与在时刻采样到的第l个点所在的局部平面之间的第一面面距离，表示激光雷达到组合导航系统的第一类外参，、分别表示车辆中的组合导航系统的定位中心在时刻和的位姿，、分别表示由激光雷达采样到的第k个点的模型与在时采样到的第l个点的模型，、分别表示激光雷达在时刻和时刻下的位姿，所述外参粗调模型利用如下表达式表示：15、；16、其中，表示激光雷达的外参粗调模型，表示鲁棒核函数，表示第一个残差块的协方差矩阵，表示激光雷达到组合导航系统间的初始旋转外参矩阵，表示待估计的激光雷达到组合导航系统间的估计后外参旋转矩阵，log()表示直接将so(3)空间映射到向量空间的符号表示，表示第二个残差块的协方差矩阵，所述外参调节模型利用如下表达式表示：17、；18、其中，表示激光雷达的外参调节模型，表示第一个残差块的协方差矩阵，表示由组合导航系统实时测得的组合导航数据，表示待估计的优化后的组合导航位姿，表示第二个和第三个残差块的协方差矩阵，表示粗调节后的激光雷达的估计后外参旋转矩阵，表示待估计的激光雷达的第一类外参，表示直接将se(3)空间映射到向量空间的符号。19、优选地，所述补偿矩阵利用如下表达式表示：20、；21、其中，、分别表示作为退化外参的激光雷达和激光雷达的第一类外参，表示补偿矩阵，、分别代表同一指定真实空间表面点分别在 和坐标系下的不同表示，在求解所述补偿矩阵的过程中，包括：先将所述补偿矩阵转换为点云配准表达式，再将所述补偿矩阵中的补偿矩阵参量分解为坐标旋转矩阵参量和坐标平移矩阵参量，从而通过提取当前两个激光雷达点云数据中的地平面来计算相应的坐标旋转矩阵和坐标平移矩阵，继而获得补偿矩阵的求解结果，其中，所述点云配准表达式利用如下表达式表示：22、；23、其中，表示指定真实空间表面点在雷达激光坐标系下的表示，表示指定真实空间表面点在从坐标系到坐标系的坐标旋转矩阵，表示指定真实空间表面点在雷达激光坐标系下的表示，表示表示指定真实空间表面点在从坐标系到坐标系的坐标平移矩阵，所述坐标旋转矩阵和所述坐标平移矩阵利用如下表达式表示：24、<mtable><mtr><mtd><mi>u</mi><mi>=</mi><mfrac><mrow><msub><mi>n</mi><mn>1</mn></msub><mi>×</mi><msub><mi>n</mi><mn>2</mn></msub></mrow><mrow><msub><mi>n</mi><mn>1</mn></msub><mi>×</mi><msub><mi>n</mi><mn>2</mn></msub></mrow></mfenced></mfrac></mtd></mtr><mtr><mtd><mi>θ=</mi><mi>arccos</mi><mi>(</mi><msub><mi>n</mi><mn>1</mn></msub><mi>⋅</mi><msub><mi>n</mi><mn>2</mn></msub><mi>)</mi></mtd></mtr><mtr><mtd><mi>r</mi><mi>=</mi><mi>i</mi><mi>+[</mi><mi>u</mi><msub><mi>]</mi><mi>×</mi></msub><mi>sin</mi><mi>θ+[</mi><mi>u</mi><msubsup><mi>]</mi><mi>×</mi><mn>2</mn></msubsup><mi>(1-</mi><mi>cos</mi><mi>θ)</mi></mtd></mtr><mtr><mtd><mi>t</mi><mi>=[0,0,</mi><msub><mi>d</mi><mn>1</mn></msub><mi>-</mi><msub><mi>d</mi><mn>2</mn></msub><msup><mi>]</mi><mi>t</mi></msup></mtd></mtr></mtable></mfenced>；25、其中，<mi>[</mi><msubsup><mi>n</mi><mn>1</mn><mi>t</mi></msubsup><mi>,</mi><msub><mi>d</mi><mn>1</mn></msub><mi>]</mi>、<mi>[</mi><msubsup><mi>n</mi><mn>2</mn><mi>t</mi></msubsup><mi>,</mi><msub><mi>d</mi><mn>2</mn></msub><mi>]</mi>分别定义了指定真实空间表面点在雷达激光坐标系和雷达激光坐标系下的地面平面的参数化表示，、分别表示两个地平面的法向量，、分别表示两个地平面的截距，u分别表示与和都正交的单位向量，θ表示与之间的角度，表示坐标旋转矩阵，t表示坐标平移矩阵，表示单位矩阵，t表示转置符号。26、优选地，所述每个激光雷达的初始位姿利用如下表达式计算得到：27、；28、其中，表示所有激光雷达的初始位姿，表示时刻下激光雷达的初始位姿，g0表示目标定位中心，表示时刻下的优化后的组合导航位姿，表示激光雷达的补偿矩阵，表示激光雷达的第一类外参矩阵，表示优化后的组合导航系统的位姿集合，表示补偿矩阵的集合，表示激光雷达到组合导航系统的第一类外参集合，表示激光雷达集合，表示时间集合，所述第二面面距离利用如下表达式计算得到：29、；30、其中，表示激光雷达 在采样时刻下的第k个点所在的局部平面与激光雷达在采样时刻下的第l个点所在的局部平面之间的第二面面距离，表示时刻时激光雷达的位姿，表示时刻时激光雷达的位姿，、分别表示由激光雷达在时刻采样到的第k个点与激光雷达在时刻采样到的第l个点，31、所述位姿优化模型利用如下表达式表示：32、；33、其中，表示第一个残差块的协方差矩阵，表示激光雷达在时刻下的待估计的激光雷达位姿，表示通过激光雷达 的初始位姿而形成的单位矩阵，表示第二个残差块的协方差矩阵，表示直接将se(3)空间映射到向量空间的符号，各个激光雷达间的第二类外参利用如下表达式计算得到：34、；35、其中，表示激光雷达与激光雷达之间的第二类外参，表示激光雷达在时刻下所估计得到的激光雷达位姿，表示激光雷达在时刻下所待估计的激光雷达位姿，exp()将向量空间直接映射到se(3)的符号表示，表示直接将se(3)空间映射到向量空间的符号表示，表示已同步的时间戳的集合。36、优选地，在计算所述各个激光雷达间的第二类外参的步骤之前，所述方法还包括：采用3σ原则从所述不同激光雷达在所有时刻下的位姿中去除异常位姿。37、另一方面，本发明实施例提供了一种计算机可读存储介质，其包含用于执行如上述所述的用于实现多激光雷达与组合导航标定的方法步骤的一系列指令。38、另外，本发明实施例还提供了一种用于实现多激光雷达与组合导航标定的系统，包括：初始化处理模块，其配置为获得来自多个激光雷达的原始点云数据和来自组合导航系统的组合导航数据，并计算表征每个激光雷达分别与所述组合导航数据间转换关系的初始外参；第一类外参标定模块，其配置为根据每个初始外参，考虑基于扫描的真实空间表面中的点模型服从正态分布、激光雷达时钟源差异和车身振动的因素，对当前初始外参和组合导航数据进行调节，获得各激光雷达与组合导航系统间的第一类外参和优化后的组合导航数据；地面对齐处理模块，其配置为基于所述第一类外参，获得用于对各个激光雷达与组合导航系统的外参的垂直分量进行补偿的补偿矩阵；第二类外参标定模块，其配置为根据每个激光雷达的补偿矩阵和第一类外参，结合所述优化后的组合导航数据，获得各激光雷达间的第二类外参。39、与现有技术相比，上述方案中的一个或多个实施例可以具有如下优点或有益效果：40、本发明提出了一种用于实现多激光雷达与组合导航标定的方法及系统。该方法及系统首先无须开展例如采集用于初始化的数据、准备标定物等任何的额外输入以及标定前准备，并提供完整的自动化多激光雷达和组合导航系统标定算法，最终得到每个激光雷达和组合导航系统之间的外参以及多激光雷达之间的外参。而且，本发明对环境的依赖性低，且不限定不要求特定的传感器（例如激光雷达）的类型。由于是直接基于点进行构建模型并优化求解，因此本发明不要求环境具有特定的人造特征，同时适用于结构化的人造环境及非结构化的自然环境中的标定，同时，基于点的模型适用于各类激光雷达。另外，本发明考虑了不平整地面和时间同步对组合导航系统定位精度的影响，提高激光雷达和组合导航系统之间的外参标定精度和准确性更高，精度提高了20%左右。此外，本发明还考虑了时间同步对多激光雷达标定的影响，通过估计激光雷达位姿从而间接估计激光雷达的外参，在多激光雷达没有严格时间同步的情况下，具有更加准确的估计结果。41、本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在说明书、权利要求书以及附图中所特别指出的结构来实现和获得。当前第1页12当前第1页12