CN112797981A - 一种基于正态分布的粒子滤波定位方法 - Google Patents
一种基于正态分布的粒子滤波定位方法 Download PDFInfo
- Publication number
- CN112797981A CN112797981A CN202011548146.1A CN202011548146A CN112797981A CN 112797981 A CN112797981 A CN 112797981A CN 202011548146 A CN202011548146 A CN 202011548146A CN 112797981 A CN112797981 A CN 112797981A
- Authority
- CN
- China
- Prior art keywords
- particles
- weight
- particle
- pose
- normal distribution
- 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.)
- Granted
Links
- 239000002245 particle Substances 0.000 title claims abstract description 104
- 238000000034 method Methods 0.000 title claims abstract description 34
- 238000012952 Resampling Methods 0.000 claims abstract description 9
- 238000012216 screening Methods 0.000 claims abstract description 3
- 239000011159 matrix material Substances 0.000 claims description 6
- 238000013519 translation Methods 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 238000006073 displacement reaction Methods 0.000 claims description 3
- 230000033001 locomotion Effects 0.000 description 5
- 238000005070 sampling Methods 0.000 description 4
- 230000002159 abnormal effect Effects 0.000 description 3
- 238000011065 in-situ storage Methods 0.000 description 3
- 238000001514 detection method Methods 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 230000007774 longterm Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000005192 partition Methods 0.000 description 2
- 238000000638 solvent extraction Methods 0.000 description 2
- 206010033307 Overweight Diseases 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000000717 retained effect Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
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
-
- 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
- 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
- G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
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)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于正态分布的粒子滤波定位方法，包括：基于及栅格地图分辨率将栅格地图划分若干大区域，每个大区域内部划分4个子区域，相邻子区域面积重叠，计算每个子区域内栅格点的位置均值和位置方差；构建粒子滤波器，将空闲区域作为有效区域，在有效区域内产生随机粒子；根据粒子位姿的观测数据，采用正态分布模型计算粒子权重及粒子总权重；根据粒子权重对粒子进行筛选、复制及保留，同时增加随机粒子；重采样后，更新粒子分布，使得每个粒子的权重相等，更新粒子簇的最大权重，若粒子的最大权重大于权重阈值，则最大权重粒子簇的平均位姿即为最佳位姿，即完成重定位。一定程度克服使用里程计时存在的打滑问题。
Description
技术领域
本发明属于定位技术领域，更具体地，本发明涉及一种基于正态分布的粒子滤波定位方法。
背景技术
基于路标的激光定位导航技术需要在环境中布设大量的人工路标，物料成本高，对环境有一定的要求，不能做到完全的柔性导航。目前，移动机器人同时定位与地图构建(SLAM)技术成为机器人定位导航研究的热点问题，是移动机器人自主导航的基础和前提。求解SLAM问题的方法有基于kalman滤波器的方法，基于粒子滤波的方法和基于图优化的方法。SLAM方法可以提高移动机器人的自主能力和环境适应能力，使移动机器人可以在未知的环境中进行自主定位和导航。实现移动机器人的自动导航需要加载建图系统生成的栅格地图，并在该地图实现机器人自定位和导航。
现有的技术中存在划分地图区域和粒子滤波进行定位的方法：为机器人指定初始位置区域，可以为一个或多个栅格地图分区；机器人根据初始位置区域初始化粒子滤波器参数；粒子滤波器迭代更新，包括采样粒子产生过程和重要性采样过程；当达到迭代终止条件时，停止迭代更新，并输出机器人的初始位置信息。该方案使得机器人能在任意位置、不需运动的情况下，由用户方便快捷地指定初始位置、并获取相对准确的粒子滤波器初始化参数，经过静止状态下的粒子滤波器采样、重采样，得到更为准确的初始位置估计。该方案存在如下问题：
建立栅格地图分区采用的是程序自动建立均匀分区和用户基于栅格地图中重要地标进行分区。这种分区方法可能会因为所选的区域包含的周围信息缺少或者定位点没有重要地标而导致重定位失败。而本方案采用的基于正态分布的粒子滤波定位方法，仅仅依靠激光传感器，且不需要人工干预就可原地实现重定位，连续定位过程中可快速高效地实现定位，同时一定程度克服使用里程计时存在的打滑问题。
发明内容
本发明提供了一种基于正态分布的粒子滤波定位方法，旨在改善上述问题。
本发明是这样实现的，一种基于正态分布的粒子滤波定位方法，所述方法具体包括如下步骤：
S1、构建地图正态分布概率模型：基于及栅格地图分辨率将栅格地图划分若干大区域，对于每个大区域内部划分4个子区域，相邻两子区域面积50％重叠，计算每个子区域内栅格点的坐标均值和坐标方差；
S2、初始化粒子分布：构建粒子滤波器，将空闲区域作为有效区域，在有效区域内产生随机粒子；
S3、观测模型更新：根据粒子位姿的观测数据，采用正态分布模型计算粒子权重及粒子总权重—基于粒子si的位姿将当前激光帧中的点云转换至栅格地图中，计算当前激光帧中各点云落入相应4个子区域的得分和，即为对应粒子si的权重；
S4、重采样：根据粒子权重对粒子进行筛选、复制及保留，同时增加随机粒子；
S5、粒子滤波器迭代更新：重采样后，更新粒子分布，使得每个粒子的权重相等，更新粒子簇的最大权重，若粒子的最大权重大于权重阈值，则最大权重粒子簇的平均位姿即为最佳位姿，即完成重定位。
进一步的，在步骤S1之后还包括：
里程计的运动模型更新：基于当前重定位的最佳位姿作为中心，划定局部窗口，在局部窗口内根据高斯模型随机分布粒子，基于步骤S3至步骤S5实现局部窗口内的最优位姿，即再次完成重定位。
进一步的，所述连续定位方法具体如下：
将局部窗口的重定位结果输送给里程计，基于里程计在当前时刻与上一时刻内的位移及转角完成对所有粒子位姿的预测；
获取当前时刻的激光帧，基于步骤S3至步骤S5来获取当前的最佳位姿。
进一步的，粒子si的权重
的计算公式如下：
xi′＝ξ·xi (3)
其中，xi表示当前激光帧中第i个点在机器人本体坐标系中的坐标，ξ表示基于粒子si位姿生成的旋转平移矩阵，xi′表示基于旋转平移矩阵ξ将激光帧中第i个点云坐标xi转换到地图点云后的坐标，μij表示第j个区域的坐标均值μi，∑ij表示第j个区域的坐标方差∑i。
本发明提出的基于正态分布的粒子滤波定位方法，仅仅依靠激光传感器，且不需要人工干预就可原地实现重定位，过程中可快速高效地实现定位，同时一定程度克服使用里程计时存在的打滑问题；此外，移动机器人出现异常情况后快速实现全局定位以及仅使用激光传感器实现精确定位。
附图说明
图1为本发明实施例提供的基于正态分布的粒子滤波定位方法流程图。
具体实施方式
下面对照附图，通过对实施例的描述，对本发明的具体实施方式作进一步详细的说明，以帮助本领域的技术人员对本发明的发明构思、技术方案有更完整、准确和深入的理解。
图1为本发明实施例提供的基于正态分布的粒子滤波定位方法流程图，该方法具体包括如下步骤：
步骤1)全局地图预处理：加载系统所需的全局地图(即栅格地图)，并记录全局地图中空闲区域、占用区域和未知区域。基于地图边界对地图进行裁剪，保留有效的地图区域。空闲区域即为激光雷达探测不存在障碍物的区域，占用区域即为激光雷达探测存在障碍物的区域，未知区域即为激光雷达无法探测的区域。
步骤2)构建栅格地图的正态分布概率模型：在栅格地图内根据地图边界以及栅格地图分辨率划分若干大区域。对于每个大区域内部划分4个子区域，要求四个子区域相互重叠，相邻两子区域重叠面积的50％，计算每个子区域内栅格点的坐标均值和坐标方差，如公式(1)、(2)所示。
μi，∑i分别表示地图点云中第k个点所在子区域的栅格点坐标均值和坐标方差，xk表示点云地图中第k个点在栅格地图中的坐标，在栅格地图中，提取栅格中心的坐标作为栅格点，栅格点即为地图点云中的点。
步骤3)初始化粒子分布：构建粒子滤波器，将空闲区域作为有效区域，在有效区域产生随机粒子。
步骤4)观测模型更新：根据粒子位姿的观测数据，采用正态分布模型计算粒子权重及粒子总权重，基于粒子si的位姿将当前激光帧中的点云转换至栅格地图中，计算当前激光帧中各点云落入相应4个子区域的得分和，即为对应粒子si的权重，其计算公式如(3)(4)所示：。
xi′＝ξ·xi (3)
其中，xi表示当前激光帧中第i个点在机器人本体坐标系中的坐标，ξ表示基于粒子si位姿生成的旋转平移矩阵，xi′表示基于旋转平移矩阵ξ将激光帧中第i个点云坐标xi转换到地图点云后的坐标，μij表示第j个区域的坐标均值μi，∑ij表示第j个区域的坐标方差∑i。
步骤5)重采样：根据粒子权重对粒子进行筛选、复制及保留，同时随机采样以以下概率增加，增加随机粒子。如公式(5)、(6)所示。
max{0.0，1.0-wfast/wslaw} (6)
wslow表示短期似然平均，wfast表示长期似然平均，wavg表示经验测量似然，αslow表示短期平均的指数滤波器的衰减率，αfast表示长期平均的指数滤波器的衰减率。
步骤6)粒子滤波器迭代更新：重采样后，更新粒子分布，使得每个粒子的权重相等，更新粒子簇的最大权重，若粒子的最大权重大于权重阈值，则最大权重粒子簇的平均位姿即为最佳位姿，即完成重定位。
步骤7)里程计的运动模型更新：基于当前重定位的最佳位姿作为中心，划定合适的局部窗口，在局部窗口内根据高斯模型随机分布粒子，基于步骤4)至步骤6)实现局部窗口重定位。将此时的局部重定位结果输送给里程计的运动模型，基于里程计在当前时刻与上一时刻内的位移及转角完成对所有粒子位姿的预测。
步骤8)通过激光传感器观测数据，基于步骤4)～步骤6)完成粒子滤波器迭代更新后获得最佳位姿。利用正态分布模型计算粒子权重，进行重采样操作，复制高权重粒子，舍弃低权重粒子，当完成粒子滤波器迭代更新后获得最佳位姿。
本方案提出的基于正态分布的粒子滤波定位方法，仅仅依靠激光传感器，且不需要人工干预就可原地实现重定位，过程中可快速高效地实现定位，同时一定程度克服使用里程计时存在的打滑问题；此外，移动机器人出现异常情况后快速实现全局定位以及仅使用激光传感器实现精确定位。
有益技术效果的描述角度：
1)基于正态分布的粒子滤波重定位方法能够减少对初始地图最近障碍物距离的依赖，提高重定位的时间；窗口内的正态粒子滤波重定位方法能够输送结果给里程计运动模型，较少对硬件里程计的依赖。
2)基于正态分布的粒子滤波定位方法能够在移动机器人出现异常情况快速实现全局定位，可以仅依靠激光传感器实现精确定位，一定程度克服使用里程计时存在的打滑问题。
并不受上述方式的限制，只要采用了本发明的方法构思和技术方案进行的各种非实质性的改进，或未经改进将本发明的构思和技术方案直接应用于其它场合的，均在本发明的保护范围之内。
Claims (4)
1.一种基于正态分布的粒子滤波定位方法，其特征在于，所述方法具体包括如下步骤：
S1、构建地图正态分布概率模型：基于栅格地图分辨率将栅格地图划分若干大区域，对于每个大区域内部划分4个子区域，相邻两子区域面积50％重叠，计算每个子区域内栅格点的位置均值和位置方差；
S2、初始化粒子分布：构建粒子滤波器，将空闲区域作为有效区域，在有效区域内产生随机粒子；
S3、观测模型更新：根据粒子位姿的观测数据，采用正态分布模型计算粒子权重及粒子总权重—基于粒子si的位姿将当前激光帧中的点云转换至栅格地图中，计算当前激光帧中各点云落入相应4个子区域的得分和，即为对应粒子si的权重；
S4、重采样：根据粒子权重对粒子进行筛选、复制及保留，同时增加随机粒子；
S5、粒子滤波器迭代更新：重采样后，更新粒子分布，使得每个粒子的权重相等，更新粒子簇的最大权重，若粒子的最大权重大于权重阈值，则最大权重粒子簇的平均位姿即为最佳位姿，即完成重定位。
2.如权利要求1所述基于正态分布的粒子滤波定位方法，其特征在于，在步骤S1之后还包括：
基于当前重定位的最佳位姿作为中心，划定局部窗口，在局部窗口内根据高斯模型随机分布粒子，基于步骤S3至步骤S5实现局部窗口内的最优位姿，即再次完成重定位。
3.如权利要求2所述基于正态分布的粒子滤波定位方法，其特征在于，所述连续定位方法具体如下：
将局部窗口的重定位结果输送给里程计，基于里程计在当前时刻与上一时刻内的位移及转角完成对所有粒子位姿的预测；
获取当前时刻的激光帧，基于步骤S3至步骤S5来获取当前的最佳位姿。
4.如权利要求2所述基于正态分布的粒子滤波定位方法，其特征在于，粒子si的权重psi的计算公式如下：
xi′＝ξ·xi (3)
其中，xi表示当前激光帧中第i个点在机器人本体坐标系中的坐标，ξ表示基于粒子si位姿生成的旋转平移矩阵，xi′表示基于旋转平移矩阵ξ将激光帧中第i个点坐标xi转换到地图点云后的坐标，μij表示第j个区域的坐标均值μi，∑ij表示第j个区域的坐标方差∑i。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011548146.1A CN112797981B (zh) | 2020-12-24 | 2020-12-24 | 一种基于正态分布的粒子滤波定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011548146.1A CN112797981B (zh) | 2020-12-24 | 2020-12-24 | 一种基于正态分布的粒子滤波定位方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112797981A true CN112797981A (zh) | 2021-05-14 |
| CN112797981B CN112797981B (zh) | 2024-04-05 |
Family
ID=75805372
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011548146.1A Active CN112797981B (zh) | 2020-12-24 | 2020-12-24 | 一种基于正态分布的粒子滤波定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112797981B (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114577199A (zh) * | 2022-02-17 | 2022-06-03 | 广州大学 | 一种基于Gmapping算法的垃圾分类机器人二维栅格地图构建系统 |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20140129027A1 (en) * | 2012-11-02 | 2014-05-08 | Irobot Corporation | Simultaneous Localization And Mapping For A Mobile Robot |
| CN110689576A (zh) * | 2019-09-29 | 2020-01-14 | 桂林电子科技大学 | 一种基于Autoware的动态3D点云正态分布AGV定位方法 |
| CN111061276A (zh) * | 2019-12-31 | 2020-04-24 | 芜湖哈特机器人产业技术研究院有限公司 | 基于动态区域划分的移动机器人重定位方法 |
| CN111061287A (zh) * | 2019-12-31 | 2020-04-24 | 芜湖哈特机器人产业技术研究院有限公司 | 基于粒子自收敛的移动机器人重定位方法 |
-
2020
- 2020-12-24 CN CN202011548146.1A patent/CN112797981B/zh active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20140129027A1 (en) * | 2012-11-02 | 2014-05-08 | Irobot Corporation | Simultaneous Localization And Mapping For A Mobile Robot |
| CN110689576A (zh) * | 2019-09-29 | 2020-01-14 | 桂林电子科技大学 | 一种基于Autoware的动态3D点云正态分布AGV定位方法 |
| CN111061276A (zh) * | 2019-12-31 | 2020-04-24 | 芜湖哈特机器人产业技术研究院有限公司 | 基于动态区域划分的移动机器人重定位方法 |
| CN111061287A (zh) * | 2019-12-31 | 2020-04-24 | 芜湖哈特机器人产业技术研究院有限公司 | 基于粒子自收敛的移动机器人重定位方法 |
Non-Patent Citations (1)
| Title |
|---|
| 李瑞峰;赵立军;靳新辉;: "基于粒子滤波器的室内移动机器人自定位", 华中科技大学学报(自然科学版), no. 1 * |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114577199A (zh) * | 2022-02-17 | 2022-06-03 | 广州大学 | 一种基于Gmapping算法的垃圾分类机器人二维栅格地图构建系统 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112797981B (zh) | 2024-04-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108931245A (zh) | 移动机器人的局部自定位方法及设备 | |
| US10496093B2 (en) | Movement control system, movement control device, and computer-implemented program for movement control | |
| CN110689576B (zh) | 一种基于Autoware的动态3D点云正态分布AGV定位方法 | |
| CN109521757B (zh) | 静态障碍物识别方法和装置 | |
| CN109144056A (zh) | 移动机器人的全局自定位方法及设备 | |
| CN110609290B (zh) | 激光雷达匹配定位方法及装置 | |
| CN109435955B (zh) | 一种自动驾驶系统性能评估方法、装置、设备及存储介质 | |
| CN112013841B (zh) | 一种室内动态环境下的语义slam服务机器人导航方法 | |
| US20200241112A1 (en) | Localization method and robot using the same | |
| CN111947672B (zh) | 用于检测环境变化的方法、装置、设备和介质 | |
| WO2018221455A1 (ja) | 更新装置、制御方法、プログラム及び記憶媒体 | |
| JP2022550512A (ja) | マルチエージェントシミュレーション | |
| CN109885046B (zh) | 一种基于粒子滤波的移动机器人定位加速方法 | |
| CN112329789B (zh) | 点云的提取方法、装置、计算机设备和存储介质 | |
| US11915490B2 (en) | Annotated surfel maps | |
| CN109849930A (zh) | 自动驾驶汽车的相邻车辆的速度计算方法和装置 | |
| US20210380125A1 (en) | Autonomous driving with surfel maps | |
| CN112880694A (zh) | 确定车辆的位置的方法 | |
| CN110989619B (zh) | 用于定位对象的方法、装置、设备和存储介质 | |
| WO2022078342A1 (zh) | 动态占据栅格估计方法及装置 | |
| CN112797981A (zh) | 一种基于正态分布的粒子滤波定位方法 | |
| CN114577196B (zh) | 使用光流的激光雷达定位 | |
| US11164391B1 (en) | Mixed reality object detection | |
| CN114092660A (zh) | 高精地图生成方法、装置及用于生成地图的车辆 | |
| CN110794434A (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 | ||
| CB02 | Change of applicant information |
Country or region after: Zhong Guo Address after: 241000 Office Building, National Industrial Robot Product Quality Supervision and Inspection Center Park, No. 17 Shenzhou Road, Jiujiang Economic and Technological Development Zone, Wuhu City, Anhui Province Applicant after: Hart Robotics Industry Technology Research Institute in the Yangtze River Delta Address before: 241000 office building of national industrial robot product quality supervision and inspection center, No.17 Shenzhou Road, Jiujiang Economic and Technological Development Zone, Wuhu City, Anhui Province Applicant before: Wuhu Robot Industry Technology Research Institute of Harbin Institute of Technology Country or region before: Zhong Guo |
|
| CB02 | Change of applicant information | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |