CN114413887A - 一种传感器外部参数标定方法、设备及介质 - Google Patents

一种传感器外部参数标定方法、设备及介质 Download PDF

Info

Publication number
CN114413887A
CN114413887A CN202111602608.8A CN202111602608A CN114413887A CN 114413887 A CN114413887 A CN 114413887A CN 202111602608 A CN202111602608 A CN 202111602608A CN 114413887 A CN114413887 A CN 114413887A
Authority
CN
China
Prior art keywords
point cloud
cloud data
inertial
determining
sensing unit
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
CN202111602608.8A
Other languages
English (en)
Other versions
CN114413887B (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.)
Beijing Institute of Technology BIT
Advanced Technology Research Institute of Beijing Institute of Technology
Original Assignee
Beijing Institute of Technology BIT
Advanced Technology Research Institute of Beijing Institute of Technology
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 Beijing Institute of Technology BIT, Advanced Technology Research Institute of Beijing Institute of Technology filed Critical Beijing Institute of Technology BIT
Priority to CN202111602608.8A priority Critical patent/CN114413887B/zh
Publication of CN114413887A publication Critical patent/CN114413887A/zh
Application granted granted Critical
Publication of CN114413887B publication Critical patent/CN114413887B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • 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
    • 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
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • 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

Landscapes

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

Abstract

本申请提供了一种传感器外部参数标定方法及设备，本申请涉及传感器外参标定技术领域。该方法通过获取若干点云数据以及惯性传感单元采集的惯性数据。基于点云数据相应的参考帧，生成点云数据的参考时间坐标系。参考时间坐标系为将参考帧，相应的激光雷达的位置作为坐标系原点建立的。根据参考时间坐标系内的点云数据，确定预设时间间隔的各点云数据之间的位置姿态变换信息。根据惯性数据，确定惯性传感单元在预设时间间隔的惯性姿态变换信息。基于位置姿态变换信息、惯性姿态变换信息，确定卡尔曼滤波量测方程，以基于卡尔曼滤波算法，确定激光雷达与惯性传感单元之间的外部参数。

Description

