CN116721337A - 无人驾驶场景下基于动态车辆检测的点云畸变矫正方法 - Google Patents

无人驾驶场景下基于动态车辆检测的点云畸变矫正方法 Download PDF

Info

Publication number
CN116721337A
CN116721337A CN202310567009.XA CN202310567009A CN116721337A CN 116721337 A CN116721337 A CN 116721337A CN 202310567009 A CN202310567009 A CN 202310567009A CN 116721337 A CN116721337 A CN 116721337A
Authority
CN
China
Prior art keywords
point cloud
vehicle
point
distortion
data
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
CN202310567009.XA
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.)
Tongji University
Original Assignee
Tongji University
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 Tongji University filed Critical Tongji University
Priority to CN202310567009.XA priority Critical patent/CN116721337A/zh
Publication of CN116721337A publication Critical patent/CN116721337A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/98—Detection or correction of errors, e.g. by rescanning the pattern or by human intervention; Evaluation of the quality of the acquired patterns
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/20—Image preprocessing
    • G06V10/24—Aligning, centring, orientation detection or correction of the image
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/20—Image preprocessing
    • G06V10/26—Segmentation of patterns in the image field; Cutting or merging of image elements to establish the pattern region, e.g. clustering-based techniques; Detection of occlusion
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/50—Context or environment of the image
    • G06V20/56—Context or environment of the image exterior to a vehicle by using sensors mounted on the vehicle
    • G06V20/58—Recognition of moving objects or obstacles, e.g. vehicles or pedestrians; Recognition of traffic objects, e.g. traffic signs, traffic lights or roads
    • 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)
  • Multimedia (AREA)
  • Theoretical Computer Science (AREA)
  • Quality & Reliability (AREA)
  • Radar Systems Or Details Thereof (AREA)

Abstract

本发明属于无人驾驶领域，提出了无人驾驶场景下基于动态车辆检测的点云畸变矫正方法，包括：步骤1.点云畸变分析与数据生成；步骤2.基于激光雷达运动解析的点云全局畸变矫正；步骤3.基于车辆运动解析的点云局部畸变矫正；步骤4.无人驾驶场景下点云畸变矫正。本发明方法为无人驾驶三维重建、三维定位提升了准确度，有效克服了无人驾驶运动行为智能化的安全隐患。

Description

