CN112732854A - 一种粒子滤波bslam方法 - Google Patents
一种粒子滤波bslam方法 Download PDFInfo
- Publication number
- CN112732854A CN112732854A CN202110028795.7A CN202110028795A CN112732854A CN 112732854 A CN112732854 A CN 112732854A CN 202110028795 A CN202110028795 A CN 202110028795A CN 112732854 A CN112732854 A CN 112732854A
- Authority
- CN
- China
- Prior art keywords
- particle
- particles
- updating
- track
- odometer
- 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
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
- G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
- G06F16/29—Geographical information databases
-
- 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
- G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
- G01S15/006—Theoretical aspects
-
- 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
- G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
- G01S15/88—Sonar systems specially adapted for specific applications
- G01S15/89—Sonar systems specially adapted for specific applications for mapping or imaging
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Databases & Information Systems (AREA)
- Data Mining & Analysis (AREA)
- Mathematical Physics (AREA)
- Computer Networks & Wireless Communication (AREA)
- General Engineering & Computer Science (AREA)
- Acoustics & Sound (AREA)
- Algebra (AREA)
- Computational Mathematics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Pure & Applied Mathematics (AREA)
- Software Systems (AREA)
- Navigation (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Abstract
本发明公开了一种粒子滤波BSLAM方法,包括:步骤一:算法初始化;步骤二:在每个t时刻输入对海底地形的深度测量值z(t)与里程计更新值v(t)以及里程计DR(t)后,对P(1:N)中所有粒子进行运动更新、闭环检测、观测更新、粒子历史轨迹更新;步骤三:当t时刻所有粒子都进行了步骤二后,判断是否需要进行重采样,若是:将所有经过观测更新的粒子进行重采样,t=t+1,转入步骤二,否则t=t+1,转入步骤二;步骤四:当t=T时,以最终粒子集中每个粒子轨迹的平均值结合对应时刻的观测数据z(1:t)生成海底地形图并输出。本发明只需要在每一时刻输入里程计数据和由多波束声纳获得的地形测深,即可在没有先验地形图的情况下实现AUV的同步定位与建图。
Description
技术领域
本发明涉及一种粒子滤波BSLAM方法,特别是一种使用粒子自身储存轨迹进行权值更新的粒子滤波BSLAM方法,属于海底地形同步定位与建图领域。
背景技术
BSLAM以多波束声纳为传感器收集海底地形信息,从而实现智能水下机器人不依赖于外界提供信息和先验地形数据的情况下进行同步定位与建图,粒子滤波是实现这一方法的重要手段之一。但目前的粒子滤波BSLAM依赖于栅格进行地形匹配,每个粒子都需要储存一副栅格地图,这导致了算法在运行中占用了很大的内存开销,且粒子在重采样时需要复制其内部的整个栅格地图,同样造成了很大的时间开销。因此,需要一种可以在小内存和时间消耗的情况下进行BSLAM的方法。
目前针对使用粒子自身储存轨迹进行权值更新的粒子滤波BSLAM没有相关方法,较为相似的是:公开日为2020年1月24日,公开号为CN110726415A,发明名称为“一种自适应的水下多波束同步定位与建图方法”,该方法使用粒子集均值的轨迹作为算法生成的地图,但是无法处理粒子在重采样后聚类,由粒子集均值所生成轨迹不是最优轨迹的情况,而且需要在重采样后加入更新均值地图部分,算法较为复杂。
发明内容
针对上述现有技术,本发明要解决的技术问题是提供一种使用粒子自身储存轨迹进行权值更新的粒子滤波BSLAM方法,能够减少计算开销,且算法结构简单,有利于在计算资源受限的水下机器人工作。
为解决上述技术问题,本发明的一种粒子滤波BSLAM方法,包括以下步骤:
步骤一:算法初始化,包括:
1)设定粒子数N、粒子闭环检测半径R、粒子匹配子地图Mp的测线数目Np、粒子轨迹子地图Mh的测线数目Nh、算法总步数T;
步骤二:在每一时刻t,2≤t≤T输入对海底的深度测量值z(t)与里程计更新值v(t)以及里程计DR(t)后,对P(1:N)中所有粒子进行操作,具体为:
步骤三:当t时刻所有粒子都进行了步骤二后,判断是否需要进行重采样,定义进行观测更新的粒子总数为Nth,定义有效粒子数:
步骤四:当t=T时,以最终粒子集中每个粒子轨迹的平均值结合对应时刻的观测数据z(1:t)生成海底地形图并输出,结束。
本发明还包括:
步骤二中观测更新具体为:
步骤2.2:生成粒子匹配子地图轨迹:
步骤2.3:使用反距离加权算法对Mp进行插值,插值位置为Mh中测点的位置,获得包含L个共有有效测点的测深序列定义为zp,定义Mh中相同位置的有效测深序列为zh,计算粒子权值:
本发明的有益效果:本发明是一种基于粒子滤波的海底地形同步定位与建图方法,本发明方法只需要在每一时刻输入里程计数据和由多波束声纳获得的地形测深,即可在没有先验地形图的情况下实现AUV的同步定位与建图。本发明所提出的粒子滤波BSLAM算法基于粒子自身储存的轨迹进行粒子加权,相较于基于栅格地图的粒子滤波BSLAM算法,算法结构简单,内存占用小,计算效率高。
附图说明
图1为本发明方法流程框图;
图2为观测更新流程框图。
具体实施方式
下面结合附图对本发明具体实施方式做进一步说明。
结合图1,主程序包括如下步骤:
步骤一:算法初始化:1)设定粒子数N、粒子闭环检测半径R、粒子匹配子地图Mp的测线数目Np、粒子轨迹子地图Mh的测线数目Nh、算法总步数T。2)初始化粒子集P(1:N),对于任一粒子P(n)(n为粒子序数),其内部储存粒子当前状态粒子自身历史轨迹粒子权值被设定为默认值wn=1/N(t为时间步长)。3)t=1时刻,P(1:N)的状态和自身历史轨迹被初始化为由GPS获得的先验状态(GPSt=1),转入步骤二;
步骤二:在每一时刻t(2≤t≤T)输入对海底的深度测量值z(t)与里程计更新值v(t)以及里程计DR(t)后,对P(1:N)中所有粒子(以粒子n为例)进行:
其中ω为里程计的噪声;
步骤三:当t时刻所有粒子都进行了步骤二后,判断是否需要进行重采样。定义进行观测更新的粒子总数为Nth,定义有效粒子数:
步骤四:当t=T时,以最终粒子集中每个粒子轨迹的平均值结合对应时刻的观测数据z(1:t)生成海底地形图并输出,算法结束。
结合图2,观测更新包括如下步骤:
步骤二:生成粒子匹配子地图轨迹:
步骤三:使用反距离加权算法对Mp进行插值,插值位置为Mh中测点的位置,获得包含L个共有有效测点的测深序列定义为zp,定义Mh中相同位置的有效测深序列为zh。计算粒子权值:
Claims (2)
1.一种粒子滤波BSLAM方法,其特征在于,包括以下步骤:
步骤一:算法初始化,包括:
1)设定粒子数N、粒子闭环检测半径R、粒子匹配子地图Mp的测线数目Np、粒子轨迹子地图Mh的测线数目Nh、算法总步数T;
步骤二:在每一时刻t,2≤t≤T输入对海底的深度测量值z(t)与里程计更新值v(t)以及里程计DR(t)后,对P(1:N)中所有粒子进行操作,具体为:
步骤三:当t时刻所有粒子都进行了步骤二后,判断是否需要进行重采样,定义进行观测更新的粒子总数为Nth,定义有效粒子数:
步骤四:当t=T时,以最终粒子集中每个粒子轨迹的平均值结合对应时刻的观测数据z(1:t)生成海底地形图并输出,结束。
2.根据权利要求1所述一种粒子滤波BSLAM方法,其特征在于:步骤二所述观测更新具体为:
步骤2.2:生成粒子匹配子地图轨迹:
步骤2.3:使用反距离加权算法对Mp进行插值,插值位置为Mh中测点的位置,获得包含L个共有有效测点的测深序列定义为zp,定义Mh中相同位置的有效测深序列为zh,计算粒子权值:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110028795.7A CN112732854B (zh) | 2021-01-11 | 2021-01-11 | 一种粒子滤波bslam方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110028795.7A CN112732854B (zh) | 2021-01-11 | 2021-01-11 | 一种粒子滤波bslam方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112732854A true CN112732854A (zh) | 2021-04-30 |
CN112732854B CN112732854B (zh) | 2023-03-31 |
Family
ID=75590053
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110028795.7A Active CN112732854B (zh) | 2021-01-11 | 2021-01-11 | 一种粒子滤波bslam方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112732854B (zh) |
Citations (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105509755A (zh) * | 2015-11-27 | 2016-04-20 | 重庆邮电大学 | 一种基于高斯分布的移动机器人同步定位与地图构建方法 |
CN107246873A (zh) * | 2017-07-03 | 2017-10-13 | 哈尔滨工程大学 | 一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法 |
US20180075643A1 (en) * | 2015-04-10 | 2018-03-15 | The European Atomic Energy Community (Euratom), Represented By The European Commission | Method and device for real-time mapping and localization |
CN108645413A (zh) * | 2018-06-06 | 2018-10-12 | 江苏海事职业技术学院 | 一种移动机器人的同时定位与地图创建的动态纠正方法 |
CN108692701A (zh) * | 2018-05-28 | 2018-10-23 | 佛山市南海区广工大数控装备协同创新研究院 | 基于粒子滤波器的移动机器人多传感器融合定位方法 |
CN109084777A (zh) * | 2018-09-13 | 2018-12-25 | 黎建军 | 基于自适应遗传算法的粒子滤波定位与地图构建方法 |
CN110260856A (zh) * | 2019-06-26 | 2019-09-20 | 北京海益同展信息科技有限公司 | 一种建图方法和装置 |
CN110320520A (zh) * | 2019-06-26 | 2019-10-11 | 哈尔滨工程大学 | 一种测深信息同步定位与建图的鲁棒后端图优化方法 |
CN110333720A (zh) * | 2019-07-10 | 2019-10-15 | 国网四川省电力公司电力科学研究院 | 一种基于粒子滤波的slam优化方法 |
CN110555225A (zh) * | 2019-03-28 | 2019-12-10 | 陕西理工大学 | 一种基于分层粒子群优化算法的rbpf-slam计算方法 |
CN110608742A (zh) * | 2019-09-27 | 2019-12-24 | 五邑大学 | 基于粒子滤波slam的地图构建方法及装置 |
CN110726415A (zh) * | 2019-10-21 | 2020-01-24 | 哈尔滨工程大学 | 一种自适应的水下多波束同步定位与建图方法 |
CN110763239A (zh) * | 2019-11-14 | 2020-02-07 | 华南智能机器人创新研究院 | 滤波组合激光slam建图方法及装置 |
CN111044036A (zh) * | 2019-12-12 | 2020-04-21 | 浙江大学 | 基于粒子滤波的远程定位方法 |
US20200158862A1 (en) * | 2018-11-19 | 2020-05-21 | Invensense, Inc. | Method and system for positioning using radar and motion sensors |
-
2021
- 2021-01-11 CN CN202110028795.7A patent/CN112732854B/zh active Active
Patent Citations (15)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20180075643A1 (en) * | 2015-04-10 | 2018-03-15 | The European Atomic Energy Community (Euratom), Represented By The European Commission | Method and device for real-time mapping and localization |
CN105509755A (zh) * | 2015-11-27 | 2016-04-20 | 重庆邮电大学 | 一种基于高斯分布的移动机器人同步定位与地图构建方法 |
CN107246873A (zh) * | 2017-07-03 | 2017-10-13 | 哈尔滨工程大学 | 一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法 |
CN108692701A (zh) * | 2018-05-28 | 2018-10-23 | 佛山市南海区广工大数控装备协同创新研究院 | 基于粒子滤波器的移动机器人多传感器融合定位方法 |
CN108645413A (zh) * | 2018-06-06 | 2018-10-12 | 江苏海事职业技术学院 | 一种移动机器人的同时定位与地图创建的动态纠正方法 |
CN109084777A (zh) * | 2018-09-13 | 2018-12-25 | 黎建军 | 基于自适应遗传算法的粒子滤波定位与地图构建方法 |
US20200158862A1 (en) * | 2018-11-19 | 2020-05-21 | Invensense, Inc. | Method and system for positioning using radar and motion sensors |
CN110555225A (zh) * | 2019-03-28 | 2019-12-10 | 陕西理工大学 | 一种基于分层粒子群优化算法的rbpf-slam计算方法 |
CN110320520A (zh) * | 2019-06-26 | 2019-10-11 | 哈尔滨工程大学 | 一种测深信息同步定位与建图的鲁棒后端图优化方法 |
CN110260856A (zh) * | 2019-06-26 | 2019-09-20 | 北京海益同展信息科技有限公司 | 一种建图方法和装置 |
CN110333720A (zh) * | 2019-07-10 | 2019-10-15 | 国网四川省电力公司电力科学研究院 | 一种基于粒子滤波的slam优化方法 |
CN110608742A (zh) * | 2019-09-27 | 2019-12-24 | 五邑大学 | 基于粒子滤波slam的地图构建方法及装置 |
CN110726415A (zh) * | 2019-10-21 | 2020-01-24 | 哈尔滨工程大学 | 一种自适应的水下多波束同步定位与建图方法 |
CN110763239A (zh) * | 2019-11-14 | 2020-02-07 | 华南智能机器人创新研究院 | 滤波组合激光slam建图方法及装置 |
CN111044036A (zh) * | 2019-12-12 | 2020-04-21 | 浙江大学 | 基于粒子滤波的远程定位方法 |
Non-Patent Citations (5)
Title |
---|
J. JANG 等: "Weighted Grid Partitioning for Panel-Based Bathymetric SLAM", 《OCEANS 2019 - MARSEILLE》 * |
S. BARKBY 等: "Bathymetric SLAM with no map overlap using Gaussian Processes", 《2011 IEEE/RSJ INTERNATIONAL CONFERENCE ON INTELLIGENT ROBOTS AND SYSTEMS》 * |
王丹丹: "水下无人潜器同步定位与地图生成方法研究", 《中国博士学位论文全文数据库 工程科技Ⅱ辑》 * |
赵新洋: "基于激光雷达的同时定位与室内地图构建算法研究", 《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》 * |
马腾 等: "AUV的图优化海底地形同步定位与建图方法", 《导航定位与授时》 * |
Also Published As
Publication number | Publication date |
---|---|
CN112732854B (zh) | 2023-03-31 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Li et al. | Robust time-varying formation control for underactuated autonomous underwater vehicles with disturbances under input saturation | |
CN108803321B (zh) | 基于深度强化学习的自主水下航行器轨迹跟踪控制方法 | |
CN105180935B (zh) | 一种适用于gnss微弱信号的组合导航数据融合方法 | |
CN112558612B (zh) | 一种基于云模型量子遗传算法的异构智能体编队控制方法 | |
CN106503837B (zh) | 一种基于改进水平集算法的时间最优航路规划方法 | |
CN109507885A (zh) | 基于自抗扰的无模型自适应auv控制方法 | |
CN110726415B (zh) | 一种自适应的水下多波束同步定位与建图方法 | |
CN112612290B (zh) | 一种考虑洋流的水下航行器三维多任务路径规划方法 | |
CN113847915B (zh) | 一种捷联惯导/多普勒组合导航系统的导航方法 | |
CN114397899A (zh) | 一种仿生机器鱼三维路径跟踪控制方法及装置 | |
CN111025909A (zh) | 船舶运动控制系统的Kalman三自由度解耦滤波方法 | |
WO2021065538A1 (ja) | 点群データ同一性推定装置及び点群データ同一性推定システム | |
CN110220510A (zh) | 一种考虑地图准确性的水下机器人海底地形匹配导航路径规划方法 | |
CN114137826B (zh) | 一种基于灰狼算法的船舶动力定位自抗扰控制方法 | |
Gao et al. | Cruise speed estimation strategy based on multiple fuzzy logic and extended state observer for low-cost AUV | |
WO2021109166A1 (zh) | 三维激光定位方法及系统 | |
CN113008223B (zh) | 基于粒子滤波与图优化的海底地形同步定位与建图方法 | |
CN113063412B (zh) | 一种基于可靠性分析的多机器人协同定位与建图方法 | |
CN112732854B (zh) | 一种粒子滤波bslam方法 | |
CN114610046A (zh) | 一种考虑动态水深的无人艇动态安全轨迹规划方法 | |
CN104536944B (zh) | 基于改进的极大似然方法和置信压缩滤波的船舶参数辨识方法 | |
CN116700300A (zh) | 一种多auv分布式集群的鲁棒自适应一致性控制方法 | |
CN110481561A (zh) | 无人驾驶车辆自动控制信号生成方法和装置 | |
CN116300913A (zh) | 一种基于视觉信息的无人艇多约束局部路径规划方法 | |
CN112904719B (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 |