一种单目视觉水下航行器导航定位校正方法与流程

本发明涉及水下目标定位技术领域，具体涉及一种单目视觉水下航行器导航定位校正方法。

背景技术：

水下导航是水下航行器的关键技术之一，现有的水下导航技术主要有：航位推算、惯性导航以及水下声学定位等。

由于惯性导航系统一般体积较大，价格昂贵，定位误差随时间不断积累；水下声学定位方法通过水下传感器网络实现，水下传感器网络使用声波通信，传播介质是水，这与无线电波在空中传播有显著的不同，它具有高时延，时延不断动态变化的特点，水声信号传播面临着衰减大、通信信道带宽低、多径效应严重、误码率高等问题，水下声学定位系统通常价格昂贵，安装校准耗时长要求高，作用距离有限。因此，航位推算由于其经济性、高效性仍然是目前水下航行器的主流导航算法之一，但其精度受传感器性能影响较大，同时存在时延累计误差。因此，在远距离航行或开展定位精度要求很高的水下作业时，需要通过外部手段进行定位校正。

目前的定位系统有它们自身的缺陷，不能够为在重要的水下任务中追踪水下航行器提供足够精确的定位。首先，通过单次的信号到达时间差得出的定位存在很大的误差，这种误差主要是由于环境的噪声造成的。其次，在现有的基于距离的定位系统中，定位方法都假定水下声音传播的速度是恒定为1500m/s，同时，这些方法还假定在水下三维环境中，声音是沿着直线传播的。但是，这些假设都有一定的局限性，在对定位精度要求比较高的水下任务中是不能够成立的。通过水下声学定位对航位推算进行修正是目前常用的方法之一，但其成本高、安装校准复杂，更为重要的是水下环境复杂容易造成水下声波的多次反射、“混响”，从而严重影响其定位精度；声学定位的时延性使其无法实现连续定位；定位能力存在盲区，系统的可靠性相对较差。

技术实现要素：

本发明的目的在于提供一种单目视觉水下航行器导航定位校正方法，具有成本低、精度高、使用方便简单等特点，可用于替代现有的水声定位导航校正技术。

本发明实施例提供一种单目视觉水下航行器导航定位校正方法，包括：

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

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

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

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

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

所述步骤一，包括：

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

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

所述步骤二，包括：

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

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

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

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

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

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

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

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

视觉量测算法为：如图2所示，航行器在o1处时，大小为|ab|的标志物在摄像机中的成像大小为|a1b1|，在o2处时其成像大小为|a2b2|，由几何关系得：

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

成像大小|aibi|由图像中标志物的大小(wi,hi)按下式计算：

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

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

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

所述步骤三，包括：

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

其中，所述的其他不同位置不在之前各采样点与标志物的连线上。

所述步骤四，包括：

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

其中，所述的得到位置信息的具体方法为：

(1)按下式计算出标志物在大地坐标下的位置向量

上式中，表示航行器在航行过程中用于姿态角引起的旋转矩阵，其值在确定姿态角向量ai后唯一确定，角标i表示第i次采集到信息；

(2)由上式知，采集n次后计算获得n-1个标志物位置向量由于视觉量测存在误差，计算获得的值与实际已知标志物的位置向量x之间一定存在误差，利用遗传算法进行回归优化计算，获得最后一次采样时，标志物相对于摄像机坐标系下的位置向量遗传算法中待优化变量为适应度函数为：

优化计算后获得最后一次采样时，标志物相对于摄像机坐标系下的位置向量

所述步骤五，包括：

通过计算水下航行器的导航定位校正值，完成导航定位校正；

其中，所述的计算水下航行器的导航定位校正值具体方法为：

计算水下航行器的导航定位校正值xa，即此时航行器相对于大地坐标系的位置的公式为：

上式中，表示最后一次采集信息时，姿态角向量所确定的旋转矩阵。

本发明的有益效果在于：

1.一种利用单目视觉进行水下航行器导航定位校正的方法，仅利用一台水下摄像机即可实现航行器导航定位校正，具有成本低、精度高、方便灵活等特点；

附图说明

