CN113008223A - 基于粒子滤波与图优化的海底地形同步定位与建图方法 - Google Patents

基于粒子滤波与图优化的海底地形同步定位与建图方法 Download PDF

Info

Publication number
CN113008223A
CN113008223A CN202110233287.2A CN202110233287A CN113008223A CN 113008223 A CN113008223 A CN 113008223A CN 202110233287 A CN202110233287 A CN 202110233287A CN 113008223 A CN113008223 A CN 113008223A
Authority
CN
China
Prior art keywords
particle
particles
map
track
mapping
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
Application number
CN202110233287.2A
Other languages
English (en)
Other versions
CN113008223B (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.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202110233287.2A priority Critical patent/CN113008223B/zh
Publication of CN113008223A publication Critical patent/CN113008223A/zh
Application granted granted Critical
Publication of CN113008223B publication Critical patent/CN113008223B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • 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
    • 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
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • 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
    • G06F17/15—Correlation function computation including computation of convolution operations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Data Mining & Analysis (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Computational Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Acoustics & Sound (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Computing Systems (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)

Abstract

本发明属于海底地形同步定位与建图技术领域，具体涉及一种基于粒子滤波与图优化的海底地形同步定位与建图方法。本发明设计了一种使用位姿图结构对粒子自身储存轨迹进行更新，并使用位姿图在每一时刻结束时生成此时刻输出地图的粒子滤波BSLAM算法。本发明只需要在每一时刻输入里程计数据和由多波束声获得的地形测深，在没有先验地形图的情况下实现AUV的同步定位与建图。本发明所提出的图优化混合粒子滤波BSLAM算法相较于基于子地图匹配的粒子滤波BSLAM算法，在大累积误差下仍能实时准确的提供定位与建图结果。

Description

基于粒子滤波与图优化的海底地形同步定位与建图方法
技术领域
本发明属于海底地形同步定位与建图技术领域，具体涉及一种基于粒子滤波与图优化的海底地形同步定位与建图方法。
背景技术
测深信息同步定位与建图(BathymetricSimultaneousLocalizationandMapping,BSLAM)以多波束声呐为传感器收集海底地形信息，能够实现智能水下机器人在不依赖于外界提供信息的情况下，独立长时间的准确定位与建图，粒子滤波是实现这一方法的重要手段之一。但因为粒子滤波只能求解当前时刻的最优状态，在大累积误差的情况下会产生性能下降的问题。除此之外，因为基于子地图匹配的粒子滤波BSLAM中每一个粒子都会储存一副地图，选择这些粒子中最优地图存在问题。现有方法中使用粒子储存地图重复区域的最小均方误差作为选择的标准，但是存在着计算复杂度高，结果不稳定，误差较大的问题。因为BSLAM输出的地图需要为其它功能(如路径规划)提供信息，因此，需要一种在大累积误差下，可以准确实时生成地图的BSLAM方法。本发明在子地图匹配粒子滤波BSLAM的基础上提出了一种基于图优化理论的粒子地图更新方法与最优地图生成算法，能实现智能水下机器人在航位推算累积误差较大的情况下，能实时的提供准确的建图结果。
目前针对使用图优化理论进行地图更新与最终地图生成的粒子滤波BSLAM没有相关方法，较为相似的是：公开日为2020年1月24日，公开号为CN110726415A，发明名称为“一种自适应的水下多波束同步定位与建图方法”，该方法虽然可以使用在大累积误差下实时准确的进行定位与建图，但是无法处理粒子在重采样后聚类，由粒子集均值所生成轨迹不是最优轨迹的情况，而且需要在重采样后加入更新均值地图部分，算法较为复杂。除此之外，这种方法存在设定参数较多的问题。
发明内容
本发明的目的在于提供在没有先验地形图的情况下实现AUV的同步定位与建图的一种基于粒子滤波与图优化的海底地形同步定位与建图方法。
本发明的目的通过如下技术方案来实现：包括以下步骤：
步骤1：设定粒子数N、粒子闭环检测半径R、粒子匹配子地图Mp的测线数目Np、粒子轨迹子地图Mh的测线数目Nh、算法总步数T；初始化粒子集P(1:N)；对于任一粒子P(n)，其内部储存粒子当前状态
Figure BDA0002959029990000011
粒子自身历史轨迹
Figure BDA0002959029990000012
粒子权值wn＝1/N、粒子储存的闭环集合Ln；n为粒子序数；t为时间步长；
t0＝1时刻粒子集P(1:N)的状态
Figure BDA0002959029990000021
和自身历史轨迹根据GPS获得的先验状态
Figure BDA0002959029990000022
设定，
Figure BDA0002959029990000023
将t0＝1时刻以及对应的粒子位姿加入粒子集P(1:N)中各粒子储存的闭环集合Ln，
Figure BDA0002959029990000024
初始化t＝2；
步骤2：获取当前t时刻输入的对海底的深度测量值z(t)、里程计更新值v(t)以及里程计的轨迹DR(t)，更新粒子集P(1:N)中各粒子的当前状态
Figure BDA0002959029990000025
Figure BDA0002959029990000026
其中，ω为里程计的噪声；
步骤3：计算粒子集P(1:N)中各粒子的当前状态
Figure BDA0002959029990000027
与粒子自身历史轨迹
Figure BDA0002959029990000028
的最小欧氏距离rn；若rn＜R，则将
Figure BDA0002959029990000029
加入粒子P(n)储存的闭环集合Ln，执行步骤4进行地图更新；否则，忽略地图更新与观测更新且粒子P(n)不加入重采样步骤；其中，M为L中闭环的总数；
Figure BDA00029590299900000210
为第M个闭环所对应的开始时间；
Figure BDA00029590299900000211
为第M个闭环所对应的结束时间；
步骤4：以粒子集P(1:N)中粒子P(n)储存的闭环集合Ln中粒子P(n)的历史位姿为待优化变量，构建并求解位姿图，更新粒子P(n)的轨迹
Figure BDA00029590299900000212
后执行步骤5；
若粒子P(n)中含有M个闭环，则位姿图中有2M+1个节点、M个二元闭环边、2M-1个三元弱数据关联边；其中不同类型边的误差函数设置为：
闭环边：
Figure BDA00029590299900000213
弱数据关联边：
Figure BDA00029590299900000214
其中，m为粒子P(n)储存的闭环集合Ln中第m个闭环；tk为位姿图中第k个节点所对应的时间；
步骤5：重新对粒子集P(1:N)中各粒子进行闭环检测，计算粒子的当前状态
Figure BDA00029590299900000215
与粒子自身历史轨迹
Figure BDA00029590299900000216
的最小欧氏距离rn；若rn＜R，则记录该粒子最小欧氏距离rn对应的时刻tr，执行观测更新；
步骤5.1：生成粒子匹配子地图轨迹xp，并将xp与其对应的测深序列
Figure BDA0002959029990000031
结合生成粒子匹配子地图Mp；
Figure BDA0002959029990000032
步骤5.2：生成和粒子轨迹子地图轨迹xh，并将xh与其对应的测深序列
Figure BDA0002959029990000033
结合生成粒子轨迹子地图Mh；
Figure BDA0002959029990000034
步骤5.3：使用反距离加权算法对Mp进行插值，插值位置为Mh中测点的位置，获得包含L个共有有效测点的测深序列定义为zp，定义Mh中相同位置的有效测深序列为zh，计算粒子权值wn：
Figure BDA0002959029990000035
其中，
Figure BDA0002959029990000036
与
Figure BDA0002959029990000037
分别为zp与zh的第l个有效测点的测深值；σ2为传感器的方差；
步骤5.4：将
Figure BDA0002959029990000038
加入粒子自身历史轨迹
Figure BDA0002959029990000039
步骤6：获取执行观测更新的粒子总数Nth，计算有效粒子数Neff；若
Figure BDA00029590299900000313
则将所有经过观测更新的粒子进行重采样，执行步骤7；否则，直接执行步骤7；
Figure BDA00029590299900000311
其中，
Figure BDA00029590299900000312
为第i个粒子的归一化权值；
步骤7：根据所有粒子储存的历史轨迹，每隔ts个时刻取一帧为待优化节点，以每个粒子在此时刻储存的对应状态与待优化节点对应时刻状态的差为边，并与弱数据关联边构成位姿图，根据位姿图求解出轨迹Trj(1:t)，然后令t＝t+1，返回步骤2。
本发明还可以包括：
所述的步骤6中重采样的方法具体为：
步骤6.1：输入经过观测更新之后的粒子集PE，定义E为粒子集PE中粒子总数；将粒子集PE中粒子权值进行归一化，定义重采样后粒子集
Figure BDA0002959029990000044
步骤6.2：定义一个在(0,E-1)之间均匀分布的随机数Rad；定义临时粒子权值和为Ws，并将其初始化为Ws＝we＝1，e为PE中粒子序数；初始化序数m＝1；
步骤6.3：计算U＝Rad+(m-1)·E-1；若U＞Ws，则计算e＝e+1，Ws＝Ws+w(e)直到U≤Ws，转入步骤6.4；否则，直接转入步骤6.4；
步骤6.4：将第P(e)号粒子加入重采样后粒子集
Figure BDA0002959029990000041
令m＝m+1；
步骤6.5：若m＝E+1则算法结束并输出
Figure BDA0002959029990000042
否则，返回步骤6.3。
所述的步骤7的具体步骤为：
步骤7.1：输入粒子集P(1:N)，定义时间间隔ts，每隔ts个时刻取一轨迹点Trj(1+kts)为待优化节点，其中k为节点序数；定义K为最终位姿图中节点的总数；
步骤7.2：位姿图结构中共KN个一元边与K-2个弱数据关联边，弱数据关联边与步骤4中地图更新使用的位姿图中的弱数据关联边定义相同，一元边由
Figure BDA0002959029990000043
计算，tk为位姿图中第k个节点所对应的时刻，0＜k＜K，N与n分别为粒子总数与粒子的序号，0＜n≤N；
步骤7.3：求解位姿图，输出此时刻最优地图Trj(1:t)。
本发明的有益效果在于：
本发明是一种在子地图匹配粒子滤波BSLAM的基础上使用位姿图对粒子进行地图更新、以及在每一时刻结束后使用位姿图生成输出地图的BSLAM技术。本发明设计了一种使用位姿图结构对粒子自身储存轨迹进行更新，并使用位姿图在每一时刻结束时生成此时刻输出地图的粒子滤波BSLAM算法。本发明只需要在每一时刻输入里程计数据和由多波束声获得的地形测深，在没有先验地形图的情况下实现AUV的同步定位与建图。本发明所提出的图优化混合粒子滤波BSLAM算法相较于基于子地图匹配的粒子滤波BSLAM算法，在大累积误差下仍能实时准确的提供定位与建图结果。
附图说明
图1为本发明的整体流程框图。
图2为本发明中粒子地图更新位姿图结构示意图。
图3为本发明中观测更新流程框图。
图4为BSLAM最终地图生成算法所使用的位姿图结构示意图。
具体实施方式
下面结合附图对本发明做进一步描述。
本发明公开的是一种海底地形同步定位与建图方法，主要是一种在子地图匹配粒子滤波BSLAM的基础上使用位姿图对粒子进行地图更新、以及在每一时刻结束后使用位姿图生成输出地图的BSLAM技术。
本发明主要步骤包括：一是算法初始化：1)设定粒子数N、粒子闭环检测半径R、粒子匹配子地图(定义为Mp)的测线数目(定义为Np)、粒子轨迹子地图(定义为Mh)的测线数目(定义为Nh)、算法总步数T。2)初始化粒子集P(1:N)，对于任一粒子P(n)(n为粒子序数)，其内部储存粒子当前状态
Figure BDA0002959029990000051
粒子自身t＝1时刻到t-1时刻历史轨迹
Figure BDA0002959029990000052
(t为时间步长)、粒子权值wn＝1/N、粒子储存的闭环集合L。3)t＝1时刻，P(1:N)的状态和自身历史轨迹被初始化为由GPS获得的这一时刻的状态，将这一时刻以及对应的粒子位姿加入集合L，
Figure BDA0002959029990000053
t0＝1，转入步骤二。二是在每个t(t＞1)时刻输入对海底地形的深度测量值z(t)与里程计更新值v(t)以及里程计DR(t)后，对P(1:N)中所有粒子(以粒子n为例)进行：1)运动更新：
Figure BDA0002959029990000054
其中ω为里程计的噪声。2)闭环检测：计算
Figure BDA0002959029990000055
与
Figure BDA0002959029990000056
的最小欧氏距离r，若r＜R,将
Figure BDA0002959029990000057
加入闭环集合L(其中M为L中闭环的总数，
Figure BDA0002959029990000058
Figure BDA0002959029990000059
为这一闭环对应的时间)。然后进行粒子地图更新，否则忽略地图更新与观测更新且粒子n不加入重采样步骤。3)地图更新：根据闭环集合L构建位姿图，由此更新这一粒子储存的轨迹。4)观测更新：重新对粒子进行2)闭环检测，若r＜R,记录其对应的时刻tr。以
Figure BDA00029590299900000510
与里程计的轨迹
Figure BDA00029590299900000511
结合对应时刻观测
Figure BDA00029590299900000512
构成Mp；以
Figure BDA00029590299900000513
与里程计的轨迹
Figure BDA00029590299900000514
轨迹结合对应时刻观测
Figure BDA00029590299900000515
构成Mh，使用反距离加权算法对Mp在Mh相应位置进行插值，计算wn后将
Figure BDA00029590299900000516
加入粒子自身历史轨迹
Figure BDA00029590299900000517
转入步骤二进行下一个粒子的更新，否则直接转入步骤二进行下一个粒子的更新。三是当所有粒子更新完毕后，判断是否需要进行重采样，若是：将所有经过观测更新的粒子进行重采样，转入步骤四，否则直接转入步骤四。四是根据所有粒子储存的历史轨迹，构建位姿图结构生成算法此时刻输出的轨迹Trj(1:t)，当t＞T时，算法结束，否则t＝t+1，转入步骤二。
本发明涉及海底地形同步定位与建图领域，是一种基于粒子滤波与图优化理论的海底地形同步定位与建图方法。本发明的目的在于设计一种使用位姿图结构对粒子自身储存轨迹进行更新、并使用位姿图在每一时刻结束时生成此时刻算法输出地图的粒子滤波BSLAM算法。该方法只需要在每一时刻输入里程计数据和由多波束声获得的地形测深，在没有先验地形图的情况下实现AUV的同步定位与建图。本发明所提出的图优化混合粒子滤波BSLAM算法相较于基于子地图匹配的粒子滤波BSLAM算法，在大累积误差下仍能实时准确的提供定位与建图结果。
步骤一：算法初始化：1)设定粒子数N、粒子闭环检测半径R、粒子匹配子地图(定义为Mp)的测线数目(定义为Np)、粒子轨迹子地图(定义为Mh)的测线数目(定义为Nh)、算法总步数T。2)初始化粒子集P(1:N)，对于任一粒子P(n)(n为粒子序数)，其内部储存粒子当前状态
Figure BDA0002959029990000061
t＝1到t-1时刻的粒子自身历史轨迹
Figure BDA0002959029990000062
粒子权值被设定为默认值wn＝1/N(t为时间步长)、粒子储存的闭环集合L。3)t＝1时刻，P(1:N)的状态
Figure BDA0002959029990000063
和自身历史轨迹被初始化为由GPS获得的先验状态(GPSt＝1)，
Figure BDA0002959029990000064
t0＝1，将这一时刻以及对应的粒子位姿加入集合L，
Figure BDA0002959029990000065
转入步骤二；
步骤二：在每一时刻t(2≤t≤T)输入对海底的深度测量值z(t)与里程计更新值v(t)以及里程计DR(t)后，对P(1:N)中所有粒子(以粒子n为例)进行：
1)运动更新：使用含有噪声ω的里程计模型更新
Figure BDA0002959029990000066
Figure BDA0002959029990000067
2)闭环检测：计算
Figure BDA0002959029990000068
与粒子自身历史轨迹的最小欧氏距离r，若r＜R,将
Figure BDA0002959029990000069
加入闭环集合L(其中M为L中闭环的总数)
Figure BDA00029590299900000610
为第M个闭环所对应的时间。然后进行粒子地图更新，否则忽略地图更新与观测更新且粒子n不加入重采样步骤。
3)地图更新：以闭环集合L中的粒子历史位姿为待优化变量，构建位姿图，若有粒子中含有M个闭环，则位姿图中有2M+1个节点，M个二元闭环边，2M-1个三元弱数据关联边。其中不同类型边的误差函数被设置为：
闭环边：
Figure BDA00029590299900000611
弱数据关联边：
Figure BDA0002959029990000071
其中m为集合L中第m个闭环，tk为位姿图中第k个节点所对应的时间。求解这一位姿图，更新第n个粒子储存的轨迹
Figure BDA0002959029990000072
之后进行观测更新。
4)观测更新：重新对粒子进行闭环检测，若r＜R,记录其对应的时刻tr。以
Figure BDA0002959029990000073
与里程计的轨迹
Figure BDA0002959029990000074
结合对应时刻观测
Figure BDA0002959029990000075
构成Mp；以
Figure BDA0002959029990000076
与里程计的轨迹
Figure BDA0002959029990000077
轨迹结合对应时刻观测
Figure BDA0002959029990000078
构成Mh，使用反距离加权算法对Mp在Mh相应位置进行插值，并计算wn。之后，将
Figure BDA0002959029990000079
加入粒子自身历史轨迹
Figure BDA00029590299900000710
转入步骤二进行下一个粒子的更新，否则直接转入步骤二进行下一个粒子的更新；
步骤三：当t时刻所有粒子都进行了步骤二后，判断是否需要进行重采样。定义进行观测更新的粒子总数为Nth，定义有效粒子数：
Figure BDA00029590299900000711
其中
Figure BDA00029590299900000712
为第i个粒子的归一化权值。若
Figure BDA00029590299900000713
将所有经过观测更新的粒子进行重采样，转入步骤四，否则直接转入步骤四；
步骤四：根据所有粒子储存的历史轨迹，每隔ts个时刻取一帧为待优化节点，以每个粒子在此时刻储存的对应状态与待优化节点对应时刻状态的差为边，并与弱数据关联边构成位姿图,根据位姿图求解出轨迹Trj(1:t)。然后t＝t+1，转入步骤二。
步骤二中，观测更新算法包括如下步骤：
步骤2.1：输入粒子t、tr时刻状态
Figure BDA00029590299900000714
观测值序列z(1:t)，里程计轨迹DR(1:t)，转入步骤2.2；
步骤2.2：生成粒子匹配子地图轨迹：
Figure BDA00029590299900000715
并将xp与其对应的测深序列
Figure BDA00029590299900000716
结合生成粒子匹配子地图Mp；同样的，生成粒子轨迹子地图轨迹：
Figure BDA00029590299900000717
并将xh与其对应的测深序列
Figure BDA0002959029990000081
结合生成粒子轨迹子地图Mh，转入步骤2.3；
步骤2.3：使用反距离加权算法对Mp进行插值，插值位置为Mh中测点的位置，获得包含L个共有有效测点的测深序列定义为zp,定义Mh中相同位置的有效测深序列为zh。计算粒子权值：
Figure BDA0002959029990000082
其中：
Figure BDA0002959029990000083
与
Figure BDA0002959029990000084
分别为zp与zh的第l个有效测点的测深值；σ2为传感器的方差。算法结束。
步骤三中，低方差重采样算法包括以下步骤：
步骤3.1：输入经过观测更新之后的粒子集PE，定义E为PE的粒子总数。之后将PE中粒子权值进行归一化。定义重采样后粒子集
Figure BDA0002959029990000085
转入步骤3.2；
步骤3.2：定义一个在(0,E-1)之间均匀分布的随机数Rad；定义临时粒子权值和为Ws，并将其初始化为Ws＝we＝1，其中e为PE中粒子序数。初始化序数m＝1,转入步骤3.3；
步骤3.3：计算值U＝Rad+(m-1)·E-1，转入步骤3.4；
步骤3.4：若U＞Ws，则计算e＝e+1，Ws＝Ws+w(e)直到U≤Ws，转入步骤3.5，否则直接转入步骤3.5；
步骤3.5：将第P(e)号粒子加入重采样后粒子集
Figure BDA0002959029990000086
m＝m+1，若m＝E+1则算法结束并输出
Figure BDA0002959029990000087
否则转入步骤3.3。
步骤四中，BSLAM最优地图生成算法包括以下步骤：
步骤4.1：输入粒子集P(1:N)，定义时间间隔ts,每隔ts个时刻取一轨迹点Trj(1+kts)为待优化节点，其中k为节点序数。定义K为最终位姿图中节点的总数。转入步骤4.2；
步骤4.2：位姿图结构中共KN个一元边与K-2个弱数据关联边，其中弱数据关联边与权利要求1所述的步骤二中地图更新使用的位姿图中的弱数据关联边定义相同，一元边由
Figure BDA0002959029990000088
0＜k＜K 0＜n≤N计算，其中tk为位姿图中第k个节点所对应的时刻，N与n分别为粒子总数与粒子的序号。转入步骤4.3；
步骤4.3：求解这一位姿图，输出此时刻最优地图Trj(1:t)，算法结束。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种基于粒子滤波与图优化的海底地形同步定位与建图方法，其特征在于，包括以下步骤：
步骤1：设定粒子数N、粒子闭环检测半径R、粒子匹配子地图Mp的测线数目Np、粒子轨迹子地图Mh的测线数目Nh、算法总步数T；初始化粒子集P(1:N)；对于任一粒子P(n)，其内部储存粒子当前状态
Figure FDA0002959029980000011
粒子自身历史轨迹
Figure FDA0002959029980000012
粒子权值wn＝1/N、粒子储存的闭环集合Ln；n为粒子序数；t为时间步长；
t0＝1时刻粒子集P(1:N)的状态
Figure FDA0002959029980000013
和自身历史轨迹根据GPS获得的先验状态
Figure FDA0002959029980000014
设定，
Figure FDA0002959029980000015
将t0＝1时刻以及对应的粒子位姿加入粒子集P(1:N)中各粒子储存的闭环集合Ln，
Figure FDA0002959029980000016
初始化t＝2；
步骤2：获取当前t时刻输入的对海底的深度测量值z(t)、里程计更新值v(t)以及里程计的轨迹DR(t)，更新粒子集P(1:N)中各粒子的当前状态
Figure FDA0002959029980000017
Figure FDA0002959029980000018
其中，ω为里程计的噪声；
步骤3：计算粒子集P(1:N)中各粒子的当前状态
Figure FDA0002959029980000019
与粒子自身历史轨迹
Figure FDA00029590299800000110
的最小欧氏距离rn；若rn＜R，则将
Figure FDA00029590299800000111
加入粒子P(n)储存的闭环集合Ln，执行步骤4进行地图更新；否则，忽略地图更新与观测更新且粒子P(n)不加入重采样步骤；其中，M为L中闭环的总数；
Figure FDA00029590299800000112
为第M个闭环所对应的开始时间；
Figure FDA00029590299800000113
为第M个闭环所对应的结束时间；
步骤4：以粒子集P(1:N)中粒子P(n)储存的闭环集合Ln中粒子P(n)的历史位姿为待优化变量，构建并求解位姿图，更新粒子P(n)的轨迹
Figure FDA00029590299800000114
后执行步骤5；
若粒子P(n)中含有M个闭环，则位姿图中有2M+1个节点、M个二元闭环边、2M-1个三元弱数据关联边；其中不同类型边的误差函数设置为：
闭环边：
Figure FDA00029590299800000115
弱数据关联边：
Figure FDA0002959029980000021
其中，m为粒子P(n)储存的闭环集合Ln中第m个闭环；tk为位姿图中第k个节点所对应的时间；
步骤5：重新对粒子集P(1:N)中各粒子进行闭环检测，计算粒子的当前状态
Figure FDA0002959029980000022
与粒子自身历史轨迹
Figure FDA0002959029980000023
的最小欧氏距离rn；若rn＜R，则记录该粒子最小欧氏距离rn对应的时刻tr，执行观测更新；
步骤5.1：生成粒子匹配子地图轨迹xp，并将xp与其对应的测深序列
Figure FDA0002959029980000024
结合生成粒子匹配子地图Mp；
Figure FDA0002959029980000025
步骤5.2：生成和粒子轨迹子地图轨迹xh，并将xh与其对应的测深序列
Figure FDA0002959029980000026
结合生成粒子轨迹子地图Mh；
Figure FDA0002959029980000027
步骤5.3：使用反距离加权算法对Mp进行插值，插值位置为Mh中测点的位置，获得包含L个共有有效测点的测深序列定义为zp，定义Mh中相同位置的有效测深序列为zh，计算粒子权值wn：
Figure FDA0002959029980000028
其中，
Figure FDA0002959029980000029
与
Figure FDA00029590299800000210
分别为zp与zh的第l个有效测点的测深值；σ2为传感器的方差；
步骤5.4：将
Figure FDA00029590299800000211
加入粒子自身历史轨迹
Figure FDA00029590299800000212
步骤6：获取执行观测更新的粒子总数Nth，计算有效粒子数Neff；若
Figure FDA00029590299800000213
则将所有经过观测更新的粒子进行重采样，执行步骤7；否则，直接执行步骤7；
Figure FDA00029590299800000214
其中，
Figure FDA00029590299800000215
为第i个粒子的归一化权值；
步骤7：根据所有粒子储存的历史轨迹，每隔ts个时刻取一帧为待优化节点，以每个粒子在此时刻储存的对应状态与待优化节点对应时刻状态的差为边，并与弱数据关联边构成位姿图，根据位姿图求解出轨迹Trj(1:t)，然后令t＝t+1，返回步骤2。
2.根据权利要求1所述的一种基于粒子滤波与图优化的海底地形同步定位与建图方法，其特征在于：所述的步骤6中重采样的方法具体为：
步骤6.1：输入经过观测更新之后的粒子集PE，定义E为粒子集PE中粒子总数；将粒子集PE中粒子权值进行归一化，定义重采样后粒子集
Figure FDA0002959029980000031
步骤6.2：定义一个在(0,E-1)之间均匀分布的随机数Rad；定义临时粒子权值和为Ws，并将其初始化为Ws＝we＝1，e为PE中粒子序数；初始化序数m＝1；
步骤6.3：计算U＝Rad+(m-1)·E-1；若U＞Ws，则计算e＝e+1，Ws＝Ws+w(e)直到U≤Ws，转入步骤6.4；否则，直接转入步骤6.4；
步骤6.4：将第P(e)号粒子加入重采样后粒子集
Figure FDA0002959029980000032
令m＝m+1；
步骤6.5：若m＝E+1则算法结束并输出
Figure FDA0002959029980000033
否则，返回步骤6.3。
3.根据权利要求1或2所述的一种基于粒子滤波与图优化的海底地形同步定位与建图方法，其特征在于：所述的步骤7的具体步骤为：
步骤7.1：输入粒子集P(1:N)，定义时间间隔ts，每隔ts个时刻取一轨迹点
Figure FDA0002959029980000035
为待优化节点，其中k为节点序数；定义K为最终位姿图中节点的总数；
步骤7.2：位姿图结构中共KN个一元边与K-2个弱数据关联边，弱数据关联边与步骤4中地图更新使用的位姿图中的弱数据关联边定义相同，一元边由
Figure FDA0002959029980000034
计算，tk为位姿图中第k个节点所对应的时刻，0＜k＜K，N与n分别为粒子总数与粒子的序号，0＜n≤N；
步骤7.3：求解位姿图，输出此时刻最优地图Trj(1:t)。
CN202110233287.2A 2021-03-03 2021-03-03 基于粒子滤波与图优化的海底地形同步定位与建图方法 Active CN113008223B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110233287.2A CN113008223B (zh) 2021-03-03 2021-03-03 基于粒子滤波与图优化的海底地形同步定位与建图方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110233287.2A CN113008223B (zh) 2021-03-03 2021-03-03 基于粒子滤波与图优化的海底地形同步定位与建图方法

