水下机器人控制系统及基于声纳图像目标识别的航向控制
方法
技术领域
背景技术
[0002] 随着人类对海洋开发步伐的前进，水下机器人作为一种海洋开发的探测工具被发明出来，水下机器人被投放到水下，需要向目标开进，但由于水下接收不到GPS
信号，技术成熟的GPS导航在水下不可用。目前人为实时操控通过水下摄像头观察前方
视野，不断调整水下机器人的航向；在自动控制方面，目前水下机器人在水下作业时定向导航通用的都是采用惯导器件，但惯性制导中，
陀螺仪由于受
摩擦力、机械精密误差等因素，
精度不高，长时间运行后误差会变大,同时控制舱里电
磁场的干扰,给陀螺仪正常工作带来很大的干扰,精度大大降低。随着计算机技术及数字图像技术的发展，图像制导技术发展起来，该技术通常都是采用成像
传感器采集目标图像，利用目标图像来制导，最终将被导物导向目标，其优点是不受
电磁干扰。但是由于在水下，摄像头的可视距离较低，从而影响了图像制导技术的应用。而声纳的探测成像范围很远，因此,研究一种基于声纳图像目标识别的水下机器人航向控制方法具有重要意义。
发明内容
[0003] 本发明的目的在于提供一种水下机器人控制系统及基于声纳图像目标识别的航向控制方法，实现水下机器人航向对准目标的自动控制，解决了长时间手动调节航向工作效率低，和水下惯导受电磁干扰而导致制导精度不高的的问题，实现了水下机器人航向的自动控制。
[0004] 本发明的目的通过以下技术方案予以实现：
[0005] 一种水下机器人控制系统，包括水面控制系统、水下控制系统，所述水面控制系统放置在岸上或者船上，水下控制系统安装在水下检测与作业机器人上；水面控制系统和水下控制系统通过脐带缆5相连，实现
电能、
控制信号、数据的传输；水面控制系统包括
单片机控制面板1、工控机2、
液晶显示屏3、电源箱4，所述单片机控制面板1、液晶显示屏3和工控机2相连，工控机2与脐带缆5相连，电源箱4与脐带缆5相连；
[0006] 所述水下系统包括脐带缆5、K60嵌入式微
控制器6、电源单元7、声纳8、水下
云摄像台9、
电机单元10、机械手11、惯导磁力计12、传感器单元13；所述脐带缆5与K60嵌入式
微控制器6、电源单元7、声纳8、水下云摄像台9相连；所述K60嵌入式微控制器6通过脐带缆5与水面控制系统相连，接收水面控制系统的控制指令并将水下采集的数据传送给水面控制系统；所述K60嵌入式微控制器6与电机单元10、机械手11、惯导磁力计12、传感器单元13相连；所述电源单元7给K60嵌入式微控制器6、声纳8、水下云摄像台9、电机单元10、机械手11、惯导磁力计12、传感器单元13供电；所述声纳8将水下声纳数据通过脐带缆5传送到水面控制系统；所述水下云摄像台9将水下视频数据传送到水面控制系统；所述电机单元10接收K60嵌入式微控制器6的速度和方向信息，驱动螺旋桨旋转，并将
电流值返回给K60嵌入式微控制器6；所述机械手11接收K60嵌入式微控制器的方向信息，控制机械手张、合；所述惯导磁力计12将导航数据传给K60嵌入式微控制器6；所述传感器单元13将深度信息、漏水信息、温湿度信息传给K60嵌入式微控制器6，经由脐带缆5传送到水面控制系统。
[0007] 一种水下机器人控制系统的基于声纳图像目标识别的航向控制方法，包括以下步骤：
[0008] 第一步：声纳回
波数据成像，声纳头发出
声波，声纳头采集回波数据成像，得到水下视野前方声纳原始图像；
[0009] 第二步：声纳
图像处理，包括图像增强、
图像分割，图像增强包括灰度变换和中值滤波，图像分割采用基本的全局
阈值处理；
[0010] 第三步：目标识别，包括目标特征的提取、匹配，目标特征提取采用归一化中心距作为目标匹配特征，匹配采用距离度量，计算未知模式的不变距特征与目标模式不变距特征的欧式距离，若距离小于一定范围，则认为属于同一目标图像；
[0011] 第四步：目标质心坐标计算，采用计算图像的两个一阶距，和零阶距，来计算图像的质心，即二值图像的几何中心
[0012] 第五步：目标质心偏差的计算，计算目标质心坐标与图像显示区域原点(0,0)的差值：
[0013]
[0014] 第六步：对偏差e进行PID控制公式的计算
[0015]
[0016] 将控制量u输出到水下控制系统的航向控制电机的PWM值，调节航向，使航向向着目标质心方向偏转；
[0017] 第七步：水下机器人在航向控制电机的推动下往目标航向调节；
[0018] 第八步：间隔一段时间，待航向调节响应完毕，返回到第1步，依次循环整个流程,形成闭环控制；
[0019] 第九步：最终水下机器人的航向调节到指向目标质心。
[0020] 本发明的目的还可以通过以下技术措施来进一步实现：
[0021] 前述水下机器人控制系统的基于声纳图像目标识别的航向控制方法，其中第二步所述中值滤波是基于排序统计理论的一种能有效抑制噪声的非线性
信号处理技术，是把数字图像或数字序列中一点的值用该点的一个领域中各点值的中值代替，从而消出孤立的噪声点；实现方法为：设二维图像的
像素灰度集合为{Xi,j，(i,j)∈Z2},Z2是二维整数集，规定二维滑动模板大小为A＝m×n，在图像上的每个像素点进行滑动，窗口的像素值中值被定义为：
[0022]
[0023] 上式表示把模板窗口内的奇数个像素按灰度值大小排序，生成单调上升或下降的二维数据序列，取中间像素赋给Yi,j，然后Yi,j取代二维窗口A中的中心像素值作为输出；
[0024] 所述灰度变换采用线性灰度变换，令原图像像素灰度f(x,y)范围为[a,b],线性变换后图像像素灰度g(x,y)的范围为[c,d][11],则有灰度f(x,y)与灰度g(x,y)之间的关系式：
[0025]
[0026] 经过灰度变换之后，声纳图像
对比度增加，目标得以凸显，进行后续图像处理；
[0027] 图像分割采用基本的全局阈值处理，采用
迭代算法自动估计一幅图像的的阈值，步骤如下：
[0028] 1)为全局阈值T选择一个初始估计值；
[0029] 2)在下式中：
[0030]
[0031] 用T分割该图像，这将产生两组像素：G1由灰度值大于T的所有像素组成，G2由所有小于等于T的像素组成；
[0032] 3)对G1和G2的像素分别计算平均灰度值m1和m2；
[0033] 4)计算一个新的阈值：
[0034]
[0035] 5)重复步骤2到步骤4，直到连续迭代中的T值间差小于一个预定义的参数ΔT为止。
[0036] 前述水下机器人控制系统的基于声纳图像目标识别的航向控制方法，其中第三步所述目标特征提取采用归一化中心距作为目标匹配特征，归一化中心距具有平移、旋转、比例不变性；大小为M×N的数字图像f(x,y)的二维(p+q)阶距定义为：
[0037]
[0038] 其中p＝0,1,2,···和q＝0,1,2,···是整数，相应的(p+q)阶中心距定义为[0039]
[0040] 式中，p＝0,1,2,···和q＝0,1,2,···是整数，其中
[0041]
[0042]
[0043] 由ηpq表示的归一化中心距定义为
[0044]
[0045] 式中，
[0046]
[0047] 其中p+q＝2,3,···，采用由二阶距和三阶距构造的不变距：
[0048] φ2＝(η20-η02)2+4η112 (11)
[0049] 作为不变距特征，具有图像的平移、缩放、旋转不变性；
[0050] 匹配采用距离度量，计算未知模式的不变距特征与目标模式不变距特征的欧式距离，若距离小于一定范围，则认为属于同一目标图像；距离度量公式为：
[0051] d＝||φ20-φ2|| (12)
[0052] 其中，φ2是目标模式的不变距特征，φ20是未知模式的不变距特征，采用距离d0作为判定界限，若d＜d0，则未知模式属于目标模式，若d＞d0则未知模式不属于目标模式。
[0053] 前述水下机器人控制系统的基于声纳图像目标识别的航向控制方法，其中第四步所述两个一阶距：
[0054]
[0055]
[0056] 零阶距
[0057]
[0058] 质心坐标
[0059]
[0060] 与
现有技术相比，本发明的有益效果是：本发明的水下检测与作业机器人控制系统采用模
块化设计，方便安装与拆卸；配备了水下摄像头，声纳，机械手，水上控制系统可以实时监控水下情况，机械手可以在水下抓取物体；本发明采用了基于声纳目标识别的航向控制方法，克服了传统惯性导航受
电子舱磁场干扰航向数据不稳定的弊端，本发明的航向控制的精度较高。
附图说明
[0061] 图1为本发明的ROV控制系统整体结构
框图；
[0062] 图2为水面控制系统结构框图；
[0063] 图3为水下控制系统结构框图；
[0064] 图4为声纳图像处理过程图；
[0065] 图5水下机器人
位置偏差的PID闭环控制图；
具体实施方式
[0067] 下面结合附图和具体
实施例对本发明作进一步说明。
[0068] 本发明的水下结构物检测与作业机器人控制系统，如图1所示，系统包括水面和水下两个系统，水面系统放置在岸边或者母船上，水下系统安装在水下结构物检测机器人上；水面控制系统包括单片机控制面板1，工控机2，液晶显示屏3，电源箱4；水下控制系统包括K60嵌入式微控制器6、电源单元7、声纳8、水下云摄像台9、电机单元10、机械手11、惯导磁力计12、传感器单元13。
[0069] 水面控制系统如图2所示，包括单片机控制面板1，工控机2，液晶显示屏3，电源箱4；单片机控制面板1与工控机2相连，接收4个方向控制摇杆(水平电机)106,2个方向控制摇杆(垂直电机)107，2个方向控制摇杆(横向电机)108，
横滚旋钮(垂向电机)109，电机增益旋钮110，摄像机
角度调节旋钮111，探照灯角度调节旋钮112，定航按钮113，定深按钮114，视频图像输出选择按钮(摄像机、声纳)115，
键盘116，
手柄接口117的操作指令，将操作指令传送到工控机2，工控机通过脐带缆5传送到水下控制系统。电子舱
过热指示灯101，电子舱漏水指示灯102，摄像
机舱过热指示灯103，摄像机舱漏水指示灯104，电机过流指示灯105接收来自工控机2的报警信息，灯点亮说明水下控制系统出现了故障；液晶显示屏3，显示界面包括声纳图像显示区301，视频图像显示区302，水平仪303，罗盘304，航向
姿态信息305，定航定深输入设定306。接收来自工控机2的数据，显示水下声纳图像，显示水下视频图像，显示水下机器人的水平状态，航向姿态信息，同时采集定航定深输入设定306，将数据发送给工控机2；工控机2，包括3路485转USB通信模块201，视频采集卡202。3路485转USB通信模块分别与单片机控制面板1，液晶显示屏2，脐带缆5相连，实现工控机2与单片机控制面板1、液晶显示屏3、水下控制系统的通信；电源箱4将220VAC转换成400VDC通过脐带缆5传送到水下控制系统。
[0070] 水下控制系统如图3所示，包括K60嵌入式微控制器6、电源单元7、声纳8、水下云摄像台9、电机单元10、机械手11、惯导磁力计12、传感器单元13；声纳8与脐带缆5的2根数据线相连，给水面控制系统传送声纳数据；水下云摄像台9与脐带缆5的2根数据线相连，给水面控制系统传送视频数据；K60嵌入式微控制器6选择是基于ARM Cortex-M4
内核的微控制器，包括有
定时器、
模数转换器、
存储器、串行通信模块、以太网控制、通用输入输出口，系统上电后，嵌入式微控制器6首先对各模块功能进行初始化，完成后向水面系统发送一个自检完成的信号，之后进入从机状态，接收水面系统的指令，并作出相应控制。
[0071] 电源单元7与脐带缆的2根电源线相连，高压电源板701接收水面电源箱4的400VDC电源，将400VDC转换成8路48V500WDC，低压电源板702将48VDC转换成12VDC，将12VDC转换成5VDC，将5VDC转换成3.3VDC；分别给电机单元10、声纳8、水下云摄像台9、K60嵌入式微控制器6、机械手11、惯导磁力计12、传感器单元13供电。K60嵌入式微控制器6通过uart转485模块601与脐带缆5的2根数据线相连，接收水面控制系统的控制指令和发送水下数据到水面控制系统。5路PWM信号5路方向信号602接收来自K60嵌入式微控制器6的PWM信号、电机方向信号，给5路电机驱动1006，控制5路电机的转向、转速，5路电流采集1007采集5路电机的电流模拟量，经5路AD采集603转
化成数字量,给K60嵌入式微控制器6，如果电流达到极限值，则K60嵌入式微控制器6发出控制指令，停止电流过大的那一路电机，起到保护电机的作用，同时电流值经Uart转485模块11通过脐带缆5传送到水面控制系统，水面控制系统可以实时监测水下电机电流值；2路PWM信号模块604与机械手11相连，控制机械手的张开、关闭。Uart通信605接收来自惯导磁力计12的导航数据，再通过Uart转485模块经脐带缆5传送到水面控制系统，实时监测水下作业与监测机器人的的航向、姿态信息；传感器单元13包括深度计
1301、漏水检测模块1302、温
湿度传感器1303；深度计1301采集深度传感器转换的
电压值，给AD采集模块606，将电压模拟量转换成数字量；漏水检测模块1302采集漏水传感器的电压值，经AD采集模块606转换成数字量；温湿度传感器1303采集
温度湿度传感器的电压值，经AD采集模块606转换成数字量；K60嵌入式微控制器6接收到传感器单元13AD转换后的数字量，通过Uart转485模块601将
传感器数据上传到水面控制系统；惯导磁力计12采集的航向值的数字量和深度计1301采集的深度值的数字量可以被用来做定航定深控制。
[0072] 本发明的基于声纳图像目标识别的航向控制方法，该方法采用计算目标图像质心坐标与图像视野原点坐标的偏差来调节水下机器人的航向，整个过程如图4所示，具体步骤如下：
[0073] 第一步：声纳回波数据成像，声纳头发出声波，声纳头采集回波数据成像，得到水下视野前方声纳原始图像。
[0074] 第二步：声纳图像处理，包括图像增强，图像分割。图像增强包括中值滤波和灰度变换，图像分割采用基本的全局阈值处理。中值滤波，中值滤波是基于排序统计理论的一种能有效抑制噪声的非线性信号处理技术，典型的中值滤波是把数字图像或数字序列中一点的值用该点的一个领域中各点值的中值代替，从而消出孤立的噪声点。实现方法：设二维图像的像素灰度集合为{Xi,j，(i,j)∈Z2},Z2是二维整数集。规定某种结构的二维滑动模板大小为A＝m×n(一般含奇数个像素)在图像上的每个像素点进行滑动，窗口的像素值中值被定义为：
[0075]
[0076] 上式表示把模板窗口内的奇数个像素按灰度值大小排序，生成单调上升(或下降)的二维数据序列，取中间像素赋给Yi,j，然后Yi,j取代二维窗口A中的中心像素值作为输出。
[0077] 灰度变换采用线性灰度变换，令原图像像素灰度f(x,y)范围为[a,b],线性变换后图像像素灰度g(x,y)的范围为[c,d][11],则有灰度f(x,y)与灰度g(x,y)之间的关系式：
[0078]
[0079] 经过灰度变换之后，声纳
图像对比度增加，目标得以凸显，有利于后续图像处理。
[0080] 图像分割采用基本的全局阈值处理，当物体和
背景像素的灰度分布十分明显时，可以用适用于整个图像的单个(全局)阈值。采用迭代算法自动估计一幅图像的的阈值，步骤如下：
[0081] 1：为全局阈值T选择一个初始估计值。
[0082] 2：在式子：
[0083]
[0084] 式中，用T分割该图像。这将产生两组像素：G1由灰度值大于T的所有像素组成，G2由所有小于等于T的像素组成。
[0085] 3：对G1和G2的像素分别计算平均灰度值(均值)m1和m2。
[0086] 4：计算一个新的阈值：
[0087]
[0088] 5：重复步骤2到步骤4，直到连续迭代中的T值间差小于一个预定义的参数ΔT为止。
[0089] 第三步：目标识别，包括目标特征的提取，匹配。目标特征提取采用归一化中心距作为目标匹配特征，归一化中心距具有平移、旋转、比例不变性。大小为M×N的数字图像f(x,y)的二维(p+q)阶距定义为
[0090]
[0091] 其中p＝0,1,2,···和q＝0,1,2,···是整数。相应的(p+q)阶中心距定义为[0092]
[0093] 式中，p＝0,1,2,···和q＝0,1,2,···是整数，其中
[0094]
[0095]
[0096] 由ηpq表示的归一化中心距定义为
[0097]
[0098] 式中，
[0099]
[0100] 其中p+q＝2,3,···。采用由二阶距和三阶距构造的不变距：
[0101] φ2＝(η20-η02)2+4η112 (11)
[0102] 作为不变距特征，具有图像的平移、缩放、旋转不变性。
[0103] 匹配采用距离度量，计算未知模式的不变距特征与目标模式不变距特征的欧式距离，若距离小于一定范围，则认为属于同一目标图像。距离度量公式为：
[0104] d＝||φ20-φ2|| (12)
[0105] 其中，φ2是目标模式的不变距特征，φ20是未知模式的不变距特征，采用距离d0作为判定界限，若d＜d0，则未知模式属于目标模式，若d＞d0则未知模式不属于目标模式。
[0106] 第四步：目标质心坐标计算，采用计算图像的两个一阶距，和零阶距，来计算图像的质心，即二值图像的几何中心 两个一阶距
[0107]
[0108]
[0109] 零阶距
[0110]
[0111] 质心坐标
[0112]
[0113] 第五步：目标质心偏差的计算，计算目标质心坐标与图像显示区域原点(0,0)的差值：
[0114]
[0115] 第六步：对偏差e进行PID控制公式的计算
[0116]
[0117] 将控制量u输出到水下控制系统的航向控制电机的PWM值，调节航向，使航向向着目标质心方向偏转。
[0118] 第七步：水下机器人在航向控制电机的推动下往目标航向调节。
[0119] 第八步：间隔一段时间，待航向调节响应完毕，返回到第1步，依次循环整个流程,形成闭环控制。
[0120] 第九步：最终水下机器人的航向调节到指向目标质心。
[0121] 除上述实施例外，本发明还可以有其他实施方式，凡采用等同替换或等效变换形成的技术方案，均落在本发明要求的保护范围内。
