CN112882056A - 基于激光雷达的移动机器人同步定位与地图构建方法 - Google Patents
基于激光雷达的移动机器人同步定位与地图构建方法 Download PDFInfo
- Publication number
- CN112882056A CN112882056A CN202110051887.7A CN202110051887A CN112882056A CN 112882056 A CN112882056 A CN 112882056A CN 202110051887 A CN202110051887 A CN 202110051887A CN 112882056 A CN112882056 A CN 112882056A
- Authority
- CN
- China
- Prior art keywords
- particles
- weight
- map
- algorithm
- particle
- 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
- 230000001360 synchronised effect Effects 0.000 title claims abstract description 17
- 238000010276 construction Methods 0.000 title abstract description 7
- 239000002245 particle Substances 0.000 claims abstract description 143
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 98
- 238000012952 Resampling Methods 0.000 claims abstract description 48
- 238000000034 method Methods 0.000 claims abstract description 40
- 238000005070 sampling Methods 0.000 claims abstract description 8
- 238000009616 inductively coupled plasma Methods 0.000 claims description 26
- 238000001914 filtration Methods 0.000 claims description 14
- 238000004364 calculation method Methods 0.000 claims description 8
- 238000013507 mapping Methods 0.000 claims description 6
- 238000005457 optimization Methods 0.000 claims description 6
- 239000011541 reaction mixture Substances 0.000 claims description 6
- 238000010606 normalization Methods 0.000 claims description 4
- 238000013519 translation Methods 0.000 claims description 4
- 238000012163 sequencing technique Methods 0.000 claims description 2
- 238000012847 principal component analysis method Methods 0.000 abstract description 2
- 230000000694 effects Effects 0.000 description 8
- 238000004088 simulation Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 4
- 230000009466 transformation Effects 0.000 description 4
- 230000015556 catabolic process Effects 0.000 description 3
- 238000006731 degradation reaction Methods 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 241000408495 Iton Species 0.000 description 1
- 238000000342 Monte Carlo simulation Methods 0.000 description 1
- 230000002902 bimodal effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000000513 principal component analysis Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- 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/89—Lidar systems specially adapted for specific applications for mapping or imaging
-
- 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
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开的基于激光雷达的移动机器人同步定位与地图构建方法，其特征在于，包括以下步骤：步骤1、点云匹配；步骤2、抽样；步骤3、更新权重；步骤4、重采样；步骤5、更新地图。本发明基于激光雷达的移动机器人同步定位与地图构建方法，着重提供良好的点云初始状态，从采样点的选取和匹配策略方面对激光雷达快速点云配准算法进行了改进，利用主成分分析法对相邻帧之间的点云进行粗配准，再采用改进点到线迭代最近点配准算法校正粗配准结果完成精确配准。同时在重采样算法中，在多次复制大权重粒子集合的情况下引入小权重粒子集合，改善粒子多样性缺乏，提高了移动机器人定位精度。
Description
技术领域
本发明属于移动机器人定位方法技术领域，具体涉及一种基于激光雷达的移动机器人同步定位与地图构建方法。
背景技术
智能化机器人在面临复杂不确定地形环境如何确定自身位置，提供优质的地图具有重要的理论研究价值和实际应用意义。同步定位与地图构建(SLAM：SimultaneousLocalization And Mapping)利用机器人携带的传感器在未知环境下观测目标，根据机器人与目标之间的相对位置和里程信息来估计机器人与目标点的全局坐标。SLAM问题一直是机器人研究中的一个基础且重要的开放性研究热点。
求解SLAM问题的方法大致可分为基于概率估计的方法和基于非概率估计的方法两大类。基于概率估计的方法主要有扩展卡尔曼滤波(EKF：Extended Kalman Filter)、FastSLAM与粒子滤波(PF：Particle Filter)等。粒子滤波的优点在于解决如状态递归估计或高维非线性、非高斯时变系统的概率推理等复杂问题。粒子滤波作为一种有效解决SLAM问题的方法仍然存在粒子退化问题。后来，Murphy等人提出采用Rao-Blackwillisedparticle filter(RBPF)求解SLAM问题。
RBPF-SLAM是一种基于粒子滤波的SLAM算法，利用粒子来表示机器人的位置和姿态。它广泛应用于机器人同步定位和地图构建。SLAM问题可以描述为“估计”-“观测”-“校正”过程：机器人在一个未知环境中从未知的初始位置，利用系统的运动模型和里程计获取自身的运动信息和系统的观测模型，感知环境信息来估计机器人的轨迹，构建增量的环境地图。
发明内容
本发明的目的在于提供一种基于激光雷达的移动机器人同步定位与地图构建方法，解决了现有SLAM算法中移动机器人定位精度低、构建地图不准确的问题。
本发明所采用的技术方案是：基于激光雷达的移动机器人同步定位与地图构建方法，包括以下步骤：
步骤1、点云匹配：当机器人在移动过程中获得来自激光雷达的扫描数据时，采用点到线的改进ICP配准算法，对机器人位姿进行精确估计；
步骤2、抽样：对上一代第i个粒子
估计得到当前时刻粒子
选取激光雷达匹配结果作为优化提议分布；
步骤3、更新权重：根据权重计算公式计算出每个粒子
的权值
步骤4、重采样：当Neff<Nth时，采用改进粒子滤波重采样方法对当前粒子进行重采样；
步骤5、更新地图：根据第i个粒子表示的移动机器人位姿和当前观测情况，对第i个粒子构建的地图进行更新。
本发明的特点还在于，
步骤1中点到线的改进ICP配准算法的具体步骤为：
步骤1.1、设置距离误差阈值δ；
步骤1.2、将当前帧的点云数据根据初始位姿投影到参考坐标系中；
步骤1.3、对于点云中的每一个点pi，在参考帧中找到与其最近的两个点
和
步骤1.4、通过下式(4)计算配准误差：
式(4)中，索引i和j分别代表点云p和q中的点，R和T分别为旋转和平移矩阵，ni为线段
的法向量；
步骤1.5、当配准误差E小于误差阈值δ，停止配准迭代，否则返回步骤1.2。
步骤2中对上一代第i个粒子
估计得到当前时刻粒子
选取激光雷达匹配结果作为优化提议分布，优化提议分布如式(5)所示：
式(5)中，
表示环境地图m和机器人轨迹x1:t＝x1,x2,…,xt的联合后验概率密度，
表示利用里程计信息估计机器人位姿，p(zt|m(i),xt)表示用传感器信息z1:t＝z1,z2,…,zt-1更新地图m，
表示用里程计信息和传感器信息z1:t＝z1,z2,…,zt-1更新地图m。
步骤3中根据权重计算公式式(6)计算出每个粒子
的权值
式(6)中，
为前一时刻粒子权重，
表示用里程计信息和传感器信息z1:t＝z1,z2,…,zt-1更新地图m。
步骤4具体为：
步骤4.1、设定重采样门限阈值Nth，当有效粒子数
小于阈值Nth时，进行重采样步骤4.2，否则进行下一时刻的滤波过程，其中
表示归一化权值；
步骤4.2、对权重大于阈值wt的粒子进行重采样，将高权值粒子进行复制生成M个粒子，复制次数为粒子总数N与粒子权重的乘积取整floor(N*w)，高权值粒子集记为
其中r＝1,2,…,M表示粒子个数；
步骤4.3、对权重小于阈值wt的粒子，进行随机重采样生成低权值粒子集记为
其中j＝1,2,…,N-M表示粒子个数；
步骤4.4、分别对步骤4.2和步骤4.3产生的两部分粒子进行排序，得到新的粒子集
步骤4.5、对步骤4.4得到的新的粒子集进行权重归一化后对其进行后验均值估计，即得到粒子滤波估计的最终状态。
本发明的有益效果是：本发明基于激光雷达的移动机器人同步定位与地图构建方法，着重提供良好的点云初始状态，从采样点的选取和匹配策略方面对激光雷达快速点云配准算法进行了改进，利用主成分分析法对相邻帧之间的点云进行粗配准，再采用改进点到线迭代最近点配准算法校正粗配准结果完成精确配准。同时在重采样算法中，在多次复制大权重粒子集合的情况下引入小权重粒子集合，改善粒子多样性缺乏，提高了移动机器人定位精度。
附图说明
图1是本发明基于激光雷达的移动机器人同步定位与地图构建方法中粒子滤波重采样算法的流程示意图；
图2a)是本发明实施例中原始点云分布图，图2b)是本发明实施例中传统ICP配准后点云分布图，图2c)是本发明实施例中改进ICP配准后点云分布图；
图3是本发明实施例中改进重采样粒子滤波、MSV算法和传统重采样粒子滤波三种算法滤波状态估计曲线图；
图4是本发明实施例中改进重采样粒子滤波、MSV算法和传统重采样粒子滤波三种算法滤波均方根误差曲线图；
图5a)是本发明实施例中传统重采样滤波后粒子分布图，图5b)是本发明实施例中MSV算法滤波后粒子分布图，图5c)是本发明实施例中改进重采样滤波后粒子分布图；
图6a)是本发明实施例中地图构建的室外走廊真实环境，图6b)是本发明实施例中地图构建的室内真实环境；
图7a)是本发明实施例中传统重采样粒子滤波算法在实验室外走廊环境下构建地图效果图，图7b)是本发明实施例中MSV算法在实验室外走廊环境下构建地图效果图，图7c)是本发明实施例中改进重采样与点云配准算法在实验室外走廊环境下构建地图效果图；
图8a)是本发明实施例中传统重采样粒子滤波算法在实验室环境下构建地图效果图，图8b)是本发明实施例中MSV算法在实验室环境下构建地图效果图，图8c)是本发明实施例中改进重采样与点云配准算法在实验室环境下构建地图效果图。
具体实施方式
下面结合附图以及具体实施方式对本发明进行详细说明。
本发明提供了一种基于激光雷达的移动机器人同步定位与地图构建方法，具体按照如下步骤实施：
步骤1，点云匹配：当机器人在移动过程中获得来自激光雷达的扫描数据时，采用改进点云配准算法，对机器人位姿进行精确估计。
ICP算法配准的目标是通过一个三维变换矩阵将源点云集p＝{pi}转换为目标点云集q＝{qj}，且尽可能使两个点云坐标在同一位置重合。两点云位置配准误差为：
求解误差函数的最小值，获得下一次位姿变换估计值：
上式中
表示旋转平移操作符，Sref为旋转平移参考面，k为匹配算法迭代次数，索引i和j分别代表点云p和q中的点，R和T分别为旋转和平移矩阵，ICP算法原理简单、直观，但算法的效率和全局最优收敛取决于初始迭代值。
PCA(Principal Component Analysis)是一种有效的简化检测数据集方法，可以减少数据集维数，反映数据集对方差贡献的最大特征。因此，相似度大的两片点云，通过PCA算法只要把其参考坐标系调整到一致，即可达到粗配准目的，粗配准为点到线ICP点云精配准提供了良好的点云初始变换。
传统ICP配准算法采用两点间欧氏距离最近点作为配准点，易受到噪声的干扰。因此，本发明在对应点匹配中，将最近的点度量从两点之间的欧氏距离改变为到两点连线的垂直距离，对点云粗配准结果进行点到线ICP算法精确配准校正以提高匹配的精度和收敛速度，实现移动机器人的精确定位。点到线ICP解决曲面匹配问题，可以表示如下：给出一个参考Sref和一组点云{pi}，最小化点pi旋转平移到其在表面Sref上投影的距离：
上式中ni为曲面在投影点处的法线。Qk是PCA算法对点云粗配后准得到的初始变换值。
基于点到线的改进ICP配准算法步骤：
步骤1.1，设置距离误差阈值δ，改进算法采用点到线的距离作为误差；
步骤1.2，将当前帧的点云数据根据初始位姿投影到参考坐标系中；
步骤1.3，对于点云中的每一个点pi，在参考帧中找到与它最近的两个点
和
步骤1.4，计算配准误差，误差计算采用下式：
步骤1.5，当误差小于误差阈值δ，停止配准迭代，否则返回步骤1.2。
步骤2，抽样：对上一代第i个粒子
估计得到当前时刻粒子
选取激光雷达匹配结果作为优化提议分布，表示为：
式(5)中，
表示环境地图m和机器人轨迹x1:t＝x1,x2,…,xt的联合后验概率密度，
表示利用里程计信息估计机器人位姿，p(zt|m(i),xt)表示用传感器信息z1:t＝z1,z2,…,zt-1更新地图m，
表示用里程计信息和传感器信息z1:t＝z1,z2,…,zt-1更新地图m。
步骤3，更新权重：根据权重计算公式计算出每个粒子
的权值
权重公式为：
式(6)中，
为前一时刻粒子权重，
表示用里程计信息和传感器信息z1:t＝z1,z2,…,zt-1更新地图m。
步骤4，如图1所示，重采样：当Neff<Nth时，采用本发明改进粒子滤波重采样方法对当前粒子进行重采样，改善粒子退化问题。
优化的重采样算法对于粒子滤波来说既可保持样本的有效性和多样性，还可提高滤波器的性能。传统重采样方法完全丢弃了权值较小的粒子,多次迭代后削弱了粒子的多样性，导致收敛精度不足。针对该问题本发明提出一种改进重采样方法，将所有粒子按权值进行分类，根据预先设定的粒子权值阈值wt，在大权重和小权重粒子集合内分别进行采样，最后对新的粒子集进行权重归一化，进入下一步滤波过程。与传统重采样方法相比，改进重采样着重于在大权重集合内进行采样，大量复制粒子，同时在小权重集合内采用随机重采样方法获取粒子，在保证高权重粒子数的同时低权重粒子的选取机会增大，增加粒子多样性，进而提高机器人状态的估计精度。
改进重采样算法步骤如下：
步骤4.1，设定重采样门限阈值Nth，当有效粒子数
(其中
表示归一化权值)，小于阈值Nth时,进行重采样步骤4.2，否则进行下一时刻的滤波过程。
步骤4.2，对权重大于阈值wt的粒子进行重采样，将高权值粒子进行复制生成M个粒子，复制次数为粒子总数N与粒子权重的乘积取整floor(N*w)，高权值粒子集记为
其中r＝1,2,…,M表示粒子个数。
步骤4.3，对权重小于阈值wt的粒子，进行随机重采样生成低权值粒子集记为
其中j＝1,2,…,N-M表示粒子个数。
步骤4.4，分别对步骤4.2和步骤4.3产生的两部分粒子进行排序，得到新的粒子集
步骤4.5，对新的粒子集进行权重归一化，完成后面的滤波过程。
步骤5，更新地图：根据第i个粒子表示的移动机器人位姿和当前观测情况，对第i个粒子构建的地图进行更新。
为验证本发明的有益效果：从算法配准精度、估计性能以及算法运行时间进行了性能仿真分析，同时将本发明算法与传统重采样传统粒子滤波以及文献所提MSV算法进行了性能对比。
选取典型的单变量非平稳模型进行仿真分析，该模型具有典型的非线性特征，且后验分布具有双峰特性，其状态方程和观测方程分别为：
其中，k为状态时间，x(k)为状态向量，z(k)为观测向量，v(k-1)和n(k)是零均值高斯随机变量，其方差分别为Q(k-1)和R(k)。
对目标位置估计误差性能采用均方根误差(RMSE)评价：
其中x(k)和
为k时刻的滤波状态真实值和估计值，k是蒙特卡罗模拟次数。
用激光雷达数据集对原始ICP算法和本发明改进ICP算法进行点云匹配仿真实验，原点云数据与目标点云数据的配准结果如图2所示，图2(a)为原始点云分布，图2(b)为传统ICP配准后点云分布，图2(c)为本发明改进ICP算法配准后点云分布。由图2中箭头所指区域可知，图2(b)图中目标点云和源点云交叉部分较多，完全覆盖面积较少，图2(c)图中目标点云完全覆盖源云的面积较大，因此，本发明改进配准算法的点云配准准确度更高。在机器人定位过程中，优良的配准算法可以进一步提高激光雷达点云的配准精度，从而实现移动机器人的精确定位。
此外，本发明对传统ICP算法和改进ICP算法的运行时间进行了比较，对于相同点云数据集，传统ICP算法进行点云配准运行时间需要12.537s，而本发明不但改进了精配准算法，同时增加了粗配准，改进精配准算法与原始ICP配准算法相比，算法运行时间大大减小，因此加上粗配准时间依旧比原始ICP精配准算法运行时间短，总共时间为8.063s，比传统ICP算法直接配准时间节省了4.474s，由此说明改进算法的实时性效果更好。
对改进重采样粒子滤波、MSV算法和传统重采样粒子滤波三种算法进行了性能对比。仿真中Q(k-1)和R(k)分别取10和1，状态初始值x(0)＝0.1，N取100次，粒子数设定为100。图3是三种粒子滤波算法对状态估计值的仿真对比，图4是三种粒子滤波算法状态估计均方根误差。由图3和图4可知，本发明提出的改进重采样粒子滤波状态估计值与真实值接近，均方根误差均小于其它两种算法，说明本发明所提算法提高了粒子的多样性和估计精度。
图5(a)、图5(b)、图5(c)依次为传统重采样算法、MSV算法和本发明改进重采样粒子滤波三种算法滤波后粒子分布情况。由图5可知改进重采样粒子滤波算法拟合精度高，大部分粒子分布紧密靠近真值，只存在个别粒子样本发散，可以有效提高地图估计精度。
为了进一步分析改进算法的实时性，对传统重采样、MSV算法和本发明改进重采样粒子滤波三种算法在相同初始粒子数情况下进行多次仿真实验，统计算法运行时间均值，如表1所示。可以看出，在相同粒子个数下，改进算法的运行时间低于传统算法以及MSV算法，验证了本发明所改进重采样粒子滤波算法在地图构建具有一定的实时性。
表1 三种算法运行时间对比
Table 1 Comparison of running time between traditional algorithm andimproved algorithm
在实际情况下，机器人的工作环境比仿真环境更加复杂，环境特征更加丰富，存在行人等不确定因素，为了验证改进算法在这些情况下的有效性，分别采用传统重采样算法、MSV算法、本发明改进重采样算法以及融合本发明改进重采样与配准算法，基于Turtlebot机器人对西安理工大学教五楼室内和室外走廊环境分别进行了SLAM试验。构建地图时所需的粒子个数如表2所示，每个环境重复10次试验。实验场景一是室外走廊，实验场景二为室内，由于室内障碍物较多环境复杂，所以构建地图所需粒子数多于室外所需粒子数。
表2 构建地图所需粒子数
Table 2 Number of particles required to construct a map
图6(a)和图6(b)依次为进行地图构建的室外走廊和室内真实环境。采用RBPF-SLAM算法构建的网格图如图7和图8所示。由图7和图8圈标记地方可以看出，本发明所提改进算法构建出来的地图与真实场景一致，原算法构建的地图在拐角处存在偏移和重影的问题，而改进算法构建的地图更清晰、更整齐。对比图7(a)、图7(b)和图7(c)，图7(a)中传统算法没有构建出障碍物完整的形状，图7(b)中MSV算法对障碍物的构建不够完整，地图中灭火器和电梯口位置构建不清晰，而图7(c)中本发明改进重采样与点云配准算法构建的地图信息更加精确的反映环境信息，线条较为清晰，表示障碍物对应所在位置的单元栅格状态更为确定，同时对于电梯口等拐角位置信息的构建更加完整。
由图8中间部分箭头所标注的圈为室内摆放的障碍物，可以看出室内靠墙两边的书桌位置以及中间矩形障碍物的形状被清晰的构建出来，图8(c)所构建的障碍物效果明显优于图8(a)和图8(b),所以本发明所提改进算法对于障碍物的构建效果提升也很明显，可以建立正确且一致性良好的环境地图，算法的定位精度得到了一定程度上的改善。因为原始SLAM算法由于在粒子滤波重采样过程中，注重复制大权值粒子和频繁的重采样导致粒子多样性丧失、粒子退化，导致机器人位姿与估计位姿不重合，构建地图过程中出现地图倾斜与墙体重影，MSV算法采用计算最小采样方差的方式，使得重采样前后粒子分布保持一致，差异达到最小化，来避免一些权值较小但是可能是真实值的粒子被删除，缓解了样本贫化问题，但是计算复杂度增加，粒子分布比较集中，只能达到有限精度，而改进算法克服了原始算法和MSV算法的缺点，并且由表2可以看出，改进算法用于地图构建，用更少的粒子数，构建出更加精确的地图，减小了算法计算复杂度，进一步验证了改进算法构建环境地图的有效性。
Claims (5)
1.基于激光雷达的移动机器人同步定位与地图构建方法，其特征在于，包括以下步骤：
步骤1、点云匹配：当机器人在移动过程中获得来自激光雷达的扫描数据时，采用点到线的改进ICP配准算法，对机器人位姿进行精确估计；
步骤2、抽样：对上一代第i个粒子
估计得到当前时刻粒子
选取激光雷达匹配结果作为优化提议分布；
步骤3、更新权重：根据权重计算公式计算出每个粒子
的权值
步骤4、重采样：当Neff<Nth时，采用改进粒子滤波重采样方法对当前粒子进行重采样；
步骤5、更新地图：根据第i个粒子表示的移动机器人位姿和当前观测情况，对第i个粒子构建的地图进行更新。
2.如权利要求1所述的基于激光雷达的移动机器人同步定位与地图构建方法，其特征在于，所述步骤1中点到线的改进ICP配准算法的具体步骤为：
步骤1.1、设置距离误差阈值δ；
步骤1.2、将当前帧的点云数据根据初始位姿投影到参考坐标系中；
步骤1.3、对于点云中的每一个点pi，在参考帧中找到与其最近的两个点
和
步骤1.4、通过下式(4)计算配准误差：
式(4)中，索引i和j分别代表点云p和q中的点，R和T分别为旋转和平移矩阵，ni为线段
的法向量；
步骤1.5、当配准误差E小于误差阈值δ，停止配准迭代，否则返回步骤1.2。
3.如权利要求1所述的基于激光雷达的移动机器人同步定位与地图构建方法，其特征在于，所述步骤2中对上一代第i个粒子
估计得到当前时刻粒子
选取激光雷达匹配结果作为优化提议分布，优化提议分布如式(5)所示：
式(5)中，
表示环境地图m和机器人轨迹x1:t＝x1,x2,…,xt的联合后验概率密度，
表示利用里程计信息估计机器人位姿，p(zt|m(i),xt)表示用传感器信息z1:t＝z1,z2,…,zt-1更新地图m，
表示用里程计信息和传感器信息z1:t＝z1,z2,…,zt-1更新地图m。
4.如权利要求1所述的基于激光雷达的移动机器人同步定位与地图构建方法，其特征在于，所述步骤3中根据权重计算公式式(6)计算出每个粒子
的权值
式(6)中，
为前一时刻粒子权重，
表示用里程计信息和传感器信息z1:t＝z1,z2,…,zt-1更新地图m。
5.如权利要求1所述的基于激光雷达的移动机器人同步定位与地图构建方法，其特征在于，所述步骤4具体为：
步骤4.1、设定重采样门限阈值Nth，当有效粒子数
小于阈值Nth时，进行重采样步骤4.2，否则进行下一时刻的滤波过程，其中
表示归一化权值；
步骤4.2、对权重大于阈值wt的粒子进行重采样，将高权值粒子进行复制生成M个粒子，复制次数为粒子总数N与粒子权重的乘积取整floor(N*w)，高权值粒子集记为
其中r＝1,2,…,M表示粒子个数；
步骤4.3、对权重小于阈值wt的粒子，进行随机重采样生成低权值粒子集记为
其中j＝1,2,…,N-M表示粒子个数；
步骤4.4、分别对步骤4.2和步骤4.3产生的两部分粒子进行排序，得到新的粒子集
步骤4.5、对步骤4.4得到的新的粒子集进行权重归一化后对其进行后验均值估计，即得到粒子滤波估计的最终状态。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110051887.7A CN112882056B (zh) | 2021-01-15 | 2021-01-15 | 基于激光雷达的移动机器人同步定位与地图构建方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110051887.7A CN112882056B (zh) | 2021-01-15 | 2021-01-15 | 基于激光雷达的移动机器人同步定位与地图构建方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112882056A true CN112882056A (zh) | 2021-06-01 |
| CN112882056B CN112882056B (zh) | 2024-04-09 |
Family
ID=76049562
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110051887.7A Active CN112882056B (zh) | 2021-01-15 | 2021-01-15 | 基于激光雷达的移动机器人同步定位与地图构建方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112882056B (zh) |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113379841A (zh) * | 2021-06-21 | 2021-09-10 | 上海仙工智能科技有限公司 | 一种基于相位相关法与因子图的激光slam方法及其可读存储介质 |
| CN113514810A (zh) * | 2021-07-07 | 2021-10-19 | 北京信息科技大学 | Mimo雷达观测噪声优化方法及装置 |
| CN114489036A (zh) * | 2021-07-25 | 2022-05-13 | 西北农林科技大学 | 一种基于slam的室内机器人导航控制方法 |
| CN114723795A (zh) * | 2022-04-18 | 2022-07-08 | 长春工业大学 | 基于改进最近点配准的斗轮机无人化作业定位建图方法 |
| CN115014352A (zh) * | 2022-06-01 | 2022-09-06 | 浙江大学 | 一种基于建议分布地图的室内全局定位方法 |
| CN115267812A (zh) * | 2022-07-28 | 2022-11-01 | 广州高新兴机器人有限公司 | 一种基于高亮区域的定位方法、装置、介质及机器人 |
| CN115291241A (zh) * | 2022-08-29 | 2022-11-04 | 太原理工大学 | 一种基于SLAM的针对辐射工厂的α/β辐射地图构建方法 |
| CN115388893A (zh) * | 2022-08-25 | 2022-11-25 | 西安电子科技大学芜湖研究院 | 基于遗传的滤波slam算法的移动机器人建图方法 |
| CN116309907A (zh) * | 2023-03-06 | 2023-06-23 | 中国人民解放军海军工程大学 | 一种基于改进的Gmapping算法的移动机器人建图方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103983963A (zh) * | 2014-06-09 | 2014-08-13 | 北京数字绿土科技有限公司 | 一种多站地基激光雷达数据的自动配准方法 |
| EP3078935A1 (en) * | 2015-04-10 | 2016-10-12 | The European Atomic Energy Community (EURATOM), represented by the European Commission | Method and device for real-time mapping and localization |
| CN107991683A (zh) * | 2017-11-08 | 2018-05-04 | 华中科技大学 | 一种基于激光雷达的机器人自主定位方法 |
| CN110412596A (zh) * | 2019-07-10 | 2019-11-05 | 上海电机学院 | 一种基于图像信息和激光点云的机器人定位方法 |
| CN110927740A (zh) * | 2019-12-06 | 2020-03-27 | 合肥科大智能机器人技术有限公司 | 一种移动机器人定位方法 |
-
2021
- 2021-01-15 CN CN202110051887.7A patent/CN112882056B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103983963A (zh) * | 2014-06-09 | 2014-08-13 | 北京数字绿土科技有限公司 | 一种多站地基激光雷达数据的自动配准方法 |
| EP3078935A1 (en) * | 2015-04-10 | 2016-10-12 | The European Atomic Energy Community (EURATOM), represented by the European Commission | Method and device for real-time mapping and localization |
| CN107991683A (zh) * | 2017-11-08 | 2018-05-04 | 华中科技大学 | 一种基于激光雷达的机器人自主定位方法 |
| CN110412596A (zh) * | 2019-07-10 | 2019-11-05 | 上海电机学院 | 一种基于图像信息和激光点云的机器人定位方法 |
| CN110927740A (zh) * | 2019-12-06 | 2020-03-27 | 合肥科大智能机器人技术有限公司 | 一种移动机器人定位方法 |
Non-Patent Citations (1)
| Title |
|---|
| 章弘凯;陈年生;范光宇;: "基于粒子滤波的智能机器人定位算法", 计算机应用与软件, no. 02 * |
Cited By (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113379841A (zh) * | 2021-06-21 | 2021-09-10 | 上海仙工智能科技有限公司 | 一种基于相位相关法与因子图的激光slam方法及其可读存储介质 |
| CN113379841B (zh) * | 2021-06-21 | 2024-04-30 | 上海仙工智能科技有限公司 | 一种基于相位相关法与因子图的激光slam方法及其可读存储介质 |
| CN113514810B (zh) * | 2021-07-07 | 2023-07-18 | 北京信息科技大学 | Mimo雷达观测噪声优化方法及装置 |
| CN113514810A (zh) * | 2021-07-07 | 2021-10-19 | 北京信息科技大学 | Mimo雷达观测噪声优化方法及装置 |
| CN114489036A (zh) * | 2021-07-25 | 2022-05-13 | 西北农林科技大学 | 一种基于slam的室内机器人导航控制方法 |
| CN114723795A (zh) * | 2022-04-18 | 2022-07-08 | 长春工业大学 | 基于改进最近点配准的斗轮机无人化作业定位建图方法 |
| CN115014352A (zh) * | 2022-06-01 | 2022-09-06 | 浙江大学 | 一种基于建议分布地图的室内全局定位方法 |
| CN115267812A (zh) * | 2022-07-28 | 2022-11-01 | 广州高新兴机器人有限公司 | 一种基于高亮区域的定位方法、装置、介质及机器人 |
| CN115388893A (zh) * | 2022-08-25 | 2022-11-25 | 西安电子科技大学芜湖研究院 | 基于遗传的滤波slam算法的移动机器人建图方法 |
| CN115388893B (zh) * | 2022-08-25 | 2024-05-14 | 西安电子科技大学芜湖研究院 | 基于遗传的滤波slam算法的移动机器人建图方法 |
| CN115291241A (zh) * | 2022-08-29 | 2022-11-04 | 太原理工大学 | 一种基于SLAM的针对辐射工厂的α/β辐射地图构建方法 |
| CN115291241B (zh) * | 2022-08-29 | 2024-04-26 | 太原理工大学 | 一种基于SLAM的针对辐射工厂的α/β辐射地图构建方法 |
| CN116309907A (zh) * | 2023-03-06 | 2023-06-23 | 中国人民解放军海军工程大学 | 一种基于改进的Gmapping算法的移动机器人建图方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112882056B (zh) | 2024-04-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112882056A (zh) | 基于激光雷达的移动机器人同步定位与地图构建方法 | |
| CN109798896B (zh) | 一种室内机器人定位与建图方法及装置 | |
| CN105865449B (zh) | 基于激光和视觉的移动机器人的混合定位方法 | |
| CN107991683B (zh) | 一种基于激光雷达的机器人自主定位方法 | |
| CN110689576B (zh) | 一种基于Autoware的动态3D点云正态分布AGV定位方法 | |
| CN109556611B (zh) | 一种基于图优化和粒子滤波的融合定位方法 | |
| CN109579849A (zh) | 机器人定位方法、装置和机器人及计算机存储介质 | |
| Einhorn et al. | Finding the adequate resolution for grid mapping-cell sizes locally adapting on-the-fly | |
| CN113483747B (zh) | 基于具有墙角信息的语义地图改进amcl定位方法及机器人 | |
| CN111551184B (zh) | 一种移动机器人slam的地图优化方法及系统 | |
| CN113108773A (zh) | 一种融合激光与视觉传感器的栅格地图构建方法 | |
| CN109932713A (zh) | 定位方法、装置、计算机设备、可读存储介质和机器人 | |
| CN110763239B (zh) | 滤波组合激光slam建图方法及装置 | |
| CN115952691B (zh) | 多站无源时差交叉联合定位系统的优化布站方法及装置 | |
| CN110514567B (zh) | 基于信息熵的气体源搜索方法 | |
| Leung et al. | An improved weighting strategy for rao-blackwellized probability hypothesis density simultaneous localization and mapping | |
| CN114608585A (zh) | 一种移动机器人同步定位与建图方法及装置 | |
| CN115201849A (zh) | 一种基于矢量地图的室内建图方法 | |
| CN113406658A (zh) | 一种基于点线特征扫描匹配的移动机器人定位方法 | |
| CN110333513B (zh) | 一种融合最小二乘法的粒子滤波slam方法 | |
| CN114415661B (zh) | 基于压缩三维空间点云的平面激光slam与导航方法 | |
| Kwon et al. | Efficiency improvement in Monte Carlo localization through topological information | |
| Zeng et al. | An Indoor 2D LiDAR SLAM and Localization Method Based on Artificial Landmark Assistance | |
| CN116563354A (zh) | 一种结合特征提取和聚类算法的激光点云配准方法 | |
| CN115855086A (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 |