无人驾驶场景下基于动态车辆检测的点云畸变矫正方法
技术领域
本发明涉及无人驾驶领域，具体涉及无人驾驶场景下基于动态车辆检测的点云畸变矫正方法。
背景技术
在无人驾驶场景中，车载激光雷达的运动及场景中物体的运动都会使激光雷达采集的点云产生畸变，会对点云配准、三维定位、三维重建等产生干扰，研究如何有效的消除点云畸变具有重大意义。目前的点云畸变矫正算法主要分成先处理算法和后处理算法两种。
先处理算法为了获得更好的实时性，在点云采集过程中利用先验知识处理点云畸变：通过分段配准算法来尽量降低点云畸变对点云配准的影响，将一帧点云拆分成多帧点云子集来细化点云数据，然后将拆分出的点云子集分别与全局地图进行匹配，该算法变相提高了点云的采样频率，以此来减少点云畸变对三维重建的影响，尽管该算法实现比较简单，但在配准过程中取得了不错的效果；通过线性插值方法来消除点云畸变对三维重建的影响，线性插值算法首先获取前后两帧数据整体的旋转及平移变化，然后根据时间等比例给矫正不同点云数据点畸变。先处理算法在点云配准过程中没有考虑激光雷达运动状态的变化，导致误差不断累计。
后处理方法相较于先处理的方法往往具有更好的效果，但是实时性较差。通过上采样预积分方法处理IMU数据来解析物体的运动，该方法无需对点云采集系统的运动做出提前假设，而是在采集到点云后通过解析IMU采集到的加速度数据得到系统的运动特征，从而处理点云的运动失真问题；通过使用异方差界标估计双目立体摄像机采集数据偏差的方法；通过神经网络减少视图模型的漂移问题；通过在Slam系统中加入高斯处理矫正里程计的运动误差。
综上所述，虽然点云畸变矫正领域已有大量研究及方法，但是考虑到无人驾驶场景复杂的行车环境，仍然需要考虑如下问题：
(1)未考虑复杂的车辆运动：现有的方法仅仅考虑解析激光雷达的运动矫正点云畸变，未考虑场景中复杂的车辆运动。同时，场景中各车辆运动具有差异性，需要分别处理不同车辆的运动。
(2)未考虑点云局部畸变：现有的点云畸变矫正方法仅从全局上对点云畸变进行矫正，而忽略了无人驾驶场景中车辆运动导致的局部畸变，导致矫正后点云与真实场景(车辆自身与其他车辆的位置关系、其他车辆之间的位置关系)存在错误的映射关系，存在安全风险。
发明内容
针对以上问题，本发明首先通过解析激光雷达运动给出了点云全局畸变矫正方法；然后，基于点云动态车辆检测研究了点云动态车辆聚类以及车辆刚体运动估计，给出了点云局部畸变矫正方法；在此基础上，给出了无人驾驶场景下点云畸变矫正方法。实验表明，该方法通过动态车辆运动区域预测提高了车辆运动估计的准确率，有效矫正了点云畸变。从而为无人驾驶三维重建、三维定位提升了准确度，有效克服了无人驾驶运动行为智能化的安全隐患。
本发明的技术方案实现具体包括如下步骤：
步骤1.点云畸变分析与数据生成
步骤1.1点云畸变分析
步骤1.2点云畸变数据生成
步骤2.基于激光雷达运动解析的点云全局畸变矫正
步骤3.基于车辆运动解析的点云局部畸变矫正
步骤3.1点云动态车辆聚类方法
步骤3.2动态车辆刚体运动估计
步骤3.3动态车辆运动区域预测
步骤3.4点云局部畸变矫正
步骤4.无人驾驶场景下点云畸变矫正
与现有技术相比，本发明具有如下有益效果
本发明公开了一种无人驾驶场景下基于动态车辆检测的点云畸变矫正方法，提供了一种能够准确预测无人驾驶场景中由于车辆运动造成激光雷达采集的点云不能正确映射物体之间的真实位置关系等导致点云产生畸变的问题，有效矫正了点云畸变。从而为无人驾驶三维重建、三维定位提升了准确度，有效克服了无人驾驶运动行为智能化的安全隐患。
附图说明
图1全局畸变示意图。
图2局部畸变示意图。
图3仿真点云畸变数据示例。
图4点云全局畸变矫正示意图。
图5点云全局畸变矫正算法流程图。
图6点云动态车辆聚类分割示意图。
图7动态车辆点云聚类算法流程图。
图8动态车辆刚体运动估计示意图。
图9动态车辆运动区域预测示意图。
图10点云局部畸变矫正算法流程图。
图11点云畸变矫正算法流程图。
图12各点云配准算法在不同输入数据上的表现。
图13各点云配准算法对于不同点云数目车辆估计的旋转误差均值。
图14各点云配准算法对于不同点云数目车辆估计的旋转误差中位数。
图15各点云配准算法对于不同点云数目车辆估计的位移误差均值。
图16各点云配准算法对于不同点云数目车辆估计的位移误差均值。
图17MDDC方法点云畸变矫正可视化结果示例。
图18MDDC方法对车辆形变的矫正。
图19本发明的流程图。
具体实施方式
程久军等发明人于2023年5月15日申请的《无人驾驶场景下点云动态车辆检测方法》(申请人：同济大学，专利申请号：2023105469838)，该申请公开一种无人驾驶场景下点云动态车辆检测方法，其中，首先提出了无人驾驶场景下通过仿真生成点云数据以及动态车辆标注方法，在此基础上，给出了基于特征嵌入的点云动态车辆检测方法，并对其进行消融和对比实验。实验结果表明，该方法有效检测了无人驾驶场景点云中车辆的运动状态，且预测精度高于其它方法。从而不需要通过大量先验知识就能直接感知无人驾驶场景内车辆的运动情况，使得能够准确判别车辆和激光雷达的相对运动是否发生点云畸变，从而提升了无人驾驶车辆智能感知的准确性。该申请的技术方案实现具体包括如下步骤：
步骤1.动态车辆标注和构建训练数据集；
步骤2.搭建点云动态车辆检测模型并运行点云动态车辆检测算法；
步骤2.1数据预处理模块；
步骤2.2对点云进行特征提取的特征编码层；
步骤2.3特征嵌入层；
步骤2.4特征解码层；
步骤2.5输出层；
步骤2.6设计损失函数。
本发明方法基于上述申请《无人驾驶场景下点云动态车辆检测方法》的方法，提供了一种能够准确预测无人驾驶场景中由于车辆运动造成激光雷达采集的点云不能正确映射物体之间的真实位置关系等导致点云产生畸变的问题，有效矫正了点云畸变。从而为无人驾驶三维重建、三维定位提升了准确度，有效克服了无人驾驶运动行为智能化的安全隐患。
下面将结合具体实施例及其附图对本申请提供的技术方案作进一步说明。结合下面说明，本申请的优点和特征将更加清楚。
本发明具体包括如下5个方面：
步骤1.点云畸变分析与数据生成
步骤1.1点云畸变分析
步骤1.2点云畸变数据生成
步骤2.基于激光雷达运动解析的点云全局畸变矫正
步骤3.基于车辆运动解析的点云局部畸变矫正
步骤3.1点云动态车辆聚类方法
步骤3.2动态车辆刚体运动估计
步骤3.3动态车辆运动区域预测
步骤3.4点云局部畸变矫正
步骤4.无人驾驶场景下点云畸变矫正
步骤5.仿真实验验证
详述如下：
步骤1点云畸变分析与数据生成
本发明用到的符号如表1所示。无人驾驶场景中包含大量运动状态各异的车辆，同时由于智能交通系统以及无人驾驶车群技术的发展，车辆往往保持高速行驶，使得场景中车辆运动导致的点云畸变不可忽视。而现存的无人驾驶场景下点云数据集为保证采集到的数据质量，往往数据采集车辆行驶速度不高，且场景多为限速的驾驶场景，难以满足研究需求。本发明在对无人驾驶场景下点云畸变分析的基础上，提出一种点云畸变数据生成方法，并模拟无人驾驶场景下点云的畸变。
表1
(1)点云畸变分析
由于当前缺乏针对无人驾驶场景下点云畸变矫正及动态车辆检测的点云数据集，本发明使用仿真软件生成仿真实验数据集。在生成仿真实验数据集之前，对无人驾驶场景下点云畸变进行分析及重定义。
首先对无人驾驶场景下点云畸变的组成部分进行定义及分析。无人驾驶场景中激光雷达采集到的点云产生的畸变主要来自两个方面：①激光雷达运动导致的畸变；②场景中物体(以车辆为主体)运动导致的畸变。
1)激光雷达运动导致的畸变
在无人驾驶场景中，激光雷达的运动主要为其随无人驾驶车辆的平移以及旋转。如图1所示，激光雷达平移运动导致的畸变体现在点云上，一方面会使采集到的点云相对于场景中物体发生形变及相对位置变化，另一方面也可能导致采集的点云丢失部分信息。平移导致的畸变点云可表示如公式(1)：
其中P为无畸变的点云；为因平移导致的畸变点云；pi无畸变点云中的数据点；pj为因平移畸变消失的数据点；di为点云平移畸变系数。
而激光雷达旋转运动导致的畸变体现在点云上主要为物体表面的曲率发生变化和物体部分信息丢失。旋转导致的畸变点云可表示如公式(2)：
其中P为无畸变的点云；为因旋转导致的畸变点云；pi无畸变点云中的数据点；pj为因旋转畸变消失的数据点；λi为点云旋转畸变系数。
由于激光雷达导致的点云畸变作用在采集到的点云的所有数据点上，所以将由激光雷达运动导致的点云畸变称为全局畸变PDg。
影响激光雷达运动导致的全局畸变程度的因素主要是激光雷达所属车辆行驶的速度。
2)场景中物体运动导致的畸变
场景中物体运动导致的畸变本质上也是物体与激光雷达在采集一帧数据的时间间隔内发生了相对位移，如图2所示。场景中物体的运动导致的畸变点云如公式(3)所示：
其中P为无畸变的点云；为因场景中物体运动导致的畸变点云；/>为场景中静止物体对应的点云；pi为畸变点云中的数据点；pj为因场景中物体运动导致的消失的数据点；pk为场景中静止物体对应的数据点；di和λi分别为点云平移和旋转畸变系数。
由于无人驾驶场景的复杂性，场景中往往存在静止的物体(建筑、树木)和运动的物体(车辆)，这使得场景中物体运动导致的畸变往往具有局部性，所以将由场景中物体运动导致的点云畸变定义为局部畸变PDl。
则，定义无人驾驶场景点云畸变PD为：
PD＝{PDg，PDl} (4)
其中，PD为无人驾驶场景下点云畸变；PDg为点云全局畸变；PDl为点云局部畸变。
定义无人驾驶场景点云畸变矫正真值为某一时间点场景的快照，即假设在该时刻激光雷达瞬间采集的图像为该时刻的真值。即：
truth＝Point(t)where vlidar＝0and vvehicle＝0 (5)
其中，truth为时刻t的真值，等同于从该时刻起激光雷达和车辆均处于静止状态时采集的图像。
(2)点云畸变数据生成
在对无人驾驶场景下点云畸变分析的基础上，通过调整激光雷达以及场景中车辆模型运动的方式来获得不同的仿真点云畸变数据，主要通过以下三种方式获得模拟数据：1)场景保持静止，激光雷达从不同位置开始，以不同的速度运动，并采集点云数据；2)场景中车辆模型从不同位置开始，以不同的速度运动，同时激光雷达保持静止，并采集点云数据；3)激光雷达和车辆同时以不同的速度运动，并采集数据。
为了模拟点云采集过程中激光雷达及车辆的运动，以激光雷达每采集一列数据的时间为时间间隔移动仿真场景中激光雷达及车辆的位置。/>可以由式(6)计算得到：
其中，为每两次位置变换的时间间隔；f为激光雷达的扫描频率；dpi为激光雷达的扫描分辨率。
在时间内，通过移动激光雷达及点云的位置来形成相对位置变化以产生点云畸变，同时给不同的车辆及激光雷达赋予不同的速度以模拟复杂的无人驾驶场景。在/>时间内激光雷达采集到的点云数据点的畸变可表示如下：
其中，为/>时间间隔内由于车辆模型集合M与激光雷达的相对运动导致的点云畸变集合；/>为激光雷达在间隔/>的运动向量，/>为模型M中j点在时间间隔/>的运动向量，受时间间隔/>内模型M的旋转、平移等影响；/>为模型的仿射变换；为模型M在时间间隔/>内的运动向量。
在采集数据的过程中，给数据加入随机的高斯噪声，该噪声主要由以下场景产生的数据偏差组成：1)激光雷达在采集点云的过程产生的距离误差；2)激光雷达采集点云过程中出现的离群点；3)在激光雷达及车辆运动速度中加入扰动噪声。
对于每一个场景，分别采集四张图像：①第一帧时间内静止激光雷达采集的静止场景无噪声点云；②第一帧时间内动态激光雷达采集的动态场景有噪声点云；③第二帧时间内静止激光雷达采集的静止场景无噪声点云；④第二帧时间内动态激光雷达采集的动态场景有噪声点云。如图3示例所示，其中，激光雷达保持静止，车辆1保持静止，车辆2以50km/h的速度向左运动，车辆3以110km/h的速度向左运动。
步骤2基于激光雷达运动解析的点云全局畸变矫正
无人驾驶场景下点云畸变主要来自激光雷达运动导致的全局畸变以及场景内以车辆为主体的物体运动导致的局部畸变。
本发明通过解析激光雷达运动矫正点云的全局畸变，以消除激光雷达运动对后续场景内车辆运动解析的干扰，为后续局部畸变矫正提供基础。
全局畸变矫正示意图如图4所示。
首先给出全局畸变矫正公式推导，然后给出点云全局畸变矫正算法。点云全局畸变可表示为式(8)：
offsetglobal＝f(ΔRLidar) (8)
其中，offsetlocal为点云全局畸变补偿；ΔRLidar为激光雷达在时间间隔内的运动；f(ΔRLidar)表示根据激光雷达的刚体运动对点云进行的点云全局畸变矫正。
假设激光雷达在采集一帧数据的时间间隔内匀速运动，则车辆在采集第k帧点云时的速度vk可由式(9)表示：
其中，Xk,Xk+1分别为车辆在开始采集第k帧和开始采集第k+1帧时的位置；为激光雷达采集一帧数据的时间。
在得到激光雷达在采集第k帧点云时的速度后，对于第k帧点云中的数据点pi来说，其由激光雷达运动导致的数据点pi的畸变补偿量Θi可由式(10)得到：
其中，ts为开始采集第k帧点云时的时间戳；ti为采集数据点pi时的时间戳；Δti为ti和ts两者之间的差值。
对于激光雷达中数据点采集的时间，可以通过数据点在以激光雷达为原点的坐标系中的位置计算得到。数据点pi到激光雷达原点的单位方向向量仅由激光雷达的垂直视场角大小、激光雷达分辨率、激光雷达在采集当前帧时已扫描过的列数决定。而垂直视场大小和分辨率并不会随着时间变化。而激光雷达所扫描的列数则随着时间均匀增大。而点pi所在的列数ζi可由式(11)计算得到：
其中，αi为在以激光雷达为原点的坐标系下点pi对应的单位向量的角度。
在本发明中，仅考虑激光雷达在垂直方向z上的旋转变化，则αi可由式(12)表示
其中，pi,x,pi,y分别为数据点pi在x方向及y方向的坐标，可知。
根据上述分析，数据点pi的畸变补偿量Θi可通过式(13)计算得到：
由于采集到的点云存在噪声，点云之间并不是按照采集时激光雷达激光点的扫描方式排列，所以在计算αi时使用线性插值的方法使其大小符合激光雷达的扫描特性，即将离散的点云数据点移动到规则的扫描角度上。则经过线性插值后点pi对应的单位向量角度α′i可由式(14)表示：
α′i＝n·dpi or(n+1)·dpi,where n·dpi<αi<(n+1)·dpi (14)
在计算得到补偿量Θi后，点云Pk经过全局畸变矫正方法消除激光雷达运动导致的畸变后得到的点云Pi g可由式(15)表示：
Pi g＝{p′i＝pi+Θi|pi∈Pk} (15)
点云全局畸变矫正算法流程图如图5所示。点云全局畸变矫正算法具体步骤如下所示：
步骤2.1输入存在畸变的点云帧Pi。
步骤2.2如果存在点云数据点pi∈Pi，转步骤2.3；否则，转步骤2.8。
步骤2.3解析得到点云数据点pi的坐标x,y,z。
步骤2.4根据坐标x及坐标y计算得到该点云数据点对应的激光雷达光束角度。
步骤2.5计算激光雷达光束在一帧激光雷达数据中的编号，并通过线性插值的方式进行矫正。
步骤2.6对激光雷达点云编码，以便后续实验处理。同时计算该激光束对应帧内时间。
步骤2.7计算点云pi在x方向和y方向上的点云全局畸变，并对点云全局畸变进行矫正，将矫正后的点云数据点放入Pi g中。
步骤2.8输出经全局畸变矫正后的点云。
步骤3基于车辆运动解析的点云局部畸变矫正
点云的局部畸变主要来自于场景中车辆运动，因此，首先，通过点云动态车辆聚类方法分割出同一帧点云上不同的动态车辆；然后，提出了一种基于点云配准的动态车辆刚体运动估计方法解析场景内不同车辆的运动，并通过动态车辆运动区域预测提高车辆运动估计的准确率；最后，基于车辆运动解析结果给出点云局部畸变矫正方法。
步骤3.1点云动态车辆聚类方法
如图6所示，在通过点云动态车辆检测方法得到点云数据点的运动状态后，由于不同的车辆具有不同的运动状态，需要对处于运动状态的点云进行分割，以分别对不同车辆导致的点云畸变进行不同的处理。
激光雷达采集的点云具有近密远疏的特性，即距离较近的位置点云往往较密集，距离较远的位置点云往往较稀疏。如采用等距离的距离阈值点云聚类算法，当距离过大时，很容易将近处的多个动态车辆对应的点云聚类为一个动态车辆，当距离较小时，较远处车辆点云可能难以聚合。本发明在欧式聚类算法的基础上，提出了一种改进的动态距离阈值车辆点云聚类算法，该算法旨在正确聚合处于不同距离的车辆点云数据，相比于欧式聚类算法，其能根据点云数据点所处位置动态调整距离阈值，以正确分割不同远近的车辆。
该方法首先根据动态车辆检测结果将点云分为两个不同的集合，分别为动态点云集合Datadyn和静态点云集合Datasta，并以动态点云集合Datadyn建立KD-tree(K-DimensionTree)，随后从动态点云集合随机选择一个数据点pi放入单个车辆集合vehicle中，作为聚合的起始点，随后通过Kd-Tree寻找距离pi最近的26个点，选择最近的26个点主要是为了覆盖数据点的各个方位，防止同一车辆被错误的分割为多辆车辆。分别计算这26个点到点到pi的距离，并将距离小于动态距离Dis的点加入单个车辆集合vehicle中，并重新作为聚合起始点寻找其它属于单个车辆集合vehicle的数据点。其中动态距离Dis可由式(16)表示：
其中，pi,x,pi,y分别为数据点pi在x方向、y方向和z方向的坐标；θV为激光雷达垂直视场角大小，max(θV)取最大的垂直视场角；γ为扩大系数，取1.5。
在完成单个车辆集合vehicle的聚合后，计算vehicle中点的数量，因车辆数据点较少时，对点云整体的畸变矫正影响不大，所以忽略点数量小于Ncol的车辆，Ncol的计算方式定义如下：
Ncol＝τ·col (17)
其中，col为激光雷达垂直方向激光束个数；τ为扩大系数，取3。
动态车辆点云聚类算法流程图如图7所示。动态车辆点云聚类算法具体步骤如下所示：
步骤3.1.1输入经点云动态车辆检测方法FEMD处理后的点云帧Pi。
步骤3.1.2初始化已访问节点集合visited和车辆集合Vehicles。
步骤3.1.3根据点云Pi中数据点的运动状态，将Pi中数据点分为动态点云集合Datadyn和静态点云集合Datasta。
步骤3.1.4以动态点云集合Datadyn建立KDTree。
步骤3.1.5存在点云数据点pi∈Datadyn且未被访问，转步骤3.1.6，否则，转步骤3.1.14。
步骤3.1.6如果数据点pi未在已访问集合visited中，转步骤3.1.7；否则，转步骤3.1.5。
步骤3.1.7初始化车辆vehicle和队列Queue。并将数据点pi放入单个车辆集合vehicle、队列Queue和已访问节点集合vistied中。
步骤3.1.8如果队列Queue不为空，转步骤3.1.9；否则，转步骤3.1.13。
步骤3.1.9取队列的第一个元素pk并将其从队列中删除，通过kdt查询距离pk最近的26个元素，得到集合distancei。
步骤3.1.10如果存在元素pj∈distancei且未被访问，转步骤3.1.11；否则，转步骤3.1.13。
步骤3.1.11如果点pj与pk之间的距离小于动态距离Dis，则转步骤3.1.12；否则，转步骤3.1.10。
步骤3.1.12将数据点pj放入单个车辆集合vehicle、队列Queue和已访问节点集合visited中。
步骤3.1.13如果车辆vehicle中点数量大于128，则将其放入车辆集合Vehicles中。
步骤3.1.14输出分割后的车辆集合Vehicles。
步骤3.2动态车辆刚体运动估计
如图8所示，局部畸变来自场景中车辆的运动，本发明基于点云配准算法对场景中各车辆的运动进行估计。
因为激光雷达采集连续两帧的时间间隔较小，本发明将采集连续两帧时间间隔内场景内车辆的运动同样看成匀速直线运动。通过解析连续两帧点云中不同车辆的位置变换解析其运动，从而对不同车辆导致的畸变分别进行矫正，即：
其中，offsetlocal为点云局部畸变补偿；vehiclek表示点云经过车辆聚合方法分割出的运动车辆；n表示运动车辆的个数；表示车辆在时间间隔/>内的运动；表示根据车辆vehiclei的运动对其进行的点云局部畸变矫正。
对于点云Pi中的车辆vehiclek，其在连续两帧时间间隔内的运动/>可由式(19)表示：
其中，r表示车辆vehiclek对应点云数据点pi的旋转变化；t表示车辆vehiclek对应点云数据点pi的平移变化。
可以将式表达为仿射变换的形式，如式(20)所示：
其中，r为3×3的旋转矩阵，而t为3×1的位移矩阵，该变换矩阵总共包含六个自由度，分别为绕x,y,z轴的旋转及位移。由于本发明研究的对象为道路上行驶的车辆，可以将车辆看成刚体，则对于任意的pi∈vehiclek，均具有相同的旋转矩阵和位移矩阵。
在估计车辆的刚体运动之前，先通过步骤4中的方法对点云的全局畸变进行矫正。在去除全局畸变后，点云中剩余的畸变则为车辆刚体运动导致的局部畸变。由于激光雷达的运动，采集的前后两帧点云并非在同一坐标系下，为更简便的估计车辆的刚体运动，通过坐标变换将第二帧图像点云坐标系转换为第一帧点云坐标系。坐标转换同样可以表示为仿射变换形式：
其中，(x1,y1,z1)和(x2,y2,z2)分别代表激光雷达采集到的连续两帧点云中第一帧数据和第二帧数据的坐标系；rl和tl则分别代表激光雷达在采集两帧数据间的旋转变换和位移变换。
随后，通过点云配准算法分别估计不同车辆的刚体运动情况。点云配准算法能够通过寻找两帧点云中的对应点估计变换矩阵来对齐两帧点云。在连续两帧间，假设车辆保持同一速度运动，则同一车辆在连续两帧上具有相近的畸变，保证了通过点云配准估计点云运动的鲁棒性。同时，点云配准算法能够在两帧点云部分重叠的情况下完成配准，所以通过点云配准进行运动估计也能解决激光雷达运动导致的信息丢失问题。通过点云配准进行运动估计可由式表示：
其中，表示通过点云配准算法估计的车辆运动变换仿射矩阵；pi→CP(pj)表示在第二帧点云pnext中寻找点云pi的对应点；f(pi→CP(pj))表示根据对应点关系估计车辆的刚体运动。
步骤3.3动态车辆运动区域预测
在无人驾驶场景中存在着许多的车辆，众多车辆中可能存在大量的车辆有着相似的形状，通过点云形状难以区分。本发明提出一种基于动态车辆运动区域预测的点云分割算法，基于之前点云帧估计出的车辆速度，针对点云动态车辆聚类方法中分割出的不同车辆，对第二帧数据采取不同的分割区域，以减少车辆在连续两帧上的匹配错误率。
由于车辆运动具有惯性，在较短时间内，车辆速度不可能瞬间提高，而是有一个加速或减速的过程，利用这个特性。如图9所示，以车辆位置为中心，根据其速度及估计的最大加速度大小划分出其在下一帧时间内可能出现的区域，从该区域内寻找其在下一帧数据上的可能的对应位置。
根据现阶段车辆加减速规律，对车辆在其速度方向及垂直于其速度方向做出如下假设：车辆在其速度方向上及垂直于速度方向上的最大加速大小为10m/s2，最小加速度大小为-10m/s2。则假设车辆的速度为v0，在一帧时间间隔t后，其在速度方向及垂直于速度方向的位移可能值为：
其中，x1为车辆速度方向预测位移值，x2为车辆速度反方向预测位移值；y1和y2为垂直于车辆速度方向预测位移值，且互为反方向；a为车辆的最大加速度。
同时，需要估计第一帧时车辆的占地范围，对于点云动态车辆聚类方法分割出的每一辆车辆，计算其点云坐标在速度方向及垂直于速度方向的最大值和最小值，作为车辆的范围。同时，由于激光雷达往往只能采集到车辆一部分侧面的点云图像，所以根据此方法测算出的车辆范围往往不能完全覆盖车辆的真实范围，为了避免安全隐患，在预测出的车量范围上加上补偿值，在其速度方向加上车辆的长度，约3m，在垂直于速度方向上加上车辆的宽度，约2m，如式(24)所示：
其中，vehiclex,1为车辆速度方向车辆预估范围；vehiclex,2为车辆速度反方向车辆预估范围；vehicley,1和vehicley,2分别为垂直于车辆速度方向车辆预估范围，且互为反方向；offset为补偿值。
所以，车辆在下一帧可能出现的范围可表示为(x1+vehiclex,1,x2+vehiclex,2,y1+vehicley,1,y2+vehicley,2)组成的矩形框区域。
步骤3.4点云局部畸变矫正
在完成对一帧点云中所有动态车辆的刚体运动估计后，基于车辆运动解析结果，对点云的局部畸变进行矫正。在进行点云的局部畸变矫正之前，通过解析点云数据点编码获得点云中每个数据点的时间戳。点云的全局畸变和局部畸变之间为线性关系，全局畸变矫正过程中点云的移动并不会对局部畸变产生影响。在得到车辆在两帧点云间的运动信息后，点云的局部畸变矫正可以表示为：
其中，为车辆vehiclek对应点云的局部畸变补偿；timei为车辆vehiclek中点云数据点pi对应的时间戳；timestart为激光雷达开始采集第一帧点云数据时的时间戳。
则经过局部畸变矫正后的点云可表示为式(26)：
其中，为经局部畸变矫正后的点云；Pi g为经全局畸变矫正后的点云；n为点云Pi中运动车辆个数；/>为运动车辆vehiclek对应的点云数据点。
点云局部畸变矫正算法流程图如图10所示。点云局部畸变矫正算法具体步骤如下：
步骤3.4.1初始化连续两帧点云，并通过全局畸变矫正方法矫正点云Pi g及
后作为模型的输入。
步骤3.4.2通过点云动态车辆聚类算法分割点云Pi g，并得到运动车辆集合vehicle_seti，同时给vehicle_seti中车辆对应点云编号。
步骤3.4.3通过点云动态车辆检测算法检测中运动车辆，并根据检测结果分割点云/>获得只包含运动车辆的点云/>
步骤3.4.4遍历车辆集合vehicle_seti，对vehicle_seti中每一车辆vehiclek，通过动态车辆刚体运动估计方法计算每一车辆的刚体变换矩阵，并将计算结果放入集合T_set中。
步骤3.4.5存在点云数据点pi∈Pi g且未被访问，转步骤3.4.6；否则，转步骤3.4.7。
步骤3.4.6判断点云pi是否属于运动车辆对应点，如果不属于，则直接将点云数据点pi放入集合如果属于，则获得对应车辆的编号，并从T_set中获取对应车辆的刚体变换矩阵；根据基于激光雷达运动解析的点云全局畸变矫正节中的编码结果，解码得到对应点的时间戳；计算得到点云的局部畸变补偿，并矫正点云pi的位置，得到点云数据点/>将点云数据点/>放入集合/>
步骤3.4.7输出经局部畸变矫正后的点云
步骤4无人驾驶场景下点云畸变矫正
本发明提出的无人驾驶场景下点云畸变矫正方法(MDDC)通过基于激光雷达运动解析的全局畸变矫正方法矫正点云的全局畸变，在对点云全局畸变矫正的基础上，通过解析场景中动态车辆的运动矫正点云局部畸变，以实现无人驾驶场景下点云畸变矫正。点云畸变矫正算法流程图如图11所示，具体算法流程如下：
步骤4.1输入连续的两帧点云Pk,Pk+1及激光雷达的运动速度vlidar。
步骤4.2对点云Pk,Pk+1进行全局畸变矫正得到点云
步骤4.3对点云进行去噪及下采样处理，得到数据预处理后的点云
步骤4.4通过点云动态车辆检测方法检测两帧点云中的动态车辆，得到预测标签lable。
步骤4.5根据动态车辆检测结果对点云中动态车辆进行运动解析，得到车辆运动信息infmotion。
步骤4.6基于车辆运动信息infmotion对点云进行局部畸变矫正，得到无畸变点云P′k。
步骤4.7输出无畸变点云P′k。
步骤5仿真实验验证
(1)评价指标
实验采用的评价指标分别包括：旋转误差、平移误差、Hausdorff距离、Chamfer距离、EMD距离。
1)旋转误差：给定预测旋转矩阵R及旋转矩阵真值RGT，其旋转误差为：
其中，δ(R,RGT)为旋转误差；trace(R-1RGT)为R-1RGT矩阵的迹。
2)平移误差：给定预测平移矩阵t及平移矩阵真值tGT，其平移误差为：
δ(t,tGT)＝‖t-tGT‖ (28)
其中，δ(t,tGT)为平移误差；‖t-tGT‖取向量t及向量tGT的距离。
3)Hausdorff距离：给定两个点云P1和P2，Hausdorff距离定义如下：
dH(P1,P2)＝max(dh(P1,P2),dh(P2,P1)) (30)
其中，dh(P1,P2)和dh(P2,P1)分别表示点云P1到P2和点云P2到P1的单向霍夫曼距离；dH(P1,P2)表示双向霍夫曼距离。Hausdorff距离主要衡量两个点云之间的最大不相关性。
4)Chamfer距离：给定两个点云P1和P2，Chamfer距离定义如下：
其中，n(P1)和n(P2)分别表示点云P1到P2中点的数量。Chamfer距离主要衡量两个点云之间的区别程度，Chamfer距离越小，两个点云之间的区别越小。
5)EMD距离：给定两个点云P1和P2，EMD距离定义如下：
其中，φ为点云P1到P2的映射；EMD距离表示点云P1转换为点云P2的代价，主要衡量两帧点云之间的相似性。
(2)实验结果分析
1)动态车辆刚体运动估计
首先对比不同的点云配准方法①ICP、②RANCSC、③FGR、④EEMR在无人驾驶车辆刚体运动估计上的表现，以选择最合适的点云配准方法进行动态车辆刚体运动估计。其中EEMR代表基于深度学习的点云配准方法，为体现各算法的泛化性，在ModelNet40数据集上训练各种基于深度学习的方法如EEMR、PointNetLK、PCRNet、DCP、PRNet、RPM-Net，并对比其在无人驾驶车辆点云数据集上的配准效果，其中EEMR算法表现较好，而其它算法由于泛化性原因均不能有效的配准车辆点云，主要原因是EEMR通过深度学习算法提取特征后，使用置信模块进行点云配准，所以本实验选择EEMR代表基于深度学习的算法，并在无人驾驶仿真数据集上对模型参数进行微调。而RANCSC和FGR均使用FPFH提取点云特征。
将上述算法分别在两个不同的输入数据上进行比较：①第二帧原始数据；②经过动态车辆运动区域预测算法处理后的数据。同时，将上述算法与另一个车辆运动估计方法CPME在位移误差上进行对比，以验证本发明车辆运动估计算法的性能。
ICP、RANCSC、FGR与EEMR算法在两个不同的输入数据上的准确率如图12所示。在本实验中，定义估计的点云刚体运动的旋转误差小于5度并且点云的位移误差小于0.5m的数据为准确值，其中旋转误差和位移误差均转换到真实场景尺度下。从图中可以看出，相比于输入的数据中第二帧数据为原始数据，在经过动态车辆运动区域预测算法对数据处理后，各算法的准确度均有所提高，其中ICP算法提升较大，主要是因为当场景中包含较多数据时，ICP算法容易陷入局部最优，导致预测错误，而经过动态车辆运动区域预测算法处理后，能减少环境中点云及其它车辆点云的干扰，有效提升ICP算法的预测准确度。而RANSAC算法相较于ICP算法在原始点云上及经动态车辆运动区域预测方法处理后点云上均有较好的表现，主要是RANSAC通过采样的方式迭代的配准点云，具有更好的全局点云配准能力；同时，基于车辆运动的特性，RANSAC也有着优良的初始矩阵，提高了其配准的准确度。另外，FGR及EEMR算法的表现一直优于ICP算法及RANSAC算法，这主要是因为FGR及EEMR算法具有更好的精配准调整模块，能够有效的减少运动估计的旋转误差及位移误差，具有更好的特征拟合能力及鲁棒性。
实验表明，动态车辆运动区域预测方法能有效提高车辆运动估计的准确率，接下来实验数据均以动态车辆运动区域预测方法处理后的第二帧点云作为输入，对比不同配准算法对于不同数据点个数的车辆的运动估计表现：①车辆点云数据点个数大于等于1000的数据；②由于遮挡、距离较远等原因导致的车辆点云数据点数量小于1000的数据。
ICP、RANCSC、FGR与EEMR算法在不同点云数目的车辆数据上运动估计的旋转误差均值如图13所示，从图中可以看出，相对于其它算法，ICP算法在不同数目车辆点云数据上的旋转误差均值均较高，主要是车辆点云数据中存在较多的相似特征如车身侧平面，当连续两帧点云中车辆相隔较远时，ICP算法容易陷入局部最优，导致精度下降。在大于1000个数据点的车辆上，EEMR算法相较于其它算法具有更好的表现，旋转误差均值为1.9206，低于RANSAC算法的2.654及FGR算法的2.417，主要是基于深度学习算法进行特征提取的EEMR算法具有更强的点云特征提取能力，当点云数据点较多时，EEMR算法能提取到更多的有效特征。相较于车辆点云数目大于等于1000的数据，各算法在车辆点云数目小于1000的数据上的运动估计旋转误差均值均有所升高，说明当车辆点云数目较少时，各算法估计精度均有所下降。其中RANSAC算法旋转误差上升较小，主要是车辆大部分时候只绕z轴旋转，所以RANSAC算法能具有较好的初始旋转矩阵，同时基于采样的RANSAC算法能有效的处理含有无效点、离群点的情况。而FGR及EEMR在车辆点云数目小于1000时旋转估计误差均有所提升，主要是因为当数据点较少时，FGR及EEMR能提取的特征较少，并且遮挡可能会导致车辆的点云数目及形状发生变化，导致FGR和EEMR算法容易出现错误的估计。从整体上看，EEMR具有更好的车辆旋转估计性能。
ICP、RANCSC、FGR与EEMR算法在不同点云数目的车辆数据上的旋转误差中位数如图14所示，可以看出，旋转误差中位数均小于旋转误差均值，这主要是因为当激光雷达及场景中车辆运动时，会导致一部分车辆出现鬼影或者截断的现象，点云配准算法难以对此类数据的车辆运动情况进行有效的估计，导致出现部分异常数据，旋转误差均值被拉高。在车辆点云数据点数目大于1000的数据上，EEMR算法及FGR算法均有较低的旋转误差中位数，分别为1.6509和1.5481，说明当车辆点云数目较多时，两种算法具有更高的旋转估计精度，而EEMR算法旋转误差均值低于FGR算法，说明EEMR算法相较与FGR算法，具有更好的异常值处理能力。综合来看，EEMR算法具有更好的旋转估计精度及鲁棒性。
将本发明使用的动态车辆刚体运动估计方法与其它车辆运动估计方法CPME在位移误差上进行对比。
基于ICP、RANCSC、FGR与EEMR算法进行运动估计和CPME方法进行运动估计在不同点云数目的车辆数据上的位移误差均值如图15所示，从图中可以看出，当车辆点云数目大于1000时，CPME算法的位移误差均值接近基于RANSAC算法进行动态车辆刚体运动估计时的位移误差均值，高于FGR算法以及EEMR算法，说明通过改进点云配准算法能够显著的提高本发明的动态车辆刚体运动估计算法的性能，并且当点云配准算法性能较优时，能够有比CPME算法更好的性能。当车辆点云数目小于1000时，CPME算法的位移误差显著升高，而本发明的动态车辆刚体运动估计算法仍然能保持较优秀的性能，其中基于FGR算法和基于EEMR表现最优，分别为1.16m和1.06m，这主要是因为CPME算法基于中心点估计车辆运动情况，当车辆出现遮挡等导致特征不完整时，CPME算法容易出现中心点偏移，导致较大的误差，而本发明基于配准的车辆运动估计算法在车辆特征不完整时仍然能完成特征的匹配，保证算法的稳定性。所以，基于配准的车辆估计算法能具更优秀的车辆运动估计性能和鲁棒性。
基于ICP、RANCSC、FGR与EEMR算法进行运动估计和CPME方法进行运动估计在不同点云数目的车辆数据上的位移误差中位数如图16所示。相较于位移估计均值，位移估计中位数均大幅降低，这主要是因为异常值的出现会大幅拉高位移误差的中位数。当车辆点云数目大于等于1000时，FGR和EEMR算法的位移误差中位数均大幅度低于位移误差均值，分别为0.277m和0.189m，说明两种算法均具有较好的位移估计精度，同时显著低于CPME算法的0.54m，说明基于配准的车辆运动估计算法能具有更高的运动估计精度。当车辆点云数目小于1000时，CPME算法的平移误差中位数显著高于基于RANSAC、FGR、以及EEMR，说明当车辆点云特征不完整时，基于配准的车辆运动估计算法能从更少的特征中提取更多的信息以提高动态车辆刚体运动估计的精度，而CPME则对车辆特征的完整性较敏感。总的来说，基于配准的动态车辆刚体运动估计算法能达到更高的精度。
综上，通过实验验证了动态车辆运动区域预测算法对车辆运动估计的正向影响，且在无人驾驶点云仿真数据集上，基于深度学习方法进行特征提取的EEMR算法为代表的动态车辆刚体运动估计算法有更好的车辆运动估计性能，具有比CPME算法更高的估计精度。在接下来的点云畸变矫正中，使用以EEMR算法为基础的动态车辆刚体运动估计算法解析车辆的运动。
2)点云畸变矫正
接下来，用可视化结果和量化指标对比说明无人驾驶场景下点云畸变矫正算法MDDC的可行性。
图17为MDDC方法在无人驾驶仿真数据集上的畸变矫正可视化结果。为更准确的对比两种点云畸变矫正算法的性能，忽略点云噪声的干扰。图①无畸变点云图像作为实验的真值。
(a)原始点云图像
图②为激光雷达采集到的原始图像，其中左下角车辆的速度为0km/h，右下角车辆的速度为85km/h，而激光雷达则以95km/h的速度向右运动。相比于无畸变点云图像①，可以看到虽然场景中右下角车辆在高速移动，但是激光雷达感知不到场景中车辆的运动，主要是因为此时车辆以及激光雷达几乎以相同的速度运动，使得同一帧间激光雷达与车辆的相对位置不变，导致场景中车辆出现伪静止的现象。对比原始点云图像及无畸变点云图像在水平方向上即激光雷达运动方向的长度，分别约为26m和28.5m，可知点云图像在激光雷达运动方向上发生了形变，这主要是因为激光雷达的运动使得点云在激光雷达运动方向上产生了全局畸变，并且使得点云中一部分信息丢失，同时右下角车辆的运动也遮蔽了右上角车辆，使得部分车辆信息丢失。
(b)全局畸变矫正后点云图像
图③为原始点云图像②经过全局畸变矫正后的点云图像。全局畸变矫正后图像在水平方向上即激光雷达运动方向上的长度与无畸变点云图像吻合，说明全局畸变矫正算法能有效矫正由于激光雷达运动导致的畸变。全局畸变矫正后图像中运动车辆位置与无畸变点云图像差距较大，且出现了断裂的现象，说明全局畸变矫正算法能有效的消除由激光雷达运动导致的畸变，但是无法处理场景中车辆运动导致的畸变。
(c)局部畸变矫正后点云图像
图④为在全局畸变矫正矫正基础上进行全局畸变矫正后的点云图像，即通过MDDC方法矫正后的点云图像。图④相较于点云无畸变图像①在水平方向上具有几乎相等的长度，说明MDDC方法能有效矫正由于激光雷达运动导致的点云全局畸变。图④中无论静态车辆还是运动车辆位置均与图①吻合，可知MDDC算法能有效的矫正运动车辆的位置，说明MDDC算法对点云局部畸变矫正的有效性。
(d)同一坐标系下对比图像
图⑤为经过全局畸变矫正后的点云图像、经过MDDC算法矫正后的点云图像、无畸变点云图像在同一坐标系下的点云图像。其中红色为无畸变点云数据，蓝色为经过全局畸变矫正算法处理后的点云数据，绿色为经MDDC算法处理后的点云数据，从图中可以看出，全局畸变矫正算法处理后的图像在整体形状上与无畸变点云图像吻合，但是运动车辆对应点云存在较大差距，说明其能有效的矫正激光雷达运动导致的全局畸变，但是无法矫正场景中车辆运动导致的局部畸变。而MDDC算法矫正后的点云图像整体上与无畸变点云图像吻合，说明其能有效的矫正点云的全局畸变和局部畸变。
综上所述，MDDC算法能有效的矫正无人驾驶场景下点云畸变。
经MDDC方法对车辆形变矫正的效果如图18所示，其中蓝色为原始点云，红色为无畸变点云，绿色为经MDDC方法处理后的点云，场景中车辆速度为80km/h，向左运动。可以看出，由于场景中车辆的运动，会使得车辆的整体长度增加，车辆点云发生局部形变。而通过MDDC算法处理后，车辆恢复到与无畸变时相同的长度，说明MDDC方法能有效的矫正由于场景中车辆运动导致的车辆形变。
接下来对比原点云和MDDC方法处理后点云在不同指标上的表现，以验证MMDC方法对无人驾驶场景下点云畸变矫正的可行性。
原始点云和经过MDDC方法处理后点云相较于无畸变点云在Hausdorff距离、Chamfer距离、EMD距离上的平均值如表2所示。其中，Hausdorff距离用来衡量两个点云集合的最大不相关性，从表中可以看出，经过MDDC方法处理后，相较于原始点云，与无畸变点云的Hausdorff距离平均值有所下降，但是下降幅度并不明显，这主要是因为点云动态车辆检测算法存在一定的误差，而这部分误差的来源往往是部分离散的噪声点，对这一部分数据，MDDC方法难以矫正，这使得大部分点云数据经过MDDC方法矫正后仍然有着和原始点云相同的Hausdorff距离。Chamfer距离表达形式与Hausdorff距离有着一定的相似性，主要衡量两个点云的相似程度。从表中可以看出，经过MDDC方法处理后的点云与无畸变点云的Chamfer距离平均值显著下降，说明经过处理后，点云与真值的相似性更高，MDDC方法能有效的矫正无人驾驶场景下点云畸变，结合可视化结果分析，说明MDDC方法能使存在畸变的点云矫正为无畸变点云的形状。EMD距离主要反映两个点云集合分布的差别大小，从表中可以看出，MDDC方法处理后的点云相较于原始点云与无畸变点云的EMD距离平均值下降了一半左右，说明经过MDDC方法处理后，点云分布更接近于真值，主要是点云局部畸变矫正方法能通过解析车辆的运动来还原点云无局部畸变时的分布，能有效的矫正点云的局部畸变，使得点云局部畸变能一定程度上还原为无畸变点云的分布。
表2
综上所述，通过可视化结果和不同指标上MDDC方法处理后点云和原始点云与无畸变点云的多指标对比，验证了MDDC方法能在一定程度上矫正无人驾驶场景下点云畸变。
本发明针对目前无人驾驶场景下未考虑复杂的车辆运动以及点云与真实场景(车辆自身与其他车辆的位置关系、其他车辆之间的位置关系)存在错误的映射关系等问题，基于(程久军等发明人于2023年5月15日申请的《无人驾驶场景下点云动态车辆检测方法》(申请人：同济大学，专利申请号：2023105469838))，提出了一种无人驾驶场景下基于动态车辆检测的点云畸变矫正方法：首先，通过解析激光雷达运动给出了点云全局畸变矫正方法；然后，基于点云动态车辆检测研究了点云动态车辆聚类以及车辆刚体运动估计，给出了点云局部畸变矫正方法；在此基础上，给出了无人驾驶场景下点云畸变矫正方法。实验表明，该方法通过动态车辆运动区域预测提高了车辆运动估计的准确率，有效矫正了点云畸变。为无人驾驶三维重建、三维定位提升了准确度，有效克服了无人驾驶运动行为智能化的安全隐患。从而提升了无人驾驶车辆智能感知的准确性，有望突破无人驾驶运动行为决策智能化的障碍。
上述描述仅是对本申请较佳实施例的描述，并非是对本申请范围的任何限定。任何熟悉该领域的普通技术人员根据上述揭示的技术内容做出的任何变更或修饰均应当视为等同的有效实施例，均属于本申请技术方案保护的范围。

