CN112612862B - 一种基于点云配准的栅格地图定位方法 - Google Patents

一种基于点云配准的栅格地图定位方法 Download PDF

Info

Publication number
CN112612862B
CN112612862B CN202011550794.0A CN202011550794A CN112612862B CN 112612862 B CN112612862 B CN 112612862B CN 202011550794 A CN202011550794 A CN 202011550794A CN 112612862 B CN112612862 B CN 112612862B
Authority
CN
China
Prior art keywords
particles
point cloud
grid map
weight
pose
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
CN202011550794.0A
Other languages
English (en)
Other versions
CN112612862A (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.)
Wuhu Robot Technology Research Institute of Harbin Institute of Technology
Original Assignee
Wuhu Robot Technology Research Institute of Harbin Institute of Technology
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 Wuhu Robot Technology Research Institute of Harbin Institute of Technology filed Critical Wuhu Robot Technology Research Institute of Harbin Institute of Technology
Priority to CN202011550794.0A priority Critical patent/CN112612862B/zh
Publication of CN112612862A publication Critical patent/CN112612862A/zh
Application granted granted Critical
Publication of CN112612862B publication Critical patent/CN112612862B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
    • G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
    • G06F16/29—Geographical information databases
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T11/00—2D [Two Dimensional] image generation
    • G06T11/001—Texturing; Colouring; Generation of texture or colour

Landscapes

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

Abstract

本发明公开了一种基于点云配准的栅格地图定位方法，包括：在初始位姿附近根据高斯模型随机分布粒子；将当前激光帧点云与栅格地图进行相关性匹配，将得分最高的候选位姿作为初始位姿；针对激光帧P1和P2在栅格地图中的投影点构建目标函数T*，迭代输出最优位姿；将最优位姿输送给里程计，里程计完成对所有粒子位姿的预测；根据粒子位置的观测数据，采用似然域计算粒子权重和粒子总权重；根据粒子权重对粒子进行筛选，复制高权重的粒子，舍弃低权重粒子，同时增加随机粒子，更新粒子分布，同时比较更新粒子簇最大权重；检测粒子簇中最大权重是否大于设定的权重阈值，若检测结果为是，则获得此时最佳的位姿。一定程度克服使用里程计时存在的打滑问题。

Description

