CN115359115A - 一种动态环境下基于多传感器的同时定位与建图方法 - Google Patents

一种动态环境下基于多传感器的同时定位与建图方法 Download PDF

Info

Publication number
CN115359115A
CN115359115A CN202211040010.9A CN202211040010A CN115359115A CN 115359115 A CN115359115 A CN 115359115A CN 202211040010 A CN202211040010 A CN 202211040010A CN 115359115 A CN115359115 A CN 115359115A
Authority
CN
China
Prior art keywords
point cloud
dynamic
laser
points
laser point
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
CN202211040010.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.)
Zhejiang University of Technology ZJUT
Original Assignee
Zhejiang University of Technology ZJUT
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 Zhejiang University of Technology ZJUT filed Critical Zhejiang University of Technology ZJUT
Priority to CN202211040010.9A priority Critical patent/CN115359115A/zh
Publication of CN115359115A publication Critical patent/CN115359115A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T1/00—General purpose image data processing
    • G06T1/20—Processor architectures; Processor configuration, e.g. pipelining
    • 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/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • 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/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/74—Image or video pattern matching; Proximity measures in feature spaces
    • G06V10/761—Proximity, similarity or dissimilarity measures
    • 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/70—Arrangements for image or video recognition or understanding using pattern recognition or machine learning
    • G06V10/762—Arrangements for image or video recognition or understanding using pattern recognition or machine learning using clustering, e.g. of similar faces in social networks
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Databases & Information Systems (AREA)
  • General Health & Medical Sciences (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Computing Systems (AREA)
  • Artificial Intelligence (AREA)
  • Health & Medical Sciences (AREA)
  • Multimedia (AREA)
  • Image Processing (AREA)

Abstract

一种动态环境下基于多传感器的同时定位与建图方法，包括：首先，利用YOLOV5进行动态目标检测并发送检测框；利用惯性测量单元(IMU)矫正点云畸变；提取地面激光点云，方便后续的点云聚类；提取检测框内的激光点云，聚类出动态物体的激光点云；在矫正畸变后的激光点云中去除聚类出的动态物体点云用于后续算法计算；利用迭代卡尔曼滤波融合激光点云和IMU信息，得到当前的位姿信息；根据位姿信息，将去除畸变和动态物体后的点云与点云地图叠加，获得实时更新后的点云地图；利用关键帧和词袋模型以及激光点云进行回环检测。

Description

一种动态环境下基于多传感器的同时定位与建图方法
技术领域
本发明涉及自主移动机器人技术领域，尤其是涉及动态环境下的多传感器融合的定位和建图方法。
背景技术
基于单一传感器的定位与建图方法难以应对较为复杂的环境。例如，仅仅依靠纯激光雷达的方法难以很好地解决点云运动畸变的问题、适应环境的变化以及应对动态环境。
以激光雷达为主要传感器进行定位和建图的方案，其精度主要依赖于点云的帧间匹配，而当点云中存在较多动态点时，其定位很有可能会产生漂移，即使定位尚且准确，其建立的点云地图中也会存在大量的“鬼影”，从而影响地图的复用。
现有技术中关于动态环境下的同时定位与建图方法，专利CN112734836A公开了一种基于视觉定位的方法，但这种方法相对于以激光雷达为主传感器的方法来说难以得到准确的地图尺度信息，并且在缺少光照的条件下难以运行。专利CN113724387A公开了一种基于语义分割的方法，但这种方法难以在计算资源有限的平台上实时运行。
发明内容
为了弥补现有技术的上述不足，本发明提出了一种动态环境下基于多传感器的同时定位与建图方法。采用相机和激光雷达获取环境信息，利用基于YOLO V5的目标检测方法检测动态物体，利用惯性测量单元(IMU)获取机器人的角速度和加速度信息来对点云运动畸变和检测框位置进行矫正，通过基于欧式距离的条件聚类方法在检测框内的点云中提取出目标点云，去除提取出的动态点云使算法的定位和建图更加准确可靠。
动态环境下基于多传感器的同时定位与建图方法，包括以下步骤：
步骤1：图像处理程序在GPU中单独运行，利用相机实时获取当前环境中的图像信息，并进行基于YOLO V5的动态目标检测，输出检测到的动态物体的检测框信息。
步骤2：利用惯性测量单元(IMU)和激光点云的时间戳信息矫正点云运动畸变。通过线性插值和IMU预积分的方法，得到每个激光点对应时间戳时刻相对于当前帧初始时刻的相对运动信息。根据激光点的相对运动数据，将激光点矫正到正确的位置上。
步骤3：提取地面激光点云，方便后续的点云聚类。默认环境中的动态物体存在于可行地面之上，因此先提取出地面激光点云来大大降低聚类难度。去除动态物体上的点云后，再将地面点云与去除动态物体点的非地面点云合并，用于后续的点云匹配。
步骤4：将激光点云投影到图像数据上，提取检测框内的激光点云，聚类出动态物体的激光点云。
步骤5：将点云分为动态点云和非动态点云。保留动态物体的激光点云，可供后续的动态避障算法使用。在矫正畸变后的激光点云中去除聚类出的动态物体点云用于算法的定位和建图。
步骤6：利用迭代卡尔曼滤波融合激光点云和IMU信息，得到机器人当前相对于上一帧数据的相对位姿和世界坐标系下的绝对位姿。
步骤7：根据位姿信息，将去除畸变和动态物体后的点云与点云地图叠加，获得实时更新后的点云地图。
步骤8：保留相机关键帧，利用关键帧和词袋模型以及激光点云进行回环检测，进一步减小累计误差。
作为一种优选方案，步骤3中提取地面激光点云，具体步骤包括：
步骤3.1：将矫正运动畸变后的激光点云根据XY坐标在XY平面上以一定的边长进行栅格化。
步骤3.2：对于每一个栅格内的点云寻找其中的最低点，如果该最低点的绝对高度高于某一阈值，则认为该栅格内没有地面点，否则，将栅格内所有与最低点高度差不超过预设阈值的激光点认为是可能的地面点。
步骤3.3：求出所有筛选出的可能的地面点的高度均值，将超出均值一定范围的点从可能的地面点中去除，剩余的点作为地面点。
作为一种优选方案，步骤4中提取检测框内的激光点云，聚类出动态物体的激光点云，具体步骤包括：
步骤4.1：利用相机的内参数据和相机雷达事先标定得到的外参数据，将激光雷达点云数据投影到图像数据上。
步骤4.2：相机和激光雷达数据分别在GPU和CPU上并行处理，并且不用进行相机和雷达话题的同步，而是根据相机和雷达数据获取的时间戳信息，利用IMU的角速度和加速度信息对检测框位置进行补偿。在时间差内对IMU信息进行积分，得到机器人在相机和雷达数据获取时刻之间产生的相对运动，用计算出来的相对运动再将图像和相机数据进行进一步对准，之后将检测框内的点云进行提取。
步骤4.3：由于过远的动态物体上只有极少数的点，对同时定位和建图算法的影响极小，而一定高度以上的点云中一般不包括动态物体，所以先将提取出来的点云根据距离和高度进行直通滤波。然后再利用基于欧式距离的聚类方法，当一个点周围的点在一定范围内时，将周围的点认为是同一类的点，当不能再加入新的点时，则认为聚类出一个完整的类。将检测框内的所有点云进行聚类后，再根据每个类中的点云数量、点云形态等信息筛选出目标动态物体上的点云。
作为一种优选方案，步骤8中利用关键帧和词袋模型以及激光点云进行回环检测，具体步骤包括：
步骤8.1：生成图像关键帧，用词袋模型通过统计特征类型来描述该关键帧，以表征向量的形式记录关键帧以及当前位姿。
步骤8.2：将新生成的关键帧与之前的关键帧进行相似度对比，取出相似度较高的关键帧对应位姿下的点云地图中的点云。
步骤8.3：将取出的几帧点云与当前点云进行匹配，将匹配度最高的作为回环结果加入到后端优化中。
本发明的优点和积极效果是：
1.本发明能够较为快速地提取一般环境下的地面点云，便于地面上的点云聚类。
2.本发明利用激光雷达、相机和IMU结合的方法，能够消除动态环境对地图构建的影响，提升定位和地图的精度。
3.本发明采用相机和激光雷达并行计算的方法，相对串行方法能够大大缩短计算时间，具有较好的实时性，并且能够在计算资源有限的平台上运行。
附图说明
图1是本发明方法的流程示意图。
图2是本发明的动态点云提取流程示意图。
图3a是本发明的消除动态点的侧视图,图3b是本发明的未消除动态点的侧视图。
图4a是本发明的消除动态点的俯视图，图4b是本发明的未消除动态点的俯视图。
具体实施方法
下面结合附图对本发明作进一步的详细说明：
动态环境下基于多传感器的同时定位与建图方法，其整体流程如图1所示，具体包括以下步骤：
步骤1：图像处理程序在GPU中单独运行，利用相机实时获取当前环境中的图像信息，并进行基于YOLO V5的动态目标检测，输出检测到的动态物体的检测框信息。检测到当前环境中的动态目标后，将当前图像中检测到的动态物体的检测框以两点(xmin,ymin)，(xmax,ymax)的形式记录，并发送到ROS节点中去。
步骤2：利用惯性测量单元(IMU)和激光点云的时间戳信息矫正点云运动畸变。通过IMU预积分的方法，得到一帧点云时间内初始时刻到末尾时刻的运动信息。通过线性插值的方法，得到每个激光点对应时间戳时刻相对于当前帧初始时刻的相对运动信息。根据激光点的相对运动数据，将激光点矫正到正确的位置上。
步骤3：提取地面激光点云，方便后续的点云聚类。默认环境中的动态物体存在于可行地面之上，因此先提取出地面激光点云大大降低聚类难度，去除动态物体上的点云后，再将地面点云与去除动态物体点的非地面点云合并，用于后续的点云匹配。
步骤3.1：将矫正运动畸变后的激光点云根据XY坐标在XY平面上以一定的边长进行栅格化。
步骤3.2：对于每一个栅格内的点云寻找其中的最低点，如果该最低点的绝对高度高于某一阈值，则认为该栅格内没有地面点，否则，将该最低点作为地面基准点，将栅格内所有与最低点高度差不超过预设阈值的激光点认为是可能的地面点。
步骤3.3：求出所有筛选出来的可能的地面点的高度均值，将超出均值一定范围的点认为是噪点和非地面点，从可能的地面点中去除，将剩余的点作为地面点。
步骤4：动态点云提取流程如图2所示。将去除地面点云的激光点云投影到图像数据上，提取检测框内的激光点云，聚类出动态物体的激光点云。
步骤4.1：利用相机标定的内参数据和相机雷达事先标定得到的外参数据，将激光雷达点云数据投影到图像数据上。
步骤4.2：相机和激光雷达数据分别在GPU和CPU上并行处理，并且不用进行相机和雷达话题的同步，而是根据相机和雷达数据获取的时间戳信息，利用IMU的角速度和加速度信息对检测框位置进行补偿。在时间差内对IMU信息进行积分，得到机器人在相机和雷达数据获取时刻之间产生的相对运动，用计算出来的相对运动再将图像和相机数据进行进一步对准，之后将检测框内的点云进行提取。
步骤4.3：由于过远的动态物体上只有极少数的点，对同时定位和建图算法的影响极小，而一定高度以上的点云中一般不包括动态物体，所以先将提取出来的点云根据距离和高度进行直通滤波。然后再利用基于欧式距离的聚类方法，当一个点周围的点在一定范围内时，将周围的点认为是同一类的点，当不能再加入新的点时，则认为聚类出一个完整的类。由于目标检测的检测框内的物体以目标动态物体为主，目标动态物体上的激光点数量应占据检测框内所有点云数量的大多数，因此设置最小激光点数量要求，将检测框内的所有点云进行聚类后，将小于最小激光点数量要求的类去除，将剩下的各个类按照点云数量降序排列，再根据每个类中的点云形态等信息筛选出目标动态物体上的点云。
步骤5：依据上述步骤将点云分为动态点云和非动态点云。保留动态物体的激光点云，可供后续的动态避障算法使用。在矫正畸变后的激光点云中去除聚类出的动态物体点云用于后续算法的定位和建图。
步骤6：利用迭代卡尔曼滤波融合激光点云和IMU信息，得到当前的位姿。
步骤7：根据位姿信息，将去除畸变和动态点云后的点云与点云地图叠加，获得实时更新后的点云地图。消除动态点云和未消除动态点云的效果对比如图3、图4所示，其中图3是侧视对比图，图4是俯视对比图。
步骤8：保留相机关键帧，利用关键帧和词袋模型以及激光点云进行回环检测，进一步减小累计误差。
步骤8.1：生成图像关键帧，用词袋模型通过统计特征类型来描述该关键帧，以表征向量的形式记录关键帧以及当前位姿。
步骤8.2：将新生成的关键帧与之前的关键帧进行相似度对比，取出相似度较高的关键帧对应位姿下的点云地图中的点云。
步骤8.3：将取出的几帧点云与当前点云进行匹配，将匹配度最高的作为回环结果加入到后端优化中。
需要强调的是，本发明所述的实施案例是说明性的，而不是限定性的，因此本发明包括并不限于具体实施方案中所述的实施例，凡是由本领域技术人员根据本发明的技术方案得出类似的其它实施方式，同样属于本发明的保护范围。

Claims (4)

1.动态环境下基于多传感器的同时定位与建图方法，包括以下步骤：
步骤1：图像处理程序在GPU中单独运行，利用相机实时获取当前环境中的图像信息，并进行基于YOLO V5的动态目标检测，输出检测到的动态物体的检测框信息；
步骤2：利用惯性测量单元(IMU)和激光点云的时间戳信息矫正点云运动畸变；通过线性插值和IMU预积分的方法，得到激光点对应时间戳时刻相对于当前帧初始时刻的相对运动信息；根据激光点的相对运动数据，将激光点矫正到正确的位置上；
步骤3：提取地面激光点云，方便后续的点云聚类；默认环境中的动态物体存在于可行地面之上，因此先提取出地面激光点云大大降低聚类难度，去除动态物体上的点云后，再将地面点云与去除动态物体点的非地面点云合并，用于后续的点云匹配；
步骤4：将激光点云投影到图像数据上，提取检测框内的激光点云，聚类出动态物体的激光点云；
步骤5：将点云分为动态点云和非动态点云；保留动态物体的激光点云，可供后续的动态避障算法使用；在矫正畸变后的激光点云中去除聚类出的动态物体点云用于算法的定位和建图；
步骤6：利用迭代卡尔曼滤波融合激光点云和IMU信息，得到机器人当前相对于上一帧数据的相对位姿和世界坐标系下的绝对位姿；
步骤7：根据位姿信息，将去除畸变和动态物体后的点云与点云地图叠加，获得实时更新后的点云地图；
步骤8：保留相机关键帧，利用关键帧和词袋模型以及激光点云进行回环检测，进一步减小累计误差。
2.如权力要求1所述的动态环境下基于多传感器的同时定位与建图方法，其特征在于：步骤3中提取地面激光点云，具体步骤包括：
步骤3.1：将矫正运动畸变后的激光点云根据XY坐标在XY平面上以一定的边长进行栅格化；
步骤3.2：对于每一个栅格内的点云寻找其中的最低点，如果该最低点的绝对高度高于某一阈值，则认为该栅格内没有地面点，否则，将栅格内所有与最低点高度差不超过预设阈值的激光点认为是可能的地面点；
步骤3.3：求出所有筛选出的可能的地面点的高度均值，将超出均值一定范围的点从可能的地面点中去除，剩余的点作为地面点。
3.如权力要求1所述的动态环境下基于多传感器的同时定位与建图方法，其特征在于：步骤4中提取检测框内的激光点云，聚类出动态物体的激光点云，具体步骤包括：
步骤4.1：利用相机的内参数据和相机雷达事先标定得到的外参数据，将激光雷达点云数据投影到图像数据上；
步骤4.2：相机和激光雷达数据分别在GPU和CPU上并行处理，并且不用进行相机和雷达话题的同步，而是根据相机和雷达数据获取的时间戳信息，利用IMU的角速度和加速度信息对检测框位置进行补偿；在时间差内对IMU信息进行积分，得到机器人在相机和雷达数据获取时刻之间产生的相对运动，用计算出来的相对运动再将图像和相机数据进行进一步对准，之后将检测框内的点云进行提取；
步骤4.3：先将提取出来的点云根据距离和高度进行直通滤波；然后再利用基于欧式距离的聚类方法，当一个点周围的点在一定范围内时，将周围的点认为是同一类的点，当不能再加入新的点时，则认为聚类出一个完整的类；将检测框内的所有点云进行聚类后，再根据每个类中的点云数量、点云形态等信息筛选出目标动态物体上的点云。
4.如权力要求1所述的动态环境下基于多传感器的同时定位与建图方法，其特征在于：步骤8中关键帧和词袋模型以及激光点云进行回环检测，具体步骤包括：
步骤8.1：生成图像关键帧，用词袋模型通过统计特征类型来描述该关键帧，以表征向量的形式记录关键帧以及当前位姿；
步骤8.2：将新生成的关键帧与之前的关键帧进行相似度对比，取出相似度较高的关键帧对应位姿下的点云地图中的点云；
步骤8.3：将取出的几帧点云与当前点云进行匹配，将匹配度最高的作为回环结果加入到后端优化中。
CN202211040010.9A 2022-08-29 2022-08-29 一种动态环境下基于多传感器的同时定位与建图方法 Pending CN115359115A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211040010.9A CN115359115A (zh) 2022-08-29 2022-08-29 一种动态环境下基于多传感器的同时定位与建图方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211040010.9A CN115359115A (zh) 2022-08-29 2022-08-29 一种动态环境下基于多传感器的同时定位与建图方法

Publications (1)

Publication Number Publication Date
CN115359115A true CN115359115A (zh) 2022-11-18

Family

ID=84003907

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211040010.9A Pending CN115359115A (zh) 2022-08-29 2022-08-29 一种动态环境下基于多传感器的同时定位与建图方法

Country Status (1)

Country Link
CN (1) CN115359115A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115880364A (zh) * 2023-02-09 2023-03-31 广东技术师范大学 基于激光点云和视觉slam的机器人位姿估计方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115880364A (zh) * 2023-02-09 2023-03-31 广东技术师范大学 基于激光点云和视觉slam的机器人位姿估计方法
CN115880364B (zh) * 2023-02-09 2023-05-16 广东技术师范大学 基于激光点云和视觉slam的机器人位姿估计方法

Similar Documents

Publication Publication Date Title
US10996062B2 (en) Information processing device, data management device, data management system, method, and program
CN111258313B (zh) 多传感器融合slam系统及机器人
CN111275763B (zh) 闭环检测系统、多传感器融合slam系统及机器人
CN110261870B (zh) 一种用于视觉-惯性-激光融合的同步定位与建图方法
JP7326720B2 (ja) 移動体位置推定システムおよび移動体位置推定方法
CN112734852B (zh) 一种机器人建图方法、装置及计算设备
CN112634451B (zh) 一种融合多传感器的室外大场景三维建图方法
CN107833236B (zh) 一种动态环境下结合语义的视觉定位系统和方法
WO2022188094A1 (zh) 一种点云匹配方法及装置、导航方法及设备、定位方法、激光雷达
CN108648194B (zh) 基于cad模型三维目标识别分割和位姿测量方法及装置
AU2013343222A1 (en) Cloud feature detection
CN112233177A (zh) 一种无人机位姿估计方法及系统
CN115376109B (zh) 障碍物检测方法、障碍物检测装置以及存储介质
CN112419497A (zh) 基于单目视觉的特征法与直接法相融合的slam方法
CN114140527A (zh) 一种基于语义分割的动态环境双目视觉slam方法
CN114627395B (zh) 基于嵌套靶标的多旋翼无人机角度分析方法、系统及终端
CN114187418A (zh) 回环检测方法、点云地图构建方法、电子设备及存储介质
CN115359115A (zh) 一种动态环境下基于多传感器的同时定位与建图方法
CN117218350A (zh) 一种基于固态雷达的slam实现方法及系统
CN117419719A (zh) 一种融合imu的三维激光雷达定位与建图方法
CN116147618B (zh) 一种适用动态环境的实时状态感知方法及系统
CN116045965A (zh) 一种融合多传感器的环境地图构建方法
CN115294358A (zh) 特征点提取方法、装置、计算机设备及可读存储介质
CN117611762B (zh) 一种多层级地图构建方法、系统及电子设备
WO2022179047A1 (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