Claims (9)

1.无人驾驶场景下基于动态车辆检测的点云畸变矫正方法，其特征在于，包括如下步骤：
步骤1.点云畸变分析与数据生成；具体包括：
步骤1.1点云畸变分析；
步骤1.2点云畸变数据生成；
步骤2.基于激光雷达运动解析的点云全局畸变矫正；
步骤3.基于车辆运动解析的点云局部畸变矫正；具体包括：
步骤3.1点云动态车辆聚类方法；
步骤3.2动态车辆刚体运动估计；
步骤3.3动态车辆运动区域预测；
步骤3.4点云局部畸变矫正；
步骤4.无人驾驶场景下点云畸变矫正。
2.如权利要求1所述的无人驾驶场景下基于动态车辆检测的点云畸变矫正方法，其特征在于，所述步骤1中，点云畸变数据生成：
激光雷达每采集一列数据的时间为时间间隔移动仿真场景中激光雷达及车辆的位置，/>由式(6)计算得到：
其中，为每两次位置变换的时间间隔；f为激光雷达的扫描频率；dpi为激光雷达的扫描分辨率；
在时间内激光雷达采集到的点云数据点的畸变表示如下：
其中，为/>时间间隔内由于车辆模型集合M与激光雷达的相对运动导致的点云畸变集合；/>为激光雷达在间隔/>的运动向量，/>为模型M中j点在时间间隔/>的运动向量，受时间间隔/>内模型M的旋转、平移等影响；/>为模型的仿射变换；/>为模型M在时间间隔/>内的运动向量。
3.如权利要求1所述的无人驾驶场景下基于动态车辆检测的点云畸变矫正方法，其特征在于，所述步骤2：
点云全局畸变表示为式(8)：
offsetglobal＝f(ΔRLidar) (8)
其中，offsetlocal为点云全局畸变补偿；ΔRLidar为激光雷达在时间间隔内的运动；f(ΔRLidar)表示根据激光雷达的刚体运动对点云进行的点云全局畸变矫正；
车辆在采集第k帧点云时的速度vk由式(9)表示：
其中，Xk,Xk+1分别为车辆在开始采集第k帧和开始采集第k+1帧时的位置；为激光雷达采集一帧数据的时间；
由激光雷达运动导致的数据点pi的畸变补偿量Θi由式(10)得到：
其中，ts为开始采集第k帧点云时的时间戳；ti为采集数据点pi时的时间戳；Δti为ti和ts两者之间的差值；
数据点pi到激光雷达原点的单位方向向量仅由激光雷达的垂直视场角大小、激光雷达分辨率、激光雷达在采集当前帧时已扫描过的列数决定，点pi所在的列数ζi由式(11)计算得到：
其中，αi为在以激光雷达为原点的坐标系下点pi对应的单位向量的角度；
激光雷达在垂直方向z上的旋转变化，则αi由式(12)表示
其中，pi,x,pi,y分别为数据点pi在x方向及y方向的坐标；
数据点pi的畸变补偿量Θi通过式(13)计算得到：
经过线性插值后点pi对应的单位向量角度α′i由式(14)表示：
α′i＝n·dpi or(n+1)·dpi,where n·dpi＜αi＜(n+1)·dpi (14)
点云Pk经过全局畸变矫正方法消除激光雷达运动导致的畸变后得到的点云由式(15)表示：
4.如权利要求3所述的无人驾驶场景下基于动态车辆检测的点云畸变矫正方法，其特征在于，所述步骤2，算法过程如下：
步骤2.1输入存在畸变的点云帧Pi；
步骤2.2如果存在点云数据点pi∈Pi，转步骤2.3；否则，转步骤2.8；
步骤2.3解析得到点云数据点pi的坐标x,y,z；
步骤2.4根据坐标x及坐标y计算得到该点云数据点对应的激光雷达光束角度；
步骤2.5计算激光雷达光束在一帧激光雷达数据中的编号，并通过线性插值的方式进行矫正；
步骤2.6对激光雷达点云编码，以便后续实验处理；同时计算该激光束对应帧内时间；
步骤2.7计算点云pi在x方向和y方向上的点云全局畸变，并对点云全局畸变进行矫正，将矫正后的点云数据点放入中；
步骤2.8输出经全局畸变矫正后的点云。
5.如权利要求1所述的无人驾驶场景下基于动态车辆检测的点云畸变矫正方法，其特征在于，所述步骤3：
步骤3.1点云动态车辆聚类方法
动态距离Dis由式(16)表示：
其中，pi，x，pi，y分别为数据点pi在x方向、y方向和z方向的坐标；θV为激光雷达垂直视场角大小，max(θV)取最大的垂直视场角；γ为扩大系数，取1.5；
车辆数据点较少时，Ncol的计算方式定义如下：
Ncol＝τ·col (17)
其中，col为激光雷达垂直方向激光束个数；τ为扩大系数，取3；
点云动态车辆聚类，其运行的算法过程如下：
步骤3.1.1输入经点云动态车辆检测方法FEMD处理后的点云帧Pi；
步骤3.1.2初始化已访问节点集合visited和车辆集合Vehicles；
步骤3.1.3根据点云Pi中数据点的运动状态，将Pi中数据点分为动态点云集合Datadyn和静态点云集合Datasta；
步骤3.1.4以动态点云集合Datadyn建立KDTree；
步骤3.1.5存在点云数据点pi∈Datadyn且未被访问，转步骤3.1.6，否则，转步骤3.1.14；
步骤3.1.6如果数据点pi未在已访问集合visited中，转步骤3.1.7；否则，转步骤3.1.5；
步骤3.1.7初始化车辆vehicle和队列Queue，并将数据点pi放入单个车辆集合vehicle、队列Queue和已访问节点集合visited中；
步骤3.1.8如果队列Queue不为空，转步骤3.1.9；否则，转步骤3.1.13；
步骤3.1.9取队列的第一个元素pk并将其从队列中删除，通过kdt查询距离pk最近的26个元素，得到集合distancei；
步骤3.1.10如果存在元素pj∈distancei且未被访问，转步骤3.1.11；否则，转步骤3.1.13；
步骤3.1.11如果点pj与pk之间的距离小于动态距离Dis，则转步骤3.1.12；否则，转步骤3.1.10；
步骤3.1.12将数据点pj放入单个车辆集合vehicle、队列Queue和已访问节点集合visited中；
步骤3.1.13如果车辆vehicle中点数量大于128，则将其放入车辆集合Vehicles中；
步骤3.1.14输出分割后的车辆集合Vehicles。
6.如权利要求1所述的无人驾驶场景下基于动态车辆检测的点云畸变矫正方法，其特征在于，所述步骤3：
步骤3.2动态车辆刚体运动估计
对不同车辆导致的畸变分别进行矫正，即：
其中，offsetlocal为点云局部畸变补偿；vehiclek表示点云经过车辆聚合方法分割出的运动车辆；n表示运动车辆的个数；表示车辆在时间间隔/>内的运动；/>表示根据车辆vehiclei的运动对其进行的点云局部畸变矫正；
对于点云Pi中的车辆vehiclek，其在连续两帧时间间隔内的运动/>由式(19)表示：
其中，r表示车辆vehiclek对应点云数据点pi的旋转变化；t表示车辆vehiclek对应点云数据点pi的平移变化；
将式表达为仿射变换的形式，如式(20)所示：
其中，r为3×3的旋转矩阵，而t为3×1的位移矩阵，该变换矩阵总共包含六个自由度，分别为绕x，y，z轴的旋转及位移；将车辆看成刚体，则对于任意的pi∈vehiclek，均具有相同的旋转矩阵和位移矩阵；
通过坐标变换将第二帧图像点云坐标系转换为第一帧点云坐标系；坐标转换同样表示为仿射变换形式：
其中，(x1，y1，z1)和(x2，y2，z2)分别代表激光雷达采集到的连续两帧点云中第一帧数据和第二帧数据的坐标系；rl和tl则分别代表激光雷达在采集两帧数据间的旋转变换和位移变换；
通过点云配准进行运动估计由式表示：
其中，表示通过点云配准算法估计的车辆运动变换仿射矩阵；pi→CP(pj)表示在第二帧点云pnext中寻找点云pi的对应点；f(pi→CP(pj))表示根据对应点关系估计车辆的刚体运动。
7.如权利要求1所述的无人驾驶场景下基于动态车辆检测的点云畸变矫正方法，其特征在于，所述步骤3：
步骤3.3动态车辆运动区域预测
对车辆在其速度方向及垂直于其速度方向做出如下假设：车辆在其速度方向上及垂直于速度方向上的最大加速大小为10m/s2，最小加速度大小为-10m/s2；则假设车辆的速度为v0，在一帧时间间隔t后，其在速度方向及垂直于速度方向的位移可能值为：
其中，x1为车辆速度方向预测位移值，x2为车辆速度反方向预测位移值；y1和y2为垂直于车辆速度方向预测位移值，且互为反方向；a为车辆的最大加速度；
车辆范围往往不能完全覆盖车辆的真实范围，为了避免安全隐患，在预测出的车量范围上加上补偿值，在其速度方向加上车辆的长度，约3m，在垂直于速度方向上加上车辆的宽度，约2m，如式(24)所示：
其中，vehiclex，1为车辆速度方向车辆预估范围；vehiclex，2为车辆速度反方向车辆预估范围；vehicley，1和vehicley，2分别为垂直于车辆速度方向车辆预估范围，且互为反方向；offset为补偿值；
车辆在下一帧可能出现的范围表示为(x1+vehiclex，1，x2+vehiclex，2，y1+vehicley，1，y2+vehicley，2)组成的矩形框区域。
8.如权利要求1所述的无人驾驶场景下基于动态车辆检测的点云畸变矫正方法，其特征在于，所述步骤3：
步骤3.4点云局部畸变矫正
点云的局部畸变矫正表示为：
其中，为车辆vehiclek对应点云的局部畸变补偿；timei为车辆vehiclek中点云数据点pi对应的时间戳；timestart为激光雷达开始采集第一帧点云数据时的时间戳；
则经过局部畸变矫正后的点云表示为式(26)：
其中，为经局部畸变矫正后的点云；/>为经全局畸变矫正后的点云；n为点云Pi中运动车辆个数；/>为运动车辆vehiclek对应的点云数据点；
点云局部畸变矫正，其运行的算法过程如下：
步骤3.4.1初始化连续两帧点云，并通过全局畸变矫正方法矫正点云及/>后作为模型的输入；
步骤3.4.2通过点云动态车辆聚类算法分割点云并得到运动车辆集合vehicle_seti，同时给vehicle_seti中车辆对应点云编号；
步骤3.4.3通过点云动态车辆检测算法检测中运动车辆，并根据检测结果分割点云获得只包含运动车辆的点云/>
步骤3.4.4遍历车辆集合vehicle_seti，对vehicle_seti中每一车辆vehiclek，通过动态车辆刚体运动估计方法计算每一车辆的刚体变换矩阵，并将计算结果放入集合T_set中；
步骤3.4.5存在点云数据点且未被访问，转步骤3.4.6；否则，转步骤3.4.7；
步骤3.4.6判断点云pi是否属于运动车辆对应点，如果不属于，则直接将点云数据点pi放入集合如果属于，则获得对应车辆的编号，并从T_set中获取对应车辆的刚体变换矩阵；根据基于激光雷达运动解析的点云全局畸变矫正节中的编码结果，解码得到对应点的时间戳；计算得到点云的局部畸变补偿，并矫正点云pi的位置，得到点云数据点/>将点云数据点/>放入集合/>
步骤3.4.7输出经局部畸变矫正后的点云
9.如权利要求1所述的无人驾驶场景下基于动态车辆检测的点云畸变矫正方法，其特征在于，所述步骤4：
无人驾驶场景下点云畸变矫正方法(MDDC)通过基于激光雷达运动解析的全局畸变矫正方法矫正点云的全局畸变，在对点云全局畸变矫正的基础上，通过解析场景中动态车辆的运动矫正点云局部畸变，以实现无人驾驶场景下点云畸变矫正，其运行的算法过程如下：
步骤4.1输入连续的两帧点云Pk，Pk+1及激光雷达的运动速度vlidar；
步骤4.2对点云Pk，Pk+1进行全局畸变矫正得到点云
步骤4.3对点云进行去噪及下采样处理，得到数据预处理后的点云/>
步骤4.4通过点云动态车辆检测方法检测两帧点云中的动态车辆，得到预测标签lable；
步骤4.5根据动态车辆检测结果对点云中动态车辆进行运动解析，得到车辆运动信息infmotion；
步骤4.6基于车辆运动信息infmotion对点云进行局部畸变矫正，得到无畸变点云P′k；
步骤4.7输出无畸变点云P′k。
CN202310567009.XA 2023-05-18 2023-05-18 无人驾驶场景下基于动态车辆检测的点云畸变矫正方法 Pending CN116721337A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310567009.XA CN116721337A (zh) 2023-05-18 2023-05-18 无人驾驶场景下基于动态车辆检测的点云畸变矫正方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310567009.XA CN116721337A (zh) 2023-05-18 2023-05-18 无人驾驶场景下基于动态车辆检测的点云畸变矫正方法