一种基于点云配准的栅格地图定位方法
技术领域
本发明属于定位技术领域，更具体地，本发明涉及一种基于点云配准的栅格地图定位方法。
背景技术
目前常用的移动机器人定位导航方式主要有电磁导航、磁带导航、二维码导航、激光导航等。像电磁导航、磁带导航等导航方式需要维护，路径柔性差，不适用于复杂路径的状况，并且无法实现精确定位。目前，移动机器人同时定位与地图构建(SLAM)技术成为机器人定位导航研究的热点问题，SLAM可以提高移动机器人的自主能力和环境适应能力，使移动机器人可以在未知的环境中进行自主定位和导航。基于粒子滤波的定位方法已成为主流，它是依据传感器的观测结果更新粒子的权重并进行重采样，不断迭代使粒子收敛至正确的位姿。这种方法严重依赖里程计信息，可能存在里程计打滑的现象而导致定位不准。
现有技术方案是都采用粒子滤波进行定位，随机产生N个粒子组成粒子群，根据机器人传感器测得的机器人实时移动距离和实时旋转角度，更新粒子群；对于每一个粒子计算激光雷达点云与地图的障碍物的重合数量作为每一个粒子的得分，以每一个粒子的得分为权重计算粒子群的加权位姿均值，作为AMCL估计位姿；将AMCL估计位姿作为初始值，使用基于高斯牛顿迭代法的扫描匹配算法，得到扫描匹配位姿，作为机器人运行的当前时刻的最优位姿；使用AMCL算法重采样粒子群，最终得到机器人运行时全程最优位姿作为定位结果。该方案是将AMCL的结果作为初始值，采用高斯牛顿迭代的扫描匹配算法对位姿进一步优化，得到最优位姿。该方案存在如下问题：
使用了里程计数据，可能存在打滑现象，同时它将所有粒子的得分为权重计算粒子群的加权位姿均值，导致AMCL估计位姿不够准确。而本方案采用基于点云配准的栅格地图定位方法，仅仅依靠激光传感器，可快速高效地实现定位，同时一定程度克服使用里程计时存在的打滑问题。
发明内容
本发明提供了一种基于点云配准的栅格地图定位方法，旨在改善上述问题。
本发明是这样实现的，一种基于点云配准的栅格地图定位方法，所述方法包括如下步骤：
S1、构建粒子滤波器，在初始位姿附近根据高斯模型随机分布粒子，保证粒子落入全局地图的空闲区域内；
S2、将当前激光帧点云与栅格地图进行相关性匹配，将得分最高的候选位姿(tx,ty,θ)作为初始位姿；
S3、非线性优化匹配：根据上一步将激光帧点云P2投影到栅格地图G1中，同时将激光帧点云P1投影到栅格地图G2中，针对激光帧P1和P2在栅格地图中的投影点构建目标函数T*，迭代输出最优位姿；
S4、运动模型更新：将步骤S3获得的最优位姿输送给里程计，基于里程计在当前时刻与上一时刻内的位移及转角完成对所有粒子位姿的预测；
S5、激光传感器观测模型更新：根据粒子位置的观测数据，采用似然域计算粒子权重和粒子总权重。
S6、重采样：根据粒子权重对粒子进行筛选，复制高权重的粒子，舍弃低权重粒子，同时增加随机粒子；
S7、粒子滤波器迭代更新：重采样后，更新粒子分布，同时比较更新粒子簇最大权重，
S8、检测粒子簇中最大权重是否大于设定的权重阈值，若检测结果为是，则获得此时最佳的位姿，否则执行步骤S7。
进一步的，所述步骤S1具体包括如下步骤：
S11、生成栅格概率地图：从初始位姿处的激光点云开始，基于相邻两帧的激光点云P1、激光点云P2生成对应的栅格地图G1、栅格地图G2；
S12、相关性匹配：以当前位置为中心，划定搜索窗口，按照位移步长和角度步长在搜索窗口中遍历所有的候选位姿，并在每一个候选位姿下，将点云P2投影到栅格地图G1中，将投影栅格的占据概率累加得到当前候选位姿的得分，将得分最高的候选位姿(tx,ty,θ)作为初始位姿。
进一步的，目标函数如公式(2)所示：
Figure BDA0002857680710000031
Si(T)表示将激光帧中的第i个点云变换到栅格地图坐标系下，M(Si(T))表示激光帧中第i个点云在对应栅格地图上的位置被占据的概率；
进一步的，粒子Si的权重计算公式如(3)、(4)所示：
Figure BDA0002857680710000032
Figure BDA0002857680710000033
基于当前粒子Si的位姿将当前激光帧中的点云转换至栅格地图中，(xk,yk)表示当前激光帧中的第k个点云在栅格地图中的坐标，(x,y)为障碍物坐标；dist表示表示随机粒子距最近障碍物距离，
Figure BDA0002857680710000034
表示测距似然，代表粒子Si的权重，zhit、zranddom及zmax分别代表测距误差混合权重的不同部分，分别代表测量噪声、无法解释的随机测量及测量失败，σhit为测量噪声的标准差。
本发明提出一种基于点云配准的栅格地图定位方法，仅仅依靠激光传感器，可快速高效地实现定位，同时一定程度克服使用里程计时存在的打滑问题。
本发明通过相关性匹配能快速得到相邻点云运动的初始值；非线性优化对相关性匹配的初始值进行进一步的迭代优化，得到相邻点云运动的精确值；再对每个例子进行运动更新，通过似然域地图对粒子的运动进行打分，得到不同的权重，保留权重较高的粒子，去除权重较低的粒子，得到机器人的最有位姿。相交于现有的amcl定位算法，本发明不需要里程计就可取得相同的结果，对设备的适应性更强；此外，本发明可避免里程计打滑带来的定位错误，定位结果的稳定性更好；本发明采用相邻激光帧计算运动量，能有效避免时间戳误差带来的定位误差，定位结果的精度更高。
附图说明
图1为本发明实施例提供的基于点云配准的栅格地图定位方法流程图。
具体实施方式
下面对照附图，通过对实施例的描述，对本发明的具体实施方式作进一步详细的说明，以帮助本领域的技术人员对本发明的发明构思、技术方案有更完整、准确和深入的理解。
图1为本发明实施例提供的基于点云配准的栅格地图定位方法流程图，该方法具体包括如下步骤：
步骤1)初始化粒子滤波器：构建粒子滤波器，在初始位姿附近根据高斯模型随机分布粒子，保证粒子落入全局地图的空闲区域内，空闲区域即为激光雷达探测不存在障碍物的区域，空闲区域为全局地图的有效区域；
步骤2)点云预处理：利用统计概率滤波算法对原始激光点云进行滤波，剔除离散点，获得滤波后的点云。
步骤3)生成栅格概率地图：从初始位姿处的激光点云开始，基于相邻两帧的激光点云P1、P2生成对应的栅格地图G1、G2。
步骤4)相关性匹配：以当前位置为中心，划定搜索窗口，按照位移步长和角度步长在搜索窗口中遍历所有的候选位姿，并在每一个候选位姿下，将点云P2投影到栅格地图G1中，将投影栅格的占据概率累加得到当前候选位姿的得分，将得分最高的候选位姿(tx,ty,θ)作为初始位姿进入下一步的计算。
位移步长为一个栅格分辨率，角度步长如公式(1)所示。
Figure BDA0002857680710000051
r为栅格地图的分辨率，dmax为当前点云的最远点距离，a为角度步长系数。
步骤5)非线性优化匹配。根据上一步将激光帧点云P2投影到栅格地图G1中，同时将激光帧点云P1投影到栅格地图G2中，对所有的点可以构建目标函数T*，迭代更新，输出最优位姿。如公式(2)所示。
Figure BDA0002857680710000052
Si(T)表示将激光帧中的第i个点云变换到栅格地图坐标系下，M(Si(T))表示激光帧中第i个点云在对应栅格地图上的位置被占据的概率。
步骤6)运动模型更新：将步骤5)获得的最优位姿输送给里程计，基于里程计在当前时刻与上一时刻内的位移及转角完成对所有粒子位姿的预测。
步骤7)激光传感器观测模型更新：根据粒子位置的观测数据，采用似然域计算粒子权重和粒子总权重。如公式(3)、(4)所示。
Figure BDA0002857680710000053
Figure BDA0002857680710000054
基于当前粒子Si的位姿将当前激光帧中的点云转换至栅格地图中，(xk,yk)表示当前激光帧中的第k个点云在栅格地图中的坐标，(x,y)为障碍物坐标；dist表示表示随机粒子距最近障碍物距离，
Figure BDA0002857680710000061
表示测距似然，代表粒子Si的权重，zhit、zranddom及zmax分别代表测距误差混合权重的不同部分，分别代表测量噪声、无法解释的随机测量及测量失败，σhit为测量噪声的标准差。
步骤8)重采样：根据粒子权重对粒子进行筛选，复制高权重的粒子，舍弃低权重粒子，同时随机采样如公式(5)所示的概率增加随机粒子。
max{0.0，1.0-wfast/wslaw} (5)
其中，wslow表示短期似然平均；wfast表示长期似然平均。
步骤9)粒子滤波器迭代更新：重采样后，更新粒子分布，同时比较更新粒子簇最大权重，
步骤10)检测粒子簇中最大权重是否大于设定的权重阈值，若检测结果为是，则获得此时最佳的位姿，否则执行步骤9)。
本发明提出一种基于点云配准的栅格地图定位方法，仅仅依靠激光传感器，可快速高效地实现定位，同时一定程度克服使用里程计时存在的打滑问题。并不受上述方式的限制，只要采用了本发明的方法构思和技术方案进行的各种非实质性的改进，或未经改进将本发明的构思和技术方案直接应用于其它场合的，均在本发明的保护范围之内。

