[发明专利]基于关键约束帧的前视成像声呐水下定位与导航方法

权利要求书

1.基于关键约束帧的前视成像声呐水下定位与导航方法，其特征在于，包括以下步骤：

步骤1：前端数据处理：水下机器人依据惯性导航设备产生的数据得到推测的机器人位姿；通过二维前视成像声呐获取二维声呐图像，经过特征提取与匹配后，得到声呐连续两帧之间的特征匹配，并进行特征点全局编号，以feature_id表示，每一帧观测到的所有特征点均以像素坐标形式按feature_id顺序存储到特征点数据库，若匹配成功或检测到已经存在于特征点数据库中，则判定为旧特征点，全局编号不更新；否则判定为当前帧观测到的新特征点，按特征观测的先后顺序依次编号；

步骤2：声呐关键约束帧的识别与存储：读取连续两帧中机器人在世界坐标系下的位姿和所有特征点像素坐标，并将特征点像素坐标转换成基于声呐极坐标系的球面坐标；判定当前帧是否为欠约束帧，若是，则另行处理；若否，则进一步判定当前帧是否为声呐关键约束帧，若是，则将当前帧存储至关键约束帧库；具体方法为：

步骤2.1：读取连续两帧中所有的特征点像素坐标，并将上一帧中的特征点像素坐标转换成基于声呐极坐标系的球面坐标；判断当前帧与上一帧的特征匹配数量N_match是否不低于最小规定值N_th-match，若是，则直接判定为欠约束帧；若否，则进行下一步；

步骤2.2：读取连续两帧对应的机器人在世界坐标系下的位姿数据，根据上一次后端优化得到的上一帧机器人位姿x_i-1，i＝1,2,3,...、惯性导航设备得到的当前帧机器人位姿x_i、当前声呐帧和上一帧的所有特征点球面坐标l_j，j＝1,...,M,计算对应测量函数的雅克比矩阵A及其奇异值；

步骤2.3：若最小奇异值σ_min小于一定阈值σ_th-low，则判定当前帧为欠约束帧；若最小奇异值大于一定阈值σ_th-high，则判定当前帧为关键约束帧，并存储至关键约束帧库；

步骤2.4：判定为欠约束帧的当前声呐帧，不加入滑动窗口优化，其对应的机器人位姿由惯性导航设备预测更新，对应的水下特征坐标不更新；

步骤3：从关键约束帧库中选择可加入滑动窗口优化的过往帧：对于关键约束帧库中存储的过往帧，判断其能否加入到当前滑动窗口优化中，若能则选择性加入，若否则不加入；具体方法为：

步骤3.1在关键约束帧库中，对于待加入到滑动窗口优化的过往帧，先判断该过往帧匹配的特征和当前帧匹配的特征中feature_id相同的数量N_coFea是否不小于阈值N_th-coFea，若是，则进行下一步；

步骤3.2在满足步骤3.1条件的所有过往帧中，将所有过往帧按最小奇异值的大小从高到低排序，并计算所有帧最小奇异值的平均值σ_min-mean，选择最小奇异值不小于σ_min-mean的过往帧加入到滑动窗口优化；

步骤4：窗口大小控制与优化：判断当前滑动窗口的大小是否超过最大限制，若是，则从当前窗口内移除部分过往帧；若否，则在窗口确定后开始通过非线性优化器对滑动窗口进行优化；优化完成后，得到估计的机器人相对位姿及水下特征点球面坐标，同时更新机器人和水下特征点的全局坐标信息，并反馈至下一次优化的输入中。

2.根据权利要求1所述的基于关键约束帧的前视成像声呐水下定位与导航方法，其特征在于，所述的前视成像声呐为多波束二维前视成像声呐。

3.根据权利要求1所述的基于关键约束帧的前视成像声呐水下定位与导航方法，其特征在于，步骤4所述的滑动窗口大小不是固定的，可以根据是否满足步骤3的判定准则进行自动调节，其大小在2和最大窗口限制N_Smax之间取值；若当前滑动窗口大小超过最大限制时，则只保留当前窗口内最小奇异值排序在前N_Smax-2的过往帧，并移除其余的过往帧。