CN110726415B - 一种自适应的水下多波束同步定位与建图方法 - Google Patents

Abstract

本发明属于海底地形同步定位与建图领域，具体涉及一种自适应的水下多波束同步定位与建图方法。本发明采用反距离加权方法实现在不提取环境特征的情况下进行数据关联并通过库尔贝克‑莱布勒散度采样实现粒子数实时调整的基于粒子滤波的AUV测深数据同步定位与建图技术。本发明通过粒子分地图与均值轨迹分地图匹配实现不需要提取特征情况下的数据关联，通过库尔贝克—莱布勒散度实时控制粒子数，同时采用地形丰富度对粒子权值进行修正，提高了算法的鲁棒性。本发明不需要在海底地形中提取特征，可以实时控制粒子数，同时能够保证建图精度和实时性，适用性好，计算开销小，可以保证实时性。

Description

一种自适应的水下多波束同步定位与建图方法

技术领域

本发明属于海底地形同步定位与建图领域，具体涉及一种自适应的水下多波束同步定位与建图方法。

背景技术

同步定位与建图(Simultaneous Localization and Mapping,SLAM)提供了一种AUV在没有先验地图的情况下实现全海域航行的方法。粒子滤波是进行水下多波束声纳SLAM的重要手段之一。传统粒子滤波方法由于固定了粒子数目。计算开销较大。除此之外，海底地形趋于平缓，典型的粒子滤波SLAM框架如FastSLAM需要识别海洋环境中尺度独立的特征，导致适用性较差。因此，需要一种可以在不需要对海底特征识别和建模的情况下，能够实时控制粒子数的粒子滤波SLAM方法。

目前针对自适应的水下多波束同步定位与建图没有相关方法，较为相似的是使用其它传感器和算法来实现水下SLAM的方法。

公开日为2019年1月11日，公开号为CN109186610A，发明名称为“一种AUV地形匹配导航的鲁棒BSLAM方法”的专利申请，该方法以多波束声纳为传感器，使用图优化算法进行水下SLAM，计算开销较大。

公开日为2019年3月29日，公开号为CN109541606A，发明名称为“单水声信标测距辅助的水下同步定位与构图方法”的专利申请，该方法以单水声信标测距辅助来进行水下SLAM，需要提前布置水声信标，适用性较差。

发明内容

本发明的目的在于提供在不需要对海底特征识别和建模的情况下，能够实时控制粒子数，同时保证建图精度和实时性的一种自适应的水下多波束同步定位与建图方法。

本发明的目的通过如下技术方案来实现：包括以下步骤：

步骤一：初始粒子数M₍₀₎、粒子集P₍₀₎和粒子权值w₍₀₎；根据GPS数据初始化粒子集均值轨迹mean₍₀₎；设定算法总步数T和检测半径R，设定当前算法步数t＝1；

步骤二：若t＝1，根据里程计对t时刻粒子集P_(t)进行运动更新，得到t时刻粒子均值轨迹mean_(t)，并令t＝t+1后返回步骤二；若1<t<T，构建t-1时刻临时粒子集P_(t-1)，初始化粒子标号i＝0，初始化t时刻粒子数M_(t)＝0，初始化空直方图与非空位数k＝1；若t>T，则判定完成建图，结束计算；

步骤三：令M_(t)＝M_(t)+1，i＝i+1，判断M_(t)与t-1时刻的粒子集数目M_(t-1)的大小关系，若M_(t)≤M_(t-1)，则按下式对第i号粒子进行运动更新：

其中，为第i号粒子于t时刻的位姿；u_t为t时刻控制；ε_t-1为t-1时刻误差；若M_(t)＞M_(t-1)，则从P_(t-1)中随机抽取一粒子作为第i号粒子/>进行运动更新；

步骤四：判定第i号粒子是否进入回环范围；若第i号粒子/>进入回环范围，则对第i号粒子/>进行加权；若第i号粒子/>未进入回环范围，则第i号粒子/>权值不变；