图1为本发明所述水下航行器和水下标志物的位置关系示意图；

图2为本发明摄像头成像示意图；

图3为本发明数据采集位置切换示意图；

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图对本发明做进一步描述：

图1为本发明所述水下航行器和水下标志物的位置关系示意图；

图2为本发明摄像头成像示意图；

图3为本发明数据采集位置切换示意图。

本发明的技术方案是这样实现的：

本发明中单目视觉导航定位校正系统由4大子系统组成：(1)水下航行器载体子系统(简称：航行器)；(2)水下航行器控制子系统(简称：控制系统)；(3)航位推算子系统(简称：航位推算)；(4)单目视觉辨识与量测子系统(简称：视觉系统)。

本发明包括以下几大步骤：

第一步，水下航行器航行到预定定位校正区域，单目视觉辨识与量测子系统追踪到已知位置信息的水下标志物(可以为人为布置标志物，也可以是自然标志物，简称：标志物)；

第二步，调整水下航行器姿态，控制航行器朝向水下标志物航行一段距离，航行时视觉系统采集图像信息，同时航位推算子系统记录下航行器这段时间的位置、位移及姿态信息；

第三步，水下航行器围绕标志物运动至其他位置后，重复第二步操作。在不同位置重复上述过程4次以上。

第四步，汇总单目视觉辨识与量测子系统及航位推算子系统的输出信息，进行回归优化分析，获得水下航行器此时相对水下标志物的位置信息。

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

所述第一步中，选取的预定水下标志物的特征在于：水下标志物的位置信息已知，记为x＝[x，y，z]，预定水下标志物可以人为布置，也可为海底自然标志物，其具体大小可以未知，但其大小不能超过视觉量测工作距离的

所述第二步的详细步骤为：

(1)调整水下航行器姿态，使摄像头朝向水下标志物，采集并记录此时水下航行器的位置向量姿态角向量图像中水下标志物的置信度p、大小(w,h)。其中上标g表示相对于大地坐标系。

(2)如图1所示，控制航行器朝向水下标志物航行δt秒，记录此时水下航行器的位置信息、姿态角信息及图像中水下标志物信息；

(3)保持姿态，重复(2)若干次(不小于4次)，采集记录此时水下航行器的位置向量姿态角向量ai及图像中水下标志物的大小(wi,hi)。其中角标i表示第i次采集到信息。

(4)通过视觉量测算法计算获得水下标志物距离摄像头坐标系下的位置向量摄像头朝向水下标志物时，标志物在视觉系统采集到图像的正中央，则因此只需计算即可确定位置向量其中上标c表示相对于摄像机坐标系。

视觉量测算法原理如下：

如图2所示，航行器在o1处时，大小为|ab|的标志物在摄像机中的成像大小为|a1b1|，在o2处时其成像大小为|a2b2|，由几何关系可知：

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

成像大小|aibi|可由图像中标志物的大小(wi,hi)按下式计算：

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

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

上式中表示安装偏角引起的旋转矩阵，表示安装位置偏移引起的偏移向量，其值在安装摄像头时可以确定。

所述第三步中，航行器沿一条航线校正信息采集完成后需机动至另一位置采集信息，该位置不在之前各采样点与标志物的连线上。如图3所示，航行器在航线1上采集信息，完成后移动航线2上继续采集，依次类推完成采集过程。

所述第四步的详细步骤为：

(1)按下式计算出标志物在大地坐标下的位置向量

其中，表示航行器在航行过程中用于姿态角引起的旋转矩阵，其值在确定姿态角向量ai后唯一确定；角标i表示第i次采集到信息。

(2)由(1)中可知，采集n次即可计算获得n-1个标志物位置向量由于视觉量测存在误差，计算获得的值与实际已知标志物的位置向量x之间一定存在误差。利用遗传算法进行回归优化计算，待优化变量采用二进制数进行编码，初始种群数量50～300。适应度函数为：

个体被选择的概率与适应度值成正比，并按照轮盘赌选择的方法选出新的种群，交叉概率为80％～95％，变异概率为2％～15％，遗传代数为50～100代。优化计算后获得最后一次采样时，标志物相对于摄像机坐标系下的位置向量

