[发明专利]一种用于水下机器人导航的声呐地图构建与重定位方法

权利要求书

1.一种水下机器人的重定位方法，其特征在于，包括以下步骤：

(Ⅰ)构建全局地图，将构建的全局地图用正方形网格划分，以全局地图中的地图特征点为中心，绘制每个地图特征点的协方差椭圆；

(Ⅱ)提取水下机器人当前位置利用声呐观测到的特征点，以观测到的特征点为中心绘制其协方差椭圆；

(Ⅲ)匹配水下机器人当前位置观测到的特征点与全局地图中的地图特征点；

若匹配成功，则利用匹配信息修正水下机器人的航位；

若匹配不成功，则水下机器人根据自身导航系统推算出的位置进行定位，等到下次获得观测信息后继续进行匹配；

步骤(Ⅰ)中，构建全局地图的方法包括以下步骤：

(1)水下机器人利用扩展卡尔曼滤波进行导航，同时采集海底形貌的声呐图像，从声呐图像中提取地图特征并标记位置，建立局部地图；当前局部地图中地图特征数量达到预设值后，关闭该幅局部地图，开启新的局部地图；

(2)将建立的局部地图按照先后序列进行联结，得到全局地图；

(3)将得到的全局地图进行优化；

步骤(Ⅲ)中，根据如下规则匹配水下机器人当前位置观测到的特征点与全局地图中的地图特征点：

(a)若两个协方差椭圆占据共同的网格，则表示两个特征点满足独立兼容条件，两个特征点匹配成功；

(b)若一个协方差椭圆同时与另外两个协方差椭圆占据了共同的网格，则占据网格数量最多的为最可能的关联特征，即视为正确匹配；

(c)若一个协方差椭圆同时与另外两个协方差椭圆占据了共同的网格，且占据的网格数量相同，则两个特征点实际距离短的为最可能关联特征，即视为正确匹配。

2.根据权利要求1所述的水下机器人的重定位方法，其特征在于，步骤(1)中，在i时刻初始化的局部地图中，以水下机器人i时刻的位置R_i作为参考基准，水下机器人的初始化定位为初始协方差矩阵当j时刻地图特征F₁,F₂,...,F_n数量达到预设值时，关闭该局部地图；i时刻至j时刻得到的局部地图表示为：

其中，为地图特征状态变量，分别表示j时刻水下机器人的位置R_j、地图特征F₁、F₂、...、F_n的位置坐标；为对应的协方差矩阵。

3.根据权利要求2所述的水下机器人的重定位方法，其特征在于，i时刻至j时刻的局部地图关闭后，建立j时刻至k时刻的局部地图，以水下机器人j时刻的位置R_j作为参考基准，表示为：

其中，为地图特征状态变量，分别表示k时刻水下机器人的位置R_k、地图特征G₁、G₂、...、G_m的位置坐标；为对应的协方差矩阵。

4.根据权利要求3所述的水下机器人的重定位方法，其特征在于，步骤(2)中，将两幅局部地图和进行联结，获得随机地图具体形式为：

将所有局部地图联结后得到全局地图。

5.根据权利要求1所述的水下机器人的重定位方法，其特征在于，步骤(3)中，对全局地图进行优化包括：

(3-1)通过独立兼容最近邻的数据关联方法得到两幅局部地图交界处的相互关联特征；

(3-2)利用修正的观测方程来表示相互关联特征的置信水平；将修正后的观测方程线性化，得到的值代入扩展卡尔曼滤波算法中，得到更新后的地图特征状态变量与其协方差矩阵；

(3-3)对更新后的地图特征状态变量与其协方差矩阵转换至同一个参考基准。

6.根据权利要求1所述的水下机器人的重定位方法，其特征在于，步骤(Ⅰ)和步骤(Ⅱ)中，所述的协方差椭圆满足卡方分布其中，d表示系统阶数，α表示该特征点的置信度水平。

7.根据权利要求1所述的水下机器人的重定位方法，其特征在于，步骤(Ⅲ)中，若匹配成功，则利用匹配信息修正水下机器人的航位，方法为：

定义水下机器人当前位置观测到的特征点在全局地图中的位置为x_a，根据水下机器人导航信息推算出的该特征的位置为x_b，水下机器人位置为x_robot；则修正后的水下机器人定位位置为x_robot+(x_a-x_b)。