步骤五：判断第i号粒子是否落入直方图空位；计算第i号粒子/>距其他M_(t)-1个粒子的欧式距离/>若第i号粒子/>与某一粒子的距离在界限D内，则判定第i号粒子/>未落入空位，保持k不变；若第i号粒子/>与所有粒子的距离/>均大于界限D，则判定第i号粒子/>落入空位，并令k＝k+1；

步骤六：计算界限粒子数M_x；

步骤六：若M_(t)<M_x，则返回步骤三；若M_(t)＝M_x，则保留t时刻的M_(t)值；

步骤七：判断是否进行重采样；若进行重采样，则修正历史均值轨迹地图并更新历史均值轨迹；若不进行重采样，则只更新历史均值轨迹，不进行修正；

步骤八：t＝t+1，回到步骤二。

本发明还可以包括：

所述的步骤六中计算界限粒子数M_x的方法具体为：

其中，z_1-δ和ε均为固有参数。

所述的步骤七中判断是否进行重采样的方法具体为：判定N_eff与N_th的大小：

其中，N_eff为重采样判据；N_th为重采样界限；为第i号粒子权值；若N_eff＜N_th，则判定进行重采样。

所述的步骤七中若进行重采样，则修正历史均值轨迹地图并更新历史均值轨迹的方法具体为：进行重采样后，粒子均值位置随重采样发生突变，此时刻移动向量move_(t)，线性修正回环时刻t_(resample)至t时刻的均值轨迹并更新历史均值轨迹mean_(1:t)；其中t时刻均值轨迹的移动向量move_(t)的具体公式为：

其中，move_(t)为t时刻均值轨迹的移动向量；t_(resample)为与t时刻对应的历史轨迹回环时刻；t_(s)∈[t_(resample),t]，s为标记，表示属于此回环内的时刻。

所述的步骤四中若第i号粒子进入回环范围，则对第i号粒子/>进行加权的方法具体为：

步骤4.1：输入第i号粒子的位姿x_(i)与历史均值轨迹mean_(1:t-1)，计算x_(i)距所有历史均值轨迹点的距离X_(1:t-1)；

步骤4.2：提取满足条件D<R的mean_(1:t-1)的子集m,记录子集m所对应所有时刻的均值t_(resample)，以m的位置为基准，结合其相应时刻的观测，构成均值轨迹分地图mmap；其中t_(resample)向下取整；

步骤4.3：以x_(i)为基准，将t-n的均值轨迹移动到x_(i)，结合t-n时刻至t时刻的观测，构成粒子分地图pmap；其中n为自定义的测线数目；

步骤4.4：分别计算mmap与pmap的地形丰富度α_(mmap)与α_(pmap)；

其中，map为地图；为测量误差；/>为地图方差；

步骤4.5：以mmap为基础，使用反距离加权算法插值出其在pmap中测点位置的估计值，根据mmap与pmap同一测点v的估计值与真实值，按下式为第i号粒子加权；

其中，为第v点的估计值；/>为第v点的真实值；/>为估计方差；/>为观测方差；V为测点总数。

本发明的有益效果在于：

本发明提出一种基于库尔贝克—莱布勒散度(Kullback-Leibler Divergence,KLD)采样的自适应的水下多波束同步定位与建图方法。本发明采用反距离加权方法实现在不提取环境特征的情况下进行数据关联并通过库尔贝克-莱布勒散度采样实现粒子数实时调整的基于粒子滤波的AUV测深数据同步定位与建图技术。本发明通过粒子分地图与均值轨迹分地图匹配实现不需要提取特征情况下的数据关联，通过库尔贝克—莱布勒散度实时控制粒子数，同时采用地形丰富度对粒子权值进行修正，提高了算法的鲁棒性。本发明不需要在海底地形中提取特征，可以实时控制粒子数，同时能够保证建图精度和实时性，适用性好，计算开销小，可以保证实时性。

附图说明

图1是本发明的总体流程图。

图2是本发明中粒子回环检测与粒子加权流程图。

具体实施方式

下面结合附图对本发明做进一步描述。

