CN115979240A - 基于限制增广的新息叠加同步定位建图方法 - Google Patents
基于限制增广的新息叠加同步定位建图方法 Download PDFInfo
- Publication number
- CN115979240A CN115979240A CN202211550548.4A CN202211550548A CN115979240A CN 115979240 A CN115979240 A CN 115979240A CN 202211550548 A CN202211550548 A CN 202211550548A CN 115979240 A CN115979240 A CN 115979240A
- Authority
- CN
- China
- Prior art keywords
- mobile robot
- moment
- robot
- vector
- observation
- 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
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
Description
技术领域
本发明涉及一种基于限制增广的新息叠加同步定位建图方法,属于移动机器人定位建图领域。
背景技术
随着计算机技术的快速发展和开源软件的兴起,机器人技术得以长足发展,尤其是移动机器人已经在各个领域得到应用,如家政服务、室外巡检救援、交通物流、医学机械、教育等,都发挥着十分重要的作用。定位建图一直是移动机器人导航领域的研究热点,也是移动机器人真正意义上实现自主作业的一个重要条件。SLAM算法可以确定移动机器人自身在环境中的位置,同时可以确定其周围的环境情况。SLAM算法可以看成是对移动机器人自身位姿以及周围环境特征的估计算法。
扩展卡尔曼滤波(Extended Kalman Filter,EKF)是目前机器人导航领域中一种较为经典的SLAM方法,其保留了传统卡尔曼滤波(Kalman Filter,KF)可以融合观测数据和估计数据的特性,并且在实际应用中移动机器人运动模型和观测模型为非线性时,它仍可以对移动机器人位姿和周围环境进行估计。但标准扩展卡尔曼滤波存在一些局限性,标准扩展卡尔曼滤波是将观测噪声和过程噪声当作已知量进行计算;但在实际情况中,系统的观测噪声和过程噪声是无法得知的,随着滤波次数的增加,滤波器会出现一种发散现象,即估计值和实际值之间的误差会越来越大。自适应扩展卡尔曼滤波(Adaptive ExtendedKalman Filter,AEKF)在标准扩展卡尔曼滤波的基础上引入渐消因子来实时修正观测噪声和过程噪声,保证算法收敛,但是在加入渐消因子后会导致算法的精度有所降低。新息叠加自适应扩展卡尔曼滤波(Innovation Superposed Adaptive Extended Kalman Filter,IS-AEKF)算法通过将单个时刻新息扩展为多个时刻新息矩阵,有效利用相邻时刻信息,使得IS-AEKF同步定位建图算法精度更高,即算法对移动机器人的状态估计以及对环境中路标点的特征估计更加准确。但移动机器人运动过程中,随着观测到路标点数量的增加,系统状态向量的维数不断增加,会导致算法运算速度降低。
发明内容
针对如何提高移动机器人定位建图过程中精度和速度的问题,本发明提供一种基于限制增广的新息叠加同步定位建图方法。
本发明提供一种基于限制增广的新息叠加同步定位建图方法,包括:
S3、判断观测值中路标点是否在k-1时刻的状态向量中,若是,则利用观测值及k-1时刻的状态向量采用新息叠加自适应扩展卡尔曼滤波同步定位建图方法更新k时刻移动机器人的状态向量,若否,将路标点加入地图,同时加入至k-1时刻的状态向量中,利用观测值Zk及k-1时刻的状态向量采用新息叠加自适应扩展卡尔曼滤波同步定位建图方法更新k时刻移动机器人的状态向量;
S3中,利用观测值Zk及k-1时刻的状态向量采用新息叠加自适应扩展卡尔曼滤波同步定位建图方法更新k时刻移动机器人的状态向量,包括:
S31、获取移动机器人在SLAM系统中的运动方程;
S32、根据移动机器人在SLAM系统中的运动方程,建立移动机器人SLAM观测方程;
dk表示加权系数,Pk表示状态向量后验估计的协方差;
作为优选,S34中,
W表示f(·)对Xk求偏导得到的雅各比矩阵;
f(·)表示非线性机器人系统状态转移函数;
Xk=f(Xk-1,uk)+wk,uk表示非线性机器人系统的输入控制量,wk表示非线性机器人系统的过程噪声;
θk-1分别表示k-1时刻机器人运动方向与x轴夹角,vk表示k时刻机器人线速度,ωk表示k时刻机器人角速度,Δt表示k-1时刻到k时刻所经过的时间;
作为优选,加权系数dk为:
b表示渐消因子,0<b<1,bk+1表示b的k+1次幂。
作为优选,移动机器人在SLAM系统中的运动方程为:
作为优选,S32中根据移动机器人在SLAM系统中的状态方程,建立移动机器人SLAM观测方程为:
本发明的有益效果,本发明通过设计基于限制增广的新息叠加同步定位建图方法,解决了自适应扩展卡尔曼滤波SLAM算法在对移动机器人状态估计时,定位建图算法精度降低且运行速度慢的问题,使移动机器人定位建图算法收敛的同时,提高算法对移动机器人位姿及环境中路标特征估计精度以及运行速度。发明通过将单一时刻的新息扩展为多时刻新息,使得迭代计算参数估计过程中,相邻时刻数据可以重复利用,提高了算法对机器人位姿及环境特征的估计精度并使得算法的性能更稳定,并且在整个扩展后的新息矩阵中使用更加接近系统真实值的上一时刻观测值,使得算法预测精度进一步提高。本发明对系统状态向量增广进行限制,当某一时刻传感器观测到的特征点较多时,约束传感器观测范围。使算法在任意时刻只对满足条件的路标点信息进行计算,系统状态向量维数不会快速增大,减少算法的计算量,提高算法运行速度。
附图说明
图1为本发明的流程图;
图2为移动机器人SLAM系统模型;
图3为各算法在x方向上的误差,横坐标表示时间,纵坐标表示误差;lais slam-error表示本实施方式中采用限制增广的新息叠加同步定位建图方法的误差,is aekfslam-error表示新息叠加自适应扩展卡尔曼滤波定位建图方法误差,aekf slam-error表示自适应扩展卡尔曼滤波定位建图方法误差,ekf slam-error表示扩展卡尔曼滤波定位建图方法误差;
图4为各算法在y方向上的误差;
图5为各算法下旋转角θ的误差。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例,都属于本发明保护的范围。
需要说明的是,在不冲突的情况下,本发明中的实施例及实施例中的特征可以相互组合。
下面结合附图和具体实施例对本发明作进一步说明,但不作为本发明的限定。
本实施方式的基于限制增广的新息叠加同步定位建图方法,通过图1说明本发明的具体实施方式,本实施方式是一种基于限制增广的新息叠加同步定位建图方法。
步骤3、判断观测值中路标点是否在k-1时刻的状态向量中,若是,则利用观测值及k-1时刻的状态向量采用自适应扩展卡尔曼滤波同步定位建图方法更新k时刻移动机器人的状态向量,若否,将路标点加入地图,同时加入至k-1时刻的状态向量中,利用观测值Zk及k-1时刻的状态向量采用自适应扩展卡尔曼滤波同步定位建图方法更新k时刻移动机器人的状态向量。
本实施方式设计限制增广的新息叠加同步定位建图方法,给定一个变量s,来限制系统状态向量增广,在k时刻传感器观测信息可能为以下情况:
2)这种情况时,k-1时刻状态向量维数与k时刻传感器观测向量维数相差较大,如果将观测到的特征点全部加入状态向量,会使状态向量维度增加很多,降低算法精度。本实施方式可以约束传感器观测范围,令传感器最大观测范围为βmax,路标点与机器人之间距离为β。当β≤βmax时,按照IS-AEKF-SLAM算法更新其位置信息,并扩展系统状态向量;当β>βmax,则删除观测值中该路标点信息。
本实施方式利用IS-AEKF-SLAM算法对移动机器人的噪声进行实时修正,对移动机器人的状态进行估计,对状态向量增广进行限制进行算法设计,具体是按照以下步骤实施的:
步骤31、建立移动机器人SLAM系统运动方程,通过给定一个平移速度vk和一个旋转速度ωk来控制机器人运动。假设在单位时间内机器人的控制输入是恒定的,即给定的平移速度和旋转速度是不随时间变化的,则在此恒定输入的控制下,机器人会在k-1时刻到k时刻完成一段圆弧运动,如图2所示,其中,Ak-1为k-1时刻位姿,由(xk-1,yk-1,θk-1)T所表示;Ak为k时刻机器人位姿,由(xk,yk,θk)T所表示,xk、yk、θk分别表示k时刻移动机器人x轴坐标和y轴坐标以及运动方向与x轴夹角;机器人运动学方程为式(1),其中wk为移动机器人系统的过程噪声;Δt为k-1时刻到k时刻所用时间。
由于在SLAM点位建图过程中,不仅需要对机器人的运动进行估计,还需要对地图中各路标进行估计,所以需要将地图中的路标点信息加入到系统状态方程中,这样就可以得到机器人在整个SLAM系统中的状态方程式(2)。
步骤33:建立IS-AEKF-SLAM模型,对标准的自适应扩展卡尔曼滤波定位建图算法进行改进,将多次叠加的新息加入滤波算法中,从而提高算法精度。
设机器人系统的非线性模型为式(4)和式(5)。
Xk=f(Xk-1,uk)+wk (4)
f(·)表示非线性机器人系统状态转移函数;uk表示非线性机器人系统的输入控制量,wk表示非线性机器人系统的过程噪声。
Zk=h(Xk)+vk (5)
h(·)表示非线性机器人系统的测量函数,vk表示非线性机器人系统的观测噪声;
算法的预测步骤如下,系统状态预测方程为式(6);机器人系统状态预测值协方差矩阵通过式(8)算得,其中W为f(·)对wk求偏导得到的雅克比矩阵,通过式(4)和式(6)可以得到f对求偏导数雅克比矩阵F为式(7)。
算法的更新步骤如下,其中为k-1时刻到k时刻的新息,由式(9)求得;通过式(3)可以求得h(·)对求偏导得到的雅克比矩阵H为式(10),其中,Hj为映射矩阵,由式(11)求得;Kk为卡尔曼增益矩阵;为机器人系统状态的后验估计,由式(13)求得;Pk为后验估计的协方差,通过式(14)求得;为对k时刻系统观测噪声估计值、过程噪声估计值的均值以及过程噪声估计值的协方差矩阵,由式(15)、(16)、(18)求得;dk为加权系数,由式(19)求得;b为渐消因子,可以对系统的观测噪声和过程噪声进行实时修正,保证算法收敛。
b表示渐消因子,0<b<1,bk+1表示b的k+1次幂。
将多个时刻新息叠加后得到式(19)。
则可将系统的状态更新方程改写为式(21)。
移动机器人的观测噪声估计值更新方程可改写为式(21)。
若删除了观测值中C个路标点信息,则I=I-C,算法更新部分为:
其中,Znew,k为删除路标点信息后的观测值,由式(22)求得。为观测向量,由式(23)求得,获得限制增广的新息矩阵(24);Knew,k为限制增广后的卡尔曼增益矩阵,由式(25)求得;为限制增广后系统状态向量后验估计,由式(26)求得;Pnew,k为限制增广后的状态估计协方差矩阵,由式(27)求得,为限制增广后系统的过程噪声估计值及观测噪声估计值及各自的协方差矩阵,分别由式(28)、式(29)、式(30)求得。
在具体操作时,本发明通过设计算法估计的机器人位姿与机器人实际运动位姿之间的误差以及算法估计的路标点位置与实际路标点位置的误差,将所得结果进行比较,最终结果如图3、图4、图5以及表1所示,其中,图3为四种算法对于机器人系统在x轴方向的预测状态与系统在x轴方向的真实状态之间的误差;图4为四种算法对于机器人系统在y轴方向的预测状态与系统在y轴方向的真实状态之间的误差;图5为在四种算法下移动机器人运动过程中的运动方向与x轴夹角的预测值与真实值之间的误差;表1为地图中路标数量不同情况下,各算法完成建图所需时间。由仿真结果可以看出,移动机器人分别采用四种算法定位建图,其中基于限制增广的新息叠加同步定位建图方法精度更高,运行速度也更快。
表1各算法完成建图所需时间
虽然在本文中参照了特定的实施方式来描述本发明,但是应该理解的是,这些实施例仅仅是本发明的原理和应用的示例。因此应该理解的是,可以对示例性的实施例进行许多修改,并且可以设计出其他的布置,只要不偏离所附权利要求所限定的本发明的精神和范围。应该理解的是,可以通过不同于原始权利要求所描述的方式来结合不同的从属权利要求和本文中所述的特征。还可以理解的是,结合单独实施例所描述的特征可以使用在其他所述实施例中。
Claims (5)
1.基于限制增广的新息叠加同步定位建图方法,其特征在于,所述方法包括:
S3、判断观测值中路标点是否在k-1时刻的状态向量中,若是,则利用观测值及k-1时刻的状态向量采用新息叠加自适应扩展卡尔曼滤波同步定位建图方法更新k时刻移动机器人的状态向量,若否,将路标点加入地图,同时加入至k-1时刻的状态向量中,利用观测值Zk及k-1时刻的状态向量采用新息叠加自适应扩展卡尔曼滤波同步定位建图方法更新k时刻移动机器人的状态向量;
S3中,利用观测值Zk及k-1时刻的状态向量采用新息叠加自适应扩展卡尔曼滤波同步定位建图方法更新k时刻移动机器人的状态向量,包括:
S31、获取移动机器人在SLAM系统中的运动方程;
S32、根据移动机器人在SLAM系统中的运动方程,建立移动机器人SLAM观测方程;
dk表示加权系数,Pk表示状态向量后验估计的协方差;
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211550548.4A CN115979240B (zh) | 2022-12-05 | 2022-12-05 | 基于限制增广的新息叠加同步定位建图方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211550548.4A CN115979240B (zh) | 2022-12-05 | 2022-12-05 | 基于限制增广的新息叠加同步定位建图方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN115979240A true CN115979240A (zh) | 2023-04-18 |
CN115979240B CN115979240B (zh) | 2023-09-29 |
Family
ID=85961936
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211550548.4A Active CN115979240B (zh) | 2022-12-05 | 2022-12-05 | 基于限制增广的新息叠加同步定位建图方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115979240B (zh) |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20050251328A1 (en) * | 2004-04-05 | 2005-11-10 | Merwe Rudolph V D | Navigation system applications of sigma-point Kalman filters for nonlinear estimation and sensor fusion |
CN106482736A (zh) * | 2016-07-11 | 2017-03-08 | 安徽工程大学 | 一种基于平方根容积卡尔曼滤波的多机器人协同定位算法 |
CN109163720A (zh) * | 2018-08-27 | 2019-01-08 | 广西科技大学 | 基于渐消记忆指数加权的卡尔曼滤波跟踪方法 |
CN110232662A (zh) * | 2019-05-10 | 2019-09-13 | 昆明理工大学 | 一种人脸位姿协同系统下的多新息抗扰滤波方法 |
CN115328168A (zh) * | 2022-09-19 | 2022-11-11 | 西安交通大学 | 基于自适应强跟踪的移动机器人同步定位与建图方法及系统 |
-
2022
- 2022-12-05 CN CN202211550548.4A patent/CN115979240B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20050251328A1 (en) * | 2004-04-05 | 2005-11-10 | Merwe Rudolph V D | Navigation system applications of sigma-point Kalman filters for nonlinear estimation and sensor fusion |
CN106482736A (zh) * | 2016-07-11 | 2017-03-08 | 安徽工程大学 | 一种基于平方根容积卡尔曼滤波的多机器人协同定位算法 |
CN109163720A (zh) * | 2018-08-27 | 2019-01-08 | 广西科技大学 | 基于渐消记忆指数加权的卡尔曼滤波跟踪方法 |
CN110232662A (zh) * | 2019-05-10 | 2019-09-13 | 昆明理工大学 | 一种人脸位姿协同系统下的多新息抗扰滤波方法 |
CN115328168A (zh) * | 2022-09-19 | 2022-11-11 | 西安交通大学 | 基于自适应强跟踪的移动机器人同步定位与建图方法及系统 |
Non-Patent Citations (4)
Title |
---|
YI YINGMMIN等: "Landmark Sequence Data Association for Simultaneous Localization and Mapping of Robots", CYBERNETICS AND INFORMATION TECHNOLOGIES, vol. 14, no. 3 * |
ZHOU Z等: "A Multi-innovation with forgetting factor based EKF-SLAM method for mobile robots", ASSEMMBLY AUTOMATION, vol. 41, no. 1 * |
刘毛毛等: "基于多新息理论的EKF改进算法", 计算机应用研究, vol. 32, no. 5 * |
周振等: "基于改进多新息理论的EKF-SLAM算法", 自动化与仪器仪表, no. 6 * |
Also Published As
Publication number | Publication date |
---|---|
CN115979240B (zh) | 2023-09-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109597864B (zh) | 椭球边界卡尔曼滤波的即时定位与地图构建方法及系统 | |
Huang et al. | An observability-constrained sliding window filter for SLAM | |
CN108896047B (zh) | 分布式传感器网络协同融合与传感器位置修正方法 | |
CN107741745A (zh) | 一种实现移动机器人自主定位与地图构建的方法 | |
CN110763239A (zh) | 滤波组合激光slam建图方法及装置 | |
CN112097774B (zh) | 基于自适应卡尔曼滤波与平均跟踪的分布式地图融合方法 | |
Taylor | An analysis of observability-constrained Kalman filtering for vision-aided navigation | |
CN113076634B (zh) | 一种多机协同无源定位方法、装置及系统 | |
CN111815684B (zh) | 基于统一残差模型的空间多元特征配准优化方法及装置 | |
Jayakumar et al. | Risk-sensitive filters for recursive estimation of motion from images | |
CN115979240A (zh) | 基于限制增广的新息叠加同步定位建图方法 | |
CN115328168A (zh) | 基于自适应强跟踪的移动机器人同步定位与建图方法及系统 | |
He et al. | A SLAM algorithm of fused EKF and particle filter | |
CN113689501B (zh) | 一种基于收敛点的双机协同目标机定位跟踪控制方法 | |
CN111998854B (zh) | 基于Cholesky分解计算的精确扩展Stirling插值滤波方法 | |
CN108681621A (zh) | 基于Chebyshev正交多项式扩展RTS Kalman平滑方法 | |
CN109474892B (zh) | 基于信息形式的强鲁棒传感器网络目标跟踪方法 | |
Wang et al. | Jacobian estimation with adaptive Kalman filter for uncalibrated visual servoing | |
CN110543919B (zh) | 一种机器人定位控制方法、终端设备及存储介质 | |
Hani et al. | Realtime autonomous navigation in V-Rep based static and dynamic environment using EKF-SLAM | |
de Souza Pinto et al. | EKF-SLAM with autonomous exploration using a low cost robot | |
CN111426322A (zh) | 同时估计状态和输入的自适应目标跟踪滤波方法及系统 | |
CN111216146A (zh) | 一种适用于网络化机器人系统的二部一致性量化控制方法 | |
CN113156968B (zh) | 一种移动机器人的路径规划方法及系统 | |
Gharib et al. | Anfis based ukf-slam path planning method |
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 |