CN116182905A - 激光雷达和组合惯导的时空外参标定方法、装置及系统 - Google Patents

激光雷达和组合惯导的时空外参标定方法、装置及系统 Download PDF

Info

Publication number
CN116182905A
CN116182905A CN202310217039.8A CN202310217039A CN116182905A CN 116182905 A CN116182905 A CN 116182905A CN 202310217039 A CN202310217039 A CN 202310217039A CN 116182905 A CN116182905 A CN 116182905A
Authority
CN
China
Prior art keywords
inertial navigation
radar
information
parameter
time offset
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.)
Pending
Application number
CN202310217039.8A
Other languages
English (en)
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.)
Weichai Power Co Ltd
Weifang Weichai Power Technology Co Ltd
Original Assignee
Weichai Power Co Ltd
Weifang Weichai Power Technology Co Ltd
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 Weichai Power Co Ltd, Weifang Weichai Power Technology Co Ltd filed Critical Weichai Power Co Ltd
Priority to CN202310217039.8A priority Critical patent/CN116182905A/zh
Publication of CN116182905A publication Critical patent/CN116182905A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • 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
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Manufacturing & Machinery (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本申请提供一种激光雷达和组合惯导的时空外参标定方法、装置及系统，涉及自动驾驶技术领域。其中，该方法包括：获取激光点云数据和组合惯导数据；得到待用激光点云数据，得到待用组合惯导数据；确定雷达里程计信息；确定组合惯导里程计信息；根据雷达里程计信息和组合惯导里程计信息，标定初步的时间偏移参数；标定初步的空间参数；确定精准的时间偏移参数和精准的外参矩阵；确定待校验雷达里程计信息和待校验组合惯导里程计信息；标定目标时间偏移参数和目标空间参数，以完成激光雷达和组合惯导的时空外参标定。本申请具有能提高时空外参标定的精度的优点。

Description

激光雷达和组合惯导的时空外参标定方法、装置及系统
技术领域
本申请实施例涉及自动驾驶技术领域，尤其涉及一种激光雷达和组合惯导的时空外参标定方法、装置及系统。
背景技术
在自动驾驶技术的发展过程中，需要将激光雷达感知的障碍物信息投影到世界坐标系下，以及在多传感器融合定位时需要融合激光雷达及组合惯导的位置信息。通常激光雷达和组合导航安装在车辆不同的位置，组合导航一般安装在，激光雷达一般安装在车外。因此不仅需要获取激光雷达和组合惯导之间的空间位置关系，还需要对传感器数据进行时间同步，因此对激光雷达和组合惯导的时间及空间外参标定是自动驾驶的关键步骤之一。
目前，现有技术中，自动驾驶的交通工具上安装激光雷达和组合导航系统后，基于激光雷达采集的激光点云数据得到第一齐次变换矩阵，基于组合导航系统采集的组合惯导数据得到第二齐次变换矩阵，之后根据第一齐次变换矩阵和第二齐次变换矩阵，完成激光雷达和组合惯导之间的时空外参标定。
然而，发明人发现现有技术仍然存在时空外参标定精度亟待提高的问题。
发明内容
本申请提供一种激光雷达和组合惯导的时空外参标定方法、装置及系统，解决了现有技术中进行激光雷达和组合惯导的时空外参标定时仍然存在的标定精度有待提高的问题，本申请能提高时空外参标定的精度。
第一方面，本申请提供一种激光雷达和组合惯导的时空外参标定方法，包括：
获取激光点云数据和组合惯导数据；
对所述激光点云数据进行校验处理和噪声过滤处理，得到待用激光点云数据，并对所述组合惯导数据进行校验处理和噪声过滤处理，得到待用组合惯导数据；
根据所述待用激光点云数据，确定雷达里程计信息；根据所述待用组合惯导数据，确定组合惯导里程计信息；
根据所述雷达里程计信息和所述组合惯导里程计信息，标定初步的时间偏移参数；
根据所述初步的时间偏移参数、所述雷达里程计信息和所述组合惯导里程计信息，标定初步的空间参数；
根据所述初步的时间偏移参数、所述初步的空间参数、所述雷达里程计信息和所述组合惯导里程计信息，确定精准的时间偏移参数和精准的外参矩阵；
根据所述精准的时间偏移参数、所述精准的外参矩阵、所述雷达里程计信息和所述组合惯导里程计信息，确定待校验雷达里程计信息和待校验组合惯导里程计信息；
根据所述精准的时间偏移参数、所述精准的外参矩阵、所述待校验雷达里程计信息和所述待校验组合惯导里程计信息，标定目标时间偏移参数和目标空间参数，以完成激光雷达和组合惯导的时空外参标定。
在一种可能的实现方式中，所述对所述激光点云数据进行校验处理和噪声过滤处理，得到待用激光点云数据，并对所述组合惯导数据进行校验处理和噪声过滤处理，得到待用组合惯导数据，包括：通过轨迹评估工具对所述激光点云数据进行精确度校验处理，确定轨迹精确度高的激光点云数据；通过非零相低通滤波器对所述轨迹准确度高的激光点云数据进行噪声过滤处理，得到待用激光点云数据；对所述组合惯导数据进行固定解校验处理，确定有固定解的组合惯导数据；通过所述非零相低通滤波器对所述有固定解的组合惯导数据进行噪声过滤处理，得到待用组合惯导数据。
在一种可能的实现方式中，所述雷达里程计信息包括雷达角速度信息、雷达线加速度信息和雷达旋转位姿信息，所述组合惯导里程计信息包括含惯导角速度信息、惯导线加速度信息和组合惯导旋转位姿信息；相应地，所述根据所述雷达里程计信息和所述组合惯导里程计信息，标定初步的时间偏移参数，包括：根据所述雷达角速度信息和雷达线加速信息，确定雷达角速度测量值和雷达线加速度测量值；根据所述惯导角速度信息和惯导线加速度信息，确定惯导角速度测量值和惯导线加速度测量值；根据所述雷达角速度测量值、所述雷达线加速度测量值、所述惯导角速度测量值和所述惯导线加速度测量值，确定提前的组合惯导时间数据；根据所述提前的组合惯导时间数据，确定离散步长；根据所述离散步长、所述雷达旋转位姿信息和所述组合惯导旋转位姿信息，标定初步的时间偏移参数。
在一种可能的实现方式中，所述雷达角速度信息包括雷达角速度测量值、雷达角速度偏置值和雷达角速度噪声值，所述雷达线加速度信息包括雷达线加速度测量值、雷达线加速度偏置值和雷达线加速度噪声值；相应地，所述根据所述雷达角速度信息和雷达线加速信息，确定雷达角速度测量值和雷达线加速度测量值，的计算公式为：
Figure BDA0004115354920000031
Figure BDA0004115354920000032
式中，ωmL为雷达角速度测量值，amL为雷达线加速度测量值，所述
Figure BDA0004115354920000033
为雷达角速度真值，/>
Figure BDA0004115354920000034
为雷达线加速度真值，bwL为雷达角速度偏置值，baL为雷达线加速度偏置值，nwL为雷达角加速度噪声值，nL为雷达线加速度噪声值。
在一种可能的实现方式中，所述惯导角速度信息包括惯导角速度测量值、惯导角速度偏置值和惯导角速度噪声值，所述惯导线加速度信息包括惯导线加速度测量值、惯导线加速度偏置值和惯导线加速度噪声值；相应地，所述根据所述惯导角速度信息和惯导线加速信息，确定惯导角速度测量值和惯导线加速度测量值，的计算公式为：
Figure BDA0004115354920000035
Figure BDA0004115354920000036
式中，ωmi为惯导角速度测量值，ami为惯导线加速度测量值，所述
Figure BDA0004115354920000037
为惯导角速度真值，/>
Figure BDA0004115354920000038
为惯导线加速度真值，bw为惯导角速度偏置值，ba为惯导线加速度偏置值，nwi为惯导角速度噪声值，ni为惯导线加速度噪声值。/>
在一种可能的实现方式中，所述提前的组合惯导时间数据包括雷达与组合导航之间的时间偏移和相邻两帧激光雷达之间的时间间隔；相应地，所述根据所述提前的组合惯导时间数据，确定离散步长，的计算公式为：
Figure BDA0004115354920000039
式中，d为离散步长，
Figure BDA0004115354920000041
为雷达与组合导航之间的时间偏移值，Δt为雷达与组合导航之间的时间偏移和相邻两帧激光雷达之间的时间间隔。
在一种可能的实现方式中，所述根据所述离散步长、所述雷达旋转位姿信息和所述组合惯导旋转位姿信息，标定初步的时间偏移参数，的计算公式为：
d*＝argmax∑||ωik+d||*||ωLK||
式中，d*待求解的初步的时间偏移参数，ωik+d为k+d时刻的组合惯导旋转位姿信息，d为离散步长，ωLk为k时刻的雷达旋转位姿信息。
在一种可能的实现方式中，所述初步的空间参数包括初步的旋转外参和初步的平移外参；相应地，所述根据所述初步的时间偏移参数、所述雷达里程计信息和所述组合惯导里程计信息，标定初步的空间参数，包括：根据所述初步的时间偏移、所述雷达里程计信息和所述组合惯导里程计信息，确定初步的外参矩阵；根据所述初步的外参矩阵，确定初步的旋转外参；根据所述旋转外参，确定初步的平移外参。
在一种可能的实现方式中，所述组合惯导里程计信息还包括惯导旋转速度信息和惯导角度信息；相应地，所述根据所述初步的时间偏移参数、所述初步的空间参数、所述雷达里程计信息和所述组合惯导里程计信息，确定精准的时间偏移参数和精准的外参矩阵，包括：根据所述初步的时间偏移参数、所述初步的空间参数、所述雷达里程计信息、所述惯导旋转速度信息和所述惯导角度信息，确定精准的时间偏移参数和精准的外参矩阵。
在一种可能的实现方式中，所述根据所述初步的时间偏移参数、所述初步的空间参数、所述雷达里程计信息、所述惯导旋转速度信息和所述惯导角度信息，确定精准的时间偏移参数和精准的外参矩阵，的计算公式为：
Figure BDA0004115354920000042
式中，RLi *为待求解的精准的外参矩阵，bω *为待求解的偏置值，δt*为待求解的精准的时间偏移参数，RLi为初步的空间参数，ωLk为k时刻的雷达旋转位姿信息、bω为惯导角速度偏置值、
Figure BDA0004115354920000043
为K时刻的惯导旋转速度、δt为初步的时间偏移参数，/>
Figure BDA0004115354920000051
为k时刻的惯导角度。
在一种可能的实现方式中，所述精准的外参矩阵包括平移向量和旋转向量，所述待校验雷达里程计信息包括雷达位姿变换矩阵，所述待校验惯导里程计信息包括惯导位姿变换矩阵；相应地，所述根据所述精准的时间偏移参数、所述精准的外参矩阵、所述待校验雷达里程计信息和所述待校验组合惯导里程计信息，标定目标时间偏移参数和目标空间参数，以完成激光雷达和组合惯导的时空外参标定，包括：根据所述精准的时间偏移参数、所述平移向量、所述旋转向量、所述雷达位姿变换矩阵和所述惯导位姿变换矩阵，标定目标时间偏移参数和目标空间参数，以完成激光雷达和组合惯导的时空外参标定。
在一种可能的实现方式中，所述根据所述精准的时间偏移参数、所述平移向量、所述旋转向量、所述雷达位姿变换矩阵和所述惯导位姿变换矩阵，标定目标时间偏移参数和目标空间参数，的计算公式为：
Figure BDA0004115354920000052
式中，q*为目标空间参数矩阵，t*为目标时间偏移参数矩阵，λ1、λ2和λ为预设未旋转矩阵的权重系数，v`i为矩阵
Figure BDA0004115354920000053
的向量表示形式，vi为矩阵/>
Figure BDA0004115354920000054
的向量表示形式，
Figure BDA0004115354920000055
为第i帧雷达和第i+1帧雷达之间的位姿变换矩阵，/>
Figure BDA0004115354920000056
为第i帧惯导和第i+1帧惯导之间的位姿变换矩阵，I为预存单位矩阵，t为平移向量，q为旋转向量，/>
Figure BDA0004115354920000057
为旋转向量q的模长。
在一种可能的实现方式中，所述根据所述精准的时间偏移参数、所述精准的外参矩阵、所述待校验雷达里程计信息和所述待校验组合惯导里程计信息，标定目标时间偏移参数和目标空间参数之后，还包括：对所述待校验雷达里程计信息和所述待校验组合惯导里程计信息进行轨迹校验处理，生成轨迹校验结果；若判定所述轨迹校验结果满足预设校验条件，则将所述目标时间偏移参数和所述目标空间参数作为时空标定结果输出；若判定所述轨迹校验结果满足预设校验失败条件，则返回到所述根据所述精准的时间偏移参数、所述精准的外参矩阵、所述雷达里程计信息和所述组合惯导里程计信息，确定待校验雷达里程计信息和待校验组合惯导里程计信息的步骤。
在一种可能的实现方式中，所述获取激光点云数据和组合惯导数据，包括：在开阔的场景下获取激光点云数据和组合惯导数据。
第二方面，本申请提供了一种激光雷达和组合惯导的时空外参标定装置，包括：
数据获取模块，用于获取激光点云数据和组合惯导数据；
数据预处理模块，对所述激光点云数据进行校验处理和噪声过滤处理，得到待用激光点云数据，并对所述组合惯导数据进行校验处理和噪声过滤处理，得到待用组合惯导数据；
里程计模块，用于根据所述待用激光点云数据，确定雷达里程计信息；用于根据所述待用组合惯导数据，确定组合惯导里程计信息；
时空外参标定模块，用于根据所述雷达里程计信息和所述组合惯导里程计信息，标定初步的时间偏移参数；
所述时空外参标定模块，还用于根据所述初步的时间偏移参数、所述雷达里程计信息和所述组合惯导里程计信息，标定初步的空间参数；
所述时空外参标定模块，还用于根据所述初步的时间偏移参数、所述初步的空间参数、所述雷达里程计信息和所述组合惯导里程计信息，确定精准的时间偏移参数和精准的外参矩阵；
所述里程计模块，还用于根据所述精准的时间偏移参数、所述精准的外参矩阵、所述雷达里程计信息和所述组合惯导里程计信息，确定待校验雷达里程计信息和待校验组合惯导里程计信息；
所述时空外参标定模块，还用于根据所述精准的时间偏移参数、所述精准的外参矩阵、所述待校验雷达里程计信息和所述待校验组合惯导里程计信息，标定目标时间偏移参数和目标空间参数，以完成激光雷达和组合惯导的时空外参标定。
第三方面，本申请提供了一种激光雷达和组合惯导的时空外参标定系统，包括：
激光雷达设备，用于采集激光点云数据；
惯性导航设备，用于采集组合惯导数据；
一个或多个处理器；存储装置，用于存储一个或多个程序；当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如第一方面描述的激光雷达和组合惯导的时空外参标定方法。
第四方面，本申请提供了一种计算机可读存储介质，所述计算机可读存储介质中存储有计算机一项所执行指令，当处理器执行所述计算机执行指令时，实现如上述第一方面描述的激光雷达和组合惯导的时空外参标定方法。
本申请提供的激光雷达和组合惯导的时空外参标定方法、装置及系统，先通过对获取的激光点云数据和组合惯导数据都进行校验处理和噪声过滤处理，得到待用激光点云数据和待用组合惯导数据，减少不可用的数据和数据中的噪声对时空参数标定精度的影响。进而根据所述待用激光点云数据和待用组合惯导数据，得到雷达里程计信息和组合惯导里程计信息，然后标定初步的时间偏移参数。之后根据初步的时间偏移参数、雷达里程计信息和组合惯导里程计信息，标定初步的空间参数。再采用时间偏移参数和旋转外参同步优化的方案，根据初步的时间偏移参数、初步的空间参数、雷达里程计信息和组合惯导信息，求解精准的时间偏移参数和精准的外参矩阵。得到精准的时间偏移参数之后，重新计算得到待校验雷达里程计信息和待校验组合惯导里程计信息。最后根据精准的时间偏移参数、所述精准的外参矩阵、所述待校验雷达里程计信息和所述待校验组合惯导里程计信息，标定目标时间偏移参数和目标空间参数，完成激光雷达和组合惯导的时空外参标定，进而提高时空外参标定的精度。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本申请实施例提供的激光雷达和组合惯导的时空外参标定方法的应用场景示意图；
图2为本申请实施例提供的激光雷达和组合惯导的时空外参标定方法的流程示意图；
图3为本申请实施例提供的一种激光雷达和组合惯导的时空外参标定装置的结构示意图；
图4为本申请另一实施例提供的激光雷达和组合惯导的时空外参标定装置结构示意图；
图5为本申请实施例提供的激光雷达和组合惯导的时空外参标定系统的硬件结构示意图。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
目前，在自动驾驶技术的发展过程中，需要将激光雷达感知的障碍物信息投影到世界坐标系下，以及在多传感器融合定位时需要融合激光雷达及组合惯导的位置信息；因此不仅需要知道激光雷达和组合惯导之间的空间位置关系，还需要对传感器数据进行时间同步，因此对激光雷达和组合惯导的时间及空间外参标定是自动驾驶的关键步骤之一，其中，空间外参包括世界坐标系下的x轴坐标值、y轴坐标值、z轴坐标值、以x轴为旋转轴的俯仰角pitch、以y轴为旋转轴的航向角yaw以及以z轴为旋转轴的翻滚角roll。目前，现有技术中，自动驾驶的交通工具上安装激光雷达和组合导航系统后，基于激光雷达采集的激光点云数据得到第一齐次变换矩阵，基于组合导航系统采集的组合惯导数据得到第二齐次变换矩阵，之后根据第一齐次变换矩阵和第二齐次变换矩阵，完成激光雷达和组合惯导之间的空间外参标定。发明人发现，上述现有技术在标定激光雷达和组合惯导之间的空间外参时，由于在标定空间参数的同时，未考虑时间偏移参数对空间参数标定结果的影响，因此仍然存在标定精度亟需提高的问题。
为解决上述技术问题，本申请实施例提供了以下解决问题的技术构思：首先，将组合导航系统采集的组合惯导数据和激光雷达采集的激光点云数据进行数据校验和噪声过滤处理，然后先计算出初步的时间偏移之后，进行初步的空间参数求解。之后，再计算精准的时间偏移参数和精准的空间参数，最后再重新确定新的雷达里程计信息和惯导里程计信息，并进行最终的时间偏移参数和空间偏移参数的标定，避免降低数据中的噪声和不可用数据对时空外参标定精度的影响，从初步的时空外参到最终的时空外参的过程中，不断地缩小标定误差，进而提高时空外参标定的精度。
图1为本申请实施例提供的激光雷达和组合惯导的时空外参标定方法的应用场景示意图，如图1所示，包括：服务器101、终端102、激光雷达103和组合导航系统104。
其中，服务器101，用于控制终端102运动，并获取激光雷达103在终端102的运动过程中采集激光点云数据，获取组合导航系统104在终端102的运动过程中采集位置信息，以及进行激光雷达和组合惯导的时空外参标定。终端102可以是汽车、飞机、无人机等具有水平移动和旋转姿态变化的交通工具。激光雷达103，安装于终端102的内部，激光雷达103用于发射激光信号，并接收反射信号和终端102前方物体的位置及反射强度信息，以生成激光点云数据。组合导航系统104可以由GNSS(Global Navigation Satellite System，全球导航卫星系统)和惯性导航系统(下文简称惯导系统)组成，组合导航系统104安装于终端102的内部，GNSS用于接收卫星信号，并解算卫星信号得到GNSS位置信息，惯导系统用于短时间内纠正不准确的GNSS位置信息，并最终得到精准的位置信息，生成组合惯导数据。
图2为本申请实施例提供的激光雷达和组合惯导的时空外参标定方法的流程示意图，本实施例的执行主体可以为图1所示实施例中的服务器101，也可以是其他的计算机的相关设备，对此实施例不作特别限制。
如图2所示，该激光雷达和组合惯导的时空外参标定方法包括：
S201：获取激光点云数据和组合惯导数据。
本实施例中，激光点云数据为三维激光雷达设备扫描得到的空间点的数据集，每一个空间点包含了三维坐标信息。例如：该三维坐标信息可以包括世界坐标系中的x轴坐标、y轴坐标、z轴坐标、颜色信息、反射强度信息和回波次数信息。
本实施例中，组合惯导数据为GNSS数据和惯导数据的融合数据。例如：组合惯导数据可以包括姿态数据、速度数据、位置数据和加速度数据。
S202：对激光点云数据进行校验处理和噪声过滤处理，得到待用激光点云数据，并对组合惯导数据进行校验处理和噪声过滤处理，得到待用组合惯导数据。
本实施例中，校验处理指的是判断激光点云数据和组合惯导数据的数据量是否符合要求、数据是否有效、组合惯导数据是否出现没有固定解后保留目标数据的过程。噪声过滤处理指的是减轻激光点云数据和组合惯导数据中的噪声的影响，以及滤除噪声带来的时间延迟的过程。
本实施例中，待用激光点云数据指的是校验后有效的数据，例如：标定X位置，需要X方向的数据有足够多的变化，对于姿态数据，则需要采集交通工具进行足够多的弯道行驶信息进而产生满足条件的姿态数据。当组合惯导系统在遮挡场景下会出现位置输出不准确，这种位置输出不准确往往是在没有固定解的情况下出现的。
在上述实施例的基础上，作为本申请一可选的实施例，步骤S202包括：
S202a：通过轨迹评估工具对激光点云数据进行精确度校验处理，确定轨迹精确度高的激光点云数据。
S202b：通过非零相低通滤波器对轨迹准确度高的激光点云数据进行噪声过滤处理，得到待用激光点云数据。
S202c：对组合惯导数据进行固定解校验处理，确定有固定解的组合惯导数据。
S202d：通过非零相低通滤波器对有固定解的组合惯导数据进行噪声过滤处理，得到待用组合惯导数据。
S203：根据待用激光点云数据，确定雷达里程计信息；根据待用组合惯导数据，确定组合惯导里程计信息。
本实施例中，轨迹评估工具是用于视觉里程计和定位与地图构建SLAM问题的评估工具。组合导航数据可通过观察法，确定是否有固定解。
本实施例中，雷达里程计全称为激光雷达里程计，指的是一个点云配准过程，配准后得到的转化矩阵也是当前帧相对上一帧的位姿，所有的待用激光点云数据通过特定的里程计算法得出的激光雷达里程计信息，在图中可以显示成一条用于估计交通工具移动时的位姿变换的轨迹。其中，特定的里程计算法可以是ICP(Iterative Closest Point，迭代最近点)算法，或NDT(Normal Distributions Transform，正态分布变化)算法。
本实施例中，组合惯导里程计信息指的是惯性测量单元IMU传感器的测量值来确定移动物体相对于给定起点的位置、方向、高度和线速度等数据。所有的组合惯导数据经由惯导系统处理后输出的组合惯导里程计信息，在图中可以显示成一条用于估计交通工具的位姿信息的轨迹。
S204：根据雷达里程计信息和组合惯导里程计信息，标定初步的时间偏移参数。
本实施例中，初步的时间偏移参数可通过雷达里程计信息和组合惯导里程计信息之间的互相关性计算得到。
具体地，在本申请一可选的实施例中，雷达里程计信息包括雷达角速度信息、雷达线加速度信息和雷达旋转位姿信息，组合惯导里程计信息包括含惯导角速度信息、惯导线加速度信息和组合惯导旋转位姿信息。相应地，步骤S204，包括：
S204a：根据雷达角速度信息和雷达线加速信息，确定雷达角速度测量值和雷达线加速度测量值。
以上述实施例为基础，在本申请一可选的实施例中，雷达角速度信息包括雷达角速度测量值、雷达角速度偏置值和雷达角速度噪声值，雷达线加速度信息包括雷达线加速度测量值、雷达线加速度偏置值和雷达线加速度噪声值，相应地，步骤S204a的计算公式为：
Figure BDA0004115354920000111
Figure BDA0004115354920000112
式中，ωmL为雷达角速度测量值，amL为雷达线加速度测量值，
Figure BDA0004115354920000113
为雷达角速度真值，/>
Figure BDA0004115354920000114
为雷达线加速度真值，bwL为雷达角速度偏置值，baL为雷达线加速度偏置值，nwL为雷达角加速度噪声值，nL为雷达线加速度噪声值。
S204b：根据惯导角速度信息和惯导线加速度信息，确定惯导角速度测量值和惯导线加速度测量值。
在上述实施例的基础上，作为本申请一可选的实施例，惯导角速度信息包括惯导角速度测量值、惯导角速度偏置值和惯导角速度噪声值，惯导线加速度信息包括惯导线加速度测量值、惯导线加速度偏置值和惯导线加速度噪声值。相应地，步骤S204b的计算公式为：
Figure BDA0004115354920000121
Figure BDA0004115354920000122
式中，ωmi为惯导角速度测量值，ami为惯导线加速度测量值，
Figure BDA0004115354920000123
为惯导角速度真值，/>
Figure BDA0004115354920000124
为惯导线加速度真值，bw为惯导角速度偏置值，ba为惯导线加速度偏置值，nwi为惯导角速度噪声值，ni为惯导线加速度噪声值。
S204c：根据雷达角速度测量值、雷达线加速度测量值、惯导角速度测量值和惯导线加速度测量值，确定提前的组合惯导时间数据。
S204d：根据提前的组合惯导时间数据，确定离散步长。
以上述实施例为基础，在本申请一可选的实施例中，提前的组合惯导时间数据包括雷达与组合导航之间的时间偏移和相邻两帧激光雷达之间的时间间隔。相应地，步骤S204的计算公式为：
Figure BDA0004115354920000125
式中，d为离散步长，
Figure BDA0004115354920000126
为雷达与组合导航之间的时间偏移值，Δt为雷达与组合导航之间的时间偏移和相邻两帧激光雷达之间的时间间隔。
S204e：根据离散步长、雷达旋转位姿信息和组合惯导旋转位姿信息，标定初步的时间偏移参数。
以上述实施例为基础，在本申请一可选的实施例中，步骤S204e的计算公式为：
d*＝argmax∑||ωik+d||*||ωLk||
式中，d*待求解的初步的时间偏移参数，ωik+d为k+d时刻的组合惯导旋转位姿信息，d为离散步长，ωLk为k时刻的雷达旋转位姿信息。
在本申请一可选的实施例中，激光雷达里程计信息和组合惯导里程计信息之间的旋转关系可以用如下公式表示：
ωik+d＝ωlk
本实施例中，激光雷达里程计信息和组合惯导里程计信息之间存在未知且稳定的时间偏移量，因此需要将组合惯导里程计信息提前按照上述稳定的时间偏移量与雷达里程计进行时间对齐。由于雷达里程计信息和组合惯导里程计信息在离散时间中，因此，将组合惯导里程计信息提前的操作基本上是在离散步长中进行的。此时不考虑激光雷达和组合惯导的空间外参，使用互相关性来进行量化，再通过求解步骤S204e的计算公式求解初步的时间偏移参数。当||ωik+d||与||ωLk||的乘积最大时说明二者相似性高，此时说明求得的d*为最优解。
S205：根据初步的时间偏移参数、雷达里程计信息和组合惯导里程计信息，标定初步的空间参数。
本实施例中，初步的空间参数和初步的时间偏移参数，作为激光雷达和组合惯导的时空外参。
在本申请一可选的实施例中，初步的空间参数包括初步的旋转外参和初步的平移外参。相应地，步骤S205，包括：
S205a：根据初步的时间偏移参数、雷达里程计信息和组合惯导里程计信息，确定初步的外参矩阵；
S205b：根据初步的外参矩阵，确定初步的旋转外参；
S205c：根据旋转外参，确定初步的平移外参。
本实施例中，初步的外参矩阵可采用手眼标定的方法求解得到，初步的外参矩阵可以包括旋转矩阵和平移向量。初步的旋转外参可通过对初步的外参矩阵进行求解得到，求解初步的外参矩阵之前，先按照初步的时间偏移参数将雷达里程计信息和组合惯导里程计信息进行对齐，对齐之后根据对齐后的雷达里程计信息和组合惯导里程计信息中提取数据建立等式数组。然后，可采用奇异值分解SVD法进行初步的外参矩阵求解，得到初步的旋转外参，再使用SVD法根据初步的选择外参求解得到初步的平移外参。
S206：根据初步的时间偏移参数、初步的空间参数、雷达里程计信息和组合惯导里程计信息，确定精准的时间偏移参数和精准的外参矩阵。
本实施例中，确定精准的时间偏移参数和精准的外参矩阵可采用设立代价函数并求解代价函数的方法。
具体地，在本申请一可选的实施例中，组合惯导里程计信息还包括惯导旋转速度信息和惯导角度信息。相应地，步骤S206，包括：
根据初步的时间偏移参数、初步的空间参数、雷达里程计信息、惯导旋转速度信息和惯导角度信息，确定精准的时间偏移参数和精准的外参矩阵。
以上述实施例为基础，在本申请一可选的实施例中，步骤S206的计算公式为：
Figure BDA0004115354920000141
式中，RLi *为待求解的精准的外参矩阵，bω *为待求解的偏置值，δt*为待求解的精准的时间偏移参数，RLi为初步的空间参数，ωLk为k时刻的雷达旋转位姿信息、bω为惯导角速度偏置值、
Figure BDA0004115354920000142
为K时刻的惯导旋转速度、δt为初步的时间偏移参数，/>
Figure BDA0004115354920000143
为k时刻的惯导角度。
本实施例中，由于数据是离散的，因此步骤S204d计算的离散步长是激光点云数据中相邻两帧之间的时间间隔的倍数。假设在一小段时间内角加速度是通过常量插值得到的，即ωi(tk+d*Δt+δt)≈ωik`+δtΩik`，其中，ωi为组合惯导的旋转位姿信息，Δt为相邻两帧之间的时间间隔，tk为离散时间，d为离散步长，δt为小于相邻两帧之间的时间间隔Δt的雷达里程计信息和组合惯导里程计之间的时间间隔，ba是组合惯导的旋转偏置；Ωik`是组合惯导在σt内的旋转角速度。
对步骤S206的计算公式进行求解，待求解的精准的外参矩阵RLi *为步骤S206公式等号后面求得的数值最小时最优的外参矩阵，同理，待求解的偏置值bω *为公式等号后面求得的数值最小时最优的偏置值，待求解的精准的时间偏移参数δt*为公式等号后面求得的数值最小时最优的时间偏移参数，例如可采用非线性优化问题的数值求解器ceres solver进行迭代求解，得到精确的时间偏移参数δt*。
S207：根据精准的时间偏移参数、精准的外参矩阵、雷达里程计信息和组合惯导里程计信息，确定待校验雷达里程计信息和待校验组合惯导里程计信息。
本实施例中，基于精准的时间偏移参数、精准的外参矩阵、雷达里程计信息和组合惯导里程计信息，待校验雷达里程计信息是在雷达里程计计算过程中加入惯导进行运动去畸变得到的，待校验组合惯导里程计信息可通过优化IMU偏置得到。
S208：根据精准的时间偏移参数、精准的外参矩阵、待校验雷达里程计信息和待校验组合惯导里程计信息，标定目标时间偏移参数和目标空间参数，以完成激光雷达和组合惯导的时空外参标定。
本实施例中，标定目标时间偏移参数和目标空间参数时，可通过对待校验雷达里程计信息和待校验组合惯导里程计信息这两条轨迹进行非线性最优化外参求解法进行标定。
具体地，在本申请一可选的实施例中，精准的外参矩阵包括平移向量和旋转向量，待校验雷达里程计信息包括雷达位姿变换矩阵，待校验惯导里程计信息包括惯导位姿变换矩阵。相应地，步骤S208，包括：
根据精准的时间偏移参数、平移向量、旋转向量、雷达位姿变换矩阵和惯导位姿变换矩阵，标定目标时间偏移参数和目标空间参数，以完成激光雷达和组合惯导的时空外参标定。
以上述实施例为基础，在本申请一可选的实施例中，步骤S208，的计算公式为：
Figure BDA0004115354920000151
式中，q*为目标空间参数矩阵，t*为目标时间偏移参数矩阵，λ1、λ2和λ为预设未旋转矩阵的权重系数，v`i为矩阵
Figure BDA0004115354920000152
的向量表示形式，vi为矩阵/>
Figure BDA0004115354920000153
的向量表示形式，/>
Figure BDA0004115354920000154
为第i帧雷达和第i+1帧雷达之间的位姿变换矩阵，/>
Figure BDA0004115354920000155
为第i帧惯导和第i+1帧惯导之间的位姿变换矩阵，I为预存单位矩阵，t为平移向量，q为旋转向量，/>
Figure BDA0004115354920000156
为旋转向量q的模长。
本实施例中，求解步骤S208的计算公式时可采用非线性最优化外参求解法。例如：该非线性最优化外参求解法可以是对偶四元数方法。
综上，本申请实施例提供的激光雷达和组合惯导的时空外参标定方法，先通过对获取的激光点云数据和组合惯导数据都进行校验处理和噪声过滤处理，得到待用激光点云数据和待用组合惯导数据，减少不可用的数据和数据中的噪声对时空参数标定精度的影响。进而根据待用激光点云数据和待用组合惯导数据，得到雷达里程计信息和组合惯导里程计信息，然后标定初步的时间偏移参数。之后根据初步的时间偏移参数、雷达里程计信息和组合惯导里程计信息，标定初步的空间参数。再采用时间偏移参数和旋转外参同步优化的方案，根据初步的时间偏移参数、初步的空间参数、雷达里程计信息和组合惯导信息，求解精准的时间偏移参数和精准的外参矩阵。得到精准的时间偏移参数之后，重新计算得到待校验雷达里程计信息和待校验组合惯导里程计信息，最后根据精准的时间偏移参数、精准的外参矩阵、待校验雷达里程计信息和待校验组合惯导里程计信息，标定目标时间偏移参数和目标空间参数，完成激光雷达和组合惯导的时空外参标定，进而提高时空外参标定的精度。
以上述实施例为基础，在本申请一可选的实施例中，步骤S208之后，还包括：
步骤A：对待校验雷达里程计信息和待校验组合惯导里程计信息进行轨迹校验处理，生成轨迹校验结果。
步骤B：若判定轨迹校验结果满足预设校验条件，则将目标时间偏移参数和目标空间参数作为时空标定结果输出。
步骤C：若判定轨迹校验结果满足预设校验失败条件，则返回到根据精准的时间偏移参数、精准的外参矩阵、雷达里程计信息和组合惯导里程计信息，确定待校验雷达里程计信息和待校验组合惯导里程计信息的步骤。
本实施例中，预设校验失败条件可以是轨迹校验不通过。其中，轨迹校验处理包括验证数据量的大小，验证空间参数如x轴坐标值、y轴坐标值、z轴坐标值、以x轴为旋转轴的俯仰角pitch、以y轴为旋转轴的航向角yaw以及以z轴为旋转轴的翻滚角roll这些参数的变化量是否满足预设需求。示例性的，求解上述实施例中的公式过程中，假如一个公式中有n个参数，则待雷达里程计信息和待校验组合惯导信息中的数据点的数量N要大于n，且N的数值越大越好，这样才能保证6个空间外参变化足够大，标定的空间参数也越精准。
综上，本申请实施例提供的激光雷达和组合惯导的时空外参标定方法，通过对待校验雷达里程计信息和待校验惯导里程计信息这两条轨迹进行轨迹校验，并返回到确定重新步骤S206，进而反复求解，得到目标时间偏移参数和目标空间参数，再进行轨迹校验，如此反复直至轨迹校验通过后再完成最终的激光雷达和组合惯导的时空外参标定，再输最终的目标时间偏移参数和目标空间参数，通过多次反复的计算进一步提高时空外参标定精度。
在上述实施例的基础上，作为本申请一可选的实施例，步骤S201包括：
步骤S201a：在开阔的场景下获取激光点云数据和组合惯导数据。
本实施例中，开阔的场景指的是交通工具可运行的空间为开阔无障碍物的场景。例如，无人机的开阔场景为郊区的空地上空。无人驾驶车辆在郊区的空地地面上，当标定空间参数中的航向角yaw需要足够大的侧向加速度，标定俯仰角pitch时需要足够大的俯仰加速度值，标定翻滚角roll时需要足够大的向心加速度，反馈到驾驶场景中，则是需要急转弯、起伏程度大和左右摆动行驶方向等行驶工况。
综上，本申请实施例提供的激光雷达和组合惯导的时空外参标定方法，通过在开阔场景下获取激光点云数据和组合惯导数据，以起到降低数据获取的难度和驾驶危险性的作用。
图3为本申请实施例提供的一种激光雷达和组合惯导的时空外参标定装置的结构示意图，该装置包括：数据获取模块301、数据预处理模块302、里程计模块303和时空外参标定模块304。
其中，数据获取模块301，用于获取激光点云数据和组合惯导数据。
数据预处理模块302，对激光点云数据进行校验处理和噪声过滤处理，得到待用激光点云数据，并对组合惯导数据进行校验处理和噪声过滤处理，得到待用组合惯导数据。
里程计模块303，用于根据待用激光点云数据，确定雷达里程计信息；用于根据待用组合惯导数据，确定组合惯导里程计信息。
时空外参标定模块304，用于根据雷达里程计信息和组合惯导里程计信息，标定初步的时间偏移参数。
时空外参标定模块304，还用于根据初步的时间偏移参数、雷达里程计信息和组合惯导里程计信息，标定初步的空间参数。
时空外参标定模块304，还用于根据初步的时间偏移参数、初步的空间参数、雷达里程计信息和组合惯导里程计信息，确定精准的时间偏移参数和精准的外参矩阵。
里程计模块303，还用于根据精准的时间偏移参数、精准的外参矩阵、雷达里程计信息和组合惯导里程计信息，确定待校验雷达里程计信息和待校验组合惯导里程计信息。
时空外参标定模块304，还用于根据精准的时间偏移参数、精准的外参矩阵、待校验雷达里程计信息和待校验组合惯导里程计信息，标定目标时间偏移参数和目标空间参数，以完成激光雷达和组合惯导的时空外参标定。
本实施例提供的激光雷达和组合惯导的时空外参标定装置，可用于执行上述方法实施例的技术方案，其实现原理和技术效果类似，本实施例此处不再赘述。
在本申请一可选的实施例中，数据预处理模块302，具体用于：通过轨迹评估工具对激光点云数据进行精确度校验处理，确定轨迹精确度高的激光点云数据；通过非零相低通滤波器对轨迹准确度高的激光点云数据进行噪声过滤处理，得到待用激光点云数据；对组合惯导数据进行固定解校验处理，确定有固定解的组合惯导数据；通过非零相低通滤波器对有固定解的组合惯导数据进行噪声过滤处理，得到待用组合惯导数据。
在本申请一可选的实施例中，雷达里程计信息包括雷达角速度信息、雷达线加速度信息和雷达旋转位姿信息，组合惯导里程计信息包括含惯导角速度信息、惯导线加速度信息和组合惯导旋转位姿信息。相应地，时空外参标定模块304，具体用于：根据雷达角速度信息和雷达线加速信息，确定雷达角速度测量值和雷达线加速度测量值；根据惯导角速度信息和惯导线加速度信息，确定惯导角速度测量值和惯导线加速度测量值；根据雷达角速度测量值、雷达线加速度测量值、惯导角速度测量值和惯导线加速度测量值，确定提前的组合惯导时间数据；根据提前的组合惯导时间数据，确定离散步长；根据离散步长、雷达旋转位姿信息和组合惯导旋转位姿信息，标定初步的时间偏移参数。
在本申请一可选的实施例中，雷达角速度信息包括雷达角速度测量值、雷达角速度偏置值和雷达角速度噪声值，雷达线加速度信息包括雷达线加速度测量值、雷达线加速度偏置值和雷达线加速度噪声值。相应地，时空外参标定模块304，用于确定雷达角速度测量值和雷达线加速度测量值，的计算公式为：
Figure BDA0004115354920000191
Figure BDA0004115354920000192
式中，ωmL为雷达角速度测量值，amL为雷达线加速度测量值，
Figure BDA0004115354920000193
为雷达角速度真值，/>
Figure BDA0004115354920000194
为雷达线加速度真值，bwL为雷达角速度偏置值，baL为雷达线加速度偏置值，nwL为雷达角加速度噪声值，nL为雷达线加速度噪声值。
在本申请一可选的实施例中，惯导角速度信息包括惯导角速度测量值、惯导角速度偏置值和惯导角速度噪声值，惯导线加速度信息包括惯导线加速度测量值、惯导线加速度偏置值和惯导线加速度噪声值。相应地，时空外参标定模块304，用于确定惯导角速度测量值和惯导线加速度测量值的计算公式为：
Figure BDA0004115354920000195
Figure BDA0004115354920000196
式中，ωmi为惯导角速度测量值，ami为惯导线加速度测量值，
Figure BDA0004115354920000197
为惯导角速度真值，/>
Figure BDA0004115354920000198
为惯导线加速度真值，bw为惯导角速度偏置值，ba为惯导线加速度偏置值，nwi为惯导角速度噪声值，ni为惯导线加速度噪声值。
在本申请一可选的实施例中，提前的组合惯导时间数据包括雷达与组合导航之间的时间偏移和相邻两帧激光雷达之间的时间间隔。相应地，时空外参标定模块304，用于确定离散步长的计算公式为：
Figure BDA0004115354920000199
式中，d为离散步长，
Figure BDA00041153549200001910
为雷达与组合导航之间的时间偏移值，Δt为雷达与组合导航之间的时间偏移和相邻两帧激光雷达之间的时间间隔。
在本申请一可选的实施例中，时空外参标定模块304，用于标定初步的时间偏移参数的计算公式为：
d*＝arg max∑||ωik+d||*||ωLk||
式中，d*待求解的初步的时间偏移参数，ωik+d为k+d时刻的组合惯导旋转位姿信息，d为离散步长，ωLk为k时刻的雷达旋转位姿信息。
在本申请一可选的实施例中，初步的空间参数包括初步的旋转外参和初步的平移外参。相应地，时空外参标定模块304，还具体用于：根据初步的时间偏移、雷达里程计信息和组合惯导里程计信息，确定初步的外参矩阵；根据初步的外参矩阵，确定初步的旋转外参；根据旋转外参，确定初步的平移外参。
在本申请一可选的实施例中，组合惯导里程计信息还包括惯导旋转速度信息和惯导角度信息。时空外参标定模块304，还具体用于：根据初步的时间偏移参数、初步的空间参数、雷达里程计信息、惯导旋转速度信息和惯导角度信息，确定精准的时间偏移参数和精准的外参矩阵。
在本申请一可选的实施例中，时空外参标定模块304，用于确定精准的时间偏移参数和精准的外参矩阵的计算公式为：
Figure BDA0004115354920000204
式中，RLi *为待求解的精准的外参矩阵，bω *为待求解的偏置值，δt*为待求解的精准的时间偏移参数，RLi为初步的空间参数，ωLk为k时刻的雷达旋转位姿信息、bω为惯导角速度偏置值、
Figure BDA0004115354920000201
为K时刻的惯导旋转速度、δt为初步的时间偏移参数，/>
Figure BDA0004115354920000202
为k时刻的惯导角度。
在本申请一可选的实施例中，精准的外参矩阵包括平移向量和旋转向量，待校验雷达里程计信息包括雷达位姿变换矩阵，待校验惯导里程计信息包括惯导位姿变换矩阵。相应地，时空外参标定模块304，还具体用于：根据精准的时间偏移参数、平移向量、旋转向量、雷达位姿变换矩阵和惯导位姿变换矩阵，标定目标时间偏移参数和目标空间参数，以完成激光雷达和组合惯导的时空外参标定。
在本申请一可选的实施例中，时空外参标定模块304，用于标定目标时间偏移参数和目标空间参数，的计算公式为：
Figure BDA0004115354920000203
式中，q*为目标空间参数矩阵，t*为目标时间偏移参数矩阵，λ1、λ2和λ为预设未旋转矩阵的权重系数，
vi为矩阵
Figure BDA0004115354920000211
的向量表示形式，vi为矩阵/>
Figure BDA0004115354920000212
的向量表示形式，/>
Figure BDA0004115354920000213
为第i帧雷达和第i+1帧雷达之间的位姿变换矩阵，/>
Figure BDA0004115354920000214
为第i帧惯导和第i+1帧惯导之间的位姿变换矩阵，I为预存单位矩阵，t为平移向量，q为旋转向量，/>
Figure BDA0004115354920000215
为旋转向量q的模长。
参考图4，图4为本申请另一实施例提供的激光雷达和组合惯导的时空外参标定装置结构示意图。如图4所示，在本申请一可选的实施例中，与图4所示实施例的不同之处在于，该激光雷达和组合惯导的时空外参标定装置还包括：轨迹校验模块305。
其中，轨迹校验模块305，用于对待校验雷达里程计信息和待校验组合惯导里程计信息进行轨迹校验处理，生成轨迹校验结果。时空外参标定模块304，用于若判定轨迹校验结果满足预设校验条件，则将目标时间偏移参数和目标空间参数作为时空标定结果输出。时空外参标定模块304，还用于若判定轨迹校验结果满足预设校验失败条件，则返回到里程计模块303的执行阶段。
在本申请一可选的实施例中，数据获取模块301，具体用于：在开阔的场景下获取激光点云数据和组合惯导数据。
上述装置实施例提供的激光雷达和组合惯导的时空外参标定装置，可用于执行上述方法实施例的技术方案，其实现原理和技术效果类似，此处不再赘述。
参考图5，图5为本申请实施例提供的激光雷达和组合惯导的时空外参标定系统的硬件结构示意图，如图5所示，该系统包括：激光雷达设备501、惯性导航设备502、一个或多个处理器503以及存储装置504。
其中，激光雷达设备501，用于采集激光点云数据；
惯性导航设备502，用于采集组合惯导数据；
一个或多个处理器503；存储装置504，用于存储一个或多个程序；当一个或多个程序被一个或多个处理器执行，使得一个或多个处理器503获取激光点云数据和组合惯导数据后实现上述方法实施例所涉及的各个步骤。具体可以参见上述方法实施例中的相关描述。
可选地，存储装置504既可以是独立的，也可以跟处理器503集成在一起。
当存储装置503独立设置时，该系统还包括总线505，用于连接存储装置504和处理器503。
本申请实施例还提供一种计算机可读存储介质，计算机可读存储介质中存储有计算机执行指令，当处理器执行计算机执行指令时，实现如上述方法实施例描述的步骤。
本申请实施例还提供一种计算机程序产品，包括计算机程序，该计算机程序被处理器执行时实现如上述方法实施例描述的步骤。
在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，上述模块的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个模块可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或模块的间接耦合或通信连接，可以是电性，机械或其它的形式。
上述作为分离部件说明的模块可以是或者也可以不是物理上分开的，作为模块显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案。
另外，在本发明各个实施例中的各功能模块可以集成在一个处理单元中，也可以是各个模块单独物理存在，也可以两个或两个以上模块集成在一个单元中。上述模块成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。
上述以软件功能模块的形式实现的集成的模块，可以存储在一个计算机可读取存储介质中。上述软件功能模块存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器执行本申请各个实施例的方法的部分步骤。
应理解，上述处理器可以是中央处理单元(Central Processing Unit，简称CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，简称DSP)、专用集成电路(Application Specific Integrated Circuit，简称ASIC)等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合发明所公开的方法的步骤可以直接体现为硬件处理器执行完成，或者用处理器中的硬件及软件模块组合执行完成。
存储器可能包含高速RAM存储器，也可能还包括非易失性存储NVM，例如至少一个磁盘存储器，还可以为U盘、移动硬盘、只读存储器、磁盘或光盘等。
总线可以是工业标准体系结构(Industry Standard Architecture，简称ISA)总线、外部设备互连(Peripheral Component Interconnect，简称PCI)总线或扩展工业标准体系结构(Extended Industry Standard Architecture，简称EISA)总线等。总线可以分为地址总线、数据总线、控制总线等。为便于表示，本申请附图中的总线并不限定仅有一根总线或一种类型的总线。
上述存储介质可以是由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器(SRAM)，电可擦除可编程只读存储器(EEPROM)，可擦除可编程只读存储器(EPROM)，可编程只读存储器(PROM)，只读存储器(ROM)，磁存储器，快闪存储器，磁盘或光盘。存储介质可以是通用或专用计算机能够存取的任何可用介质。
一种示例性的存储介质耦合至处理器，从而使处理器能够从该存储介质读取信息，且可向该存储介质写入信息。当然，存储介质也可以是处理器的组成部分。处理器和存储介质可以位于专用集成电路(Application Specific Integrated Circuits，简称ASIC)中。当然，处理器和存储介质也可以作为分立组件存在于电子设备或主控设备中。
本领域普通技术人员可以理解：实现上述各方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成。前述的程序可以存储于一计算机可读取存储介质中。该程序在执行时，执行包括上述各方法实施例的步骤；而前述的存储介质包括：ROM、RAM、磁碟或者光盘等各种可以存储程序代码的介质。
以说明本申请的技术方案，而非对其限制；尽管参照前述各实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的范围。

Claims (16)

1.一种激光雷达和组合惯导的时空外参标定方法，其特征在于，包括：
获取激光点云数据和组合惯导数据；
对所述激光点云数据进行校验处理和噪声过滤处理，得到待用激光点云数据，并对所述组合惯导数据进行校验处理和噪声过滤处理，得到待用组合惯导数据；
根据所述待用激光点云数据，确定雷达里程计信息；根据所述待用组合惯导数据，确定组合惯导里程计信息；
根据所述雷达里程计信息和所述组合惯导里程计信息，标定初步的时间偏移参数；
根据所述初步的时间偏移参数、所述雷达里程计信息和所述组合惯导里程计信息，标定初步的空间参数；
根据所述初步的时间偏移参数、所述初步的空间参数、所述雷达里程计信息和所述组合惯导里程计信息，确定精准的时间偏移参数和精准的外参矩阵；
根据所述精准的时间偏移参数、所述精准的外参矩阵、所述雷达里程计信息和所述组合惯导里程计信息，确定待校验雷达里程计信息和待校验组合惯导里程计信息；
根据所述精准的时间偏移参数、所述精准的外参矩阵、所述待校验雷达里程计信息和所述待校验组合惯导里程计信息，标定目标时间偏移参数和目标空间参数，以完成激光雷达和组合惯导的时空外参标定。
2.根据权利要求1所述的方法，其特征在于，所述对所述激光点云数据进行校验处理和噪声过滤处理，得到待用激光点云数据，并对所述组合惯导数据进行校验处理和噪声过滤处理，得到待用组合惯导数据，包括：
通过轨迹评估工具对所述激光点云数据进行精确度校验处理，确定轨迹精确度高的激光点云数据；
通过非零相低通滤波器对所述轨迹准确度高的激光点云数据进行噪声过滤处理，得到待用激光点云数据；
对所述组合惯导数据进行固定解校验处理，确定有固定解的组合惯导数据；
通过所述非零相低通滤波器对所述有固定解的组合惯导数据进行噪声过滤处理，得到待用组合惯导数据。
3.根据权利要求1所述的方法，其特征在于，所述雷达里程计信息包括雷达角速度信息、雷达线加速度信息和雷达旋转位姿信息，所述组合惯导里程计信息包括含惯导角速度信息、惯导线加速度信息和组合惯导旋转位姿信息；
相应地，所述根据所述雷达里程计信息和所述组合惯导里程计信息，标定初步的时间偏移参数，包括：
根据所述雷达角速度信息和雷达线加速信息，确定雷达角速度测量值和雷达线加速度测量值；
根据所述惯导角速度信息和惯导线加速度信息，确定惯导角速度测量值和惯导线加速度测量值；
根据所述雷达角速度测量值、所述雷达线加速度测量值、所述惯导角速度测量值和所述惯导线加速度测量值，确定提前的组合惯导时间数据；
根据所述提前的组合惯导时间数据，确定离散步长；
根据所述离散步长、所述雷达旋转位姿信息和所述组合惯导旋转位姿信息，标定初步的时间偏移参数。
4.根据权利要求3所述的方法，其特征在于，所述雷达角速度信息包括雷达角速度测量值、雷达角速度偏置值和雷达角速度噪声值，所述雷达线加速度信息包括雷达线加速度测量值、雷达线加速度偏置值和雷达线加速度噪声值；
相应地，所述根据所述雷达角速度信息和雷达线加速信息，确定雷达角速度测量值和雷达线加速度测量值，的计算公式为：
Figure FDA0004115354910000021
Figure FDA0004115354910000022
式中，ωmL为雷达角速度测量值，amL为雷达线加速度测量值，所述
Figure FDA0004115354910000023
为雷达角速度真值，/>
Figure FDA0004115354910000024
为雷达线加速度真值，bwL为雷达角速度偏置值，baL为雷达线加速度偏置值，nwL为雷达角加速度噪声值，nL为雷达线加速度噪声值。
5.根据权利要求3所述的方法，其特征在于，所述惯导角速度信息包括惯导角速度测量值、惯导角速度偏置值和惯导角速度噪声值，所述惯导线加速度信息包括惯导线加速度测量值、惯导线加速度偏置值和惯导线加速度噪声值；
相应地，所述根据所述惯导角速度信息和惯导线加速信息，确定惯导角速度测量值和惯导线加速度测量值，的计算公式为：
Figure FDA0004115354910000031
Figure FDA0004115354910000032
式中，ωmi为惯导角速度测量值，ami为惯导线加速度测量值，所述
Figure FDA0004115354910000033
为惯导角速度真值，/>
Figure FDA0004115354910000034
为惯导线加速度真值，bw为惯导角速度偏置值，ba为惯导线加速度偏置值，nwi为惯导角速度噪声值，ni为惯导线加速度噪声值。
6.根据权利要求3所述的方法，其特征在于，所述提前的组合惯导时间数据包括雷达与组合导航之间的时间偏移和相邻两帧激光雷达之间的时间间隔；
相应地，所述根据所述提前的组合惯导时间数据，确定离散步长，的计算公式为：
Figure FDA0004115354910000035
式中，d为离散步长，
Figure FDA0004115354910000036
为雷达与组合导航之间的时间偏移值，Δt为雷达与组合导航之间的时间偏移和相邻两帧激光雷达之间的时间间隔。
7.根据权利要求3所述的方法，其特征在于，所述根据所述离散步长、所述雷达旋转位姿信息和所述组合惯导旋转位姿信息，标定初步的时间偏移参数，的计算公式为：
d*＝arg max∑||ωik+d||*||ωLK||
式中，d*待求解的初步的时间偏移参数，ωik+d为k+d时刻的组合惯导旋转位姿信息，d为离散步长，ωLk为k时刻的雷达旋转位姿信息。
8.根据权利要求1所述的方法，其特征在于，所述初步的空间参数包括初步的旋转外参和初步的平移外参；
相应地，所述根据所述初步的时间偏移参数、所述雷达里程计信息和所述组合惯导里程计信息，标定初步的空间参数，包括：
根据所述初步的时间偏移、所述雷达里程计信息和所述组合惯导里程计信息，确定初步的外参矩阵；
根据所述初步的外参矩阵，确定初步的旋转外参；
根据所述旋转外参，确定初步的平移外参。
9.根据权利要求3所述的方法，其特征在于，所述组合惯导里程计信息还包括惯导旋转速度信息和惯导角度信息；
相应地，所述根据所述初步的时间偏移参数、所述初步的空间参数、所述雷达里程计信息和所述组合惯导里程计信息，确定精准的时间偏移参数和精准的外参矩阵，包括：
根据所述初步的时间偏移参数、所述初步的空间参数、所述雷达里程计信息、所述惯导旋转速度信息和所述惯导角度信息，确定精准的时间偏移参数和精准的外参矩阵。
10.根据权利要求9所述的方法，其特征在于，所述根据所述初步的时间偏移参数、所述初步的空间参数、所述雷达里程计信息、所述惯导旋转速度信息和所述惯导角度信息，确定精准的时间偏移参数和精准的外参矩阵，的计算公式为：
Figure FDA0004115354910000041
式中，RLi *为待求解的精准的外参矩阵，bω *为待求解的偏置值，δt*为待求解的精准的时间偏移参数，RLi为初步的空间参数，ωLk为k时刻的雷达旋转位姿信息、bω为惯导角速度偏置值、
Figure FDA0004115354910000042
为K时刻的惯导旋转速度、δt为初步的时间偏移参数，/>
Figure FDA0004115354910000043
为k时刻的惯导角度。
11.根据权利要求1所述的方法，其特征在于，所述精准的外参矩阵包括平移向量和旋转向量，所述待校验雷达里程计信息包括雷达位姿变换矩阵，所述待校验惯导里程计信息包括惯导位姿变换矩阵；
相应地，所述根据所述精准的时间偏移参数、所述精准的外参矩阵、所述待校验雷达里程计信息和所述待校验组合惯导里程计信息，标定目标时间偏移参数和目标空间参数，以完成激光雷达和组合惯导的时空外参标定，包括：
根据所述精准的时间偏移参数、所述平移向量、所述旋转向量、所述雷达位姿变换矩阵和所述惯导位姿变换矩阵，标定目标时间偏移参数和目标空间参数，以完成激光雷达和组合惯导的时空外参标定。
12.根据权利要求11所述的方法，其特征在于，所述根据所述精准的时间偏移参数、所述平移向量、所述旋转向量、所述雷达位姿变换矩阵和所述惯导位姿变换矩阵，标定目标时间偏移参数和目标空间参数，的计算公式为：
Figure FDA0004115354910000051
式中，q*为目标空间参数矩阵，t*为目标时间偏移参数矩阵，λ1、λ2和λ为预设未旋转矩阵的权重系数，v`i为矩阵
Figure FDA0004115354910000052
的向量表示形式，vi为矩阵/>
Figure FDA0004115354910000053
的向量表示形式，/>
Figure FDA0004115354910000054
为第i帧雷达和第i+1帧雷达之间的位姿变换矩阵，/>
Figure FDA0004115354910000055
为第i帧惯导和第i+1帧惯导之间的位姿变换矩阵，I为预存单位矩阵，t为平移向量，q为旋转向量，/>
Figure FDA0004115354910000056
为旋转向量q的模长。
13.根据权利要求1所述的方法，其特征在于，所述根据所述精准的时间偏移参数、所述精准的外参矩阵、所述待校验雷达里程计信息和所述待校验组合惯导里程计信息，标定目标时间偏移参数和目标空间参数之后，还包括：
对所述待校验雷达里程计信息和所述待校验组合惯导里程计信息进行轨迹校验处理，生成轨迹校验结果；
若判定所述轨迹校验结果满足预设校验条件，则将所述目标时间偏移参数和所述目标空间参数作为时空标定结果输出；
若判定所述轨迹校验结果满足预设校验失败条件，则返回到所述根据所述精准的时间偏移参数、所述精准的外参矩阵、所述雷达里程计信息和所述组合惯导里程计信息，确定待校验雷达里程计信息和待校验组合惯导里程计信息的步骤。
14.根据权利要求1至13任一项所述的方法，其特征在于，所述获取激光点云数据和组合惯导数据，包括：
在开阔的场景下获取激光点云数据和组合惯导数据。
15.一种激光雷达和组合惯导的时空外参标定装置，其特征在于，包括：
数据获取模块，用于获取激光点云数据和组合惯导数据；
数据预处理模块，对所述激光点云数据进行校验处理和噪声过滤处理，得到待用激光点云数据，并对所述组合惯导数据进行校验处理和噪声过滤处理，得到待用组合惯导数据；
里程计模块，用于根据所述待用激光点云数据，确定雷达里程计信息；用于根据所述待用组合惯导数据，确定组合惯导里程计信息；
时空外参标定模块，用于根据所述雷达里程计信息和所述组合惯导里程计信息，标定初步的时间偏移参数；
所述时空外参标定模块，还用于根据所述初步的时间偏移参数、所述雷达里程计信息和所述组合惯导里程计信息，标定初步的空间参数；
所述时空外参标定模块，还用于根据所述初步的时间偏移参数、所述初步的空间参数、所述雷达里程计信息和所述组合惯导里程计信息，确定精准的时间偏移参数和精准的外参矩阵；
所述里程计模块，还用于根据所述精准的时间偏移参数、所述精准的外参矩阵、所述雷达里程计信息和所述组合惯导里程计信息，确定待校验雷达里程计信息和待校验组合惯导里程计信息；
所述时空外参标定模块，还用于根据所述精准的时间偏移参数、所述精准的外参矩阵、所述待校验雷达里程计信息和所述待校验组合惯导里程计信息，标定目标时间偏移参数和目标空间参数，以完成激光雷达和组合惯导的时空外参标定。
16.一种激光雷达和组合惯导的时空外参标定系统，其特征在于，包括：
激光雷达设备，用于采集激光点云数据；
惯性导航设备，用于采集组合惯导数据；
一个或多个处理器；存储装置，用于存储一个或多个程序；当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1至14任一项所述的激光雷达和组合惯导的时空外参标定方法。
CN202310217039.8A 2023-03-02 2023-03-02 激光雷达和组合惯导的时空外参标定方法、装置及系统 Pending CN116182905A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310217039.8A CN116182905A (zh) 2023-03-02 2023-03-02 激光雷达和组合惯导的时空外参标定方法、装置及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310217039.8A CN116182905A (zh) 2023-03-02 2023-03-02 激光雷达和组合惯导的时空外参标定方法、装置及系统

Publications (1)

Publication Number Publication Date
CN116182905A true CN116182905A (zh) 2023-05-30

Family

ID=86447386

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310217039.8A Pending CN116182905A (zh) 2023-03-02 2023-03-02 激光雷达和组合惯导的时空外参标定方法、装置及系统

Country Status (1)

Country Link
CN (1) CN116182905A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117706530A (zh) * 2024-02-05 2024-03-15 中国科学院自动化研究所 一种用于实现多激光雷达与组合导航标定的方法及系统
CN117706530B (zh) * 2024-02-05 2024-05-14 中国科学院自动化研究所 一种用于实现多激光雷达与组合导航标定的方法及系统

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117706530A (zh) * 2024-02-05 2024-03-15 中国科学院自动化研究所 一种用于实现多激光雷达与组合导航标定的方法及系统
CN117706530B (zh) * 2024-02-05 2024-05-14 中国科学院自动化研究所 一种用于实现多激光雷达与组合导航标定的方法及系统

Similar Documents

Publication Publication Date Title
US10436884B2 (en) Calibration of laser and vision sensors
CN111656136B (zh) 使用激光雷达的车辆定位系统
CN109270545B (zh) 一种定位真值校验方法、装置、设备及存储介质
WO2022127532A1 (zh) 一种激光雷达与imu的外参标定方法、装置及设备
CN112005079B (zh) 用于更新高清地图的系统和方法
WO2020253260A1 (zh) 时间同步处理方法、电子设备及存储介质
CN110081881A (zh) 一种基于无人机多传感器信息融合技术的着舰引导方法
CN111308415B (zh) 一种基于时间延迟的在线估计位姿的方法和设备
US20240053454A1 (en) Joint calibration method and system for external parameters of vehicle-mounted laser radars
CN112946681B (zh) 融合组合导航信息的激光雷达定位方法
CN113960622A (zh) 融合激光雷达及imu传感器信息的实时定位方法及装置
CN113767264A (zh) 参数标定方法、装置、系统和存储介质
CN110637209B (zh) 估计机动车的姿势的方法、设备和具有指令的计算机可读存储介质
CN113984044A (zh) 一种基于车载多感知融合的车辆位姿获取方法及装置
CN112964291A (zh) 一种传感器标定的方法、装置、计算机存储介质及终端
CN114459506B (zh) 在线标定全球导航卫星系统接收机与视觉惯性里程计之间外参的方法及系统
CN113580134A (zh) 视觉定位方法、设备、机器人、存储介质及程序产品
JP6905390B2 (ja) 自車位置推定用環境地図生成装置、自車位置推定装置、自車位置推定用環境地図生成プログラム、及び自車位置推定プログラム
CN116614621B (zh) 相机内感知算法的测试方法、设备和存储介质
WO2021056283A1 (en) Systems and methods for adjusting a vehicle pose
CN113591890A (zh) 一种聚类的方法和装置
CN116182905A (zh) 激光雷达和组合惯导的时空外参标定方法、装置及系统
CN116047481A (zh) 矫正点云数据畸变方法、装置、设备及存储介质
CN116105772A (zh) 一种激光雷达与imu的标定方法、装置及存储介质
CN115272408A (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