结合图1，本发明总体包括如下步骤，其中，通过步骤二、三、五、六实现了算法的自适应，步骤四实现了算法在不进行特征提取的情况下进行数据关联：

步骤一：一是设定初始粒子数M₍₀₎，初始化粒子集P₍₀₎，初始化粒子集均值轨迹mean₍₀₎，设定算法总步数T，设定检测半径R,初始化粒子权值w₍₀₎，设定当前算法步数t＝1，进入步骤二；

步骤二：判断t与T的关系。若t＝1，根据里程计对t时刻粒子集P_(t)进行运动更新,更新t时刻粒子均值轨迹mean_(t)，t＝t+1，转入步骤二，若1<t<T，则构建t-1时刻临时粒子集P_(t-1)，初始化粒子标号i＝0,t时刻粒子数M_(t)＝0，初始化空直方图与非空位数k＝1，进入步骤三，若t>T，则算法结束；

步骤三：M_(t)＝M_(t)+1,i＝i+1，判断M_(t)与t-1时刻的粒子集数目M_(t-1)的大小关系，若M_(t)≤M_(t-1)，则使用公式

其中：第i号粒子于t时刻的位姿

u_t：t时刻控制

ε_t-1：t-1时刻误差

对第i号粒子进行运动更新，否则从P_(t-1)中随机抽取一粒子作为第i号粒子/>进行运动更新，进入步骤四；

步骤四：判定是否进入回环范围，若是则对/>进行加权，否则该粒子权值不变，进入步骤五；

步骤五：计算是否落入直方图空位，即计算/>距其他M_(t)-1个粒子的欧式距离若与某一粒子的距离在界限D内，认为该粒子未落入空位，若/>与所有粒子的距离均大于D，则该粒子落入空位。若是，k＝k+1，否则k不变，若k>1，使用公式

其中：z_1-δ、ε:固有参数

计算界限粒子数Mx，进入步骤六；

步骤六：比较M_(t)与Mx大小，若M_(t)<Mx,则转入步骤三，否则进入步骤七；

步骤七：使用公式

其中：N_eff：重采样判据

N_th：重采样界限

第i号粒子权值

判定N_eff与N_th的大小，判断是否满足N_eff＜N_th条件，若满足，则进行重采样，粒子均值位置随重采样发生突变，此时刻移动向量move_(t)，根据公式

其中：t_(resample)：与t时刻对应的历史轨迹回环时刻

t_(s)∈[t_(resample),t]

s:标记，表示属于此回环内的时刻

move_(t)：t时刻均值轨迹的移动向量

线性修正回环时刻t_(resample)至t时刻的均值轨迹并更新历史均值轨迹mean_(1:t)，t＝t+1，转入步骤二，否则只更新历史均值轨迹mean_(1:t)但不进行修正，t＝t+1，转入步骤二。

结合图2，步骤四中粒子回环检测与粒子加权具体步骤如下，其中，步骤4.2与4.3实现了算法在不依赖特征提取的情况下直接应用原始数据进行数据关联，步骤4.4实现了算法根据地图方差实现粒子权值的调整：

步骤4.1：输入第i号粒子的位姿x_(i)与历史均值轨迹mean_(1:t-1)，计算x_(i)距所有历史均值轨迹点的距离X_(1:t-1)，进入步骤4.2；

步骤4.2：提取满足条件D<R的mean_(1:t-1)的子集m,记录子集m所对应所有时刻的均值(向下取整)t_(resample)，以m的位置为基准，结合其相应时刻的观测，构成均值轨迹分地图mmap，进入步骤4.3；

步骤4.3：以x_(i)为基准，将t-n(其中n为自定义的测线数目)的均值轨迹移动到x_(i)，结合t-n时刻至t时刻的观测，构成粒子分地图pmap，进入步骤4.4；

步骤4.4：使用公式

其中：map：地图

测量误差

地图方差

分别计算mmap与pmap的地形丰富度α_(mmap)与α_(pmap)，进入步骤4.5；

步骤4.5：以mmap为基础，使用反距离加权算法插值出其在pmap中测点位置的估计值，根据mmap与pmap同一测点v的估计值与真实值，使用公式

