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

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

【技术实现步骤摘要】

本专利技术属于水下机器人自主导航领域，特别涉及一种用于水下自主导航与定位的方法。
技术介绍
水下自主导航与定位主要是通过机器人实现，目前机器人中均采用的传统SLAM(即时定位与地图构建，下文简称SLAM)方法在密集杂波和特征数目多且变化剧烈的水下环境中存在数据关联精度低、计算复杂的问题，且在SLAM实现过程中，由于机器人运动过程中的误差、传感器的观测误差、以及传感器不确定性造成地图特征漏检等问题，都将影响数据关联过程中的准确性，所以传统SLAM方法的准确性对地图特征信息的数据关联非常敏感。基于随机有限集合理论的概率假设密度SLAM方法是一种在估计地图特征时不依赖数据关联，并将传统SLAM方法中所忽略的由于传感器不确定性造成的地图特征漏检概率带入地图特征滤波的SLAM新方法，非常适用于数据关联模糊、目标特征多且变化剧烈的水下环境中。但由于概率假设密度(ProbabilityHypothesisDensity,下文简称PHD)算法的特性，需要提供一定先验信息才能递归进行滤波运算，而SLAM过程是在未知环境中从未知位置进行运动，无法提供先验信息，为此传统方法是将移动机器人上一时刻的观测集合作为当前时刻的先验信息集合，带入PHD滤波当中从而估计出新出现地图特征的位置。然而，由于新生目标集合仅仅采用上一时刻的观测集合，因此在地图特征位置及数目的估计精度上存在不足。
技术实现思路
专利技术目的：为了克服现有传统SLAM方法在密集杂波和特征数目多且变化剧烈的水下环境中存在数据关联精度低、计算复杂的不足以及现有随机有限集SLAM方法中先验信息不足的问题，本专利技术提供了一种数据关联精度高，计算简单的用于水下自主导航与定位的方法。技术方案：本专利技术提供了一种用于水下自主导航与定位的方法，包括以下步骤：步骤1：将机器人反馈的每一时刻地图特征信息、地图特征观测信息以及传感器不确定造成的漏检概率都建模为随机有限集形式；步骤2：使用Rao-Blackwellised粒子滤波器进行机器人位姿估计得到机器人位姿先验估计的粒子集合；步骤3：根据步骤2中获得的机器人先验估计的粒子集合对每个粒子所对应的机器人位姿进行地图估计，完成对地图信息的预测和更新；步骤4：根据步骤3获得的PHD滤波器对每个粒子地图估计结果调整粒子权重,得到调整过后的粒子集后，通过采样粒子的加权平均获得k时刻的机器人位姿，从而实现导航和定位。进一步，所述步骤3中对每个粒子所对应的机器人位姿进行地图估计的方法为：包括以下步骤：步骤301：地图预测，根据公式Vk|k-1(m|Xk)＝Vk-1|k-1(m|Xk-1)+b(m|Xk)得到k时刻预测可能出现的地图特征随机有限集合(RandomFiniteSet,下文简称RFS)的PHD；其中，Vk-1|k-1(m|Xk-1)表示k-1时刻根据机器人位姿Xk-1得到的后验地图特征信息RFS的PHD，b(m|Xk)表示随着机器人运动可能进入机器人视域内新生地图特征信息RFS的PHD；初始时刻，使用k-1时刻观测集合作为k时刻可能出现新地图特征信息；当机器人经过PHD-SLAM更新出已探测区域地图信息后，将已探测地图信息中靠近机器人位姿的地图特征作为先验信息，加入上一时刻观测集合中一起作为当前时刻的b(mXk)带入当前时刻的PHD预测阶段；步骤302：更新阶段，根据公式：更新地图信息，式中Vk|k(m|Xk)表示k时刻后验地图特征信息RFS的PHD，Λ(m|Xk)＝PD(m|Xk)gk(z|m,Xk)，PD(m|Xk)表示位姿为Xk的机器人能够探测到位于m处地图特征的概率，ck(z)表示k时刻由杂波引起的杂波RFS的PHD，gk(z|m,Xk)表示机器人携带的传感器模型；步骤303：对更新阶段PHD滤波器的结果进行剪切与合并。这样有效提高了对地图特征点位置和数目的估计精度及地图滤波运算速度。进一步，所述步骤4中调整每个粒子权重大小的方法为对粒子的重要性采样，对每个粒子对应的地图特征进行滤波，将得到的结果与机器人采集到的地图特征进行比较，如果比较的结果误差越小，就表示粒子与机器人的位置越准确，从而增加对应粒子的权重；如果相反，就减小对应粒子的权重。这样得到的定位更加的精准。进一步，所述步骤4中还包括粒子重采样。这样有效解决了粒子退化的问题。有益效果：与现有技术相比，本专利技术使用随机有限集方法建模实现SLAM方法，解决了传统SLAM方法在密集杂波和特征数目多且变化剧烈的水下环境中存在数据关联精度低、计算复杂的问题。使用了混合新生地图信息方法，增加了地图预测更新过程中PHD滤波器的先验信息，当机器人重新回到已探测过的环境中时，通过增加先验信息，改进PHD-SLAM方法对地图特征数目估计及地图特征位置精度估计。使用空地图特征策略提高机器人位姿后验估计的运算速度。从而使SLAM方法能够在更复杂的环境中得到应用。附图说明图1为本专利技术工作流程图；图2为本专利技术中混合新生地图信息预测阶段流程图；图3为本专利技术中提供的实施例的具体实施方式流程图。具体实施方式下面结合附图对本专利技术做更进一步的解释。如图1所示，本专利技术公开的一种用于水下自主导航与定位的方法，主要包括以下步骤：步骤1、将机器人反馈的k时刻地图特征信息、地图特征观测信息以及传感器不确定造成的漏检概率都建模为随机有限集形式。地图特征集合的数目从零到任意大小随意变化，随着在机器人不断出现在机器人视域内的地图特征而单调增加，每个地图特征之间是相互独立的。M＝φ(没有地图特征)M＝{m1本文档来自技高网...

【技术保护点】
一种用于水下自主导航与定位的方法，其特征在于：包括以下步骤：步骤1：将机器人反馈的每一时刻地图特征信息、地图特征观测信息以及传感器不确定造成的漏检概率都建模为随机有限集形式；步骤2：使用Rao‑Blackwellised粒子滤波器进行机器人位姿估计得到机器人位姿先验估计的粒子集合；步骤3：根据步骤2中获得的机器人先验估计的粒子集合对每个粒子所对应的机器人位姿进行地图估计，完成对地图信息的预测和更新；步骤4：根据步骤3获得的PHD滤波器对每个粒子地图估计结果调整粒子权重,得到调整过后的粒子集后，通过采样粒子的加权平均获得k时刻的机器人位姿，从而实现导航和定位。

【技术特征摘要】
1.一种用于水下自主导航与定位的方法，其特征在于：包括以下步骤：步骤1：将机器人反馈的每一时刻地图特征信息、地图特征观测信息以及传感器不确定造成的漏检概率都建模为随机有限集形式；步骤2：使用Rao-Blackwellised粒子滤波器进行机器人位姿估计得到机器人位姿先验估计的粒子集合；步骤3：根据步骤2中获得的机器人先验估计的粒子集合对每个粒子所对应的机器人位姿进行地图估计，完成对地图信息的预测和更新；步骤4：根据步骤3获得的PHD滤波器对每个粒子地图估计结果调整粒子权重,得到调整过后的粒子集后，通过采样粒子的加权平均获得k时刻的机器人位姿，从而实现导航和定位。2.根据权利要求1所述的用于水下自主导航与定位的方法，其特征在于：所述步骤3中对每个粒子所对应的机器人位姿进行地图估计的方法为：包括以下步骤：步骤301：地图预测，根据公式Vk|k-1(m|Xk)＝Vk-1|k-1(m|Xk-1)+b(m|Xk)得到k时刻预测可能出现的地图特征随机有限集合的概率假设密度；其中，Vk-1|k-1(m|Xk-1)表示k-1时刻根据机器人位姿Xk-1得到的后验地图特征信息RFS的PHD，b(m|Xk)表示随着机器人运动可能进入机器人视域内新生地图特征信息RFS的PHD；初始时刻，使用k-1时刻观测集合作为k时刻可能出现新地图特征信息；当机器人经过PHD-SLAM更新出已探测区域地图信息后，将已探测地图信息...

【专利技术属性】
技术研发人员：章飞，史剑鸣，孙陶莹，曾庆军，
申请(专利权)人：江苏科技大学，
类型：发明
国别省市：江苏;32