CN115328168A - 基于自适应强跟踪的移动机器人同步定位与建图方法及*** - Google Patents
基于自适应强跟踪的移动机器人同步定位与建图方法及*** Download PDFInfo
- Publication number
- CN115328168A CN115328168A CN202211137399.9A CN202211137399A CN115328168A CN 115328168 A CN115328168 A CN 115328168A CN 202211137399 A CN202211137399 A CN 202211137399A CN 115328168 A CN115328168 A CN 115328168A
- Authority
- CN
- China
- Prior art keywords
- mobile robot
- time
- pose
- estimation
- covariance
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 101
- 230000003044 adaptive effect Effects 0.000 title claims abstract description 34
- 238000013507 mapping Methods 0.000 title claims abstract description 33
- 230000001360 synchronised effect Effects 0.000 title claims abstract description 32
- 238000001914 filtration Methods 0.000 claims abstract description 53
- 230000008569 process Effects 0.000 claims abstract description 49
- 238000010276 construction Methods 0.000 claims abstract description 11
- 239000000654 additive Substances 0.000 claims abstract description 7
- 230000000996 additive effect Effects 0.000 claims abstract description 7
- 238000004422 calculation algorithm Methods 0.000 claims description 49
- 238000005259 measurement Methods 0.000 claims description 38
- 239000011159 matrix material Substances 0.000 claims description 22
- 230000002596 correlated effect Effects 0.000 claims description 14
- 150000001875 compounds Chemical class 0.000 claims description 10
- 238000012546 transfer Methods 0.000 claims description 10
- 238000004364 calculation method Methods 0.000 claims description 9
- 238000012937 correction Methods 0.000 claims description 6
- 230000007613 environmental effect Effects 0.000 claims description 5
- 230000003313 weakening effect Effects 0.000 claims description 5
- 238000009826 distribution Methods 0.000 claims description 4
- 230000008859 change Effects 0.000 abstract description 6
- 238000012544 monitoring process Methods 0.000 abstract description 2
- 230000006870 function Effects 0.000 description 16
- 238000010586 diagram Methods 0.000 description 10
- 238000004590 computer program Methods 0.000 description 7
- 238000002474 experimental method Methods 0.000 description 5
- 238000012545 processing Methods 0.000 description 5
- 230000006872 improvement Effects 0.000 description 4
- 230000035772 mutation Effects 0.000 description 4
- 238000004088 simulation Methods 0.000 description 4
- 238000005457 optimization Methods 0.000 description 3
- 238000003860 storage Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000005562 fading Methods 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 230000002159 abnormal effect Effects 0.000 description 1
- 230000003321 amplification Effects 0.000 description 1
- 230000003416 augmentation Effects 0.000 description 1
- 238000005094 computer simulation Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 229910052739 hydrogen Inorganic materials 0.000 description 1
- 239000001257 hydrogen Substances 0.000 description 1
- 125000004435 hydrogen atom Chemical class [H]* 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000003199 nucleic acid amplification method Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000010355 oscillation Effects 0.000 description 1
- 239000011541 reaction mixture Substances 0.000 description 1
- 238000012827 research and development Methods 0.000 description 1
- 230000026676 system process Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
- G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Optics & Photonics (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于自适应强跟踪的移动机器人同步定位与建图方法及***,所述方法包括以下步骤:假设同步定位与地图构建过程中的噪声均为加性高斯噪声的前提下,根据移动机器人的底盘样式和传感器的特性构建移动机器人的运动学模型和观测模型,初始化获取移动机器人在全局坐标系下的初始位姿和初始估计误差协方差;基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计,实现移动机器人的实时位姿估计和地图构建。本发明提供的方法,能够在移动机器人发生运动状态突变或模型参数失配时,通过监测新息的变化对过程噪声进行自适应估计,使移动机器人保持较强的位姿跟踪能力和良好的路标估计精度。
Description
技术领域
本发明属于移动机器人同步定位与建图及自主导航技术领域,特别涉及一种基于自适应强跟踪的移动机器人同步定位与建图方法及***。
背景技术
近些年来,随着移动机器人领域相关技术的发展,人们越来越重视拥有自主导航能力并能自主完成设定任务的移动机器人的研发;具备自主导航能力的机器人越来越多的应用在森林火灾检测、快递物流搬运、餐饮店送餐等场景,而实现移动机器人路径规划和自主导航的基础是高精度的机器人位姿估计和精确的环境地图。
同步定位与地图构建(Simultaneous Localization and Mapping,SLAM)技术,是指移动机器人在未知环境中同时完成自身定位、路标估计以及构建环境地图的过程。根据后端优化策略的不同,SLAM可分为基于贝叶斯滤波和基于非线性优化两类框架;其中,相较于非线性优化算法,贝叶斯滤波算法简单易行,发展也相对成熟。
现有的扩展卡尔曼滤波(Extended Kalman Filter,EKF)算法通过对非线性***进行泰勒展开并取一阶近似,实现较简单,已经广泛应用于非线性***;然而EKF算法由于舍弃了***的高阶信息,导致机器人位姿估计的精度较低,并且对于模型中不准确的参数没有自适应能力;在EKF基础上提出的强跟踪滤波(Strong Tracking Filter,STF)算法在一定程度上提高了机器人位姿估计精度,但是未能实现对噪声的自适应估计,对机器人运动状态的突变和参数的失配没有良好的自适应能力,并且在实际应用于SLAM问题时容易出现滤波发散现象。综上,亟需一种基于自适应强跟踪的移动机器人同步定位与建图方法。
发明内容
本发明的目的在于提供一种基于自适应强跟踪的移动机器人同步定位与建图方法及***,以解决上述存在的技术问题。本发明提供的方法,能够在移动机器人发生运动状态突变或模型参数失配时,通过监测新息的变化对过程噪声进行自适应估计,使移动机器人保持较强的位姿跟踪能力和良好的路标估计精度,能提高移动机器人SLAM算法的鲁棒性。
为达到上述目的,本发明采用以下技术方案:
本发明提供的一种基于自适应强跟踪的移动机器人同步定位与建图方法,包括以下步骤:
假设同步定位与地图构建过程中的噪声均为加性高斯噪声的前提下,根据移动机器人的底盘样式和传感器的特性构建移动机器人的运动学模型和观测模型;对所述移动机器人的运动学模型和观测模型进行初始化,获取移动机器人在全局坐标系下的初始位姿和初始估计误差协方差;
基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计,实现移动机器人的实时位姿估计和地图构建;
其中,所述基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计过程中,k时刻迭代估计的步骤包括,
基于k-1时刻移动机器人位姿和k-1时刻估计误差协方差,利用扩展卡尔曼滤波方法和移动机器人的运动学模型,获得k时刻移动机器人位姿预测值;
获取k时刻传感器对环境路标的观测信息并进行路标数据关联,获得k时刻与k-1时刻的路标关联结果;基于所述路标关联结果,获得关联后的k时刻传感器观测信息;
判断所述移动机器人同步定位与建图方法的迭代结果是否发散,获得判断结果;基于所述判断结果,利用k-1时刻移动机器人位姿、k-1时刻估计误差协方差、移动机器人的运动学模型和观测模型对k时刻估计误差协方差进行时间更新,获得k时刻估计误差协方差预测值;
基于所述关联后的k时刻传感器观测信息、所述k时刻移动机器人位姿预测值、所述k时刻估计误差协方差预测值,通过量测更新获得k时刻移动机器人的位姿和k时刻估计误差协方差。
本发明的进一步改进在于,构建获得的移动机器人的运动学模型和观测模型表示为,
式中,zk为k时刻传感器的观测信息,uk为k时刻机器人的输入,f(·)为状态转移函数,h(·)为量测函数,wk-1和vk为互不相关的过程噪声和量测噪声,均服从均值为0、协方差分别为Rk和Qk高斯分布;为k时刻移动机器人的状态信息,为移动机器人在二维空间中的坐标位置,为移动机器人的朝向偏离全局坐标系的夹角。
本发明的进一步改进在于,所述判断所述移动机器人同步定位与建图方法的迭代结果是否发散,获得判断结果的过程中:
是否发散的判据表达式为,
满足所述判据表达式时,需要引入强跟踪算法抑制滤波发散。
本发明的进一步改进在于,所述基于所述判断结果,利用k-1时刻移动机器人位姿、k-1时刻估计误差协方差、移动机器人的运动学模型和观测模型对k时刻估计误差协方差进行时间更新,获得k时刻估计误差协方差预测值的过程中,
当滤波已经发散,引入自适应强跟踪算法,计算自适应强跟踪滤波器参数,并对估计误差协方差进行时间更新,包括:
式中,0<ρ≤1为遗忘因子;
获取参数矩阵Mk和Nk,表达式为,
引入自适应强跟踪滤波算法后,估计误差协方差的时间更新公式为,
式中,Pk-1|k-1为k-1时刻的估计误差协方差,λkQk-1项可视为当噪声参数失配时对噪声的修正项。
本发明的进一步改进在于,所述基于所述关联后的k时刻传感器观测信息、所述k时刻移动机器人位姿预测值、所述k时刻估计误差协方差预测值,通过量测更新获得k时刻移动机器人的位姿和k时刻估计误差协方差的过程中,
量测更新的步骤包括:
更新估计误差协方差矩阵Pk|k,更新表达式为Pk|k=(I-KkHk)Pk|k-1;
式中,I为单位矩阵。
本发明提供的一种基于自适应强跟踪的移动机器人同步定位与建图***,包括:
模型获取及初始化模块,用于假设同步定位与地图构建过程中的噪声均为加性高斯噪声的前提下,根据移动机器人的底盘样式和传感器的特性构建移动机器人的运动学模型和观测模型;对所述移动机器人的运动学模型和观测模型进行初始化,获取移动机器人在全局坐标系下的初始位姿和初始估计误差协方差;
迭代估计模块,用于基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计,实现移动机器人的实时位姿估计和地图构建;
其中,所述基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计过程中,k时刻迭代估计的步骤包括,
基于k-1时刻移动机器人位姿和k-1时刻估计误差协方差,利用扩展卡尔曼滤波方法和移动机器人的运动学模型,获得k时刻移动机器人位姿预测值;
获取k时刻传感器对环境路标的观测信息并进行路标数据关联,获得k时刻与k-1时刻的路标关联结果;基于所述路标关联结果,获得关联后的k时刻传感器观测信息;
判断所述移动机器人同步定位与建图方法的迭代结果是否发散,获得判断结果;基于所述判断结果,利用k-1时刻移动机器人位姿、k-1时刻估计误差协方差、移动机器人的运动学模型和观测模型对k时刻估计误差协方差进行时间更新,获得k时刻估计误差协方差预测值;
基于所述关联后的k时刻传感器观测信息、所述k时刻移动机器人位姿预测值、所述k时刻估计误差协方差预测值,通过量测更新获得k时刻移动机器人的位姿和k时刻估计误差协方差。
本发明的进一步改进在于,所述模型获取及初始化模块中,构建获得的移动机器人的运动学模型和观测模型表示为,
式中,zk为k时刻传感器的观测信息,uk为k时刻机器人的输入,f(·)为状态转移函数,h(·)为量测函数,wk-1和vk为互不相关的过程噪声和量测噪声,均服从均值为0、协方差分别为Rk和Qk高斯分布;为k时刻移动机器人的状态信息,为移动机器人在二维空间中的坐标位置,为移动机器人的朝向偏离全局坐标系的夹角。
本发明的进一步改进在于,所述迭代估计模块中,实现所述判断所述移动机器人同步定位与建图方法的迭代结果是否发散,获得判断结果的过程中:
是否发散的判据表达式为,
满足所述判据表达式时,需要引入强跟踪算法抑制滤波发散。
本发明的进一步改进在于,所述迭代估计模块中,实现所述基于所述判断结果,利用k-1时刻移动机器人位姿、k-1时刻估计误差协方差、移动机器人的运动学模型和观测模型对k时刻估计误差协方差进行时间更新,获得k时刻估计误差协方差预测值的过程中,
当滤波已经发散,引入自适应强跟踪算法,计算自适应强跟踪滤波器参数,并对估计误差协方差进行时间更新,包括:
式中,0<ρ≤1为遗忘因子;
获取参数矩阵Mk和Nk,表达式为,
引入自适应强跟踪滤波算法后,估计误差协方差的时间更新公式为,
式中,Pk-1|k-1为k-1时刻的估计误差协方差,λkQk-1项可视为当噪声参数失配时对噪声的修正项。
本发明的进一步改进在于,所述迭代估计模块中,实现所述基于所述关联后的k时刻传感器观测信息、所述k时刻移动机器人位姿预测值、所述k时刻估计误差协方差预测值,通过量测更新获得k时刻移动机器人的位姿和k时刻估计误差协方差的过程中,
量测更新的步骤包括:
更新估计误差协方差矩阵Pk|k,更新表达式为Pk|k=(I-KkHk)Pk|k-1;
式中,I为单位矩阵。
与现有技术相比,本发明具有以下有益效果:
本发明提供的移动机器人同步定位与建图方法,基于自适应强跟踪算法并结合机器人***模型和激光雷达的量测数据,能够在移动机器人运动状态突变、***参数建模不准确及参数失配等情况下对机器人位姿和路标位置进行高精度估计,有助于提高移动机器人在未知环境中的定位和建图精度。相比于EKF、STF等传统SLAM方法,本发明提供的基于自适应强跟踪的移动机器人同步定位与建图方法可以获得准确的机器人位姿和路标估计结果,能够实现未知环境的在线识别,所构建的高精度全局环境地图可用于移动机器人的路径规划和自主导航。本发明在STF算法框架的基础上,通过在时间更新步直接将渐消因子作用于过程噪声协方差矩阵,实现了对***过程噪声的自适应估计和滤波增益的实时修正。本发明基于协方差匹配的思想建立了强跟踪滤波的引入条件,解决了STF算法中因渐消因子计算异常导致的滤波发散现象,提高了机器人在模型不确定和状态突变场景下的位置跟踪能力和路标估计精度。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面对实施例或现有技术描述中所需要使用的附图做简单的介绍;显而易见地,下面描述中的附图是本发明的一些实施例,对于本领域普通技术人员来说,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的一种基于自适应强跟踪的移动机器人同步定位与建图方法的流程示意图;
图2是本发明实施例中,以差分底盘移动机器人为例进行***建模的示意图;
图3是本发明实施例中,进行仿真实验的场景示意图;
图4是本发明实施例中,使用EKF算法进行机器人SLAM的实验结果;
图5是本发明实施例中,使用STF算法进行机器人SLAM的实验结果;
图6是本发明实施例中,使用自适应强跟踪滤波算法进行机器人SLAM的实验结果;
图7是本发明实施例中,机器人使用EKF和自适应强跟踪滤波两种算法所得到的机器人位姿估计结果的均方误差变化曲线示意图。
具体实施方式
为了使本技术领域的人员更好地理解本发明方案,下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分的实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都应当属于本发明保护的范围。
需要说明的是,本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象,而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换,以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外,术语“包括”和“具有”以及他们的任何变形,意图在于覆盖不排他的包含,例如,包含了一系列步骤或单元的过程、方法、***、产品或设备不必限于清楚地列出的那些步骤或单元,而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
下面结合附图对本发明做进一步详细描述:
请参阅图1,本发明实施例提供的一种基于自适应强跟踪的移动机器人同步定位与建图方法,具体包括以下步骤:
步骤S1:对移动机器人进行建模。假设移动机器人SLAM过程中的噪声均为加性噪声,根据移动机器人的底盘样式和传感器的特性构建移动机器人的运动学模型和观测模型。
本发明实施例的步骤S1的具体建模过程如下:
针对某一移动机器人,选取移动机器人工作场景中的某一点作为坐标原点构建全局坐标系,同时选取移动机器人的几何中心作为坐标原点构建局部坐标系;
根据移动机器人不同底盘的运动方式和传感器的测量原理,可以构建抽象化的移动机器人运动学方程和观测方程,表示为:
式中,zk为k时刻的量测信息,uk为k时刻机器人的输入,f(·)为状态转移函数,h(·)为量测函数,wk-1和vk为互不相关的过程噪声和量测噪声,均服从零均值高斯分布,噪声协方差分别为Qk和Rk。
请参阅图2,本发明实施例中,以差分型底盘移动机器人为例对移动机器人进行建模;图2中,以点Og为全局坐标系原点,以移动机器人几何中心Or为局部坐标系原点,建立移动机器人的动力学方程和观测方程如下:
式中,(xr,k,yr,k,θk)为k时刻机器人在全局坐标系下的位姿,εk和ωk分别为机器人在k时刻的线速度和角速度,T为采样周期,wk-1为过程噪声。rk和分别为k时刻第i个路标到机器人几何中心的距离和与机器人行驶方向的夹角,(xi,k,yi,k)为第i个路标在全局坐标系下的坐标,vk为传感器的量测噪声。
请参阅图3,本实施例中,在MATLAB中搭建一个长宽均为20m的仿真场地,以场地中心为圆心,分别以3m、5m、8m为半径,间隔30°均匀设置三圈路标点,如图3中“*”所示。设定机器人所携带激光雷达的最大量测距离为5m。为了模拟机器人在运行过程中模型参数失配和状态突变现象,在仿真过程中均匀设置了三段不同的过程噪声Qk和线速度εk,详细的参数设定请参阅表1。
表1.过程噪声和线速度参数
步骤S4:获取传感器的观测信息并进行路标数据关联。在将路标数据融入到全局地图中前,新观测到的路标与地图中已存在的路标必然存在数据关联。为了保证算法的收敛性,在每一时刻传感器完成对路标信息的采样后,均需要将k时刻的路标信息与k-1时刻的路标信息进行数据关联。对传感器观测到的路标的处理可以分为以下两类,包括:
(1)若该路标已经被观测过。只需要在后续量测更新中对该路标的状态进行更新,获取最新的路标状态。
步骤S5:判断滤波器是否发散。本实施例中,基于协方差匹配思想构建了误差发散判据,仅在***模型参数失配或状态突变引起滤波器发散时才引入强跟踪算法,详细的推导过程为:
当***状态未发生突变或***模型准确时,满足下式:
***状态的突变或模型参数的失配将导致新息增加,不再满足正交性原理,根据新息序列的这一特性,可以构造滤波器发散的判据:
式中,κ≥1为可调系数,可根据需要选择,当κ=1时为最严格的判断条件;tr[·]为矩阵求迹运算符;当滤波器满足上式时,证明预测模型可能出现参数失配,或量测信息受到了严重的干扰,此时需要引入强跟踪算法抑制滤波发散。
步骤S6:对估计误差协方差进行时间更新。根据步骤S5中滤波是否发散的判断结果,可以将误差协方差的更新分为如下两类情况:
(1)滤波尚未发散。此时不需要引入自适应强跟踪算法,仅需要按照传统EKF算法中估计误差协方差的计算公式进行时间更新即可:
(2)滤波已经发散。此时需要引入自适应强跟踪算法,计算自适应强跟踪滤波器参数,并对估计误差协方差进行时间更新。自适应强跟踪算法的关键在于求解时变减消因子λk,通常寻取最优的λk需要复杂的求解过程,不适用于在线计算,故而工程中一般使用近似次优公式,其计算过程如下:
步骤S6.1:首先计算新息序列协方差矩阵Vk:
式(11)中,0<ρ≤1为遗忘因子。
步骤S6.2:计算两个参数矩阵Mk和Nk:
式(13)中,参数β≥1为弱化因子,可增强状态估计的平滑性。
步骤S6.3:λk可通过下式得出:
引入自适应强跟踪滤波算法后,估计误差协方差的时间更新公式变为:
式(15)中,λkQk-1项可视为当噪声参数失配时对噪声的修正项。
步骤S7:量测更新。量测更新这一步主要是根据步骤S4中传感器的量测信息对状态进行校正,获取状态的后验估计xk|k并更新估计误差协方差Pk|k。
本发明实施例中,量测更新环节的详细步骤如下:
步骤S7.1:计算卡尔曼滤波增益Kk:
步骤S7.3:更新估计误差协方差矩阵Pk|k:
Pk|k=(I-KkHk)Pk|k-1 (18)
步骤S8:循环迭代步骤S1至步骤S7,从而实现移动机器人的位姿估计和地图构建过程。
请参阅图4至图7,为本实施例中使用扩展卡尔曼滤波、强跟踪滤波和自适应强跟踪滤波三种算法的模拟移动机器人同步定位与建图的实验结果,观察移动机器人在整个建图过程中估计轨迹与实际运动轨迹的偏离程度以及机器人位置估计均方误差的变化情况。
图4至图6分别为移动机器人使用扩展卡尔曼滤波、强跟踪滤波和自适应强跟踪滤波三种算法的同步定位与建图实验的结果。结合图4至图6这三张图片可以看出,当过程噪声较小且噪声先验与真实值相同时,三种算法的估计轨迹与真实轨迹基本一致;随着噪声的增加和机器人状态的突变,EKF算法在机器人位姿估计中的误差逐渐增大,STF算法的估计结果在经过一段时间的震荡后迅速发散,而自适应强跟踪滤波算法仍能实现较好的估计效果。整个仿真过程中,自适应强跟踪滤波算法的估计轨迹与真实轨迹基本重合。除此之外,对比实验结果还容易看出,自适应强跟踪滤波算法在路标位置的估计上也明显优于另外两种算法。
图7为EKF算法和自适应强跟踪滤波在整个SLAM过程中机器人位姿估计的均方误差变化曲线,可以看出,相比于EKF算法,自适应强跟踪滤波算法的均方误差曲线相对平缓,且均方误差绝对值明显小于EKF算法,这说明自适应强跟踪滤波算法在机器人SLAM的位姿估计中优于EKF算法。
下述为本发明的装置实施例,可以用于执行本发明方法实施例。对于装置实施例中未纰漏的细节,请参照本发明方法实施例。
本发明再一实施例中,提供的一种基于自适应强跟踪的移动机器人同步定位与建图***,包括:
模型获取及初始化模块,用于假设同步定位与地图构建过程中的噪声均为加性高斯噪声的前提下,根据移动机器人的底盘样式和传感器的特性构建移动机器人的运动学模型和观测模型;对所述移动机器人的运动学模型和观测模型进行初始化,获取移动机器人在全局坐标系下的初始位姿和初始估计误差协方差;
迭代估计模块,用于基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计,实现移动机器人的实时位姿估计和地图构建;
其中,所述基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计过程中,k时刻迭代估计的步骤包括,
基于k-1时刻移动机器人位姿和k-1时刻估计误差协方差,利用扩展卡尔曼滤波方法和移动机器人的运动学模型,获得k时刻移动机器人位姿预测值;
获取k时刻传感器对环境路标的观测信息并进行路标数据关联,获得k时刻与k-1时刻的路标关联结果;基于所述路标关联结果,获得关联后的k时刻传感器观测信息;
判断所述移动机器人同步定位与建图方法的迭代结果是否发散,获得判断结果;基于所述判断结果,利用k-1时刻移动机器人位姿、k-1时刻估计误差协方差、移动机器人的运动学模型和观测模型对k时刻估计误差协方差进行时间更新,获得k时刻估计误差协方差预测值;
基于所述关联后的k时刻传感器观测信息、所述k时刻移动机器人位姿预测值、所述k时刻估计误差协方差预测值,通过量测更新获得k时刻移动机器人的位姿和k时刻估计误差协方差。
本领域内的技术人员应明白,本申请的实施例可提供为方法、***、或计算机程序产品。因此,本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且,本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(***)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中,使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品,该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上,使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理,从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
最后应当说明的是:以上实施例仅用以说明本发明的技术方案而非对其限制,尽管参照上述实施例对本发明进行了详细的说明,所属领域的普通技术人员应当理解:依然可以对本发明的具体实施方式进行修改或者等同替换,而未脱离本发明精神和范围的任何修改或者等同替换,其均应涵盖在本发明的权利要求保护范围之内。
Claims (10)
1.一种基于自适应强跟踪的移动机器人同步定位与建图方法,其特征在于,包括以下步骤:
假设同步定位与地图构建过程中的噪声均为加性高斯噪声的前提下,根据移动机器人的底盘样式和传感器的特性构建移动机器人的运动学模型和观测模型;对所述移动机器人的运动学模型和观测模型进行初始化,获取移动机器人在全局坐标系下的初始位姿和初始估计误差协方差;
基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计,实现移动机器人的实时位姿估计和地图构建;
其中,所述基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计过程中,k时刻迭代估计的步骤包括,
基于k-1时刻移动机器人位姿和k-1时刻估计误差协方差,利用扩展卡尔曼滤波方法和移动机器人的运动学模型,获得k时刻移动机器人位姿预测值;
获取k时刻传感器对环境路标的观测信息并进行路标数据关联,获得k时刻与k-1时刻的路标关联结果;基于所述路标关联结果,获得关联后的k时刻传感器观测信息;
判断所述移动机器人同步定位与建图方法的迭代结果是否发散,获得判断结果;基于所述判断结果,利用k-1时刻移动机器人位姿、k-1时刻估计误差协方差、移动机器人的运动学模型和观测模型对k时刻估计误差协方差进行时间更新,获得k时刻估计误差协方差预测值;
基于所述关联后的k时刻传感器观测信息、所述k时刻移动机器人位姿预测值、所述k时刻估计误差协方差预测值,通过量测更新获得k时刻移动机器人的位姿和k时刻估计误差协方差。
4.根据权利要求3所述的一种基于自适应强跟踪的移动机器人同步定位与建图方法,其特征在于,所述基于所述判断结果,利用k-1时刻移动机器人位姿、k-1时刻估计误差协方差、移动机器人的运动学模型和观测模型对k时刻估计误差协方差进行时间更新,获得k时刻估计误差协方差预测值的过程中,
当滤波已经发散,引入自适应强跟踪算法,计算自适应强跟踪滤波器参数,并对估计误差协方差进行时间更新,包括:
式中,0<ρ≤1为遗忘因子;
获取参数矩阵Mk和Nk,表达式为,
引入自适应强跟踪滤波算法后,估计误差协方差的时间更新公式为,
式中,Pk-1|k-1为k-1时刻的估计误差协方差,λkQk-1项可视为当噪声参数失配时对噪声的修正项。
6.一种基于自适应强跟踪的移动机器人同步定位与建图***,其特征在于,包括:
模型获取及初始化模块,用于假设同步定位与地图构建过程中的噪声均为加性高斯噪声的前提下,根据移动机器人的底盘样式和传感器的特性构建移动机器人的运动学模型和观测模型;对所述移动机器人的运动学模型和观测模型进行初始化,获取移动机器人在全局坐标系下的初始位姿和初始估计误差协方差;
迭代估计模块,用于基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计,实现移动机器人的实时位姿估计和地图构建;
其中,所述基于自适应强跟踪扩展卡尔曼滤波方法对移动机器人的位姿进行迭代估计过程中,k时刻迭代估计的步骤包括,
基于k-1时刻移动机器人位姿和k-1时刻估计误差协方差,利用扩展卡尔曼滤波方法和移动机器人的运动学模型,获得k时刻移动机器人位姿预测值;
获取k时刻传感器对环境路标的观测信息并进行路标数据关联,获得k时刻与k-1时刻的路标关联结果;基于所述路标关联结果,获得关联后的k时刻传感器观测信息;
判断所述移动机器人同步定位与建图方法的迭代结果是否发散,获得判断结果;基于所述判断结果,利用k-1时刻移动机器人位姿、k-1时刻估计误差协方差、移动机器人的运动学模型和观测模型对k时刻估计误差协方差进行时间更新,获得k时刻估计误差协方差预测值;
基于所述关联后的k时刻传感器观测信息、所述k时刻移动机器人位姿预测值、所述k时刻估计误差协方差预测值,通过量测更新获得k时刻移动机器人的位姿和k时刻估计误差协方差。
9.根据权利要求8所述的一种基于自适应强跟踪的移动机器人同步定位与建图***,其特征在于,所述迭代估计模块中,实现所述基于所述判断结果,利用k-1时刻移动机器人位姿、k-1时刻估计误差协方差、移动机器人的运动学模型和观测模型对k时刻估计误差协方差进行时间更新,获得k时刻估计误差协方差预测值的过程中,
当滤波已经发散,引入自适应强跟踪算法,计算自适应强跟踪滤波器参数,并对估计误差协方差进行时间更新,包括:
式中,0<ρ≤1为遗忘因子;
获取参数矩阵Mk和Nk,表达式为,
引入自适应强跟踪滤波算法后,估计误差协方差的时间更新公式为,
式中,Pk-1|k-1为k-1时刻的估计误差协方差,λkQk-1项可视为当噪声参数失配时对噪声的修正项。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211137399.9A CN115328168A (zh) | 2022-09-19 | 2022-09-19 | 基于自适应强跟踪的移动机器人同步定位与建图方法及*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211137399.9A CN115328168A (zh) | 2022-09-19 | 2022-09-19 | 基于自适应强跟踪的移动机器人同步定位与建图方法及*** |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115328168A true CN115328168A (zh) | 2022-11-11 |
Family
ID=83913740
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211137399.9A Pending CN115328168A (zh) | 2022-09-19 | 2022-09-19 | 基于自适应强跟踪的移动机器人同步定位与建图方法及*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115328168A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115979240A (zh) * | 2022-12-05 | 2023-04-18 | 哈尔滨理工大学 | 基于限制增广的新息叠加同步定位建图方法 |
-
2022
- 2022-09-19 CN CN202211137399.9A patent/CN115328168A/zh active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115979240A (zh) * | 2022-12-05 | 2023-04-18 | 哈尔滨理工大学 | 基于限制增广的新息叠加同步定位建图方法 |
CN115979240B (zh) * | 2022-12-05 | 2023-09-29 | 哈尔滨理工大学 | 基于限制增广的新息叠加同步定位建图方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109597864B (zh) | 椭球边界卡尔曼滤波的即时定位与地图构建方法及*** | |
Guivant et al. | Simultaneous localization and map building using natural features and absolute information | |
CN111207774A (zh) | 一种用于激光-imu外参标定的方法及*** | |
CN106772524B (zh) | 一种基于秩滤波的农业机器人组合导航信息融合方法 | |
EP3159658A1 (en) | Method and device for real-time target location and map creation | |
CN107084714A (zh) | 一种基于RoboCup3D的多机器人协作目标定位方法 | |
Demim et al. | A new adaptive smooth variable structure filter SLAM algorithm for unmanned vehicle | |
Demim et al. | Cooperative SLAM for multiple UGVs navigation using SVSF filter | |
CN112904358A (zh) | 基于几何信息的激光定位方法 | |
Tang et al. | Robot tracking in SLAM with Masreliez-Martin unscented Kalman filter | |
CN108303095B (zh) | 适用于非高斯***的鲁棒容积目标协同定位方法 | |
CN111291471A (zh) | 一种基于l1正则无迹变换的约束多模型滤波方法 | |
Lin et al. | Graph-based SLAM in indoor environment using corner feature from laser sensor | |
CN115328168A (zh) | 基于自适应强跟踪的移动机器人同步定位与建图方法及*** | |
CN113709662B (zh) | 一种基于超宽带的自主式三维反演定位方法 | |
Burguera et al. | Probabilistic sonar scan matching for robust localization | |
Garcia et al. | Portable multi-hypothesis Monte Carlo localization for mobile robots | |
Font et al. | Mobile robot localization. Revisiting the triangulation methods | |
Tang et al. | An improved H-infinity unscented FastSLAM with adaptive genetic resampling | |
CN115950414A (zh) | 一种不同传感器数据的自适应多重融合slam方法 | |
Fabresse et al. | Active perception for 3D range-only simultaneous localization and mapping with UAVs | |
Xu et al. | Estimating the odometry error of a mobile robot by neural networks | |
Goronzy et al. | Weighted online calibration for odometry of mobile robots | |
Wang et al. | Simultaneous localization and mapping embedded with particle filter algorithm | |
Benedettelli et al. | Multi-robot SLAM using M-Space feature representation |
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 |