Claims (4)

1.一种基于点云配准的栅格地图定位方法，其特征在于，所述方法包括如下步骤：
S1、构建粒子滤波器，在初始位姿附近根据高斯模型随机分布粒子，粒子落入栅格地图的空闲区域；
S2、将当前激光帧点云与栅格地图进行相关性匹配，将得分最高的候选位姿(tx,ty,θ)作为初始位姿；
S3、基于初始位姿将激光帧点云P2投影到栅格地图G1中，将激光帧点云P1投影到栅格地图G2，针对激光帧P1和P2在栅格地图中的投影点构建目标函数T*，迭代输出最优位姿；
S4、将最优位姿输送给里程计，基于里程计在当前时刻与上一时刻内的位移及转角完成对所有粒子位姿的预测；
S5、根据粒子位置的观测数据，采用似然域计算粒子权重和粒子总权重；
S6、根据粒子权重对粒子进行筛选，复制高权重的粒子，舍弃低权重粒子，同时增加随机粒子，更新粒子分布，同时比较更新粒子簇最大权重，
S7、检测粒子簇中最大权重是否大于设定的权重阈值，若检测结果为是，则获得此时最佳的位姿，否则执行步骤S6。
2.如权利要求1所述基于点云配准的栅格地图定位方法，其特征在于，所述步骤S1具体包括如下步骤：
S11、生成栅格概率地图：从初始位姿处的激光点云开始，基于相邻两帧的激光点云P1、激光点云P2生成对应的栅格地图G1、栅格地图G2；
S12、相关性匹配：以当前位置为中心，划定搜索窗口，按照位移步长和角度步长在搜索窗口中遍历所有的候选位姿，并在每一个候选位姿下，将点云P2投影到栅格地图G1中，将投影栅格的占据概率累加得到当前候选位姿的得分，将得分最高的候选位姿(tx,ty,θ)作为初始位姿。
3.如权利要求1所述基于点云配准的栅格地图定位方法，其特征在于，目标函数如公式(2)所示：
Figure FDA0003508704000000021
Si(T)表示将激光帧中的第i个点云变换到栅格地图坐标系下，M(Si(T))表示激光帧中第i个点云在对应栅格地图上的位置被占据的概率。
4.如权利要求1所述基于点云配准的栅格地图定位方法，其特征在于，粒子Si的权重计算公式如(3)、(4)所示：
Figure FDA0003508704000000022
Figure FDA0003508704000000023
基于当前粒子Si的位姿将当前激光帧中的点云转换至栅格地图中，(xk,yk)表示当前激光帧中的第k个点云在栅格地图中的坐标，<x,y>occupied in map为栅格地图中的障碍物坐标；dist表示随机粒子距最近障碍物距离，qSi表示测距似然，代表粒子Si的权重，zhit、zranddom及zmax分别代表测距误差混合权重的不同部分，分别代表测量噪声、无法解释的随机测量及测量失败，σhit为测量噪声的标准差。
CN202011550794.0A 2020-12-24 2020-12-24 一种基于点云配准的栅格地图定位方法 Active CN112612862B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011550794.0A CN112612862B (zh) 2020-12-24 2020-12-24 一种基于点云配准的栅格地图定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011550794.0A CN112612862B (zh) 2020-12-24 2020-12-24 一种基于点云配准的栅格地图定位方法

