[发明专利]面向水下机器人集群的视觉定位与近距离密集编队方法

权利要求书

1.面向水下机器人集群的视觉定位与近距离密集编队方法，其特征在于，水下机器人集群至少由3台载体构成，在水下机器人集群载体上搭载矢量标志灯组、标量标志灯组、水下摄像机、图像处理及编队控制器；其中一台为领航者，其余为跟随者，领航者按照航行任务自主航行，通过后视摄像机拍摄集群内其余跟随者的位置，从而判断是否跟随；跟随者通过自身载体艏段及艉段的摄像机拍摄领航者图像，定位自身载体位置并计算其在当前集群内的移动目标位置，实现无通信条件下基于光学图像位置纠偏的集群队形编队；

所述矢量标志灯组，设于水下机器人载体中间舱段内，所述矢量标志灯组内的激光器穿过载体透射窗向周围水域环境发射激光，使得在水下机器人载体轴向视角方向构成矢量光学图像；

所述标量标志灯组，为设于水下机器人载体艏段及艉段的环形光源，用于构成表征载体艏段及艉段位置的标量光学图像；

所述水下摄像机，设于水下机器人载体艏段及艉段，用于拍摄图像；

所述图像处理及编队控制器，用于输出指令控制矢量标志灯组、标量标志灯组的开启，控制领航者或跟随者的水下摄像机拍摄图像，对图像进行图像处理从而定位图像中载体的像素位置，领航者控制器判断跟随者是否跟随以及跟随者控制器计算其在当前集群内的移动控制参数并执行；所述图像处理及编队控制器内包括处理器存储器，存储器中存储有程序，处理器加载程序执行以下步骤；

步骤1、领航者和跟随者的控制器均输出指令控制矢量标志灯组、标量标志灯组的开启，控制水下摄像机拍摄图像；

步骤2、领航者执行区域检测优先级步骤拍摄图像，进行图像处理，判断跟随者是否跟随，若长时间未发现跟随者或跟随者距离大于设定值时减速行驶；

步骤3、跟随者执行区域检测优先级步骤拍摄图像，进行图像处理，定位集群内领航者和其他成员位置，计算其在当前集群内的移动控制参数；

所述领航随者在区域C、跟随者在区域A拍摄载体中间舱段的矢量光学图像并图像处理后，执行如下矢量位置定位步骤：

a.采用拟合算法对图像中存在的直线进行多次拟合，得到图像中所有激光光线方程；

b.计算两两矢量光线的交点坐标，并结合实际光线交点三维坐标与相机标定内参，采用PNP算法计算图像中目标水下机器人相对当前视角的水下机器人自身的位置和姿态；所述实际光线交点为已知的预先标定的交点坐标；

c.根据当前视角下其他水下机器人的位置和姿态调整所述编队队形中当前水下机器人目标位置，当前跟随者以调整后目标位置为输入，通过PID算法计算水下机器人艏向角以及前进速率作为动力学控制器的输入，水下机器人更新自身运动状态，而后循环检测位置，实现水下机器人立体编队。

2.根据权利要求1所述的面向水下机器人集群的视觉定位与近距离密集编队方法，其特征在于，所述矢量标志灯组为1个激光器和分束镜，分束镜将激光分成设定方向的若干束，且分束后的激光射线位于载体同一截面圆上。

3.根据权利要求1所述的面向水下机器人集群的视觉定位与近距离密集编队方法，其特征在于，所述矢量标志灯组为若干个激光器，且激光器出光口设于载体同一截面圆上、激光射线在同一平面。

4.根据权利要求1所述的面向水下机器人集群的视觉定位与近距离密集编队方法，其特征在于，所述矢量标志灯组包括若干个激光器和棱镜的组合，激光器向棱镜发射激光，棱镜向周围水域环境反射激光；所述激光在棱镜上的反射点设于载体同一截面圆上，且激光射线在同一平面。

5.根据权利要求2-4任意一项所述的面向水下机器人集群的视觉定位与近距离密集编队方法，其特征在于，所述激光器、分束镜、棱镜分别通过安装座按照预设角度安装于水下机器人舱段内部，使得激光光线与水下机器人载体成预设夹角，激光光束构成矢量光线图案；通过改变上述安装座的角度改变矢量光线的图案和特征；所述特征为矢量光线的交点位置；

所述水下机器人载体艏段设有前、上、下、左、右五个方向的水下摄像机，艉段设有后视摄像机；

所述矢量标志灯组为四个，任意相邻两个水下摄像机构成一组，其余两个构成一组，激光光学图像包括4条射线，两个交点。