其中：第v点的估计值

第v点的真实值

估计方差

观测方差

V:测点总数

为第i号粒子赋值。

本发明公开的是一种自适应的水下多波束同步定位与建图方法，主要是一种采用反距离加权方法实现在不提取环境特征的情况下进行数据关联并通过库尔贝克-莱布勒散度采样实现粒子数实时调整的基于粒子滤波的AUV测深数据同步定位与建图技术。本发明通过粒子分地图与均值轨迹分地图匹配实现不需要提取特征情况下的数据关联，通过库尔贝克—莱布勒散度实时控制粒子数，同时采用地形丰富度对粒子权值进行修正，提高了算法的鲁棒性。主要步骤包括：一是设定初始粒子数M₍₀₎，初始化时间t＝0时刻粒子集P₍₀₎，初始化粒子集均值轨迹mean₍₀₎，设定算法总步数T，设定检测半径R,设定当前算法步数t＝1，进入步骤二；二是判断t与T的大小关系，若t＝1，则对t时刻粒子集P_(t)进行简单更新，更新t时刻粒子均值轨迹mean_(1:t)，t＝t+1，转入步骤二，若1<t<T，则构建t-1时刻临时粒子集P_(t-1)，初始化粒子标号i＝0,t时刻粒子数M_(t)＝0，初始化空直方图与非空位数k＝1，进入步骤三，若t>T，则算法结束；三是M_(t)＝M_(t)+1,i＝i+1，判断M_(t)与t-1时刻的粒子数M_(t-1)的大小关系，若有M_(t)≤M_(t-1)，则直接对第i号粒子进行运动更新，否则从上一时刻临时粒子集中随机抽取一粒子作为第i号粒子/>进行运动更新，进入步骤四；四是判定/>是否进入回环范围，若是则对/>进行加权，否则粒子权值不变，进入步骤五；五是计算/>是否落入直方图空位，若是，k＝k+1，否则k不变，若k>1，计算界限粒子数Mx，进入步骤六；六是比较M_(t)与Mx大小，若M_(t)<Mx,则转入步骤三，否则进入步骤七；七是判断算法是否满足重采样条件，若满足，则进行重采样并修正和更新历史均值轨迹，t＝t+1，转入步骤二，否则更新历史均值轨迹但不进行修正，t＝t+1，转入步骤二。其中，步骤二、三、五、六实现了算法的自适应，步骤四实现了算法在不进行特征提取的情况下进行数据关联的过程。

本发明的有益效果在于提出的自适应的水下多波束同步定位与建图方法不需要在海底地形中提取特征，适用性好，计算开销小，可以保证实时性。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种自适应的水下多波束同步定位与建图方法，其特征在于，包括以下步骤：

步骤一：初始粒子数M₍₀₎、粒子集P₍₀₎和粒子权值w₍₀₎；根据GPS数据初始化粒子集均值轨迹mean₍₀₎；设定算法总步数T和检测半径R，设定当前算法步数t＝1；

步骤二：若t＝1，根据里程计对t时刻粒子集P_(t)进行运动更新，得到t时刻粒子均值轨迹mean_(t)，并令t＝t+1后返回步骤二；若1<t<T，构建t-1时刻临时粒子集P_(t-1)，初始化粒子标号i＝0，初始化t时刻粒子数M_(t)＝0，初始化空直方图与非空位数k＝1；若t>T，则判定完成建图，结束计算；

步骤三：令M_(t)＝M_(t)+1，i＝i+1，判断M_(t)与t-1时刻的粒子集数目M_(t-1)的大小关系，若M_(t)≤M_(t-1)，则按下式对第i号粒子进行运动更新：

其中，为第i号粒子于t时刻的位姿；u_t为t时刻控制；ε_t-1为t-1时刻误差；若M_(t)＞M_(t-1)，则从P_(t-1)中随机抽取一粒子作为第i号粒子/>进行运动更新；

步骤四：判定第i号粒子是否进入回环范围；若第i号粒子/>进入回环范围，则对第i号粒子/>进行加权；若第i号粒子/>未进入回环范围，则第i号粒子/>权值不变；

