CN106403953B

CN106403953B - 一种用于水下自主导航与定位的方法

Info

Publication number: CN106403953B
Application number: CN201610802019.7A
Authority: CN
Inventors: 章飞; 史剑鸣; 孙陶莹; 曾庆军
Original assignee: Jiangsu University of Science and Technology
Current assignee: Jiangsu University of Science and Technology
Priority date: 2016-09-05
Filing date: 2016-09-05
Publication date: 2019-07-16
Anticipated expiration: 2036-09-05
Also published as: CN106403953A

Abstract

本发明是一种用于水下自主导航与定位的方法，属于水下机器人自主导航领域，该方法使用随机有限集方法对SLAM问题进行建模，能更准确的描述地图特征信息、地图特征观测信息、杂波等因素。在地图特征估计方面，使用混合新生地图信息方法改善了预测阶段PHD滤波器先验信息的不足，改进了传统概率假设密度‑同步定位与地图创建(PHD‑SLAM)方法对地图特征数目及地图特征位置的估计精度。在机器人位姿估计方面，使用空地图特征策略，提高了机器人位姿估计的计算速度。

Description

一种用于水下自主导航与定位的方法

技术领域

本发明属于水下机器人自主导航领域，特别涉及一种用于水下自主导航与定位的方法。

背景技术

水下自主导航与定位主要是通过机器人实现，目前机器人中均采用的传统SLAM(即时定位与地图构建，下文简称SLAM)方法在密集杂波和特征数目多且变化剧烈的水下环境中存在数据关联精度低、计算复杂的问题，且在SLAM实现过程中，由于机器人运动过程中的误差、传感器的观测误差、以及传感器不确定性造成地图特征漏检等问题，都将影响数据关联过程中的准确性，所以传统SLAM方法的准确性对地图特征信息的数据关联非常敏感。

基于随机有限集合理论的概率假设密度SLAM方法是一种在估计地图特征时不依赖数据关联，并将传统SLAM方法中所忽略的由于传感器不确定性造成的地图特征漏检概率带入地图特征滤波的SLAM新方法，非常适用于数据关联模糊、目标特征多且变化剧烈的水下环境中。但由于概率假设密度(Probability Hypothesis Density,下文简称PHD)算法的特性，需要提供一定先验信息才能递归进行滤波运算，而SLAM过程是在未知环境中从未知位置进行运动，无法提供先验信息，为此传统方法是将移动机器人上一时刻的观测集合作为当前时刻的先验信息集合，带入PHD滤波当中从而估计出新出现地图特征的位置。然而，由于新生目标集合仅仅采用上一时刻的观测集合，因此在地图特征位置及数目的估计精度上存在不足。

发明内容

发明目的：为了克服现有传统SLAM方法在密集杂波和特征数目多且变化剧烈的水下环境中存在数据关联精度低、计算复杂的不足以及现有随机有限集SLAM方法中先验信息不足的问题，本发明提供了一种数据关联精度高，计算简单的用于水下自主导航与定位的方法。

技术方案：本发明提供了一种用于水下自主导航与定位的方法，包括以下步骤：

步骤1：将机器人反馈的每一时刻地图特征信息、地图特征观测信息以及传感器不确定造成的漏检概率都建模为随机有限集形式；

步骤2：使用Rao-Blackwellised粒子滤波器进行机器人位姿估计得到机器人位姿先验估计的粒子集合；

步骤3：根据步骤2中获得的机器人先验估计的粒子集合对每个粒子所对应的机器人位姿进行地图估计，完成对地图信息的预测和更新；

步骤4：根据步骤3获得的PHD滤波器对每个粒子地图估计结果调整粒子权重,得到调整过后的粒子集后，通过采样粒子的加权平均获得k时刻的机器人位姿，从而实现导航和定位。

进一步，所述步骤3中对每个粒子所对应的机器人位姿进行地图估计的方法为：包括以下步骤：

步骤302：更新阶段，根据公式：

步骤303：对更新阶段PHD滤波器的结果进行剪切与合并。这样有效提高了对地图特征点位置和数目的估计精度及地图滤波运算速度。

