CN111427370B - 一种基于稀疏位姿调整的移动机器人的Gmapping建图方法 - Google Patents

一种基于稀疏位姿调整的移动机器人的Gmapping建图方法 Download PDF

Info

Publication number
CN111427370B
CN111427370B CN202010515565.9A CN202010515565A CN111427370B CN 111427370 B CN111427370 B CN 111427370B CN 202010515565 A CN202010515565 A CN 202010515565A CN 111427370 B CN111427370 B CN 111427370B
Authority
CN
China
Prior art keywords
pose
robot
particle
map
updating
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
CN202010515565.9A
Other languages
English (en)
Other versions
CN111427370A (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.)
Beijing University of Civil Engineering and Architecture
Original Assignee
Beijing University of Civil Engineering and Architecture
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 Beijing University of Civil Engineering and Architecture filed Critical Beijing University of Civil Engineering and Architecture
Priority to CN202010515565.9A priority Critical patent/CN111427370B/zh
Publication of CN111427370A publication Critical patent/CN111427370A/zh
Application granted granted Critical
Publication of CN111427370B publication Critical patent/CN111427370B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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
    • G05D1/0253—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 extracting relative motion information from a plurality of images taken successively, e.g. visual odometry, optical flow
    • 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
    • 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
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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 or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明涉及机器人领域，具体提供了一种基于稀疏位姿调整的移动机器人的Gmapping建图方法，包括：S1：初始化粒子位姿与分布，S2：扫描匹配；S3：计算采样位置
Figure 722813DEST_PATH_IMAGE001
的目标分布；S4：计算高斯近似；S5：更新第i个粒子的权重；S6：更新粒子地图；S3，S4的同时并行S3’位姿图构建及S4’闭环约束。本发明解决了原有Gmapping算法在较少粒子时存在的边界模糊、缺失、及滑移的技术问题，构建精度高，边界清晰完整，稳定性好。

Description

