CN113947639B - 基于多雷达点云线特征的自适应在线估计标定系统及方法 - Google Patents

基于多雷达点云线特征的自适应在线估计标定系统及方法 Download PDF

Info

Publication number
CN113947639B
CN113947639B CN202111256788.9A CN202111256788A CN113947639B CN 113947639 B CN113947639 B CN 113947639B CN 202111256788 A CN202111256788 A CN 202111256788A CN 113947639 B CN113947639 B CN 113947639B
Authority
CN
China
Prior art keywords
point cloud
line
estimation
points
radar
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.)
Active
Application number
CN202111256788.9A
Other languages
English (en)
Other versions
CN113947639A (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 Sinian Zhijia Technology Co ltd
Original Assignee
Beijing Sinian Zhijia 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 Beijing Sinian Zhijia Technology Co ltd filed Critical Beijing Sinian Zhijia Technology Co ltd
Priority to CN202111256788.9A priority Critical patent/CN113947639B/zh
Publication of CN113947639A publication Critical patent/CN113947639A/zh
Application granted granted Critical
Publication of CN113947639B publication Critical patent/CN113947639B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T3/00—Geometric image transformations in the plane of the image
    • G06T3/08—Projecting images onto non-planar surfaces, e.g. geodetic screens
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/10—Segmentation; Edge detection
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10032—Satellite or aerial image; Remote sensing
    • G06T2207/10044—Radar image
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Radar Systems Or Details Thereof (AREA)
  • Navigation (AREA)

Abstract

本申请公开了一种基于多雷达点云线特征的自适应在线估计标定系统及方法，用于对无人集装箱卡车车身安装的多雷达进行在线外参估计和自动标定，从而消除或降低由于传感器安装偏移导致的外参误差，保障融合点云能够投影到统一的车体坐标，为后续感知任务提供精准的原始输出信息。同时，针对无人集卡所在的港口场景存在大量集装箱体，设计了基于点云体素分割和平面拟合的线特征抽取算法，以及对应的基于点云线特征进行同步定位与建图的SLAM算法用于估计车辆里程变化。最终，利用SLAM技术获取准确的里程变化信息，并反向优化雷达外参信息和更新自适应卡尔曼滤波器增益(也就是噪声估计)，从而实现调控闭环。

Description

基于多雷达点云线特征的自适应在线估计标定系统及方法
技术领域
本申请涉及传感器、参数估计和自动驾驶感知技术领域，尤其涉及一种基于多雷达点云线特征的自适应在线估计标定系统及方法。
背景技术
无人机装箱卡车体型巨大，包括车头、拖挂和箱体三部分组成，针对集装箱卡车的感知系统需要综合考虑车身总体范围内的视野尽可能大和死角尽可能小，从而保障自动化作业过程中的安全性。通常情况下需要利用多颗激光雷达来实现对车身周边的全覆盖感知，特别是针对拖挂的箱体位置检测；因此，绝大多数无人集装箱卡车装配了多颗激光雷达。
将雷达标定到以车体为中心的车体坐标系，并与其它传感器协同是车辆装配前的必要步骤之一，通常情况下可通过眼手标定估计每个雷达的轨迹，然后进行轨迹之间的配准获得外参；或基于各雷达的重叠视场进行基于标定参考物的关键点同名点检测，进行两两雷达之间的标定。上述方法主要是基于离线标定设计，需要在车辆出厂前完成。为了保障无人集卡的感知视野，通常的雷达安装方案兼顾车身两侧和车顶，传感器之间间距较大。而无人集卡车头由多块金属拼接，属于非刚体结构；通过支架装配的多个雷达之间也非刚体连接，进一步使得雷达之间的稳定性降低。当车辆行驶过程发生震动，所标定的外参无法保障雷达点云能够投影到统一的坐标系，不同雷达点云形成的重影，从而影响后续的目标感知和SLAM定位等下游任务。
此外，无人集卡作业过程需要对箱体，箱体质量大、冲击性强，起放震动对车身零部件影响大。在车辆长时间作业后传感器容易出现轻微的安装偏移，从而影响外参数据，导致投影错误。如当不同雷达投影存在误差，地面车道线信息将出现多条、而路面以上的动态目标则出现重影，对应的检测结果则产生偏差。如果不及时发现，也将影响后续感知任务的作业。而针对传感器长时间工作偏移带来的误差也需要通过停止作业，进行离线标定完成，从而影响总体工作效率。
发明内容
为了解决上述技术问题，本申请实施例提供一种基于多雷达点云线特征的自适应在线估计标定系统及方法，用于对无人集装箱卡车车身安装的多雷达进行在线外参估计和自动标定，从而消除或降低由于传感器安装偏移导致的外参误差，保障融合点云能够投影到统一的车体坐标，为后续感知任务提供精准的原始输出信息。
本申请实施例第一方面提供了一种基于多雷达点云线特征的自适应在线估计标定系统，包括：
外参标定模块，获取所有雷达到车体中心的距离偏移和旋转偏移量形成外参矩阵，基于初始参数对雷点点云进行外参估计；
位姿估计模块，针对车身中心点的运动状态进行卡尔曼滤波，基于上一个时间车身状态和间隔时间，估计车身的最新运动状态；
特征抽取模块，对选择区域内的点云采用体素化分割后获取每个本体素内的所有平面，抽取相邻平面之间的相交线附近的点作为线特征的点；
位姿估计模块，基于局部地图的线特征的点和当前雷达帧获取的线特征点配对并形成多组估算结果后，将多组估算结果形成的最终估算结果反馈至全局地图，并更新全局地图和估算里程计信息。
进一步地，所述外参标定模块中，将基于初始点云和初始获得的外参矩阵作为计算种子，通过手工测量所安装的多颗雷达到车体中心的距离偏移和旋转偏移，采用NDT或ICP算法对雷达点云进行外参估计。
进一步地，所述位姿估计模块采用自适应卡尔曼滤波算法对车身位置进行估计，针对多雷达系统构建多个噪声估计，并通过结构化参数系统和自适应估计算法进行自动切换，获得车辆运动位姿信息变化后调整所获得的雷达点云在车体坐标系的位姿。
进一步地，所述特征抽取模块包括：
点云分割模块：基于点云进行体素化分割，将整个场景内所选取的区域内的点云进行均匀划分；
平面获取模块：基于每个体素内的点云进行随机采用一直化的迭代平面抽取算法，获取本体素内的所有平面以及每个三维空间的平面公式；
特征抽取模块：针对相邻的平面，基于平面的夹角进行过滤，保留平面夹角在预设范围内的平面对，并求取对应的相交线；将相交线附近预设范围内的点作为线特征的点，并基于法向进行排序和编号，用于后续配准使用。
进一步地，所述平面获取模块中对每个平面的点数量进行过滤，对于所包含的点数量不超过预设下限值数量的平面进行剔除。
进一步地，所述位姿估计模块；基于线特征的SLAM建图与里程估计模块，包括：
建图模块，构建一个基于上一帧里程信息所在位置的局部地图，该局部地图的范围小于全局地图范围、大于单帧点云数据；
配对模块，获取基于点云线特征抽取模块在当前局部地图中的线特征以及基于法向排序和编号的标签，将线特征的点使用近邻搜索寻找局部地图中的对应特征点，基于法向约束进行线特征的配对。
估算模块，基于每对由局部地图获取的线特征点和由当前雷达帧获取的线特征点采用ICP算法获得多组估算结果，并剔除偏差较大的外参估计结果，基于剩余配准外参数据求取均值作为最终结果；
更新模块，将当前帧的最终结果投影到全局地图中，更新全局地图和估算里程计信息；利用新估算的里程计信息更新自适应卡尔曼滤波算法的增益，并较准和更新传感器外参。
本申请实施例第二方面提供了一种基于多雷达点云线特征的自适应在线估计标定方法，包括：
S100:获取所有雷达到车体中心的距离偏移和旋转偏移量形成外参矩阵，基于初始参数对雷点点云进行外参估计；
S200：针对车身中心点的运动状态进行卡尔曼滤波，基于上一个时间车身状态和间隔时间，估计车身的最新运动状态；
S300：对选择区域内的点云采用体素化分割后获取每个本体素内的所有平面，抽取相邻平面之间的相交线附近的点作为线特征的点；
S400：基于局部地图的线特征的点和当前雷达帧获取的线特征点配对并形成多组估算结果后，将多组估算结果形成的最终估算结果反馈至全局地图，并更新全局地图和估算里程计信息。
进一步地，所述步骤S200采用自适应卡尔曼滤波算法对车身位置进行估计，针对多雷达系统构建多个噪声估计，并通过结构化参数系统和自适应估计算法进行自动切换，获得车辆运动位姿信息变化后调整所获得的雷达点云在车体坐标系的位姿。
进一步地，所述步骤S300包括：
301)基于点云进行体素化分割，将整个场景内所选取的区域内的点云进行均匀划分；
302)基于每个体素内的点云进行随机采用一直化的迭代平面抽取算法，获取本体素内的所有平面以及每个三维空间的平面公式；
303)针对相邻的平面，基于平面的夹角进行过滤，保留平面夹角在预设范围内的平面对，并求取对应的相交线；将相交线附近预设范围内的点作为线特征的点，并基于法向进行排序和编号，用于后续配准使用。
进一步地，所述步骤S400包括：
401)构建一个基于上一帧里程信息所在位置的局部地图，该局部地图的范围小于全局地图范围、大于单帧点云数据；
402)获取基于点云线特征抽取模块在当前局部地图中的线特征以及基于法向排序和编号的标签，将线特征的点使用近邻搜索寻找局部地图中的对应特征点，基于法向约束进行线特征的配对。
403)基于每对由局部地图获取的线特征点和由当前雷达帧获取的线特征点采用ICP算法获得多组估算结果，并剔除偏差较大的外参估计结果，基于剩余配准外参数据求取均值作为最终结果；
404)将当前帧的最终结果投影到全局地图中，更新全局地图和估算里程计信息；利用新估算的里程计信息更新自适应卡尔曼滤波算法的增益，并较准和更新传感器外参。
本发明的优点在于：
(1)针对多雷达环境观测系统可能导致噪声分布存在差异的情况，使用自适应卡尔曼滤波算法来估计车体运动信息，能够进一步提升卡尔曼滤波对噪声的估计精度，同时提供更加稳定的车体运动位姿信息；
(2)基于港口无人集卡所在的场景，提出了基于体素分割的并行平面拟合，以及基于平面相交的点云线特征抽取算法，能够稳定的描述港口场景的点云特征，同时考虑了算法的并行化，提升算法效率；
(3)基于点云线特征SLAM建图与定位算法能够基于数据量较少的点云线特征进行全局建图与里程估计，同时，基于法向约束的配准策略和全局最优化方法进一步提升帧间点云的配准效率和精度；
(4)所设计的在线配准系统无需特殊的参考目标，即可通过一定时间的在线运行和SLAM算法完成对雷达外参的在线标定；同时，算法可与SLAM建图与定位系统无缝衔接，提供车辆的实时定位信息。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为多雷达车体安装示意图；
图2为系统全局流程图；
图3为点云线特征抽取示意图；
图4为三个雷达点云单独显示示意图；
图5为手工测量三个雷达外参偏移的示意图；
图6为直接使用ICP进行粗外参估计投影结果示意图；
图7为随着算法迭代估计外参的点云误差变化示意图；
图8为最终配准结果示意图。
具体实施方式
为使得本申请的申请目的、特征、优点能够更加的明显和易懂，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，下面所描述的实施例仅仅是本申请一部分实施例，而非全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本申请保护的范围。
下面结合附图和具体实施例，进一步阐明本发明。
在本申请的描述中，需要理解的是，术语“上”、“下”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本申请和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本申请的限制。
针对现有技术的问题，本申请提出的一种基于多雷达点云线特征的自适应在线估计标定系统及方法，用于对无人集装箱卡车车身安装的多雷达进行在线外参估计和自动标定，从而消除或降低由于传感器安装偏移导致的外参误差，保障融合点云能够投影到统一的车体坐标，为后续感知任务提供精准的原始输出信息。同时，针对无人集卡所在的港口场景存在大量集装箱体，设计了基于点云体素分割和平面拟合的线特征抽取算法，以及对应的基于点云线特征进行同步定位与建图的SLAM算法用于估计车辆里程变化。最终，利用SLAM技术获取准确的里程变化信息，并反向优化雷达外参信息和更新自适应卡尔曼滤波器增益(也就是噪声估计)，从而实现调控闭环。
本发明提出的在线标定系统具体包括：
外参标定模块，获取所有雷达到车体中心的距离偏移和旋转偏移量形成外参矩阵，基于初始参数对雷点点云进行外参估计。
具体地：外参标定模块通过手工测量所安装的多颗雷达到车体中心的距离偏移t和旋转偏移r，然后基于初始点云和手工测量的外参矩阵Tei＝[r，t]作为计算种子，其中i代表对应的雷达编号，使用NDT或ICP算法对雷达点云进行外参估计。但由于雷达安装为了保障车辆周围的视场(Field of View，FOV)的覆盖，因此，雷达之间的重叠FOV通常较小，仅能获得相对准确的粗外参信息。
位姿估计模块，针对车身中心点的运动状态进行卡尔曼滤波，基于上一个时间车身状态和间隔时间，估计车身的最新运动状态。
具体地：位姿估计模块针对车身中心点的运动状态进行卡尔曼滤波，基于上一个时间车身状态和间隔时间，估计车身的最新运动状态。由于所能够观测的车身运动主要来自安装在车体的多个雷达，而不同的雷达由于安装位置不同，运动过程中的FOV存在一定的差异。在进行后续建图和里程估计时，存在的噪声分布各不相同。传统的卡尔曼滤波和扩展卡尔曼滤波要求目标测量系统时线性的，且状态空间模型已知，噪声分布服从高斯模型钱噪声的协方差已知，在引入多雷达观测系统的情况下难以完全满足上述要求。因此，采用自适应卡尔曼滤波算法对车身位置进行估计，针对多雷达系统构建多个噪声估计，并通过结构化参数系统和自适应估计算法进行自动切换，从而提高对车体运动变化的推算精度，并基于所获得的车辆运动位姿Tm信息变化进一步调整所获得的雷达点云在车体坐标系的位姿。
特征抽取模块，对选择区域内的点云采用体素化分割后获取每个本体素内的所有平面，抽取相邻平面之间的相交线附近的点作为线特征的点。
具体地：特征抽取模块针对港口集装箱码头存在大量箱体和人工建筑，激光雷达点云具有大量显著的线特征。首先，基于点云进行体素化分割，将整个场景内感兴趣区域(Regionof Interest，ROI)内的点云进行均匀划分，方便并行化加速。然后基于每个体素内的点云进行随机采用一直化RANdom SAmple Consensus(RANSAC)的迭代平面抽取算法，获取本体素内的所有平面(且仅考虑点数量大于一定阈值的平面)，以及每个三维空间的平面公式aX+bY+cZ＝d，其中XYZ代表轴，abcd均为系数。然后对相邻的平面，基于平面的夹角进行过滤，保留平面夹角在一定范围内的平面对，并求取对应的相交线。最终，将相交线附近一定范围内的点作为线特征的点，并基于法向进行排序和编号，用于后续配准。
位姿估计模块，基于局部地图的线特征的点和当前雷达帧获取的线特征点配对并形成多组估算结果后，将多组估算结果形成的最终估算结果反馈至全局地图，并更新全局地图和估算里程计信息。
具体地：基于线特征的SLAM建图与里程估计模块，由于港口场景存在大量人工建筑和箱体，因此丰富的线特征不会导致特征丢失，且相对稳定；同时，基于线特征抽取的点能够降低计算成本。首先，建图过程维护一个基于上一帧里程信息所在位置的局部地图，该地图要远小于全局地图范围，但大于单帧点云数据。基于相同的线特征抽取流程，获得当前局部地图中的线特征，以及基于法向排序和编号的标签。由于车辆行驶过程种场景中的大部分物体不会发生改变，基于当前雷达帧的线特征点使用近邻搜索寻找局部地图中的对应特征点，进一步过滤配准点候选集合的规模，并基于法向约束进行线特征的配对。基于每对由局部地图获取的线特征点和由当前雷达帧获取的线特征点进行ICP算法。获得多组估算结果，并剔除偏差较大的外参估计结果，基于剩余配准外参数据求取均值作为最终结果。并将当前帧按估算结果投影到全局地图中，更新全局地图和估算里程计信息。
最后，利用新估算的里程计信息更新自适应卡尔曼滤波算法的增益，并较准和更新传感器外参。
相应地，本发明提供的在线标定系统，包括以下步骤：
步骤100)安装雷达到车体，并通过手工或CAD测量安装雷达到车体中心，一般指惯导安装位置的外参信息Tei＝[x，y，z，roll，pitch，yaw]；其中，xyz为第i个雷达到惯导中心(车体中心)在三维空间的偏移量，roll，pitch，yaw分别为雷达安装到车体坐标系三个坐标轴的偏差角。上述值可仅精确到分米和10度左右偏差。然后，采集雷达点云信息，首先将顶部雷达采用手眼标定方法标定到车体坐标系；接着，使用NDT或ICP将左右两个雷达标定到中间雷达上，标定时使用初始外参信息作为种子，并得到优化后的、各雷达到车体中心的外参Tei。当不存在惯导时，则以车顶雷达为车体坐标进行外参估计。
步骤200)启动车辆在厂区或测试区行驶，并采集雷达点云信息(j时刻i雷达的点云)。
将每帧点云基于自身外参投影到车体中心，获得车体坐标系下的粗雷达并基于该观测值和相邻时间，使用自适应卡尔曼滤波算法(AdaptiveKalman Filter，AKF)估计车体的运动位姿变化/>并估计噪声的协方差。其中，VPoint为直接观测信息，/>为基于后续SLAM里程估计得到的间接信息(实际值，需等待步骤400输出)，基于AKF估计最新/>为估计结果(当前步骤可获取)。并基于实际值预测误差噪声、更新增益方程和计算残差。
步骤300)基于当前帧点云(不考虑来自哪个雷达)在ROI范围内(如ROI＝[-30，70，-40，40，-5，5])进行体素空间分割，如20m，则获得以20米为边长的立方体点云空间。然后基于ransac进行平面拟合，获取每个体素内的平面，并基于平面夹角约束获得满足条件的相交直线，直线距离0.3m范围内的点，以及直线法向。得到当前帧的点云线特征。
步骤400)基于当前车辆所在估计位置，在SLAM所构建的全局点云地图中获取局部子图，选取范围为当前位置的前后100m，左右100m范围。采用步骤3中相同的方法获取局部子图的点云线特征集合。基于局部子图的点云线特征和当前雷达的点云线特征进行全局配准，遍历当前帧的点云线特征/>基于FLANN算法获取子图特征中距离较近的线特征点，见，并基于法向进行过滤，构建当前点云帧到局部地图的特征点集对获得潜在匹配的线特征点，采用ICP算法进行配准。
对于每组线特征，可以获得一组估计配准外参Tx，接着采用模糊估计的方法(如上式所示)，利用Ransac剔除所有Tx中的离群值，然后基于均值估计最终的外参矩阵Tf＝Mean(Ransac(T1，T2，...，Tx))。最终，基于Tf将当前帧点云投影到全局地图，更新全局地图信息，并估算里程变化。并将该结果返回给步骤200)，用于更新自适应卡尔曼滤波器的因子估计，以及修正对应雷达的外参。
作为一个具体的实施例，本发明主要的四个阶段工作流如图2所示，下面结合一个实施例具体说明。
步骤100)针对如图1所示的集卡车头支架安装了顶雷达、左雷达和右雷达，而由于安装位置和角度不同，各雷达FOV仅有部分重叠，三个雷达所获取的点云数据如图4所示。通过手工测量，将雷达标定到车头后轮中心位置，部分测量偏移结果如图5所示。首先基于ICP算法和手工测量值进行估算，得到一组雷达初始外参Tei＝[x，y，z，roll，pitch，yaw]，其投影结果如图6所示。
步骤200)驾驶车辆在线特征丰富的场景行驶，并启动自适应卡尔曼滤波算法估计车辆运动状态。考虑雷达未进行同步，系统收到的最近一帧点云为t时刻i号雷达的点云，则投影到车体坐标下的点云为/>基于上一帧t-1时刻和当前帧t时刻的点云SLAM里程估计结果推算车体中心的运动位姿变化，包括平移t＝[x，y，z]T和旋转r＝[roll，pitch，yaw]T共计6个自由度在SE(3)李群代数下的变化估计车体运动的位姿变化/>用于计算最新的点云/>
步骤300)针对当前点云帧基于ROI过滤，并对ROI范围内的点云进行体素分割，接着基于ransac对体素内点云进行平面拟合，获得满足平面内点大于200的若干点云平面aiX+biY+ciZ+di＝0，以及平面法向normi＝[a/d，b/d，c/d]。基于平面相交公式(如下)(排除法向夹角不在[30，150]度的平面对)，求取相交直线，以及到直线距离小于0.2m的点作为线特征集合线特征抽取效果如3所示。
步骤400)，基于当前步骤200的车体粗略位姿，获取SLAM局部地图，若初始情况，则原地停留20秒积累点云。采用步骤300相同算法获取局部地图的线特征集合基于步骤3所得的线特征集合/>基于FLann在/>中进行近邻搜索，并通过法向约束进行线特征集配对，够成多组特征集对/>针对没对数据采用ICP进行配置估计，其中来自局部地图的点作为基底(Base)，求取将当前帧点云投影到基底的旋转矩阵Tx。
对于每组线特征，可以获得一组估计配准外参Tx，接着采用模糊估计的方法，利用Ransac剔除所有Tx中的离群值，然后基于均值估计最终的外参矩阵Tf＝Mean(Ransac(T1，T2，...，Tx))。最终，基于Tf将当前帧点云投影到全局地图，更新全局地图信息，并估算里程变化。并将该结果返回给步骤200，用于更新自适应卡尔曼滤波器的因子估计，以及修正对应雷达的外参。
通过多轮次迭代，每个雷达基于外参投影到车体的平均旋转误差(Root MeanSquare Error，RMSE)如图7所示会逐渐收敛。最终，基于所估计的外参投影得到的结果如图8所示，能够消除对环境感知的重影。
以上详细描述了本发明的优选实施方式，但是本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种等同变换(如数量、形状、位置等)，这些等同变换均属于本发明的保护。