进一步，所述步骤4中调整每个粒子权重大小的方法为对粒子的重要性采样，对每个粒子对应的地图特征进行滤波，将得到的结果与机器人采集到的地图特征进行比较，如果比较的结果误差越小，就表示粒子与机器人的位置越准确，从而增加对应粒子的权重；如果相反，就减小对应粒子的权重。这样得到的定位更加的精准。

进一步，所述步骤4中还包括粒子重采样。这样有效解决了粒子退化的问题。

有益效果：与现有技术相比，本发明使用随机有限集方法建模实现SLAM方法，解决了传统SLAM方法在密集杂波和特征数目多且变化剧烈的水下环境中存在数据关联精度低、计算复杂的问题。使用了混合新生地图信息方法，增加了地图预测更新过程中PHD滤波器的先验信息，当机器人重新回到已探测过的环境中时，通过增加先验信息，改进PHD-SLAM方法对地图特征数目估计及地图特征位置精度估计。使用空地图特征策略提高机器人位姿后验估计的运算速度。从而使SLAM方法能够在更复杂的环境中得到应用。

附图说明

图1为本发明工作流程图；

图2为本发明中混合新生地图信息预测阶段流程图；

图3为本发明中提供的实施例的具体实施方式流程图。

具体实施方式

下面结合附图对本发明做更进一步的解释。

如图1所示，本发明公开的一种用于水下自主导航与定位的方法，主要包括以下步骤：

步骤1、将机器人反馈的k时刻地图特征信息、地图特征观测信息以及传感器不确定造成的漏检概率都建模为随机有限集形式。地图特征集合的数目从零到任意大小随意变化，随着在机器人不断出现在机器人视域内的地图特征而单调增加，每个地图特征之间是相互独立的。

M＝φ(没有地图特征)

M＝{m¹}(一个状态为m¹的地图特征)

M＝{m¹,m²}(两个地图特征m¹≠m²)

M＝{m¹,...,mⁿ}(n个地图特征m¹≠...≠mⁿ)

SLAM过程中地图特征观测信息集合不仅包括实际地图特征还包括杂波、虚警，因此观测集合的数目同样是从零到任意大小随意变化。每个观测信息包含传感器获得地图特征的距离及角度。

Z＝φ(没有地图特征被探测)

Z＝{z¹}(一个状态为z¹被探测)

Z＝{z¹,z²}(两个地图特征被探测)

(个地图特征被探测)

步骤2、在随机有限集模型下，将k时刻SLAM联合后验概率估计转换为两个“独立”的估计过程，即机器人位姿估计，以及该状态估计条件下的环境地图估计。机器人位姿估计使用RB粒子滤波器进行位姿估计，根据机器人速度v，k-1时刻的控制量U_k-1，前一状态位置X_k-1，及机器人运动方程得到机器人先验估计

步骤3、得到每个粒子的先验估计后，对每个粒子所对应的机器人位姿进行地图估计，完成对地图信息的预测和更新。地图估计包括三个阶段，1.地图预测阶段，2.更新阶段，3.PHD滤波器剪切与合并。

(1)地图预测阶段：

V_k|k-1(m|X_k)＝V_k-1|k-1(m|X_k-1)+b(m|X_k) (1)

在估计地图特征目标数目以及地图特征精度中，地图预测阶段起重要作用，本发明使用的混合新生目标信息的方法，增加预测阶段中的新生地图特征RFS的PHD。如图2所示，当机器人从初始时刻出发时由于缺乏先验信息，使用k-1时刻的观测信息集合作为k时刻可能出现新地图特征信息。当机器人经过PHD-SLAM更新出已探测区域地图信息后，将已探测地图信息中靠近机器人位姿的地图特征作为先验信息，加入上一时刻观测集合中一起作为当前时刻的b(m|X_k)带入当前时刻的PHD预测阶段。

b(m|X_k)＝Z_k-1+M_f (2)