所述第五步中，由下式计算水下航行器的导航定位校正值xa，即此时航行器相对于大地坐标系的位置。

其中表示最后一次采集信息时，姿态角向量所确定的旋转矩阵。

本发明通过以下技术方案来实现上述目的：

1.单目视觉导航定位校正系统组成包括：

(1)水下航行器载体子系统，用于搭载其他子系统完成相应的任务，下文简称：航行器；

(2)水下航行器控制子系统，用于控制水下航行器完成相应的运动动作，下文简称：控制系统；

(3)航位推算子系统，用于提供水下航行器导航信息，实时输出航行器的位置、姿态等信息，下文简称：航位推算；

(4)单目视觉辨识与量测子系统，用于识别水下标志物，并计算标志物与水下航行器之间的相对位置，实时输出图像中水下标志物的置信度、大小等信息，下文简称：视觉系统。

2.实现单目视觉导航定位校正的主要步骤包括：

第一步，水下航行器航行到预定定位校正区域，单目视觉辨识与量测子系统追踪到已知位置信息的水下标志物(可以为人为布置标志物，也可以是自然标志物，简称：标志物)；

第二步，调整水下航行器姿态，控制航行器朝向水下标志物航行一段距离，航行时视觉系统采集图像信息，同时航位推算子系统记录下航行器这段时间的位置、位移及姿态信息；

第三步，水下航行器围绕标志物运动至其他位置后，重复第二步操作。在不同位置重复上述过程4次以上。

第四步，汇总单目视觉辨识与量测子系统及航位推算子系统的输出信息，进行回归优化分析，获得水下航行器此时相对水下标志物的位置信息。

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

所述第一步中，选取的预定水下标志物的特征在于：水下标志物的位置信息已知，记为x＝[x，y，z]，预定水下标志物可以人为布置，也可为海底自然标志物，其具体大小可以未知，但其大小不能超过视觉量测工作距离的

所述第二步的详细步骤为：

(1)调整水下航行器姿态，使摄像头朝向水下标志物，采集并记录此时水下航行器的位置向量姿态角向量图像中水下标志物的置信度p、大小(w,h)。其中上标g表示相对于大地坐标系。

(2)控制航行器朝向水下标志物航行δt秒，记录此时水下航行器的位置信息、姿态角信息及图像中水下标志物信息；

(3)保持姿态，重复(2)若干次(不小于4次)，采集记录此时水下航行器的位置向量姿态角向量ai及姿态角向量ai及图像中水下标志物的大小(wi,hi)。其中角标i表示第i次采集到信息。

(4)通过视觉量测算法计算获得水下标志物距离摄像头坐标系下的位置向量摄像头朝向水下标志物时，标志物在视觉系统采集到图像的正中央，因此其中上标c表示相对于摄像机坐标系。考虑到在安装时，摄像头坐标系与航行器随体坐标系之间存在安装偏差，则随体坐标系下标志物的位置向量由下式可得：

上式中表示安装偏角引起的旋转矩阵，表示安装位置偏移引起的偏移向量，其值在安装摄像头时可以确定。

所述第三步中，航行器沿一条航线校正信息采集完成后需机动至另一位置采集信息，该位置不在之前各采样点与标志物的连线上。

所述第四步的详细步骤为：

(1)按下式计算出标志物在大地坐标下的位置向量

其中，表示航行器在航行过程中用于姿态角引起的旋转矩阵，其值在确定姿态角向量ai后唯一确定；角标i表示第i次采集到信息。

(2)由(1)中可知，采集n次即可计算获得n-1个标志物位置向量由于视觉量测存在误差，计算获得的值与实际已知标志物的位置向量x之间一定存在误差。利用遗传算法进行回归优化计算，获得最后一次采样时，标志物相对于摄像机坐标系下的位置向量遗传算法中待优化变量为适应度函数为：

所述第五步中，由下式计算水下航行器的导航定位校正值xa，即此时航行器相对于大地坐标系的位置。

其中表示最后一次采集信息时，姿态角向量所确定的旋转矩阵。