CN111504317B - 一种基于单线激光雷达的室内定位方法 - Google Patents

一种基于单线激光雷达的室内定位方法 Download PDF

Info

Publication number
CN111504317B
CN111504317B CN202010156745.2A CN202010156745A CN111504317B CN 111504317 B CN111504317 B CN 111504317B CN 202010156745 A CN202010156745 A CN 202010156745A CN 111504317 B CN111504317 B CN 111504317B
Authority
CN
China
Prior art keywords
map
data
matching
local
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.)
Active
Application number
CN202010156745.2A
Other languages
English (en)
Other versions
CN111504317A (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.)
Zhongzhen Tongfu Jiangsu Robot Co ltd
Original Assignee
Zhongzhen Tongfu Jiangsu Robot Co ltd
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 Zhongzhen Tongfu Jiangsu Robot Co ltd filed Critical Zhongzhen Tongfu Jiangsu Robot Co ltd
Priority to CN202010156745.2A priority Critical patent/CN111504317B/zh
Publication of CN111504317A publication Critical patent/CN111504317A/zh
Application granted granted Critical
Publication of CN111504317B publication Critical patent/CN111504317B/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/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于单线激光雷达的室内定位方法，包括以下步骤：(1)使用单线激光雷达采集的得到的数据构建占据栅格地图；(2)采用步骤(1)中的方法构建局部地图，并进行局部匹配；(3)采用步骤(1)中的方法构建全局地图，并进行全局匹配；(4)当得到一次全局匹配结果时，将全局地图与局部地图在该次全局匹配结果的时间戳上约束并加入优化队列；为避免偶然误差，在优化队列达到一定数量时进行一次最小二乘优化，更新当前局部地图中的历史定位轨迹，并将每个点位姿更新，以最终得到实时精确的位姿。本发明一种基于单线激光雷达的室内定位方法，能够实现多种场景的高精度室内定位，为机器人稳定运行及工作提供了较大的支持。

Description