Publications (1)

Publication Number Publication Date
CN116721337A true CN116721337A (zh) 2023-09-08

Family

ID=87868812

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310567009.XA Pending CN116721337A (zh) 2023-05-18 2023-05-18 无人驾驶场景下基于动态车辆检测的点云畸变矫正方法

Country Status (1)

Country Link
CN (1) CN116721337A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117168472A (zh) * 2023-10-31 2023-12-05 北京理工大学前沿技术研究院 无人驾驶车辆的重定位方法、系统、存储介质及设备
CN117670162A (zh) * 2023-12-06 2024-03-08 珠海市格努信息技术有限公司 一种场内智能物流解决方法
CN118196762A (zh) * 2024-05-20 2024-06-14 上海几何伙伴智能驾驶有限公司 三维占据任务动静态真值稠密生成方法

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117168472A (zh) * 2023-10-31 2023-12-05 北京理工大学前沿技术研究院 无人驾驶车辆的重定位方法、系统、存储介质及设备
CN117168472B (zh) * 2023-10-31 2024-02-13 北京理工大学前沿技术研究院 无人驾驶车辆的重定位方法、系统、存储介质及设备
CN117670162A (zh) * 2023-12-06 2024-03-08 珠海市格努信息技术有限公司 一种场内智能物流解决方法
CN118196762A (zh) * 2024-05-20 2024-06-14 上海几何伙伴智能驾驶有限公司 三维占据任务动静态真值稠密生成方法