式中，Z_k-1表示k-1时刻的地图特征观测集合，如果当前时刻为k时刻，k-1则表示上一时刻；M_f表示已探测地图中靠近机器人位姿的地图特征的RFS的PHD，ν表示机器人的速度，R表示机器人所携带传感器扫描半径，M表示经过PHD-SLAM方法得出的已探测地图特征信息。根据机器人速度ν和传感器扫描半径R，确定出一个阈值T，阈值T为机器人速度ν和传感器扫描半径R的和；当已探索过的地图特征集合内有地图特征位置与机器人位置之间距离小于这一阈值时，将该地图特征作为可能出现在当前时刻机器人视域内的新地图特征，γ(m|X_k-1)表示已探测地图特征中小于阈值的地图特征形成的RFS的PHD。

混合新生地图信息策略中的b(m|X_k)不仅包括k-1时刻的观测集合还加入了已探测区域内地图信息中靠近机器人位姿的地图特征集合，当机器人视域内已探测过的地图特征重新出现时，加入b(m|X_k)中作为先验信息的这部分地图特征，经过PHD-SLAM地图滤波后，重新出现的地图特征的权重会增加，从而提高了对地图特征点位置和数目的估计精度。

(2)更新阶段：

式中V_k|k(m|X^k)表示k时刻后验地图特征信息RFS的PHD，

更新阶段用以确定真实地图特征所在位置，利用传感器获得地图特征观测集合与预测阶段的地图特征进行后验概率估计确定地图特征所在的位置。

(3)PHD滤波器剪切与合并

预测阶段及地图特征观测集合中包含很多虚警及杂波，这些虚警及杂波在更新阶段会产生大量权重很小无效的RFS，从而增加下一步SLAM过程的运算速度，因此需要对更新阶段PHD滤波器的结果进行剪切与合并。

步骤4、根据步骤3PHD滤波器对每个粒子地图估计结果，调整每个粒子权重大小。得到调整过后的粒子集后，k时刻的机器人位姿通过采样粒子的加权平均获得，从而实现导航和定位。

如图3所示，本发明提供的实施例中采用Rao-Blackwellised粒子滤波器实现机器人位姿后验估计。地图信息采用高斯混合概率假设密度进行预测和更新。具体包括如下步骤：

步骤1：将SLAM问题描述建模为随机有限集形式。

1)地图特征信息集合

PHD-SLAM方法的观测信息以及地图特征不再采用向量序列的形式表示，而是使用随机有限集合形式来表示。

式中，M_k表示k时刻机器人探索过的地图特征集合，M_k-1表示截止到k-1时刻机器人已经探索过的地图特征集合，FoV(X_k)表示在k时刻，位姿为X_k的机器人的视域范围内地图特征的集合，表示不在M_k-1地图内的地图特征集合。

2)地图特征观测集合

在真实情况中，由于传感器的漏检以及传感器自身产生的虚假测量，真实地图特征观测的产生是无序的，特征与特征之间没有内在关联，特征观测的数目也是无法确定的，因此使用随机有限集合的形式可以更准确的表示地图特征观测，在k时刻，位姿为X_k的机器人获得的特征观测集合表示为Z_k，其RFS模型为

式中D_k(m,X_k)表示位于m处的位姿为X_k的机器人由真实地图特征产生的测量RFS，C_k(X_k)表示杂波产生的测量RFS。中测量数目随意变化，且每个测量值都是无序排列。

D_k(m,X_k)建模为一个Bernoulli型RFS，D_k(m,X_k)＝φ的概率为1-P_D(m|X_k)，即真实地图特征没有被传感器检测到的概率为1-P_D(m|X_k)；D_k(m,X_k)＝{z}的概率密度为P_D(m|X_k)g_k(z|m,X_k)，即真实地图特征能够被传感器检测到的概率为P_D(m|X_k)g_k(z|m,X_k)；其中P_D(m|X_k)表示当机器人位姿为X_k时，能够探测到位于m处的地图特征概率。g_k(z|m,X_k)表示机器人携带的传感器模型。

步骤2：PHD-SLAM使用RB粒子滤波器实现机器人先验位姿估计：

机器人位姿由不同权值的粒子集表示，即其中，表示第i个粒子在k时刻的权重，表示第i个粒子在k时刻所代表的机器人位姿，N表示粒子集中粒子的总数，i表示粒子的编号；机器人的状态服从状态转移方程，则每个粒子位姿的先验分布表示为其中表示第i个粒子从0时刻到k-1时刻所表示的所有机器人位姿，U_0:k-1表示从0时刻到k-1时刻所有的控制量，X₀表示机器人初始位姿。