Claims (6)

1.一种基于多雷达点云线特征的自适应在线估计标定系统，其特征在于：包括
外参标定模块，获取所有雷达到车体中心的距离偏移和旋转偏移量形成外参矩阵，基于初始参数对雷达点云进行外参估计；
第一位姿估计模块，针对车身中心点的运动状态进行卡尔曼滤波，基于上一个时间车身状态和间隔时间，估计车身的最新运动状态；
特征抽取模块，对选择区域内的点云采用体素化分割后获取每个本体素内的所有平面，抽取相邻平面之间的相交线附近的点作为线特征的点；
第二位姿估计模块，基于局部地图的线特征的点和当前雷达帧获取的线特征点配对并形成多组估算结果后，将多组估算结果形成的最终估算结果反馈至全局地图，并更新全局地图和估算里程计信息；
所述特征抽取模块包括：
点云分割模块：基于点云进行体素化分割，将整个场景内所选取的区域内的点云进行均匀划分；
平面获取模块：基于每个体素内的点云进行随机采用一致化的迭代平面抽取算法，获取本体素内的所有平面以及每个三维空间的平面公式；
特征抽取模块：针对相邻的平面，基于平面的夹角进行过滤，保留平面夹角在预设范围内的平面对，并求取对应的相交线；将相交线附近预设范围内的点作为线特征的点，并基于法向进行排序和编号，用于后续配准使用；
所述第二位姿估计模块包括：
建图模块，构建一个基于上一帧里程信息所在位置的局部地图，该局部地图的范围小于全局地图范围、大于单帧点云数据；
配对模块，获取基于点云线特征抽取模块在当前局部地图中的线特征以及基于法向排序和编号的标签，将线特征的点使用近邻搜索寻找局部地图中的对应特征点，基于法向约束进行线特征的配对；
估算模块，基于每对由局部地图获取的线特征点和由当前雷达帧获取的线特征点采用ICP算法获得多组估算结果，并剔除偏差大于预设值的外参估计结果，基于剩余配准外参数据求取均值作为最终结果；
更新模块，将当前帧的最终结果投影到全局地图中，更新全局地图和估算里程计信息；利用新估算的里程计信息更新自适应卡尔曼滤波算法的增益，并较准和更新传感器外参。
2.根据权利要求1所述的基于多雷达点云线特征的自适应在线估计标定系统，其特征在于，
所述外参标定模块中，将基于初始点云和初始获得的外参矩阵作为计算种子，通过手工测量所安装的多颗雷达到车体中心的距离偏移和旋转偏移，采用NDT或ICP算法对雷达点云进行外参估计。
3.根据权利要求2所述的基于多雷达点云线特征的自适应在线估计标定系统，其特征在于，
所述第一位姿估计模块采用自适应卡尔曼滤波算法对车身位置进行估计，针对多雷达系统构建多个噪声估计，并通过结构化参数系统和自适应估计算法进行自动切换，获得车辆运动位姿信息变化后调整所获得的雷达点云在车体坐标系的位姿。
4.根据权利要求1所述的基于多雷达点云线特征的自适应在线估计标定系统，其特征在于，
所述平面获取模块中对每个平面的点数量进行过滤，对于所包含的点数量不超过预设下限值数量的平面进行剔除。
5.一种基于多雷达点云线特征的自适应在线估计标定方法，其特征在于：包括
S100:获取所有雷达到车体中心的距离偏移和旋转偏移量形成外参矩阵，基于初始参数对雷达点云进行外参估计；
S200：针对车身中心点的运动状态进行卡尔曼滤波，基于上一个时间车身状态和间隔时间，估计车身的最新运动状态；
S300：对选择区域内的点云采用体素化分割后获取每个本体素内的所有平面，抽取相邻平面之间的相交线附近的点作为线特征的点；
S400：基于局部地图的线特征的点和当前雷达帧获取的线特征点配对并形成多组估算结果后，将多组估算结果形成的最终估算结果反馈至全局地图，并更新全局地图和估算里程计信息；
所述步骤S300包括：
301)基于点云进行体素化分割，将整个场景内所选取的区域内的点云进行均匀划分；
302)基于每个体素内的点云进行随机采用一致化的迭代平面抽取算法，获取本体素内的所有平面以及每个三维空间的平面公式；
303)针对相邻的平面，基于平面的夹角进行过滤，保留平面夹角在预设范围内的平面对，并求取对应的相交线；将相交线附近预设范围内的点作为线特征的点，并基于法向进行排序和编号，用于后续配准使用；
所述步骤S400包括：
401)构建一个基于上一帧里程信息所在位置的局部地图，该局部地图的范围小于全局地图范围、大于单帧点云数据；
402)获取基于点云线特征抽取模块在当前局部地图中的线特征以及基于法向排序和编号的标签，将线特征的点使用近邻搜索寻找局部地图中的对应特征点，基于法向约束进行线特征的配对；
403)基于每对由局部地图获取的线特征点和由当前雷达帧获取的线特征点采用ICP算法获得多组估算结果，并剔除偏差大于预设值的外参估计结果，基于剩余配准外参数据求取均值作为最终结果；
404)将当前帧的最终结果投影到全局地图中，更新全局地图和估算里程计信息；利用新估算的里程计信息更新自适应卡尔曼滤波算法的增益，并较准和更新传感器外参。
6.根据权利要求5所述的基于多雷达点云线特征的自适应在线估计标定方法，其特征在于，所述步骤S200采用自适应卡尔曼滤波算法对车身位置进行估计，针对多雷达系统构建多个噪声估计，并通过结构化参数系统和自适应估计算法进行自动切换，获得车辆运动位姿信息变化后调整所获得的雷达点云在车体坐标系的位姿。
CN202111256788.9A 2021-10-27 2021-10-27 基于多雷达点云线特征的自适应在线估计标定系统及方法 Active CN113947639B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111256788.9A CN113947639B (zh) 2021-10-27 2021-10-27 基于多雷达点云线特征的自适应在线估计标定系统及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111256788.9A CN113947639B (zh) 2021-10-27 2021-10-27 基于多雷达点云线特征的自适应在线估计标定系统及方法

