[发明专利]一种单目视觉水下航行器导航定位校正方法

权利要求书

1.一种单目视觉水下航行器导航定位校正方法，其特征在于，包括：

步骤一：通过将水下航行器航行到预定的定位校正区域，根据单目视觉辨识与量测子系统识别功能，追踪到水下标志物；

步骤二：通过调整水下航行器姿态，控制水下航行器向步骤一所述的水下标志物航行一段距离，航行过程中，根据单目视觉辨识与量测子系统采集得到图像信息，根据航位推算子系统记录得到这段时间内水下航行器位置、水下航行器位移及水下航行器姿态信息；

步骤三：通过将水下航行器围绕步骤一所述的水下标志物运动至不同位置，重复步骤二的全部操作，在所述的不同位置重复4次以上；

步骤四：通过汇总单目视觉辨识与量测子系统的输出信息以及航位推算子系统的输出信息，进行回归优化分析计算，得到此时水下航行器相对步骤一所述的水下标志物的位置信息；

步骤五：通过计算水下航行器的导航定位校正值，完成导航定位校正。

2.根据权利要求1所述的一种单目视觉水下航行器导航定位校正方法，其特征在于，所述步骤一，包括：

通过将水下航行器航行到预定的定位校正区域，根据单目视觉辨识与量测子系统识别功能，追踪到已知位置信息的水下标志物；

其中，所述的水下标志物的位置信息已知，记为X＝[x，y，z]，预定的水下标志物为人为布置或海底自然标志物，其具体大小未知，但其大小不超过视觉量测工作距离的

3.根据权利要求1所述的一种单目视觉水下航行器导航定位校正方法，其特征在于：所述步骤二，包括：

通过调整水下航行器姿态，控制水下航行器向步骤一所述的水下标志物航行一段距离，航行过程中，根据单目视觉辨识与量测子系统采集得到图像信息，根据航位推算子系统记录得到这段时间内水下航行器位置、水下航行器位移及水下航行器姿态信息；

其中，所述的得到这段时间内水下航行器位置、水下航行器位移及水下航行器姿态信息的具体方法为：

第一步：调整水下航行器姿态，使摄像头朝向步骤一所述的水下标志物，采集并记录此时水下航行器的位置向量姿态角向量图像中水下标志物的置信度P、大小(w,h)；

上式中，上标G表示相对于大地坐标系；

第二步：控制水下航行器朝向步骤一所述的水下标志物航行Δt秒，记录此时水下航行器的位置信息、水下航行器的姿态角信息以及图像中步骤一所述的水下标志物信息；

第三步：保持水下航行器姿态，重复第二步4次以上，采集记录此时水下航行器的位置向量姿态角向量A_i以及图像中水下标志物的大小(w_i,h_i)；

上式中，角标i表示第i次采集到信息；

第四步：根据视觉量测算法计算获得水下标志物距离摄像头坐标系下的位置向量摄像头朝向水下标志物时，标志物在视觉系统采集到图像的正中央，因此

视觉量测算法为：水下航行器在预定的定位校正区域(O₁)处时，大小为|AB|的标志物在摄像机中的成像大小为|A₁B₁|；水下航行器在朝向所述的水下标志物航行Δt秒后的位置(O₂)处时，大小为|AB|的标志物在摄像机中的成像大小为|A₂B₂|，由几何关系得：

上式中，s为水下航行器两次采样前后的位移，由航位推算获得，求解上式获得位置向量同理求解采样时的其他位置向量

成像大小|A_iB_i|由图像中标志物的大小(w_i,h_i)按下式计算：

上式中，dx、dy为摄像机中感光单元中单个像素的实际物理尺寸，其值通过摄像机标定求出；

考虑到在安装时，摄像头坐标系与水下航行器随体坐标系之间存在安装偏差，则随体坐标系下标志物的位置向量由下式得：

上式中，上标C表示相对于摄像机坐标系，表示安装偏角引起的旋转矩阵，表示安装位置偏移引起的偏移向量，其值在安装摄像头时确定。