Publications (2)

Publication Number Publication Date
CN112612862A CN112612862A (zh) 2021-04-06
CN112612862B true CN112612862B (zh) 2022-06-24

Family

ID=75244783

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011550794.0A Active CN112612862B (zh) 2020-12-24 2020-12-24 一种基于点云配准的栅格地图定位方法

Country Status (1)

Country Link
CN (1) CN112612862B (zh)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113177974A (zh) * 2021-05-19 2021-07-27 上海商汤临港智能科技有限公司 一种点云配准方法、装置、电子设备及存储介质
CN113865588B (zh) * 2021-08-24 2024-03-26 上海交通大学 一种机器人定位方法和装置
CN113607173B (zh) * 2021-09-14 2023-10-20 成都睿芯行科技有限公司 一种基于fpga的机器人激光定位方法
CN113989375A (zh) * 2021-11-16 2022-01-28 杭州云象商用机器有限公司 机器人定位方法、装置、设备及可读存储介质
CN115222767B (zh) * 2022-04-12 2024-01-23 广州汽车集团股份有限公司 一种基于空间车位的跟踪方法及系统
CN115267812A (zh) * 2022-07-28 2022-11-01 广州高新兴机器人有限公司 一种基于高亮区域的定位方法、装置、介质及机器人
CN117570998B (zh) * 2024-01-17 2024-04-02 山东大学 基于反光柱信息的机器人定位方法及系统