一种基于单线激光雷达的室内定位方法
技术领域
本发明涉及一种基于单线激光雷达的室内定位方法。
背景技术
在室内机器人领域，机器人需要时刻确定自身在环境中的位置以执行下一步任务。常见的应用场景有扫地机器人，仓储机器人、车辆车库内自动泊车等。
由于室内难以接收到卫星信号，因此GPS等定位方法无法在室内进行快速准确地定位，因此室内一般采用其他传感器进行定位，目前常见的室内定位方案包括Wifi、蓝牙、RFID等，但这些定位方式都需要对已有的室内场景加以改造，如增加信号发射器等，不便于使用。另外，采用这些方式的室内定位精度难以达到厘米级，无法应用在对定位精度要求较高的场景。
发明内容
本发明的目的是提供一种基于单线激光雷达的室内定位方法，能够实现多种场景的高精度室内定位。
为达到上述目的，本发明采用的技术方案是：
一种基于单线激光雷达的室内定位方法，包括以下步骤：
(1)使用单线激光雷达采集的得到的数据构建占据栅格地图：在所述占据栅格地图中，对于一个点，用p(s＝0)来表示它是空状态的概率，用p(s＝1)来表示它是占据状态的概率，两者的和为1；在此引入两者的比值来作为点的状态：Odd(s)＝p(s＝1)/p(s＝0)；在所述占据栅格地图中，一个点状态的数值越大，就表示越肯定它是占据状态，相反数值越小，就表示越肯定它是空状态；
(2)采用步骤(1)中的方法构建局部地图，并进行局部匹配；
(3)采用步骤(1)中的方法构建全局地图，并进行全局匹配；
(4)每得到一次全局匹配结果时，将所述全局地图与所述局部地图在该次全局匹配结果的时间戳上约束并加入优化队列；为避免偶然误差，在优化队列达到一定数量时进行一次最小二乘优化，更新当前所述局部地图中的历史定位轨迹，并将每个点位姿更新，以最终得到实时精确的位姿。
优选地，在步骤(1)中，对于一个点，若出现新的测量值z，需要更新其状态，即更新为在测量值z发生条件下的s的状态，通过不断更新，以构建所述占据栅格地图。
优选地，在步骤(2)中，所述局部地图始终保留一定范围内的数据，随着机器人的运动，所述局部地图会不断更新，新的局部地图数据产生的同时丢弃旧的局部地图数据。
优选地，在步骤(2)中，设当前测量数据为
Figure BDA0002404336670000021
用空间变换函数
Figure BDA0002404336670000022
表示将
Figure BDA0002404336670000023
做
Figure BDA0002404336670000024
的变换，则匹配问题可以转化为最大化似然函数：
Figure BDA0002404336670000025
在式(1)中，p为概率密度函数，由上一时刻的测量数据进行栅格化，然后计算每个网格的多维正态分布得到；式(1)可等价于：
Figure BDA0002404336670000026
在式(2)中，将当前时刻的测量数据
Figure BDA0002404336670000027
的每个数据点找出对应的网格点，将其代入概率密度函数p进行计算，并对式(2)使用牛顿法进行迭代求解，直到收敛；当收敛时，即求得最大化似然函数情况下的
Figure BDA0002404336670000028
求出空间变换
Figure BDA0002404336670000029
即可由上一时刻位姿解算出当前时刻位姿，即完成了局部定位。
更优选地，在步骤(3)中，在所述全局地图的基础上，构造由粗到细的多张不同分辨率的构造地图：
(a)将测量数据分别顺时针旋转0～360°，具体角分辨率需要根据实际情况选择，得到新的不同旋转角度下的测量数据；
(b)将不同旋转角度下的测量数据按照多个分辨率分别进行栅格化，将这些栅格化后的数据作为候选者；
(c)取最低分辨率的所述构造地图，将候选者从原点开始按照该分辨率下的一定步长进行平移；持续将平移后得到的数据与所述构造地图进行匹配，直至平移后得到的数据高于事先设定的可信度阈值，将该数据作为筛选后的候选者；
(d)将筛选后的候选者与更高分辨率的所述构造地图进行同样的策略匹配，直到得到最高分辨率下可信度最高的平移值和旋转值，则为匹配得出的位姿结果。
更进一步优选地，在步骤(c)中，所述匹配方式与所述局部匹配的方式相同。
由于上述技术方案的运用，本发明与现有技术相比具有下列优点：本发明一种基于单线激光雷达的室内定位方法，能够实现多种场景的高精度室内定位，为机器人稳定运行及工作提供了较大的支持。
附图说明
附图1为本实施例中构建得到的占据栅格地图；
附图2为本实施例中通过本发明算法得到的位置轨迹与使用高精度测量仪器测量得到的真实位置点的参照对比图。
具体实施方式
下面结合附图来对本发明的技术方案作进一步的阐述。
上述一种基于单线激光雷达的室内定位方法，包括以下步骤：
(1)使用单线激光雷达采集的得到的数据构建占据栅格地图，单线激光雷达会向固定的方向发射激光束，发射出的激光遇到障碍物会被反射，这样就能得到激光从发射到收到的时间差，就可以计算出单线激光雷达到该方向上最近障碍物的距离。
在占据栅格地图中，对于一个点，用p(s＝0)来表示它是空状态的概率，用p(s＝1)来表示它是占据状态的概率，两者的和为1。在此引入两者的比值来作为点的状态：Odd(s)＝p(s＝1)/p(s＝0)。在占据栅格地图中，一个点状态的数值越大，就表示越肯定它是占据状态，相反数值越小，就表示越肯定它是空状态；对于一个点，若出现新的测量值z，需要更新其状态，即更新为在测量值z发生条件下的s的状态，通过不断更新，以构建该占据栅格地图。使用单线激光雷达的数据通过以上方法在某一楼层建立的占据栅格地图如图1所示，其中，颜色越深，代表障碍物出现几率越大，颜色越淡，代表障碍物出现几率越小。
考虑到计算资源以及实时性，在此将匹配问题分为两个子问题：局部定位及全局定位问题。局部定位为较短时间内机器人行进过程中的定位问题，即已知上一时刻的位姿，在局部地图内估计当前时刻的位姿。由于局部定位的误差会累积，因此需要辅助全局定位来不断纠正偏移，减小误差。局部匹配算法可以理解为前后两次测量值之间的匹配，全局匹配算法为当前测量值与全局地图的匹配。局部匹配实时但存在误差累积；全局匹配较为耗时但精度高，可消除累积误差。
(2)采用步骤(1)中的方法构建局部地图，并进行局部匹配；局部地图始终保留一定范围内的数据，随着机器人的运动，局部地图会不断更新，新的局部地图数据产生的同时丢弃旧的局部地图数据。
设当前测量数据为
Figure BDA0002404336670000041
用空间变换函数
Figure BDA0002404336670000042
表示将
Figure BDA0002404336670000043
做
Figure BDA0002404336670000044
的变换，则匹配问题可以转化为最大化似然函数：
Figure BDA0002404336670000045
在式(1)中，p为概率密度函数，由上一时刻的测量数据进行栅格化，然后计算每个网格的多维正态分布得到；式(1)可等价于：
Figure BDA0002404336670000046
在式(2)中，将当前时刻的测量数据
Figure BDA0002404336670000047
的每个数据点找出对应的网格点，将其代入概率密度函数p进行计算，并对式(2)使用牛顿法进行迭代求解，直到收敛；当收敛时，即求得最大化似然函数情况下的
Figure BDA0002404336670000048
求出空间变换
Figure BDA0002404336670000049
即可由上一时刻位姿解算出当前时刻位姿，即完成了局部定位。
(3)采用步骤(1)中的方法构建全局地图，并进行全局匹配；该建图过程建立的是涵盖整个场景的地图。
在全局地图的基础上，构造由粗到细的多张不同分辨率的构造地图：
(a)将测量数据分别顺时针旋转0～360°，具体角分辨率需要根据实际情况选择，以得到新的不同旋转角度下的测量数据；
(b)将不同旋转角度下的测量数据按照多个分辨率分别进行栅格化，将这些栅格化后的数据作为候选者；
(c)取最低分辨率的构造地图，将候选者从原点开始按照该分辨率下的一定步长进行平移；持续将平移后得到的数据与构造地图进行匹配(匹配方式与局部匹配的方式相同)，直至平移后得到的数据高于事先设定的可信度阈值，将该数据作为筛选后的候选者；
(d)将筛选后的候选者与更高分辨率的构造地图进行同样的策略匹配，直到得到最高分辨率下可信度最高的平移值和旋转值，则为匹配得出的位姿结果。
(4)每得到一次全局匹配结果时，将全局地图与局部地图在该次全局匹配结果的时间戳上约束并加入优化队列；为避免偶然误差，在优化队列达到一定数量时进行一次最小二乘优化，更新当前局部地图中的历史定位轨迹，并将每个点位姿更新，以最终得到实时精确的位姿。
参考图2所示，其中连续曲线为为本发明算法定位得到的机器人位置轨迹，点状轨迹为使用高精度测量仪器测量得到的真实位置点轨迹。可以看出，本发明算法的定位结果与实际位置的吻合度较高。
上述实施例只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人士能够了解本发明的内容并加以实施，并不能以此限制本发明的保护范围，凡根据本发明精神实质所作的等效变化或修饰，都应涵盖在本发明的保护范围内。