Publications (2)

Publication Number Publication Date
CN113947639A CN113947639A (zh) 2022-01-18
CN113947639B true CN113947639B (zh) 2023-08-18

Family

ID=79332745

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111256788.9A Active CN113947639B (zh) 2021-10-27 2021-10-27 基于多雷达点云线特征的自适应在线估计标定系统及方法

Country Status (1)

Country Link
CN (1) CN113947639B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114565681B (zh) * 2022-03-01 2022-11-22 禾多科技（北京）有限公司 一种相机标定方法、装置、设备、介质及产品
CN116022159B (zh) * 2023-03-31 2023-06-13 禾多科技（北京）有限公司 车辆移动控制方法、装置、电子设备和计算机可读介质
CN117092624A (zh) * 2023-09-27 2023-11-21 山东大学 一种外参标定方法、系统、介质及设备
CN117367412B (zh) * 2023-12-07 2024-03-29 南开大学 一种融合捆集调整的紧耦合激光惯导里程计与建图方法

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109100741A (zh) * 2018-06-11 2018-12-28 长安大学 一种基于3d激光雷达及图像数据的目标检测方法
CN110596683A (zh) * 2019-10-25 2019-12-20 中山大学 一种多组激光雷达外参标定系统及其方法
CN111709981A (zh) * 2020-06-22 2020-09-25 高小翎 特征线融合的激光点云与模拟图像的配准方法
CN112132857A (zh) * 2020-09-18 2020-12-25 福州大学 动态环境混合视觉系统的动态物检测和静态地图重建方法
CN112258600A (zh) * 2020-10-19 2021-01-22 浙江大学 一种基于视觉与激光雷达的同时定位与地图构建方法
CN112484725A (zh) * 2020-11-23 2021-03-12 吉林大学 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法
US11002859B1 (en) * 2020-02-27 2021-05-11 Tsinghua University Intelligent vehicle positioning method based on feature point calibration
CN112837383A (zh) * 2021-03-01 2021-05-25 东南大学 相机与激光雷达重标定方法、装置及计算机可读存储介质
CN113345018A (zh) * 2021-05-31 2021-09-03 湖南大学 一种动态场景下的激光单目视觉融合定位建图方法

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109100741A (zh) * 2018-06-11 2018-12-28 长安大学 一种基于3d激光雷达及图像数据的目标检测方法
CN110596683A (zh) * 2019-10-25 2019-12-20 中山大学 一种多组激光雷达外参标定系统及其方法
US11002859B1 (en) * 2020-02-27 2021-05-11 Tsinghua University Intelligent vehicle positioning method based on feature point calibration
CN111709981A (zh) * 2020-06-22 2020-09-25 高小翎 特征线融合的激光点云与模拟图像的配准方法
CN112132857A (zh) * 2020-09-18 2020-12-25 福州大学 动态环境混合视觉系统的动态物检测和静态地图重建方法
CN112258600A (zh) * 2020-10-19 2021-01-22 浙江大学 一种基于视觉与激光雷达的同时定位与地图构建方法
CN112484725A (zh) * 2020-11-23 2021-03-12 吉林大学 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法
CN112837383A (zh) * 2021-03-01 2021-05-25 东南大学 相机与激光雷达重标定方法、装置及计算机可读存储介质
CN113345018A (zh) * 2021-05-31 2021-09-03 湖南大学 一种动态场景下的激光单目视觉融合定位建图方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于三维激光点云分割匹配的同步定位与构图算法研究;姬兴亮;《中国优秀硕士学位论文全文数据库 (信息科技辑)》;I138-892 *