Family Cites Families (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100809352B1 (ko) * 2006-11-16 2008-03-05 삼성전자주식회사 파티클 필터 기반의 이동 로봇의 자세 추정 방법 및 장치
US9020637B2 (en) * 2012-11-02 2015-04-28 Irobot Corporation Simultaneous localization and mapping for a mobile robot
CN105509755B (zh) * 2015-11-27 2018-10-12 重庆邮电大学 一种基于高斯分布的移动机器人同步定位与地图构建方法
US9766349B1 (en) * 2016-09-14 2017-09-19 Uber Technologies, Inc. Localization and tracking using location, signal strength, and pseudorange data
CN107991683B (zh) * 2017-11-08 2019-10-08 华中科技大学 一种基于激光雷达的机器人自主定位方法
CN111486842B (zh) * 2019-01-29 2022-04-15 深圳市优必选科技有限公司 重定位方法及装置、机器人
CN109682382B (zh) * 2019-02-28 2020-09-08 电子科技大学 基于自适应蒙特卡洛和特征匹配的全局融合定位方法
CN111060888B (zh) * 2019-12-31 2023-04-07 芜湖哈特机器人产业技术研究院有限公司 一种融合icp和似然域模型的移动机器人重定位方法
CN111539994B (zh) * 2020-04-28 2023-04-18 武汉科技大学 一种基于语义似然估计的粒子滤波重定位方法
CN111915675B (zh) * 2020-06-17 2023-06-23 广西综合交通大数据研究院 基于粒子漂移的粒子滤波点云定位方法及其装置和系统

Also Published As

Publication number Publication date
CN112612862A (zh) 2021-04-06

Similar Documents

Publication Publication Date Title
CN112612862B (zh) 一种基于点云配准的栅格地图定位方法
CN106574975B (zh) 使用外围信号的轨迹匹配
CN110007670B (zh) 移动机器人定位建图方法
CN107426687B (zh) 面向WiFi/PDR室内融合定位的自适应卡尔曼滤波方法
JP2019215853A (ja) 測位のための方法、測位のための装置、デバイス及びコンピュータ読み取り可能な記憶媒体
US9244152B1 (en) Determining device locations using movement, signal strength
CN110689576A (zh) 一种基于Autoware的动态3D点云正态分布AGV定位方法
CN112882056B (zh) 基于激光雷达的移动机器人同步定位与地图构建方法
CN112230243B (zh) 一种移动机器人室内地图构建方法
CN110132284B (zh) 一种基于深度信息的全局定位方法
CN113777600B (zh) 一种多毫米波雷达协同定位跟踪方法
CN110986956B (zh) 一种基于改进的蒙特卡洛算法的自主学习全局定位方法
CN111121791A (zh) 隐马尔可夫模型在地图匹配中的优化方法及gps定位方法
WO2016112758A1 (zh) 终端的定位方法及装置
CN112702699B (zh) 一种融合UWB和LiDAR的室内定位方法
CN108871365B (zh) 一种航向约束下的状态估计方法及系统
CN111259332B (zh) 一种杂波环境下的模糊数据关联方法及多目标跟踪方法
CN108152812B (zh) 一种调整网格间距的改进agimm跟踪方法
CN114137562B (zh) 一种基于改进全局最邻近的多目标跟踪方法
CN115493601A (zh) 基于路网匹配的车辆自主定位方法、装置、存储介质
CN112379393B (zh) 一种列车碰撞预警方法及其装置
CN113191427B (zh) 一种多目标车辆跟踪方法及相关装置
CN112612034B (zh) 基于激光帧和概率地图扫描的位姿匹配方法
CN114236480A (zh) 一种机载平台传感器系统误差配准算法
CN110333513B (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