Claims (3)

1.一种基于单线激光雷达的室内定位方法，其特征在于：包括以下步骤：
(1)使用单线激光雷达采集的得到的数据构建占据栅格地图：在所述占据栅格地图中，对于一个点，用p(s＝0)来表示它是空状态的概率，用p(s＝1)来表示它是占据状态的概率，两者的和为1；在此引入两者的比值来作为点的状态：Odd(s)＝p(s＝1)/p(s＝0)；在所述占据栅格地图中，一个点状态的数值越大，就表示越肯定它是占据状态，相反数值越小，就表示越肯定它是空状态；
(2)采用步骤(1)中的方法构建局部地图，并进行局部匹配；
(3)采用步骤(1)中的方法构建全局地图，并进行全局匹配；
(4)每得到一次全局匹配结果时，将所述全局地图与所述局部地图在该次全局匹配结果的时间戳上约束并加入优化队列；为避免偶然误差，在优化队列达到一定数量时进行一次最小二乘优化，更新当前所述局部地图中的历史定位轨迹，并将每个点位姿更新，以最终得到实时精确的位姿；
在步骤(2)中，所述局部地图始终保留一定范围内的数据，随着机器人的运动，所述局部地图会不断更新，新的局部地图数据产生的同时丢弃旧的局部地图数据；
在步骤(2)中，设当前测量数据为
Figure FDA0003259006090000011
用空间变换函数
Figure FDA0003259006090000012
表示将
Figure FDA0003259006090000013
做
Figure FDA0003259006090000014
的变换，则匹配问题可以转化为最大化似然函数：
Figure FDA0003259006090000015
在式(1)中，p为概率密度函数，由上一时刻的测量数据进行栅格化，然后计算每个网格的多维正态分布得到；式(1)可等价于：
Figure FDA0003259006090000016
在式(2)中，将当前时刻的测量数据
Figure FDA0003259006090000017
的每个数据点找出对应的网格点，将其代入概率密度函数p进行计算，并对式(2)使用牛顿法进行迭代求解，直到收敛；当收敛时，即求得最大化似然函数情况下的
Figure FDA0003259006090000018
求出空间变换
Figure FDA0003259006090000019
即可由上一时刻位姿解算出当前时刻位姿，即完成了局部定位；
在步骤(3)中，在所述全局地图的基础上，构造由粗到细的多张不同分辨率的构造地图：
(a)将测量数据分别顺时针旋转0～360°，具体角分辨率需要根据实际情况选择，得到新的不同旋转角度下的测量数据；
(b)将不同旋转角度下的测量数据按照多个分辨率分别进行栅格化，将这些栅格化后的数据作为候选者；
(c)取最低分辨率的所述构造地图，将候选者从原点开始按照该分辨率下的一定步长进行平移；持续将平移后得到的数据与所述构造地图进行匹配，直至平移后得到的数据高于事先设定的可信度阈值，将该数据作为筛选后的候选者；
(d)将筛选后的候选者与更高分辨率的所述构造地图进行同样的策略匹配，直到得到最高分辨率下可信度最高的平移值和旋转值，则为匹配得出的位姿结果。
2.根据权利要求1所述的一种基于单线激光雷达的室内定位方法，其特征在于：在步骤(1)中，对于一个点，若出现新的测量值z，需要更新其状态，即更新为在测量值z发生条件下的s的状态，通过不断更新，以构建所述占据栅格地图。
3.根据权利要求1所述的一种基于单线激光雷达的室内定位方法，其特征在于：在步骤(c)中，所述匹配方式与所述局部匹配的方式相同。
CN202010156745.2A 2020-03-09 2020-03-09 一种基于单线激光雷达的室内定位方法 Active CN111504317B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010156745.2A CN111504317B (zh) 2020-03-09 2020-03-09 一种基于单线激光雷达的室内定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010156745.2A CN111504317B (zh) 2020-03-09 2020-03-09 一种基于单线激光雷达的室内定位方法

