CN107390681B - 一种基于激光雷达与地图匹配的移动机器人实时定位方法 - Google Patents
一种基于激光雷达与地图匹配的移动机器人实时定位方法 Download PDFInfo
- Publication number
- CN107390681B CN107390681B CN201710475024.6A CN201710475024A CN107390681B CN 107390681 B CN107390681 B CN 107390681B CN 201710475024 A CN201710475024 A CN 201710475024A CN 107390681 B CN107390681 B CN 107390681B
- Authority
- CN
- China
- Prior art keywords
- map
- robot
- pixel
- laser radar
- obstacle
- 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.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims abstract description 30
- 230000004888 barrier function Effects 0.000 claims abstract description 32
- 238000013507 mapping Methods 0.000 claims abstract description 6
- 241001269238 Data Species 0.000 claims abstract description 4
- 241000209094 Oryza Species 0.000 claims description 6
- 235000007164 Oryza sativa Nutrition 0.000 claims description 6
- 230000005856 abnormality Effects 0.000 claims description 6
- 235000009566 rice Nutrition 0.000 claims description 6
- 230000001186 cumulative effect Effects 0.000 claims description 5
- 238000011161 development Methods 0.000 claims description 5
- 239000011159 matrix material Substances 0.000 claims description 5
- 241000669244 Unaspis euonymi Species 0.000 claims description 3
- 230000000694 effects Effects 0.000 claims description 3
- 238000005192 partition Methods 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 235000013339 cereals Nutrition 0.000 claims 1
- 238000013506 data mapping Methods 0.000 claims 1
- 230000007613 environmental effect Effects 0.000 abstract description 3
- 238000000342 Monte Carlo simulation Methods 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 8
- 230000033001 locomotion Effects 0.000 description 8
- 230000004807 localization Effects 0.000 description 4
- 239000002245 particle Substances 0.000 description 2
- 206010047571 Visual impairment Diseases 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 230000007812 deficiency Effects 0.000 description 1
- 230000006735 deficit Effects 0.000 description 1
- 230000006698 induction Effects 0.000 description 1
- NJPPVKZQTLUDBO-UHFFFAOYSA-N novaluron Chemical compound C1=C(Cl)C(OC(F)(F)C(OC(F)(F)F)F)=CC=C1NC(=O)NC(=O)C1=C(F)C=CC=C1F NJPPVKZQTLUDBO-UHFFFAOYSA-N 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0234—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using optical markers or beacons
- G05D1/0236—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using optical markers or beacons in combination with a laser
-
- 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/20—Instruments for performing navigational calculations
-
- 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/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
- G01S17/46—Indirect determination of position data
- G01S17/48—Active triangulation systems, i.e. using the transmission and reflection of electromagnetic waves other than radio waves
-
- 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/93—Lidar systems specially adapted for specific applications for anti-collision purposes
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
- G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
- G05D1/0291—Fleet control
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Automation & Control Theory (AREA)
- Aviation & Aerospace Engineering (AREA)
- Computer Networks & Wireless Communication (AREA)
- Optics & Photonics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于激光雷达与地图匹配的移动机器人实时定位方法，其步骤包括：建立环境的2D地图；对2D地图，按照固定间隔距离进行横纵棋盘平均分块；对地图中的每一块进行假设推演，假设块的左上位置为机器人的初始位置，在每一块的基础上枚举方向；对于每一个假设，映射激光雷达的点云数据到地图像素上；建立匹配度模型来评判每一个假设的得分，对于每一个假设，累计激光雷达所有点云数据所对应的像素距离“障碍物像素”曼哈顿距离的和；选取和最小的块和方向作为机器人的起始位置和方向；在机器人移动中使用蒙特卡洛法跟踪其实时位置。本发明可让机器人只通过自身激光传感器和已知地图感知出自身所处的位置，而不要人工干预与环境标志物辅助。
Description
技术领域
本发明涉及移动机器人定位的技术领域，尤其是指一种基于激光雷达与地图匹配的移动机器人实时定位方法。
背景技术
随着科学技术的高度发展，机器人系统的应用领域越来越广泛，如工业、农业、医疗等。随着机器人系统的广泛应用，智能化成为其发展的一个重要方向。移动机器人作为其中一种重要的服务机器人类型，其智能化的一个方向就是在移动过程中的导航与避障。而由计算机控制的移动机器人在运动过程中重要的环节就是计算机要知道机器人在何处，也就是机器人定位的问题。
近年来，传感器领域涌现出多种适用于消费市场的产品。对移动机器人来说，传感器是感受外界信息的重要媒介。在消费级产品中，传感器的价格和性能通常受到限制，性能优良但是价格昂贵的传感器一般用于工业甚至国防领域，而面向消费市场的产品，需要充分考虑传感器的实际性能和有效适用范围等等。目前的消费市场中，移动机器人在家居环境的应用有着广阔的市场，在适用于家居环境的传感器和机器人模型相结合的产品中，又以距离传感器的应用最为普遍
目前的距离传感器产品中，有使用超声波作为感应媒介的，也有使用可见光通过光栅产生结构光来测量距离的，还有使用不可见光，例如红外光作为激光源，根据三角反射原理，来测量光路上障碍物离激光源的距离。这种利用红外光作为激光源的距离传感器又被称为激光雷达。它使用和雷达相似的反射原理，在激光的有效范围内的障碍由于遮挡光源产生了反射，由此可以得到与激光源相同平面上的2D障碍距离信息。
移动机器人仅有当前位置的障碍距离信息是不够的，对于智能移动机器人来说，在移动过程中需要保存已经走过的路径，并保存经过处的障碍物距离信息，这就是SLAM算法，一种地图创建算法。目前的智能机器人对于障碍地图的创建已经拥有了成熟的解决方案，我们只需要通过遥控控制机器人的移动，在移动过程中，计算机可以通过SLAM算法把路径上激光传感器所扫描到的障碍物构成一幅2D平面地图。该地图拥有一定的比例尺，可以做到和现实中的2D环境对应。在这个地图中，包含两种像素，一是障碍物像素，代表该像素所对应的地方存在激光雷达扫描到的障碍，二是空闲区域，代表没有障碍遮挡的区域，激光总是从机器人所处的激光源发出，穿过空闲区域，在障碍物处反射。
在移动机器人所处的环境，比如说家居环境中，我们一般认为环境是封闭的，也就是说在机器人创建完地图后，地图可以被保存下来，在环境保持不变的情况下，机器人总是位于地图的有效位置，我们说的机器人定位问题都是基于已创建好的地图的环境中，在给定的地图中完成机器人位置的确定。
传统的移动机器人在已知地图和环境下，需要手动机器人的起始位置来启动机器人按地图行走。且传统的移动机器人，即使给其读入的环境的地图，在控制其移动之前，机器人是无法感知到自己所处的位置的，只有在人为在地图上给机器人指出初始位置的情况下，机器人系统才能使用蒙特卡洛法跟踪机器人的位置。
发明内容
本发明的目的在于克服现有技术的缺点与不足，提供了一种基于激光雷达与地图匹配的移动机器人实时定位方法，可以让机器人在环境中只通过自身激光传感器和已知地图感知出自身所处的位置，而不要人工的干预与环境标志物的辅助，大大提高机器人的智能化与自动化，具有广泛的适应性。
为实现上述目的，本发明所提供的技术方案为：一种基于激光雷达与地图匹配的移动机器人实时定位方法，包括如下步骤：
1)建立2D地图：使用SLAM算法，机器人在环境中移动的过程中，根据激光雷达测距的原理，建立环境的2D障碍地图；在地图中，有效区域被分成两类，分别是障碍物像素和空闲像素，其中障碍物像素代表此像素对应的现实空间有障碍物机器人无法到达或越过，而空闲像素代表该空间没有障碍，机器人能够通过；
2)对2D地图进行分块：对地图的横向与纵向，以相等的距离进行划分，使地图被分成正方形块；以每个块的左上角的像素来代表该块，对地图分块能够在不影响定位的实际效果的情况下降低精度以提高时间与空间效率；
3)假设推演：对于步骤2)中每一个的分块，假设机器人的起始位置处于该块的左上角，并且枚举假设机器人面对的方向；对于每一次假设，把当前激光雷达所扫描到的点云图按照假设的位置为中心，按假设的方向将所有的点云数据通过坐标系转换的方式映射到地图像素中；
4)建立匹配度模型：根据步骤3)中做的每一次假设，需要建立匹配度模型来评判假设的优劣，找出最优的假设，也就是实际上机器人在环境中所处的位置，以映射点云数据后得到的像素点和地图中最近的“障碍物像素”的曼哈顿距离作为匹配度模型的评判依据；
5)利用蒙特卡洛算法跟踪机器人：根据步骤4)的描述，通过假设，验证出机器人当前启动的时候所处的近似的初始位置，当机器人被控制着移动的时候，使用蒙特卡洛算法与粒子滤波器结合，能够实时跟踪机器人的移动状态并且把机器人的实时位置更新在地图上；
6)异常处理：当机器人被搬离地面的时候，通过光电传感器检测到机器人轮胎离地，则停止对机器人发出控制指令，并且报告异常状态，直到机器人重新在地面上被安放好，这时重新启动步骤2)～4)，重新确定机器人的当前的位置作为新的起始位置，之后进入步骤5)正常跟踪机器人的实时位置。
步骤1)中所建立的2D地图，地图与现实世界能够做到相对应，地图上的像素代表现实中的5cm×5cm的区域，现实中无高度信息的2D位置能够映射地图上；在地图文件中有地图描述部分，包括地图文件的宽度Width像素和高度Height像素；为了建立地图与现实世界的联系，分别建立世界地图坐标系和地图文件像素坐标系：
地图文件由Width X Height像素的图片组成，看成一个矩阵，每一个像素由行列(height,width)坐标确定，其中图片左上位置为(0,0)，width为宽度方向，height为高度方向；通过定义地图(Height-1，0)像素也就是左下角像素的世界坐标(Xlowerleft,Ylowerleft)，默认世界坐标按照米为单位，则现实世界的世界坐标(x,y)和地图上像素(height,width)的对应关系为：
x＝Xlowerleft+width·resolution
y＝Ylowerleft+(Height-height)·resolution
其中resolution为0.05，代表每像素长宽0.05米，即地图的比例尺。
步骤2)中所述分块方法，具体如下：
由于移动机器人大小以半径20cm的产品居多，所以初始定位的精度在20cm以内即可，根据步骤1)中所设定的地图比例尺，把4×4像素结合成一个像素分块，也就是说实际环境中20cm×20cm的区域为一块，以每一块的左上角的像素代表该块区域，能够提高定位的时间和空间效率。
步骤3)中所述假设推演过程，具体如下：
从上至下，从左至右地枚举地图上的每一个分块，以分块左上角的像素点代表该分块；假设这个像素点所代表的现实中的区域即为机器人启动时所处的起始位置，并且还需要枚举机器人所面向的方向；激光雷达通过传送各个角度的障碍物距离信息来构成激光点云数据；定义disti为激光雷达内部极坐标系中i度位置上障碍物距离激光雷达中心的距离，单位为米；
当假设地图上(width,height)像素为机器人启动时在现实中所对应的位置，并且θ为机器人所面对的方向的时候，激光雷达的点云数据disti将被映射到地图上的位置(heighti,widthi)为：
这样，通过坐标系转换，在每一个位置和方向的假设中，都能够把点云数据映射到地图上。
步骤4)中所述建立匹配度模型来评判假设的优劣，从而寻找出最优的位置和方向，来定位机器人的起始位置，具体如下：
先定义一个像素的“障碍偏移度”，如果一个像素是一个“障碍物像素”，则取它的“障碍偏移度”为0，如果一个像素是一个“空闲像素”，则它的“障碍偏移度”为它到距离最近的“障碍物像素”的曼哈顿距离；
对于每一个假设的位置和方位，映射点云数据后把得到的点云数据所处的地图上的像素的“障碍偏移度”累加，得到的和，定义为该次假设的“匹配度”；如果假设的位置和方位与实际的机器人在现实中的位置与方位越接近，则点云数据映射后的像素“障碍偏移度”越小，机器人在实际中所扫描到的障碍与地图上的“障碍物像素”的匹配度越大；所以选择累加和最小的假设作为定位结果，来代表机器人在现实中的位置和方向。
把所有“障碍物像素”标记“障碍偏移度”为0后通过数据结构队列把这些像素入队，通过广度优先搜索原理，使用4-联通扩展法，使得扩展的像素赋值为出队像素的值+1，即可在O(Height*Width)复杂度内完成对所有像素“障碍偏移度”的预处理并保存下来，在假设推演阶段中映射点云数据后，能够直接使用。
本发明与现有技术相比，具有如下优点与有益效果：
1、机器人在启动后，可以根据激光雷达所传输的距离信息，在地图上通过假设推演的方式，在假设中映射激光雷达的点云数据与地图像素进行匹配验证，通过枚举找出匹配度最佳的假设作为机器人起始位置的定位。可以不需要人工的干预即可进行定位。
2、本发明仅需要一般的激光雷达作为距离传感器，不需要对环境进行改造，不需要像传统方法中需要利用环境中的贴地标、反光条等改造，适用性广泛。
3、在机器人启动过程中即可完成初始位置的定位，时间在数秒之内即可完成。在完成初始位置的定位后，利用蒙特卡洛算法可以在实时的时间内跟踪机器人在移动中的定位。
4、利用光电传感器，可以检测出移动机器人的意外搬起，在搬动后重新运行初始定位方法，即可自动重新定位机器人。
附图说明
图1为本发明方法流程图。
图2为机器人使用的2D地图示意图。
图3为机器人所处实验环境示意图。
图4为移动机器人和传感器组合示意图。
图5为激光雷达点云数据示意图。
图6为激光雷达内部极坐标系示意图。
图7为地图分块示意图。
图8a为障碍示意图。
图8b为障碍偏差度示意图。
图9为机器人近似的初始位置。
图10为移动机器人在现实中的移动路径。
图11为移动机器人运动中在地图中的跟踪定位。
具体实施方式
下面结合具体实施例对本发明作进一步说明。
如图1所示，本实施例所提供的基于激光雷达与地图匹配的移动机器人实时定位方法，包括以下步骤：
1)建立2D地图：使用SLAM算法，当机器人在环境中移动的过程中，根据激光雷达测距的原理，建立环境的2D障碍地图。在地图中，有效区域被分成两类，分别是障碍物像素和空闲像素。其中障碍物像素代表此像素对应的现实空间有障碍物机器人无法到达或越过，而空闲像素代表该空间没有障碍，机器人可以通过。
2)对2D地图进行分块：对地图的横向与纵向，以相等的距离进行划分，使地图被分成与棋盘类似的正方形块。以每个块的左上角的像素来代表该块。对地图分块可以在不影响定位的实际效果的情况下降低精度以提高时间与空间效率。
3)假设推演：对于步骤2)中每一个的分块，我们假设机器人的起始位置处于该块的左上角，并且枚举假设机器人面对的方向。对于每一次假设，我们把当前激光雷达所扫描到的点云图按照假设的位置为中心，按假设的方向将所有的点云数据通过坐标系转换的方式映射到地图像素中。
4)建立匹配度模型：根据步骤3)中做的每一次假设，我们需要建立匹配度模型来评判假设的优劣，找出最优的假设，也就是实际上机器人在环境中所处的位置，我们以映射点云数据后得到的像素点和地图中最近的“障碍物像素”的曼哈顿距离作为匹配度模型的评判依据。
5)利用蒙特卡洛算法跟踪机器人：根据步骤4)的描述，我们通过假设，验证出机器人当前启动的时候所处的近似的初始位置，当机器人被控制着移动的时候，我们使用蒙特卡洛算法与粒子滤波器结合，可以实时跟踪机器人的移动状态并且把机器人的实时位置更新在地图上。
6)异常处理：当机器人被搬离地面的时候，我们通过光电传感器检测到机器人轮胎离地，则停止对机器人发出控制指令，并且报告异常状态，直到机器人重新在地面上被安放好，这时重新启动步骤2)～4)，重新确定机器人的当前的位置作为新的起始位置，之后进入步骤5)正常跟踪机器人的实时位置。
上述步骤1)中所建立的2D地图，地图与现实世界可以做到相对应，地图上的像素代表现实中的5cm×5cm的区域，现实中无高度信息的2D位置可以映射地图上；在地图文件中有地图描述部分，包括地图文件的宽度Width像素和高度Height像素；为了建立地图与现实世界的联系，我们分别建立了世界地图坐标系和地图文件像素坐标系：
地图文件由Width X Height像素的图片组成，可以看成一个矩阵，每一个像素由行列(height,width)坐标确定，其中图片左上位置为(0,0)，width为宽度方向，height为高度方向。我们通过定义地图(Height-1，0)像素也就是左下角像素的世界坐标(Xlowerleft,Ylowerleft)，默认世界坐标按照米为单位，则现实世界的世界坐标(x,y)和地图上像素(height,width)的对应关系为：
x＝Xlowerleft+width·resolution
y＝Ylowerleft+(Height-height)·resolution
其中resolution为0.05，代表每像素长宽0.05米，即地图的比例尺。
上述步骤2)中所述的分块方法：
由于移动机器人大小以半径20cm左右的产品居多，所以初始定位的精度可以在20cm以内即可，根据步骤1)中所设定的地图比例尺，我们可以把4×4像素结合成一个像素分块，也就是说实际环境中20cm×20cm的区域为一块，以每一块的左上角的像素代表该块区域。可以提高定位的时间和空间效率。
上述步骤3)中所述假设推演过程：
我们从上至下，从左至右地枚举地图上的每一个分块，以分块左上角的像素点代表该分块。假设这个像素点所代表的现实中的区域即为机器人启动时所处的起始位置，并且还需要枚举机器人所面向的方向。激光雷达通过传送各个角度的障碍物距离信息来构成激光点云数据。我们定义disti为激光雷达内部极坐标系中i度位置上障碍物距离激光雷达中心的距离，单位也是米。
当我们假设地图上(width,height)像素为机器人启动时在现实中所对应的位置，并且θ为机器人所面对的方向的时候，激光雷达的点云数据disti将被映射到地图上的位置(heighti,widthi)为：
这样，我们通过坐标系转换，在每一个位置和方向的假设中，我们都可以把点云数据映射到地图上。
上述步骤4)中建立匹配度模型来评判假设的优劣，从而寻找出最优的位置和方向，来定位机器人的起始位置：
我们先定义一个像素的“障碍偏移度”，如果一个像素是一个“障碍物像素”，则取它的“障碍偏移度”为0，如果一个像素是一个“空闲像素”，则它的“障碍偏移度”为它到距离最近的“障碍物像素”的曼哈顿距离。
对于每一个假设的位置和方位，映射点云数据后把得到的点云数据所处的地图上的像素的“障碍偏移度”累加，得到的和，定义为该次假设的“匹配度”。如果假设的位置和方位与实际的机器人在现实中的位置与方位越接近，则点云数据映射后的像素“障碍偏移度”越小，机器人在实际中所扫描到的障碍与地图上的“障碍物像素”的匹配度越大。所以我们选择累加和最小的假设作为定位结果，来代表机器人在现实中的位置和方向。
优化方法：我们把所有“障碍物像素”标记“障碍偏移度”为0后通过数据结构队列把这些像素入队，通过广度优先搜索原理，使用4-联通扩展法，使得扩展的像素赋值为出队像素的值+1，即可在O(Height*Width)复杂度内完成对所有像素“障碍偏移度”的预处理并保存下来，在假设推演阶段中映射点云数据后，可以直接使用。
下面为采用上述方法实现的移动机器人在实验室房间内的初始定位与移动中实时定位，包括以下内容：
一、环境2D地图与实地示意
如图2如示，机器人使用的2D地图的有效部分由黑白两种像素构成，黑色部分代表环境中的障碍物，白色代表空闲空间。而灰色部分是地图中的无效部分。而图3为真实的实验环境的布置，根据我们激光传感器的原理，真实环境可以与2D地图做到相对应。其中，根据描述文件对于(Xlowerleft,Ylowerleft)的记录和地图大小Width与Height可以计算出图中“+”处为世界地图中的原点(0,0)所在处。
二、移动机器人平台
我们使用Kobuki移动机器人底座，和廉价的激光雷达传感器，其连接如图4所示。我们使用一台miniPC作为机器人的控制器，安装ROS机器人操作系统来搭建移动机器人操作系统。
激光雷达的内部数据格式和内部坐标系示意如图5和图6所示。
三、机器人初始位置的定位
如图7所示，我们对地图进行分块，横纵交叉线条的交点即为棋盘分块后代表该块的像素。我们对这些交点进行从上到下，从左到右，进行枚举假设。每一个交点像素，我们假设为机器人的起始位置，在这个基础上，我们对360度的朝向也要进行假设，其中每15度做一次假设验证。
在创建匹配度模型的过程中，需要计算像素的“障碍偏差度”，我们以地图中一小部分障碍为例，如图8a所示。图中一小部分障碍我们放大以后对应的其像素的障碍偏差度矩阵为图8b中所描述的，其中星号代表该像素本身就是障碍物像素，它的偏差度就是0，其余像素的偏差度为距离其最近的障碍物像素的曼哈顿距离，保存在矩阵中的数字如图所示。
根据坐标系转换公式，我们把激光雷达的数据映射到地图上。如图9所示，图中的黑色模型位置为映射激光雷达数据得到的地图匹配度的和最优的像素和方向。与图3中机器人实际的位置近似对应。
四、移动机器人运动跟踪
如图10所示，移动机器人在环境中可以实现避障和导航，机器人系统中根据地图中的机器人位置和环境地图中空闲的部分，找寻到可行路径。根据我们定位好的机器人的初始位置，我们在控制机器人的移动中，可以使用蒙特卡洛算法，根据机器人的轮胎运动信息和机器人传感器跟踪机器人在运动中的位置，如图11所示。ROS系统可以支持每秒10帧的对移动机器人的实时定位与跟踪。
以上所述实施例只为本发明之较佳实施例，并非以此限制本发明的实施范围，故凡依本发明之形状、原理所作的变化，均应涵盖在本发明的保护范围内。
Claims (5)
1.一种基于激光雷达与地图匹配的移动机器人实时定位方法，其特征在于，包括如下步骤：
1)建立2D地图：使用SLAM算法，机器人在环境中移动的过程中，根据激光雷达测距的原理，建立环境的2D障碍地图；在地图中，有效区域被分成两类，分别是障碍物像素和空闲像素，其中障碍物像素代表此像素对应的现实空间有障碍物机器人无法到达或越过，而空闲像素代表该空间没有障碍，机器人能够通过；
2)对2D地图进行分块：对地图的横向与纵向，以相等的距离进行划分，使地图被分成正方形块；以每个块的左上角的像素来代表该块，对地图分块能够在不影响定位的实际效果的情况下降低精度以提高时间与空间效率；
3)假设推演：对于步骤2)中每一个的分块，假设机器人的起始位置处于该块的左上角，并且枚举假设机器人面对的方向；对于每一次假设，把当前激光雷达所扫描到的点云图按照假设的位置为中心，按假设的方向将所有的点云数据通过坐标系转换的方式映射到地图像素中；
4)建立匹配度模型：根据步骤3)中做的每一次假设，需要建立匹配度模型来评判假设的优劣，找出最优的假设，也就是实际上机器人在环境中所处的位置，以映射点云数据后得到的像素点和地图中最近的“障碍物像素”的曼哈顿距离作为匹配度模型的评判依据；
其中，所述建立匹配度模型来评判假设的优劣，从而寻找出最优的位置和方向，来定位机器人的起始位置，具体如下：
先定义一个像素的“障碍偏移度”，如果一个像素是一个“障碍物像素”，则取它的“障碍偏移度”为0，如果一个像素是一个“空闲像素”，则它的“障碍偏移度”为它到距离最近的“障碍物像素”的曼哈顿距离；
对于每一个假设的位置和方位，映射点云数据后把得到的点云数据所处的地图上的像素的“障碍偏移度”累加，得到的和，定义为该次假设的“匹配度”；如果假设的位置和方位与实际的机器人在现实中的位置与方位越接近，则点云数据映射后的像素“障碍偏移度”越小，机器人在实际中所扫描到的障碍与地图上的“障碍物像素”的匹配度越大；所以选择累加和最小的假设作为定位结果，来代表机器人在现实中的位置和方向；
5)利用蒙特卡洛算法跟踪机器人：根据步骤4)的描述，通过假设，验证出机器人当前启动的时候所处的近似的初始位置，当机器人被控制着移动的时候，使用蒙特卡洛算法与粒子滤波器结合，能够实时跟踪机器人的移动状态并且把机器人的实时位置更新在地图上；
6)异常处理：当机器人被搬离地面的时候，通过光电传感器检测到机器人轮胎离地，则停止对机器人发出控制指令，并且报告异常状态，直到机器人重新在地面上被安放好，这时重新启动步骤2)～4)，重新确定机器人的当前的位置作为新的起始位置，之后进入步骤5)正常跟踪机器人的实时位置。
2.根据权利要求1所述的一种基于激光雷达与地图匹配的移动机器人实时定位方法，其特征在于：步骤1)中所建立的2D地图，地图与现实世界能够做到相对应，地图上的像素代表现实中的5cm×5cm的区域，现实中无高度信息的2D位置能够映射地图上；在地图文件中有地图描述部分，包括地图文件的宽度Width像素和高度Height像素；为了建立地图与现实世界的联系，分别建立世界地图坐标系和地图文件像素坐标系：
地图文件由Width X Height像素的图片组成，看成一个矩阵，每一个像素由行列(height,width)坐标确定，其中图片左上位置为(0,0)，width为宽度方向，height为高度方向；通过定义地图(Height-1，0)像素也就是左下角像素的世界坐标(Xlowerleft,Ylowerleft)，默认世界坐标按照米为单位，则现实世界的世界坐标(x,y)和地图上像素(height,width)的对应关系为：
x＝Xlowerleft+width·resolution
y＝Ylowerleft+(Height-height)·resolution
其中resolution为0.05，代表每像素长宽0.05米，即地图的比例尺。
3.根据权利要求1所述的一种基于激光雷达与地图匹配的移动机器人实时定位方法，其特征在于，步骤2)中所述分块方法，具体如下：
由于移动机器人大小以半径20cm的产品居多，所以初始定位的精度在20cm以内即可，根据步骤1)中所设定的地图比例尺，把4×4像素结合成一个像素分块，也就是说实际环境中20cm×20cm的区域为一块，以每一块的左上角的像素代表该块区域，能够提高定位的时间和空间效率。
4.根据权利要求1所述的一种基于激光雷达与地图匹配的移动机器人实时定位方法，其特征在于，步骤3)中所述假设推演过程，具体如下：
从上至下，从左至右地枚举地图上的每一个分块，以分块左上角的像素点代表该分块；假设这个像素点所代表的现实中的区域即为机器人启动时所处的起始位置，并且还需要枚举机器人所面向的方向；激光雷达通过传送各个角度的障碍物距离信息来构成激光点云数据；定义disti为激光雷达内部极坐标系中i度位置上障碍物距离激光雷达中心的距离，单位为米；
当假设地图上(width,height)像素为机器人启动时在现实中所对应的位置，并且θ为机器人所面对的方向的时候，激光雷达的点云数据disti将被映射到地图上的位置(heighti,widthi)为：
这样，通过坐标系转换，在每一个位置和方向的假设中，都能够把点云数据映射到地图上。
5.根据权利要求1所述的一种基于激光雷达与地图匹配的移动机器人实时定位方法，其特征在于：把所有“障碍物像素”标记“障碍偏移度”为0后通过数据结构队列把这些像素入队，通过广度优先搜索原理，使用4-联通扩展法，使得扩展的像素赋值为出队像素的值+1，即可在O(Height*Width)复杂度内完成对所有像素“障碍偏移度”的预处理并保存下来，在假设推演阶段中映射点云数据后，能够直接使用。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710475024.6A CN107390681B (zh) | 2017-06-21 | 2017-06-21 | 一种基于激光雷达与地图匹配的移动机器人实时定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710475024.6A CN107390681B (zh) | 2017-06-21 | 2017-06-21 | 一种基于激光雷达与地图匹配的移动机器人实时定位方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107390681A CN107390681A (zh) | 2017-11-24 |
| CN107390681B true CN107390681B (zh) | 2019-08-20 |
Family
ID=60333502
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710475024.6A Expired - Fee Related CN107390681B (zh) | 2017-06-21 | 2017-06-21 | 一种基于激光雷达与地图匹配的移动机器人实时定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107390681B (zh) |
Families Citing this family (28)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106643701B (zh) * | 2017-01-16 | 2019-05-14 | 深圳优地科技有限公司 | 一种机器人互相检测方法及装置 |
| EP3527938A1 (en) * | 2018-02-15 | 2019-08-21 | Leica Geosystems AG | Distance measuring system with layout generation functionality |
| CN108519091B (zh) * | 2018-04-28 | 2022-03-08 | 上海智蕙林医疗科技有限公司 | 一种地图的使用管理方法及系统 |
| CN108759844B (zh) * | 2018-06-07 | 2021-11-16 | 科沃斯商用机器人有限公司 | 机器人重定位与环境地图构建方法、机器人及存储介质 |
| CN108981701B (zh) * | 2018-06-14 | 2022-05-10 | 广东易凌科技股份有限公司 | 一种基于激光slam的室内定位和导航方法 |
| CN108919300B (zh) * | 2018-07-17 | 2022-07-08 | 重庆大学 | 一种面向仓库通道场景的混合地图创建方法 |
| CN109062211B (zh) * | 2018-08-10 | 2021-12-10 | 远形时空科技（北京）有限公司 | 一种基于slam识别临近空间的方法、装置、系统及存储介质 |
| CN109459734B (zh) * | 2018-10-30 | 2020-09-11 | 百度在线网络技术（北京）有限公司 | 一种激光雷达定位效果评估方法、装置、设备及存储介质 |
| CA3028599A1 (en) * | 2018-11-15 | 2020-05-15 | Beijing Didi Infinity Technology And Development Co., Ltd. | Systems and methods for correcting a high-definition map based on detection of obstructing objects |
| CN113260293B (zh) * | 2018-12-20 | 2023-02-28 | 尚科宁家运营有限公司 | 具有用于估计机器人清洁器的速度的距离传感器的机器人清洁器 |
| CN111443699B (zh) * | 2018-12-28 | 2023-05-23 | 深圳市优必选科技有限公司 | 定位漂移检测方法及装置、机器人 |
| CN109682382B (zh) * | 2019-02-28 | 2020-09-08 | 电子科技大学 | 基于自适应蒙特卡洛和特征匹配的全局融合定位方法 |
| CN110189366B (zh) * | 2019-04-17 | 2021-07-06 | 北京迈格威科技有限公司 | 一种激光粗配准方法、装置、移动终端及存储介质 |
| CN110068824B (zh) * | 2019-04-17 | 2021-07-23 | 北京地平线机器人技术研发有限公司 | 一种传感器位姿确定方法和装置 |
| CN110361026B (zh) * | 2019-06-05 | 2020-12-22 | 华南理工大学 | 一种基于3d点云的仿人机器人路径规划方法 |
| CN112179361B (zh) | 2019-07-02 | 2022-12-06 | 华为技术有限公司 | 更新移动机器人工作地图的方法、装置及存储介质 |
| CN110428438B (zh) * | 2019-07-11 | 2022-09-02 | 广州启量信息科技有限公司 | 一种单木建模方法、装置和存储介质 |
| CN112445205B (zh) * | 2019-08-15 | 2024-06-14 | 广州极飞科技股份有限公司 | 地面植保设备的控制方法、装置、设备和存储介质 |
| CN112805534B (zh) * | 2019-08-27 | 2024-05-17 | 北京航迹科技有限公司 | 定位目标对象的系统和方法 |
| CN110531766B (zh) * | 2019-08-27 | 2022-06-28 | 熵智科技(深圳)有限公司 | 基于已知占据栅格地图的持续激光slam构图定位方法 |
| CN110988586B (zh) * | 2019-10-28 | 2021-11-16 | 国电南瑞科技股份有限公司 | 一种配网带电作业机器人作业精准定位方法及系统 |
| CN110908374B (zh) * | 2019-11-14 | 2021-04-23 | 华南农业大学 | 一种基于ros平台的山地果园避障系统及方法 |
| CN112923933A (zh) * | 2019-12-06 | 2021-06-08 | 北理慧动（常熟）车辆科技有限公司 | 一种激光雷达slam算法与惯导融合定位的方法 |
| CN111240318A (zh) * | 2019-12-24 | 2020-06-05 | 华中农业大学 | 一种机器人的人员发现算法 |
| CN111398984B (zh) * | 2020-03-22 | 2022-03-29 | 华南理工大学 | 基于扫地机器人的自适应激光雷达点云校正与定位方法 |
| CN111462072B (zh) * | 2020-03-30 | 2023-08-29 | 北京百度网讯科技有限公司 | 点云图质量检测方法、装置以及电子设备 |
| CN112113565A (zh) * | 2020-09-22 | 2020-12-22 | 温州科技职业学院 | 一种农业温室环境的机器人定位系统 |
| CN117008598A (zh) * | 2023-01-03 | 2023-11-07 | 北京石头创新科技有限公司 | 用于使设备向指定位置行进的方法、装置及自移动设备 |
Family Cites Families (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2015017691A1 (en) * | 2013-08-02 | 2015-02-05 | Irobot Corporation | Time-dependent navigation of telepresence robots |
| US9436926B2 (en) * | 2014-02-25 | 2016-09-06 | Savioke, Inc. | Entryway based authentication system |
| CN105607071B (zh) * | 2015-12-24 | 2018-06-08 | 百度在线网络技术（北京）有限公司 | 一种室内定位方法及装置 |
| CN105865449B (zh) * | 2016-04-01 | 2020-05-05 | 深圳市杉川机器人有限公司 | 基于激光和视觉的移动机器人的混合定位方法 |
| CN105928505B (zh) * | 2016-04-19 | 2019-01-29 | 深圳市神州云海智能科技有限公司 | 移动机器人的位姿确定方法和设备 |
| CN106052674B (zh) * | 2016-05-20 | 2019-07-26 | 青岛克路德机器人有限公司 | 一种室内机器人的slam方法和系统 |
| CN106199626B (zh) * | 2016-06-30 | 2019-08-09 | 上海交通大学 | 基于摆动激光雷达的室内三维点云地图生成系统及方法 |
| CN106681331A (zh) * | 2017-02-24 | 2017-05-17 | 王红军 | 一种基于测地线理论的栅格路径规划方法 |
-
2017
- 2017-06-21 CN CN201710475024.6A patent/CN107390681B/zh not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| CN107390681A (zh) | 2017-11-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107390681B (zh) | 一种基于激光雷达与地图匹配的移动机器人实时定位方法 | |
| US12033388B2 (en) | Positioning method, apparatus, device, and computer-readable storage medium | |
| CN109283538B (zh) | 一种基于视觉和激光传感器数据融合的海上目标大小检测方法 | |
| CN102800083B (zh) | 基于双目视觉网格划分匹配算法的农作物喷雾定位方法 | |
| KR102428050B1 (ko) | 정보 보완 방법, 차선 인식 방법, 지능형 주행 방법 및 관련 제품 | |
| US20210078173A1 (en) | System and method of controlling obstacle avoidance of robot, robot and storage medium | |
| CN103064416B (zh) | 巡检机器人室内外自主导航系统 | |
| CN110737271B (zh) | 一种水面机器人自主巡航系统及方法 | |
| CN103901895B (zh) | 一种基于无极FastSLAM算法和匹配优化目标定位方法及机器人 | |
| CN106052674A (zh) | 一种室内机器人的slam方法和系统 | |
| CN107831777A (zh) | 一种飞行器自主避障系统、方法及飞行器 | |
| CN107368094A (zh) | 一种无人机植保作业航线规划方法及装置 | |
| JP2023522262A (ja) | 地図生成方法、装置、記憶媒体及びプロセッサ | |
| CN105856243A (zh) | 一种移动智能机器人 | |
| CN109658432A (zh) | 一种移动机器人的边界生成方法及系统 | |
| CN112150805B (zh) | 一种可行驶区域的确定方法、装置、设备及存储介质 | |
| CN111207762B (zh) | 地图生成方法、装置、计算机设备和存储介质 | |
| CN109760064A (zh) | 一种移动机器人自身位置的调整方法和装置 | |
| CN108789421A (zh) | 基于云平台的云机器人交互方法和云机器人及云平台 | |
| CN115774444B (zh) | 一种基于稀疏导航地图的路径规划优化方法 | |
| CN112232139B (zh) | 一种基于Yolo v4与Tof算法相结合的避障方法 | |
| CN111612823A (zh) | 一种基于视觉的机器人自主跟踪方法 | |
| CN110058263A (zh) | 一种车辆行驶过程中的物体定位方法 | |
| CN109685849A (zh) | 一种移动机器人的出界判定方法及系统 | |
| CN116339351A (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 | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20190820 |
|
| CF01 | Termination of patent right due to non-payment of annual fee |