一种基于稀疏位姿调整的移动机器人的Gmapping建图方法
技术领域
本发明涉及机器人技术领域，尤其涉及一种基于稀疏位姿调整的移动机器人的Gmapping建图方法。
背景技术
近年来，随着“工业4.0”、“智能制造”及“中国制造2025”等概念的提出，机器人领域取得了长足的进步与蓬勃的发展。在服务机器人领域，室内移动机器人的研究成为了一个热点问题。当前，对室内移动机器人的研究主要围绕地图构建、定位、导航等方面展开，即解决移动机器人的“我是谁”、“我在哪儿”及“我要去哪儿”的问题。但以上问题并不是孤立存在的，在未知环境下精确的地图构建依赖于准确的定位信息，而精准的定位依赖于准确的建图，移动机器人的自主导航与路径规划则依赖于准确的环境地图与定位，所以移动机器人的研究的首要问题就是建图与定位问题。
由于Gmapping-SLAM算法在环境复杂、里程计收到噪声干扰且噪声不全为高斯噪声时，可能会产生滤波器的过度过滤问题。基于此，本发明针对Gmapping-SLAM进行了改进，使用图优化理论和闭环检测对Gmapping的扫描匹配过程进行了全局非线性优化，使其可以在保持原有构建地图的速度的基础上，增加了构建栅格地图的质量，从而实现对Gmapping-SLAM的优化。
发明内容
为解决原有Gmapping算法在较少粒子时存在的边界模糊、缺失、及滑移的技术问题，本发明提供一种基于稀疏位姿调整的移动机器人的Gmapping建图方法，包括：
S1：初始化粒子位姿与分布，通过上一时刻第i个粒子的机器人位姿
Figure DEST_PATH_IMAGE001
与里程计信息
Figure DEST_PATH_IMAGE002
估计该时刻第i个粒子的机器人估计位姿
Figure DEST_PATH_IMAGE003
，计算提议分布p；
S2：基于地图信息
Figure DEST_PATH_IMAGE004
、机器人估计位姿
Figure DEST_PATH_IMAGE005
、观测量
Figure DEST_PATH_IMAGE006
，匹配扫描第i个粒子的机器人估计位姿
Figure DEST_PATH_IMAGE007
的周边区域；
若扫描匹配成功则进入S3，计算机器人位姿的极大似然估计值
Figure DEST_PATH_IMAGE008
；并判断是否启动线性优化线程，若启动线性优化流程，则执行S3及S3’；否则，仅执行S3；
若扫描匹配失败，则跳过S3、S4，计算机器人位姿
Figure DEST_PATH_IMAGE009
，更新第i个粒子的权重
Figure DEST_PATH_IMAGE010
；
所述计算机器人位姿
Figure 869045DEST_PATH_IMAGE009
的计算公式为：
Figure DEST_PATH_IMAGE011
；
所述更新第i个粒子的权重
Figure 97770DEST_PATH_IMAGE010
的计算公式为：
Figure DEST_PATH_IMAGE012
；
S3：通过计算扫描匹配区域内各点的均值与协方差矩阵，计算采样位置
Figure DEST_PATH_IMAGE013
的目标分布
Figure DEST_PATH_IMAGE014
，计算归一化因子
Figure DEST_PATH_IMAGE015
；
所述归一化因子
Figure 752874DEST_PATH_IMAGE015
的计算公式为：
Figure DEST_PATH_IMAGE016
；
S4：计算高斯近似
Figure DEST_PATH_IMAGE017
，计算第i个粒子的更新位姿
Figure DEST_PATH_IMAGE018
；
所述高斯近似
Figure 521984DEST_PATH_IMAGE017
的计算公式为：
Figure DEST_PATH_IMAGE019
，
Figure DEST_PATH_IMAGE020
；
所述第i个粒子的更新位姿
Figure 398673DEST_PATH_IMAGE018
的计算公式为：
Figure DEST_PATH_IMAGE021
；
S5：更新第i个粒子的权重，所述第i个粒子的权重的更新公式为：
Figure DEST_PATH_IMAGE022
；
S6：更新机器人位姿与粒子地图，所述更新机器人位姿的方法为：通过所述第i个粒子的更新位姿
Figure 34185DEST_PATH_IMAGE018
与位姿修正量
Figure DEST_PATH_IMAGE023
更新机器人位姿
Figure DEST_PATH_IMAGE024
，所述更新粒子地图的方法为：通过所述直接稀疏特征矩阵H、第i个粒子的更新位姿
Figure DEST_PATH_IMAGE025
、观测量
Figure DEST_PATH_IMAGE026
，及更新第i个粒子的地图及采样粒子集更新粒子地图及粒子信息；
S7：判断地图创建是否完成，若地图创建完成，则结束流程；
若地图创建未完成，则判断是否进行粒子重采样；
判断是否进行粒子重采样的方法为：
若有效样本量
Figure DEST_PATH_IMAGE027
小于阈值T，进行粒子重采样，返回S5；
若有效样本量
Figure DEST_PATH_IMAGE028
大于等于阈值T，则不进行粒子重采样，返回S2；
S3’：构建位姿图；所述位姿图构建包括，计算机器人极大似然估计值，然后进入S4’；
S4’：通过闭环约束计算机器人位姿修正量
Figure 737437DEST_PATH_IMAGE023
；
所述
Figure 749386DEST_PATH_IMAGE023
的计算公式为：
Figure DEST_PATH_IMAGE029
，
Figure DEST_PATH_IMAGE030
，
其中，
Figure DEST_PATH_IMAGE031
为上一时刻机器人位姿，
Figure DEST_PATH_IMAGE032
为里程计信息，
Figure DEST_PATH_IMAGE033
为地图信息，
Figure DEST_PATH_IMAGE034
为第i个粒子的机器人估计位姿，
Figure DEST_PATH_IMAGE035
为观测量，
Figure DEST_PATH_IMAGE036
为精度矩阵，
Figure DEST_PATH_IMAGE037
为归一化因子，
Figure DEST_PATH_IMAGE038
为第i个粒子的权重。
优选的，所述提议分布的计算公式为：
Figure DEST_PATH_IMAGE039
，其中，
Figure DEST_PATH_IMAGE040
为上一时刻机器人位姿，
Figure DEST_PATH_IMAGE041
为地图信息，
Figure DEST_PATH_IMAGE042
为观测量；
所述第i个粒子的机器人估计位姿
Figure DEST_PATH_IMAGE043
的计算公式为：
Figure DEST_PATH_IMAGE044
，
其中，
Figure DEST_PATH_IMAGE045
表示标准位姿合成运算符。
优选的，所述极大似然估计值
Figure DEST_PATH_IMAGE046
的计算公式为：
Figure DEST_PATH_IMAGE047
，其中，
Figure DEST_PATH_IMAGE048
为地图信息，
Figure DEST_PATH_IMAGE049
为观测量，
Figure DEST_PATH_IMAGE050
为第i个粒子的机器人估计位姿。
优选的，所述有效样本量
Figure DEST_PATH_IMAGE051
的计算公式为：
Figure DEST_PATH_IMAGE052
，其中，
Figure DEST_PATH_IMAGE053
为粒子归一化权重。
优选的，所述误差雅克比矩阵J的计算方法为：
Figure DEST_PATH_IMAGE054
；
Figure DEST_PATH_IMAGE055
Figure DEST_PATH_IMAGE056
；
Figure DEST_PATH_IMAGE057
；
Figure DEST_PATH_IMAGE058
，
Figure DEST_PATH_IMAGE059
，
Figure DEST_PATH_IMAGE060
，
Figure DEST_PATH_IMAGE061
；
其中，
Figure DEST_PATH_IMAGE062
为观测量函数，
Figure DEST_PATH_IMAGE063
为第i时刻机器人位姿，
Figure DEST_PATH_IMAGE064
为误差函数，
Figure DEST_PATH_IMAGE065
为机器人位置向量
Figure DEST_PATH_IMAGE066
，
Figure DEST_PATH_IMAGE067
为机器人偏航角，
Figure DEST_PATH_IMAGE068
为关于
Figure 501090DEST_PATH_IMAGE067
的
Figure DEST_PATH_IMAGE069
变换矩阵。
附图说明
图1为实施例一提供的基于稀疏位姿调整的移动机器人的Gmapping建图方法算法流程图
图2为实施例二提供的基于稀疏位姿调整的移动机器人的Gmapping建图方法的移动机器人SLAM过程示意图
图3为实施例二提供的基于稀疏位姿调整的移动机器人的Gmapping建图方法的移动机器人坐标系图
图4为实施例二提供的基于稀疏位姿调整的移动机器人的Gmapping建图方法的激光雷达坐标表示图
图5为实施例二提供的基于稀疏位姿调整的移动机器人的Gmapping建图方法的相邻时刻机器人位姿估计图
图6（a）为实施例二提供的基于稀疏位姿调整的移动机器人的Gmapping建图方法的无障碍添加时建图实验使用15个粒子的Gmapping算法建图
图6（b）为实施例二提供的基于稀疏位姿调整的移动机器人的Gmapping建图方法的无障碍添加时建图实验使用15个粒子的本发明算法建图。
具体实施方式
下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护范围。
实施例一
本实施例提供了一种基于稀疏位姿调整的移动机器人的Gmapping建图方法，如图1所示，步骤包括：
S1：初始化粒子位姿与分布，通过上一时刻第i个粒子的机器人位姿
Figure DEST_PATH_IMAGE070
与里程计信息
Figure DEST_PATH_IMAGE071
估计该时刻第i个粒子的机器人估计位姿
Figure DEST_PATH_IMAGE072
，计算提议分布p；
S2：基于地图信息
Figure DEST_PATH_IMAGE073
、机器人估计位姿
Figure DEST_PATH_IMAGE074
、观测量
Figure DEST_PATH_IMAGE075
，匹配扫描第i个粒子的机器人估计位姿
Figure 681404DEST_PATH_IMAGE074
的周边区域；
若扫描匹配成功则进入S3，计算机器人位姿的极大似然估计值
Figure DEST_PATH_IMAGE076
；并判断是否启动线性优化线程，若启动线性优化流程，则执行S3及S3’；否则，仅执行S3；
若扫描匹配失败，则跳过S3、S4，计算机器人位姿
Figure DEST_PATH_IMAGE077
，更新第i个粒子的权重
Figure DEST_PATH_IMAGE078
；
所述计算机器人位姿
Figure 809635DEST_PATH_IMAGE077
的计算公式为：
Figure DEST_PATH_IMAGE079
；
所述更新第i个粒子的权重
Figure DEST_PATH_IMAGE080
的计算公式为：
Figure DEST_PATH_IMAGE081
；
S3：通过计算扫描匹配区域内各点的均值与协方差矩阵，计算采样位置的目标分布
Figure DEST_PATH_IMAGE082
，计算归一化因子
Figure DEST_PATH_IMAGE083
；
所述归一化因子
Figure 422013DEST_PATH_IMAGE083
的计算公式为：
Figure DEST_PATH_IMAGE084
；
S4：计算高斯近似
Figure DEST_PATH_IMAGE085
，计算第i个粒子的更新位姿
Figure DEST_PATH_IMAGE086
；
所述高斯近似
Figure 319300DEST_PATH_IMAGE085
的计算公式为：
Figure DEST_PATH_IMAGE087
，
Figure DEST_PATH_IMAGE088
；
所述第i个粒子的更新位姿
Figure 811461DEST_PATH_IMAGE086
的计算公式为：
Figure DEST_PATH_IMAGE089
；
S5：更新第i个粒子的权重，所述第i个粒子的权重的更新公式为：
Figure DEST_PATH_IMAGE090
；
S6：更新机器人位姿与粒子地图，所述更新机器人位姿的方法为：通过所述第i个粒子的更新位姿
Figure 866136DEST_PATH_IMAGE086
与位姿修正量
Figure DEST_PATH_IMAGE091
更新机器人位姿
Figure DEST_PATH_IMAGE092
，所述更新粒子地图的方法为：通过所述直接稀疏特征矩阵H、第i个粒子的更新位姿
Figure 515161DEST_PATH_IMAGE086
、观测量
Figure DEST_PATH_IMAGE093
，及更新第i个粒子的地图及采样粒子集更新粒子地图及粒子信息；
S7：判断地图创建是否完成，若地图创建完成，则结束流程；
若地图创建未完成，则判断是否进行粒子重采样；
判断是否进行粒子重采样的方法为：
若有效样本量
Figure DEST_PATH_IMAGE094
小于阈值T，进行粒子重采样，返回S5；
若有效样本量
Figure 220948DEST_PATH_IMAGE094
大于等于阈值T，则不进行粒子重采样，返回S2；
S3’：构建位姿图；所述位姿图构建包括，计算机器人极大似然估计值
Figure DEST_PATH_IMAGE095
，然后进入S4’；
S4’：通过闭环约束计算机器人位姿修正量
Figure 369164DEST_PATH_IMAGE091
；
所述
Figure 98086DEST_PATH_IMAGE091
的计算公式为：
Figure DEST_PATH_IMAGE096
，
Figure DEST_PATH_IMAGE097
，
其中，
Figure DEST_PATH_IMAGE098
为上一时刻机器人位姿，
Figure DEST_PATH_IMAGE099
为里程计信息，
Figure DEST_PATH_IMAGE100
为地图信息，
Figure DEST_PATH_IMAGE101
为第i个粒子的机器人估计位姿，
Figure DEST_PATH_IMAGE102
为观测量，
Figure DEST_PATH_IMAGE103
为精度矩阵，
Figure DEST_PATH_IMAGE104
为归一化因子，
Figure DEST_PATH_IMAGE105
为第i个粒子的权重。
优选的，所述提议分布的计算公式为：
Figure DEST_PATH_IMAGE106
，其中，
Figure 488485DEST_PATH_IMAGE098
为上一时刻机器人位姿，
Figure 298047DEST_PATH_IMAGE100
为地图信息，
Figure 69693DEST_PATH_IMAGE102
为观测量；
所述第i个粒子的机器人估计位姿的计算公式为：
Figure DEST_PATH_IMAGE107
，
其中，
Figure DEST_PATH_IMAGE108
表示标准位姿合成运算符。
优选的，所述极大似然估计值
Figure DEST_PATH_IMAGE109
的计算公式为：
Figure DEST_PATH_IMAGE110
，其中，
Figure 895698DEST_PATH_IMAGE100
为地图信息，
Figure 840520DEST_PATH_IMAGE102
为观测量，
Figure 193004DEST_PATH_IMAGE101
为第i个粒子的机器人估计位姿。
优选的，所述有效样本量
Figure 135553DEST_PATH_IMAGE051
的计算公式为：
Figure 885072DEST_PATH_IMAGE052
，其中，
Figure 305689DEST_PATH_IMAGE053
为粒子归一化权重。
优选的，所述误差雅克比矩阵J的计算方法为：
Figure 840575DEST_PATH_IMAGE054
；
Figure 954025DEST_PATH_IMAGE055
Figure 692305DEST_PATH_IMAGE056
；
Figure 916613DEST_PATH_IMAGE057
；
Figure 978110DEST_PATH_IMAGE058
，
Figure 324777DEST_PATH_IMAGE059
，
Figure 737304DEST_PATH_IMAGE060
，
Figure 76887DEST_PATH_IMAGE061
；
其中，
Figure 258470DEST_PATH_IMAGE062
为观测量函数，
Figure 510460DEST_PATH_IMAGE063
为第i时刻机器人位姿，
Figure 410283DEST_PATH_IMAGE064
为误差函数，
Figure 789443DEST_PATH_IMAGE065
为机器人位置向量
Figure 91111DEST_PATH_IMAGE066
，
Figure 514002DEST_PATH_IMAGE067
为机器人偏航角，
Figure 901121DEST_PATH_IMAGE068
为关于
Figure 270922DEST_PATH_IMAGE067
的
Figure 473103DEST_PATH_IMAGE069
变换矩阵。
本实施例主要针对移动机器人在大型室内复杂环境下的2D激光雷达建图方法进行了研究。对其中的具有代表性、应用广泛的ROS Gmapping-SLAM进行了深入研究，针对其在大型复杂非结构化室内场景下的地图构建过程中的边界模糊与边界畸变等问题，提出了一种基于SPA优化的改进Gmapping算法。实现了系统建模与搭建基于ROS与Stage的实验平台，在此基础上进行了两组实验并对实验结果进行了分析。最后得出了相同粒子数下，本发明提出的基于SPA优化的改进Gmapping算法具有更高的建图精度、鲁棒性与稳定性的结论。并通过研究发现，使用本发明提出的基于SPA优化的改进Gmapping算法相比ROS Gmapping-SLAM可以降低Gmapping-SLAM建图所需的粒子数，节省了计算资源，降低了对硬件的要求，具有更广泛的应用范围和实用价值。
实施例二
本实施例在实施例一的基础上，进一步提供了一种基于稀疏位姿调整的移动机器人的Gmapping建图方法，如图2-图6（b）所示。
当前，所有的激光雷达2D SLAM算法在地图构建过程中均依赖于概率模型，其本质是移动机器人的状态估计问题。应用概率模型的优点即是可以增强系统对观测噪声的鲁棒性与系统在测量估计过程中正式表示其不确定性的能力。基于概率模型的激光雷达2DSLAM（2维平面同时定位与建图方法）主要依靠贝叶斯定理求解。基于滤波器的SLAM主要可分为基于卡尔曼滤波器KF（Kalman filters）的SLAM和基于粒子滤波PF（Particlefilters）的SLAM方法。
KF滤波器是贝叶斯滤波器的成功实现之一，主要包含状态空间预测与状态更新两个阶段。为解决移动机器人模型的动态非线性问题，诞生了扩展卡尔曼滤波器EKF（Extended Kalman Filter），采用1阶泰勒公式，将非线性问题线性化。由于基于卡尔曼滤波器的SLAM计算量大，系统易受环境噪声影响鲁棒性较差且长时间运行存在线性化误差积累等缺点，催生出了基于PF的SLAM方法。
基于粒子滤波器PF的SLAM方法是贝叶斯滤波器的另一个应用，后验概率由一组加权的粒子表示，赋予每个粒子重要性，并假设当前状态只与上一状态有关。基于PF的SLAM代表性方法是Montemarlo（蒙特卡罗）等提出的FastSLAM1.0（实时同时定位与建图框架）及FastSLAM2.0（实时同时定位与建图框架）算法。其核心思想是每个粒子具有K个KF（卡尔曼滤波器），以此来计算地图K个地标的位置，从而实现对地图的估计，通过PF（粒子滤波器）对机器人轨迹进行估计。该算法的时间复杂度要低于EKF-SLAM（基于扩展卡尔曼滤波器的同时定位与建图方法）。算法主要分为采样、分配权重、重采样和地图估计四个过程。
由于FastSLAM（实时同时定位与建图框架）的提议分布中没有考虑观测量，所以导致其提议分布具有盲目性。同时，每当有观察到新的观测量，系统就要重新计算所有粒子的权重，并进行重采样过程，而这就导致了算法所需粒子数目过多及粒子退化问题。在大规模长时间建图过程中，会导致算法陷入局部最优，从而导致算法失效的问题。针对此问题，Grisettiyz等人提出了基于RBPF（改进的粒子滤波器）的改进SLAM方法，也就是ROS系统中集成的Gmapping算法。该算法在采样过程中的提议分布中加入了观测量，使得粒子仅分布于机器人位姿的局部范围；更新了权重的计算公式，使其具有递归性；加入了自适应重采样过程，仅当粒子数小于有效粒子数时，开始重采样过程，大大缩减了重采样的次数与所需粒子数。移动机器人SLAM过程示意图如图1所示。从t-1时刻到t+2时刻的移动机器人的位姿表示为
Figure DEST_PATH_IMAGE111
，里程计的运动控制量表示为
Figure DEST_PATH_IMAGE112
，地标表示为
Figure DEST_PATH_IMAGE113
，激光雷达的扫描观测量表示为
Figure DEST_PATH_IMAGE114
。
由于Gmapping-SLAM（基于Gmapping算法的同时定位与建图方法）在提议分布中加入了观测量，更新了权重计算公式，并加入了自适应重采样过程，使得其在初始化时具有较为准确的估计值。采用概率估计的方法，当前时刻的估计量仅与上一时刻的状态有关，减少了内存消耗，保证其可以使用较少粒子的情况下，高效建出较为精确的地图。但在大规模复杂室内环境中，特别是里程计信息受到噪声干扰时，Gmapping-SLAM的滤波器会产生过度过滤的问题；而在观察期间，由于观测不良或者重叠过小会导致扫描匹配的问题，从而影响了建图质量。基于Gmapping-SLAM建图过程中出现的一些问题，国内外的一些学者提出了针对Gmapping-SLAM的改进方法。
罗元等提出了一种针对RBPF-SLAM（基于改进粒子滤波器的同时定位与建图方法）的改进算法，在特定状态下的粒子集中选取一个代表粒子，进行KF（卡尔曼滤波器）状态估计与更新；同时结合了Gmapping-SLAM（基于Gmapping算法的同时定位与建图方法）的提议分布与自适应重采样技术，降低了算法复杂度，提高了系统的实时性，但降低了系统的鲁棒性。王田橙等针对粒子退化与粒子衰竭等问题，提出了一种改进的SLAM方法。该方法使用区域粒子群优化算法调整粒子的提议分布，使用部分高斯分布重采样方法对权值过高过低的粒子进行重采样，提高了建图精度，提升了效率。Hui Xiong等提出了一种基于粒子滤波的扫描匹配SLAM方法，该方法可以在线构建复杂环境的二维栅格地图，可以减少内存消耗和循环闭合的问题，并构建较为准确的地图。Liu Fuchun等提出了一种改进的RBPF-SLAM（基于改进粒子滤波器的同时定位与建图方法），在激光扫描分区时使用动态阈值，使用几何匹配和定位的高精度特征改进了RBPF的重要性密度函数，提升了系统的鲁棒性与环境构建的一致性。Taizhi Lv等提出了FC＆ASD-PSO（分数演算和阿尔法稳定分布）的改进FastSLAM2.0（实时同时定位与建图框架2.0）算法，该算法引入了调整噪声协方差矩阵Qt和Rt的步骤，使噪声协方差矩阵的值与实际值相匹配，减少了累积误差的影响，提升了建图精度。
以上学者均对基于RBPF（改进粒子滤波器）的SLAM做出了一些研究与改动，但是对于在机器人领域应用广泛的Gmapping-SLAM（基于Gmapping算法的同时定位与建图方法）缺乏改进。因此本发明针对大范围复杂环境下Gmapping-SLAM进行了优化改进，在保证Gmapping算法使用较少粒子、快速建图的优势上，加入后端闭环检测与基于位姿图优化的方法，使其构建更加准确的环境地图。
稀疏位姿调整SPA（Sparse Pose Adjustment）位姿图优化方法是一种可以从约束图中在线计算稀疏矩阵并使用直接稀疏cholesky分解法求解线性系统的方法，是一种非线性优化的位姿图优化方法。非线性优化是一种全局优化的方法，与EKF（扩展卡尔曼滤波器）等基于滤波器的方法相比，非线性优化可以有效地消除系统的线性误差与累积误差，从而得到比较好的优化效果与地图。基于图的优化最早被认为是一种消耗计算资源的方法，近年来随着机器人系统计算能力的快速发展与一系列更为高效的优化方法出现，基于位姿图的优化方法逐渐成为了研究热点之一。在移动机器人位姿图构建的过程中，将机器人的每个时刻下的运动状态（位姿和观测量）看成一个节点，运动状态之间的约束（坐标转换）看成一条边，将一个个节点通过边连接起来，就构成了位姿图。因此，地图构建过程就可以转化为找到满足当前所有约束下的最优节点，使用图的均值表示地图。其目标是共同优化节点的位姿，从而将边带来的误差最小化，基于图优化的SLAM框架图如图2所示。
非线性优化经典方法有捆绑调整Bundle Adjustment、Levenberg-Marquardt非线性优化器及Graph-SLAM（基于位姿图的同时定位与建图）等方法。但传统的非线性优化随着环境的增大，内存会呈现快速增长的趋势，计算量会大幅增加，时间复杂度较高。SPA（基于稀疏位姿调整的位姿图优化方法）是基于直接稀疏分解cholesky方法，采用构造稀疏矩阵的方式进行后端图优化，节省了内存，降低了算法复杂度，适用于大规模的场景建图过程中。同经典非线性优化的方法相比，具有以下几点优势。
1)SPA考虑了约束中的协方差信息，更加准确。
2)SPA具有很强的鲁棒性，在增量处理地图与批量处理下均高效，且具有较低的故障率和较快的收敛速度。
3)与EKF等滤波器相比，SPA是完全非线性方法。每次迭代时，SPA会针对当前位姿的所有约束进行线性化优化。
4)SPA在建图过程中可以连续优化位姿图，从而增量的构建地图。在大型环境且闭环较大时也无需借助子图存储或复杂分区的方案，可以占用很少的计算资源实现所有节点的最佳全局估计。
本实施例提出了一种基于SPA优化算法改进的Gmapping地图构建方法，将图优化过程作为与前端交错运行的单独线程实现对地图构建过程中的优化。算法步骤如下：
Step1：初始化机器人位姿估计，获得由上一时刻机器人位姿
Figure DEST_PATH_IMAGE115
与里程计信息
Figure DEST_PATH_IMAGE116
估计的第i个粒子的机器人位姿
Figure DEST_PATH_IMAGE117
，提出提议分布，式中
Figure DEST_PATH_IMAGE118
表示标准位姿合成运算符。计算公式如（1）（2）所示。
Figure DEST_PATH_IMAGE119
（1）
Figure DEST_PATH_IMAGE120
（2）
Step2：进行扫描匹配。基于地图信息
Figure DEST_PATH_IMAGE121
、机器人估计位姿
Figure DEST_PATH_IMAGE122
、观测量
Figure DEST_PATH_IMAGE123
从
Figure 394791DEST_PATH_IMAGE122
周围有限区域进行扫描匹配。若扫描匹配成功，则求解机器人位姿的极大似然估计值
Figure DEST_PATH_IMAGE124
，计算公式如（3）所示；并运行图优化线程，进行位姿图构建、闭环检测与SPA优化，公式如（4-8）所示。式中H为直接稀疏特征矩阵，构造线性系统函数模型如（4）所示。
Figure DEST_PATH_IMAGE125
为梯度下降法到Newton-Euler（牛顿-欧拉）法的转换因子，
Figure DEST_PATH_IMAGE126
为线性方程待求量。J为误差雅克比矩阵，定义为
Figure DEST_PATH_IMAGE127
，如公式（5）所示。
Figure DEST_PATH_IMAGE128
为精度矩阵，其值为协方差的倒数。第i时刻机器人位姿定义为
Figure DEST_PATH_IMAGE129
，两个位姿
Figure DEST_PATH_IMAGE130
、
Figure DEST_PATH_IMAGE131
之间的观测偏移量定义为
Figure DEST_PATH_IMAGE132
，
Figure DEST_PATH_IMAGE133
为关于
Figure DEST_PATH_IMAGE134
的
Figure DEST_PATH_IMAGE135
矩阵。观测量测量定义为
Figure DEST_PATH_IMAGE136
，计算见（6）所示，
Figure DEST_PATH_IMAGE137
为误差函数，计算见公式（7）、（8）所示。
若扫描匹配失败，则跳过Step3、Step4，重新计算提议分布、更新粒子权值与位姿估计，计算公式如（9）、（10）所示。
Figure DEST_PATH_IMAGE138
（3）
Figure DEST_PATH_IMAGE139
（4）
Figure DEST_PATH_IMAGE140
（5）
Figure DEST_PATH_IMAGE141
（6）
Figure DEST_PATH_IMAGE142
（7）
Figure DEST_PATH_IMAGE143
，
Figure DEST_PATH_IMAGE144
Figure DEST_PATH_IMAGE145
，
Figure DEST_PATH_IMAGE146
（8）
Figure DEST_PATH_IMAGE147
（9）
Figure DEST_PATH_IMAGE148
（10）
Step3：从
Figure DEST_PATH_IMAGE149
扫描匹配区域进行采样，逐点计算各点的均值与协方差矩阵，评估采样位置
Figure DEST_PATH_IMAGE150
的目标分布
Figure DEST_PATH_IMAGE151
，并计算归一化因子
Figure DEST_PATH_IMAGE152
，计算公式如（11）所示。
Figure DEST_PATH_IMAGE153
（11）
Step4：计算提议分布的高斯近似
Figure DEST_PATH_IMAGE154
，计算公式如（12）、（13）所示。对第i个粒子的新位姿进行采样，计算公式如（14）所示。
Figure DEST_PATH_IMAGE155
（12）
Figure DEST_PATH_IMAGE156
（13）
Figure DEST_PATH_IMAGE157
（14）
Step5：更新权重，见公式（15）；通过图优化过程更新粒子的位姿，如公式（4）所示。
Figure DEST_PATH_IMAGE158
（15）
Step6：通过新位姿和观测量更新粒子i的地图并更新采样粒子集。
Step7：进行粒子重采样过程。计算有效样本量是否小于阈值T，计算公式如（16）所示，若小于阈值，则进行重采样，否则不需要进行重采样。若重采样成功，返回Step5；若无需重采样，返回Step1。
Figure DEST_PATH_IMAGE159
（16）
针对两轮差动机器人模型的进行坐标系转换、激光雷达建模与里程计建模。
基于激光雷达的移动机器人坐标系主要包含世界坐标系、机器人坐标系及激光雷达坐标系。在机器人坐标系的表示中，采用笛卡尔坐标系是最广泛、最直观的表示方式。世界坐标系是固定在环境中的绝对坐标系，采用直角坐标的形式描述。机器人坐标系及激光雷达坐标系是相对坐标系，需要将其转换至世界坐标系中。由于激光雷达的观测坐标采用极坐标系的表示方式，因此需要将其以直角坐标形式转换至世界坐标系中，具体转换方法在后面给出。设两轮移动机器人的世界坐标系为
Figure DEST_PATH_IMAGE160
，机器人坐标系为
Figure DEST_PATH_IMAGE161
，表示方式如图4所示。机器人坐标系的原点在机器人的中心，其坐标在世界坐标系中的表示为
Figure DEST_PATH_IMAGE162
，代表机器人在世界坐标系的位姿。设机器人坐标系中的某点坐标为
Figure DEST_PATH_IMAGE163
，与
Figure DEST_PATH_IMAGE164
夹角为
Figure DEST_PATH_IMAGE165
，则其在世界坐标系
Figure 398036DEST_PATH_IMAGE160
中的坐标
Figure DEST_PATH_IMAGE166
如（17）所示：
Figure DEST_PATH_IMAGE167
（17）
本发明研究基于单线激光雷达的室内两轮移动机器人的地图构建，故将激光雷达的观测数据作为观测量。如上文所述，由于激光雷达观测数据采用极坐标表示，故需将其转换成直角坐标形式表示，并将激光雷达的观测点坐标转换至世界坐标系中。如图4所示，激光雷达测量点与激光雷达的距离为
Figure DEST_PATH_IMAGE168
，激光测量点与机器人坐标系的夹角为
Figure DEST_PATH_IMAGE169
，k表示激光测量点的编号。设t时刻机器人位姿为
Figure DEST_PATH_IMAGE170
，激光雷达在机器人坐标系中的位置为
Figure DEST_PATH_IMAGE171
，则激光测量点在
Figure DEST_PATH_IMAGE172
中的坐标
Figure DEST_PATH_IMAGE173
由公式（18）计算得到。式中
Figure DEST_PATH_IMAGE174
、
Figure DEST_PATH_IMAGE175
及
Figure DEST_PATH_IMAGE176
为测量噪声，一般假定为零均值高斯白噪声。
Figure DEST_PATH_IMAGE177
（18）
里程计是通过获得单位时间内的编码器数据进行移动机器人的位姿估计的装置。通过转换装置将电机转动量转换成脉冲信号，计算单位时间内接收到的脉冲信号数量，从而得到当前时刻的电机转速。设
Figure DEST_PATH_IMAGE178
为单位时间内机器人车轮位移量，则通过公式（19）即可求得
Figure 696162DEST_PATH_IMAGE178
，从而获得机器人航迹模型。最终通过积分即可求得给定时刻机器人位姿，实现对相邻时刻下的机器人位姿估计。
Figure DEST_PATH_IMAGE179
,
Figure DEST_PATH_IMAGE180
（19）
式中R为车轮半径，P为光电编码器的分辨率，
Figure DEST_PATH_IMAGE181
为电机转速的减速比，
Figure DEST_PATH_IMAGE182
为单位脉冲过程中车轮的位移，
Figure DEST_PATH_IMAGE183
为单位时间内发射的脉冲数量。机器人相邻时刻位姿估计图如图5所示。
Figure DEST_PATH_IMAGE184
为相邻时刻下机器人转动的圆弧角度，
Figure DEST_PATH_IMAGE185
为相邻时刻下机器人的偏航角，
Figure DEST_PATH_IMAGE186
为机器人转动半径，
Figure DEST_PATH_IMAGE187
为机器人两轮之间的距离，
Figure DEST_PATH_IMAGE188
为右轮比左轮多运动的距离，
Figure DEST_PATH_IMAGE189
、
Figure DEST_PATH_IMAGE190
分别为左右轮的线速度。机器人的直线运动速度极为左右轮线速度的均值，如公式（20）所示。
Figure DEST_PATH_IMAGE191
（20）
由几何关系可知，
Figure DEST_PATH_IMAGE192
，设
Figure DEST_PATH_IMAGE193
，则
Figure DEST_PATH_IMAGE194
可由公式（21）求得。移动机器人的航向角的角速度
Figure DEST_PATH_IMAGE195
即可由公式（22）求得，转动半径
Figure DEST_PATH_IMAGE196
可由公式（23）求得。
Figure DEST_PATH_IMAGE197
（21）
Figure DEST_PATH_IMAGE198
（22）
Figure DEST_PATH_IMAGE199
（23）
设机器人
Figure DEST_PATH_IMAGE200
时刻的位姿为
Figure DEST_PATH_IMAGE201
，采样时间为
Figure DEST_PATH_IMAGE202
，则可求得
Figure DEST_PATH_IMAGE203
时刻机器人位姿为
Figure DEST_PATH_IMAGE204
，如公式（24）所示。
Figure DEST_PATH_IMAGE205
（24）
在本发明提出的基于SPA非线性优化改进Gmapping 2D地图构建方法及建立的两轮差动机器人模型的基础上，设计并搭建了移动机器人硬件平台、基于ROS的机器人通信架构及Stage实验仿真平台。
硬件平台主要由镭神智能的LSLIDAR-N301型激光雷达、英特尔NUC7i5BNH型主机、STM32控制板、移动机器人底盘、显示器、24V电池电源等组成。
LSLIDAR-N301激光雷达放置在机器人正前方，实时测量数据并将其应用于移动机器人的激光SLAM过程中。最大测量距离可达30m，扫描角度可达
Figure DEST_PATH_IMAGE206
（在实际使用中将其限制为
Figure DEST_PATH_IMAGE207
），角度分辨率为
Figure DEST_PATH_IMAGE208
，扫描精度可达
Figure DEST_PATH_IMAGE209
cm，发布数据的频率10Hz。
移动机器人的主机采用英特尔公司的NUC7i5BNHW微型PC，用以实时处理数据并实现建图、定位与运动规划等功能。该微型PC采用酷睿i5-7260U处理器，电源电压为12V-19V，包含1个232串口、1个HDMI接口及6个USB接口，支持蓝牙及WIFI功能，具有强大的计算能力及通信能力，支持机器人通电自启动功能、远程操控功能、网络唤醒等功能。采用英特尔IrisTM Plus Graphics 640核芯显卡及双通道DDR4-2133 SODIMMs内存，具有较强的图像处理能力。该微型PC具有功能强大、接口丰富、易于携带、便于安装、运行功耗小且支持Windows和Linux双系统运行等特点，可以满足实验的要求。
移动机器人的底盘由两个主动轮与两个万向从动轮组成；两个主动轮在机器人底盘横向对称布置，由包含减速器与光电编码器（与主动轮同轴安装）的直流电机控制系统直接驱动，通过左右轮的差速运动实现了移动机器人的全向运动，构成了双轮差分机器人运动系统；两个从动轮在机器人底盘纵向对称布置，用于辅助转向。
通信架构采用ROS分布式机器人操作平台，可以实现多线程实时通信，将功能以一个功能包（Package）及元功能包（MetaPackage）的形式封装运行，具有松耦合特性。仿真平台采用Stage仿真器，Stage具有方便准确的使用特点，由于本实验主要研究单线激光雷达的2D地图构建，采用Stage仿真器可以满足要求。仿真过程中采用戴尔笔记本电脑，使用ubuntu 16.04系统，采用英特尔CoreTM i7-7700HQ处理器，安装内存为16g，ROS平台采用kinetic的版本，可以满足仿真和实验要求。
本实施例对采用键盘控制机器人进行地图构建，并在Rviz可视化界面与Stage仿真器中进行实时显示，地图的尺寸为47
Figure DEST_PATH_IMAGE210
33m，构建地图的分辨率采用0.025m；在实验过程中将实验分成两组进行测试，首先，比较无障碍添加时相同粒子数的Gmapping算法与本发明提出的基于SPA的改进Gmapping算法的建图效果，然后，比较添加障碍物时相同粒子数的两种算法的建图效果。
第一组实验中采用控制变量法分析比较无障碍添加时的Gmapping算法与本发明提出的基于SPA优化的改进Gmapping算法地图构建效果。影响Gmapping算法地图构建的主要参数为粒子数量，对于大型规模的复杂环境地图构建过程中的最优粒子数为30个。基于此，将实验一分为三个具有不同粒子数的对照组进行分析比较。
图6（a）、图6（b）为使用15个粒子时的两种算法地图构建的对比
Gmapping-SLAM使用30个粒子所得到的地图整体精度较高，但在局部范围出现了边界模糊的问题，导致对障碍物、地图的显示不完整，影响了建图的精度；而本发明提出的基于SPA优化的改进Gmapping算法则对当前构建地图的特征进行了实时检测优化，在上述区域构建出了更加清晰准确的边界与障碍物轮廓，建立了更加准确的地图。
如图6（a）、图6（b）所示，当采用15个粒子构建地图时Gmapping-SLAM的建图精度有了一定程度的降低，出现了不同程度的边界缺失问题，影响地图构建的精度。本发明提出的基于SPA优化的改进Gmapping算法的精度同使用30个粒子时的改进算法相比略有下降，但对构建地图的精度影响很小。同图6（a）相比，改进算法的得到的地图图6（b）在相关区域的得到了更为准确的地图边界与特征，所构建的地图更加准确。
当采用5个粒子构建地图时，Gmapping-SLAM出现了较为严重的边界模糊与障碍物模糊现象，而本发明所使用的改进算法则呈现出了相对较好的建图精度，得到的障碍物特征更加清晰，地图更加规整。
综上所述，在结构化的大型室内场景中，使用相同粒子数时，本发明提出的基于SPA优化的Gmapping算法可以构建出精度优于Gmapping算法的地图，尤其是在粒子数很少的时候，可以实现与粒子数较多时接近的地图精度，具有很高的稳定性和适应性。
综上所述，从上述实验结果可知，本发明的有益效果在于：
（1）使用相同分辨率与最小匹配得分时，Gmapping算法构建地图的精度受到粒子数很大的影响，而本发明提出的基于SPA优化的改进Gmapping算法受粒子数影响较少，具有很高的鲁棒性与适应性。由于使用较多的粒子数会占用过多的计算资源，对硬件要求较高。随着粒子数的减少，Gmapping-SLAM的建图精度会随之下降，在大型复杂室内场景中，使用较少的粒子难以满足建图要求，精度较差，而本发明提出的算法可以满足使用较少粒子构建地图的要求，构建精度优于Gmapping-SLAM算法的地图，具有很高的实用价值。
（2）使用相同粒子数进行比较时，本发明提出的基于SPA优化的改进Gmapping算法建图精度明显优于Gmapping-SLAM算法。由以上实验可知，Gmapping-SLAM在地图构建过程中会出现局部边界模糊与边界畸变的问题，在使用较少粒子建图时会出现边界偏移的问题。以上问题的出现会影响构建地图的精度及使用地图进行导航的精度。而本发明所提出的基于SPA的改进Gmapping算法的地图精度明显优于Gmapping算法，得到的障碍物及边界更加清晰完整，且具有很高的稳定性。
以上所述的具体实施例，对本发明的目的，技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种基于稀疏位姿调整的移动机器人的Gmapping建图方法，其特征在于，将图优化过程作为与前端交错运行的单独线程实现对地图构建过程中的优化，包括：
S1：初始化粒子位姿与分布，通过上一时刻第i个粒子的机器人位姿
Figure 283690DEST_PATH_IMAGE001
与里程计信息
Figure 459457DEST_PATH_IMAGE002
估计该时刻第i个粒子的机器人估计位姿
Figure 384687DEST_PATH_IMAGE003
，计算提议分布p；
S2：基于地图信息
Figure 608995DEST_PATH_IMAGE004
、机器人估计位姿
Figure 732809DEST_PATH_IMAGE005
、观测量
Figure 17160DEST_PATH_IMAGE006
，匹配扫描第i个粒子的机器人估计位姿
Figure 492004DEST_PATH_IMAGE007
的周边区域；i，j为编号，t为时刻；
若扫描匹配成功则进入S3，计算机器人位姿的极大似然估计值
Figure 520003DEST_PATH_IMAGE008
；并判断是否启动线性优化流程，若启动线性优化流程，则执行S3及S3’；否则，仅执行S3；
若扫描匹配失败，则跳过S3、S4，计算机器人位姿
Figure 763902DEST_PATH_IMAGE009
，更新第i个粒子的权重
Figure 953575DEST_PATH_IMAGE010
；
所述计算机器人位姿
Figure 853398DEST_PATH_IMAGE011
的计算公式为：
Figure 502333DEST_PATH_IMAGE012
；
所述更新第i个粒子的权重
Figure 804002DEST_PATH_IMAGE013
的计算公式为：
Figure 226893DEST_PATH_IMAGE014
；
S3：计算采样位置
Figure 614012DEST_PATH_IMAGE015
的目标分布
Figure 46130DEST_PATH_IMAGE016
，计算归一化因子
Figure 936726DEST_PATH_IMAGE017
；
所述归一化因子
Figure 733780DEST_PATH_IMAGE018
的计算公式为：
Figure 404933DEST_PATH_IMAGE019
；
S4：计算高斯近似
Figure 578425DEST_PATH_IMAGE020
，计算第i个粒子的更新位姿
Figure 651424DEST_PATH_IMAGE021
；
Figure 619380DEST_PATH_IMAGE022
为t时刻各粒子的平均值；
所述高斯近似
Figure 777829DEST_PATH_IMAGE023
的计算公式为：
Figure 489433DEST_PATH_IMAGE024
，
Figure 354620DEST_PATH_IMAGE025
；
所述第i个粒子的更新位姿
Figure 555795DEST_PATH_IMAGE026
的计算公式为：
Figure 139223DEST_PATH_IMAGE027
；
S5：更新第i个粒子的权重，所述第i个粒子的权重
Figure 451255DEST_PATH_IMAGE028
的更新公式为：
Figure 436529DEST_PATH_IMAGE029
；
S6：更新机器人位姿与粒子地图，所述更新机器人位姿的方法为：通过所述第i个粒子的更新位姿
Figure 811534DEST_PATH_IMAGE030
与位姿修正量
Figure 882258DEST_PATH_IMAGE031
更新机器人位姿
Figure 935665DEST_PATH_IMAGE032
，所述更新粒子地图的方法为：通过直接稀疏特征矩阵H、第i个粒子的更新位姿
Figure 837762DEST_PATH_IMAGE030
、观测量
Figure 318422DEST_PATH_IMAGE033
，及更新第i个粒子的地图及采样粒子集更新粒子地图及粒子信息；
S7：判断地图创建是否完成，若地图创建完成，则结束流程；
若地图创建未完成，则判断是否进行粒子重采样；
判断是否进行粒子重采样的方法为：
若有效样本量
Figure 673180DEST_PATH_IMAGE034
小于阈值T，进行粒子重采样，返回S5；
若有效样本量
Figure 530277DEST_PATH_IMAGE035
大于等于阈值T，则不进行粒子重采样，返回S2；
S3’：构建位姿图；所述位姿图构建包括计算机器人极大似然估计值
Figure 286881DEST_PATH_IMAGE036
，然后进入S4’；
S4’：通过闭环约束计算机器人位姿修正量
Figure 759DEST_PATH_IMAGE037
；
所述
Figure 46075DEST_PATH_IMAGE038
的计算公式为：
Figure 441284DEST_PATH_IMAGE039
，
Figure 52394DEST_PATH_IMAGE040
，
其中，
Figure 140436DEST_PATH_IMAGE041
为精度矩阵，λ为梯度下降法到牛顿-欧拉法的转换因子，J为误差雅克比矩阵。
2.根据权利要求1所述的基于稀疏位姿调整的移动机器人的Gmapping建图方法，其特征在于，所述提议分布的计算公式为：
Figure 469786DEST_PATH_IMAGE042
；
所述第i个粒子的机器人估计位姿
Figure 668686DEST_PATH_IMAGE043
的计算公式为：
Figure 134303DEST_PATH_IMAGE044
，
其中，
Figure 393246DEST_PATH_IMAGE045
表示标准位姿合成运算符。
3.根据权利要求1所述的基于稀疏位姿调整的移动机器人的Gmapping建图方法，其特征在于，所述极大似然估计值
Figure 147575DEST_PATH_IMAGE046
的计算公式为：
Figure 943974DEST_PATH_IMAGE047
。
4.根据权利要求1所述的基于稀疏位姿调整的移动机器人的Gmapping建图方法，其特征在于，所述有效样本量
Figure 467359DEST_PATH_IMAGE048
的计算公式为：
Figure 959521DEST_PATH_IMAGE049
，其中，
Figure 935567DEST_PATH_IMAGE050
为粒子归一化权重。
CN202010515565.9A 2020-06-09 2020-06-09 一种基于稀疏位姿调整的移动机器人的Gmapping建图方法 Active CN111427370B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010515565.9A CN111427370B (zh) 2020-06-09 2020-06-09 一种基于稀疏位姿调整的移动机器人的Gmapping建图方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010515565.9A CN111427370B (zh) 2020-06-09 2020-06-09 一种基于稀疏位姿调整的移动机器人的Gmapping建图方法

