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

摘要

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

主权项

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