CN113189613B - 一种基于粒子滤波的机器人定位方法 - Google Patents

一种基于粒子滤波的机器人定位方法 Download PDF

Info

Publication number
CN113189613B
CN113189613B CN202110096774.9A CN202110096774A CN113189613B CN 113189613 B CN113189613 B CN 113189613B CN 202110096774 A CN202110096774 A CN 202110096774A CN 113189613 B CN113189613 B CN 113189613B
Authority
CN
China
Prior art keywords
map
robot
point
pose
equations
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
CN202110096774.9A
Other languages
English (en)
Other versions
CN113189613A (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.)
Guangdong University of Technology
Original Assignee
Guangdong University 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 Guangdong University of Technology filed Critical Guangdong University of Technology
Priority to CN202110096774.9A priority Critical patent/CN113189613B/zh
Publication of CN113189613A publication Critical patent/CN113189613A/zh
Application granted granted Critical
Publication of CN113189613B publication Critical patent/CN113189613B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • 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/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/22—Matching criteria, e.g. proximity measures
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods

Abstract

本发明公开了一种基于粒子滤波的机器人定位方法，包括如下步骤：1)获取视觉地图坐标；2)地图坐标转换；3)轮速计与陀螺仪运动估计；4)卡尔曼滤波。本发明属于机器人技术领域，具体是指一种结合视觉与陀螺仪，利用数据关联与多传感器融合技术，提升定位效果，为机器人配送，运输等场景提供更理想的技术方案的基于粒子滤波的机器人定位方法。

Description

