[发明专利]单水声信标测距辅助的水下同步定位与构图方法

权利要求书

1.一种单水声信标测距辅助的水下同步定位与构图方法，其特征在于，包括以下步骤：

步骤1，确定航行器运动的状态方程，依据状态方程得到预测方程；

步骤2，在实际航行过程中，航行器对环境特征进行观测，得到观测量，同时对提前布置好的单水声信标进行测距观测，得到增广的观测量；

步骤3，依据步骤2得到的增广的观测量对新观测到的路标进行数据关联，不断对特征地图和状态量进行更新，从而得到实时的定位导航结果。

2.根据权利要求1所述的单水声信标测距辅助的水下同步定位与构图方法，其特征在于，步骤1具体包括以下步骤：

步骤1-1，对侧扫声呐图像进行处理，进行特征提取，步骤1-1中提取的特征为点特征；

步骤1-2，利用EKF-SLAM系统的模型进行预测，得到预测状态量和预测状态协方差：

记水下航行器在各个时刻的状态为x₁,x₂,...,x_k，其中k是离散的时间下标；在SLAM系统中，系统的状态为水下航行器的位置和方向，用式(1)来描述状态估计问题：

其中，f(·)表示运动方程，u表示控制输入，w表示运动噪声，z表示观测数据，g(·)表示观测方程，n表示观测噪声；

依据初始信息，得到状态的预测结果，即：

式(2)表示系统的状态向量预测结果，即由k-1时刻的状态量预测得到k时刻的状态量，状态量由水下航行器的位置和方向构成，其中x_x(·)表示水下航行器在二维平面内的横坐标，x_y(·)表示水下航行器在二维平面内的纵坐标，φ(·)表示航向；

得到状态协方差的预测结果，即：

P_xx,k|k-1＝▽fP_xx,k-1|k-1▽f^T+Q_k (3)

式(3)中P表示状态协方差，▽f表示状态转换函数的雅克比矩阵，Q_k表示噪声方差。

3.根据权利要求1所述的单水声信标测距辅助的水下同步定位与构图方法，其特征在于，步骤2具体包括以下步骤：

步骤2-1，在运动过程中每间隔相同时间对特征点进行一次观测，得到观测结果：

记每一个特征点的坐标为则航行器在每一运动状态下与特征点之间的距离为：

记dx(i)＝lm_x(i)-x_x(i)，dy(i)＝lm_y(i)-x_y(i)，则观测到的航向为：

此时得到观测矩阵如下：

步骤2-2，进行单水声信标测距，得到测距结果，并将测距结果加入到观测方程当中：

每一次观测的同时，航行器与单水声信标进行一次测距工作，得到航行器与单信标之间的距离和方向角，并将航行器与单信标之间的距离和方向角加入观测方程当中进行约束；

记水下固定单信标的平面位置坐标为(a,b)，对水下固定单信标进行单信标测距得到：

记dx1(i)＝a-x_x(i)，dy1(i)＝b-x_y(i)，则得到航向为：

此时得到对单信标观测矩阵如下：

同时，将对单信标观测矩阵(9)加入到原观测矩阵(6)当中，得到最终的观测矩阵：

z＝(z r) (10)。

4.根据权利要求1所述的单水声信标测距辅助的水下同步定位与构图方法，其特征在于，步骤3具体包括以下步骤：

步骤3-1，对新观测到的路标进行数据关联：利用最邻近数据关联算法进行数据关联，即通过观测矩阵中的距离值与设置的最大关联距离进行判断，看是否与新观测到的路标进行关联，并将新观测到的路标加入到观测地图当中；

步骤3-2，通过步骤3-1数据关联的结果进行状态更新，对系统的状态量与状态协方差进行更新，从而得到新的运动状态：

计算实际观测与预测观测之间的差值：

式(11)中，z(·)表示实际观测量，表示预测观测量；

更新之后的状态向量为：

x(k|k)＝x(k|k-1)+K*ε(k) (12)

式(12)中，K表示卡尔曼增益，即观测值与预测值的一个权重。