一种用于实现多激光雷达与组合导航标定的方法及系统

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所述的方法，其特征在于，所述每个激光雷达的初始位姿利用如下表达式计算得到： ；其中， 表示所有激光雷达的初始位姿， 表示 时刻下激光雷达 的初始位姿，G 0 表示目标定位中心， 表示 时刻下的优化后的组合导航位姿， 表示激光雷达 的补偿矩阵， 表示激光雷达 的第一类外参矩阵， 表示优化后的组合导航系统的位姿集合， 表示补偿矩阵的集合， 表示激光雷达到组合导航系统的第一类外参集合， 表示激光雷达集合， 表示时间集合，所述第二面面距离利用如下表达式计算得到： ；其中， 表示激光雷达 在 采样时刻下的第k个点所在的局部平面与激光雷达 在 采样时刻下的第l个点所在的局部平面之间的第二面面距离， 表示时刻 时激光雷达 的位姿， 表示时刻 时激光雷达 的位姿， 、 分别表示由激光雷达 在 时刻采样到的第k个点与激光雷达 在 时刻采样到的第l个点，所述位姿优化模型利用如下表达式表示： ；其中， 表示第一个残差块的协方差矩阵， 表示激光雷达 在 时刻下的待估计的激光雷达位姿， 表示通过激光雷达 的初始位姿而形成的单位矩阵， 表示第二个残差块的协方差矩阵， 表示直接将SE(3)空间映射到 向量空间的符号，各个激光雷达间的第二类外参利用如下表达式计算得到： ；其中， 表示激光雷达 与激光雷达 之间的第二类外参， 表示激光雷达 在 时刻下所估计得到的激光雷达位姿， 表示激光雷达 在 时刻下所待估计的激光雷达位姿，Exp()将 向量空间直接映射到SE(3)的符号表示， 表示直接将SE(3)空间映射到 向量空间的符号表示， 表示已同步的时间戳的集合。 10.根据权利要求5~9中任一项所述的方法，在计算所述各个激光雷达间的第二类外参的步骤之前，所述方法还包括：采用3σ原则从所述不同激光雷达在所有时刻下的位姿中去除异常位姿。 11.一种计算机可读存储介质，其特征在于，其包含用于执行如权利要求1~10中任一项所述的用于实现多激光雷达与组合导航标定的方法步骤的一系列指令。 12.一种用于实现多激光雷达与组合导航标定的系统，其特征在于，包括：初始化处理模块，其配置为获得来自多个激光雷达的原始点云数据和来自组合导航系统的组合导航数据，并计算表征每个激光雷达分别与所述组合导航数据间转换关系的初始外参；第一类外参标定模块，其配置为根据每个初始外参，考虑基于扫描的真实空间表面中的点模型服从正态分布、激光雷达时钟源差异和车身振动的因素，对当前初始外参和组合导航数据进行调节，获得各激光雷达与组合导航系统间的第一类外参和优化后的组合导航数据；地面对齐处理模块，其配置为基于所述第一类外参，获得用于对各个激光雷达与组合导航系统的外参的垂直分量进行补偿的补偿矩阵；第二类外参标定模块，其配置为根据每个激光雷达的补偿矩阵和第一类外参，结合所述优化后的组合导航数据，获得各激光雷达间的第二类外参。