Publications (2)

Publication Number Publication Date
CN113008223A true CN113008223A (zh) 2021-06-22
CN113008223B CN113008223B (zh) 2022-10-28

Family

ID=76403220

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110233287.2A Active CN113008223B (zh) 2021-03-03 2021-03-03 基于粒子滤波与图优化的海底地形同步定位与建图方法

Country Status (1)

Country Link
CN (1) CN113008223B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114577210A (zh) * 2022-02-24 2022-06-03 哈尔滨工程大学 基于地图信息矩阵的跨区域检测算法

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20120022784A1 (en) * 2010-06-25 2012-01-26 Thales Navigation filter for a navigation system using terrain correlation
US20130006528A1 (en) * 2011-06-29 2013-01-03 Ixblue Navigation device and process integrating several hybrid inertial navigation systems
US20130016584A1 (en) * 2011-07-15 2013-01-17 Teledyne Scientific & Imaging Llc Methods and apparatus for obtaining sensor motion and position data from underwater acoustic signals
CA2926732A1 (en) * 2013-10-16 2015-04-23 Xtralis Technologies Ltd Aspirated particle detection with various flow modifications
WO2015073406A1 (en) * 2013-11-13 2015-05-21 Elwha Llc Dead reckoning system for vehicles
CN107589748A (zh) * 2017-08-21 2018-01-16 江苏科技大学 基于UnscentedFastSLAM算法的AUV自主导航方法
CN108562287A (zh) * 2018-01-08 2018-09-21 哈尔滨工程大学 一种基于自适应采样粒子滤波的水下地形辅助导航方法
US20180328738A1 (en) * 2017-05-15 2018-11-15 Fuji Xerox Co., Ltd. Systems and methods for utilizing graph based map information as priors for localization using particle filter
CN109186610A (zh) * 2018-10-15 2019-01-11 哈尔滨工程大学 一种auv地形匹配导航的鲁棒bslam方法
AU2018207854A1 (en) * 2017-01-13 2019-08-01 Carrosserie Hess Ag Method for predicting future driving conditions for a vehicle
CN110320520A (zh) * 2019-06-26 2019-10-11 哈尔滨工程大学 一种测深信息同步定位与建图的鲁棒后端图优化方法
CN110726415A (zh) * 2019-10-21 2020-01-24 哈尔滨工程大学 一种自适应的水下多波束同步定位与建图方法
WO2020155543A1 (zh) * 2019-02-01 2020-08-06 广州小鹏汽车科技有限公司 一种slam地图拼接方法及系统

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20120022784A1 (en) * 2010-06-25 2012-01-26 Thales Navigation filter for a navigation system using terrain correlation
US20130006528A1 (en) * 2011-06-29 2013-01-03 Ixblue Navigation device and process integrating several hybrid inertial navigation systems
US20130016584A1 (en) * 2011-07-15 2013-01-17 Teledyne Scientific & Imaging Llc Methods and apparatus for obtaining sensor motion and position data from underwater acoustic signals
CA2926732A1 (en) * 2013-10-16 2015-04-23 Xtralis Technologies Ltd Aspirated particle detection with various flow modifications
WO2015073406A1 (en) * 2013-11-13 2015-05-21 Elwha Llc Dead reckoning system for vehicles
AU2018207854A1 (en) * 2017-01-13 2019-08-01 Carrosserie Hess Ag Method for predicting future driving conditions for a vehicle
US20180328738A1 (en) * 2017-05-15 2018-11-15 Fuji Xerox Co., Ltd. Systems and methods for utilizing graph based map information as priors for localization using particle filter
CN107589748A (zh) * 2017-08-21 2018-01-16 江苏科技大学 基于UnscentedFastSLAM算法的AUV自主导航方法
CN108562287A (zh) * 2018-01-08 2018-09-21 哈尔滨工程大学 一种基于自适应采样粒子滤波的水下地形辅助导航方法
CN109186610A (zh) * 2018-10-15 2019-01-11 哈尔滨工程大学 一种auv地形匹配导航的鲁棒bslam方法
WO2020155543A1 (zh) * 2019-02-01 2020-08-06 广州小鹏汽车科技有限公司 一种slam地图拼接方法及系统
CN110320520A (zh) * 2019-06-26 2019-10-11 哈尔滨工程大学 一种测深信息同步定位与建图的鲁棒后端图优化方法
CN110726415A (zh) * 2019-10-21 2020-01-24 哈尔滨工程大学 一种自适应的水下多波束同步定位与建图方法

