[发明专利]基于粒子滤波与图优化的海底地形同步定位与建图方法

权利要求书

1.一种基于粒子滤波与图优化的海底地形同步定位与建图方法，其特征在于，包括以下步骤：

步骤1：设定粒子数N、粒子闭环检测半径R、粒子匹配子地图M_p的测线数目N_p、粒子轨迹子地图M_h的测线数目N_h、算法总步数T；初始化粒子集P_(1:N)；对于任一粒子P_(n)，其内部储存粒子当前状态粒子自身历史轨迹粒子权值w_n＝1/N、粒子储存的闭环集合L_n；n为粒子序数；t为时间步长；

t₀＝1时刻粒子集P_(1:N)的状态和自身历史轨迹根据GPS获得的先验状态设定，将t₀＝1时刻以及对应的粒子位姿加入粒子集P_(1:N)中各粒子储存的闭环集合L_n，初始化t＝2；

步骤2：获取当前t时刻输入的对海底的深度测量值z_(t)、里程计更新值v_(t)以及里程计的轨迹DR_(t)，更新粒子集P_(1:N)中各粒子的当前状态

其中，ω为里程计的噪声；

步骤3：计算粒子集P_(1:N)中各粒子的当前状态与粒子自身历史轨迹的最小欧氏距离r_n；若r_n＜R，则将加入粒子P_(n)储存的闭环集合L_n，执行步骤4进行地图更新；否则，忽略地图更新与观测更新且粒子P_(n)不加入重采样步骤；其中，M为L_n中闭环的总数；为第M个闭环所对应的开始时间；为第M个闭环所对应的结束时间；

步骤4：以粒子集P_(1:N)中粒子P_(n)储存的闭环集合L_n中粒子P_(n)的历史位姿为待优化变量，构建并求解位姿图，更新粒子P_(n)的轨迹后执行步骤5；

若粒子P_(n)中含有M个闭环，则位姿图中有2M+1个节点、M个二元闭环边、2M-1个三元弱数据关联边；其中不同类型边的误差函数设置为：

闭环边：

弱数据关联边：

其中，m为粒子P_(n)储存的闭环集合L_n中第m个闭环；t_k为位姿图中第k个节点所对应的时间；

步骤5：重新对粒子集P_(1:N)中各粒子进行闭环检测，计算粒子的当前状态与粒子自身历史轨迹的最小欧氏距离r_n；若r_n＜R，则记录该粒子最小欧氏距离r_n对应的时刻t_r，执行观测更新；

步骤5.1：生成粒子匹配子地图轨迹x_p，并将x_p与其对应的测深序列结合生成粒子匹配子地图M_p；

步骤5.2：生成和粒子轨迹子地图轨迹x_h，并将x_h与其对应的测深序列结合生成粒子轨迹子地图M_h；

步骤5.3：使用反距离加权算法对M_p进行插值，插值位置为M_h中测点的位置，获得包含L个共有有效测点的测深序列定义为z_p，定义M_h中相同位置的有效测深序列为z_h，计算粒子权值w_n：

其中，与分别为z_p与z_h的第l个有效测点的测深值；σ²为传感器的方差；

步骤5.4：将加入粒子自身历史轨迹

步骤6：获取执行观测更新的粒子总数N_th，计算有效粒子数N_eff；若则将所有经过观测更新的粒子进行重采样，执行步骤7；否则，直接执行步骤7；

其中，为第i个粒子的归一化权值；

步骤7：根据所有粒子储存的历史轨迹，每隔t_s个时刻取一帧为待优化节点，以每个粒子在此时刻储存的对应状态与待优化节点对应时刻状态的差为边，并与弱数据关联边构成位姿图，根据位姿图求解出轨迹Trj_(1:t)，然后令t＝t+1，返回步骤2。