一种基于粒子滤波的机器人定位方法
技术领域
本发明属于机器人技术领域，具体是指一种基于粒子滤波的机器人定位方法。
背景技术
基于SLAM导航的自主移动机器人导航系统相较于传统的磁导航，二维码导航，反射板导航等方式，拥有着无需改造导航环境，可自主规划路径等优势，已经广泛应用于多种室内移动机器人中。当前已经从地图构建，定位，感知，路径规划与运动控制五大基础模块开展了大量的工作，来赋能移动机器人自主化，智能化。
众所周知，定位是机器人导航的最核心模块，其他模块需基于定位准确的条件来保证机器人稳定工作，而SLAM导航是利用外部传感器信息与先验地图数据关联和内部传感器信息自身估计，来实现机器人精确定位。因此为了提高自主移动机器人SLAM导航定位性能，开展地图与定位相关联的工作是十分有必要的。现有的一种基于ORB特征点的SLAM系统，利用相机采集图像来构建视觉特征点地图，从而实现重定位与连续位姿估计，但是该算法生成的地图不保存障碍信息，不能应用于机器人路径规划与避障当中；而基于图优化的激光SLAM系统，构建出便于机器人路径规划的激光栅格地图，利用激光等传感器的数据关联实现定位，但是由于单线激光信息并不那么丰富，从而导致重定位效果不佳；一种基于ROS的包含建图、定位、路径规划的机器人导航框架，是为数不多打通多模块的开源工作，针对机器人落地提出了许多实用的设计。
基于前人的研究基础上，提出了一种结合视觉与陀螺仪的室内移动机器人改进粒子滤波定位方法，该定位方法是在ROS中导航框架的定位模块AMCL的基础上，为改进其初始化定位效果不稳定与过程定位效果欠佳的两方面，结合视觉与陀螺仪，利用数据关联与多传感器融合技术，提升定位效果，为机器人配送，运输等场景提供更理想的技术方案。
发明内容
为了解决上述难题，本发明提供了一种结合视觉与陀螺仪，利用数据关联与多传感器融合技术，提升定位效果，为机器人配送，运输等场景提供更理想的技术方案的基于粒子滤波的机器人定位方法。
为实现上述目的，本发明采取的技术方案如下：一种基于粒子滤波的机器人定位方法，包括如下步骤：
1)获取视觉地图坐标：获取视觉地图坐标主要由词袋模型与PNP(Pespective-N-Point)两个部分组成，通过相机最新获取的当前图像帧，根据词袋模型与地图中的关键帧进行图像匹配，得到当前帧特征点与三维地图点的对应关系，再利用PNP恢复出当前相机相对于视觉地图的变换关系，从而获取得到视觉地图坐标；图像匹配中利用词袋模型分为两个步骤：第一是产生一个词袋，通过对大量的图片提取特征点，并对这些特征点进行k叉树聚类，得到的叶子节点则为每一个词；第二是使用词袋模型，图片可以通过词袋模型来进行编码，得到词向量，作为进行图片相似度与快速匹配的依据，词向量是关于词索引与其权重的向量，可以通过式(1)来判断两图像词向量v1与v2的相似度，得分越大则越相似；
Figure BDA0002914611360000021
当获取当前帧特征点与地图点的匹配关系，则可以根据2D-3D点的对应关系使用PNP算法来得到当前帧相机的位姿，先假设当前相机位姿李代数ξ与最相似图片的位姿一样，又已知相机内参K，地图点尺度si，地图点三维坐标Pi，对应的二维点像素坐标ui，通过把地图点投影到当前帧上，利用彼此重投影误差的最小二乘形式来构建代价函数，如式(2)，通过优化得到最优相机位姿；
Figure BDA0002914611360000031
2)地图坐标转换：从ORBSLAM2与Cartographer得到的两个地图，即视觉的特征点地图与激光的灰度栅格地图，且两个地图为独立构建，后续需要采样拟合求取彼此变换关系，才可以使获取得到的视觉地图坐标变换至机器人常用的灰度栅格地图中使用，变换关系见式(3)；
GP＝GTF FP (3)
其中GP为灰度栅格地图中的齐次坐标，FP为特征点地图上的齐次坐标，GTF为从特征点地图到灰度栅格地图的变换矩阵；
GTF是通过让机器人运动至均匀散布在活动范围的多个位置，根据第i位置从两SLAM系统得到的GPi与FPi是指代同一物理位置，两者之间差了个GTF变换，利用两组三维点对应关系计算式(4)，(5)，(6)，(7)，(8)，从而得到GTF；
Figure BDA0002914611360000032
Figure BDA0002914611360000033
Figure BDA0002914611360000034
Figure BDA0002914611360000035
Figure BDA0002914611360000041
3)轮速计与陀螺仪运动估计：利用左右轮独立驱动实现前行与旋转的运动机构，在采样时间间隔Δt中，从两轮编码器中经比率换算得到的左右轮走过的距离dl与dr，已知L为左右轮距，(xk,yk,θk)为k时刻下机器人的运动状态，轮速计模型根据式(9)，(10)，(11)求出(xk+1,yk+1,θk+1)的机器人状态；
Figure BDA0002914611360000042
Figure BDA0002914611360000043
Figure BDA0002914611360000044
陀螺仪获取得到某时刻下的机器人角速度，已知k时刻下的机器人姿态四元数为
Figure BDA0002914611360000045
陀螺仪测量得到的角速度为ωk，k+1时刻下陀螺仪测量到的角速度为ωk+1，根据式(12)，(13)可以求出k+1时刻下的机器人姿态四元数
Figure BDA0002914611360000046
Figure BDA0002914611360000047
Figure BDA0002914611360000048
4)卡尔曼滤波：卡尔曼滤波由下面五个方程组成，其中式(14)，(15)为状态转移方程，式(16)，(17)，(18)为状态更新方程，根据k时刻下后验的机器人位姿
Figure BDA0002914611360000049
后验的协方差
Figure BDA00029146113600000410
运动方程的协方差Q，控制量uk，测量方程的协方差R，k+1时刻的测量数据zk+1，得到k+1时刻下后验的机器人位姿
Figure BDA0002914611360000051
后验的协方差
Figure BDA0002914611360000052
Figure BDA0002914611360000053
Figure BDA0002914611360000054
Figure BDA0002914611360000055
Figure BDA0002914611360000056
Figure BDA0002914611360000057
本发明采取上述结构取得有益效果如下：本发明一种基于粒子滤波的机器人定位方法，ROS导航框架的定位模块AMCL，是一种自适应的粒子滤波方法，其一般使用轮速计与激光雷达两个传感器数据以及先验地图信息进行机器人的位姿估计，对于其在初始定位与过程定位方面的问题，改进方法提出对应的改进措施，一方面是通过获取视觉地图坐标，并地图坐标转换，从而实现初始化定位，另一方面是通过对视觉里程计、陀螺仪、轮速计分别得到的位姿估计进行卡尔曼滤波融合，从而提高过程定位质量。
附图说明
图1为一种基于粒子滤波的机器人定位方法的改进图；
图2为一种基于粒子滤波的机器人定位方法的初始化定位结果比对图；
图3为一种基于粒子滤波的机器人定位方法的过程定位质量图；
图4为一种基于粒子滤波的机器人定位方法的到点定位精度对比图。
具体实施方式
下面将对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
发明一种基于粒子滤波的机器人定位方法，包括以下步骤：
获取视觉地图坐标主要由词袋模型与PNP(Pespective-N-Point)两个部分组成，通过相机最新获取的当前图像帧，根据词袋模型与地图中的关键帧进行图像匹配，得到当前帧特征点与三维地图点的对应关系，再利用PNP恢复出当前相机相对于视觉地图的变换关系，从而获取得到视觉地图坐标。
图像匹配中利用词袋模型分为两个步骤：第一是产生一个词袋，通过对大量的图片提取特征点，并对这些特征点进行k叉树聚类，得到的叶子节点则为每一个词；第二是使用词袋模型，图片可以通过词袋模型来进行编码，得到词向量，作为进行图片相似度与快速匹配的依据，词向量是关于词索引与其权重的向量，可以通过式(1)来判断两图像词向量v1与v2的相似度，得分越大则越相似。
Figure BDA0002914611360000061
当获取当前帧特征点与地图点的匹配关系，则可以根据2D-3D点的对应关系使用PNP算法来得到当前帧相机的位姿，先假设当前相机位姿李代数ξ与最相似图片的位姿一样，又已知相机内参K，地图点尺度si，地图点三维坐标Pi，对应的二维点像素坐标ui，通过把地图点投影到当前帧上，利用彼此重投影误差的最小二乘形式来构建代价函数，如式(2)，通过优化得到最优相机位姿。
Figure BDA0002914611360000062
地图坐标转换，见图1，改进方法采用了从ORBSLAM2与Cartographer得到的两个地图，即视觉的特征点地图与激光的灰度栅格地图，且两个地图为独立构建，后续需要采样拟合求取彼此变换关系，才可以使获取得到的视觉地图坐标变换至机器人常用的灰度栅格地图中使用，变换关系见式(3)；
GP＝GTF FP (3)
其中GP为灰度栅格地图中的齐次坐标，FP为特征点地图上的齐次坐标，GTF为从特征点地图到灰度栅格地图的变换矩阵。
本文采样拟合GTF是通过让机器人运动至均匀散布在活动范围的多个位置，根据第i位置从两SLAM系统得到的GPi与FPi是指代同一物理位置，两者之间差了个GTF变换，利用两组三维点对应关系计算式(4)，(5)，(6)，(7)，(8)，从而得到GTF。
Figure BDA0002914611360000071
Figure BDA0002914611360000072
Figure BDA0002914611360000073
Figure BDA0002914611360000074
Figure BDA0002914611360000075
轮速计与陀螺仪运动估计：视觉里程计、陀螺仪、轮速计，皆可估计机器人的运动状态，得到其位姿变换。视觉里程计得到位姿的方法原理与上述一致。轮速计与机器人运动机构相关，本文采用的是差速轮，即仅利用左右轮独立驱动实现前行与旋转的运动机构。在采样时间间隔Δt中，从两轮编码器中经比率换算得到的左右轮走过的距离dl与dr，已知L为左右轮距，(xk,yk,θk)为k时刻下机器人的运动状态，轮速计模型根据式(9)，(10)，(11)求出(xk+1,yk+1,θk+1)的机器人状态。
Figure BDA0002914611360000076
Figure BDA0002914611360000077
Figure BDA0002914611360000078
陀螺仪可以获取得到某时刻下的机器人角速度。本文通过IMU(惯性测量单元)中的陀螺仪获取角速度，从而积分得到机器人姿态变化。已知k时刻下的机器人姿态四元数为
Figure BDA0002914611360000081
陀螺仪测量得到的角速度为ωk，k+1时刻下陀螺仪测量到的角速度为ωk+1，根据式(12)，(13)可以求出k+1时刻下的机器人姿态四元数
Figure BDA0002914611360000082
Figure BDA0002914611360000083
Figure BDA0002914611360000084
卡尔曼滤波：对于上述各种的机器人的运动状态预测，都有各自的位姿估计，不过根据各个传感器的特性与数据获取方式，皆有对应的优劣，针对这些优劣，采用卡尔曼滤波数据融合的方法根据各数据的协方差对应加权，得到优势互补的最优位姿估计。
卡尔曼滤波由下面五个方程组成，其中式(14)，(15)为状态转移方程，式(16)，(17)，(18)为状态更新方程。根据k时刻下后验的机器人位姿
Figure BDA0002914611360000085
后验的协方差
Figure BDA0002914611360000086
运动方程的协方差Q，控制量uk，测量方程的协方差R，k+1时刻的测量数据zk+1，得到k+1时刻下后验的机器人位姿
Figure BDA0002914611360000087
后验的协方差
Figure BDA0002914611360000088
Figure BDA0002914611360000089
Figure BDA00029146113600000810
Figure BDA00029146113600000811
Figure BDA00029146113600000812
Figure BDA00029146113600000813
本文根据上述公式，通过协方差来加权得到较好的估计位姿。在位置x，y估计方面，视觉里程计与轮速计估计误差设置一样，即结果各取一半。在姿态θ方面，把陀螺仪估计误差设置很小，轮速计与视觉里程计估计误差设置较大，即结果主要取自陀螺仪。
具体实施方式
实验结果与分析
实验准备：本文实验使用的平台，由一个差速轮自制底盘，一个SCIK的tim-561型号单线激光雷达，一个小觅智能的S1040型号双目相机，一个ALUBI的IMLPMS-ME1型号IMU和一台CPU为i7-9750H的戴尔笔记本组成，实验场地为一个回字形的室内办公场地，在此场地进行关于初始化定位，过程定位，到点定位的三个实验。初始化实验则在该场地中机器人运动范围，对比测试两种方法的初始化定位平均成功率与平均定位时间。过程定位实验则是让机器人运动，进行两种方法的定位质量比较。到点定位实验则是从系统最终到点定位精度来整体衡量改进效果。
初始化定位：该实验让机器人在活动范围均匀分布的位置进行初始化定位，对于改进方法则是让其当前帧与视觉特征点地图进行数据关联获取全局位置，对于原方法则是让AMCL中的粒子全局分布，经过旋转与激光采样对粒子收敛，获取全局位置。经过各40组实验测试，结果见图2，新系统的成功得到准确的初始化定位次数为38次，失败2次，成功率为95％，平均定位时间为2.67s；旧系统成功得到准确的初始化定位次数为17次，失败23次，成功率为42.5％，平均定位时间为16.68s。改进方法失败主要由于早晚靠窗处的光照强度差异过大，导致视觉特征点地图中的特征与当前帧的特征关联不上从而导致失败.
过程定位：图3为按照既定轨迹改进方法与原方法的定位质量对比图。该定位质量为计算当前激光雷达扫描信息投影至激光灰度栅格地图上，激光点投影至占据栅格的数量占总激光点数的百分比。由数据可见添加了视觉与陀螺仪对系统的定位效果有了明显的提高，而在40-70s的区间内新系统的定位质量出现下降趋势，是因为该路段存在较多椅子与部分可移动的障碍物，在地图构建到测试的时间间隔中被挪动，从而出现定位质量效果下降，但仍然比原方法的效果要好。
到点定位：为了获取机器人系统整体的定位效果，以机器人重复到点定位精度作为指标对比改进前后的精度提升情况。实验过程先让机器人记录下某一指定地点，根据机器人底盘的铝合金板后侧与右侧作为标准，把贴纸贴在地面，以此作为标记，记作位置0，后续让机器人规划离开，再重新到该点，以同样方法作标记，两个系统各重复3组，一组10次，测量其x,y,θ上的误差，并统计其平均误差。实验数据与改进方法实验效果见图4。
本文针对室内移动机器人配送、运输等场景，从机器人落地的实用性出发，基于机器人操作系统中成熟的开源导航框架定位模块进行改进，在其粒子滤波定位的基础上，加入视觉与陀螺仪的冗余数据，利用数据关联与多传感器融合的技术，尝试改进初始化定位与过程定位的不足。改进方法相较于原方法得到稳定方面，精度方面的提升，为自主移动机器人定位提供一种较好的解决方法。
以上对本发明及其实施方式进行了描述，这种描述没有限制性，实际的结构并不局限于此。总而言之如果本领域的普通技术人员受其启示，在不脱离本发明创造宗旨的情况下，不经创造性的设计出与该技术方案相似的结构方式及实施例，均应属于本发明的保护范围。