一种传感器外部参数标定方法、设备及介质
技术领域
本申请涉及传感器外参标定技术领域，尤其涉及一种传感器外部参数标定方法、设备及介质。
背景技术
位置服务从专业领域的有限需求走向大众用户的泛在需求，从孤立的局域延伸到无缝的全域，复杂环境下依靠单一定位手段均无法满足要求。高效、稳健的多源信息融合高精度定位技术成为了实时获取空间信息的一种新趋势。精确的传感器相对位姿关系是多源传感器信息融合的基础，也是制约高精度导航定位的重要因素，是保证融合效果的先决条件。为了满足该系统的定位精度，传感器之间的参数标定至关重要，参数标定精度直接影响融合定位结果。
现有技术的参数标定方法大多分为两类，一类需要在室内环境采集数据，利用墙体约束获取可靠的激光雷达数据，采集过程中需要有较大的旋转与平移，增加了采集难度，标定结果也不稳定，精度不高，并且不适用于无人驾驶的汽车标定；另一类需要借助辅助工具，如反射器，或者特征较明显的球体、柱体等辅助测量，对环境要求高，设备复杂，不易实现，使用不方便。
发明内容
本申请实施例提供了一种传感器外部参数标定方法、设备及介质，用于解决需要人为参与设置辅助工具，参数标定对环境、设备要求高，标定精度不高，测量繁琐的技术问题。
一方面，本申请提供了一种传感器外部参数标定方法，该方法包括：
获取若干点云数据以及惯性传感单元采集的惯性数据。基于点云数据相应的参考帧，生成点云数据的参考时间坐标系。参考时间坐标系为将参考帧，相应的激光雷达的位置作为坐标系原点建立的。根据参考时间坐标系内的点云数据，确定预设时间间隔的各点云数据之间的位置姿态变换信息。根据惯性数据，确定惯性传感单元在预设时间间隔的惯性姿态变换信息。基于位置姿态变换信息、惯性姿态变换信息，确定卡尔曼滤波量测方程，以基于卡尔曼滤波算法，确定激光雷达与惯性传感单元之间的外部参数。
在本申请的一种实现方式中，根据惯性数据相应的时间戳，确定点云数据中各激光点相应的速度数据。确定预设时间内，各激光点的速度数据相匹配的各激光点，为相同帧点云数据。将参考时间坐标系内的点云数据的坐标值，更新为各相同帧点云数据的坐标值。
在本申请的一种实现方式中，确定参考时间坐标系中，参考帧的下一帧点云数据，作为待匹配点云数据。通过正态分布变换NDT算法，确定参考帧点云数据与待匹配点云数据之间的位移信息、旋转信息。将位移信息以及旋转信息，作为位置姿态变换信息。
在本申请的一种实现方式中，对参考帧与其下一帧之间的惯性数据进行积分处理，分别确定惯性传感单元在下一帧的位置公式及旋转公式。基于下一帧的位置公式及旋转公式，确定参考帧至下一帧的位置增量及旋转增量。将参考帧至下一帧的位置增量及旋转增量，作为惯性姿态变换信息。
在本申请的一种实现方式中，确定参考帧至下一帧的位置增量以及参考帧至下一帧的位移信息。根据位置增量与位移信息的差值，生成卡尔曼滤波量测方程中的位置量测方程。确定参考帧至下一帧的旋转增量以及参考帧至下一帧的旋转信息。基于旋转增量及旋转信息，生成卡尔曼滤波量测方程中的旋转量测方程。
在本申请的一种实现方式中，将激光雷达采集的若干原始点云数据，输入点云采样模型。根据点云采样模型，确定满足预设条件的各点云数据。其中，预设条件为点云数据的分辨率大于预设分辨率且其点云数量大于预设值。
在本申请的一种实现方式中，确定推荐定位信息GPRMC。根据GPRMC，确定协调世界时UTC。根据UTC，同步激光雷达及惯性传感单元的时间。其中，激光雷达及惯性传感单元预先进行脉冲同步，脉冲同步通过惯性传感单元的秒脉冲输出端发送秒脉冲至激光雷达的秒脉冲接收端实现。
在本申请的一种实现方式中，卡尔曼滤波量测方程具体公式：
Figure BDA0003432380550000031
Figure BDA0003432380550000032
其中，δp为量测的系统位移向量，
Figure BDA0003432380550000033
为预设时间间隔的惯性姿态变换信息中位移增量，p为预设时间间隔的点云数据之间的位置姿态变换信息中的位移信息。
Figure BDA0003432380550000034
为量测的系统旋转向量，
Figure BDA0003432380550000035
为预设时间间隔的惯性姿态变换信息中旋转增量，
Figure BDA0003432380550000036
为正态分布变换NDT算法在第k帧至第k+1帧的旋转矩阵，
Figure BDA0003432380550000037
为激光雷达与惯性传感单元的外部参数，I为单位矩阵。
另一方面，本申请实施例还提供了一种传感器外部参数标定设备，该设备包括：
至少一个处理器；以及，与至少一个处理器通信连接的存储器。其中，存储器存储有可被至少一个处理器执行的指令，指令被至少一个处理器执行，以使至少一个处理器能够：
获取若干点云数据以及惯性传感单元采集的惯性数据。基于点云数据相应的参考帧，生成点云数据的参考时间坐标系。参考时间坐标系为将参考帧，相应的激光雷达的位置作为坐标系原点建立的。根据参考时间坐标系内的点云数据，确定预设时间间隔的各点云数据之间的位置姿态变换信息。根据惯性数据，确定惯性传感单元在预设时间间隔的惯性姿态变换信息。基于位置姿态变换信息、惯性姿态变换信息，确定卡尔曼滤波量测方程，以基于卡尔曼滤波算法，确定激光雷达与惯性传感单元之间的外部参数。
再一方面，本申请实施例还提供了一种传感器外部参数标定的溯源非易失性计算机存储介质，存储有计算机可执行指令，该计算机可执行指令设置为：
获取若干点云数据以及惯性传感单元采集的惯性数据。基于点云数据相应的参考帧，生成点云数据的参考时间坐标系。参考时间坐标系为将参考帧，相应的激光雷达的位置作为坐标系原点建立的。根据参考时间坐标系内的点云数据，确定预设时间间隔的各点云数据之间的位置姿态变换信息。根据惯性数据，确定惯性传感单元在预设时间间隔的惯性姿态变换信息。基于位置姿态变换信息、惯性姿态变换信息，确定卡尔曼滤波量测方程，以基于卡尔曼滤波算法，确定激光雷达与惯性传感单元之间的外部参数。
通过上述方案，无需人为参与，设置辅助参数标定工具，实现激光雷达与惯性传感单元的外部参数标定。本申请的传感器外部参数标定方法，不需要严格要求环境条件，也不需要除激光雷达与惯性传感单元以外的设备进行参与外部参数标定的工作。通过将激光雷达与惯性传感单元各自采集到的数据，进行卡尔曼滤波处理，可以实现外部参数标定精度高，测量结果实时更新，外部参数标定方式简单。
附图说明
此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：
图1为本申请实施例中一种传感器外部参数标定方法的一种流程示意图；
图2为本申请实施例中一种传感器外部参数标定方法中激光雷达与惯性传感单元的相对位置的示意图；
图3为本申请实施例中一种传感器外部参数标定设备的结构示意图。
具体实施方式
为使本申请的目的、技术方案和优点更加清楚，下面将结合本申请具体实施例及相应的附图对本申请技术方案进行清楚、完整地描述。显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
目前，自动驾驶汽车的激光雷达与惯性传感单元(Inertial Measurement Unit，IMU)作为汽车“眼睛”，激光雷达与IMU之间的外部参数标定的精度，影响采集到的环境数据的准确性，若外参标定存在误差，可能降低汽车对外界的感知能力，影响自动驾驶能力。
基于此，本申请实施例提供了一种传感器外部参数标定方法、设备及介质，用来解决人为参与设置辅助工具，参数标定对环境、设备要求高，标定精度不高，测量繁琐的问题。
以下结合附图，详细说明本申请的各个实施例。
本申请实施例提供了一种传感器外部参数标定方法，该方法应用于激光雷达与IMU的外部参数标定，激光雷达与IMU电连接。如图1所示，该方法可以包括步骤S101-S108：
S101，服务器确定推荐定位信息GPRMC。
服务器可以通过GPS模块，获取GPRMC。GPRMC用于传递协调世界时(UniversalTime Coordinated，UTC)时间。
需要说明的是，服务器作为传感器外部参数标定方法的执行主体，仅为示例性存在，执行主体不仅限于服务器，本申请对此不作限定。
S102，服务器根据GPRMC，确定协调世界时UTC。
服务器可以将GPRMC的报文进行解析，从而得到UTC时间。
S103，服务器根据UTC，同步激光雷达及惯性传感单元的时间。
其中，激光雷达及惯性传感单元预先进行脉冲同步，脉冲同步通过惯性传感单元的秒脉冲输出端发送秒脉冲至激光雷达的秒脉冲接收端实现。
在本申请实施例中，IMU将秒脉冲发送至激光雷达，激光雷达根据接收到的IMU秒脉冲，实现激光雷达的秒脉冲同步，同时，IMU和激光雷达根据UTC，完成IMU和激光雷达的硬件时间同步。
S104，服务器获取若干点云数据以及惯性传感单元采集的惯性数据。
在本申请实施例中，服务器可以通过激光雷达，获取若干的点云数据，该点云数据可以是在激光雷达采集到原始点云数据后，处理得到的。本申请对于激光雷达所采集点云数据的环境，如是否有辅助工具、是否移动、室内或室外等条件，不作具体限定。激光雷达所处自动驾驶汽车，可以是正在正常行驶过程中的汽车，并在行驶过程中，激光雷达可以实时采集外界的点云数据。点云数据与惯性数据为六自由度数据。
由于激光雷达在实际的使用过程中，采集到的数据中点云数量多，若直接将采集的点云用于外参标定，可能耗费服务器处理时间。因此，在激光雷达采集点云之后，本申请可以执行以下方法：
首先，服务器将激光雷达采集的若干原始点云数据，输入点云采样模型。
激光雷达在采集到原始点云数据之后，服务器可以将原始点云数据输入到点云采样模型，在本申请实施例中，该点云采样模型可以对原始点云数据进行自适应分辨率下采样及点云数量的处理。例如点云采样模型可以将原始点云数据进行分辨率调整，即将原始点云数据的分辨率进行下采样，直至下采样后的原始点云数据相应的点云数量达到设定的点云数量。该设定的点云数量可以是用于限制点云数据中最低点云数量的，同时还可设置一最低分辨率，保证分辨率下采样之后，不影响外参标定的计算。
在本申请实施例中，若原始点云数据在初始的分辨率下采样之后，点云数量小于上述最低点云数量，服务器可以将原始点云数据进行提高分辨率，重新进行下采样操作，保证下采样后的点云数量大于最低点云数量。
然后，服务器根据点云采样模型，确定满足预设条件的各点云数据。
其中，预设条件为点云数据的分辨率大于预设分辨率且其点云数量大于预设值。
在服务器将原始点云数据输入点云采样模型后，点云采样模型通过上述自适应分辨率下采样及点云数量的处理，在原始点云数据经过下采样后原始点云数据的分辨率大于预设分辨率(上述最低分辨率)、下采样后原始点云数据的点云数量大于预设值(上述最低点云数量)的情况下，将下采样后的原始点云数据作为服务器进行传感器外部参数标定的点云数据。
通过上述方案，可以将点云数量大，处理耗时的原始点云数据进行下采样处理，从而提高服务器在外参标定时，数据处理的速度，保证外参标定效率。
S105，服务器基于点云数据相应的参考帧，生成点云数据的参考时间坐标系。
参考时间坐标系为将参考帧，相应的激光雷达的位置作为坐标系原点建立的。
服务器可以将点云数据中的某一帧点云数据，作为参考帧，将参考帧的时间作为建立参考时间坐标系的时间。服务器将得到的点云数据均转换到该参考时间坐标系中。
此外，在本申请的另一实施例中，由于激光雷达采集点云数据时，汽车可能在不停运动，导致每一帧的点云数据中的各个激光点位置与真实位置存在偏差。因此，本申请可以执行以下方法，解决上述问题：
首先，服务器根据惯性数据相应的时间戳，确定点云数据中各激光点相应的速度数据。
服务器可以在获取到IMU采集的惯性数据之后，确定各个时间戳的惯性数据，并将各时间戳与点云数据的时间相匹配。匹配之后既可以确定点云数据中各激光点的速度数据，速度数据包括线速度和角速度。
在本申请实施例中，采用IMU辅助点云畸变矫正的手段，进行矫正点云畸变，既可以避免估计点云畸变的方式存在较大误差，也可以避免要安装设备，增加较大成本。
然后，服务器确定预设时间内，各激光点的速度数据相匹配的各激光点，为相同帧点云数据。
预设时间可以在实际使用过程中进行设定，预设时间可以与两帧数据之间的时间间隔相对应，比如相邻帧A与B的时间间隔为T，那么预设时间可以选择为T。服务器可以确定预设时间T内，各激光点的速度数据相匹配的激光点，可以通过k-d树的方式进行匹配，得到激光点最近邻的激光点，并计算距离，总距离最小时可以认为匹配。服务器将激光点的速度数据相匹配的激光点作为相同帧点云数据。
最后，服务器将参考时间坐标系内的点云数据的坐标值，更新为各相同帧点云数据的坐标值。
服务器在得到相同帧点云数据之后，服务器可以通过点云畸变矫正算法，对相同帧点云数据进行矫正，然后将参考时间坐标系内的点云数据的坐标值，更新为矫正后的相同帧点云数据的坐标值。
至此，服务器可以生成包含准确地点云数据的参考时间坐标系。
S106，服务器根据参考时间坐标系内的点云数据，确定预设时间间隔的各点云数据之间的位置姿态变换信息。
在本申请实施例中，服务器根据参考时间坐标系内的点云数据，确定预设时间间隔的各点云数据之间的位置姿态变换信息，具体包括：
首先，服务器确定参考时间坐标系中，参考帧的下一帧点云数据，作为待匹配点云数据。
在本申请实施例中，服务器可以在参考时间坐标系中，选择参考帧的下一帧点云数据作为待匹配点云数据，在实际使用过程中，建立参考时间坐标系之后，参考帧可以选择参考时间坐标系中任一帧点云数据，作为参考帧，例如参考帧为当前时间相应的前一帧点云数据。
然后，服务器通过正态分布变换NDT算法，确定参考帧点云数据与待匹配点云数据之间的位移信息、旋转信息。
本申请实施例中，服务器通过NDT算法，对点云数据进行均匀栅格化，采用高斯模型近似表示每个栅格内的点云数据。最大化待匹配点云数据中的所有点落在最近的高斯模型上的概率乘积。
具体地通过正态分布表示NDT算法的体素单元中每个三维点位置测量样本的概率分布，其表达式如下公式：
Figure BDA0003432380550000091
式中p(x)为概率分布，C是体素单元中的协方差矩阵(公式如下)，q是体素单元中的均值向量，c为常量。
Figure BDA0003432380550000092
式中xi(i＝1，…，n)为体素单元格中的所有点。
每个体素单元格中的q和Q可以定义为以下公式：
Figure BDA0003432380550000093
Figure BDA0003432380550000094
在本申请实施例中，服务器需要将下一帧点云数据投影到参考帧的坐标系，将点云中每个投影点的概率分布之和作为每个坐标变换参数的分数值s(p)进行评测，评测函数可表示为下式的形式：
Figure BDA0003432380550000095
式中x′i是通过空间变换参数x转换到参考帧数据坐标系的雷达坐标，qi表示xi对应的均值向量，C表示xi对应的协方差矩阵，三维空间变换向量T(P，x)可表示为式(5)的形式
Figure BDA0003432380550000101
上式中，
Figure BDA0003432380550000102
其中t，r为平移和旋转参数，
Figure BDA0003432380550000103
为旋转角；t＝[txtytz]，平移参数；r＝[rxryrz]，旋转参数；
Figure BDA0003432380550000104
服务器通过上述NDT算法，即可确定待匹配点云数据与参考帧点云数据之间的位移信息
Figure BDA0003432380550000105
和旋转信息T(p，x)。
最后，服务器将位移信息以及旋转信息，作为位置姿态变换信息。
服务器可以将位移信息与旋转信息作为激光雷达所对应的汽车的位置姿态变换信息，以进行外参标定的计算。
S107，服务器根据惯性数据，确定惯性传感单元在预设时间间隔的惯性姿态变换信息。
在本申请实施例中，IMU可以实时采集汽车速度、位置、旋转的数据。若要进行外参标定，服务器需要将惯性数据与点云数据进行时间对齐处理，由于上述S106已得到待匹配点云数据与参考帧点云数据之间的位移信息以及旋转信息，那么可以通过惯性数据得到位移信息相应的位置增量以及旋转信息相应的旋转增量。
在本申请实施例中，服务器根据惯性数据，确定惯性传感单元在预设时间间隔的惯性姿态变换信息，具体包括：
首先，服务器对参考帧与其下一帧之间的惯性数据进行积分处理，分别确定惯性传感单元在下一帧的位置公式及旋转公式。
在本申请实施例中，服务器在得到IMU采集的位置、速度、旋转的数据之后，可以通过IMU预积分算法，得到IMU的位置、速度、旋转的连续变化公式，并根据中值法，得到参考帧与下一帧的位置、速度、旋转的变化公式。具体地，服务器根据惯性数据，确定惯性传感单元在预设时间间隔的惯性姿态变换信息，具体包括：
首先，服务器对参考帧与其下一帧之间的惯性数据进行积分处理，分别确定惯性传感单元在下一帧的位置公式及旋转公式。
在本申请实施例中，通过下述方式对惯性数据进行积分处理，将参考帧第k帧和下一帧第k+1帧之间的所有惯性数据进行积分，可得第k+1帧的位置、速度和旋转(PVQ)，这里的旋转采用的四元数。
Figure BDA0003432380550000111
Figure BDA0003432380550000112
Figure BDA0003432380550000113
其中，
Figure BDA0003432380550000114
表示世界坐标系第k+1帧位置，
Figure BDA0003432380550000115
表示第k帧速度，
Figure BDA0003432380550000116
表示旋转矩阵，gw表示地球重力加速度，
Figure BDA0003432380550000117
表示四元数，Ω表示角速度矩阵，
Figure BDA0003432380550000118
表示加速度零偏，
Figure BDA0003432380550000119
表示角速度零偏，
Figure BDA00034323805500001110
和
Figure BDA00034323805500001111
为IMU测量的加速度和角速度，是在Body自身坐标系得到的，世界坐标系是IMU所在的惯导系。
上述公式给出的是连续时刻的当前位置、速度、旋转矩阵的迭代公式，下面给出基于中值法的公式，IMU积分出来的第j时刻的物理量可以作为第j帧值。
Figure BDA00034323805500001112
Figure BDA00034323805500001113
Figure BDA00034323805500001114
其中：
Figure BDA00034323805500001115
Figure BDA00034323805500001116
服务器通过上述公式，得到了下一帧的位置公式及旋转公式。
由于IMU的预积分需要依赖第k帧的v和R，当我们在构造卡尔曼滤波方程时，不需要关注更新第k帧的v和R，只需要关注每两激光雷达采集帧的位置和旋转增量即可。因此，将优化变量从第k帧到第k+1帧的IMU预积分项中分离开来，可化简为：
Figure BDA0003432380550000121
Figure BDA0003432380550000122
Figure BDA0003432380550000123
其次，服务器基于下一帧的位置公式及旋转公式，确定参考帧至下一帧的位置增量及旋转增量。
通过上述的位置公式及旋转公式，服务器既可以得到位置增量及旋转增量。其中位置增量及旋转增量的公式如下：
Figure BDA0003432380550000124
Figure BDA0003432380550000125
其中，
Figure BDA0003432380550000126
表示参考帧至下一帧连续的位置增量，
Figure BDA0003432380550000127
参考帧至下一帧连续的旋转增量。
接着，服务器将参考帧至下一帧的位置增量及旋转增量，作为惯性姿态变换信息。
通过上述方案，将IMU得到的惯性数据进行预积分处理，可以实现与激光雷达采集到的点云数据完成同步地处理，给后续地外参标定提供了噪声小、误差少的可信数据。
S108，服务器基于位置姿态变换信息、惯性姿态变换信息，确定卡尔曼滤波量测方程，以基于卡尔曼滤波算法，确定激光雷达与惯性传感单元之间的外部参数。
在本申请实施例中，卡尔曼滤波方程中的状态方程和量测方程可以表示为：
Figure BDA0003432380550000131
式中：X(t)为系统状态向量，F(t)为系统状态矩阵，G(t)为系统噪声分布矩阵，Wd(t)为系统噪声向量Z(t)为观测向量，H(t)为观测矩阵，V(t)为观测噪声向量，其中Wd(t)与V(t)相互独立且不相关服从零均值高斯分布。
在本申请实施例中，卡尔曼滤波量测方程的具体公式如下：
Figure BDA0003432380550000132
Figure BDA0003432380550000133
其中，δp为量测的系统位移向量(相当于Z(t))，
Figure BDA0003432380550000134
为预设时间间隔的惯性姿态变换信息中位移增量，p为预设时间间隔的点云数据之间的位置姿态变换信息中的位移信息；
Figure BDA0003432380550000135
为量测的系统旋转向量(相当于Z(t))，
Figure BDA0003432380550000136
为预设时间间隔的惯性姿态变换信息中旋转增量，
Figure BDA0003432380550000137
为NDT算法在第k帧至第k+1帧的旋转矩阵，
Figure BDA0003432380550000138
为激光雷达与惯性传感单元的外部参数，I为单位矩阵。
在本申请实施例中，激光雷达与惯性传感单元在实际的运动过程中，相对位置示意图如图2所示。由于相对位置不变，因此卡尔曼滤波状态方程如下：
Figure BDA0003432380550000139
Figure BDA00034323805500001310
式中，
Figure BDA00034323805500001311
为预测的系统位移向量(相当于X(t))，
Figure BDA00034323805500001312
为预测的系统位移向量(相当于X(t))。通过上述公式以及卡尔曼滤波算法，进行迭代运算，从而得到更新的外部参数
Figure BDA00034323805500001313
本申请通过上述方案，将激光雷达与IMU采集的数据构造卡尔曼滤波方程，通过卡尔曼滤波方程迭代优化，得到最优解的外部参数。可以避免激光雷达与IMU所采集的数据之间产生累积误差，可以减小因为噪声与安装误差等问题造成的数据模糊问题。本申请通过将激光雷达与IMU的数据关联，并利用卡尔曼滤波算法进行迭代计算，可以使外部参数计算具有更高的鲁棒性，提高了外部参数标定的准确性。
此外，本申请可以用于室外的外部参数标定，无需借助任何辅助工具，解决了人工测量误差较大且人工测量比较麻烦的技术问题，相对于现有标定方式，操作简单，方便工程使用，可以提高标定效率与车辆定位精度。
图3为本申请实施例提供的一种传感器外部参数标定设备的结构示意图，如图3所示，该设备包括：
至少一个处理器；以及，与至少一个处理器通信连接的存储器。其中，存储器存储有可被至少一个处理器执行的指令，指令被至少一个处理器执行，以使至少一个处理器能够：
获取若干点云数据以及惯性传感单元采集的惯性数据。基于点云数据相应的参考帧，生成点云数据的参考时间坐标系。参考时间坐标系为将参考帧，相应的激光雷达的位置作为坐标系原点建立的。根据参考时间坐标系内的点云数据，确定预设时间间隔的各点云数据之间的位置姿态变换信息。根据惯性数据，确定惯性传感单元在预设时间间隔的惯性姿态变换信息。基于位置姿态变换信息、惯性姿态变换信息，确定卡尔曼滤波量测方程，以基于卡尔曼滤波算法，确定激光雷达与惯性传感单元之间的外部参数。
本申请实施例还提供了一种传感器外部参数标定的溯源非易失性计算机存储介质，存储有计算机可执行指令，计算机可执行指令设置为：
获取若干点云数据以及惯性传感单元采集的惯性数据。基于点云数据相应的参考帧，生成点云数据的参考时间坐标系。参考时间坐标系为将参考帧，相应的激光雷达的位置作为坐标系原点建立的。根据参考时间坐标系内的点云数据，确定预设时间间隔的各点云数据之间的位置姿态变换信息。根据惯性数据，确定惯性传感单元在预设时间间隔的惯性姿态变换信息。基于位置姿态变换信息、惯性姿态变换信息，确定卡尔曼滤波量测方程，以基于卡尔曼滤波算法，确定激光雷达与惯性传感单元之间的外部参数。
本申请中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于设备、介质实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
本申请实施例提供的设备、介质与方法是一一对应的，因此，设备、介质也具有与其对应的方法类似的有益技术效果，由于上面已经对方法的有益技术效果进行了详细说明，因此，这里不再赘述设备、介质的有益技术效果。
还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。
以上所述仅为本申请的实施例而已，并不用于限制本申请。对于本领域技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本申请的权利要求范围之内。

