CN107990893B - 二维激光雷达slam中探测环境发生突变的检测方法 - Google Patents

二维激光雷达slam中探测环境发生突变的检测方法 Download PDF

Info

Publication number
CN107990893B
CN107990893B CN201711188165.6A CN201711188165A CN107990893B CN 107990893 B CN107990893 B CN 107990893B CN 201711188165 A CN201711188165 A CN 201711188165A CN 107990893 B CN107990893 B CN 107990893B
Authority
CN
China
Prior art keywords
carrier
laser radar
dimensional
time
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
CN201711188165.6A
Other languages
English (en)
Other versions
CN107990893A (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 CN201711188165.6A priority Critical patent/CN107990893B/zh
Publication of CN107990893A publication Critical patent/CN107990893A/zh
Application granted granted Critical
Publication of CN107990893B publication Critical patent/CN107990893B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Optical Radar Systems And Details Thereof (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了二维激光雷达SLAM中探测环境发生突变的检测方法。依次采集传感器数据，惯性递推，进行激光雷达数据匹配，最后对比惯性递推求解的载体位置变化量与激光雷达数据匹配配求解的载体位置变化量的差值，若大于阈值则激光雷达探测的环境发生突变。在应用二维激光雷达SLAM方法时，通过本发明公开的方法，能够准确地检测出环境是否发生突变。

Description

二维激光雷达SLAM中探测环境发生突变的检测方法
技术领域
本发明属于机器人自主导航技术领域，特别涉及了二维激光雷达SLAM中探测环境发生突变的检测方法。
背景技术
在无GPS(Global Positioning System)环境中，通常利用同步定位与地图构建(SLAM，Simultaneous Localization and Mapping)技术给机器人进行自主导航。激光雷达能够获取高精度的测距信息，且不依赖于外界环境的光照条件，常作为SLAM的传感器。
目前大多二维激光雷达SLAM方法应用于静态环境中，即所探测的环境不变，而实际应用环境大多为动态环境，环境会随时间发生变化，造成传统的SLAM方法定位误差较大。比如二维激光雷达SLAM方法应用于飞行器时，不同高度的二维水平面环境一般不同，当飞行器的高度变化后，导致探测的二维水平面环境发生突变，造成SLAM方法的定位误差变大。因此，在实际应用中激光雷达SLAM方法需要考虑到探测的环境发生突变的情况对SLAM进行改进，其中激光雷达探测的环境是否发生突变的检测方法尤其重要。
发明内容
为了解决上述背景技术提出的技术问题，本发明旨在提供二维激光雷达SLAM中探测环境发生突变的检测方法，在应用激光雷达SLAM方法时能够准确地检测出环境是否发生突变。
为了实现上述技术目的，本发明的技术方案为：
二维激光雷达SLAM中探测环境发生突变的检测方法，包括以下步骤：
(1)采集惯性传感器数据和激光雷达数据；
(2)根据惯性传感器数据递推载体的二维位置变化量；
(3)利用激光雷达数据进行匹配求解载体的二维位置变化量；
(4)对比步骤(2)惯性递推的二维位置变化量与步骤(3)匹配求解的二维位置变化量，若二者差值的绝对值大于预设阈值，则认为所探测的环境发生突变。
进一步地，二维激光雷达SLAM中全局坐标系的建立方法为，以初始时刻载体的位置为原点，以二维激光雷达所在二维水平面为X-O-Y平面，其中X轴和Y轴分别与初始时刻载体的右向和前向重合，Z轴为天向；航向角为载体的前向与全局坐标系的Y轴方向的夹角，以顺时针方向为正。
进一步地，所述惯性传感器数据包括陀螺仪数据和加速度计数据，陀螺仪和加速度计固定在载体上，且其安装与载体坐标系重合。
进一步地，步骤(2)的过程如下：
Figure BDA0001480508650000021
Figure BDA0001480508650000022
Figure BDA0001480508650000023
Figure BDA0001480508650000024
Figure BDA0001480508650000025
Figure BDA0001480508650000026
上式中，当前时刻记为k时刻，
Figure BDA0001480508650000027
为k时刻载体的天向角速度，
Figure BDA0001480508650000028
和
Figure BDA0001480508650000029
为k时刻载体的右向和前向的加速度，
Figure BDA00014805086500000210
和
Figure BDA00014805086500000211
依次为从机器人组合导航系统中得到的k-1时刻载体在全局坐标系下X轴、Y轴方向的速度和航向角，
Figure BDA00014805086500000212
和
Figure BDA00014805086500000213
依次为k时刻载体在全局坐标系下X轴、Y轴方向的速度和航向角，Δt为从k-1时刻到k时刻的时间，
Figure BDA0001480508650000031
和
Figure BDA0001480508650000032
分别为k时刻载体在Δt内全局坐标系下X轴和Y轴方向的位置变化量，
Figure BDA0001480508650000033
为递推出的载体从k-1时刻到k时刻的位置变化量。
进一步地，在步骤(3)中，Sk和Sk-1分别为k时刻和k-1时刻激光雷达数据，根据ICP算法，先通过kd-tree进行最邻近点搜索，搜索Sk中每个激光点在Sk-1中的最近激光点，以此方式将Sk和Sk-1的激光点进行配对，然后利用奇异值分解法求解载体位置变化量。
采用上述技术方案带来的有益效果：
通过本发明公开的方法，在动态环境下应用激光雷达SLAM方法时，能够准确地检测所探测的环境是否发生突变，为SLAM过程中改进载体位姿解算算法提供了一个准确的参考判断，当探测的环境发生突变时对位姿解算部分进行相应的处理，从而能够有效地减小动态环境下传统SLAM方法的定位误差。
附图说明
图1是本发明的基本流程图。
具体实施方式
以下将结合附图，对本发明的技术方案进行详细说明。
在本发明中，导航系为激光雷达SLAM中的全局坐标系，航向角为载体前向与全局坐标系的Y轴方向的夹角，顺时针方向为正，其中全局坐标系建立方法如下：
以初始时刻载体的位置为原点，以二维激光雷达所在二维水平面为X-O-Y平面，其中X轴和Y轴分别与初始时刻载体的右向和前向重合，Z轴为天向。
陀螺仪和加速度计固定在激光雷达的载体上，且其安装与载体坐标系重合，载体坐标系建立方法如下：
以载体的位置为原点，以二维激光雷达所在二维水平面为X-O-Y平面，其中X轴和Y轴分别为载体的右向和前向，Z轴为天向。
参照图1，本发明的具体步骤如下。
步骤一：当前时刻记为k时刻，采集加速度计、陀螺仪和激光雷达数据，
Figure BDA0001480508650000041
为k时刻载体在天向的角速度，
Figure BDA0001480508650000042
和
Figure BDA0001480508650000043
为k时刻载体的右向和前向的加速度信息，k时刻的激光雷达数据记为Sk。
步骤二：从机器人组合导航系统中得k-1时刻载体在全局坐标系下X轴、Y轴方向的速度和航向，分别记为
Figure BDA0001480508650000044
和
Figure BDA0001480508650000045
则可根据下式递推载体从k-1时刻到k时刻的位置变化量
Figure BDA0001480508650000046
Figure BDA0001480508650000047
Figure BDA0001480508650000048
Figure BDA0001480508650000049
Figure BDA00014805086500000410
Figure BDA00014805086500000411
Figure BDA00014805086500000412
上式中，
Figure BDA00014805086500000413
和
Figure BDA00014805086500000414
分别为载体在全局坐标系下X轴、Y轴方向的速度和航向，Δt为从k-1时刻到k时刻的时间，
Figure BDA00014805086500000415
和
Figure BDA00014805086500000416
分别为载体在Δt内全局坐标系下X轴和Y轴方向的位置变化量。
步骤三：根据ICP算法将Sk与Sk-1匹配求解位置变化量
Figure BDA00014805086500000417
首先通过kd-tree进行最邻近点搜索方法搜索Sk中每个激光点在上一时刻激光雷达数据Sk-1中的最近激光点，以此方式将Sk和Sk-1的激光点进行配对，然后利用奇异值分解法求解载体位置变化量
Figure BDA00014805086500000418
步骤四：设置距离阈值L，若
Figure BDA0001480508650000051
则激光雷达所探测的环境发生突变，其中阈值设置的方法如下：
二维激光雷达SLAM中利用激光雷达数据进行匹配求解载体位姿的时间周期为Δt，动态情况下在Δt时间内利用载体上加速度计和陀螺仪递推载体的二维位置的误差为σ，设置的阈值L与σ相近。在本实施例中，阈值设置为L＝1.2σ。
实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (5)

1.二维激光雷达SLAM中探测环境发生突变的检测方法，其特征在于，包括以下步骤：
(1)采集惯性传感器数据和激光雷达数据；
(2)根据惯性传感器数据递推载体的二维位置变化量；
(3)利用激光雷达数据进行匹配求解载体的二维位置变化量；
(4)对比步骤(2)惯性递推的二维位置变化量与步骤(3)匹配求解的二维位置变化量，若二者差值的绝对值大于预设阈值，则认为所探测的环境发生突变。
2.根据权利要求1所述二维激光雷达SLAM中探测环境发生突变的检测方法，其特征在于，二维激光雷达SLAM中全局坐标系的建立方法为，以初始时刻载体的位置为原点，以二维激光雷达所在二维水平面为X-O-Y平面，其中X轴和Y轴分别与初始时刻载体的右向和前向重合，Z轴为天向；航向角为载体的前向与全局坐标系的Y轴方向的夹角，以顺时针方向为正。
3.根据权利要求2所述二维激光雷达SLAM中探测环境发生突变的检测方法，其特征在于，所述惯性传感器数据包括陀螺仪数据和加速度计数据，陀螺仪和加速度计固定在载体上，且其安装与载体坐标系重合。
4.根据权利要求3所述二维激光雷达SLAM中探测环境发生突变的检测方法，其特征在于，步骤(2)的过程如下：
Figure FDA0002417791620000011
Figure FDA0002417791620000012
Figure FDA0002417791620000013
Figure FDA0002417791620000014
Figure FDA0002417791620000021
Figure FDA0002417791620000022
上式中，当前时刻记为k时刻，
Figure FDA0002417791620000023
为k时刻载体的天向角速度，
Figure FDA0002417791620000024
和
Figure FDA0002417791620000025
为k时刻载体的右向和前向的加速度，
Figure FDA0002417791620000026
和
Figure FDA0002417791620000027
依次为从机器人组合导航系统中得到的k-1时刻载体在全局坐标系下X轴、Y轴方向的速度和航向角，
Figure FDA0002417791620000028
和
Figure FDA0002417791620000029
依次为k时刻载体在全局坐标系下X轴、Y轴方向的速度和航向角，Δt为从k-1时刻到k时刻的时间，
Figure FDA00024177916200000210
和
Figure FDA00024177916200000211
分别为k时刻载体在Δt内全局坐标系下X轴和Y轴方向的位置变化量，
Figure FDA00024177916200000212
为递推出的载体从k-1时刻到k时刻的位置变化量。
5.根据权利要求1-4中任意一项所述二维激光雷达SLAM中探测环境发生突变的检测方法，其特征在于，在步骤(3)中，设Sk和Sk-1分别为k时刻和k-1时刻激光雷达数据，根据ICP算法，先通过kd-tree进行最邻近点搜索，搜索Sk中每个激光点在Sk-1中的最近激光点，以此方式将Sk和Sk-1的激光点进行配对，然后利用奇异值分解法求解载体位置变化量。
CN201711188165.6A 2017-11-24 2017-11-24 二维激光雷达slam中探测环境发生突变的检测方法 Active CN107990893B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711188165.6A CN107990893B (zh) 2017-11-24 2017-11-24 二维激光雷达slam中探测环境发生突变的检测方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711188165.6A CN107990893B (zh) 2017-11-24 2017-11-24 二维激光雷达slam中探测环境发生突变的检测方法

Publications (2)

Publication Number Publication Date
CN107990893A CN107990893A (zh) 2018-05-04
CN107990893B true CN107990893B (zh) 2020-07-24

Family

ID=62032750

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711188165.6A Active CN107990893B (zh) 2017-11-24 2017-11-24 二维激光雷达slam中探测环境发生突变的检测方法

Country Status (1)

Country Link
CN (1) CN107990893B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110763232B (zh) * 2018-07-25 2021-06-29 深圳市优必选科技有限公司 一种机器人及其导航定位方法和装置
CN112033412A (zh) * 2020-09-07 2020-12-04 中国南方电网有限责任公司超高压输电公司天生桥局 一种提高巡检机器人定位精度的方法及装置

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB201116959D0 (en) * 2011-09-30 2011-11-16 Bae Systems Plc Vehicle localisation with 2d laser scanner and 3d prior scans
KR20160099336A (ko) * 2015-02-12 2016-08-22 재단법인대구경북과학기술원 모바일 매핑 시스템
CN105371840B (zh) * 2015-10-30 2019-03-22 北京自动化控制设备研究所 一种惯性/视觉里程计/激光雷达的组合导航方法
CN105953798B (zh) * 2016-04-19 2018-09-18 深圳市神州云海智能科技有限公司 移动机器人的位姿确定方法和设备
CN106525053A (zh) * 2016-12-28 2017-03-22 清研华宇智能机器人（天津）有限责任公司 一种基于多传感器融合的移动机器人室内定位方法

Also Published As

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

Similar Documents

Publication Publication Date Title
CN111207774B (zh) 一种用于激光-imu外参标定的方法及系统
CN111258313B (zh) 多传感器融合slam系统及机器人
CN111024066B (zh) 一种无人机视觉-惯性融合室内定位方法
CN110261870B (zh) 一种用于视觉-惯性-激光融合的同步定位与建图方法
CN109975792B (zh) 基于多传感器融合矫正多线激光雷达点云运动畸变的方法
CN107991680B (zh) 动态环境下基于激光雷达的slam方法
CN108253963B (zh) 一种基于多传感器融合的机器人自抗扰定位方法以及定位系统
CN113311411B (zh) 一种用于移动机器人的激光雷达点云运动畸变校正方法
CN111578957B (zh) 基于三维点云地图定位的智能车纯追踪循迹方法
CN106814753B (zh) 一种目标位置矫正方法、装置及系统
CN106441275A (zh) 一种机器人规划路径的更新方法及装置
CN111982099B (zh) 机器人混合定位方法、装置、设备及计算机可读介质
CN107389968B (zh) 一种基于光流传感器和加速度传感器的无人机定点实现方法和装置
CN109579838A (zh) Agv小车的定位方法及定位系统
CN108627152B (zh) 一种微型无人机基于多传感器数据融合的导航方法
CN111161337A (zh) 一种动态环境下的陪护机器人同步定位与构图方法
CN107990893B (zh) 二维激光雷达slam中探测环境发生突变的检测方法
CN114061573B (zh) 地面无人车辆编队定位装置及方法
CN114136315B (zh) 一种基于单目视觉辅助惯性组合导航方法及系统
CN114137975A (zh) 一种超声波辅助融合定位的无人车导航纠偏方法
CN115639547A (zh) 多线激光雷达与gnss_ins联合标定方法、系统及介质
Lei et al. Visual localization strategy for indoor mobile robots in the complex environment
CN115046543A (zh) 一种基于多传感器的组合导航方法及系统
CN115993089B (zh) 基于pl-icp的在线四舵轮agv内外参标定方法
CN114543786B (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