一种基于水下图像信息的航行器自主编队方法

1.本发明属于水下航行器视觉领域，涉及一种基于水下图像信息的航行器自主编队方法。


背景技术：

2.地球海洋面积约为3.6亿平方公里，约占地球表面积的71％。海洋不仅是地球上生命的摇篮，而且蕴藏着比陆地丰富得多的资源。在海洋开发过程中，水下航行器(auv)作为辅助深海探测的工具得到了广泛的应用，拓宽了人们对未知水下环境的认识。
3.随着auv任务复杂度的增加，单个auv逐渐难以满足实际需求，多auv编队控制技术开始成为auv控制领域的热点问题之一。相比于单个auv，多auv协同作业扩大了探测范围，增加了工作效率，且不会因为个体auv的失效而造成任务失败，保证了系统的冗余性，具有巨大应用价值。
4.视觉传感器在短程探测方面具有明显的优势，如精度高、体积小、重量轻、携带方便，适用于近距离、高精度的目标探测和编队控制。现阶段应用于auv视觉定位的主要是几何法。几何法通过在水下航行器上安装的相机获取视觉目标上的n个特征点在图像系下的位置坐标，通过pnp算法求解了水下航行器相对于目标位置和姿态。基于获得的姿态与距离信息进行编队控制。


技术实现要素：