Non-Patent Citations (8)

* Cited by examiner, † Cited by third party
Title
M. RAHMATI等: ""SLAM-based underwater adaptive sampling using autonomous vehicles"", 《PROC. MTS/IEEE OCEANS, CHARLESTON, SC, USA》 *
MA, TENG等: ""AUV robust bathymetric simultaneous localization and mapping"", 《OCEAN ENGINEERING》 *
NORGREN等: ""A Multibeam-Based SLAM Algorithm for Iceberg Mapping Using AUVs"", 《IEEE ACCESS》 *
STEPHEN BARKBY等: ""Bathymetric particle filter SLAM using trajectory maps"", 《INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH》 *
丛正: ""基于多波束测深的同时定位与建图研究 "", 《万方数据库》 *
陈小龙: ""AUV水下地形匹配辅助导航技术研究"", 《中国优秀博硕士学位论文全文数据库(博士)工程科技Ⅱ辑》 *
鞠纯纯，等: ""基于粒子滤波器的 SLAM 的仿真研究"", 《系统仿真学报》 *
韩月，等: ""基于改进粒子滤波的 AUV 海底地形辅助定位方法"", 《智能系统学报》 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114577210A (zh) * 2022-02-24 2022-06-03 哈尔滨工程大学 基于地图信息矩阵的跨区域检测算法