Similar Documents

Publication Publication Date Title
CN110569704B (zh) 一种基于立体视觉的多策略自适应车道线检测方法
CN110223348B (zh) 基于rgb-d相机的机器人场景自适应位姿估计方法
CN116721337A (zh) 无人驾驶场景下基于动态车辆检测的点云畸变矫正方法
CN109242884B (zh) 基于JCFNet网络的遥感视频目标跟踪方法
Pantilie et al. SORT-SGM: Subpixel optimized real-time semiglobal matching for intelligent vehicles
KR20160123668A (ko) 무인자동주차 기능 지원을 위한 장애물 및 주차구획 인식 장치 및 그 방법
CN110021029B (zh) 一种适用于rgbd-slam的实时动态配准方法及存储介质
CN114495064A (zh) 一种基于单目深度估计的车辆周围障碍物预警方法
Zhou et al. Lane information extraction for high definition maps using crowdsourced data
CN117422971A (zh) 一种基于跨模态注意力机制融合的双模态目标检测方法与系统
CN114299405A (zh) 一种无人机图像实时目标检测方法
CN116310673A (zh) 一种基于点云与图像特征融合的三维目标检测方法
CN112270694B (zh) 基于激光雷达扫描图进行城市环境动态目标检测的方法
CN114577196B (zh) 使用光流的激光雷达定位
CN113221739B (zh) 基于单目视觉的车距测量方法
CN112446885B (zh) 一种动态环境下基于改进的语义光流法的slam方法
Cheng et al. G-Fusion: LiDAR and Camera Feature Fusion on the Ground Voxel Space
Gählert et al. Single-shot 3d detection of vehicles from monocular rgb images via geometry constrained keypoints in real-time
CN113076988A (zh) 基于神经网络的移动机器人视觉slam关键帧自适应筛选方法
Chen et al. Amobile system combining laser scanners and cameras for urban spatial objects extraction
CN117409386A (zh) 一种基于激光视觉融合的垃圾定位方法
CN113837243B (zh) 基于边缘信息的rgb-d相机动态视觉里程计方法
Zeng et al. The monocular ranging based on stable target detection and tracking for intelligent vehicle
Dang et al. Fast object hypotheses generation using 3D position and 3D motion
CN115170826A (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