Also Published As

Publication number Publication date
CN113947639A (zh) 2022-01-18

Similar Documents

Publication Publication Date Title
CN113947639B (zh) 基于多雷达点云线特征的自适应在线估计标定系统及方法
CN111337941B (zh) 一种基于稀疏激光雷达数据的动态障碍物追踪方法
CN110068836B (zh) 一种智能驾驶电动清扫车的激光雷达路沿感知系统
CN111583369B (zh) 一种基于面线角点特征提取的激光slam方法
CN113781582B (zh) 基于激光雷达和惯导联合标定的同步定位与地图创建方法
CN111457902B (zh) 基于激光slam定位的水域测量方法及系统
CN113870343B (zh) 相对位姿标定方法、装置、计算机设备和存储介质
CN111656136A (zh) 使用激光雷达的车辆定位系统
CN110859044A (zh) 自然场景中的集成传感器校准
CN112987065B (zh) 一种融合多传感器的手持式slam装置及其控制方法
CN110764110B (zh) 路径导航方法、装置及计算机可读存储介质
JP2022140374A (ja) 姿勢推定方法及び装置、並びに関連デバイス及び記憶媒体
EP4124829A1 (en) Map construction method, apparatus, device and storage medium
CN113903011B (zh) 一种适用于室内停车场的语义地图构建及定位方法
CN113819905A (zh) 一种基于多传感器融合的里程计方法及装置
US11677931B2 (en) Automated real-time calibration
CN114325634A (zh) 一种基于激光雷达的高鲁棒性野外环境下可通行区域提取方法
CN115908539A (zh) 一种目标体积自动测量方法和装置、存储介质
CN111829514A (zh) 一种适用于车辆底盘集成控制的路面工况预瞄方法
CN115792894A (zh) 一种多传感器融合车辆目标跟踪方法、系统及存储介质
Wu et al. AFLI-Calib: Robust LiDAR-IMU extrinsic self-calibration based on adaptive frame length LiDAR odometry
CN116520295A (zh) 一种基于自动建图的多激光雷达标定方法
CN115546303A (zh) 室内停车场的定位方法、装置、车辆及存储介质
CN111736137B (zh) LiDAR外部参数标定方法、系统、计算机设备及可读存储介质
CN117635721A (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
GR01 Patent grant
GR01 Patent grant