步骤3：使用GMPHD滤波器实现每个粒子对地图信息预测更新：

1)预测阶段

根据每个粒子采样位置，可以得到k时刻新出现特征的高斯混合项：

表示机器人新探测出现的特征的高斯量的数量，分别表示第i个粒子中的第j个高斯量的权重，均值和方差；b表示机器人新探测出现的特征。

对预测阶段的采用混合新生地图信息策略，不仅包括上一时刻的观测集合形成的高斯项，还包括已探测地图信息中地图特征位置与机器人位置距离小于阈值T的地图特征的高斯项。

第i个粒子所对应的先验地图的PHD表示为其高斯混合表达式为：

式中表示上一时刻经过PHD滤波后得到高斯量个数，分别表示高斯量对应的权重均值和方差，表示第i个粒子k-1时刻的位姿。

根据公式(1)可知，地图预测的高斯混合PHD表示为:

其中，表示k时刻地图特征预测的高斯混合项包括k时刻新出现的地图特征高斯混合项，以及k-1时刻PHD滤波后得到的高斯混合项。

2)更新阶段

每个粒子的观测似然函数也采用高斯混合的形式，更新后地图的后验PHD同样具有高斯混合的形式

其中μ_k|k,P_k|k,S_k通过基于EKF-PHD滤波器更新获得。

3)PHD的剪切、合并与地图特征的提取

观测集合中存在大量虚假观测，地图特征经过PHD滤波后虚假观测将产生许多权重极小的高斯分量，这些高斯分量对地图的估计没有帮助，但会大大影响整个地图滤波的计算速度，因此对于高斯权重小于T_p的高斯分量丢弃，本实施例中T_p＝10^-5，另一方面每个粒子由于自身位置的误差对每个真实地图特征的估计也会产生偏差，因此在状态提取之前需对经过剪切后，高斯分量相互位置距离小于门限T_m的高斯分量进行合并，提高地图估计的精度。PHD剪切、合并完成后，对高斯分量权重大于地图特征提取门限T_f的高斯分量进行地图特征提取，其高斯项的均值表示地图特征的位置，满足门限T_f高斯分量的个数表示地图特征的数目。

步骤4：利用步骤3获得的PHD滤波器对每个粒子地图估计结果调整粒子权重,得到调整过后的粒子集后，通过采样粒子的加权平均获得k时刻的机器人位姿。

1)粒子重要性采样：

机器人位姿的后验分布由不同权值的粒子集表示，即表示k时刻第i个粒子的权重，表示第i个粒子对应的位姿。权重的递推公式为：

是权重递归的关键，对于计算使用结构简单，计算量小的空地图策略，的计算公式为：

式中表示k时刻杂波形成PHD，表示更新后的地图特征数目，表示预测地图特征数目，λ_c表示杂波的个数。

2)粒子重采样：

为解决粒子退化问题，需要进行粒子重采样，粒子重采样完成后得到新的粒子集

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。

Claims

1.一种用于水下自主导航与定位的方法，其特征在于：包括以下步骤：

步骤4：根据步骤3获得的PHD滤波器对每个粒子地图估计结果调整粒子权重,得到调整过后的粒子集后，通过采样粒子的加权平均获得k时刻的机器人位姿，从而实现导航和定位；

其中，所述步骤3中对每个粒子所对应的机器人位姿进行地图估计的方法为：包括以下步骤：

步骤302：更新阶段，根据公式：

步骤303：对更新阶段PHD滤波器的结果进行剪切与合并。

2.根据权利要求1所述的用于水下自主导航与定位的方法，其特征在于：所述步骤4中调整每个粒子权重大小的方法为对粒子的重要性采样，对每个粒子对应的地图特征进行滤波，将得到的结果与机器人采集到的地图特征进行比较，如果比较的结果误差越小，就表示粒子与机器人的位置越准确，从而增加对应粒子的权重；如果相反，就减小对应粒子的权重。

3.根据权利要求1所述的用于水下自主导航与定位的方法，其特征在于：所述步骤4中还包括粒子重采样。