Publications (2)

Publication Number Publication Date
CN111504317A CN111504317A (zh) 2020-08-07
CN111504317B true CN111504317B (zh) 2021-11-16

Family

ID=71868989

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010156745.2A Active CN111504317B (zh) 2020-03-09 2020-03-09 一种基于单线激光雷达的室内定位方法

Country Status (1)

Country Link
CN (1) CN111504317B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113298870B (zh) * 2021-05-07 2023-03-28 中国科学院深圳先进技术研究院 一种物体的姿态跟踪方法、装置、终端设备和存储介质

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102017217972A1 (de) * 2017-10-10 2019-04-11 Robert Bosch Gmbh Verfahren und Vorrichtung zum Erzeugen eines inversen Sensormodells und Verfahren zum Erkennen von Hindernissen
CN109978925B (zh) * 2017-12-27 2021-04-20 深圳市优必选科技有限公司 一种机器人位姿的识别方法及其机器人
CN110376604B (zh) * 2019-08-09 2022-11-15 北京智行者科技股份有限公司 基于单线激光雷达的路沿检测方法
CN110531766B (zh) * 2019-08-27 2022-06-28 熵智科技(深圳)有限公司 基于已知占据栅格地图的持续激光slam构图定位方法

Also Published As

Publication number Publication date
CN111504317A (zh) 2020-08-07

Similar Documents

Publication Publication Date Title
CN108921947B (zh) 生成电子地图的方法、装置、设备、存储介质以及采集实体
CN110645974B (zh) 一种融合多传感器的移动机器人室内地图构建方法
KR102628778B1 (ko) 위치결정을 위한 방법, 컴퓨팅 기기, 컴퓨터 판독가능 저장 매체 및 컴퓨터 프로그램
CN110889808B (zh) 一种定位的方法、装置、设备及存储介质
CN111680673B (zh) 一种栅格地图中动态物体的检测方法、装置及设备
CN112363158B (zh) 机器人的位姿估计方法、机器人和计算机存储介质
CN112904358B (zh) 基于几何信息的激光定位方法
US20070026872A1 (en) Method for determining a relative position of a mobile unit by comparing scans of an environment and mobile unit
CN111066064A (zh) 使用误差范围分布的网格占用建图
KR101888295B1 (ko) 레이저 거리 센서의 측정 거리에 대해 추정된 거리 유형의 신뢰성을 평가하는 방법 및 이를 이용한 이동 로봇의 위치 추정 방법
CN112731354B (zh) Agv上激光雷达位姿的自标定方法
CN111913169B (zh) 激光雷达内参、点云数据的修正方法、设备及存储介质
CN112731337B (zh) 地图构建方法、装置和设备
CN111678516B (zh) 一种基于激光雷达的有界区域快速全局定位方法
CN113933820B (zh) 一种无标定物的激光雷达外参标定方法
CN112505671B (zh) Gnss信号缺失环境下毫米波雷达目标定位方法及装置
CN111504317B (zh) 一种基于单线激光雷达的室内定位方法
CN114565726A (zh) 一种非结构化动态环境下的同时定位与建图方法
CN117590362B (zh) 一种多激光雷达外参标定方法、装置和设备
CN114111791B (zh) 一种智能机器人室内自主导航方法、系统及存储介质
EP3940670B1 (en) Information processing device and mobile robot
CN113566817B (zh) 一种车辆定位方法及装置
CN113534110B (zh) 一种多激光雷达系统静态标定方法
CN114488178A (zh) 定位方法及设备
CN114659518B (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