Also Published As

Publication number Publication date
CN113008223B (zh) 2022-10-28

Similar Documents

Publication Publication Date Title
CN108803321B (zh) 基于深度强化学习的自主水下航行器轨迹跟踪控制方法
Li et al. Robust time-varying formation control for underactuated autonomous underwater vehicles with disturbances under input saturation
CN109459040B (zh) 基于rbf神经网络辅助容积卡尔曼滤波的多auv协同定位方法
CN109241552B (zh) 一种基于多约束目标的水下机器人运动规划方法
CN110181508B (zh) 水下机器人三维航路规划方法及系统
CN114199248B (zh) 一种基于混合元启发算法优化anfis的auv协同定位方法
CN110220510B (zh) 一种考虑地图准确性的水下机器人海底地形匹配导航路径规划方法
CN113052372A (zh) 一种基于深度强化学习的动态auv追踪路径规划方法
CN110716575A (zh) 基于深度双q网络强化学习的uuv实时避碰规划方法
CN114397899A (zh) 一种仿生机器鱼三维路径跟踪控制方法及装置
CN113008223B (zh) 基于粒子滤波与图优化的海底地形同步定位与建图方法
CN113391633A (zh) 一种面向城市环境的移动机器人融合路径规划方法
Kramer et al. Robust small robot localization from highly uncertain sensors
Yan et al. A novel fastslam algorithm based on iterated unscented kalman filter
CN116466701A (zh) 一种无人水面艇能耗最小轨迹规划方法
CN116243717A (zh) 考虑海流不确定性的水下机器人时间最优路径规划方法
CN110481561A (zh) 无人驾驶车辆自动控制信号生成方法和装置
Efe et al. Adaptive Kalman filters for manoeuvring target tracking
CN112732854B (zh) 一种粒子滤波bslam方法
CN112904719B (zh) 一种适用于水下机器人位置环形区域跟踪控制方法
CN115562258A (zh) 基于神经网络的机器人社会自适应路径规划方法及系统
CN111221340B (zh) 一种基于粗粒度特征的可迁移视觉导航设计方法
CN116578102B (zh) 水下自主航行器避障方法、装置、计算机设备和存储介质
CN114637197A (zh) 基于自学习的船舶协同控制方法
Rhoads et al. Efficient guidance in finite time flow fields

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