Claims (10)

1.一种传感器外部参数标定方法，其特征在于，所述方法应用于激光雷达与惯性传感单元的外部参数标定，所述激光雷达与所述惯性传感单元电连接；所述方法包括：
获取若干点云数据以及所述惯性传感单元采集的惯性数据；
基于所述点云数据相应的参考帧，生成所述点云数据的参考时间坐标系；所述参考时间坐标系为将所述参考帧，相应的所述激光雷达的位置作为坐标系原点建立的；
根据所述参考时间坐标系内的所述点云数据，确定预设时间间隔的各所述点云数据之间的位置姿态变换信息；
根据所述惯性数据，确定所述惯性传感单元在所述预设时间间隔的惯性姿态变换信息；
基于所述位置姿态变换信息、所述惯性姿态变换信息，确定卡尔曼滤波量测方程，以基于卡尔曼滤波算法，确定所述激光雷达与所述惯性传感单元之间的外部参数。
2.根据权利要求1所述方法，其特征在于，基于所述点云数据相应的参考帧，生成所述点云数据的参考时间坐标系之后，所述方法还包括：
根据所述惯性数据相应的时间戳，确定所述点云数据中各激光点相应的速度数据；
确定预设时间内，各所述激光点的所述速度数据相匹配的各所述激光点，为相同帧点云数据；
将所述参考时间坐标系内的所述点云数据的坐标值，更新为各所述相同帧点云数据的坐标值。
3.根据权利要求1所述方法，其特征在于，根据所述参考时间坐标系内的所述点云数据，确定预设时间间隔的各所述点云数据之间的位置姿态变换信息，具体包括：
确定所述参考时间坐标系中，所述参考帧的下一帧所述点云数据，作为待匹配点云数据；
通过正态分布变换NDT算法，确定参考帧点云数据与所述待匹配点云数据之间的位移信息、旋转信息；
将所述位移信息以及所述旋转信息，作为所述位置姿态变换信息。
4.根据权利要求1所述方法，其特征在于，根据所述惯性数据，确定所述惯性传感单元在所述预设时间间隔的惯性姿态变换信息，具体包括：
对所述参考帧与其下一帧之间的所述惯性数据进行积分处理，分别确定所述惯性传感单元在所述下一帧的位置公式及旋转公式；
基于所述下一帧的所述位置公式及所述旋转公式，确定所述参考帧至所述下一帧的位置增量及旋转增量；
将所述参考帧至所述下一帧的所述位置增量及所述旋转增量，作为所述惯性姿态变换信息。
5.根据权利要求1所述方法，其特征在于，基于所述位置姿态变换信息、所述惯性姿态变换信息，确定卡尔曼滤波量测方程，具体包括：
确定所述参考帧至下一帧的位置增量以及所述参考帧至所述下一帧的位移信息；
根据所述位置增量与所述位移信息的差值，生成所述卡尔曼滤波量测方程中的位置量测方程；
确定所述参考帧至所述下一帧的旋转增量以及所述参考帧至所述下一帧的旋转信息；
基于所述旋转增量及所述旋转信息，生成所述卡尔曼滤波量测方程中的旋转量测方程。
6.根据权利要求1所述方法，其特征在于，获取若干点云数据之前，所述方法还包括：
将激光雷达采集的若干原始点云数据，输入点云采样模型；
根据所述点云采样模型，确定满足预设条件的各所述点云数据；其中，所述预设条件为所述点云数据的分辨率大于预设分辨率且其点云数量大于预设值。
7.根据权利要求1所述方法，其特征在于，获取若干点云数据以及所述惯性传感单元采集的惯性数据之前，所述方法还包括：
确定推荐定位信息GPRMC；
根据所述GPRMC，确定协调世界时UTC；
根据所述UTC，同步所述激光雷达及所述惯性传感单元的时间；其中，所述激光雷达及所述惯性传感单元预先进行脉冲同步，所述脉冲同步通过所述惯性传感单元的秒脉冲输出端发送秒脉冲至所述激光雷达的秒脉冲接收端实现。
8.根据权利要求1所述方法，其特征在于，基于所述位置姿态变换信息、所述惯性姿态变换信息，确定卡尔曼滤波量测方程，具体包括：
所述卡尔曼滤波量测方程具体公式：
Figure FDA0003432380540000031
Figure FDA0003432380540000032
其中，δp为量测的系统位移向量，
Figure FDA0003432380540000033
为所述预设时间间隔的惯性姿态变换信息中位移增量，p为所述预设时间间隔的所述点云数据之间的位置姿态变换信息中的位移信息；
Figure FDA0003432380540000034
为量测的系统旋转向量，
Figure FDA0003432380540000035
为所述预设时间间隔的惯性姿态变换信息中旋转增量，
Figure FDA0003432380540000036
为正态分布变换NDT算法在第k帧至第k+1帧的旋转矩阵，
Figure FDA0003432380540000037
为所述激光雷达与所述惯性传感单元的所述外部参数，I为单位矩阵。
9.一种传感器外部参数标定设备，其特征在于，所述设备包括：
至少一个处理器；以及，
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够：
获取若干点云数据以及惯性传感单元采集的惯性数据；
基于所述点云数据相应的参考帧，生成所述点云数据的参考时间坐标系；所述参考时间坐标系为将所述参考帧，相应的激光雷达的位置作为坐标系原点建立的；
根据所述参考时间坐标系内的所述点云数据，确定预设时间间隔的各所述点云数据之间的位置姿态变换信息；
根据所述惯性数据，确定所述惯性传感单元在所述预设时间间隔的惯性姿态变换信息；
基于所述位置姿态变换信息、所述惯性姿态变换信息，确定卡尔曼滤波量测方程，以基于卡尔曼滤波算法，确定所述激光雷达与所述惯性传感单元之间的外部参数。
10.一种传感器外部参数标定的溯源非易失性计算机存储介质，存储有计算机可执行指令，其特征在于，所述计算机可执行指令设置为：
获取若干点云数据以及惯性传感单元采集的惯性数据；
基于所述点云数据相应的参考帧，生成所述点云数据的参考时间坐标系；所述参考时间坐标系为将所述参考帧，相应的激光雷达的位置作为坐标系原点建立的；
根据所述参考时间坐标系内的所述点云数据，确定预设时间间隔的各所述点云数据之间的位置姿态变换信息；
根据所述惯性数据，确定所述惯性传感单元在所述预设时间间隔的惯性姿态变换信息；
基于所述位置姿态变换信息、所述惯性姿态变换信息，确定卡尔曼滤波量测方程，以基于卡尔曼滤波算法，确定所述激光雷达与所述惯性传感单元之间的外部参数。
CN202111602608.8A 2021-12-24 2021-12-24 一种传感器外部参数标定方法、设备及介质 Active CN114413887B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111602608.8A CN114413887B (zh) 2021-12-24 2021-12-24 一种传感器外部参数标定方法、设备及介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111602608.8A CN114413887B (zh) 2021-12-24 2021-12-24 一种传感器外部参数标定方法、设备及介质

