CN114061583B - 基于自适应晶格卡尔曼滤波的移动机器人状态估计及自主导航方法 - Google Patents
基于自适应晶格卡尔曼滤波的移动机器人状态估计及自主导航方法 Download PDFInfo
- Publication number
- CN114061583B CN114061583B CN202111227391.7A CN202111227391A CN114061583B CN 114061583 B CN114061583 B CN 114061583B CN 202111227391 A CN202111227391 A CN 202111227391A CN 114061583 B CN114061583 B CN 114061583B
- Authority
- CN
- China
- Prior art keywords
- mobile robot
- variance
- lattice
- state
- kalman filtering
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 72
- 238000001914 filtration Methods 0.000 title claims abstract description 25
- 238000005070 sampling Methods 0.000 claims abstract description 38
- 238000005259 measurement Methods 0.000 claims abstract description 32
- 238000004364 calculation method Methods 0.000 claims abstract description 17
- 230000003044 adaptive effect Effects 0.000 claims description 22
- 238000009826 distribution Methods 0.000 claims description 18
- 238000006073 displacement reaction Methods 0.000 claims description 12
- 238000013507 mapping Methods 0.000 claims description 7
- 230000001186 cumulative effect Effects 0.000 claims description 6
- 239000011159 matrix material Substances 0.000 claims description 4
- 238000000342 Monte Carlo simulation Methods 0.000 claims description 3
- 230000006978 adaptation Effects 0.000 claims description 3
- 238000003491 array Methods 0.000 claims description 3
- 238000009827 uniform distribution Methods 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims 1
- 238000005516 engineering process Methods 0.000 abstract description 6
- 230000009286 beneficial effect Effects 0.000 abstract description 3
- 238000010276 construction Methods 0.000 abstract description 2
- 238000012937 correction Methods 0.000 abstract description 2
- 230000006870 function Effects 0.000 description 4
- 238000011549 displacement method Methods 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000009499 grossing Methods 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
Images
Classifications
-
- 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
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N7/00—Computing arrangements based on specific mathematical models
- G06N7/01—Probabilistic graphical models, e.g. probabilistic networks
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Data Mining & Analysis (AREA)
- Mathematical Optimization (AREA)
- Artificial Intelligence (AREA)
- Computational Mathematics (AREA)
- Probability & Statistics with Applications (AREA)
- Evolutionary Computation (AREA)
- Mathematical Analysis (AREA)
- Algebra (AREA)
- Pure & Applied Mathematics (AREA)
- Computing Systems (AREA)
- General Engineering & Computer Science (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Feedback Control In General (AREA)
Abstract
本发明公开了一种基于自适应晶格卡尔曼滤波的移动机器人状态估计及自主导航方法,旨在解决现有的SLAM技术无法处理非线性不确定系统、定位导航精度不高、计算量大的技术问题。本发明中自适应晶格卡尔曼滤波‑SLAM(ALKF‑SLAM)自主导航算法对于移动机器人的系统模型的噪声方差是否精确已知没有要求,并通过减少状态采样点,降低计算复杂度和计算成本,提高状态估计的精度和数值稳定性。利用晶格采样点,并根据量测残差序列和方差对量测方差进行修正。其次,利用基于上述修正的量测方差和晶格采样点设计的衰减因子对状态方差进行了修正。本发明的ALKF‑SLAM算法,有利于提高移动机器人在未知环境中地图构建和自身定位的精度。
Description
技术领域
本发明涉及移动机器人自主导航技术领域,具体涉及一种基于自适应晶格卡尔曼滤波的移动机器人状态估计及自主导航方法。
背景技术
随着人工智能和机器人技术的快速发展,移动机器人在慢慢应用到社会的各个领域,比如办事助理、AGV搬运、安保巡逻、灾难救援、空间探测、变电站巡检等等。移动机器人在未知环境中对多传感器采集的数据进行融合,对未知环境标志物进行地图构建以及定位自身位置。机器人的环境感知和精确定位问题是移动机器人实现自主导航的关键技术,是研究移动机器人的热门领域,其被誉为移动机器人界的“圣杯”。
同步地定位与地图创建(Simultaneous Localization and Mapping,SLAM)技术,是解决机器人在未知环境中导航问题的最佳手段。机器人的SLAM技术在移动过程中通过传感器来精确估计自身位姿,同时增量式的构建真实环境地图。SLAM问题的求解方法可分为基于平滑的方法和基于滤波器的方法。基于滤波器的方法主要利用递归贝叶斯估计原理来实现,现有扩展卡尔曼滤波(EKF)-SLAM技术解决了卡尔曼滤波只能处理线性高斯系统的限制,但EKF是利用泰勒展开的一阶项来近似非线性系统,无法处理强非线性系统问题,处理过程中出现的截断误差严重影响了机器人的导航定位精度以及雅克比矩阵的求解大大增加计算度。
发明内容
本发明提供一种基于自适应晶格卡尔曼滤波(ALKF)的移动机器人状态估计及自主导航方法,以解决现有的SLAM技术无法处理非线性不确定系统、定位导航精度不高、计算量大的技术问题。
为解决上述技术问题,本发明采用如下技术方案:
设计一种基于自适应晶格卡尔曼滤波的移动机器人状态估计及自主导航方法,包括:
S2)对移动机器人的位置、方位角等状态进行采样,得到状态的晶格采样点集为:
S3)对移动机器人第k步的状态进行一步预测:
S4)获取方差的一步预测:
S5)对步骤S3)中的状态一步预测重新进行晶格采样:
状态方差由衰减系数μk调整,其计算公式为:
S8)计算滤波增益矩阵Kk:
式中,Pzz为自适应后的移动机器人量测预测方差;Pxz为状态预测值与量测预测值的方差;
S9)更新状态:
S10)计算状态估计误差的方差Pk:
其中Pk,k-1为自适应过后的状态方差,
进一步的,所述步骤S2)中,得到状态的晶格采样点集的方法如下:利用确定的晶格点逼近多元积分的拟蒙特卡罗方法(QMC)。在这里,生成的格点由正态累积分布的逆映射函数对格点进行变换,得到样本点的权重。为了简化计算和提高计算效率,采用秩为1格的晶格规则对进行采样,是一种产生低差点的模化算术方法。
对状态采样用秩为1格的晶格规则如下:
式中,g(a)包含d个整数[a0,a2,a3,...,ad-1]T,a是一个具有点数N的互质整数。
为了获得无偏积分计算并提高估计精度,根据克兰利-帕特森位移方法(模1位移)生成随机位移格点,则状态采样的位移格点及其权重为:
式中,Δ是一个位移因子,它由超立方体上均匀分布生成的[0,1)d,si为状态采样的位移晶格点的权重,φ-1是一维正态累积分布φ的逆映射函数
本发明的第二方面是:
设计一种基于自适应晶格卡尔曼滤波的移动机器人自主导航方法,包括如下步骤:
Ⅰ)建立移动机器人的导航系统模型方程,并初始化;
Ⅱ)建立SLAM概率模型并采用贝叶斯滤波进行预测和更新;
Ⅲ)采用上所述移动机器人状态估计方法对机器人的位置和方位角进行更新并输出。
其次是观测更新,利用传感器k时刻的量测测数据zk对先验概率分布进行矫正,获得后验概率分布:
式中,η为归一化系数。
进一步的,在所述步骤Ⅰ)中,wk和vk的方差阵分别为Qk和Rk,并且满足:
与现有技术相比,本发明的有益技术效果在于:
本发明对于移动机器人的系统模型的噪声方差是否精确已知没有要求,并通过减少状态采样点,降低计算复杂度和计算成本,提高状态估计的精度和数值稳定性。首先,利用晶格采样点,根据量测残差序列和方差,修正了量测方差。其次,利用基于上述修正的量测方差和晶格采样点设计的衰减因子对状态方差进行了修正。本发明的ALKF-SLAM算法,有利于提高移动机器人在未知环境中地图构建和自身定位的精度。
附图说明
图1为本发明在移动机器人导航时使用LKF(晶格卡尔曼滤波)-SLAM和自适应ALKF-SLAM算法的x轴均方根误差对比图。
图2为本发明在移动机器人导航时使用LKF-SLAM和自适应LKF-SLAM算法的y轴的均方根误差对比图。
具体实施方式
下面结合附图和实施例来说明本发明的具体实施方式,但以下实施例只是用来详细说明本发明,并不以任何方式限制本发明的范围。
实施例1:一种设计一种基于自适应晶格卡尔曼滤波的移动机器人状态估计方法,包括如下步骤:
S2)对移动机器人的位置、方位角等状态进行采样,得到状态的晶格采样点集为:
上式对状态进行晶格采样的方法如下:
利用确定的晶格点逼近多元积分的拟蒙特卡罗方法(QMC)。在这里,生成的格点由正态累积分布的逆映射函数对格点进行变换,得到样本点的权重。为了简化计算和提高计算效率,采用秩为1格的晶格规则对进行采样,是一种产生低差点的模化算术方法。
对状态采样用秩为1格的晶格规则如下:
式中,g(a)包含d个整数[a0,a2,a3,...,ad-1]T,a是一个具有点数N的互质整数。
为了获得无偏积分计算并提高估计精度,根据克兰利-帕特森位移方法(模1位移)生成随机位移格点,则状态采样的位移格点及其权重为:
式中,Δ是一个位移因子,它由超立方体上均匀分布生成的[0,1)d,si为状态采样的位移晶格点的权重,φ-1是一维正态累积分布φ的逆映射函数
S3)对移动机器人第k步的状态进行一步预测:
S4)获取方差的一步预测:
S5)对状态一步预测的进行重新晶格采样:
状态方差由衰减系数μk调整,其计算公式为:
S8)计算滤波增益矩阵Kk:
式中,Pzz为自适应后的移动机器人量测预测方差;Pxz为状态预测值与量测预测值的方差;
S9)更新状态:
S10)计算状态估计误差的方差Pk:
其中Pk,k-1为自适应过后的状态方差。
基于自适应晶格卡尔曼滤波-SLAM的移动机器人自主导航方法,包括如下步骤:
Ⅰ)建立移动机器人的导航系统模型方程,并初始化如下;
Ⅱ)建立SLAM概率模型并采用贝叶斯滤波进行预测和更新;
Ⅲ)采用上所述移动机器人状态估计方法对机器人的位置和方位角进行更新并输出。
其次是观测更新,利用传感器k时刻的量测测数据zk对先验概率分布进行矫正,获得后验概率分布:
式中,η为归一化系数。
在所述步骤Ⅰ)中,wk和vk的方差阵分别为Qk和Rk,并且满足:
上面结合附图和实施例对本发明作了详细的说明,但是,所属技术领域的技术人员能够理解,在不脱离本发明宗旨的前提下,还可以对上述实施例中的各个具体参数进行变更,形成多个具体的实施例,均为本发明的常见变化范围,在此不再一一详述。
Claims (5)
1.一种基于自适应晶格卡尔曼滤波的移动机器人状态估计方法,其特征在于,包括:
S2)对移动机器人的位置、方位角状态进行采样,得到状态的晶格采样点集为:
得到所述状态的晶格采样点集的方法如下:利用确定的晶格点逼近多元积分的拟蒙特卡罗方法;生成的格点由正态累积分布的逆映射函数对格点进行变换,得到样本点的权重;采用秩为1格的晶格规则对进行采样;
对状态采样用秩为1格的晶格规则如下:
式中,g(a)包含d个整数[a0,a2,a3,...,ad-1]T,a是一个具有点数N的互质整数;
根据克兰利-帕特森位移方法,模1位移生成随机位移格点,则状态采样的位移格点及其权重为:
式中,Δ是一个位移因子,它由超立方体上均匀分布生成的[0,1)d,si为状态采样的位移晶格点的权重,φ-1是一维正态累积分布φ的逆映射函数;
S3)对移动机器人第k步的状态进行一步预测:
S4)获取方差的一步预测:
S5)对步骤S3)中的状态一步预测重新进行晶格采样:
状态方差由衰减系数μk调整,其计算公式为:
S8)计算滤波增益矩阵Kk:
式中,Pzz,k为自适应后的移动机器人量测预测方差;Pxz,k为状态预测值与量测预测值的方差;
S9)更新状态:
S10)计算状态估计误差的方差Pk:
其中Pk,k-1为自适应过后的状态方差,
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111227391.7A CN114061583B (zh) | 2021-10-21 | 2021-10-21 | 基于自适应晶格卡尔曼滤波的移动机器人状态估计及自主导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111227391.7A CN114061583B (zh) | 2021-10-21 | 2021-10-21 | 基于自适应晶格卡尔曼滤波的移动机器人状态估计及自主导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114061583A CN114061583A (zh) | 2022-02-18 |
CN114061583B true CN114061583B (zh) | 2023-07-07 |
Family
ID=80235201
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111227391.7A Active CN114061583B (zh) | 2021-10-21 | 2021-10-21 | 基于自适应晶格卡尔曼滤波的移动机器人状态估计及自主导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114061583B (zh) |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102456231A (zh) * | 2010-10-19 | 2012-05-16 | 威锋数位开发股份有限公司 | 将目标点阵字体转化成目标轮廓字体的方法及系统 |
AU2012328156A1 (en) * | 2011-10-28 | 2014-05-22 | Bae Systems Plc | Identification and analysis of aircraft landing sites |
CN107683460A (zh) * | 2015-05-05 | 2018-02-09 | 凯恩迪股份有限公司 | 在传统处理器上模拟量子样计算的quanton表示 |
DE102018001533A1 (de) * | 2018-02-27 | 2019-08-29 | Mbda Deutschland Gmbh | Echtzeitverfahren zur dynamischen Koordinaten- und Parameterschätzung mit gitterbasierten nicht-linearen probabilistischen Filtern |
CN110278525A (zh) * | 2019-05-21 | 2019-09-24 | 袁正道 | 一种高精度室内无线定位方法 |
-
2021
- 2021-10-21 CN CN202111227391.7A patent/CN114061583B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN102456231A (zh) * | 2010-10-19 | 2012-05-16 | 威锋数位开发股份有限公司 | 将目标点阵字体转化成目标轮廓字体的方法及系统 |
AU2012328156A1 (en) * | 2011-10-28 | 2014-05-22 | Bae Systems Plc | Identification and analysis of aircraft landing sites |
CN107683460A (zh) * | 2015-05-05 | 2018-02-09 | 凯恩迪股份有限公司 | 在传统处理器上模拟量子样计算的quanton表示 |
DE102018001533A1 (de) * | 2018-02-27 | 2019-08-29 | Mbda Deutschland Gmbh | Echtzeitverfahren zur dynamischen Koordinaten- und Parameterschätzung mit gitterbasierten nicht-linearen probabilistischen Filtern |
CN110278525A (zh) * | 2019-05-21 | 2019-09-24 | 袁正道 | 一种高精度室内无线定位方法 |
Non-Patent Citations (3)
Title |
---|
A comparison of EKF UKF fastSLAM2.0 and UKF-based fastSLAM algorithms;Z.Kurt-Yavuz;《2012 IEEE 16th international conference on intelligent engineering systems》;37-43 * |
Impaired Aircraft Performance Envelope Estimation;Menon P K;《Journal of Aircraft》;第50卷(第2期);410-424 * |
无线传感器网络覆盖与优化技术研究;刘晓爽;《中国博士学位论文全文数据库信息科技辑》(第3期);I140-72 * |
Also Published As
Publication number | Publication date |
---|---|
CN114061583A (zh) | 2022-02-18 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103644903B (zh) | 基于分布式边缘无味粒子滤波的同步定位与地图构建方法 | |
CN109597864B (zh) | 椭球边界卡尔曼滤波的即时定位与地图构建方法及系统 | |
CN104020480B (zh) | 一种带自适应因子的交互式多模型ukf的卫星导航方法 | |
CN106599368B (zh) | 基于改进粒子提议分布和自适应粒子重采样的FastSLAM方法 | |
CN104697523B (zh) | 基于迭代计算的惯性/地磁匹配定位方法 | |
CN106197428B (zh) | 一种利用测量信息优化分布式ekf估计过程的slam方法 | |
CN110514203B (zh) | 一种基于isr-ukf的水下组合导航方法 | |
CN106772524B (zh) | 一种基于秩滤波的农业机器人组合导航信息融合方法 | |
CN103940433B (zh) | 一种基于改进的自适应平方根ukf算法的卫星姿态确定方法 | |
CN108362288B (zh) | 一种基于无迹卡尔曼滤波的偏振光slam方法 | |
CN108387236B (zh) | 一种基于扩展卡尔曼滤波的偏振光slam方法 | |
CN105180935A (zh) | 一种适用于gnss微弱信号的组合导航数据融合方法 | |
CN105180938A (zh) | 一种基于粒子滤波的重力采样矢量匹配定位方法 | |
CN112432644B (zh) | 基于鲁棒自适应无迹卡尔曼滤波的无人艇组合导航方法 | |
CN110567455B (zh) | 一种求积更新容积卡尔曼滤波的紧组合导航方法 | |
CN113984054A (zh) | 基于信息异常检测的改进Sage-Husa自适应融合滤波方法及多源信息融合设备 | |
CN114970222B (zh) | 基于hasm的区域气候模式日平均气温偏差订正方法和系统 | |
CN108444479A (zh) | 基于自适应鲁棒无迹卡尔曼滤波的重力匹配方法 | |
CN109855623A (zh) | 基于Legendre多项式和BP神经网络的地磁模型在线逼近方法 | |
CN110912535B (zh) | 一种新型无先导卡尔曼滤波方法 | |
CN114608568A (zh) | 一种基于多传感器信息即时融合定位方法 | |
CN114061583B (zh) | 基于自适应晶格卡尔曼滤波的移动机器人状态估计及自主导航方法 | |
CN115655285B (zh) | 一种权值及参考四元数修正的无迹四元数姿态估计方法 | |
CN111578936B (zh) | 基于imm-ukf的惯性/超短基线多参数标定方法 | |
CN113532416B (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 |