步骤4.1：输入第i号粒子的位姿x_(i)与历史均值轨迹mean_(1:t-1)，计算x_(i)距所有历史均值轨迹点的距离X_(1:t-1)；

步骤4.2：提取满足条件D<R的mean_(1:t-1)的子集m,记录子集m所对应所有时刻的均值t_(resample)，以m的位置为基准，结合其相应时刻的观测，构成均值轨迹分地图mmap；其中t_(resample)向下取整；

步骤4.3：以x_(i)为基准，将t-n的均值轨迹移动到x_(i)，结合t-n时刻至t时刻的观测，构成粒子分地图pmap；其中n为自定义的测线数目；

步骤4.4：分别计算mmap与pmap的地形丰富度α_(mmap)与α_(pmap)；

其中，map为地图；为测量误差；/>为地图方差；

步骤4.5：以mmap为基础，使用反距离加权算法插值出其在pmap中测点v位置的估计值，根据mmap与pmap同一测点的估计值与真实值，按下式为第i号粒子加权；

其中，为第v点的估计值；/>为第v点的真实值；/>为估计方差；/>为观测方差；V为测点总数；

步骤五：判断第i号粒子是否落入直方图空位；计算第i号粒子/>距其他M_(t)-1个粒子的欧式距离/>若第i号粒子/>与某一粒子的距离在界限D内，则判定第i号粒子/>未落入空位，保持k不变；若第i号粒子/>与所有粒子的距离/>均大于界限D，则判定第i号粒子/>落入空位，并令k＝k+1；

步骤六：计算界限粒子数M_x；

步骤六：若M_(t)<M_x，则返回步骤三；若M_(t)＝M_x，则保留t时刻的M_(t)值；

步骤七：判断是否进行重采样；若进行重采样，则修正历史均值轨迹地图并更新历史均值轨迹；若不进行重采样，则只更新历史均值轨迹，不进行修正；

步骤八：t＝t+1，回到步骤二。

2.根据权利要求1所述的一种自适应的水下多波束同步定位与建图方法，其特征在于：所述的步骤六中计算界限粒子数M_x的方法具体为：

其中，z_1-δ和ε均为固有参数。

3.根据权利要求1或2所述的一种自适应的水下多波束同步定位与建图方法，其特征在于：所述的步骤七中判断是否进行重采样的方法具体为：判定N_eff与N_th的大小：

其中，N_eff为重采样判据；N_th为重采样界限；为第i号粒子权值；若N_eff＜N_th，则判定进行重采样。

4.根据权利要求1或2所述的一种自适应的水下多波束同步定位与建图方法，其特征在于：所述的步骤七中若进行重采样，则修正历史均值轨迹地图并更新历史均值轨迹的方法具体为：进行重采样后，粒子均值位置随重采样发生突变，此时刻移动向量move_(t)，线性修正回环时刻t_(resample)至t时刻的均值轨迹并更新历史均值轨迹mean_(1:t)；其中t时刻均值轨迹的移动向量move_(t)的具体公式为：

其中，move_(t)为t时刻均值轨迹的移动向量；t_(resample)为与t时刻对应的历史轨迹回环时刻；t_(s)∈[t_(resample),t]，s为标记，表示属于此回环内的时刻。

5.根据权利要求3所述的一种自适应的水下多波束同步定位与建图方法，其特征在于：所述的步骤七中若进行重采样，则修正历史均值轨迹地图并更新历史均值轨迹的方法具体为：进行重采样后，粒子均值位置随重采样发生突变，此时刻移动向量move_(t)，线性修正回环时刻t_(resample)至t时刻的均值轨迹并更新历史均值轨迹mean_(1:t)；其中t时刻均值轨迹的移动向量move_(t)的具体公式为：

其中，move_(t)为t时刻均值轨迹的移动向量；t_(resample)为与t时刻对应的历史轨迹回环时刻；t_(s)∈[t_(resample),t]，s为标记，表示属于此回环内的时刻。