Publications (2)

Publication Number Publication Date
CN114413887A true CN114413887A (zh) 2022-04-29
CN114413887B CN114413887B (zh) 2024-04-02

Family

ID=81269035

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111602608.8A Active CN114413887B (zh) 2021-12-24 2021-12-24 一种传感器外部参数标定方法、设备及介质

Country Status (1)

Country Link
CN (1) CN114413887B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115097426A (zh) * 2022-08-24 2022-09-23 盟识科技(苏州)有限公司 车载激光雷达更换后的自动标定方法、存储介质及车辆
CN115950474A (zh) * 2023-02-02 2023-04-11 广州沃芽科技有限公司 传感器的外参标定方法、装置、设备、介质和程序产品
CN116718153A (zh) * 2023-08-07 2023-09-08 成都云智北斗科技有限公司 一种基于gnss和ins的形变监测方法及系统

Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2013045927A1 (en) * 2011-09-30 2013-04-04 The Chancellor Masters And Scholars Of The University Of Oxford Determining extrinsic calibration parameters for a sensor
US20170302909A1 (en) * 2016-04-18 2017-10-19 Otis Elevator Company Auto commissioning system and method
CN109901139A (zh) * 2018-12-28 2019-06-18 文远知行有限公司 激光雷达标定方法、装置、设备和存储介质
CN109901138A (zh) * 2018-12-28 2019-06-18 文远知行有限公司 激光雷达标定方法、装置、设备和存储介质
CN110514225A (zh) * 2019-08-29 2019-11-29 中国矿业大学 一种矿井下多传感器融合的外部参数标定及精准定位方法
CN110703229A (zh) * 2019-09-25 2020-01-17 禾多科技（北京）有限公司 点云去畸变方法及车载激光雷达到imu的外参标定方法
CN110837080A (zh) * 2019-10-28 2020-02-25 武汉海云空间信息技术有限公司 激光雷达移动测量系统的快速标定方法
CN110849362A (zh) * 2019-11-28 2020-02-28 湖南率为控制科技有限公司 一种基于车载惯性的激光雷达与视觉组合导航算法
CN112285676A (zh) * 2020-10-22 2021-01-29 知行汽车科技（苏州）有限公司 激光雷达与imu外参标定方法及装置
CN112577517A (zh) * 2020-11-13 2021-03-30 上汽大众汽车有限公司 一种多元定位传感器联合标定方法和系统
CN112882053A (zh) * 2021-01-21 2021-06-01 清华大学深圳国际研究生院 一种主动标定激光雷达和编码器外参的方法
CN113310505A (zh) * 2021-06-15 2021-08-27 苏州挚途科技有限公司 传感器系统的外参标定方法、装置及电子设备
CN113327289A (zh) * 2021-05-18 2021-08-31 中山方显科技有限公司 一种多源异构传感器的同时内外参标定方法
WO2021174507A1 (zh) * 2020-03-05 2021-09-10 深圳市大疆创新科技有限公司 参数标定方法、装置、系统和存储介质
WO2021237520A1 (zh) * 2020-05-27 2021-12-02 华为技术有限公司 外参标定方法、装置、设备及存储介质

