CN117388830A - 激光雷达与惯性导航的外参标定方法、装置、设备及介质 - Google Patents
激光雷达与惯性导航的外参标定方法、装置、设备及介质 Download PDFInfo
- Publication number
- CN117388830A CN117388830A CN202311381175.7A CN202311381175A CN117388830A CN 117388830 A CN117388830 A CN 117388830A CN 202311381175 A CN202311381175 A CN 202311381175A CN 117388830 A CN117388830 A CN 117388830A
- Authority
- CN
- China
- Prior art keywords
- laser radar
- inertial navigation
- point cloud
- external parameter
- time synchronization
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 87
- 238000012545 processing Methods 0.000 claims abstract description 84
- 239000011159 matrix material Substances 0.000 claims abstract description 65
- 238000001914 filtration Methods 0.000 claims abstract description 29
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 27
- 230000010354 integration Effects 0.000 claims description 26
- 238000004364 calculation method Methods 0.000 claims description 23
- 238000003860 storage Methods 0.000 claims description 13
- 238000007781 pre-processing Methods 0.000 claims description 7
- 238000005259 measurement Methods 0.000 description 22
- 238000005457 optimization Methods 0.000 description 20
- 230000008569 process Effects 0.000 description 17
- 230000001133 acceleration Effects 0.000 description 11
- 230000004927 fusion Effects 0.000 description 10
- 238000006243 chemical reaction Methods 0.000 description 7
- 238000004590 computer program Methods 0.000 description 6
- 230000008859 change Effects 0.000 description 5
- 238000010276 construction Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 5
- 238000000605 extraction Methods 0.000 description 5
- 230000009466 transformation Effects 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 4
- 238000004891 communication Methods 0.000 description 3
- 230000007613 environmental effect Effects 0.000 description 3
- 238000000691 measurement method Methods 0.000 description 3
- 238000013519 translation Methods 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000010924 continuous production Methods 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 2
- 230000003993 interaction Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000013178 mathematical model Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 230000007704 transition Effects 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 238000013475 authorization Methods 0.000 description 1
- 230000015556 catabolic process Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000005284 excitation Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 238000003754 machining Methods 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 230000001953 sensory effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Classifications
-
- 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
-
- 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
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/25—Fusion techniques
-
- 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
Abstract
本申请提供一种激光雷达与惯性导航的外参标定方法、装置、设备及介质,涉及自动驾驶中的多传感器联合标定技术领域。该方法包括:获取目标车辆上的激光雷达采集到的点云数据和惯性导航采集到的运动数据;对点云数据和运动数据进行时间同步处理,并根据时间同步处理后的点云数据、时间同步处理后的运动数据和预设的手眼标定模型计算激光雷达与惯性导航的外参初始矩阵;利用扩展卡尔曼滤波算法对外参初始矩阵进行调整,得到激光雷达与惯性导航的外参标定结果。上述方式提高了外参标定效率。
Description
技术领域
本申请涉及自动驾驶中的多传感器融合定位技术领域,尤其涉及一种激光雷达与惯性导航的外参标定方法、装置、设备及介质。
背景技术
随着科学技术的发展,多传感器融合定位技术逐渐成为自动驾驶领域的关键技术之一。其中,激光雷达能够实时捕捉环境中的点云信息,具有较高的精度,是目前自动驾驶技术领域的主流传感器之一。惯性导航(Inertial Measurement Unit,IMU)可以实时获取车辆的运动数据,在短时间内具有较高的精度,是目前自动驾驶定位,尤其是多传感器融合定位中不可缺少的模块。融合激光雷达和IMU的定位技术是目前研究的热点之一。标定传感器是自动驾驶定位中的必要环节,也是后续传感器融合的必要步骤和先决条件,目的是将两个或多个传感器变换到统一的时空坐标系,使多传感器融合更有意义,例如:通过标定获得激光雷达和IMU之间的相对位置和姿态,并将各传感器的数据进行融合,以提高激光雷达的精度和准确性,进而提高融合定位精度。
在实际使用中,通过数模测量的方式得到的数值较为准确,但未考虑实际加工误差的影响,在实车上测量又难以保证测量结果的可靠性。目前常用的标定方法大多需要基于外部标志物来提高精度。
但是该方法存在由操作不便,标定过程复杂导致的外参标定效率低的问题。
发明内容
本申请提供了一种激光雷达与惯性导航的外参标定方法、装置、设备及介质,用以解决现有技术存在的外参标定效率低的问题。
根据本申请的第一方面,提供了一种激光雷达与惯性导航的外参标定方法,包括:
获取目标车辆上的激光雷达采集到的点云数据和惯性导航采集到的运动数据;
对所述点云数据和所述运动数据进行时间同步处理,并根据时间同步处理后的点云数据、时间同步处理后的运动数据和预设的手眼标定模型计算激光雷达与惯性导航的外参初始矩阵;
利用扩展卡尔曼滤波算法对所述外参初始矩阵进行调整,得到激光雷达与惯性导航的外参标定结果。
可选地,所述根据时间同步处理后的点云数据、时间同步处理后的运动数据和预设的手眼标定模型计算激光雷达与惯性导航的外参初始矩阵,包括:
根据时间同步处理后的点云数据构建激光雷达里程计,根据时间同步处理后的运动数据构建惯性导航里程计;
根据所述激光雷达里程计和所述惯性导航里程计,利用预设的手眼标定模型计算激光雷达与惯性导航的外参初始矩阵。
可选地,根据时间同步处理后的点云数据构建激光雷达里程计,包括:
对所述时间同步处理后的点云数据进行预处理,得到预处理后的点云数据;
从所述预处理后的点云数据中提取出各数据帧的线面特征;
从所有所述数据帧中提取出激光雷达关键帧,并根据其余数据帧的线面特征构建局部地图;其中,所述其余数据帧与所述激光雷达关键帧之间的距离满足预设条件;
通过滑动窗口法将所述激光雷达关键帧与所述局部地图进行匹配,并在匹配结果为成功的情况下,获得激光雷达里程计。
可选地,所述根据时间同步处理后的运动数据构建惯性导航里程计,包括:
根据时间同步处理后的运动数据,利用预设的IMU运动学模型进行IMU位姿解算,得到位姿解算结果;其中,IMU位姿解算包括:姿态解算、速度解算和位置解算;
根据所述位姿解算结果,确定惯性导航里程计。
可选地,所述利用扩展卡尔曼滤波算法对所述外参初始矩阵进行调整,得到激光雷达与惯性导航的外参标定结果,包括:
根据时间同步处理后的运动数据确定惯性导航的状态量,根据时间同步处理后的点云数据确定相邻两个激光雷达关键帧之间的位姿估计;
根据作为观测值的相邻两个激光雷达关键帧之间的位姿估计,作为预测值的所述惯性导航的状态量,利用所述扩展卡尔曼滤波算法计算所述预测值和所述观测值之间的差值,根据所述差值计算残差,并根据所述残差更新外参初始矩阵,得到所述外参标定结果。
可选地,所述根据时间同步处理后的运动数据确定惯性导航的状态量,包括:
对时间同步处理后的运动数据进行预积分处理,得到预积分结果;
根据所述预积分结果进行状态估计,获得惯性导航的状态量。
可选地,所述根据时间同步处理后的点云数据确定相邻两个激光雷达关键帧之间的位姿估计,包括:
根据所述预积分结果,对所述时间同步处理后的点云数据进行去畸变处理,得到去畸变点云;
对所述去畸变点云执行匹配,得到相邻两个激光雷达关键帧之间的位姿估计。
根据本申请的第二方面,提供了一种激光雷达与惯性导航的外参标定装置,包括:
获取模块,用于获取目标车辆上的激光雷达采集到的点云数据和惯性导航采集到的运动数据;
处理计算模块,用于对所述点云数据和所述运动数据进行时间同步处理,并根据时间同步处理后的点云数据、时间同步处理后的运动数据和预设的手眼标定模型计算激光雷达与惯性导航的外参初始矩阵;
调整模块,用于利用扩展卡尔曼滤波算法对所述外参初始矩阵进行调整,得到激光雷达与惯性导航的外参标定结果。
根据本申请的第三方面,提供了一种车辆控制设备,包括:至少一个处理器和存储器;
所述存储器存储计算机执行指令;
所述至少一个处理器执行所述存储器存储的计算机执行指令,使得所述至少一个处理器执行如上第一方面所述的激光雷达与惯性导航的外参标定方法。
根据本申请的第四方面,提供了一种计算机可读存储介质,所述计算机可读存储介质中存储有计算机执行指令,所述计算机执行指令被处理器执行时用于实现如上第一方面所述的激光雷达与惯性导航的外参标定方法。
根据本申请的第五方面,提供了一种计算机程序产品,包括计算机程序,该计算机程序被处理器执行时实现第一方面所述的激光雷达与惯性导航的外参标定方法。
本申请提供的一种激光雷达与惯性导航的外参标定方法,包括:获取目标车辆上的激光雷达采集到的点云数据和惯性导航采集到的运动数据;对点云数据和运动数据进行时间同步处理,并根据时间同步处理后的点云数据、时间同步处理后的运动数据和预设的手眼标定模型计算激光雷达与惯性导航的外参初始矩阵;利用扩展卡尔曼滤波算法对外参初始矩阵进行调整,得到激光雷达与惯性导航的外参标定结果。
本申请在目标车辆上测量,同时分别基于预设的手眼标定模型和扩展卡尔曼滤波算法,实现外参的初始化和优化,由于本申请采用自动化的标定流程,因此本申请能够避免手工测量方法的复杂性,进而提高外参标定效率。
应当理解,本部分所描述的内容并非旨在标识本申请的实施例的关键或重要特征,也不用于限制本申请的范围。本申请的其它特征将通过以下的说明书而变得容易理解。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分,示出了符合本申请的实施例,并与说明书一起用于解释本申请的原理。
图1为本申请实施例提供的一种激光雷达与惯性导航的外参标定方法的流程示意图;
图2为图1中步骤S20的流程示意图;
图3为图1中步骤S30的流程示意图;
图4为本申请实施例提供的另一种激光雷达与惯性导航的外参标定方法的流程示意图;
图5为本申请实施例提供的一种激光雷达与惯性导航的外参标定装置的结构示意图;
图6为本申请实施例提供的一种车辆控制设备的结构示意图。
通过上述附图,已示出本申请明确的实施例,后文中将有更详细的描述。这些附图和文字描述并不是为了通过任何方式限制本申请构思的范围,而是通过参考特定实施例为本领域技术人员说明本申请的概念。
具体实施方式
这里将详细地对示例性实施例进行说明,其示例表示在附图中。下面的描述涉及附图时,除非另有表示,不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本申请相一致的所有实施方式。
目前常用的标定方法大多需要基于外部标志物来提高精度。但是,该方法存在由操作不便,标定过程复杂导致的外参标定效率低的问题。
为了解决上述技术问题,本申请的整体发明构思为如何提供一种应用于自动驾驶中的多传感器联合标定领域,用于提高标定效率的激光雷达与惯性导航的外参标定方法。
下面以具体地实施例对本申请的技术方案以及本申请的技术方案如何解决上述技术问题进行详细说明。下面这几个具体的实施例可以相互结合,对于相同或相似的概念或过程可能在某些实施例中不再赘述。下面将结合附图,对本申请的实施例进行描述。
实施例1:
图1为本申请实施例提供的一种激光雷达与惯性导航的外参标定方法的流程示意图。如图1所示,本实施例的方法,包括:
S10、获取目标车辆上的激光雷达采集到的点云数据和惯性导航采集到的运动数据。
在本申请实施例中,本申请实施例中的激光雷达可以指3D激光雷达,或称为车载激光雷达。惯性导航或称为惯性测量单元、惯性导航单元、IMU传感器、IMU设备、IMU等。点云数据或称为激光雷达点云信息、激光雷达扫描数据、激光雷达数据、激光点云数据、激光点云等,运动数据或称为IMU数据、位置信息、IMU信息、IMU传感器数据等,点云数据和运动数据均为传感器数据。
该步骤的目的是实现数据采集,在实际应用中,本实施例可以将激光雷达和IMU固定安装在车辆上,以一定的速度行驶于标定场地,标定场地最好是具有明显环境特征的场地,记录激光雷达采集到的点云信息和IMU设备采集到的位置信息。
S20、对点云数据和运动数据进行时间同步处理,并根据时间同步处理后的点云数据、时间同步处理后的运动数据和预设的手眼标定模型计算激光雷达与惯性导航的外参初始矩阵。
本实施例对激光雷达采集到的点云数据和IMU采集到的运动数据进行时间同步处理,用于同步传感器数据的时间戳,实现时间校准,进而确保点云数据和运动数据的时间戳在统一的时间标准下。
具体的,时间同步可以体现在以下两个方面:
1)硬件时间同步:在实际车载传感器中,IMU一般与全球定位系统(GlobalPositioning System,GPS)相结合,本实施例使用GPS授时能够完成激光雷达和IMU传感器的硬件时间同步。
2)软件时间同步:本实施例可以使用数据插值方式进行时间软同步,以激光雷达作为主传感器,本实施例可以先获得主传感器的时间作为同步时间,然后根据索引结果获得这一同步时间前后的两帧激光雷达数据,然后根据前后两帧激光雷达数据的采集时刻,以及要插入的时刻,根据时间差计算权重线性插值,得到IMU传感器在同步时间的插值结果。
另外,外参初始矩阵或称为外参初值、外参初始值等。本申请实施例是在激光雷达与惯性导航的外参未知的情况下提出的方法,该方法主要分为两个部分:粗标定和外参优化,粗标定的目的是使用手眼标定模型得到一个粗略的外参初值。
S30、利用扩展卡尔曼滤波算法对外参初始矩阵进行调整,得到激光雷达与惯性导航的外参标定结果。
应理解,外参标定结果相当于下述实施例4中的最优外参。外参标定结果是一个矩阵,用于反映激光雷达坐标系和惯性导航单元坐标系之间的位姿转换关系。
本申请实施例是在激光雷达与惯性导航的外参未知的情况下提出的方法,该方法主要分为两个部分:粗标定和外参优化,在经过粗标定得到外参初值之后,本实施例在外参优化部分导入扩展卡尔曼滤波器进行外参优化。
本申请实施例在目标车辆上测量,同时分别基于预设的手眼标定模型和扩展卡尔曼滤波算法,实现外参的初始化和优化,由于本申请实施例采用自动化的标定流程,因此本申请实施例能够避免手工测量方法的复杂性,进而提高外参标定效率。
与现有技术相比,该申请实施例的有益效果在于:本申请提供的是一种高效、准确的基于扩展卡尔曼滤波算法的激光雷达与惯性导航的外参动态标定方法,具有以下优点:
(1)标定效率高:本方法采用自动化的标定流程,避免了手工测量方法的复杂性,提高了标定的效率;(2)标定精度高:本方法通过建立数学模型标定优化,可以提高标定的精度和稳定性;(3)适用性强:本方法适用于不同类型的车辆和传感器设备,具有较强的通用性和可扩展性。
因此,本申请提供的方法具有广泛的应用前景,可以应用于自动驾驶、机器人导航等领域,为实现高精度的多传感器融合定位建图提供重要支持。
在上述实施例的基础上,下面结合几个具体的实施例对本申请技术方案进行更详细的描述。
实施例2:
图2为图1中步骤S20的流程示意图。如图2所示,在步骤S20中,根据时间同步处理后的点云数据、时间同步处理后的运动数据和预设的手眼标定模型计算激光雷达与惯性导航的外参初始矩阵,包括:
S21、根据时间同步处理后的点云数据构建激光雷达里程计,根据时间同步处理后的运动数据构建惯性导航里程计。应理解,惯性导航里程计或称为IMU里程计。
S22、根据激光雷达里程计和惯性导航里程计,利用预设的手眼标定模型计算激光雷达与惯性导航的外参初始矩阵。
本实施例中的时间同步处理后的点云数据和时间同步处理后的运动数据分别用于实现激光雷达里程计和惯性导航里程计的构建,将构建好的激光雷达里程计和惯性导航里程计均输入至预设的手眼标定模型,以使该模型输出激光雷达与惯性导航的外参初始矩阵。
本申请实施例在根据上述激光雷达里程计得到相邻两帧激光雷达关键帧之间的点云配准关系和根据IMU里程计得到相邻两个时间戳之间的IMU位姿转换关系之后,构建手眼标定模型,使用非线性最小二乘解算器求解手眼标定模型的优化问题,得到外参初值,具体的:
本实施例使用手眼标定模型对激光雷达与IMU的外参进行标定,得到外参标定的初值。标准的手眼标定模型为其中,/>为两帧激光雷达关键帧之间的相对位姿转换矩阵,/>为激光雷达与IMU之间的外参矩阵,/>为两帧IMU之间的相对位姿转换矩阵;本申请实施例可以在标定过程中始终使用相对于第一帧的相对姿态,以防止由于运动激励不足导致的平移参数约束退化,IMU从当前帧到第一帧的相对位姿转换为/>其中,/>为第一帧在世界坐标系下的位姿转换矩阵,同理,激光雷达从当前帧到第一帧的相对位姿转换为/>基于上述描述,将标准的手眼标定模型变形为手眼标定模型/>本实施例还可以构造如下代价函数:/>其中,I包含所有两两相对约束;本实施例通过最小化上述代价函数的方式能够获得外参初值,初始化过程为自动化实现过程,进一步提高了标定的效率,并且该外参初值为后续外参优化提供数据支撑。
一种可能的实现方式中,在步骤S21中,根据时间同步处理后的点云数据构建激光雷达里程计,包括:
S211、对时间同步处理后的点云数据进行预处理,得到预处理后的点云数据。
S212、从预处理后的点云数据中提取出各数据帧的线面特征。
S213、从所有数据帧中提取出激光雷达关键帧,并根据其余数据帧的线面特征构建局部地图;其中,其余数据帧与激光雷达关键帧之间的距离满足预设条件。
S214、通过滑动窗口法将激光雷达关键帧与局部地图进行匹配,并在匹配结果为成功的情况下,获得激光雷达里程计。具体的,上述构建过程的具体流程如下:
1)本实施例接收经过时间校准后的激光点云数据,对采集到的激光点云数据进行预处理,具体的:设置点云读取间隔,例如:每隔20ms从原始的激光雷达数据中采集一次数据点;采集完成后去除激光点云的NaN值点;对激光点云进行降采样滤波,使用体素滤波Voxel Grid Fliter方式进行平均网格降采样,网格大小可以选择边长为0.5m的立方体,并将网格中的点云质心化为单点。
2)线面特征提取:本实施例对经过预处理的激光点云进行线面特征提取,在提取过程中,先计算曲率,再根据曲率大小提取线特征和面特征。
2.1)计算曲率:本实施例利用当前激光点前后共10个点计算当前激光点的曲率,记当前激光点为第i个点,将当前激光点前后10个点与当前激光点距离差值的平方作为曲率,计算公式为:
其中,c是曲率,pi为第i个点的三轴坐标(xi,yi,zi),pj为第i个点前后10个点中第j个点的三轴坐标(xj,yj,zj)。
2.2)特征点提取:曲率计算完成后,本实施例可以按照曲率大小分别提取线特征点和面特征点,同时为了均匀描述环境特征,本实施例还可以将一帧激光雷达数据等分为6个子区域,并将每个子区域中曲率较大的5个点选为线特征点,将曲率较小的10个点选为面特征点。
3)构建激光雷达里程计:为降低计算量,本实施例可以提取关键帧,并通过关键帧进行点云配准;此外,为避免累积误差,提升系统效率,本实施例还可以使用滑动窗口法进行点云地图匹配,具体分析如下:
本实施例提取与当前关键帧最近的n个历史关键帧,并将n个历史关键帧组成局部地图,并使用滑动窗口法不断更新关键帧完成激光雷达里程计。具体的,本实施例采用2)线面特征提取方法提取第i个关键帧的特征构成一个激光雷达点云帧Fi={Fi e,Fi f},其中Fi e为提取的线特征集合,Fi f为提取的面特征集合。
一帧激光雷达数据可以理解为激光雷达帧,当接收到新的激光雷达帧时,若当前帧为第一帧,则将当前帧设为关键帧并加入局部地图,若当前帧不是第一帧,则判断当前帧与上一关键帧之间的位姿变换是否超过一定的阈值,若是,则将当前帧设置为关键帧并加入滑动窗口中,同时删除滑动窗口中时间最久的关键帧。
当接收到新的关键帧时,本实施例基于利用之前n个关键帧的特征集合{F0,…,Fn-1}构建的局部地图(其中,/>分别是变换到世界坐标系下的局部线特征地图和局部面特征地图),将当前关键帧与局部地图匹配,并将当前关键帧的线特征点和面特征点分别变换到局部地图下,查找与当前关键帧最近的5个角点(记为pi(x,y,z),i=1,…5),并计算5个点的均值坐标(记为中心点),然后计算5个角点与中心点之间的距离,进而构建协方差矩阵,如下式:
其中,ai是第i个角点与中心点之间的距离,aix、aiy和aiz分别是距离ai在x、y和z三个坐标轴方向上的分量。
在构建完协方差矩阵之后,本实施例计算特征值,若5个角点构成直线,则表示匹配成功。同理,在局部地图中查找5个最近的平面点,若5个平面点构成平面,则表示匹配成功,将匹配成功的点提取出来,根据匹配成功的特征点计算雅可比(Jacabian)矩阵的行列式,再进行迭代优化,得到相邻两帧激光雷达关键帧之间的位姿估计。
本申请实施例对激光雷达里程计的构建过程进行了细化,该激光雷达里程计为后续提供准确的外参初始值提供数据支撑。
一种可能的实现方式中,在步骤S21中,根据时间同步处理后的运动数据构建惯性导航里程计,包括:
步骤S221、根据时间同步处理后的运动数据,利用预设的IMU运动学模型进行IMU位姿解算,得到位姿解算结果;其中,IMU位姿解算包括:姿态解算、速度解算和位置解算。
步骤S222、根据位姿解算结果,确定惯性导航里程计。
具体的,本实施例接收经过时间校准后的IMU信息,根据IMU运动学模型对相邻两个时间戳内的IMU数据进行积分,构建IMU里程计,该IMU里程计用于表示相邻两个时间戳内的IMU位姿变换。
本实施例对IMU的角速度和加速度的观测公式进行如下定义:
其中,和/>分别是IMU坐标系下t时刻的IMU观测源的角速度数据和加速度数据,且/>受到缓慢变化的IMU零偏/>和白噪声/>的影响,/>受到缓慢变化的IMU零偏/>和白噪声/>的影响。/>用于描述IMU坐标系到世界坐标系的变换矩阵,/>是角速度理论值,a(t)是加速度理论值,g是重力常数。
本实施例对加速度和角速度分别积分可得:
其中,v是IMU的速度,P是IMU的位移,R是旋转矩阵,假设IMU的角速度和加速度在积分过程中保持匀速不变,上式可以写成:
其中,是加速度的平均值,/>是角速度的平均值。这三个公式中其余参数的描述与之前描述一致,本实施例不再赘述。本实施例通过这三个公式,可以得到相邻两个时间戳之间的IMU相对位姿转换。
本申请实施例对惯性导航里程计的构建过程进行了细化,该惯性导航里程计为后续提供准确的外参初始值提供数据支撑。本申请实施例涉及自动驾驶多传感器融合定位领域,具体涉及激光雷达与IMU设备的外参标定方法,可以解决多传感器融合定位效率低的问题。
实施例3:
图3为图1中步骤S30的流程示意图。如图3所示,步骤S30、利用扩展卡尔曼滤波算法对外参初始矩阵进行调整,得到激光雷达与惯性导航的外参标定结果,包括:
S31、根据时间同步处理后的运动数据确定惯性导航的状态量,根据时间同步处理后的点云数据确定相邻两个激光雷达关键帧之间的位姿估计。
S32、根据作为观测值的相邻两个激光雷达关键帧之间的位姿估计,作为预测值的惯性导航的状态量,利用扩展卡尔曼滤波算法计算预测值和观测值之间的差值,根据差值计算残差,并根据残差更新外参初始矩阵,得到外参标定结果。
假设有如下线性离散系统,扩展卡尔曼滤波的模型为:
xk=f(xk-1,uk)+ωk;
zk=h(xk)+vk;
其中,xk为k时刻的预测值,zk为k时刻的观测值,xk-1为k-1时刻的预测值,uk为控制向量,ωk为过程演化噪声,vk为测量噪声。
扩展卡尔曼滤波预测公式包括状态向量方程和状态向量协方差方程,基于上述扩展卡尔曼滤波的模型可知,预测部分的状态向量方程为:
其中,为车辆状态的预测值。xk-1为k-1时刻的预测值,uk为控制向量。
预测部分的状态向量协方差方程为:
其中,为k时刻状态向量协方差的先验估计,Pk-1为k-1时刻状态向量的协方差矩阵,代表着状态向量每个元素之间的关系,JA为状态转移矩阵的雅可比矩阵,Qk表示预测状态的高斯噪声的协方差矩阵,其值越小代表模型越准确。
新的状态变量是根据上一状态变量/>加上已知的外部控制变量/>预测得到的,而新的状态向量协方差矩阵/>由上一状态向量协方差Pk-1加上噪声协方差矩阵Qk得到。
扩展卡尔曼滤波的更新公式/卡尔曼增益方程为:
其中,K为卡尔曼增益,为k时刻状态向量协方差的先验估计,JH为转换矩阵的雅可比矩阵,Rk为测量值的协方差矩阵。
卡尔曼增益中的测量值的协方差矩阵Rk与传感器性能有关,不会随时间变化而变化,但是在实际应用中,测量值的协方差矩阵容易受到环境干扰而进行变化。本实施例在得到矩阵Rk后,根据预测部分的误差协方差矩阵可以计算出卡尔曼增益。当卡尔曼增益为0时,这意味着预测的非常准确;当卡尔曼增益为1时,下一时刻的状态估计与观测值有很大的关系,更新部分的状态方程为:
其中,为k时刻状态向量,/>为k时刻状态向量的先验估计,/>为传感器测量值的状态向量。
更新部分的状态向量协方差方程为:
其中,Pk为状态向量的协方差矩阵,代表着状态向量每个元素之间的关系,I为矩阵,JH为转换矩阵的雅可比矩阵,为k时刻状态向量协方差的先验估计。
在本申请实施例中,扩展卡尔曼滤波的状态更新模型需要了解测量模型、测量残差以及与状态变量相关的雅可比,在状态估计值下计算得到的测量模型提供了预测的旋转和平移测量值,真实的测量值和预测值之间的差异为本实施例提供了状态更新所需的测量残差,将上述两种类型的数值导入状态更新模型,即可得到更新后的状态,进而完成一次更新。应理解,优化后得到的最终外参的符号是
本申请实施例对扩展卡尔曼滤波算法的输入、更新过程均进行了详细描述,本实施例可以在优化阶段自动计算出惯性导航的状态量和相邻两个激光雷达关键帧之间的位姿估计,进而利用扩展卡尔曼滤波算法得到准确的外参标定结果。
一种可能的实现方式中,在步骤S31中,根据时间同步处理后的运动数据确定惯性导航的状态量,包括:
S311、对时间同步处理后的运动数据进行预积分处理,得到预积分结果。
S312、根据预积分结果进行状态估计,获得惯性导航的状态量。
本实施例通过IMU传感器可以实时获取到相应的加速度及角速度信息,对其直接积分可以获得IMU位姿估计信息,但随着行驶路程不断增加,以及温度的影响,IMU产生的累积误差不断增大,直接进行积分极其容易导致最后估计位姿发生巨大漂移。
本实施例将IMU状态定义为:其中:/>和/>分别表示为在世界坐标系W下第j时刻的位置矩阵、载体IMU速度和旋转矩阵,/>为当前载体IMU的加速度偏置,/>为角速度偏置。
本实施例根据IMU数据和第i时刻的状态估计值进行离散估计,得到当前IMU的状态量为:
其中:Δt为连续两次IMU测量的时间间隔,gW为世界坐标系下的重力矢量,Rk为IMU坐标系转到世界坐标系的旋转矩阵,为i时刻世界坐标系下的位置,/>为k时刻加速度值,/>为k时刻加速度计零偏,/>为i时刻世界坐标系下的旋转矩阵,/>为k时刻在世界坐标系下的旋转矩阵,/>为k时刻角速度值,/>为k时刻角速度零偏,δ表示变化量。由此得到IMU状态估计和预积分的结果,作为扩展卡尔曼滤波算法状态更新的预测值。
本实施例可以在优化阶段自动计算出惯性导航的状态量,为后续利用扩展卡尔曼滤波算法得到准确的外参标定结果提供技术支撑。
一种可能的实现方式中,在步骤S31中,根据时间同步处理后的点云数据确定相邻两个激光雷达关键帧之间的位姿估计,包括:
S321、根据预积分结果,对时间同步处理后的点云数据进行去畸变处理,得到去畸变点云。
本实施例可以通过IMU位姿估计得到每个激光点的采集时刻,将所有的激光点映射到相同时刻,其中,tk为激光雷达扫描帧的初始时刻,tk+1为激光雷达一帧扫描完成的终止时刻,tk+1-tk为固定的扫描时间,本实施例为校正激光雷达的每个激光点,可以计算出校正的变换矩阵对于每个激光雷达的点,本实施例使用补偿矩阵/>与原始的激光点坐标进行相乘,即可校正运动畸变,最后得到校正后的激光点的坐标信息。
S322、对去畸变点云执行匹配,得到相邻两个激光雷达关键帧之间的位姿估计。
本实施例在该步骤S322中,可以对经过校正的激光雷达点云,按照步骤S214中的点云配准方式进行激光点云配准,得到相邻两帧激光雷达关键帧之间的位姿估计。
本实施例可以在优化阶段自动计算出相邻两个激光雷达关键帧之间的位姿估计,为后续利用扩展卡尔曼滤波算法得到准确的外参标定结果提供技术支撑。本申请实施例涉及自动驾驶和机器人导航中的多传感器联合标定领域,可以有效提高标定效率。
实施例4:
图4为本申请实施例提供的另一种激光雷达与惯性导航的外参标定方法的流程示意图。该方法具体是一种基于扩展卡尔曼滤波算法的激光雷达与惯性导航的外参动态标定方法,方法包括外参初始化和外参优化两个阶段,由于本申请实施例针对的是激光雷达与IMU的无源外参标定,所以本实施例在外参初始化阶段能够提供一个外参初始值,然后利用包含基于扩展卡尔曼滤波算法的优化模块进行优化,得到最优外参。如图4所示,本实施例的方法包括:
S41、获取传感器数据。
S42、进行时间同步。
S43、构建IMU里程计,该步骤S43包括以下步骤:S431、读取时间同步后的IMU数据;S432、执行IMU位姿解算;S433、计算IMU里程计。
步骤S432中的IMU位姿解算,是利用IMU测量的角速度、加速度,利用上一时刻的导航信息,推算出当前时刻的导航信息(包括姿态解算、速度解算和位置解算)。该导航信息是姿态、速度、位置的微分方程,推导出各方程的解,并将解转变成离散时间下的近似形式,从而可以在离散时间采样下,完成导航信息的求解。IMU位姿解算的目的是得到相邻两个时间戳之间的IMU传感器的位姿变化,通常包括姿态的欧拉角或四元数表示。
S44、构建激光雷达里程计,该步骤S44包括以下步骤:S441、读取时间同步后的激光雷达数据;S442、对点云数据预处理;S443、计算激光雷达里程计。
S45、手眼标定,得到外参初始值。
得到的外参初始值应用到了步骤S47中,本申请实施例进行外参标定的目的是得到激光雷达与IMU传感器之间的旋转平移关系,本实施例通过粗标定得到外参初始值,该外参初始值是个粗略的值。在点云去畸变过程中,本实施例可以将激光雷达和IMU传感器统一在同一坐标系下,此时用到外参初始值进行坐标系之间的转换。
S46、IMU预积分,并进行状态预测/转移。应理解,IMU预积分是将IMU测量值预积分为IMU状态的增量,并提供给状态估计算法。从内容上看,步骤S46中的预积分所采用的积分方法可以是IMU位姿解算方法中的一种。本实施例根据IMU数据和第i时刻的状态估计值进行离散估计,得到IMU状态估计和预积分的结果,该结果可以作为状态更新的预测值。
图4中的步骤S431和步骤S46之前的读取操作可以理解为相同的数据读取两次,步骤S431中的读取操作是第一次读取操作,其目的是为了粗标定,得到激光雷达与IMU之间的外参初始值;步骤S46之前的读取操作是第二次读取操作,其目的是为了外参优化,利用得到的外参初始值导入扩展卡尔曼滤波器进行预测、去畸变循环优化等,得到外参标定结果。同理,图4中的步骤S441和步骤S47之前的读取操作可以理解为相同的数据读取两次。
该步骤S46中IMU预积分得到的结果与步骤S43中的IMU里程计是不同的两个概念。步骤S43中的IMU里程计是一种使用惯性导航的测量数据来估算机器人的运动轨迹和姿态的方法,而IMU预积分得到的结果通常包括IMU在一段时间内的姿态变化、速度变化和位移,这些结果是通过数值积分IMU测量值得到的,用于估计IMU的运动状态;上述两个概念可以理解为:IMU里程计是一个连续的过程,而IMU预积分是IMU里程计中两个时间戳内的实现方法。
S47、点云去畸变。在该步骤中,本实施例利用IMU预积分校正相邻两个激光雷达关键帧之间的位姿,用以去除激光雷达的运动畸变。
需要注意的是,点云去畸变与点云数据预处理是完全不同的两种操作。具体的,点云数据预处理是在粗标定阶段,对采集到的激光雷达点云进行降采样处理,去除激光点云中的NaN值点等点云的处理操作,也是通过一些方法对点云质量提升的操作;而点云去畸变是由于旋转式机械激光雷达固有的缺点,在运动时容易产生运动畸变,利用IMU预积分的方式,将同一帧激光雷达点云数据统一到该帧的起始时刻,可以消除运动畸变。
S48、点云匹配。该过程或描述为点云配准。本实施例将经过运动畸变校正的点云执行scan-to-local-map匹配,得到相邻两个激光雷达关键帧之间的运动估计值,将激光雷达的运动估计值作为状态更新的观测值。
该步骤S48中的点云匹配是激光雷达里程计中的一个实现方法,而步骤S44中的激光雷达里程计是一个连续的过程,可以理解为:点云匹配是其中的实现方法。
S49、利用扩展卡尔曼滤波算法EKF更新外参。本申请实施例中的外参优化阶段在扩展卡尔曼滤波器中完成,也就是说,本实施例执行步骤S49,用于得到最优外参。
本实施例使用的扩展卡尔曼滤波器,其实质是将预测模型预测得到的车辆状态向量和传感器测量到的车辆状态向量相乘,得到新的高斯分布的均值,该均值代表最优的状态估计。
本申请实施例将步骤S46得到的IMU状态估量和预积分的结果当作预测量,将步骤S48得到的相邻两个激光雷达关键帧之间的位姿估计当作观测值,均输入扩展卡尔曼滤波器,该预测值和观测值之间的差异为模型提供了状态更新所需的测量残差。
与现有技术相比,该发明的有益效果在于:
本申请提供的是一种高效、准确的基于扩展卡尔曼滤波算法的激光雷达与惯性导航的外参动态标定方法,具有以下优点:
(1)标定效率高:本方法采用自动化的标定流程,避免了手工测量方法的复杂性,提高了标定的效率;(2)标定精度高:本方法通过建立数学模型标定优化,可以提高标定的精度和稳定性;(3)适用性强:本方法适用于不同类型的车辆和传感器设备,具有较强的通用性和可扩展性。
因此,本申请实施例提供的方法具有广泛的应用前景,可以应用于自动驾驶、机器人导航等领域,为实现高精度的多传感器融合定位建图提供重要支持。
实施例5:
图5为本申请实施例提供的一种激光雷达与惯性导航的外参标定装置的结构示意图。本实施例的装置可以为软件和/或硬件的形式。如图5所示,本实施例提供的激光雷达与惯性导航的外参标定装置,包括:获取模块51、处理计算模块52和调整模块53。其中:
获取模块51,用于获取目标车辆上的激光雷达采集到的点云数据和惯性导航采集到的运动数据。
处理计算模块52,用于对点云数据和运动数据进行时间同步处理,并根据时间同步处理后的点云数据、时间同步处理后的运动数据和预设的手眼标定模型计算激光雷达与惯性导航的外参初始矩阵。
调整模块53,用于利用扩展卡尔曼滤波算法对外参初始矩阵进行调整,得到激光雷达与惯性导航的外参标定结果。
一种可能的实现方式中,处理计算模块52用于:
根据时间同步处理后的点云数据构建激光雷达里程计,根据时间同步处理后的运动数据构建惯性导航里程计。
根据激光雷达里程计和惯性导航里程计,利用预设的手眼标定模型计算激光雷达与惯性导航的外参初始矩阵。
一种可能的实现方式中,处理计算模块52用于:
对时间同步处理后的点云数据进行预处理,得到预处理后的点云数据。
从预处理后的点云数据中提取出各数据帧的线面特征。
从所有数据帧中提取出激光雷达关键帧,并根据其余数据帧的线面特征构建局部地图;其中,其余数据帧与激光雷达关键帧之间的距离满足预设条件。
通过滑动窗口法将激光雷达关键帧与局部地图进行匹配,并在匹配结果为成功的情况下,获得激光雷达里程计。
一种可能的实现方式中,处理计算模块52用于:
根据时间同步处理后的运动数据,利用预设的IMU运动学模型进行IMU位姿解算,得到位姿解算结果;其中,IMU位姿解算包括:姿态解算、速度解算和位置解算。
根据位姿解算结果,确定惯性导航里程计。
一种可能的实现方式中,调整模块53用于:
根据时间同步处理后的运动数据确定惯性导航的状态量,根据时间同步处理后的点云数据确定相邻两个激光雷达关键帧之间的位姿估计。
根据作为观测值的相邻两个激光雷达关键帧之间的位姿估计,作为预测值的惯性导航的状态量,利用扩展卡尔曼滤波算法计算预测值和观测值之间的差值,根据差值计算残差,并根据残差更新外参初始矩阵,得到外参标定结果。
一种可能的实现方式中,调整模块53用于:
对时间同步处理后的运动数据进行预积分处理,得到预积分结果。
根据预积分结果进行状态估计,获得惯性导航的状态量。
一种可能的实现方式中,调整模块53用于:
根据预积分结果,对时间同步处理后的点云数据进行去畸变处理,得到去畸变点云。
对去畸变点云执行匹配,得到相邻两个激光雷达关键帧之间的位姿估计。
本实施例提供的激光雷达与惯性导航的外参标定装置,可用于执行上述任意方法实施例提供的激光雷达与惯性导航的外参标定方法,其实现原理和技术效果类似,此处不做赘述。
需要说明的是,本申请所涉及的用户信息和数据(包括但不限于用于分析的数据、存储的数据、展示的数据等),均为经用户授权或者经过各方充分授权的信息和数据,并且相关数据的收集、使用和处理需要遵守相关国家和地区的相关法律法规和标准,并提供有相应的操作入口,供用户选择授权或者拒绝。
也就是说,本申请的技术方案中,所涉及的用户个人信息的收集、存储、使用、加工、传输、提供和公开等处理,均符合相关法律法规的规定,且不违背公序良俗。
根据本申请的实施例,本申请还提供了一种车辆控制设备和一种可读存储介质。
图6为本申请实施例提供的一种车辆控制设备的结构示意图。该车辆控制设备包括接收器60、发送器61、至少一个处理器62和存储器63,由上述部件构成的该车辆控制设备可以用来实施本申请上述几个具体的实施例,此处不再赘述。
本申请实施例还提供一种计算机可读存储介质,计算机可读存储介质中存储有计算机执行指令,当处理器执行计算机执行指令时,实现上述实施例中方法中的各个步骤。
本申请实施例还提供一种计算机程序产品,包括计算机程序,该计算机程序被处理器执行时实现上述实施例中方法中的各个步骤。
本申请以上描述的系统和技术的各种实施方式可以在数字电子电路系统、集成电路系统、场可编程门阵列(FPGA)、专用集成电路(ASIC)、专用标准产品(ASSP)、芯片上系统的系统(SOC)、负载可编程逻辑设备(CPLD)、计算机硬件、固件、软件、和/或它们的组合中实现。这些各种实施方式可以包括:实施在一个或者多个计算机程序中,该一个或者多个计算机程序可在包括至少一个可编程处理器的可编程系统上执行和/或解释,该可编程处理器可以是专用或者通用可编程处理器,可以从存储系统、至少一个输入装置、和至少一个输出装置接收数据和指令,并且将数据和指令传输至该存储系统、该至少一个输入装置、和该至少一个输出装置。
用于实施本申请的方法的程序代码可以采用一个或多个编程语言的任何组合来编写。这些程序代码可以提供给通用计算机、专用计算机或其他可编程数据处理装置的处理器或控制器,使得程序代码当由处理器或控制器执行时使流程图和/或框图中所规定的功能/操作被实施。程序代码可以完全在机器上执行、部分地在机器上执行,作为独立软件包部分地在机器上执行且部分地在远程机器上执行或完全在远程机器或电子设备上执行。
在本申请的上下文中,计算机可读存储介质可以是有形的介质,其可以包含或存储以供指令执行系统、装置或设备使用或与指令执行系统、装置或设备结合地使用的程序。计算机可读存储介质可以是机器可读信号介质或机器可读储存介质。计算机可读存储介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体系统、装置或设备,或者上述内容的任何合适组合。计算机可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编程只读存储器(EPROM或快闪存储器)、光纤、便捷式紧凑盘只读存储器(CD-ROM)、光学储存设备、磁储存设备、或上述内容的任何合适组合。
为了提供与用户的交互,可以在计算机上实施此处描述的系统和技术,该计算机具有:用于向用户显示信息的显示装置(例如,CRT(阴极射线管)或者LCD(液晶显示器)监视器);以及键盘和指向装置(例如,鼠标或者轨迹球),用户可以通过该键盘和该指向装置来将输入提供给计算机。其它种类的装置还可以用于提供与用户的交互;例如,提供给用户的反馈可以是任何形式的传感反馈(例如,视觉反馈、听觉反馈、或者触觉反馈);并且可以用任何形式(包括声输入、语音输入或者、触觉输入)来接收来自用户的输入。
可以将此处描述的系统和技术实施在包括后台部件的计算系统(例如,作为数据电子设备)、或者包括中间件部件的计算系统(例如,应用电子设备)、或者包括前端部件的计算系统(例如,具有图形用户界面或者网络浏览器的用户计算机,用户可以通过该图形用户界面或者该网络浏览器来与此处描述的系统和技术的实施方式交互)、或者包括这种后台部件、中间件部件、或者前端部件的任何组合的计算系统中。可以通过任何形式或者介质的数字数据通信(例如,通信网络)来将系统的部件相互连接。通信网络的示例包括:局域网(LAN)、广域网(WAN)和互联网。
应该理解,可以使用上面所示的各种形式的流程,重新排序、增加或删除步骤。例如,本申请公开中记载的各步骤可以并行地执行也可以顺序地执行也可以不同的次序执行,只要能够实现本申请公开的技术方案所期望的结果,本文在此不进行限制。
上述具体实施方式,并不构成对本申请保护范围的限制。本领域技术人员应该明白的是,根据设计要求和其他因素,可以进行各种修改、组合、子组合和替代。任何在本申请的原则之内所作的修改、等同替换和改进等,均应包含在本申请保护范围之内。
Claims (10)
1.一种激光雷达与惯性导航的外参标定方法,其特征在于,包括:
获取目标车辆上的激光雷达采集到的点云数据和惯性导航采集到的运动数据;
对所述点云数据和所述运动数据进行时间同步处理,并根据时间同步处理后的点云数据、时间同步处理后的运动数据和预设的手眼标定模型计算激光雷达与惯性导航的外参初始矩阵;
利用扩展卡尔曼滤波算法对所述外参初始矩阵进行调整,得到激光雷达与惯性导航的外参标定结果。
2.根据权利要求1所述的方法,其特征在于,所述根据时间同步处理后的点云数据、时间同步处理后的运动数据和预设的手眼标定模型计算激光雷达与惯性导航的外参初始矩阵,包括:
根据时间同步处理后的点云数据构建激光雷达里程计,根据时间同步处理后的运动数据构建惯性导航里程计;
根据所述激光雷达里程计和所述惯性导航里程计,利用预设的手眼标定模型计算激光雷达与惯性导航的外参初始矩阵。
3.根据权利要求2所述的方法,其特征在于,根据时间同步处理后的点云数据构建激光雷达里程计,包括:
对所述时间同步处理后的点云数据进行预处理,得到预处理后的点云数据;
从所述预处理后的点云数据中提取出各数据帧的线面特征;
从所有所述数据帧中提取出激光雷达关键帧,并根据其余数据帧的线面特征构建局部地图;其中,所述其余数据帧与所述激光雷达关键帧之间的距离满足预设条件;
通过滑动窗口法将所述激光雷达关键帧与所述局部地图进行匹配,并在匹配结果为成功的情况下,获得激光雷达里程计。
4.根据权利要求2所述的方法,其特征在于,所述根据时间同步处理后的运动数据构建惯性导航里程计,包括:
根据时间同步处理后的运动数据,利用预设的IMU运动学模型进行IMU位姿解算,得到位姿解算结果;其中,IMU位姿解算包括:姿态解算、速度解算和位置解算;
根据所述位姿解算结果,确定惯性导航里程计。
5.根据权利要求1所述的方法,其特征在于,所述利用扩展卡尔曼滤波算法对所述外参初始矩阵进行调整,得到激光雷达与惯性导航的外参标定结果,包括:
根据时间同步处理后的运动数据确定惯性导航的状态量,根据时间同步处理后的点云数据确定相邻两个激光雷达关键帧之间的位姿估计;
根据作为观测值的相邻两个激光雷达关键帧之间的位姿估计,作为预测值的所述惯性导航的状态量,利用所述扩展卡尔曼滤波算法计算所述预测值和所述观测值之间的差值,根据所述差值计算残差,并根据所述残差更新外参初始矩阵,得到所述外参标定结果。
6.根据权利要求5所述的方法,其特征在于,所述根据时间同步处理后的运动数据确定惯性导航的状态量,包括:
对时间同步处理后的运动数据进行预积分处理,得到预积分结果;
根据所述预积分结果进行状态估计,获得惯性导航的状态量。
7.根据权利要求6所述的方法,其特征在于,所述根据时间同步处理后的点云数据确定相邻两个激光雷达关键帧之间的位姿估计,包括:
根据所述预积分结果,对所述时间同步处理后的点云数据进行去畸变处理,得到去畸变点云;
对所述去畸变点云执行匹配,得到相邻两个激光雷达关键帧之间的位姿估计。
8.一种激光雷达与惯性导航的外参标定装置,其特征在于,包括:
获取模块,用于获取目标车辆上的激光雷达采集到的点云数据和惯性导航采集到的运动数据;
处理计算模块,用于对所述点云数据和所述运动数据进行时间同步处理,并根据时间同步处理后的点云数据、时间同步处理后的运动数据和预设的手眼标定模型计算激光雷达与惯性导航的外参初始矩阵;
调整模块,用于利用扩展卡尔曼滤波算法对所述外参初始矩阵进行调整,得到激光雷达与惯性导航的外参标定结果。
9.一种车辆控制设备,其特征在于,包括:至少一个处理器和存储器;
所述存储器存储计算机执行指令;
所述至少一个处理器执行所述存储器存储的计算机执行指令,使得所述至少一个处理器执行如权利要求1至7任一项所述的激光雷达与惯性导航的外参标定方法。
10.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质中存储有计算机执行指令,所述计算机执行指令被处理器执行时用于实现如权利要求1至7任一项所述的激光雷达与惯性导航的外参标定方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311381175.7A CN117388830A (zh) | 2023-10-23 | 2023-10-23 | 激光雷达与惯性导航的外参标定方法、装置、设备及介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311381175.7A CN117388830A (zh) | 2023-10-23 | 2023-10-23 | 激光雷达与惯性导航的外参标定方法、装置、设备及介质 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN117388830A true CN117388830A (zh) | 2024-01-12 |
Family
ID=89436988
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202311381175.7A Pending CN117388830A (zh) | 2023-10-23 | 2023-10-23 | 激光雷达与惯性导航的外参标定方法、装置、设备及介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN117388830A (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117706530A (zh) * | 2024-02-05 | 2024-03-15 | 中国科学院自动化研究所 | 一种用于实现多激光雷达与组合导航标定的方法及系统 |
CN117706530B (zh) * | 2024-02-05 | 2024-05-14 | 中国科学院自动化研究所 | 一种用于实现多激光雷达与组合导航标定的方法及系统 |
-
2023
- 2023-10-23 CN CN202311381175.7A patent/CN117388830A/zh active Pending
Cited By (2)
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 |
---|---|---|
CN110243358B (zh) | 多源融合的无人车室内外定位方法及系统 | |
CN107516326B (zh) | 融合单目视觉和编码器信息的机器人定位方法和系统 | |
JP4984659B2 (ja) | 自車両位置推定装置 | |
CN114526745B (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
CN110187375A (zh) | 一种基于slam定位结果提高定位精度的方法及装置 | |
CN113358112B (zh) | 一种地图构建方法及一种激光惯性里程计 | |
CN114323033B (zh) | 基于车道线和特征点的定位方法、设备及自动驾驶车辆 | |
CN113933818A (zh) | 激光雷达外参的标定的方法、设备、存储介质及程序产品 | |
CN114413887B (zh) | 一种传感器外部参数标定方法、设备及介质 | |
CN112254729A (zh) | 一种基于多传感器融合的移动机器人定位方法 | |
CN113763549B (zh) | 融合激光雷达和imu的同时定位建图方法、装置和存储介质 | |
CN114019552A (zh) | 一种基于贝叶斯多传感器误差约束的定位置信度优化方法 | |
CN115451948A (zh) | 一种基于多传感器融合的农业无人车定位里程计方法及系统 | |
CN115685292B (zh) | 一种多源融合导航系统的导航方法和装置 | |
CN116452763A (zh) | 一种基于误差卡尔曼滤波和因子图的三维点云地图构建方法 | |
CN115290071A (zh) | 相对定位融合方法、装置、设备及存储介质 | |
CN115218906A (zh) | 面向室内slam的视觉惯性融合定位方法及系统 | |
CN113959437A (zh) | 一种用于移动测量设备的测量方法及系统 | |
CN112284381B (zh) | 视觉惯性实时初始化对准方法及系统 | |
WO2024041347A1 (zh) | 机器人以及基于多传感器的三维建图方法、存储介质 | |
CN115900697B (zh) | 对象运动轨迹信息处理方法、电子设备以及自动驾驶车辆 | |
CN109655057B (zh) | 一种六推无人机加速器测量值的滤波优化方法及其系统 | |
CN113495281B (zh) | 可移动平台的实时定位方法及装置 | |
CN117388830A (zh) | 激光雷达与惯性导航的外参标定方法、装置、设备及介质 | |
CN114910062A (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 |