[发明专利]一种粒子滤波BSLAM方法

摘要

本发明公开了一种粒子滤波BSLAM方法，包括：步骤一：算法初始化；步骤二：在每个t时刻输入对海底地形的深度测量值z(t)与里程计更新值v(t)以及里程计DR(t)后，对P(1:N)中所有粒子进行运动更新、闭环检测、观测更新、粒子历史轨迹更新；步骤三：当t时刻所有粒子都进行了步骤二后，判断是否需要进行重采样，若是：将所有经过观测更新的粒子进行重采样，t＝t+1，转入步骤二，否则t＝t+1，转入步骤二；步骤四：当t＝T时，以最终粒子集中每个粒子轨迹的平均值结合对应时刻的观测数据z(1:t)生成海底地形图并输出。本发明只需要在每一时刻输入里程计数据和由多波束声纳获得的地形测深，即可在没有先验地形图的情况下实现AUV的同步定位与建图。