Claims (1)

1.一种基于粒子滤波的机器人定位方法，其特征在于，包括如下步骤：
1)获取视觉地图坐标：获取视觉地图坐标由词袋模型与PNP(Pespective-N-Point)两个部分组成，通过相机最新获取的当前图像帧，根据词袋模型与地图中的关键帧进行图像匹配，得到当前帧特征点与三维地图点的对应关系，再利用PNP恢复出当前相机相对于视觉地图的变换关系，从而获取得到视觉地图坐标；图像匹配中利用词袋模型分为两个步骤：第一是产生一个词袋，通过对大量的图片提取特征点，并对这些特征点进行k叉树聚类，得到的叶子节点则为每一个词；第二是使用词袋模型，图片通过词袋模型来进行编码，得到词向量，作为进行图片相似度与快速匹配的依据，词向量是关于词索引与其权重的向量，通过式(1)来判断两图像词向量v1与v2的相似度，得分越大则越相似；
Figure FDA0003903733450000011
当获取当前帧特征点与地图点的匹配关系，根据2D-3D点的对应关系使用PNP算法来得到当前帧相机的位姿，先假设当前相机位姿李代数ξ与最相似图片的位姿一样，又已知相机内参K，地图点尺度si，地图点三维坐标Pi，对应的二维点像素坐标ui，通过把地图点投影到当前帧上，利用彼此重投影误差的最小二乘形式来构建代价函数，如式(2)，通过优化得到最优相机位姿；
Figure FDA0003903733450000012
2)地图坐标转换：从ORBSLAM2与Cartographer得到的两个地图，即视觉的特征点地图与激光的灰度栅格地图，且两个地图为独立构建，后续需要采样拟合求取彼此变换关系，才使获取得到的视觉地图坐标变换至机器人常用的灰度栅格地图中使用，变换关系见式(3)；
GP＝GTF FP (3)
其中GP为灰度栅格地图中的齐次坐标，FP为特征点地图上的齐次坐标，GTF为从特征点地图到灰度栅格地图的变换矩阵；
GTF是通过让机器人运动至均匀散布在活动范围的多个位置，根据第i位置从两SLAM系统得到的GPi与FPi是指代同一物理位置，两者之间差了个GTF变换，利用两组三维点对应关系计算式(4)，(5)，(6)，(7)，(8)，从而得到GTF；
Figure FDA0003903733450000021
Figure FDA0003903733450000022
Figure FDA0003903733450000023
Figure FDA0003903733450000024
Figure FDA0003903733450000025
3)轮速计与陀螺仪运动估计：利用左右轮独立驱动实现前行与旋转的运动机构，在采样时间间隔Δt中，从两轮编码器中经比率换算得到的左右轮走过的距离dl与dr，已知L为左右轮距，(xk,yk,θk)为k时刻下机器人的运动状态，轮速计模型根据式(9)，(10)，(11)求出(xk+1,yk+1,θk+1)的机器人状态；
Figure FDA0003903733450000026
Figure FDA0003903733450000031
Figure FDA0003903733450000032
陀螺仪获取得到某时刻下的机器人角速度，已知k时刻下的机器人姿态四元数为
Figure FDA0003903733450000033
陀螺仪测量得到的角速度为ωk，k+1时刻下陀螺仪测量到的角速度为ωk+1，根据式(12)，(13)求出k+1时刻下的机器人姿态四元数
Figure FDA0003903733450000034
Figure FDA0003903733450000035
Figure FDA0003903733450000036
4)卡尔曼滤波：卡尔曼滤波由下面五个方程组成，其中式(14)，(15)为状态转移方程，式(16)，(17)，(18)为状态更新方程，根据k时刻下后验的机器人位姿
Figure FDA0003903733450000037
后验的协方差
Figure FDA0003903733450000038
运动方程的协方差Q，控制量uk，测量方程的协方差R，k+1时刻的测量数据zk+1，得到k+1时刻下后验的机器人位姿
Figure FDA0003903733450000039
后验的协方差
Figure FDA00039037334500000310
Figure FDA00039037334500000311
Figure FDA00039037334500000312
Figure FDA00039037334500000313
Figure FDA00039037334500000314
Figure FDA00039037334500000315
CN202110096774.9A 2021-01-25 2021-01-25 一种基于粒子滤波的机器人定位方法 Active CN113189613B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110096774.9A CN113189613B (zh) 2021-01-25 2021-01-25 一种基于粒子滤波的机器人定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110096774.9A CN113189613B (zh) 2021-01-25 2021-01-25 一种基于粒子滤波的机器人定位方法