Publications (2)

Publication Number Publication Date
CN111427370A CN111427370A (zh) 2020-07-17
CN111427370B true CN111427370B (zh) 2020-09-04

Family

ID=71559080

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010515565.9A Active CN111427370B (zh) 2020-06-09 2020-06-09 一种基于稀疏位姿调整的移动机器人的Gmapping建图方法

Country Status (1)

Country Link
CN (1) CN111427370B (zh)

Families Citing this family (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111929699B (zh) * 2020-07-21 2023-05-09 北京建筑大学 一种顾及动态障碍物的激光雷达惯导里程计与建图方法及系统
CN111856499B (zh) * 2020-07-30 2021-06-18 浙江华睿科技有限公司 基于激光雷达的地图构建方法和装置
CN112070201A (zh) * 2020-08-26 2020-12-11 成都睿芯行科技有限公司 一种基于Gmapping提升建图速度的方法
CN114253511A (zh) * 2020-09-21 2022-03-29 成都睿芯行科技有限公司 基于激光雷达的slam硬件加速器及其实现方法
CN112241002B (zh) * 2020-10-11 2022-10-18 西北工业大学 一种基于Karto SLAM的鲁棒闭环检测新方法
CN112268561A (zh) * 2020-10-12 2021-01-26 西北工业大学 一种融合磁场信息的机器人蒙特卡罗定位方法
CN112857379B (zh) * 2021-01-22 2023-12-12 南京邮电大学 一种基于改进的Gmapping-SLAM地图更新方法及系统
CN113218408A (zh) * 2021-03-26 2021-08-06 合肥中科智驰科技有限公司 适用于多地形的多传感器融合的2Dslam方法及其系统
CN113375658B (zh) * 2021-06-15 2023-05-09 电子科技大学中山学院 移动机器人故障下同时fdd和slam的方法及系统
CN113703443B (zh) * 2021-08-12 2023-10-13 北京科技大学 一种不依赖gnss的无人车自主定位与环境探索方法
CN114577199A (zh) * 2022-02-17 2022-06-03 广州大学 一种基于Gmapping算法的垃圾分类机器人二维栅格地图构建系统
CN114993285B (zh) * 2022-04-27 2024-04-05 大连理工大学 一种基于四轮全向全驱移动机器人的二维激光雷达建图方法
CN116309907A (zh) * 2023-03-06 2023-06-23 中国人民解放军海军工程大学 一种基于改进的Gmapping算法的移动机器人建图方法
CN116203586B (zh) * 2023-05-06 2023-07-28 南京农业大学 一种基于改进Gmapping的果园二维环境地图精准构建方法及系统

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100809352B1 (ko) * 2006-11-16 2008-03-05 삼성전자주식회사 파티클 필터 기반의 이동 로봇의 자세 추정 방법 및 장치
CN105509755B (zh) * 2015-11-27 2018-10-12 重庆邮电大学 一种基于高斯分布的移动机器人同步定位与地图构建方法
CN106842230A (zh) * 2017-01-13 2017-06-13 深圳前海勇艺达机器人有限公司 移动机器人导航方法与系统
CN107063264A (zh) * 2017-04-13 2017-08-18 杭州申昊科技股份有限公司 一种适用于大规模变电站环境的机器人地图创建方法

Also Published As

Publication number Publication date
CN111427370A (zh) 2020-07-17

Similar Documents

Publication Publication Date Title
CN111427370B (zh) 一种基于稀疏位姿调整的移动机器人的Gmapping建图方法
KR102292277B1 (ko) 자율 주행 차량에서 3d cnn 네트워크를 사용하여 솔루션을 추론하는 lidar 위치 추정
CN110675307B (zh) 基于vslam的3d稀疏点云到2d栅格图的实现方法
CN107239076B (zh) 基于虚拟扫描与测距匹配的agv激光slam方法
US11594011B2 (en) Deep learning-based feature extraction for LiDAR localization of autonomous driving vehicles
US11364931B2 (en) Lidar localization using RNN and LSTM for temporal smoothness in autonomous driving vehicles
Saulnier et al. Information theoretic active exploration in signed distance fields
WO2024001629A1 (zh) 一种面向智能驾驶车辆的多传感器融合方法及系统
CN114742874A (zh) 一种基于激光与视觉异步融合的slam算法
CN115540850A (zh) 一种激光雷达与加速度传感器结合的无人车建图方法
Xu et al. Dynamic vehicle pose estimation and tracking based on motion feedback for LiDARs
CN117222915A (zh) 用于使用复合测量模型跟踪移动对象的扩张状态的系统和方法
Hu et al. A small and lightweight autonomous laser mapping system without GPS
Deng et al. Improved closed-loop detection and Octomap algorithm based on RGB-D SLAM
Cai et al. Design of Multisensor Mobile Robot Vision Based on the RBPF-SLAM Algorithm
CN115655311A (zh) 一种基于扫描匹配的阿克曼型机器人里程计标定方法
Wang et al. Research on SLAM road sign observation based on particle filter
Dai et al. Grey Wolf Resampling-Based Rao-Blackwellized Particle Filter for Mobile Robot Simultaneous Localization and Mapping
Zhang et al. An Improved Localization Algorithm for Intelligent Robot
Sun et al. Indoor Li-DAR 3D mapping algorithm with semantic-based registration and optimization
Xing et al. Simultaneous localization and mapping algorithm based on the asynchronous fusion of laser and vision sensors
Han et al. Novel cartographer using an OAK-D smart camera for indoor robots location and navigation
CN113379915B (zh) 一种基于点云融合的行车场景构建方法
Sun et al. VMC-LIO: Incorporating Vehicle Motion Characteristics in LiDAR Inertial Odometry
CN115908737A (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