CN107991680B - 动态环境下基于激光雷达的slam方法 - Google Patents

动态环境下基于激光雷达的slam方法 Download PDF

Info

Publication number
CN107991680B
CN107991680B CN201711163497.9A CN201711163497A CN107991680B CN 107991680 B CN107991680 B CN 107991680B CN 201711163497 A CN201711163497 A CN 201711163497A CN 107991680 B CN107991680 B CN 107991680B
Authority
CN
China
Prior art keywords
carrier
laser radar
laser
navigation information
moment
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
CN201711163497.9A
Other languages
English (en)
Other versions
CN107991680A (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.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN201711163497.9A priority Critical patent/CN107991680B/zh
Publication of CN107991680A publication Critical patent/CN107991680A/zh
Application granted granted Critical
Publication of CN107991680B publication Critical patent/CN107991680B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments

Abstract

本发明公开了一种动态环境下基于激光雷达的SLAM方法，首先采集传感器数据，进行激光雷达数据的预处理，利用惯性数据预测载体的导航信息，检测并剔除激光雷达数据中的代表移动物体的激光点，然后根据未剔除的激光雷达数据匹配求解位姿，并与惯性导航信息进行融合滤波，最后更新地图。在动态环境中，通过本发明公开的SLAM方法，能够获得高精度的激光雷达载体导航信息，并构建精准的地图。

Description

动态环境下基于激光雷达的SLAM方法
技术领域
本发明涉及机器人自主导航技术领域，尤其涉及动态环境下基于激光雷达的SLAM方法。
背景技术
同步定位与地图(SLAM，Simultaneous Localization and Mapping)技术是机器人自主导航技术领域中的一大研究热点，也是机器人实际应用中的关键技术。激光雷达不依赖于外界光照条件，是一种主动式探测传感器，且具备高精度的测距信息。激光雷达SLAM方法仍然是机器人SLAM方法中应用最为广泛的方法。
目前大多激光雷达SLAM技术都针对于静态环境，即整个SLAM过程中，环境不会发生任何变化，而实际应用环境大多为动态环境，存在着移动物体。机器人SLAM方法中，通常利用激光雷达扫描的点数据进行匹配来求解位姿，而激光雷达探测到移动物体的扫描点数据会对直接影响匹配的效果，造成较大的误差，不仅如此，移动物体会直接导致构图精度变差。
发明内容
发明目的：本发明提供一种动态环境下基于激光雷达的SLAM方法，以解决动态环境下已有SLAM方法中求解的载体位姿精度低和构建的地图误差大的问题；通过本发明公开的方法，能够获得高精度的激光雷达载体导航信息，并构建精准的地图。
技术方案：本发明所述的动态环境下基于激光雷达的SLAM方法包括：
(1)构建全局坐标系，并初始化地图和载体的初始导航信息；
(2)采集加速度计、陀螺仪和激光雷达数据；
(3)对激光雷达数据进行预处理，根据不同型号激光雷达的测距有效范围，将测距信息超出该有效范围的数据剔除；
(4)使用加速度计和陀螺仪数据递推激光雷达载体的预测导航信息；
(5)根据步骤(4)中激光雷达载体的预测导航信息检测并剔除激光雷达数据中代表移动物体的激光点；
(6)使用激光雷达SLAM中位姿匹配的方法匹配剔除后剩余激光雷达数据，求解载体的位姿；
(7)将步骤(4)中惯性递推的载体导航信息与步骤(6)的位姿信息进行融合滤波，得到激光雷达载体的导航信息；
(8)根据步骤(7)中滤波后的激光雷达载体的导航信息更新地图；
(9)跳转至步骤(2)。
进一步的，步骤(1)中所述构建全局坐标系的方法为：
以初始时刻载体的位置为原点，以二维激光雷达所在二维水平面为X-O-Y平面，其中X轴和Y轴分别与初始时刻载体的右向和前向重合。
进一步的，步骤(1)中所述载体的导航信息具体为载体在全局坐标系下的二维位置、速度和航向角信息，其中航向角为载体前向与全局坐标系的Y轴方向的夹角，顺时针方向为正。
进一步的，步骤(1)中初始化的地图类型为点图。
进一步的，步骤(4)中递推预测激光雷达载体的导航信息方法为：
将当前时刻记为k时刻，陀螺仪和加速度计固定在激光雷达的载体上，为k时刻载体的天向角速度，和为k时刻载体的右向和前向的加速度信息，k-1时刻载体的导航信息为ξk-1，则可根据下式递推载体k时刻的预测导航信息
上式中，和分别为k-1时刻载体在全局坐标系下的X方向坐标、Y方向坐标、X方向速度分量、Y方向速度分量和航向，和分别为k时刻预测的载体在全局坐标系下的X方向坐标、Y方向坐标、X方向速度分量、Y方向速度分量和航向，Δt为从k-1时刻到k时刻的时间。
进一步的，步骤(5)包括如下步骤：
(5.1)利用k时刻载体的预测导航信息和k-1时刻载体的导航信息ξk-1分别求解k时刻和k-1时刻的激光雷达数据Sk和Sk-1中每个激光点在全局坐标系中的坐标；
(5.2)在全局坐标系中，通过knn算法在Sk-1中搜索与Sk中的第i个激光点最邻近的激光点，其距离记为利用knn算法在Sk中搜索与Sk-1中的第i个激光点最邻近的激光点，其距离记为其中i＝1,2,…,numk，numk为Sk中的激光点的数量；
(5.3)设置距离阈值L1，若则为激光雷达探测到移动物体的激光点，若则为激光雷达探测到移动物体的激光点，这些代表移动物体的激光点集合分别记为和在Sk和Sk-1分别剔除和剩余的激光雷达数据记为和
进一步的，步骤(6)具体包括：
使用激光雷达SLAM中位姿匹配的方法通过匹配剩余的激光雷达数据和求解载体k时刻载体的位姿Zk：
上式中和分别为载体在全局坐标下的X方向坐标、Y方向坐标和航向。
进一步的，步骤(7)具体包括：
采用卡尔曼滤波，以Zk作为量测值与载体的预测导航信息进行滤波求解k时刻激光雷达载体的导航信息ξk：
上式中和分别为k时刻载体在全局坐标系下的X方向坐标、Y方向坐标、X方向速度分量、Y方向速度分量和航向。
进一步的，在步骤(8)中更新地图的方法为：
设置距离阈值L2，根据激光雷达载体的导航信息ξk求解移动物体的激光点集全部激光雷达数据Sk和剔除移动物体的剩余激光雷达数据中的激光点在全局坐标系中的坐标，然后在已构建地图中的剔除与和Sk中的激光点距离小于L2的点，再将中的激光点加载入地图中。
有益效果：本发明与现有技术相比，其显著优点是：使用本发明公开的方法，能够有效地消除移动物体对SLAM中位姿求解和地图构建的不利影响，获得高精度的激光雷达载体导航信息，并构建精准的地图。
附图说明
图1是本发明的一个实施例的流程示意图。
具体实施方式
如图1所示，本实施例提供的动态环境下基于激光雷达的SLAM方法包括：
(1)构建全局坐标系，并初始化地图和载体的初始导航信息。
其中，构建全局坐标系的方法为：以初始时刻载体的位置为原点，以二维激光雷达所在二维水平面为X-O-Y平面，其中X轴和Y轴分别与初始时刻载体的右向和前向重合。所述载体的导航信息具体为载体在全局坐标系下的二维位置、速度和航向角信息，其中航向角为载体前向与全局坐标系的Y轴方向的夹角，顺时针方向为正。初始化的地图类型为点图。
(2)采集加速度计、陀螺仪和激光雷达数据。
(3)对激光雷达数据进行预处理，根据不同型号激光雷达的测距有效范围，将测距信息超出该有效范围的数据剔除。
(4)使用加速度计和陀螺仪数据递推激光雷达载体的预测导航信息。
其中，递推预测激光雷达载体的导航信息方法为：
将当前时刻记为k时刻，陀螺仪和加速度计固定在激光雷达的载体上，为k时刻载体的天向角速度，和为k时刻载体的右向和前向的加速度信息，k-1时刻载体的导航信息为ξk-1，则可根据下式递推载体k时刻的预测导航信息
上式中，和分别为k-1时刻载体在全局坐标系下的X方向坐标、Y方向坐标、X方向速度分量、Y方向速度分量和航向，和分别为k时刻预测的载体在全局坐标系下的X方向坐标、Y方向坐标、X方向速度分量、Y方向速度分量和航向，Δt为从k-1时刻到k时刻的时间。
(5)根据激光雷达载体的导航信息检测并剔除激光雷达数据中代表移动物体的激光点。
该步骤包括如下步骤：
(5.1)利用k时刻载体的预测导航信息和k-1时刻载体的导航信息ξk-1分别求解k时刻和k-1时刻的激光雷达数据Sk和Sk-1中每个激光点在全局坐标系中的坐标；
(5.2)在全局坐标系中，通过knn算法在Sk-1中搜索与Sk中的第i个激光点最邻近的激光点，其距离记为利用knn算法在Sk中搜索与Sk-1中的第i个激光点最邻近的激光点，其距离记为其中i＝1,2,…,numk，numk为Sk中的激光点的数量；
(5.3)设置距离阈值L1，若则为激光雷达探测到移动物体的激光点，若则为激光雷达探测到移动物体的激光点，这些代表移动物体的激光点集合分别记为和在Sk和Sk-1分别剔除和剩余的激光雷达数据记为和
(6)使用激光雷达SLAM中位姿匹配的方法匹配剔除后剩余激光雷达数据，求解载体的位姿。具体方法为：
使用激光雷达SLAM中位姿匹配的方法通过匹配剩余的激光雷达数据和求解载体k时刻载体的位姿Zk：
上式中和分别为载体在全局坐标下的X方向坐标、Y方向坐标和航向。
(7)将步骤(4)中惯性递推的载体导航信息与步骤(6)的位姿信息进行融合滤波，得到激光雷达载体的导航信息。具体方法为：
采用卡尔曼滤波，以Zk作为量测值与载体的预测导航信息进行滤波求解k时刻激光雷达载体的导航信息ξk：
上式中和分别为k时刻载体在全局坐标系下的X方向坐标、Y方向坐标、X方向速度分量、Y方向速度分量和航向。
(8)根据激光雷达载体的导航信息更新地图。
更新地图的方法为：
设置距离阈值L2，根据激光雷达载体的导航信息ξk求解移动物体的激光点集全部激光雷达数据Sk和剔除移动物体的剩余激光雷达数据中的激光点在全局坐标系中的坐标，然后在已构建地图中的剔除与和Sk中的激光点距离小于L2的点，再将中的激光点加载入地图中。
(9)跳转至步骤(2)。
以上所揭露的仅为本发明一种较佳实施例而已，不能以此来限定本发明之权利范围，因此依本发明权利要求所作的等同变化，仍属本发明所涵盖的范围。

Claims (8)

1.一种动态环境下基于激光雷达的SLAM方法，其特征在于该方法包括：
(1)构建全局坐标系，并初始化地图和载体的初始导航信息；
(2)采集加速度计、陀螺仪和激光雷达数据；
(3)对激光雷达数据进行预处理，根据不同型号激光雷达的测距有效范围，将测距信息超出该有效范围的数据剔除；
(4)使用加速度计和陀螺仪数据递推激光雷达载体的预测导航信息；
(5)根据步骤(4)中激光雷达载体的预测导航信息检测并剔除激光雷达数据中代表移动物体的激光点；该步骤包括如下步骤：
(5.1)利用k时刻载体的预测导航信息和k-1时刻载体的导航信息ξk-1分别求解k时刻和k-1时刻的激光雷达数据Sk和Sk-1中每个激光点在全局坐标系中的坐标；
(5.2)在全局坐标系中，通过knn算法在Sk-1中搜索与Sk中的第i个激光点最邻近的激光点，其距离记为利用knn算法在Sk中搜索与Sk-1中的第i个激光点最邻近的激光点，其距离记为其中i＝1,2,…,numk，numk为Sk中的激光点的数量；
(5.3)设置距离阈值L1，若则为激光雷达探测到移动物体的激光点，若则为激光雷达探测到移动物体的激光点，这些代表移动物体的激光点集合分别记为和在Sk和Sk-1分别剔除和剩余的激光雷达数据记为和
(6)使用激光雷达SLAM中位姿匹配的方法匹配剔除后剩余激光雷达数据，求解载体的位姿；
(7)将步骤(4)中惯性递推的载体导航信息与步骤(6)的位姿信息进行融合滤波，得到激光雷达载体的导航信息；
(8)根据步骤(7)中滤波后的激光雷达载体的导航信息更新地图；
(9)跳转至步骤(2)。
2.根据权利要求1所述的动态环境下基于激光雷达的SLAM方法，其特征在于：步骤(1)中所述构建全局坐标系的方法为：
以初始时刻载体的位置为原点，以二维激光雷达所在二维水平面为X-O-Y平面，其中X轴和Y轴分别与初始时刻载体的右向和前向重合。
3.根据权利要求1所述的动态环境下基于激光雷达的SLAM方法，其特征在于：步骤(1)中所述载体的导航信息具体为载体在全局坐标系下的二维位置、速度和航向角信息，其中航向角为载体前向与全局坐标系的Y轴方向的夹角，顺时针方向为正。
4.根据权利要求1所述的动态环境下基于激光雷达的SLAM方法，其特征在于：步骤(1)中初始化的地图类型为点图。
5.根据权利要求1所述的动态环境下基于激光雷达的SLAM方法，其特征在于：步骤(4)中递推预测激光雷达载体的导航信息方法为：
将当前时刻记为k时刻，陀螺仪和加速度计固定在激光雷达的载体上，为k时刻载体的天向角速度，和为k时刻载体的右向和前向的加速度信息，k-1时刻载体的导航信息为ξk-1，则可根据下式递推载体k时刻的预测导航信息
上式中，和分别为k-1时刻载体在全局坐标系下的X方向坐标、Y方向坐标、X方向速度分量、Y方向速度分量和航向，和分别为k时刻预测的载体在全局坐标系下的X方向坐标、Y方向坐标、X方向速度分量、Y方向速度分量和航向，Δt为从k-1时刻到k时刻的时间。
6.根据权利要求1所述的动态环境下基于激光雷达的SLAM方法，其特征在于：步骤(6)具体包括：
使用激光雷达SLAM中位姿匹配的方法通过匹配剩余的激光雷达数据和求解载体k时刻载体的位姿Zk：
上式中和分别为载体在全局坐标下的X方向坐标、Y方向坐标和航向。
7.根据权利要求1所述的动态环境下基于激光雷达的SLAM方法，其特征在于：步骤(7)具体包括：
采用卡尔曼滤波，以Zk作为量测值与载体的预测导航信息进行滤波求解k时刻激光雷达载体的导航信息ξk：
上式中和分别为k时刻载体在全局坐标系下的X方向坐标、Y方向坐标、X方向速度分量、Y方向速度分量和航向。
8.根据权利要求1所述的动态环境下基于激光雷达的SLAM方法，其特征在于，在步骤(8)中更新地图的方法为：
设置距离阈值L2，根据激光雷达载体的导航信息ξk求解移动物体的激光点集全部激光雷达数据Sk和剔除移动物体的剩余激光雷达数据中的激光点在全局坐标系中的坐标，然后在已构建地图中的剔除与和Sk中的激光点距离小于L2的点，再将中的激光点加载入地图中。
CN201711163497.9A 2017-11-21 2017-11-21 动态环境下基于激光雷达的slam方法 Active CN107991680B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711163497.9A CN107991680B (zh) 2017-11-21 2017-11-21 动态环境下基于激光雷达的slam方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711163497.9A CN107991680B (zh) 2017-11-21 2017-11-21 动态环境下基于激光雷达的slam方法

Publications (2)

Publication Number Publication Date
CN107991680A CN107991680A (zh) 2018-05-04
CN107991680B true CN107991680B (zh) 2019-08-23

Family

ID=62031797

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711163497.9A Active CN107991680B (zh) 2017-11-21 2017-11-21 动态环境下基于激光雷达的slam方法

Country Status (1)

Country Link
CN (1) CN107991680B (zh)

Families Citing this family (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109074085B (zh) * 2018-07-26 2021-11-09 达闼机器人有限公司 一种自主定位和地图建立方法、装置和机器人
CN109507677B (zh) * 2018-11-05 2020-08-18 浙江工业大学 一种结合gps和雷达里程计的slam方法
CN110095786B (zh) * 2019-04-30 2021-02-02 北京云迹科技有限公司 基于一线激光雷达的三维点云地图生成方法及系统
CN110333495A (zh) * 2019-07-03 2019-10-15 深圳市杉川机器人有限公司 利用激光slam在长走廊建图的方法、装置、系统、存储介质
CN110530368B (zh) * 2019-08-22 2021-06-15 浙江华睿科技有限公司 一种机器人定位方法及设备
CN110702134B (zh) * 2019-10-08 2023-05-05 燕山大学 基于slam技术的车库自主导航装置及导航方法
CN110887486B (zh) * 2019-10-18 2022-05-20 南京航空航天大学 一种基于激光线辅助的无人机视觉导航定位方法
CN110954114B (zh) * 2019-11-26 2021-11-23 苏州智加科技有限公司 电子地图的生成方法、装置、终端及存储介质
CN112923933A (zh) * 2019-12-06 2021-06-08 北理慧动（常熟）车辆科技有限公司 一种激光雷达slam算法与惯导融合定位的方法
CN111275748B (zh) * 2020-01-15 2023-04-07 南京航空航天大学 动态环境下基于激光雷达的点云配准方法
CN111458722B (zh) * 2020-04-16 2022-04-01 杭州师范大学钱江学院 一种激光雷达小车在具有坡度环境下的地图构建方法
CN113439524A (zh) * 2021-06-15 2021-09-28 江苏科技大学 基于单线激光雷达自动升降装置的家用割草机器人、系统以及建图方法
WO2023000294A1 (zh) * 2021-07-23 2023-01-26 深圳市大疆创新科技有限公司 位姿估计方法、激光雷达-惯性里程计、可移动平台及存储介质

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105547305B (zh) * 2015-12-04 2018-03-16 北京布科思科技有限公司 一种基于无线定位和激光地图匹配的位姿解算方法
CN105758408A (zh) * 2016-01-05 2016-07-13 福州华鹰重工机械有限公司 局部地图构建方法及装置
CN107167148A (zh) * 2017-05-24 2017-09-15 安科机器人有限公司 同步定位与地图构建方法和设备

Also Published As

Publication number Publication date
CN107991680A (zh) 2018-05-04

Similar Documents

Publication Publication Date Title
CN107991680B (zh) 动态环境下基于激光雷达的slam方法
CN106017463B (zh) 一种基于定位传感装置的飞行器定位方法
Li et al. Multi-sensor fusion for navigation and mapping in autonomous vehicles: Accurate localization in urban environments
CN112734852B (zh) 一种机器人建图方法、装置及计算设备
CN113781582B (zh) 基于激光雷达和惯导联合标定的同步定位与地图创建方法
CN106647814A (zh) 一种基于二维码地标识别的无人机视觉辅助定位与飞控系统及方法
CN110411462A (zh) 一种gnss/惯性/车道线约束/里程计多源融合方法
CN111156998A (zh) 一种基于rgb-d相机与imu信息融合的移动机器人定位方法
CN107389968B (zh) 一种基于光流传感器和加速度传感器的无人机定点实现方法和装置
CN111288989B (zh) 一种小型无人机视觉定位方法
CN113706626B (zh) 一种基于多传感器融合及二维码校正的定位与建图方法
CN108549376A (zh) 一种基于信标的导航定位方法及系统
CN112987065A (zh) 一种融合多传感器的手持式slam装置及其控制方法
CN103644904A (zh) 一种基于sift算法的视觉导航方法
CN111025366B (zh) 基于ins及gnss的网格slam的导航系统及方法
CN108106613B (zh) 基于视觉辅助的定位方法与系统
CN112967392A (zh) 一种基于多传感器触合的大规模园区建图定位方法
CN107289910A (zh) 一种基于tof的光流定位系统
CN109737968B (zh) 基于二维LiDAR和智能手机的室内融合定位方法
CN113188557B (zh) 一种融合语义特征的视觉惯性组合导航方法
CN113763549A (zh) 融合激光雷达和imu的同时定位建图方法、装置和存储介质
CN113740864B (zh) 基于激光三维点云的探测器软着陆末段自主位姿估计方法
CN107463871A (zh) 一种基于角特征加权的点云匹配方法
CN206095257U (zh) 智能巡视机器人组合导航系统
CN107990893B (zh) 二维激光雷达slam中探测环境发生突变的检测方法

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