Publications (2)

Publication Number Publication Date
CN113189613A CN113189613A (zh) 2021-07-30
CN113189613B true CN113189613B (zh) 2023-01-10

Family

ID=76972708

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110096774.9A Active CN113189613B (zh) 2021-01-25 2021-01-25 一种基于粒子滤波的机器人定位方法

Country Status (1)

Country Link
CN (1) CN113189613B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113790726B (zh) * 2021-09-07 2024-03-29 中国科学院合肥物质科学研究院 一种融合相机、轮速计和单uwb信息的机器人室内定位方法
CN115164918B (zh) * 2022-09-06 2023-02-03 联友智连科技有限公司 语义点云地图构建方法、装置及电子设备
CN116817903B (zh) * 2023-08-24 2023-11-21 湖南大学 一种基于先验视觉引导的智能机器人全局定位方法及系统

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101576384B (zh) * 2009-06-18 2011-01-05 北京航空航天大学 一种基于视觉信息校正的室内移动机器人实时导航方法
US9037396B2 (en) * 2013-05-23 2015-05-19 Irobot Corporation Simultaneous localization and mapping for a mobile robot
CN106679648B (zh) * 2016-12-08 2019-12-10 东南大学 一种基于遗传算法的视觉惯性组合的slam方法
CN108256574B (zh) * 2018-01-16 2020-08-11 广东省智能制造研究所 机器人定位方法及装置
CN108717710B (zh) * 2018-05-18 2022-04-22 京东方科技集团股份有限公司 室内环境下的定位方法、装置及系统
CN109061703B (zh) * 2018-06-11 2021-12-28 阿波罗智能技术(北京)有限公司 用于定位的方法、装置、设备和计算机可读存储介质
CN108955688B (zh) * 2018-07-12 2021-12-28 苏州大学 双轮差速移动机器人定位方法及系统
CN110039536A (zh) * 2019-03-12 2019-07-23 广东工业大学 室内地图构造和定位的自导航机器人系统及图像匹配方法
CN111076733B (zh) * 2019-12-10 2022-06-14 亿嘉和科技股份有限公司 一种基于视觉与激光slam的机器人室内建图方法及系统