Patent Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2013045927A1 (en) * 2011-09-30 2013-04-04 The Chancellor Masters And Scholars Of The University Of Oxford Determining extrinsic calibration parameters for a sensor
US20170302909A1 (en) * 2016-04-18 2017-10-19 Otis Elevator Company Auto commissioning system and method
CN109901139A (zh) * 2018-12-28 2019-06-18 文远知行有限公司 激光雷达标定方法、装置、设备和存储介质
CN109901138A (zh) * 2018-12-28 2019-06-18 文远知行有限公司 激光雷达标定方法、装置、设备和存储介质
CN110514225A (zh) * 2019-08-29 2019-11-29 中国矿业大学 一种矿井下多传感器融合的外部参数标定及精准定位方法
CN110703229A (zh) * 2019-09-25 2020-01-17 禾多科技（北京）有限公司 点云去畸变方法及车载激光雷达到imu的外参标定方法
CN110837080A (zh) * 2019-10-28 2020-02-25 武汉海云空间信息技术有限公司 激光雷达移动测量系统的快速标定方法
CN110849362A (zh) * 2019-11-28 2020-02-28 湖南率为控制科技有限公司 一种基于车载惯性的激光雷达与视觉组合导航算法
WO2021174507A1 (zh) * 2020-03-05 2021-09-10 深圳市大疆创新科技有限公司 参数标定方法、装置、系统和存储介质
WO2021237520A1 (zh) * 2020-05-27 2021-12-02 华为技术有限公司 外参标定方法、装置、设备及存储介质
CN112285676A (zh) * 2020-10-22 2021-01-29 知行汽车科技（苏州）有限公司 激光雷达与imu外参标定方法及装置
CN112577517A (zh) * 2020-11-13 2021-03-30 上汽大众汽车有限公司 一种多元定位传感器联合标定方法和系统
CN112882053A (zh) * 2021-01-21 2021-06-01 清华大学深圳国际研究生院 一种主动标定激光雷达和编码器外参的方法
CN113327289A (zh) * 2021-05-18 2021-08-31 中山方显科技有限公司 一种多源异构传感器的同时内外参标定方法
CN113310505A (zh) * 2021-06-15 2021-08-27 苏州挚途科技有限公司 传感器系统的外参标定方法、装置及电子设备

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
彭文正;敖银辉;黄晓涛;王鹏飞;: "多传感器信息融合的自动驾驶车辆定位与速度估计", 传感技术学报, no. 08, 15 August 2020 (2020-08-15) *
杨浩;张峰;叶军涛;: "摄像机和惯性测量单元的相对位姿标定方法", 机器人, no. 04, 15 July 2011 (2011-07-15) *
熊敏君;卢惠民;熊丹;肖军浩;吕鸣;: "基于单目视觉与惯导融合的无人机位姿估计", 计算机应用, no. 2, 20 December 2017 (2017-12-20) *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115097426A (zh) * 2022-08-24 2022-09-23 盟识科技(苏州)有限公司 车载激光雷达更换后的自动标定方法、存储介质及车辆
CN115950474A (zh) * 2023-02-02 2023-04-11 广州沃芽科技有限公司 传感器的外参标定方法、装置、设备、介质和程序产品
CN115950474B (zh) * 2023-02-02 2023-12-29 广州沃芽科技有限公司 传感器的外参标定方法、装置、设备、介质和程序产品
CN116718153A (zh) * 2023-08-07 2023-09-08 成都云智北斗科技有限公司 一种基于gnss和ins的形变监测方法及系统
CN116718153B (zh) * 2023-08-07 2023-10-27 成都云智北斗科技有限公司 一种基于gnss和ins的形变监测方法及系统

