[发明专利]一种自适应的水下多波束同步定位与建图方法

权利要求书

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

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

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

步骤三：令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：提取满足条件DR的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-δ和ε均为固有参数。