CN116879870A - 一种适用于低线束3d激光雷达的动态障碍物去除方法 - Google Patents

一种适用于低线束3d激光雷达的动态障碍物去除方法 Download PDF

Info

Publication number
CN116879870A
CN116879870A CN202310678823.9A CN202310678823A CN116879870A CN 116879870 A CN116879870 A CN 116879870A CN 202310678823 A CN202310678823 A CN 202310678823A CN 116879870 A CN116879870 A CN 116879870A
Authority
CN
China
Prior art keywords
point cloud
map
resolution
point
frame
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
CN202310678823.9A
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.)
Shenzhen Wanzhida Technology Co ltd
Original Assignee
Harbin University of Science and 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 Harbin University of Science and Technology filed Critical Harbin University of Science and Technology
Priority to CN202310678823.9A priority Critical patent/CN116879870A/zh
Publication of CN116879870A publication Critical patent/CN116879870A/zh
Pending legal-status Critical Current

Links

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/483—Details of pulse systems
    • G01S7/486—Receivers
    • G01S7/487—Extracting wanted echo signals, e.g. pulse detection
    • 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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3837—Data obtained from a single source
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • 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/491—Details of non-pulse systems
    • G01S7/493—Extracting wanted echo signals
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T19/00—Manipulating 3D models or images for computer graphics
    • G06T19/20—Editing of 3D images, e.g. changing shapes or colours, aligning objects or positioning parts
    • 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)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Automation & Control Theory (AREA)
  • Architecture (AREA)
  • Computer Graphics (AREA)
  • Computer Hardware Design (AREA)
  • General Engineering & Computer Science (AREA)
  • Software Systems (AREA)
  • Theoretical Computer Science (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

针对移动机器人在室外环境中，因行人、车辆等动态物体的存在，导致激光SLAM生成的地图中存在动态物体的轨迹残影，影响后续的定位及路径规划等问题，本发明提出了一种适用于低线束3D激光雷达的动态障碍物去除方法。该方法包括以下步骤：首先对激光雷达采集到的三维点云数据进行预处理；然后进行地面分割，并将非地面点云地图投影为深度图，通过视点可见法对比与查询帧之间可见性来去除轨迹残影；考虑到点云具有距离越远越稀疏的特性，根据距离信息来自适应调节深度图的分辨率，并通过不断减小分辨率来恢复被误删的静态点。经实验证明，该方法可以在低线束的3D激光雷达上高效地去除动态障碍物，保留完整的静态地图，减小动态障碍物对SLAM系统的影响。

Description

一种适用于低线束3D激光雷达的动态障碍物去除方法
(一)技术领域
本发明属于移动机器人定位与建图领域，尤其涉及一种适用于低线束3D激光雷达的动态障碍物去除方法。
(二)背景技术
随着智能感知和智能控制技术的发展，移动机器人自主能力也随之得到提高，并且在日常生产、智慧物流和救援救灾等各个领域得到广泛应用。对周围环境的感知是移动机器人研发领域中最重要的组成部分，其核心就包括实时定位与地图构建技术(Simultaneous Localization And Mapping，SLAM)。按照移动机器人所搭载的主要环境感知传感器，可以将SLAM技术分为基于相机的视觉SLAM和基于激光雷达的激光SLAM。然而在室外环境中，由于相机容易受到光线的影响，难以在黑暗等光线受到严重干扰的场景中进行有效的环境感知，并且相机无法获取到准确的三维信息，不利于室外大场景地图的构建；而激光雷达不受环境光线的影响，能够获取厘米级的距离信息，在室外环境中的定位与建图和环境感知等任务中表现较好。
目前大多数已有的激光SLAM方法，基本上都是在基于室内且已定的应用场景下，当面对室外未知动态场景与实时移动的动态障碍物(如移动的车辆和行人等)时，会导致在最终生成的点云地图中留下其移动的轨迹，机器人不能够有效的实现动态场景下的定位与建图。动态物体的存在增加了里程计配准的不确定性，导致姿态估计精度下降；同时，点云数据中包含的移动物体的点云会被添加到最终生成的环境地图中，在地图上留下其移动的轨迹残影，被称为“鬼影”，这些动态物体的“鬼影”会被当成障碍物，并遮挡道路交通标记和其他重要的静态特征使，所建地图与真实环境出现偏差，导致不能构建准确的环境地图，影响后续的回环检测以及导航等工作。因此，去除环境中动态障碍物引起的“鬼影”，构建准确的静态地图对移动机器人是十分必要的。
针对上述问题，文献《Moving Object Segmentation in 3D LiDAR Data:ALearning-based Approach Exploiting Sequential Data》，提出了一种基于CNN的新方法，该方法使用3D LiDAR点云生成的range图像以及过去扫描生成的残差图像作为输入，输出中的标签主要区分是否属于动态物体。然而基于学习的方法，在训练好的环境下，表现良好，但通常需要很大成本的数据训练，而且对硬件设备要求较高，在不同的环境或不同的激光雷达传感器中不能有效的推广使用。
文献《Remove,then Revert:Static Point cloud Map Construction usingMultiresolution Range Images》提出了一种基于视点可见性的方法，将点云地图投影成深度图，通过对比查询帧与点云地图之间的可见性来去除动态点云，然后通过减小深度图分辨率来恢复误删的静态点。然而，这种方法是利用激光雷达的可视范围投影成深度图，当使用低线束的激光雷达时，由于在竖直方向上的线束较少，难以从稀疏的点云中获取有用的信息，生成的高分辨率深度图容易失帧，在低线束的激光雷达上动态物体的去除效果不佳。
(三)发明内容
针对现有方法的不足，本发明提供一种适用于低线束(如16线)3D激光雷达的动态障碍物去除方法，目的是为了解决传统激光SLAM在面对室外动态环境时，由于动态障碍物的存在对SLAM系统所带来的影响。首先对激光点云数据进行去畸变和滤波处理；然后再对处理后的点云数据进行地面分割，在3D激光雷达所感知到的点云数据中有很大一部分点属于地面，这些地面点是静止的，在进行动态点云去除时，可以预先将所有的地面点设置为“静态”，以减少假动态点云的数量，提高运算效率；另外，大多数移动的物体，如车辆和人都与地面相连的，良好的地面分割可以在很大程度上降低后续步骤中动态目标去除的难度；对分割好的地面点云，利用SLAM方法拼接成点云地图，并将非地面点云地图投影成深度图，通过对比查询帧与点云地图所投影的深度图的可见性来去除动态物体；最后通过调整深度图的分辨率从动态点云中恢复被误删的静态点云，并根据恢复静态点云的数量占比，来判断是否继续降低分辨率。
本发明通过对点云数据进行地面分割，只对非地面点进行投影，并结合激光雷达的距离信息，提高了在低线束的3D激光雷达上对动态物体的去除效果，减少了误删地面点的情况，并提高了运行效率；另外，考虑到当使用低分辨率的3D激光雷达时，由于其在竖直方向上的线束少，稀疏的点云包含的有用信息较少，在生成深度图后容易失帧，会严重影响算法输出质量，所以根据激光雷达距离越远点云越稀疏，尤其是在低线束的3D激光雷达上表现更为明显的特点，将激光雷达重要的距离信息设置为调节深度图分辨率的权重，使得距离激光雷达较近位置处的图像有着更加精细的分辨率，而距离较远的位置，其分辨率则低，使得在低线束3D激光雷达上能够高效的去除动态障碍物所造成的“鬼影”，使用低线束的3D激光雷达，使得建立低成本的无人系统成为了可能。一种适用于低线束3D激光雷达的动态障碍物去除方法包括以下步骤：
S1、移动机器人通过自身搭载的3D激光雷达采集周围环境的三维点云数据，并对采集到的点云数据进行去畸变和滤波处理；
S2、将处理后的激光点云进行地面分割，分为地面点云和非地面点云；
S3、将分割后的点云，构建局部地图，进行全局优化，生成具有全局一致性的点云地图，并得到机器人的位姿信息；
S4、将生成的非地面点云地图投影成深度图。
S5、选取非地面点云中的连续N次扫描，作为查询帧，根据激光雷达距离信息调节查询帧所对应深度图的分辨率，通过对比查询帧与局部地图在同一分辨率下深度图的可见性，去除点云地图中所有的可疑动态点。
S6、通过不断降低点云地图和查询帧所投影的深度图的分辨率，将之前误删的静态点恢复到静态地图中，根据恢复静态点数量的比例，来决定是否继续降低分辨率。
所述的S1中，移动机器人通过自身搭载的3D激光雷达采集周围环境的三维点云数据，并对采集到的点云数据去畸变和滤波等预处理，具体包括以下步骤：
当移动机器人发生位姿变化时，由于激光雷达发射与接收之间存在一定延时，导致激光雷达中心与探测到的障碍物间存在相对位置的变化，这会造成一帧点云内的所有点并不是在同一位置采集的，采集到的点云会失真。因此，需要考虑移动机器人在一个采集周期的自运动，消除每帧点云中存在的运动畸变。
假设tk-1时刻激光雷达开始第k次扫描，扫描到tk时刻结束，这段时间内采集到的点云用Pk表示，所谓修正运动畸变就是要将Pk中所有点云都投影到tk时刻激光雷达坐标系下，得到矫正畸变后的点云。首先，对点云进行有序化处理，可以根据下式计算得到激光点的垂直角度β和水平角度α：
结合激光雷达的水平和垂直角分辨率就能得到每个点所属的行数、列数，就可以将点云按采集顺序进行排序。
由于激光雷达采集两帧点云数据的间隔非常短，因此近似认为在这段时间内移动机器人的速度是不变的。令T(t)为任意t时刻移动机器人相对于初始位置的位姿变换，T(tk)为当前帧扫描结束时的机器人位姿(tk为第k次扫描结束时刻)。在tk时刻，T(tm)(m＜k)是已知的，于是可以根据过去的位姿估算出当前时刻的车辆位姿T(tk)；
在已知tk-1、tk时刻的位姿后，任意ti(ti∈(tk-1,tk))时刻采集到的点i(i∈Pk)对应的位姿T(ti)可以由线性插值得到，于是点i投影到tk时刻雷达坐标系下的坐标X('k,i)计算公式为：
将Pk内的所有点都按上式投影到tk时刻雷达坐标系下即得到消除运动畸变后的第k帧点云Pk'。
体素栅格滤波可以在减少数据量的同时不破坏点云数据本身的几何结构，根据输入点云的大小，确定点云在X、Y、Z坐标轴上的最大值和最小值，分别记为xmax、ymax、zmax、xmin、ymin、zmin。根据最大值和最小值确定最小包围网格的大小，长宽高分别为lx、ly、lz，如下式所示：
设置体素栅格的边长R，同时将X、Y、Z坐标轴各自划分为M、N、L份，计算栅格的数量S：
S＝M*N*L (4)
计算每个点的栅格编号hx、hy、hz，确定点云落在哪个栅格中，并计算体素栅格中的质心(cx、cy、cz)，以质心代表栅格内的所有点，质心的表示为：
式中，n为一个栅格中点云的数量。
当使用低线束的3D激光雷达时，由于点云数据本身较为稀疏，因此采用较小栅格大小以保证点云中的几何结构不被丢失。
所述的S2中，将处理后的激光点云进行地面分割，分为地面点云和非地面点云，具体包括以下步骤：
若低线束的激光雷达为16线，则其垂直分辨率为-15°～+15°，只有在水平方向以下的激光线束能够扫描到地面信息。激光雷达在t时刻获得一簇点云Pt＝{p1,p2,.....pn}，其中pi表示点云Pt中的一个激光点；激光雷达的水平分辨率为res1，垂直方向线数为16，则投影的分辨率为360/res1×16，即投影深度图的行数r由激光雷达的线数决定，列数c由发射点云的水平分辨率决定，每个像素中储存的数据是激光雷达探测物的欧式距离τi＝||pi||。
假设相邻激光线束打在障碍物上的点为PA、PB、PC，x,y,z为激光点在空间上的坐标，则垂直方向上相邻两点之间的角度α，可以用三角法则计算得到：
式中，Pi(z)(i＝B、C)代表点云在z轴方向的坐标，Δx和Δy的计算公式如下：
式中，Pi(x)(i＝A、B、C)代表点云在x轴方向的坐标，Pi(y)(i＝A、B、C)表示点云在y轴方向的坐标。
因为16线激光雷达的扫描范围为[-15°,15°]，可知地面点必然在[-15°,-1°]范围内，在该范围内，从下到上N根范围内提取地面点，设置角度阈值Δα和一个线束阈值，若在规定范围内小于角度阈值Δα，则标记为地面点。
所述的S3中，将分割后的点云，构建局部地图，进行全局优化，生成具有全局一致性的点云地图，并得到机器人的位姿信息，具体包括以下步骤：
局部地图的构建过程就是求解激光雷达扫描一周数据所形成的扫描帧相对于局部地图的位姿变换Tξ，转换过程如下式所示：
式中，P表示激光雷达扫描到的一个激光点云；表示旋转矩阵；ξx,ξy,ξz表示x,y,z三个方向的平移量，也就是相对于局部地图原点的坐标差值；
将扫描帧插入到局部地图中，局部地图将生成概率网格地图：
M:rZ×rZ→[pmin,pmax] (9)
式中，M表示概率地图；rZ表示概率网格中每个格子的大小；pmin和pmax表示概率值的范围；
求取扫描帧相对局部地图的位姿可视作一个非线性最小二乘问题，如下式所示：
式中，Ms为平滑函数；hk表示为点在查询帧坐标系中的坐标；Tξ表示为查询帧坐标系相对局部地图坐标系位姿；
将完成地面分割的点云进行迭代增量式的全局地图构建，通过第k-1帧特征点云Dk-1与第k帧特征点云Dk的位姿变换关系计算第k帧局部子地图设第k-1帧特征点云Dk-1所对应投影在全局坐标系下的局部子图为/>通过点云的帧间匹配，可得到第k帧特征点云与第k-1帧点云间的位姿变换矩阵/>依据该位姿变换矩阵可以将当前帧特征点云Dk投影在全局坐标系下，获得第k帧局部子地图的估计值/>即：
将当前帧与上一帧局部子地图进行帧间匹配可获得观测值将估计值与观测值间的误差项实时作为输出用图优化的方法进行求解。将当前优化后的局部子地图与上一帧的局部子地图/>重合部分特征点云作为当前帧全局子地图，放入历史全局子地图所在的kd-tree中，构建全局地图/>同时，将当前局部子地图/>放入kd-tree中用于下一帧特征点云执行Scan to Map操作。
所述的S4中，将生成的非地面点云地图投影成深度图，具体包括以下步骤：
为了通过对比点云地图和查询帧所投影的深度图的可见性，来去除动态物体；首先要将点云地图投影成深度图，投影大尺寸地图PM，得到一个固定大小的深度图。在投影过程中，深度图的分辨率其物理意义为水平方向和竖直方向上的角度分辨率，当为低线束的16线激光雷达时，其水平视角为360°，竖直方向的视角为30°(-15°～15°)，分辨率为1°，即单个像素表示1°水平视场和垂直视场，所以深度图的大小就是360×30。
深度图中的每个像素值都是地图点云中在某个视角下的可见点云，通过激光雷达的视场，可以将点云PM分割为多个可见地图点云其表达式为：
式中，表示在点云集合/>中距离像素点最近的点云集合。
定义可见地图点云的等效深度图为/>
式中，表示在第k帧视角下，第i行j列像素的投影强度值；m表示水平方向像素数量，由水平方向的视场角除以水平方向分辨率得出；n表示垂直方向像素数量，由垂直方向的视场角除以垂直方向分辨率得出；
则投影成二维的深度图像为/>其中(x,y)表示图像中的像素坐标，其对应的距离值可以通过下式计算：
式中，N表示激光雷达扫描的线数，di(x,y)表示第i条线上(x,y)处的距离值，表示了在某个像素(x,y)处，取所有扫描线中最近的一个距离值作为该像素的距离值。
所述的S5中，选取非地面点云中的连续N次扫描，作为查询帧，根据激光雷达距离信息调节查询帧所对应深度图的分辨率，通过对比查询帧与局部地图在同一分辨率下深度图的可见性，去除点云地图中所有的可疑动态点，具体包括以下步骤：
在点云数据中选取N次扫描组成查询帧PQ，进行批量的动态点去除(Batchdynamic point remove，BR)。根据第k帧查询帧的相对位姿将查询帧投影到局部地图同一坐标系下，并将查询帧与局部地图的深度图设置为同一分辨率。在设置深度图的分辨率时，使用激光雷达的距离信息，将其作为自适应调节深度图分辨率的权重，使得在不同距离区间的深度图具有不同大小的分辨率。
具体地，令di,j表示激光雷达在第i次扫描中第j个激光点的距离值，那么可以定义权重wi,j为：
接着，定义分辨率层数L，将距离范围划分为L个区间其中1≤l≤L，和/>分别表示第l层的距离范围。对于第l层像素(i,j)，其对应的像素值ri,j表示如下：
式中，表示深度图中像素(i,j)在第l层的权重，计算公式如下：
通过上述公式，当像素(i,j)的距离值di,j落在区间内时，它在第l层的权重为1/di,j，即距离越近，权重越大，其对应的分辨率也越大；反之距离越远，对应的分辨率也就越低。当距离在该层范围之外的点，其对应的权重为0，即不参与该层的深度图构建。
则查询帧所对应的深度图/>上的每一个点的值为/>对于点云地图/>设置相同分辨率的深度图/>其中每一个点的值为/>这样就可以根据不同距离区间的权重，来调节深度图的分辨率，使得距离激光雷达较近的位置，深度图的分辨率更细。
将局部地图和查询帧所对应的深度图调整到最大分辨率，并进行矩阵减法，可得：
对于任一点如果其对应的/>值大于设定的阈值τD，则认为该点属于动态点，将其添加进/>中，该过程为：
通过动态子集，可以计算获得与其互斥的静态子集
根据机器人运动方向的连续N个扫描，对于每个扫描将其依次与pM进行比较，并统计每一个点被标记为静态与动态点的次数，记为nS和nD，在完成所有比较后，可以得到每一个点的评价分数s(pM)，其计算公式为：
s(pM)＝αSnS+αDnD (20)
式中，αS为静态权重系数，为正值；αD为动态权重系数，为负值。
由此可以得到动态点云子集为：
所述的S6中，通过不断降低点云地图和查询帧所投影的深度图的分辨率，将之前误删的静态点恢复到静态地图中，根据恢复静态点数量的比例，来决定是否继续降低分辨率，具体包括以下步骤：
经过动态点云的去除，可以将点云集PM划分为两个互斥的子集：静态点云子集PSM和动态点云子集PDM，表示为：
在高分辨率的深度图中，容易出现误删除物体的边界或其他的一些静态点的情况，当降低深度图的分辨率后，误删的情况会明显减少。所以在通过对比最高分辨率下深度图的可见性后，通过不断减小深度图的分辨率，将被误删的静态点从动态点云子集PDM中加入到静态点云子集PSM中。
假设在最高分辨率下进行第一轮的静态点恢复中，恢复的静态点数量为n0，在进行第i(i＞1)轮恢复中，恢复的静态点数量为ni，则在第i轮中恢复的静态点数量占第一轮的比例，即第i轮的恢复率为在第i轮恢复中，深度图的分辨率为ri。如果恢复率pi大于预设阈值α，即本轮恢复的静态点数量占第一轮恢复静态点数量的比例大于预设阈值，则表示在动态点云中还含有大量的静态点，需根据可见性信息继续降点分辨率，进行下一轮迭代，以达到更好的恢复效果；反之，当恢复率pi小于阈值α时，则表示在动态点云中基本不含有静态点云，应停止降低分辨率，避免浪费计算资源。
本发明具有如下有益效果：
1.本发明通过对激光雷达获取到的点云进行去畸变和滤波处理，避免了因运动畸变而导致的位姿精度差的问题，去除了点云中部分无用信息，提高了点云数据的质量，精确的位姿可以减小投影偏差，进而避免造成静态点云的误删和动态点云的漏删。
2.由于地面点是静止的，本发明通过对点云数据进行地面分割，在进行动态点云的去除时，可以将所有地面点设为“静态”，减少了假动态点云的数量，提高了算法的鲁棒性和运行效率；同时，避免了当激光雷达的入射光线贴近地面时，部分地面点会被当做动态物体而被“误删”的情况。
3.本发明通过将点云地图投影成深度图，与查询帧所投影的深度图通过视点可见性的对比，检测并清除由于移动物体的轨迹残影所形成的“鬼影”；并根据激光雷达的距离信息，设置权重，来自适应调整深度图的分辨率，使得距离激光雷达较近位置的深度图分辨率高，较远位置深度图分辨率相对更低；使得在低线束3D激光雷达上能够高效的检测并去除动态障碍物所造成的“鬼影”，消除了移动物体对移动机器人定位及路径规划所带来的影响。
(四)附图说明
图1一种适用于低线束3D激光雷达的动态障碍物去除方法流程图；
图2一种适用于低线束3D激光雷达的动态障碍物去除方法框图；
图3含有动态障碍物轨迹残影的三维点云效果图；
图4局部点云地图所投影得到的深度图
图5查询帧所投影得到的深度图
图6局部地图与查询帧所形成的视差图
图7使用原方法去除动态障碍物轨迹残影效果图；
图8使用本发明方法去除动态障碍物轨迹残影效果图。
(五)具体实施方式
下面结合附图及具体实施方式对本发明做进一步描述。
本发明提供一种适用于低线束3D激光雷达的动态障碍物去除方法，如图1所示，包括以下步骤：
S1、移动机器人通过自身搭载的3D激光雷达采集周围环境的三维点云数据，并对采集到的点云数据进行去畸变和滤波处理；
S2、将预处理后的激光点云进行地面分割，分为地面点云和非地面点云；
S3、将分割后的点云，构建局部地图，进行全局优化，生成具有全局一致性的点云地图，并得到机器人的位姿信息；
S4、将生成的非地面点云地图投影成深度图，并根据激光雷达的距离信息，自适应调节深度图的分辨率；
S5、选取非地面点云中的连续N次扫描，作为查询帧，将点云地图转换到查询帧的坐标系下，通过对比查询帧与局部地图在同一分辨率下深度图的可见性，去除点云地图中所有的可疑动态点；
S6、通过不断降低点云地图和查询帧所投影的深度图的分辨率，将之前误删的静态点恢复到静态地图中，根据恢复静态点数量的比例，作为是否继续降低分辨率的判断条件；
所述的S1中，移动机器人通过自身搭载的3D激光雷达采集周围环境的三维点云数据，并对采集到的点云数据去畸变和滤波等预处理，具体包括以下步骤：
当移动机器人发生位姿变化时，由于激光雷达发射与接收之间存在一定延时，导致激光雷达中心与探测到的障碍物间存在相对位置的变化，对应的一系列激光束所表征的环境轮廓几何特征发生畸变。在激光雷达计算激光点坐标时，都是以接收到激光束的时刻，激光雷达自身坐标系为基础，所以在移动机器人运动过程中，每一列激光点的基准坐标系都是不一样的，这会造成一帧点云内的所有点并不是在同一位置采集的，采集到的点云会失真。因此，需要考虑移动机器人在一个采集周期的自运动，将同一帧点云统一在同一个坐标系下，消除点云中存在的运动畸变。
假设tk-1时刻激光雷达开始第k次扫描，扫描到tk时刻结束，这段时间内采集到的点云用Pk表示，所谓修正运动畸变就是要将Pk中所有点云都投影到tk时刻激光雷达坐标系下，得到矫正畸变后的点云Pk'。
由于采集到的是无序点云，对点云数据进行处理和计算时，只知道每个点的坐标，不知道点的采集顺序，所以需要先计算每个点所处的线束和在线束中的位置。可以根据下式计算得到激光点p(x,y,z)的垂直角度β和水平角度α：
再结合激光雷达的水平和垂直角分辨率可以得到每个点所属的行数、列数，就可以将点云按采集顺序进行排序。
激光雷达采集两帧点云数据的时间间隔非常短，对于10Hz的激光雷达，大约在0.1s，因此可以近似认为在这段时间内，移动机器人的速度是不变的。令T(t)为任意t时刻，移动机器人相对于初始位置的位姿变换，包含激光雷达的六自由度运动，T(tk)为当前帧扫描结束时的机器人位姿(tk为第k次扫描结束时刻)。在tk时刻，T(tm)(m＜k)是已知的，于是可以根据过去的位姿估算出当前时刻的车辆位姿T(tk)为：
T(tk)＝T(tk-1)·T(tk-2)-1·T(tk-1) (2)
在已知tk-1、tk时刻的位姿后，任意ti(ti∈(tk-1,tk))时刻采集到的点i(i∈Pk)对应的位姿T(ti)可以由线性插值得到，于是点i投影到tk时刻雷达坐标系下的坐标X('k,i)计算公式为：
将Pk内的所有点都按照上式投影到tk时刻雷达坐标系下，即得到消除运动畸变后的第k帧点云Pk'。
当激光雷达对周围环境进行扫描时，在短时间内会采集到大量的点云数据，然而这些数据中会存在冗余数据，影响系统效率，需要对采集到的点云进行滤波处理。体素栅格滤波可以在较少数据量的同时，不破坏点云数据本身的几何结构，其基本原理是在三维点云数据中，创建一个个的点位体素栅格，并将栅格中的所有点的质心进行近似，以此降低数据量。
根据输入点云的大小，确定点云在X、Y、Z坐标轴上的最大值和最小值，分别记为xmax、ymax、zmax、xmin、ymin、zmin。其中：
根据上式中的最大和最小值，确定最小包围网格的大小，长宽高分别为lx、ly、lz，如下式所示：
设置体素栅格的边长为R，同时将X、Y、Z坐标轴各自划分为M、N、L份，计算栅格的数量S：
S＝M*N*L (6)
其中，M、N、L分别由下式表示：
式中，表示向下取整，即选取这个数值最近且小于它的整数。
计算每个点的栅格编号hx、hy、hz，确定点云落在哪个栅格中，如下式所示：
计算体素栅格中的质心(cx、cy、cz)，以质心代表栅格内的所有点，质心的表示为：
式中，n为一个栅格中点云的数量。
体素栅格滤波通过改变栅格的大小决定滤波的效率和精度，网格越大，滤波效果更明显，但是更容易丢失空间中的几何特征，而栅格越小，滤波精度越高，但运行速度也会变慢，因此选取一个合适的栅格大小是关键。当使用较低分辨率的3D激光雷达时，由于点云数据本身较为稀疏，因此适合采用较小栅格大小，以保证点云中的几何结构不被丢失。
所述的S2中，将预处理后的激光点云进行地面分割，分为地面点云和非地面点云，具体包括以下步骤：
在点云数据中有很大一部分点云属于地面点云，并呈现出一定的纹理状，这会对后续的点云处理流程产生影响。一方面，地面点云会对位于地面上的物体点云造成干扰，降低算法的准确性和鲁棒性；另一方面，当激光雷达的入射光线贴近地面时，会出现部分地面点会被当做动态物体而“误删”的情况，以此可以减少假动态点的数量。
假设低线束激光雷达为16线，则其垂直分辨率为-15°～+15°，只有在水平方向以下的激光线束能够扫描到地面信息。激光雷达在t时刻获得一簇点云Pt＝{p1,p2,.....pn}，其中pi表示点云Pt中的一个激光点；激光雷达的水平分辨率为res1，垂直方向线束为16，则投影的分辨率为360/res1×16，即投影深度图的行数r由激光雷达的线数决定，列数c由发射点云的水平分辨率决定，每个像素中储存的数据是激光雷达探测物的欧式距离τi＝||pi||。
假设相邻激光线束打在障碍物上的点为PA、PB、PC，x,y,z为激光点在空间上的坐标，则垂直方向上相邻两点之间的角度α，可以用三角法则计算得到：
式中，Pi(z)(i＝B、C)代表点云在z轴方向的坐标，Δx和Δy的计算公式如下：
式中，Pi(x)(i＝A、B、C)代表点云在x轴方向的坐标，Pi(y)(i＝A、B、C)表示点云在y轴方向的坐标。
因为16线激光雷达的扫描范围为[-15°,15°]，可知地面点必然在[-15°,-1°]范围内，在该范围内，从下到上N根范围内提取地面点，设置角度阈值Δα和一个线束阈值，若在规定范围内小于角度阈值Δα，则标记为地面点。
所述的S3中，将分割后的点云，构建局部地图，进行全局优化，生成具有全局一致性的地图，并得到机器人的位姿信息，具体包括以下步骤：
局部地图构建过程就是求解激光雷达扫描一周数据所形成的扫描帧相对于局部地图的位姿变换Tξ，转换过程为如下式所示：
式中，P表示激光雷达扫描到的一个激光点云；表示旋转矩阵；ξx,ξy,ξz表示x,y,z三个方向的平移量，也就是相对于局部地图原点的坐标差值；
扫描帧插入到局部地图中，局部地图将生成概率网格地图：
M:rZ×rZ→[pmin,pmax] (13)
式中，M表示概率地图；rZ表示概率网格中每个格子的大小；pmin和pmax表示概率值的范围；
求取扫描帧相对局部地图的位姿可视作一个非线性最小二乘问题，如下式所示：
式中，Ms为平滑函数；hk表示为点在查询帧坐标系中的坐标；Tξ表示为查询帧坐标系相对局部地图坐标系位姿；
将完成地面分割的点云进行迭代增量式的全局地图构建，通过第k-1帧特征点云Dk-1与第k帧特征点云Dk的位姿变换关系计算第k帧局部子地图设第k-1帧特征点云Dk-1所对应投影在全局坐标系下的局部子图为/>通过点云的帧间匹配，可得到第k帧特征点云与第k-1帧点云间的位姿变换矩阵/>依据该位姿变换矩阵可以将当前帧特征点云Dk投影在全局坐标系下，获得第k帧局部子地图的估计值/>即：/>
将当前帧与上一帧局部子地图进行帧间匹配可获得观测值将估计值与观测值间的误差项实时作为输出用图优化的方法进行求解。将当前优化后的局部子地图与上一帧的局部子地图/>重合部分特征点云作为当前帧全局子地图，放入历史全局子地图所在的kd-tree中，构建全局地图/>同时，将当前局部子地图/>放入kd-tree中用于下一帧特征点云执行Scan to Map操作。其建图效果如图3所示，图中道路中间阴影部分即为移动物体留下的轨迹残影。
所述的S4中，将生成的非地面点云地图投影成深度图，具体包括以下步骤：
对于非地面点云，可以将其360°的环境展开转化成一张平面的深度图像，其转化后的深度图本质上是一个二维矩阵，矩阵的行数对应激光点云在水平方向上的角度范围，即0°～360°；矩阵的列数对应激光雷达在竖直方向的角度范围(其最大和最小值与激光雷达的视场匹配)，矩阵中每个元素值则对应具体水平角度和竖直角度方向上的点云深度信息。
为了通过对比点云地图和查询帧所投影的深度图的可见性，来去除动态物体；首先要将点云地图投影成深度图，投影大尺寸地图PM，得到一个固定大小的深度图。在投影过程中，深度图的分辨率其物理意义为水平方向和竖直方向上的角度分辨率，当为低线束的16线激光雷达时，其水平视角为360°，竖直方向的视角为30°(-15°～15°)，分辨率为1°，即单个像素表示1°水平视场和垂直视场，所以深度图的大小就是360×30。
在将点云地图投影成深度图时，使用特定分辨率的深度图，这种视场受限的采样地图点云称为“可见地图点云”。深度图中的每个像素值都是地图点云中在某个视角下的可见点云，通过激光雷达的视场，可以将点云PM分割为多个可见地图点云其表达式为：
式中，表示在点云集合/>中距离像素点最近的点云集合。
定义可见地图点云的等效深度图为/>
式中，表示在第k帧视角下，第i行j列像素的投影强度值；m表示水平方向像素数量，由水平方向的视场角除以水平方向分辨率得出；n表示垂直方向像素数量，由垂直方向的视场角除以垂直方向分辨率得出。
深度图中每一点的值，可以通过下式计算：
式中，N表示激光雷达扫描的线束，di(x,y)表示第i条线上(x,y)处的距离值，其含义为，在某个像素(x,y)处，取所有扫描线中最近的一个距离值作为该像素的值。
所述的S5中，选取非地面点云中的连续N次扫描，作为查询帧，将点云地图转换到查询帧的坐标系下，通过对比查询帧与局部地图在同一分辨率下深度图的可见性，去除点云地图中所有的可疑动态点，具体包括以下步骤：
为了实现对动态障碍物的去除，在点云数据中选取N次扫描组成查询帧PQ，进行批量的动态点去除(Batch dynamic point remove，BR)。根据第k帧查询帧的相对位姿将查询帧投影到局部地图/>同一坐标系下，并将查询帧与局部地图的深度图设置为同一分辨率。
在设置深度图的分辨率时，考虑到激光雷达具有越远点云越稀疏的特性，在距离激光雷达较远的位置，稀疏的点云无法反映出真实的环境信息，尤其在低线束激光雷达的激光雷达中表现更为明显。所以在设置深度图的分辨率时，使用激光雷达的距离信息，将其作为自适应调节深度图分辨率的权重，使得在不同距离区间的深度图具有不同大小的分辨率。
具体地，令di,j表示激光雷达在第i次扫描中第j个激光点的距离值，那么可以定义权重wi,j为：
接着，定义分辨率层数L，将距离范围划分为L个区间其中1≤l≤L，和/>分别表示第l层的距离范围。对于第l层像素(i,j)，其对应的像素值ri,j表示如下：
式中，表示深度图中像素(i,j)在第l层的权重，计算公式如下：
通过上述公式，当像素(i,j)的距离值di,j落在区间内时，它在第l层的权重为1/di,j，即距离越近，权重越大，其对应的分辨率也越大；反之距离越远，对应的分辨率也就越低。当距离在该层范围之外的点，其对应的权重为0，即不参与该层的深度图构建。
则查询帧所对应的深度图/>上的每一个点的值/>其计算公式为：
同理，对于点云地图设置相同分辨率的深度图/>其中每一个点的值为这样就可以根据不同距离区间的权重，来调节深度图的分辨率，使得距离激光雷达较近的位置，深度图的分辨率更细，局部地图和查询帧所投影的深度图如图4、图5所示。
如图6所示，将局部地图和查询帧所对应的深度图，调整到初始分辨率，即最大分辨率，并进行矩阵减法，将两幅深度图逐像素相减，得到这两幅深度图的视差图/>
当视差图中某一像素的像素值大于阈值时，对应的当前激光点云的深度图的像素所对应的点云为动态点，将确定的动态点从当前激光点云中去除，即对于任一点如果其对应的/>值大于设定的阈值τD，则认为该点属于动态点，将其添加进/>中，该过程为：
通过动态子集，可以计算获得与其互斥的静态子集
根据机器人运动方向的连续N个扫描，对于每个扫描将其依次通过深度图的可见性与pM进行比较，并统计每一个点被标记为静态与动态点的次数，记为nS和nD，在完成所有比较后，可以得到每一个点的评价分数s(pM)，其计算公式为：
s(pM)＝αSnS+αDnD (25)
式中，αS为静态权重系数，为正值；αD为动态权重系数，为负值。
由此可以得到动态点云子集为：
所述的S6中，通过不断降低点云地图和查询帧所投影的深度图的分辨率，将之前误删的静态点恢复到静态地图中，并根据恢复静态点数量的比例，作为判断是否继续降低分辨率的条件，具体包括以下步骤：
经过动态点云的去除，可以将点云集PM划分为两个互斥的子集：静态点云子集PSM和动态点云子集PDM，表示为：
通过对比点云地图和查询帧投影成的深度图的可见性，在最大分辨率下去除掉所有可疑的动态点，然而经过实验验证，在根据高分辨率的深度图进行可见性的对比时，容易出现误删除动态物体边界或其他的一些静态点的情况，即在删除掉的动态点中会包含一定数量的静态点，但当降低深度图的分辨率后，误删的情况会明显减少，所以通过不断降低深度图的分辨率，来将误删的静态点恢复到静态的点云地图中。
在考虑恢复被误删的静态点时，首先将分辨率设置为最高，然后根据可见性信息逐渐降低分辨率，直到恢复的静态点数量小于阈值。当降低深度图的分辨率后，可以得到一个相对更加粗糙的深度图，根据每次迭代恢复的静态点数量计算恢复率，即在本轮恢复中已恢复静态点的数量除以第一轮恢复静态点的数量，如果恢复率超过阈值，则表明在动态点云中还含有大量的静态点，即继续降低深度图的分辨率并继续进行迭代；如果恢复率低于阈值，则表明在动态点云中基本不含有静态点云，即停止降低分辨率并返回最终的静态点云地图。
假设在最高分辨率下进行第一轮的静态点恢复中，恢复的静态点数量为n0，在进行第i(i＞1)轮恢复中，恢复的静态点数量为ni，则在第i轮中恢复的静态点数量占第一轮的比例，即第i轮的恢复率为在第i轮恢复中，深度图的分辨率为ri。如果恢复率pi大于预设阈值α，即本轮恢复的静态点数量占第一轮恢复静态点数量的比例大于预设阈值，则表示在动态点云中还含有大量的静态点，需根据可见性信息继续降点分辨率，进行下一轮迭代，以达到更好的恢复效果；反之，当恢复率pi小于阈值α时，则表示在动态点云中基本不含有静态点云，应停止降低分辨率，避免浪费计算资源。
如图2所示，图中的S(k)和D(k)分别表示在第k次迭代中主要由静态点云和动态点云所组成的点云地图，而S(k)和D(k)方框的大小，即代表点云地图的大小，其中第一次删除的结果中动态点云的点云地图大小是最大的，而对应的静态点云的点云地图则是最小的；随后进入恢复阶段，每次都从动态点云的点云地图中找出一定大小的静态点并加入到之前的静态点云中；经过整个恢复阶段，静态点云会越来越大，对应的动态点云则越来越小，而最终输出的就是整个流程完成后累加所得的静态点云地图。
以下结合实验结果对本发明进行验证，图3为含有动态障碍物轨迹残影的三维点云图，从图中可以明显看出，由于移动车辆的存在，导致点云地图中含有大量的“鬼影”；图4为局部点云地图所投影得到深度图；图5为查询帧所投影得到的深度图；图6为局部点云地图所投影的深度图与查询帧所投影深度图的视差图；图7为使用原算法进行动态障碍物轨迹残影去除的三维点云图，可以看到图中含有部分动态物体轨迹残影，该方法可以明显去除由于动态障碍物引起的“鬼影”，但其去除效果不好，图中还残留了一部分未被去除干净的“鬼影”；图8为使用本发明改进后的算法进行动态障碍物轨迹残影去除的三维点云图，由图中还可以明显看出，本发明对动态障碍物的轨迹残影去除效果十分明显，可以将几乎全部的“鬼影”去除干净，保留下来了完整的静态点云地图。
最后应说明的是：以上所述具体实施方案，对本发明的发明目的、技术方案和有益效果进行了进一步说明，以上实施例仅用于说明本发明的技术方案，而非对本发明创造保护范围的限制，本领域的普通技术人员应当理解，凡在本发明的技术方案进行修改、等同替换，均包含在本发明的保护范围内。

Claims (1)

1.一种适用于低线束3D激光雷达的动态障碍物去除方法，其特征在于，包括以下步骤：
S1、移动机器人通过自身搭载的3D激光雷达采集周围环境的三维点云数据，并对采集到的点云数据进行去畸变和滤波处理；
S2、将处理后的激光点云进行地面分割，分为地面点云和非地面点云；
S3、将分割后的点云，构建局部地图，进行全局优化，生成具有全局一致性的点云地图，并得到机器人的位姿信息；
S4、将生成的非地面点云地图投影成深度图；
S5、选取非地面点云中的连续N次扫描，作为查询帧，根据激光雷达距离信息调节查询帧所对应深度图的分辨率，通过对比查询帧与局部地图在同一分辨率下深度图的可见性，去除点云地图中所有的可疑动态点；
S6、通过不断降低点云地图和查询帧所投影的深度图的分辨率，将之前误删的静态点恢复到静态地图中，并根据恢复静态点数量的比例，来决定是否继续降低分辨率；
所述的S1中，移动机器人通过自身搭载的3D激光雷达采集周围环境的三维点云数据，并对采集到的点云数据去畸变和滤波等处理，具体步骤为：
假设移动机器人在采集两帧点云数据的时间间隔内，是匀速运动，令T(t)为任意t时刻移动机器人相对于初始位置的位姿变换，T(tk)为当前帧扫描结束时机器人位姿，在tk时刻，T(tm)(m＜k)已知，于是可以根据过去位姿估算出当前时刻的车辆位姿为：
T(tk)＝T(tk-1)·T(tk-2)-1·T(tk-1) (1)
在已知tk-1、tk时刻的位姿后，任意ti(ti∈(tk-1,tk))时刻采集到的点i(i∈Pk)对应的位姿T(ti)都可以得到，于是点i投影到tk时刻雷达坐标系下的坐标X('k,i)计算公式为：
将Pk内的所有点都按上式投影到tk时刻雷达坐标系下即得到消除运动畸变后的第k帧点云Pk'；
根据输入点云的大小，确定点云在X、Y、Z坐标轴上的最大值和最小值，分别记为xmax、ymax、zmax、xmin、ymin、zmin，根据最大值和最小值确定最小包围网格的大小，长宽高分别为lx、ly、lz；设体素栅格的边长R，将X、Y、Z坐标轴各自划分为M、N、L份，并计算栅格的数量S，每个点的栅格编号hx、hy、hz，确定点云落在哪个栅格中，然后计算体素栅格中的质心(cx、cy、cz)，以质心代表栅格内的所有点；当使用低线束的3D激光雷达时，由于点云数据本身较为稀疏，因此采用较小栅格大小以保证点云中的几何结构不被丢失；
所述的S2中，将处理后的激光点云进行地面分割，分为地面点云和非地面点云，具体步骤为：
当相邻激光线束打在障碍物上的点为PA、PB、PC，x,y,z为激光点在空间上的坐标，则垂直方向上相邻两点之间的角度α，可以用三角法则计算得到：
式中，Pi(z)(i＝B、C)代表点云在z轴方向的坐标，Δx和y的计算公式如下：
式中，Pi(x)(i＝A、B、C)代表点云在x轴方向的坐标，Pi(y)(i＝A、B、C)表示点云在y轴方向的坐标；
当使用16线激光雷达，则其扫描范围为[-15°,15°]，可知地面点必然在[-15°,-1°]范围内，在该范围内，从下到上N根范围内提取地面点，设置角度阈值Δα和一个线束阈值，若在规定范围内小于角度阈值Δα，则标记为地面点；
所述的S3中，将分割后的点云，构建成局部地图，进行全局优化，生成具有全局一致性的点云地图，并得到机器人的位姿信息，具体步骤为：
局部地图构建过程就是求解激光雷达扫描一周数据所形成的扫描帧相对于局部地图的位姿变换Tξ，转换过程为如下式所示：
式中，P表示激光雷达扫描到的一个激光点云；Rξθ表示旋转矩阵；ξx,ξy,ξz表示x,y,z三个方向的平移量，也就是相对于局部地图原点的坐标差值；
扫描帧插入到局部地图中，局部地图将生成概率网格地图：
M:rZ×rZ→[pmin,pmax] (6)
式中，M表示概率地图；rZ表示概率网格中每个格子的大小；pmin和pmax表示概率值的范围；
求取扫描帧相对局部地图的位姿可视作一个非线性最小二乘问题，如下式所示：
式中，Ms为平滑函数；hk表示为点在查询帧坐标系中的坐标；Tξ表示为查询帧坐标系相对局部地图坐标系位姿；
设第k-1帧特征点云Dk-1所对应投影在全局坐标系下的局部子图为通过点云的帧间匹配，可得到第k帧特征点云与第k-1帧点云间的位姿变换矩阵/>依据该位姿变换矩阵可以将当前帧特征点云Dk投影在全局坐标系下，获得第k帧局部子地图的估计值/>当前优化后的局部子地图/>与上一帧的局部子地图/>重合部分特征点云作为当前帧全局子地图，放入历史全局子地图所在的kd-tree中，构建全局地图/>同时，将当前局部子地图放入kd-tree中用于下一帧特征点云执行Scan to Map匹配操作；
所述的S4中，将生成的非地面点云地图投影成深度图，具体包括以下步骤：
通过对比点云地图和查询帧所投影的深度图的可见性，来去除动态物体，首先要将点云地图投影成深度图；深度图中的每个像素值都是地图点云中在某个视角下的可见点云，通过激光雷达的视场，可以将点云PM分割为多个可见地图点云其表达式为：
式中，表示在点云集合/>中距离像素点最近的点云集合；
随后将投影成二维的深度图，设投影后的深度图像为/>其中(x,y)表示图像中的像素坐标，其对应的距离值可以通过下式计算：
式中，N表示激光雷达扫描的线束，di(x,y)表示第i条线上(x,y)处的距离值，其含义为，在某个像素(x,y)处，取所有扫描线中最近的一个距离值作为该像素的值；
所述的S5中，选取非地面点云中的连续N次扫描，作为查询帧，根据激光雷达距离信息调节查询帧所对应深度图的分辨率，通过对比查询帧与局部地图在同一分辨率下深度图的可见性，去除点云地图中所有的可疑动态点，具体步骤包括以下内容：
在非地面点云数据中选取N次扫描组成查询帧PQ，进行批量的动态点去除(Batchdynamic point remove，BR)；在设置深度图的分辨率时，考虑到激光雷达具有越远点云越稀疏的特性，将激光雷达距离信息作为自适应调节深度图分辨率的权重，使得在不同距离区间的深度图具有不同大小的分辨率；
令di,j表示激光雷达在第i次扫描中第j个激光点的距离值，那么可以定义权重wi,j为：
定义分辨率层数L，将距离范围划分为L个区间其中1≤l≤L，/>和/>分别表示第l层的距离范围；对于第l层像素(i,j)，其对应的像素值ri,j表示如下：
式中，表示深度图中像素(i,j)在第l层的权重，计算公式如下：
通过上述公式，当像素(i,j)的距离值di,j落在区间内时，它在第l层的权重为1/di,j，即距离越近，权重越大，其对应的分辨率也越大；反之距离越远，对应的分辨率也就越低；当距离在该层范围之外的点，其对应的权重为0，即不参与该层的深度图构建；
则查询帧所对应的深度图/>上的每一个点的值为/>将局部地图和查询帧所对应的深度图，调整到初始分辨率，即最大分辨率，并进行矩阵减法，将两幅深度图逐像素相减，得到这两幅深度图的视差图/>
对于任一点如果其对应的/>值大于设定的阈值τD，则认为该点属于动态点，将其添加进/>中，该过程为：
通过动态子集，可以计算获得与其互斥的静态子集
根据机器人运动方向的连续N个扫描，对于每个扫描将其依次通过深度图的可见性与pM进行比较，并统计每一个点被标记为静态与动态点的次数，记为nS和nD，在完成所有比较后，可以得到每一个点的评价分数s(pM)，其计算公式为：
s(pM)＝αSnS+αDnD (15)
式中，αS为静态权重系数，为正值；αD为动态权重系数，为负值；
由此可以得到动态点云子集为：
所述的S6中，通过不断降点云地图和查询帧所投影的深度图的分辨率，将之前误删的静态点恢复到静态地图中，并将恢复静态点数量的比例，作为是否继续降低分辨率的判断条件，其具体步骤为：
经过动态点云的去除，可以将点云集PM划分为两个互斥的子集：静态点云子集PSM和动态点云子集PDM，表示为：
在考虑恢复被误删的静态点时，首先将分辨率设置为最高，然后根据可见性信息逐渐降低分辨率，直到恢复的静态点数量小于阈值；假设在最高分辨率下进行第一轮的静态点恢复中，恢复的静态点数量为n0，在进行第i(i＞1)轮恢复中，恢复的静态点数量为ni，则在第i轮中恢复的静态点数量占第一轮的比例，即第i轮的恢复率为在第i轮恢复中，深度图的分辨率为ri；如果恢复率pi大于预设阈值α，即本轮恢复的静态点数量占第一轮恢复静态点数量的比例大于预设阈值，则表示在动态点云中还含有大量的静态点，需根据可见性信息继续降点分辨率，进行下一轮迭代，以达到更好的恢复效果；反之，当恢复率pi小于阈值α时，则表示在动态点云中基本不含有静态点云，应停止降低分辨率，避免浪费计算资源。
CN202310678823.9A 2023-06-08 2023-06-08 一种适用于低线束3d激光雷达的动态障碍物去除方法 Pending CN116879870A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310678823.9A CN116879870A (zh) 2023-06-08 2023-06-08 一种适用于低线束3d激光雷达的动态障碍物去除方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310678823.9A CN116879870A (zh) 2023-06-08 2023-06-08 一种适用于低线束3d激光雷达的动态障碍物去除方法

Publications (1)

Publication Number Publication Date
CN116879870A true CN116879870A (zh) 2023-10-13

Family

ID=88259442

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310678823.9A Pending CN116879870A (zh) 2023-06-08 2023-06-08 一种适用于低线束3d激光雷达的动态障碍物去除方法

Country Status (1)

Country Link
CN (1) CN116879870A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117607897A (zh) * 2023-11-13 2024-02-27 深圳市其域创新科技有限公司 基于光线投影法的动态物体去除方法及相关装置

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180209815A1 (en) * 2017-01-24 2018-07-26 Korea University Research And Business Foundation Method of estimating reliability of measurement distance of laser rangefinder, and localizating method of mobile robot using laser rangefinder
CN109725329A (zh) * 2019-02-20 2019-05-07 苏州风图智能科技有限公司 一种无人车定位方法及装置
CN111929699A (zh) * 2020-07-21 2020-11-13 北京建筑大学 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统
CN113108773A (zh) * 2021-04-22 2021-07-13 哈尔滨理工大学 一种融合激光与视觉传感器的栅格地图构建方法
CN113902860A (zh) * 2021-10-10 2022-01-07 北京工业大学 一种基于多线激光雷达点云的多尺度静态地图构建方法
WO2022103195A1 (ko) * 2020-11-12 2022-05-19 주식회사 유진로봇 로봇 시스템
CN115061470A (zh) * 2022-06-30 2022-09-16 哈尔滨理工大学 适用狭窄空间的无人车改进teb导航方法
WO2023004956A1 (zh) * 2021-07-30 2023-02-02 西安交通大学 一种在高动态环境下的激光slam方法、系统设备及存储介质

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180209815A1 (en) * 2017-01-24 2018-07-26 Korea University Research And Business Foundation Method of estimating reliability of measurement distance of laser rangefinder, and localizating method of mobile robot using laser rangefinder
CN109725329A (zh) * 2019-02-20 2019-05-07 苏州风图智能科技有限公司 一种无人车定位方法及装置
CN111929699A (zh) * 2020-07-21 2020-11-13 北京建筑大学 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统
WO2022103195A1 (ko) * 2020-11-12 2022-05-19 주식회사 유진로봇 로봇 시스템
CN113108773A (zh) * 2021-04-22 2021-07-13 哈尔滨理工大学 一种融合激光与视觉传感器的栅格地图构建方法
WO2023004956A1 (zh) * 2021-07-30 2023-02-02 西安交通大学 一种在高动态环境下的激光slam方法、系统设备及存储介质
CN113902860A (zh) * 2021-10-10 2022-01-07 北京工业大学 一种基于多线激光雷达点云的多尺度静态地图构建方法
CN115061470A (zh) * 2022-06-30 2022-09-16 哈尔滨理工大学 适用狭窄空间的无人车改进teb导航方法

Non-Patent Citations (8)

* Cited by examiner, † Cited by third party
Title
GISEOP KIM 等: "remove, then revert:static point cloud map construction using multiresolution range images", 《2020 IEEE/RSJ INTERNATIONAL CONFERENCE ON INTELLIGENT ROBOTS AND SYSTEMS (IROS)》, pages 10758 - 10765 *
HYUNGTAE LIM 等: "ERASOR: Egocentric Ratio of Pseudo Occupancy-Based Dynamic Object Removal for Static 3D Point Cloud Map Building", 《IEEE ROBOTICS AND AUTOMATION LETTERS》, pages 2272 - 2279 *
TIANTIAN LUAN 等: "A novel T-S fuzzy robust control for part transportation of aircraft carrier considering transportation time and stochastic demand", 《AEROSPACE SCIENCE AND TECHNOLOGY》, pages 1 - 17 *
傅思勇 等: "空间栅格动态划分的点云精简方法", 《光学学报》, pages 1 - 9 *
吴开阳: "基于激光雷达传感器的三维多目标检测与跟踪技术研究", 《中国优秀硕士学位论文全文数据库信息科技辑》, pages 1 - 86 *
季虹良 等: "深度图像分割的城市区域倾斜影像密集匹配点云滤波算法", 《测绘科学技术学报》, pages 491 - 495 *
栾添添 等: "基于动态变采样区域RRT的无人车路径规划", 《控制与决策》, pages 1721 - 1729 *
莫建文 等: "结合图像分割和点云分割的障碍物检测算法", 《计算机工程与设计》, pages 1855 - 1858 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117607897A (zh) * 2023-11-13 2024-02-27 深圳市其域创新科技有限公司 基于光线投影法的动态物体去除方法及相关装置

Similar Documents

Publication Publication Date Title
CN110097553B (zh) 基于即时定位建图与三维语义分割的语义建图系统
CN108647646B (zh) 基于低线束雷达的低矮障碍物的优化检测方法及装置
CN112396650B (zh) 一种基于图像和激光雷达融合的目标测距系统及方法
WO2021233029A1 (en) Simultaneous localization and mapping method, device, system and storage medium
Maddern et al. Real-time probabilistic fusion of sparse 3d lidar and dense stereo
CN114862949B (zh) 一种基于点线面特征的结构化场景视觉slam方法
CN106780560B (zh) 一种基于特征融合粒子滤波的仿生机器鱼视觉跟踪方法
CN111998862B (zh) 一种基于bnn的稠密双目slam方法
CN104794737A (zh) 一种深度信息辅助粒子滤波跟踪方法
Qian et al. Robust visual-lidar simultaneous localization and mapping system for UAV
CN114325634A (zh) 一种基于激光雷达的高鲁棒性野外环境下可通行区域提取方法
Ouyang et al. A cgans-based scene reconstruction model using lidar point cloud
CN116879870A (zh) 一种适用于低线束3d激光雷达的动态障碍物去除方法
Xiong et al. Road-Model-Based road boundary extraction for high definition map via LIDAR
CN114549549B (zh) 一种动态环境下基于实例分割的动态目标建模跟踪方法
CN116643291A (zh) 一种视觉与激光雷达联合剔除动态目标的slam方法
CN115355904A (zh) 一种针对地面移动机器人的Lidar-IMU融合的slam方法
Han et al. GardenMap: Static point cloud mapping for Garden environment
CN114648639B (zh) 一种目标车辆的检测方法、系统及装置
CN114353779B (zh) 一种采用点云投影快速更新机器人局部代价地图的方法
CN111239761B (zh) 一种用于室内实时建立二维地图的方法
CN111260709B (zh) 一种面向动态环境的地面辅助的视觉里程计方法
CN114089376A (zh) 一种基于单激光雷达的负障碍物检测方法
CN115729250A (zh) 一种无人机的飞行控制方法、装置、设备及存储介质
CN113192133A (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
TA01 Transfer of patent application right

Effective date of registration: 20240219

Address after: 518000 1002, Building A, Zhiyun Industrial Park, No. 13, Huaxing Road, Henglang Community, Longhua District, Shenzhen, Guangdong Province

Applicant after: Shenzhen Wanzhida Technology Co.,Ltd.

Country or region after: China

Address before: 150080 No. 52, Xuefu Road, Nangang District, Heilongjiang, Harbin

Applicant before: HARBIN University OF SCIENCE AND TECHNOLOGY

Country or region before: China

TA01 Transfer of patent application right