Also Published As

Publication number Publication date
CN114413887B (zh) 2024-04-02

Similar Documents

Publication Publication Date Title
He et al. An integrated GNSS/LiDAR-SLAM pose estimation framework for large-scale map building in partially GNSS-denied environments
CN114413887B (zh) 一种传感器外部参数标定方法、设备及介质
CN112859051B (zh) 激光雷达点云运动畸变的矫正方法
KR101220527B1 (ko) 센서 시스템, 이를 이용하는 환경 지도 작성 시스템 및 방법
JP2017090239A (ja) 情報処理装置、制御方法、プログラム及び記憶媒体
CN110187375A (zh) 一种基于slam定位结果提高定位精度的方法及装置
CN112731358B (zh) 一种多激光雷达外参在线标定方法
CN112781586B (zh) 一种位姿数据的确定方法、装置、电子设备及车辆
CN111862215B (zh) 一种计算机设备定位方法、装置、计算机设备和存储介质
CN111077907A (zh) 一种室外无人机的自主定位方法
CN108613675B (zh) 低成本无人飞行器移动测量方法及系统
CN110120093A (zh) 一种多元特征混合优化的rgb-d室内三维测图方法及系统
CN114608554B (zh) 一种手持slam设备以及机器人即时定位与建图方法
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN113358112A (zh) 一种地图构建方法及一种激光惯性里程计
CN111398980A (zh) 一种机载LiDAR数据处理的方法及装置
CN115451948A (zh) 一种基于多传感器融合的农业无人车定位里程计方法及系统
CN115330866A (zh) 一种融合激光雷达和深度相机点云的建图定位方法
CN115290071A (zh) 相对定位融合方法、装置、设备及存储介质
CN114111776B (zh) 定位方法及相关装置
CN113639722B (zh) 连续激光扫描配准辅助惯性定位定姿方法
Sternberg et al. Precise indoor mapping as a basis for coarse indoor navigation
CN114690229A (zh) 一种融合gps的移动机器人视觉惯性导航方法
CN113959437A (zh) 一种用于移动测量设备的测量方法及系统
EP3475653B1 (en) Integrating point cloud scans, image data, and total station data from a surveying instrument into one adjustment

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