Also Published As

Publication number Publication date
CN113189613A (zh) 2021-07-30

Similar Documents

Publication Publication Date Title
CN113189613B (zh) 一种基于粒子滤波的机器人定位方法
Ramezani et al. The newer college dataset: Handheld lidar, inertial and vision with ground truth
CN107246876B (zh) 一种无人驾驶汽车自主定位与地图构建的方法及系统
Royer et al. Monocular vision for mobile robot localization and autonomous navigation
Alonso et al. Accurate global localization using visual odometry and digital maps on urban environments
CN110243358A (zh) 多源融合的无人车室内外定位方法及系统
Kriegman et al. A mobile robot: Sensing, planning and locomotion
Zhao et al. A vehicle-borne urban 3-D acquisition system using single-row laser range scanners
JP2022019642A (ja) マルチセンサ融合に基づく測位方法及び装置
CN110361027A (zh) 基于单线激光雷达与双目相机数据融合的机器人路径规划方法
Lee et al. Robust mobile robot localization using optical flow sensors and encoders
CN109959377A (zh) 一种机器人导航定位系统及方法
CN109166140A (zh) 一种基于多线激光雷达的车辆运动轨迹估计方法及系统
CN105953796A (zh) 智能手机单目和imu融合的稳定运动跟踪方法和装置
CN111982114B (zh) 一种采用imu数据融合估计三维位姿的救援机器人
Pfaff et al. Towards mapping of cities
Wulf et al. Benchmarking urban six‐degree‐of‐freedom simultaneous localization and mapping
CN111260751B (zh) 基于多传感器移动机器人的建图方法
CN110058594A (zh) 基于示教的多传感器的移动机器人定位导航系统及方法
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN112556719B (zh) 一种基于cnn-ekf的视觉惯性里程计实现方法
CN112833892B (zh) 一种基于轨迹对齐的语义建图方法
CN114001733A (zh) 一种基于地图的一致性高效视觉惯性定位算法
CN115639823A (zh) 崎岖起伏地形下机器人地形感知与移动控制方法及系统
Sujiwo et al. Localization based on multiple visual-metric maps

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