5.要解决的技术问题
6.为了避免现有技术的不足之处，本发明提出一种基于水下图像信息的航行器自主编队方法，是一种在通讯约束的条件下，通过航行器自身搭载的视觉传感器获取领行者的视觉信息，进而解算与目标的相对位姿，实现多水下航行器协同编队。
7.技术方案
8.一种基于水下图像信息的航行器自主编队方法，其特征在于：领航水下航行器后端布有视觉识别装置，包括共面位于领航航行器的尾端的四个光信标，其中三个发光体颜色相同；跟随航行器首端设有图像采集系统；所有编队步骤如下：
9.步骤1：航行时，跟随航行器的图像采集系统采集领航航行器尾端的四个光信标的运动图像；
10.步骤2：建立相机模型下的水下航行器视觉导航模型，包括建立相机小孔模型和确定相机内外参数；
11.步骤3：对采集到的运动图像进行预处理，提取得到光源目标特征，并根据目标光源在像素坐标系上位置，得到目标光源的像素坐标；
12.步骤4：对像素坐标通过求解共面p4p的方法，确定领航水下航行器在三维空间中的位姿；
13.步骤5：对第二航行器设计控制率，以领航者为局部坐标原点，为两者期望
距离，p2＝[x2,y2]
t
为航行器在局部坐标系下的坐标，则对于第二跟随航行器控制输出为：
[0014][0015]
其中：为1号与2号航行器的期望距离；u2为2号航行器的控制输入。
[0016]
步骤6：对其余跟随航行器设计控制率，以领航者与第二航行器连线中点建立局部坐标系，以两个航行器的连线为x轴，设d
13
与d
23
分别为第三航行器分别到两个航行器的距离；θ为d
13
与d
23
的夹角且逆时针为正，则第三航行器的控制输出为其余跟随航行器的控制输出：
[0017][0018][0019]
其中：x3为3号航行器在参考坐标系下的横坐标，y3为3号航行器在参考坐标系下的纵坐标；u3为3号航行器的控制输入。
[0020]
所述步骤3中，获得领航航行器运动图像后，对获得的图像进行预处理，从环境杂光、光源信标的散射光的干扰下获取真实的光信标位置的过程为：
[0021]
步骤3.1：在提取出光信标的像素坐标后，对光源进行匹配，确定其相对位置，将唯一的绿色光源作为1号光源，其余3个蓝色光源按顺时针依次排序：
[0022]v←
max(r,g,b)
[0023][0024][0025]
步骤3.2：基于log算子对图像进行检测，提取出每个光源的边界；
[0026]
log算子定义为：
[0027][0028]
其中σ是高斯滤波器h
σ
(x,y)的标准差，是拉普拉斯算子，(x,y)是图像坐标。故使用log算子的图像可以首先通过使用高斯滤波器h
σ
(x,y)卷积图像，然后对由h
σ
(x,y)滤波的图像应用拉普拉斯算子来实现；
[0029]
当log算子与图像卷积时，其对较亮背景上σ范围内的较暗blobs或较暗背景上σ范
围内的较亮高于平均亮度σ范围内亮度低于平均亮度的blobs，与低于平均亮度σ范围内亮度高于平均亮度的blobs具有很强的响应，标准差σ计算公式为：
[0030][0031]
式中s为半径，是待检测blobs的描述子；
[0032]
步骤3.3：对3.2中提取出光源的边界轮廓采用otsu处理得到黑白图像，并提取出边界点的位置坐标；
[0033]
分割的评估函数为：
[0034][0035]
定义灰度级i在图中出现的概率pi＝gi/k，则最优阈值x
*
为：
[0036][0037]
步骤3.4：得到3.3得到的图像边界曲线，由于光源为圆形，为得到光源中心像素级坐标，利用最小二乘法对边界点进行拟合，设待拟合的圆的标准方程为：
[0038]
(x-a)2 (y-b)2＝r2[0039]
令：
[0040][0041][0042][0043][0044][0045]
解得：
[0046][0047][0048][0049]
得到圆形曲线的参数：
[0050][0051][0052][0053]
a为待拟合圆的圆心横坐标，b为待拟合圆的圆心纵坐标，r为待拟合圆的半径长度。
[0054]
所述步骤4中由的得到的像素坐标通过求解共面p4p的方法，确定领航水下航行器在三维空间中的位姿的过程为：
[0055]
步骤4.1：由光源点pi的图像坐标(ui,vi)，计算出点pi在相机的焦距归一化成像平
[0056]
面的成像点的坐标：
[0057][0058]
将p
ri
的坐标带入相机的外参数模型，得：
[0059][0060][0061]
对于n个已知的空间点，得到n组式所示的方程组，即2n个方程组；利用矩阵，将方程改写为：
[0062]
a1h1 a2h2＝0
[0063]
式中：
[0064][0065]
[0066]
h1＝[cn
rx cnry cnrz
]
t
[0067]
h2＝[co
rx cory corz c
p
rx c
p
ry c
p
rz
]
t
[0068]
||h1||＝1
[0069]
构造指标函数：
[0070]
f＝||a1h1 a2h2||2 λ(1-||h1||2)
[0071]
问题转换为在任意λ下，最小化指数函数f的问题，h1与h2的解为
[0072][0073][0074]
利用h1和h2的解，得到相机相对于参考坐标系的外参数cmr；其中，cmr的第三列由第一列和第二列叉乘得到；
[0075]
另外，相机相对于世界坐标系的外参数cmw满足：
[0076]cmw＝cm
rrmw
[0077]
其中，rmw是世界坐标系在参考坐标系中的位姿，由参考坐标系在世界坐标系中的位姿wmr获得，且：
[0078]rmw＝
wmr-1
[0079]
步骤4.2：对4.1得到的p4p的解采用递推最小二乘法进行迭代，保证求解的cmr中的姿态矩阵为单位正交矩阵；
[0080]cmw为摄像机坐标系相对于世界坐标系的变换矩阵，cmr为摄像机坐标系相对于对接坐标系的变换矩阵，rmw为对接坐标系与世界坐标系的变换矩阵。
[0081]
所述四个光源信标中一个不同发光体颜色的光源信标位于前进方向的左上方。
[0082]
所述不同发光体颜色的光源信标采用绿色光源，其余三个采用蓝色光源。
[0083]
所述视觉识别装置采用相机，
[0084]
所述水下航行器安装方位姿态控制系统，提供航行器自身的速度角速度信息。
[0085]
所述水下航行器安装多普勒测速仪，提供水下航行器的速度信息。
[0086]
有益效果
[0087]
本发明提出的一种基于水下图像信息的航行器自主编队方法，解决了在通讯约束条件下航行器实现编队的问题。与现有的方法对比，对于水下视觉信息的结算处理解决了水下图像噪声大、散射强的问题；所研究的视觉位姿估计方法将在多auv协同运动控制中为跟随者提供领航者的高精度六自由度信息，作为编队形成的基础。对编队运动的控制器设计充分利用相对位姿信息，建立局部坐标系，使航行器在不需要全局坐标的情况下实现编队运动的形成。
附图说明
[0088]
图1航行器尾端的光源指示灯示意图
[0089]
图2视觉匹配算法流程图
[0090]
图3hsv空间的光源匹配流程图
[0091]
图4编队示意图
[0092]
图5编队形成示意图
[0093]
图6各航行器与期望位置距离误差仿真图
[0094]
图7基于视觉信息编队控制流程图
具体实施方式
[0095]
现结合实施例、附图对本发明作进一步描述：
[0096]
基于单目视觉的水下航行器跟踪方法，用于多航行器协同系统。协同系统至少存在三条航行器，一条充当领航者，其余充当跟随者。航行器均搭载方位姿态测量系统与多普勒测速仪，能够实现航行器的角速度与速度控制。跟随航行器首部搭载单目相机，可实现对领航航行器运动观测。所有航行器尾部均搭载共面的发光信标充当目标点。跟随航行器在拍摄到领航航行器运动状态后，提取特征目标点的坐标信息，解算领行者的位姿信息。特别的，2号航行器仅从1号航行器(编队的总领航者)获得信息，其余跟随者均从其编号的前两个航行器获取信息，如：3号航行器从1号、2号航行器获取位姿信息。
[0097]
本实施例在硬件实现上，在领航水下航行器后端安装有视觉识别装置，视觉识别装置具有四个发光体作为光信标，通过光信标固定架布置在领航水下航行器后端周围，其中三个发光体颜色为蓝色，另一个发光体颜色为绿色。
[0098]
下航行器前端安装工业相机，最好帧率至少达到20帧以上，获取auv的前端图像信息；水下航行器中安装方位姿态测量系统，得到水下航行器实时的角速度及角度信息；在水下航行器中还安装多普勒测速仪，得到水下航行器实时的速度信息。
[0099]
下面结合附图并举实例，对本发明工作流程详细说明：
[0100]
s1：航行器四点式蓝绿光源设计与摄像机拍摄参数设定
[0101]
本发明设计了如图1所示的四点分布式圆形光源信标。四点分布式圆形光源信标由一个蓝色灯和三个绿色灯组成，蓝绿光在水下能够发出400-500nm的波长，有着最低的吸收率与散射率，有透光性强的特点。设置跟随航行器摄像机的拍摄参数，并设定跟随航行器摄像机的拍摄频率。搭载的摄像机一般要求帧率至少达到20帧以上。
[0102]
s2：对采集到的运动图像进行预处理，得到光源目标特征，并对目标光源在像素坐标系上位置进行检测，得到目标光源的像素坐标。视觉匹配流程如图2所示。
[0103]
s2-1：将相机采集到的原始图像进行色彩空间变换和初筛过滤，得到单通道的阈值图。摄像机获取的图像为rgb色彩空间下显示，本步骤将rgb色彩空间转换至hsv色彩空间，从中分离出不同强度不同颜色的区域。rgb到hsv颜色空间的转换公式为：
[0104]v←
max(r,g,b)
[0105]
[0106][0107]
通过颜色空间转换，根据转换后的颜色与亮度识别目标点光源，将绿色光源作为1号光源，其余3个蓝色光源按顺时针依次排序。
[0108]
具体为：
[0109]
图像色彩空间转换。图像中的颜色元素采用rgb颜色模型表示，rgb代表红、绿和蓝，图像的色彩空间由这三种颜色的亮度作为基向量而定义，值范围在0-255。然而，图像上的颜色很难用rgb的值来表示。因此，需要将图像的颜色空间转换为hsv颜色空间。hsv代表色调、饱和度和明度值，在opencv中，h通道的值范围为0-180，s通道的值范围为0-255，v通道的取值范围为0-255，hsv颜色模型是由rgb颜色模型转换而成的。识别的亮度最高的绿色光标为1号光源目标特征点，其余3个蓝色光源按顺时针依次排序。hsv空间的光源匹配流程如图3所示。
[0110]
s2-2：用log算子对图像进行检测，提取出每个光源的边界。由于拉普拉斯算子对噪声比较敏感，为了减少噪声的影响，引入高斯滤波，先对待处理的图像进行平滑，然后再用拉普拉斯算子提取边缘。
[0111]
具体通过log算子对得到的视觉信息做边缘检测。
[0112]
设f(x,y)为原图像在坐标(x,y)处的像素值，h
σ
(x,y)为高斯平滑函数，其定义为：
[0113][0114]
由高斯平滑函数滤波后的图像可以表示为原图像与平滑函数的卷积：
[0115][0116]
在二维图像中，拉普拉斯算子是一种二阶导数算子。对于图像f(x,y)，它在位置(x,y)的拉普拉斯值定义为：
[0117][0118]
对图像g(x,y)采用拉普拉斯算子进行边缘检测，可得：
[0119][0120]
由卷积性质有：
[0121][0122]
则log算子可以定义为：
[0123]
[0124]
其中σ是高斯滤波器h
σ
(x,y)的标准差，是拉普拉斯算子，(x,y)是图像坐标。故使用log算子的图像可以首先通过使用高斯滤波器h
σ
(x,y)卷积图像，然后对由h
σ
(x,y)滤波的图像应用拉普拉斯算子来实现。
[0125]
当log算子与图像卷积时，其对较亮背景上σ范围内的较暗blobs或较暗背景上σ范围内的较亮blobs具有很强的响应，“3σ”理论表明高斯函数99％的能量集中在其平均值的3个标准差内。因此标准差σ计算公式通常为：
[0126][0127]
式中s为半径，是待检测blobs的描述子。采用σ＝9进行边缘检测。
[0128]
s2-3：提取出光源的边界轮廓采用otsu处理得到黑白图像，并提取出边界点的位置坐标，且选用的最优阈值x
*
为：
[0129][0130]
具体提取后的边缘图像为灰度图像，将其进行otsu法(最大类间方差法)转化为黑白图像来提取出边界点的位置坐标。
[0131]
对于灰度图像i(x,y)，其总像素个数记为k，图像灰度级记为g＝0,1,2,...,l，处在灰度级i的像素个数记为gi，则有定义灰度级i在图中出现的概率为pi＝gi/k，则有假设背景和光源的灰度分割阈值为x，则图像可以划分为光源和背景两类记为c1与c2。则两类在图像中出现的概率分布可以描述为：
[0132][0133][0134]
背景和光源像素的平均灰度值分别记为μ0、μ1，则：
[0135][0136][0137]
其中μ为灰度图像的平均灰度值，对于任意阈值x有下列等式存在：
[0138]
ω0 ω1＝1,ω0μ0 ω1μ1＝μ
[0139]
背景与光源的灰度值方差可表示为：
[0140][0141][0142]
引入类内方差σw、总体方差σ和类间方差σb作为阈值x的评估函数：
[0143][0144]
其中：
[0145][0146][0147][0148]
使得λ、κ、η取最大时的x即为最优阈值，因为故λ、κ、η之间存在如下关系：
[0149][0150]
由上式可得λ、κ、η具有相同的单调性，且σ2可通过灰度图像直接求得，与x无关，故选择η作为分割的评估函数：
[0151][0152]
其中则最优阈值x
*
为：
[0153][0154]
s2-4：步骤2.3图像的边界曲线，由于光源为圆形，为得到光源中心像素级坐标，还要利用最小二乘法对边界点进行拟合。
[0155]
具体通过获得的图像中的边界曲线，通过最小二乘法对获得的边界点进行圆形曲线拟合，得到的圆心为目标特征点的光源中心，暨目标光源特征点在像素坐标系下的坐标。
[0156]
待拟合圆形曲线可描述为，其中(a,b)为圆心坐标，r为圆的半径：
[0157]
(x-a)2 (y-b)2＝r2[0158]
记我们通过图像分割得到的边界点集记为(xi,yi)(i＝1,2,...,n)，边界点至圆心的距离记为di(i＝1,2,...,n)，则有：
[0159][0160]
将边界点到圆心的距离的平方与实际圆的半径的平方差设为δi(i＝1,2,...,n)，有：
[0161][0162]
f(a,b,c)即为最小二乘拟合的损失函数，找到能使它最小的a、b、c参数。
[0163][0164][0165][0166]
最终解得：
[0167][0168][0169][0170]
其中：
[0171][0172][0173][0174][0175][0176]
则可得到圆形曲线的参数：
[0177][0178][0179][0180]
s3：由的得到的像素坐标通过求解共面p4p的方法，确定领航水下航行器在三维空间中的位姿。
[0181]
s3-1：由光源点pi的图像坐标(ui,vi)，可以计算出点pi在相机的焦距归一化成像平面的成像点的坐标：
[0182][0183]
将p
ri
的坐标带入相机的外参数模型，得：
[0184][0185][0186]
对于n个已知的空间点，可以得到n组式所示的方程组，即2n个方程组。利用矩阵，可将方程改写为：
[0187]
a1h1 a2h2＝0
[0188]
式中：
[0189][0190][0191]
h1＝[cn
rx cnry cnrz
]
t
[0192]
h2＝[co
rx cory corz c
p
rx c
p
ry c
p
rz
]
t
[0193]
||h1||＝1
[0194]
构造指标函数：
[0195]
f＝||a1h1 a2h2||2 λ(1-||h1||2)
[0196]
问题转换为在任意λ下，最小化指数函数f的问题，h1与h1的解为
[0197][0198][0199]
利用h1和h2的解，可以得到相机相对于参考坐标系的外参数cmr。其中，cmr的第三列由第一列和第二列叉乘得到。另外，相机相对于世界坐标系的外参数cmw满足：
[0200]cmw＝cm
rrmw
[0201]
其中，rmw是世界坐标系在参考坐标系中的位姿，由参考坐标系在世界坐标系中的位姿wmr获得，且：
[0202]rmw＝
wmr-1
[0203]
s3-2：以上述p4p问题的解作为初值，采用递推最小二乘法(recursive least square,rls)进行迭代，以此提高解的精度，使cmw更加接近单位正交矩阵。
[0204][0205][0206]
θ＝[cn
′
rx cn′
ry cn′
rz co′
rx co′
ry co′
rz ca′
rx ca′
ry ca′
ry c
p
′
rx c
p
′
ry
]
t
[0207]
式中cn
′r＝cnr/cp
rz
；co
′r＝cor/cp
rz
；
ca′r＝
car
/cp
rz
；cp
′
rx
＝cp
rx
/cp
rz
；
[0208]cp
′
ry
＝cp
ry
/cp
rz
；k为加强系数，用于在递推过程中增强姿态误差的作用，以便于最大限度消除姿态误差。
[0209]
rls的初始值pn由式(3-62)给出。由4点中任意3点和1个辅助点组成的4个点，分别利用式(3-59)计算然后计算出pn。利用3.4.1节中的方法获得的粗略姿态值，也通过的计算，引入到rls的初值pn。θ的初值来源于3.4.1节中的方法获得的粗略值中的姿态。
[0210][0211]
采用带有遗忘因子的rls方法计算精确的相机外参数，具体算法描述如下：
[0212][0213][0214][0215]
式中：ρ为遗忘因子。
[0216]
根据全部已知点的笛卡尔空间位置和图像坐标进行递推计算。当递推过程收敛时，获得θ的精确值，从而获得相机的精确外参数。
[0217]
s4对2号跟随航行器单独设计控制律，其步骤为：以编队领航者，即1号机器人为局部坐标原点为两者期望距离，p2＝[x2,y2]
t
为航行器在局部坐标系下的坐标，则对于2号跟随航行器；
[0218]
具体包括：对2号跟随航行器单独设计控制律。
[0219]
考虑水下航行器单积分系统
[0220][0221]
其中pi＝[xi,yi]
t
∈r2，i∈{1,2,3}是第i号航行器在局部坐标系下的位置坐标，ui∈r2是跟随航行器需要确定的控制输入。假定2号航行器的局部坐标系原点位于1号航行器的位置坐标处。选取李雅普诺夫函数：
[0222][0223]
设2号航行器在局部坐标系中的位置坐标系下的坐标为p2＝[x2,y2]
t
。则v2是正定的且关于向量是径向无界的。选取则对于所有的有：
[0224][0225]
是单积分系统的全局渐进稳定平衡点。则可以得到2号航行器的控制输入为：
[0226][0227]
s5具体包括：对编号为3的跟随航行器设计控制律。
[0228]
如图4，假设水下仿生机器人3的局部坐标系原点位于水下仿生机器人1和2连线的中点处，以水下仿生机器人1和2的连线为x轴，如图所示。设3号航行器至1号航行器的距离为d
13
，至2号航行器的距离为d
23
，d
13
与d
23
之间夹角为θ，则3号航行器的坐标p3＝[x3,y3]
t
满足：
[0229][0230]
整理得：
[0231][0232]
定义：
[0233][0234]
选取李雅普诺夫函数：
[0235][0236]
选取则则3号航行器的控制输入为：
[0237][0238]
为了验证上述导引方法的有效性，在本发明中还提供了如下实施例。
[0239]
实施例
[0240]
设有3艘航行器，其初始位置分别为u1＝[1 1]
t
，u2＝[2 3]
t
，u3＝[3 4]
t
。期望2号航行器运动距1号航行器1.34m，3号航行器距2号航行器2.2m,且1号、2号与3号航行器夹角
为160
°
；各航行器与期望位置距离误差如图6所示。