CN107730539A - 自主水下机器人控制系统及声纳目标跟踪方法 - Google Patents

自主水下机器人控制系统及声纳目标跟踪方法 Download PDF

Info

Publication number
CN107730539A
CN107730539A CN201711096917.6A CN201711096917A CN107730539A CN 107730539 A CN107730539 A CN 107730539A CN 201711096917 A CN201711096917 A CN 201711096917A CN 107730539 A CN107730539 A CN 107730539A
Authority
CN
China
Prior art keywords
image
module
target
sonar
particles
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201711096917.6A
Other languages
English (en)
Other versions
CN107730539B (zh
Inventor
曾庆军
张家敏
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jiangsu University of Science and Technology
Original Assignee
Jiangsu University of Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Jiangsu University of Science and Technology filed Critical Jiangsu University of Science and Technology
Priority to CN201711096917.6A priority Critical patent/CN107730539B/zh
Publication of CN107730539A publication Critical patent/CN107730539A/zh
Application granted granted Critical
Publication of CN107730539B publication Critical patent/CN107730539B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1671—Programme controls characterised by programming, planning systems for manipulators characterised by simulation, either to verify existing program or to create and verify new program, CAD/CAM oriented, graphic oriented programming systems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T5/00—Image enhancement or restoration
    • G06T5/70—Denoising; Smoothing
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/10—Segmentation; Edge detection
    • G06T7/136—Segmentation; Edge detection involving thresholding
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/10—Segmentation; Edge detection
    • G06T7/194—Segmentation; Edge detection involving foreground-background segmentation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/60—Analysis of geometric attributes
    • G06T7/66—Analysis of geometric attributes of image moments or centre of gravity
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20036—Morphological image processing

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Artificial Intelligence (AREA)
  • Software Systems (AREA)
  • Mathematical Physics (AREA)
  • Fuzzy Systems (AREA)
  • Evolutionary Computation (AREA)
  • Multimedia (AREA)
  • Geometry (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)

Abstract

本发明公开了一种自主水下机器人控制系统及声纳目标跟踪方法，提供了一种能够实现遥控作业和自主作业两种功能的控制系统以及一种自主水下机器人声纳目标跟踪方法。整个跟踪流程包括目标检测、目标质心点特征提取和目标跟踪三部分。在声纳目标检测方面，本发明直接从声纳原始伪彩色图像中提取出运动目标，更加方便快捷，同时引入背景更新因子实时更新背景。在声纳目标跟踪方面，本发明引入基于分类重采样的粒子滤波声纳目标跟踪算法，重采样过程中根据预设权值参考值保留大权值粒子，去掉小权值粒子，同时根据挑选出粒子个数的多少采用不同的复制方案，一定程度上保持了粒子的多样性。

Description

自主水下机器人控制系统及声纳目标跟踪方法
技术领域
本发明涉及一种自主水下机器人，尤其涉及一种自主水下机器人的控制系统及声纳目标跟踪方法，属于机器人技术领域。
背景技术
地球上海洋空间非常广阔，蕴藏着丰富的资源。随着陆地资源的不断减少，人类逐渐向海洋资源开发进军。合理开发利用海洋资源，保护海洋生物与环境对人类的发展与稳定具有重大意义。由于海洋环境具有复杂性与不确定性，通过载人探测进行海洋开发的方式极其危险，水下机器人在这样的背景下便应用而生。无人自主水下机器人作为一种无缆的自主水下机器人，与装载母船无连接，通过本身携带的高能量电池等能源来实现自主航行，可以实时将水下信息上传给母船。无人自主水下机器人具有较强的环境适应性，较大的活动范围和灵活性，在海洋资源勘探、海底工程作业、科研考察等诸多方面都有着广泛的应用，是人类最优秀的水下作业工具之一。
由于水对光电等信号的吸收比率比在空气中要高得多，而对声波信号的吸收比率较低且声波信号在水中的传输速度比在空气中快得多，这就导致基于声纳的探测手段是水下环境特别是深海环境中进行探测的主要手段。声纳系统作为无人自主水下机器人的眼睛，对于其实现避障，导航，水下信息探测等功能都有着不可取代的作用。除此之外，由于水下运动物体有可能是我们感兴趣的目标或者成为水下机器人的潜在威胁，因而对运动物体进行准确有效的跟踪对水下机器人执行对应的探测任务和确保水下机器人的安全航行都有着非常重要的意义。
本发明结合研制的自主水下机器人，首先引入一种基于RGB色彩空间的声纳目标检测算法，与传统的目标检测方法相比，本算法直接从声纳原始伪彩色图像中提取出运动目标，改变了传统目标检测方式中将伪彩色图像先转为灰度图像，经预处理后再进行目标检测的过程，更加方便快捷。由于颜色特征比灰度特征更易区别，即目标物体与背景灰度相同的区域，颜色一般不相同，因此克服了传统的背景差分法等目标检测方法在物体与背景的灰度值接近时造成分割出的目标区域存在大面积破裂或空洞的问题。
在声纳目标跟踪算法上，本发明引入一种基于分类重采样的粒子滤波声纳目标跟踪算法。与卡尔曼滤波相比，粒子滤波算法概念清晰，摆脱了解决非线性滤波问题中随机量必须满足高斯分布的制约条件，它还可以进行并行计算，能较好地满足实时场合的要求。然而随着粒子滤波迭代次数的增加会出现粒子匮乏的问题，此时需要进行粒子滤波重采样处理。常规重采样算法虽然在一定程度上缓解了粒子数退化的问题，但对于大权值粒子的多次复制会产生粒子贫化的现象，失去粒子多样性。本发明采用的基于分类重采样的粒子滤波声纳目标跟踪算法在重采样过程中根据预设权值参考值保留大权值粒子，去掉小权值粒子，同时根据挑选出粒子个数的多少采用不同的复制方案，一定程度上保持了粒子的多样性。申请号为201600180413.1名称为“基于图像检测的智能电表”的专利文献，其采用了常规的灰度变换、滤波、二值化处理等图像预处理方法，相对本发明的目标检测方法显得过于繁琐；申请号为201410422500.4名称为“基于改进粒子滤波的目标跟踪方法”的专利文献，其采用的粒子滤波重采样方法存在着计算量大的缺点。
发明内容
本发明的目的在于提供一种自主水下机器人控制系统及声纳目标跟踪方法，本发明提供一种可以实现遥控作业和自主作业两种工作模式，配备有独立电源管理系统的模块化控制系统，以及一种基于颜色空间的声纳目标检测算法和基于粒子群优化的自适应粒子滤波跟踪算法。
本发明的目的通过以下技术方案予以实现：
一种自主水下机器人控制系统，包括水面控制系统和水下控制系统，所述水面控制系统包括笔记本1、第一光电转换器2、光纤3、第一数传电台10；所述光电转换器2与笔记本1相连，所述光纤3与光电转换器2相连，数传电台10 与笔记本1相连；
所述水下控制系统包括第二光电转换器4、云台摄像头5、PC104主控板6、电源管理系统7、电池模块8、stm32从控板9、第二数传电台11、传感器模块 12、水下灯13、水平舵机14、垂直舵机15、电机模块16、漏水检测模块17、下载口18、抛载模块19；所述第二光电转换器4和第二数传电台11与PC104 主控板6相连，接收水面控制系统控制指令并将水下控制系统采集到的数据传至水面控制系统；所述电池模块8通过电源管理系统7给整个水下控制系统供电；电机模块16接收stm32从控板9发出的控制指令，驱动螺旋桨转动；水平舵机 14接收stm32从控板9的信号，改变自主水下机器人的运行方向，垂直舵机15 接收stm32从控板9的信号，控制自主水下机器人的上浮下潜；水下灯13接收 stm32从控板9的控制信号；电源管理系统7和漏水检测模块17检测电池状态信息和机器人内部环境信息发送给stm32从控板9；传感器模块12采集位置、姿态、深度、声纳数据传回PC104主控板6；PC104主控板6通过stm32从控板 9采集水下控制系统各单元数据，并通过第二光电转换器4或第二数传电台11发至水面控制系统。
一种自主水下机器人控制系统的声纳目标跟踪方法，包括以下几个步骤：
步骤1：基于RGB色彩空间的声纳目标检测算法，通过以下几个步骤实现：
1)从声纳采集到的连续H帧图像序列f1、f2、...、fH中抽取L帧图像，记为I1、I2、...、IL；当目标运动速度大于0.1m/s时，L＝H，当目标运动速度小于0.1m/s时，采用间隔3帧抽取图像的方式对L进行取值；
2)采用k＝1、2、…、L，来表示第k帧图像像素点 (i,j)处的RGB分量，i和j分别代表图像像素点横坐标和纵坐标，然后对按照递增或递减的顺序进行排序，将排序结果记为
3)取三个数列的中间值，记做将这三个值作为像素点(i,j)处背景像素所对应的 RGB值；
4)由于在实际场景中背景总是在变换，故而需要对背景进行实时更新，由于水下机器人移动速度较慢，采集到的声纳图像背景变化较慢，引入背景更新因子来自适应更新背景：
式中，为当前帧图像在像素点(i,j)处的RGB分量， In(i,j)为当前帧图像在像素点(i,j)处的像素值，为更新前的背景帧图像在像素点(i,j)处的RGB分量，Cn-1(i,j)为更新前的背景帧图像在像素点(i,j)处的像素值，Cn(i,j)为更新后的背景帧图像在像素点(i,j)处的像素值，α为背景更新因子，取为0.5，Δ1为当前帧图像与更新前的背景帧图像在像素点(i,j)处的RGB差值和，TH1为当前图像与背景图像的划分阀值，取为100；
5)对当前帧图像每个像素点的R,G,B三通道分别进行差分处理：
式中，为当前帧图像在像素点(i,j)处的RGB分量，为背景帧图像在像素点(i,j)处的RGB分量，为差值图像在像素点(i,j)处的RGB分量；
然后对差分后的3个分量进行如下处理，从而获得目标在像素点(i,j)处的二值图像RIn(i,j)：
式中，RIn(i,j)为声纳目标在像素点(i,j)处的二值图像，TH2为目标检测阀值，取为125；
6)受图像中噪声影响，运动物体内部会出现空洞或是图像不连续的现象，采用数学形态学开运算来对差分后的二值图像进行处理，通过先腐蚀运算后膨胀运算的过程达到去除声纳目标图像中的斑点噪声，平滑目标图像边缘，在不明显改变目标图像面积和特征的前提下获得较为平滑的目标图像；
步骤2：提取运动目标的质心点：
运动目标图像的质心点可由下式求出：
式中，f(x,y)表示M×N的数字图像，M和N分别代表X方向和Y方向的像素点个数，xo，yo分别为目标质心点的横坐标、纵坐标；
步骤3：进行基于分类重采样的粒子滤波声纳目标跟踪，
假设目标跟踪的数学模型为：
式中，xt表示t时刻的目标状态值，zt表示t时刻的目标观测值，f(xt-1,wt-1) 表示系统状态转移函数，h(xt,vt)表示系统观测函数，wt-1表示跟踪过程中的系统噪声，vt表示跟踪过程中的观测噪声；
1)初始化粒子和权值：t＝0时，由先验概率p(x0)采样得到i＝1,2,…,N，每个粒子权值即以目标质心点(x0,y0)为中心初始化N个等权值的目标粒子；
2)系统状态转移：t＝t+1时刻，进入下一帧声纳图像，采样计算重要性权值
式中为重要性密度函数，为了便于实现，选取先验概率密度作为重要性密度函数，即则有重要性权值
3)根据将每个粒子权值进行归一化处理；
4)进行粒子滤波重采样：随着粒子滤波迭代次数的增加会导致粒子权重集中于少数粒子上，大量计算浪费在更新对整个系统帮助很小的小权值粒子上；此时需要进行粒子滤波重采样，复制权值较大的粒子，去除权值较小的粒子；定义有效采样尺度设定有效样本数作为重采样阀值，当Neff＜Nth时，采用分类重采样算法进行重采样，主要包括粒子的挑选和复制两个过程，具体步骤如下：
(1)选取作为参考权值，将每个粒子权值与参考权值相比较，保留大于等于参考权值的粒子，去除小于参考权值的粒子；
(2)统计出挑选出的粒子个数，记为m，并将这些粒子按照权值大小排序，求取这m个粒子的权值和及平均权值，分别记为sum、ave；
(3)选取作为粒子复制的门限值，复制过程为从权值最大的粒子开始进行复制直到粒子总数达到N，当时，每个粒子的复制次数为小于等于h＝1,2,…,m的最大整数；当时，每个粒子的复制次数为大于等于h＝1,2,…,m的最小整数；对于复制之后粒子总数仍未达到N的特殊情况，按照粒子权值从大到小的顺序依次复制，直到粒子总数达到N；
(4)将重采样之后所有粒子的权值设定为即令原有的带权样本采样为等权样本
5)输出目标的状态估计
6)再次进入t+1时刻，判断是否是声纳图像结束帧，如果是则跟踪结束，如果不是则继续下一次跟踪过程，转至步骤2。
本发明的目的还可以通过以下技术措施来进一步实现：
前述自主水下机器人控制系统，其中传感器模块12包括GPS模块1201、导航模块1202、深度计模块1203、声纳模块1204，所述GPS模块1201实时采集机器人的位置信息，包括经度、纬度和高度，所述导航模块1202采集机器人的姿态信息，包括俯仰角、横滚角和航向角，所述深度计模块1203采集机器人深度值，所述声纳模块1204采集水下物体信息，所述GPS模块1201、导航模块 1202、深度计模块1203、声纳模块1204连接至PC104主控板6，将采集到的数据传至PC104主控板6，经过PC104主控板6处理之后发送至水面控制系统，采用光纤遥控作业模式时，所述数据经由光纤3发送，采用自主作业模式时，所述数据经由数传电台11发送。
前述自主水下机器人控制系统，其中电源管理系统7包括DCDC转换模块和电池监测模块；所述电池模块8采用锂电池串并而成，输出电压为12VDC，所述数传电台11、PC104主控板6、电机模块16、水下灯13、云台摄像头5、声纳模块1204采用12VDC供电方式，通过将电池模块8输出电压经过DCDC 稳压模块701稳压后进行供电，所述抛载模块19、水平舵机14、垂直舵机15、 GPS模块1201、导航模块1202、深度计模块1203采用5VDC供电方式，通过将电池模块8输出电压经过12V转5V模块703转换后进行供电，所述stm32从控板9通过单独的12V转3.3V模块702进行供电；所述电池监测模块实现对电池模块8的电压、电流、温湿度检测，在电池正常工作情况下将相应参数发送至 stm32从控板9，在过压、欠压、过流、温湿度值异常时向stm32从控板9发送报警信息。
前述自主水下机器人控制系统的声纳目标跟踪方法，其中步骤1的6)，腐蚀运算的实现方式表示如下：
S为结构元素，这里取半径为4个像素的圆作为结构元素，X为目标图像，Θ为腐蚀运算的表示符，其代表的是用结构元素S对目标图像X进行腐蚀操作； Sxy代表圆心坐标为(x,y)的结构元素S；E(X)为腐蚀运算后的目标图像，此式的含义是当结构元素S原点移动到点(x,y)处时，若S完全被X包含，则新的图像上该点为1，否则为0；腐蚀运算主要作用是消除目标图像的边界点；
膨胀运算的实现方式表示如下：
S为结构元素，这里取半径为4个像素的圆作为结构元素；X为目标图像，为膨胀运算的表示符，其代表的是用结构元素S对目标图像X进行膨胀操作； Sxy代表圆心坐标为(x,y)的结构元素S；D(X)为膨胀运算后的目标图像，此式的含义是当结构元素S原点移动到点(x,y)处时，若S与X有任何一点同时为1，则新的图像上该点为1，否则为0；若S与X完全没有相交，新的图像上点(x,y) 为0，膨胀运算主要作用是去除图像内部的细小空洞；
开运算的实现方式表示如下
S为结构元素，这里取半径为4个像素的圆作为结构元素；X为目标图像， O为开运算的表示符，其代表的是用结构元素S对目标图像X进行开操作；L(X) 为开运算后的目标图像，采用数学形态学开运算，通过先腐蚀运算后膨胀运算的过程达到去除声纳目标图像中的斑点噪声，平滑目标图像边缘，在不明显改变目标图像面积和特征的前提下获得较为平滑的目标图像。
与现有技术相比，本发明的有益效果是：
1.本发明的自主水下机器人控制系统具有模块化、易拆装的特点，具备遥控作业和自主作业两种工作模式，遥控作业模式用于对自主水下机器人的调试，自主作业模式实现自主水下机器人根据设定的航行计划自主进行水下作业。同时该自主水下机器人配备有独立的电源管理系统，能够同时实现对整个水下控制系统的供电和电池状态的监测。
2.本发明采用的基于RGB色彩空间的声纳目标检测算法可以直接从声纳伪彩色图像中提取出运动目标，比传统的目标检测中采用的先对原始图像进行一系列预处理再进行目标检测的方法更加方便快捷。同时颜色特征比灰度特征更易区别，本目标检测算法也克服了传统的背景差分法等目标检测方法在物体与背景的灰度值接近时造成分割出的目标区域存在大面积破裂或空洞的问题；本发明的基于粒子群优化的自适应粒子滤波算法将粒子群优化算法与粒子滤波算法相结合，跳过了传统粒子滤波重采样的过程，既保持了粒子的多样性，同时也大大减少了计算量。
附图说明
图1为本发明机器人控制系统结构框图；
图2为本发明机器人电源管理系统结构框图；
图3为本发明基于RGB色彩空间的声纳目标检测算法流程图；
图4为本发明基于分类重采样粒子滤波的声纳目标跟踪算法流程图。
具体实施方式
下面结合附图和具体实施例对本发明作进一步说明。
如图1所示，自主水下机器人控制系统，包括水面控制系统和水下控制系统，所述水面控制系统包括笔记本1、第一光电转换器2、光纤3、第一数传电台10。所述光电转换器2与笔记本1相连，所述光纤3与光电转换器2相连，数传电台 10与笔记本1相连。
图1中水下控制系统包括第二光电转换器4、云台摄像头5、PC104主控板6、电源管理系统7、电池模块8、stm32从控板9、第二数传电台11、传感器模块 12、水下灯13、水平舵机14、垂直舵机15、电机模块16、漏水检测模块17、下载口18、抛载模块19。所述光电转换器4和数传电台11与PC104主控板6 相连，接收水面控制系统控制指令并将水下控制系统采集到的数据传至水面控制系统。所述电池模块8通过电源管理系统7给整个水下控制系统供电。电机模块 16接收stm32从控板9发出的控制指令，驱动螺旋桨转动。水平舵机14接收stm32从控板9的信号，改变自主水下机器人的运行方向，垂直舵机15接收stm32从控板9的信号，控制自主水下机器人的上浮下潜。水下灯13接收stm32从控板 9的控制信号。电源管理系统7和漏水检测模块17检测电池状态信息和机器人内部环境信息发送给stm32从控板9。传感器模块12采集位置、姿态、深度、声纳数据传回PC104主控板6。PC104主控板6通过stm32从控板9采集水下控制系统各单元数据，并通过光电转换器4或数传电台11发至水面控制系统。
如图2所示，电源管理系统7包括DCDC转换模块和电池监测模块。所述电池模块8采用18650锂电池串并而成，输出电压为12VDC。所述数传电台11、PC104主控板6、电机模块16、水下灯13、云台摄像头5、声纳模块1204采用 12VDC供电方式，通过将电池模块8输出电压经过DCDC稳压模块701稳压后进行供电，所述抛载模块19、水平舵机14、垂直舵机15、GPS模块1201、导航模块1202、深度计模块1203采用5VDC供电方式，通过将电池模块8输出电压经过12V转5V模块703转换后进行供电，所述stm32从控板9通过单独的12V 转3.3V模块702进行供电。所述电池监测模块实现对电池模块8的电压、电流、温湿度检测，在电池正常工作情况下将相应参数发送至stm32从控板9，在过压、欠压、过流、温湿度值异常时向stm32从控板9发送报警信息。
如图4所示是基于分类重采样粒子滤波的声纳目标跟踪算法流程图，具体步骤如下：
步骤1：基于RGB色彩空间的声纳目标检测算法，本算法将视频图像处理方法引入到声纳伪彩色图像处理中，在取得良好声纳目标检测效果的同时减小了运算量。如图3所示，本算法通过以下几个步骤实现：
1)从声纳采集到的连续H帧图像序列f1、f2、...、fH中抽取L帧图像，记为I1、I2、...、IL。当目标运动速度大于0.1m/s时，L＝H，当目标运动速度小于0.1m/s时，采用间隔3帧抽取图像的方式对L进行取值。
2)采用k＝1、2、…、L来表示第k帧图像像素点 (i,j)处的RGB分量，i和j分别代表横坐标和纵坐标，然后对按照递增或递减的顺序进行排序，将排序结果记为
3)取三个数列的中间值，记做将这三个值作为点(i,j)处背景像素所对应的RGB 值。
4)由于在实际场景中背景总是在变换，故而需要对背景进行实时更新。由于水下机器人移动速度较慢，采集到的声纳图像背景变化较慢，这里引入背景更新因子来自适应更新背景：
式中，为当前帧图像在像素点(i,j)处的RGB分量， In(i,j)为当前帧图像在点(i,j)处的像素值，为更新前的背景帧图像在像素点(i,j)处的RGB分量，Cn-1(i,j)为更新前的背景帧图像在点(i,j)处的像素值，Cn(i,j)为更新后的背景帧图像在点(i,j)处的像素值，α为背景更新因子，取为0.5，Δ1为当前帧图像与更新前的背景帧图像在像素点(i,j) 处的RGB差值和，TH1为当前图像与背景图像的划分阀值，取为100。
5)对当前帧图像每个像素点的R,G,B三通道分别进行差分处理：
式中，为当前帧图像在像素点(i,j)处的RGB分量，为背景帧图像在像素点(i,j)处的RGB分量，为差值图像在像素点(i,j)处的RGB分量。
之后对差分后的3个分量进行如下处理，从而获得目标在(i,j)处的二值图像RIn(i,j)：
式中，RIn(i,j)为声纳目标在点(i,j)处的二值图像，TH2为目标检测阀值，取为125。
6)受图像中噪声影响，运动物体内部会出现空洞或是图像不连续的现象，采用数学形态学开运算来对差分后的二值图像进行处理，通过先腐蚀运算后膨胀运算的过程达到去除声纳目标图像中的斑点噪声，平滑目标图像边缘，在不明显改变目标图像面积和特征的前提下获得较为平滑的目标图像；腐蚀运算的实现方式表示如下：
S为结构元素，这里取半径为4个像素的圆作为结构元素。X为目标图像，Θ为腐蚀运算的表示符，其代表的是用结构元素S对目标图像X进行腐蚀操作； Sxy代表圆心坐标为(x,y)的结构元素S；E(X)为腐蚀运算后的目标图像。此式的含义是当结构元素S原点移动到点(x,y)处时，若S完全被X包含，则新的图像上该点为1，否则为0。腐蚀运算主要作用是消除目标图像的边界点。
膨胀运算的实现方式表示如下：
S为结构元素，这里取半径为4个像素的圆作为结构元素。X为目标图像，为膨胀运算的表示符，其代表的是用结构元素S对目标图像X进行膨胀操作； Sxy代表圆心坐标为(x,y)的结构元素S；D(X)为膨胀运算后的目标图像。此式的含义是当结构元素S原点移动到点(x,y)处时，若S与X有任何一点同时为1，则新的图像上该点为1，否则为0。若S与X完全没有相交，新的图像上点(x,y) 为0。膨胀运算主要作用是去除图像内部的细小空洞。
开运算的实现方式表示如下
S为结构元素，这里取半径为4个像素的圆作为结构元素。X为目标图像， O为开运算的表示符，其代表的是用结构元素S对目标图像X进行开操作。L(X) 为开运算后的目标图像。采用数学形态学开运算，通过先腐蚀运算后膨胀运算的过程达到去除声纳目标图像中的斑点噪声，平滑目标图像边缘，在不明显改变目标图像面积和特征的前提下获得较为平滑的目标图像。
步骤2：提取运动目标的质心点：
运动目标图像的质心可由下式求出：
式中，f(x,y)表示M×N的数字图像，xo，yo分别为目标质心点的横纵坐标。
步骤3：基于分类重采样的粒子滤波声纳目标跟踪
假设目标跟踪的数学模型为：
式中，xt表示t时刻的目标状态值，zt表示t时刻的目标观测值，f(.)表示系统状态转移函数，h(.)表示系统观测函数，wt-1表示跟踪过程中的系统噪声，vt表示跟踪过程中的观测噪声。
1)初始化粒子和权值：t＝0时，由先验概率p(x0)采样得到i＝1,2,…,N，每个粒子权值即以目标质心点(x0,y0)为中心初始化N个等权值的目标粒子。
2)系统状态转移：t＝t+1时刻，进入下一帧声纳图像，采样计算重要性权值
式中为重要性密度函数，为了便于实现，选取先验概率密度作为重要性密度函数，即则有重要性权值
3)根据将每个粒子权值进行归一化处理。
4)进行粒子滤波重采样：随着粒子滤波迭代次数的增加会导致粒子权重集中于少数粒子上。大量计算浪费在更新对整个系统帮助很小的小权值粒子上。此时需要进行粒子滤波重采样，复制权值较大的粒子，去除权值较小的粒子。定义有效采样尺度设定有效样本数作为重采样阀值，当Neff＜Nth时，采用分类重采样算法进行重采样，主要包括粒子的挑选和复制两个过程，具体步骤如下：
4.1)选取作为参考权值，将每个粒子权值与参考权值相比较，保留大于等于参考权值的粒子，去除小于参考权值的粒子。
4.2)统计出挑选出的粒子个数，记为m，并将这些粒子按照权值大小排序。求取这m个粒子的权值和及平均权值，分别记为sum、ave。
4.3)选取作为粒子复制的门限值，复制过程为从权值最大的粒子开始进行复制直到粒子总数达到N。当时，每个粒子的复制次数为小于等于h＝1,2,…,m的最大整数；当时，每个粒子的复制次数为大于等于h＝1,2,…,m的最小整数。对于复制之后粒子总数仍未达到N的特殊情况，按照粒子权值从大到小的顺序依次复制，直到粒子总数达到N。
4.4)将重采样之后所有粒子的权值设定为即令原有的带权样本采样为等权样本
5)输出目标的状态估计
6)再次进入t+1时刻，判断是否是声纳图像结束帧，如果是则跟踪结束，如果不是则继续下一次跟踪过程，转至步骤2。
除上述实施例外，本发明还可以有其他实施方式，凡采用等同替换或等效变换形成的技术方案，均落在本发明要求的保护范围内。

Claims (5)

1.一种自主水下机器人控制系统，其特征在于，包括水面控制系统和水下控制系统，所述水面控制系统包括笔记本、第一光电转换器、光纤、第一数传电台；所述光电转换器与笔记本相连，所述光纤与光电转换器相连，数传电台与笔记本相连；所述水下控制系统包括第二光电转换器、云台摄像头、PC104主控板、电源管理系统、电池模块、stm32从控板、第二数传电台、传感器模块、水下灯、水平舵机、垂直舵机、电机模块、漏水检测模块、下载口、抛载模块；所述第二光电转换器和第二数传电台与PC104主控板相连，接收水面控制系统控制指令并将水下控制系统采集到的数据传至水面控制系统；所述电池模块通过电源管理系统给整个水下控制系统供电；电机模块接收stm32从控板发出的控制指令，驱动螺旋桨转动；水平舵机接收stm32从控板的信号，改变自主水下机器人的运行方向，垂直舵机接收stm32从控板的信号，控制自主水下机器人的上浮下潜；水下灯接收stm32从控的控制信号；电源管理系统和漏水检测模块检测电池状态信息和机器人内部环境信息发送给stm32从控板；传感器模块采集位置、姿态、深度、声纳数据传回PC104主控板；PC104主控板通过stm32从控板采集水下控制系统各单元数据，并通过第二光电转换器或第二数传电台发至水面控制系统。
2.如权利要求1所述的自主水下机器人控制系统，其特征在于，所述传感器模块包括GPS模块、导航模块、深度计模块、声纳模块；所述GPS模块实时采集机器人的位置信息，包括经度、纬度和高度，所述导航模块采集机器人的姿态信息，其包括俯仰角、横滚角和航向角，所述深度计模块采集机器人深度值；所述声纳模块采集水下物体信息；所述GPS模块、导航模块、深度计模块、声纳模块连接至PC104主控板，将采集到的数据传至PC104主控板，经过PC104主控板处理之后发送至水面控制系统；采用光纤遥控作业模式时，所述数据经由光纤发送，采用自主作业模式时，所述数据经由数传电台发送。
3.如权利要求1所述的自主水下机器人控制系统，其特征在于，所述电源管理系统包括DCDC转换模块和电池监测模块；所述电池模块采用锂电池串并而成，输出电压为12VDC；所述数传电台、PC104主控板、电机模块、水下灯、云台摄像头、声纳模块采用12VDC供电方式，通过将电池模块输出电压经过DCDC稳压模块稳压后进行供电，所述抛载模块、水平舵机、垂直舵机、GPS模块、导航模块、深度计模块采用5VDC供电方式，通过将电池模块输出电压经过12V转5V模块转换后进行供电，所述stm32从控板通过单独的12V转3.3V模块进行供电；所述电池监测模块实现对电池模块的电压、电流、温湿度检测，在电池正常工作情况下将相应参数发送至stm32从控板，在过压、欠压、过流、温湿度值异常时向stm32从控板发送报警信息。
4.一种如权利要求1所述自主水下机器人控制系统的声纳目标跟踪方法，其特征在于，包括以下几个步骤：
步骤1：基于RGB色彩空间的声纳目标检测算法，通过以下几个步骤实现：
1)从声纳采集到的连续H帧图像序列f1、f2、...、fH中抽取L帧图像，记为I1、I2、...、IL；当目标运动速度大于0.1m/s时，L＝H，当目标运动速度小于0.1m/s时，采用间隔3帧抽取图像的方式对L进行取值；
2)采用k＝1、2、…、L，来表示第k帧图像像素点(i,j)处的RGB分量，i和j分别代表图像像素点横坐标和纵坐标，然后对按照递增或递减的顺序进行排序，将排序结果记为
3)取三个数列的中间值，记做将这三个值作为像素点(i,j)处背景像素所对应的RGB值；
4)由于在实际场景中背景总是在变换，故而需要对背景进行实时更新，由于水下机器人移动速度较慢，采集到的声纳图像背景变化较慢，引入背景更新因子来自适应更新背景：
<mrow> <mi>&amp;Delta;</mi> <mn>1</mn> <mo>=</mo> <mo>|</mo> <msubsup> <mi>I</mi> <mi>n</mi> <mi>R</mi> </msubsup> <mrow> <mo>(</mo> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>)</mo> </mrow> <mo>-</mo> <msubsup> <mi>C</mi> <mrow> <mi>n</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>R</mi> </msubsup> <mrow> <mo>(</mo> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>)</mo> </mrow> <mo>|</mo> <mo>+</mo> <mo>|</mo> <msubsup> <mi>I</mi> <mi>n</mi> <mi>G</mi> </msubsup> <mrow> <mo>(</mo> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>)</mo> </mrow> <mo>-</mo> <msubsup> <mi>C</mi> <mrow> <mi>n</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>G</mi> </msubsup> <mrow> <mo>(</mo> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>)</mo> </mrow> <mo>|</mo> <mo>+</mo> <mo>|</mo> <msubsup> <mi>I</mi> <mi>n</mi> <mi>B</mi> </msubsup> <mrow> <mo>(</mo> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>)</mo> </mrow> <mo>-</mo> <msubsup> <mi>C</mi> <mrow> <mi>n</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>B</mi> </msubsup> <mrow> <mo>(</mo> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>)</mo> </mrow> <mo>|</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>C</mi> <mi>n</mi> </msub> <mrow> <mo>(</mo> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>C</mi> <mrow> <mi>n</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mrow> <mo>(</mo> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> <mo>)</mo> </mrow> <mo>,</mo> <mi>&amp;Delta;</mi> <mn>1</mn> <mo>&amp;le;</mo> <mi>T</mi> <mi>H</mi> <mn>1</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>C</mi> <mi>n</mi> </msub> <mrow> <mo>(</mo> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> <mo>)</mo> </mrow> <mo>=</mo> <mrow> <mo>(</mo> <mrow> <mn>1</mn> <mo>-</mo> <mi>&amp;alpha;</mi> </mrow> <mo>)</mo> </mrow> <msub> <mi>C</mi> <mrow> <mi>n</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mrow> <mo>(</mo> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>&amp;alpha;I</mi> <mi>n</mi> </msub> <mrow> <mo>(</mo> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> <mo>)</mo> </mrow> <mo>,</mo> <mi>&amp;Delta;</mi> <mn>1</mn> <mo>&gt;</mo> <mi>T</mi> <mi>H</mi> <mn>1</mn> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>
式中，为当前帧图像在像素点(i,j)处的RGB分量，In(i,j)为当前帧图像在像素点(i,j)处的像素值，为更新前的背景帧图像在像素点(i,j)处的RGB分量，Cn-1(i,j)为更新前的背景帧图像在像素点(i,j)处的像素值，Cn(i,j)为更新后的背景帧图像在像素点(i,j)处的像素值，α为背景更新因子，取为0.5，△1为当前帧图像与更新前的背景帧图像在像素点(i,j)处的RGB差值和，TH1为当前图像与背景图像的划分阀值，取为100；
5)对当前帧图像每个像素点的R,G,B三通道分别进行差分处理：
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>D</mi> <mi>n</mi> <mi>R</mi> </msubsup> <mrow> <mo>(</mo> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> <mo>)</mo> </mrow> <mo>=</mo> <mo>|</mo> <msubsup> <mi>I</mi> <mi>n</mi> <mi>R</mi> </msubsup> <mrow> <mo>(</mo> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> <mo>)</mo> </mrow> <mo>-</mo> <msubsup> <mi>C</mi> <mi>n</mi> <mi>R</mi> </msubsup> <mrow> <mo>(</mo> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> <mo>)</mo> </mrow> <mo>|</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>D</mi> <mi>n</mi> <mi>G</mi> </msubsup> <mrow> <mo>(</mo> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> <mo>)</mo> </mrow> <mo>=</mo> <mo>|</mo> <msubsup> <mi>I</mi> <mi>n</mi> <mi>G</mi> </msubsup> <mrow> <mo>(</mo> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> <mo>)</mo> </mrow> <mo>-</mo> <msubsup> <mi>C</mi> <mi>n</mi> <mi>G</mi> </msubsup> <mrow> <mo>(</mo> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> <mo>)</mo> </mrow> <mo>|</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>D</mi> <mi>n</mi> <mi>B</mi> </msubsup> <mrow> <mo>(</mo> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> <mo>)</mo> </mrow> <mo>=</mo> <mo>|</mo> <msubsup> <mi>I</mi> <mi>n</mi> <mi>B</mi> </msubsup> <mrow> <mo>(</mo> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> <mo>)</mo> </mrow> <mo>-</mo> <msubsup> <mi>C</mi> <mi>n</mi> <mi>B</mi> </msubsup> <mrow> <mo>(</mo> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> <mo>)</mo> </mrow> <mo>|</mo> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow>
式中，为当前帧图像在像素点(i,j)处的RGB分量，为背景帧图像在像素点(i,j)处的RGB分量，为差值图像在像素点(i,j)处的RGB分量；
然后对差分后的3个分量进行如下处理，从而获得目标在像素点(i,j)处的二值图像RIn(i,j)：
<mrow> <mi>&amp;Delta;</mi> <mn>2</mn> <mo>=</mo> <mo>&amp;lsqb;</mo> <mn>1</mn> <mo>/</mo> <mn>3</mn> <msubsup> <mi>D</mi> <mi>n</mi> <mi>R</mi> </msubsup> <mrow> <mo>(</mo> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>)</mo> </mrow> <mo>+</mo> <mn>1</mn> <mo>/</mo> <mn>3</mn> <msubsup> <mi>D</mi> <mi>n</mi> <mi>G</mi> </msubsup> <mo>(</mo> <mrow> <mi>i</mi> <mo>,</mo> <mi>j</mi> </mrow> <mo>)</mo> <mo>+</mo> <mn>1</mn> <mo>/</mo> <mn>3</mn> <msubsup> <mi>D</mi> <mi>n</mi> <mi>B</mi> </msubsup> <mrow> <mo>(</mo> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>)</mo> </mrow> <mo>&amp;rsqb;</mo> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow>
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>RI</mi> <mi>n</mi> </msub> <mrow> <mo>(</mo> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>)</mo> </mrow> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mi>&amp;Delta;</mi> <mn>2</mn> <mo>&amp;GreaterEqual;</mo> <mi>T</mi> <mi>H</mi> <mn>2</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>RI</mi> <mi>n</mi> </msub> <mrow> <mo>(</mo> <mi>i</mi> <mo>,</mo> <mi>j</mi> <mo>)</mo> </mrow> <mo>=</mo> <mn>0</mn> <mo>,</mo> <mi>&amp;Delta;</mi> <mn>2</mn> <mo>&lt;</mo> <mi>T</mi> <mi>H</mi> <mn>2</mn> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow>
式中，RIn(i,j)为声纳目标在像素点(i,j)处的二值图像，TH2为目标检测阀值，取为125；
6)受图像中噪声影响，运动物体内部会出现空洞或是图像不连续的现象，采用数学形态学开运算来对差分后的二值图像进行处理，通过先腐蚀运算后膨胀运算的过程达到去除声纳目标图像中的斑点噪声，平滑目标图像边缘，在不明显改变目标图像面积和特征的前提下获得较为平滑的目标图像；
步骤2：提取运动目标的质心点：
运动目标图像的质心点可由下式求出：
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>x</mi> <mi>o</mi> </msub> <mo>=</mo> <mfrac> <mrow> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>x</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>M</mi> </munderover> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>y</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <mi>x</mi> <mi>f</mi> <mrow> <mo>(</mo> <mrow> <mi>x</mi> <mo>,</mo> <mi>y</mi> </mrow> <mo>)</mo> </mrow> </mrow> <mrow> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>x</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>M</mi> </munderover> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>y</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <mi>f</mi> <mrow> <mo>(</mo> <mrow> <mi>x</mi> <mo>,</mo> <mi>y</mi> </mrow> <mo>)</mo> </mrow> </mrow> </mfrac> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>y</mi> <mi>o</mi> </msub> <mo>=</mo> <mfrac> <mrow> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>x</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>M</mi> </munderover> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>y</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <mi>y</mi> <mi>f</mi> <mrow> <mo>(</mo> <mrow> <mi>x</mi> <mo>,</mo> <mi>y</mi> </mrow> <mo>)</mo> </mrow> </mrow> <mrow> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>x</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>M</mi> </munderover> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>y</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>N</mi> </munderover> <mi>f</mi> <mrow> <mo>(</mo> <mrow> <mi>x</mi> <mo>,</mo> <mi>y</mi> </mrow> <mo>)</mo> </mrow> </mrow> </mfrac> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> </mrow>
式中，f(x,y)表示M×N的数字图像，M和N分别代表X方向和Y方向的像素点个数，xo，yo分别为目标质心点的横坐标、纵坐标；
步骤3：进行基于分类重采样的粒子滤波声纳目标跟踪，
假设目标跟踪的数学模型为：
<mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msub> <mi>x</mi> <mi>t</mi> </msub> <mo>=</mo> <mi>f</mi> <mrow> <mo>(</mo> <msub> <mi>x</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <msub> <mi>w</mi> <mrow> <mi>t</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>z</mi> <mi>t</mi> </msub> <mo>=</mo> <mi>h</mi> <mrow> <mo>(</mo> <mrow> <msub> <mi>x</mi> <mi>t</mi> </msub> <mo>,</mo> <msub> <mi>v</mi> <mi>t</mi> </msub> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>7</mn> <mo>)</mo> </mrow> </mrow>
式中，xt表示t时刻的目标状态值，zt表示t时刻的目标观测值，f(xt-1,wt-1)表示系统状态转移函数，h(xt,vt)表示系统观测函数，wt-1表示跟踪过程中的系统噪声，vt表示跟踪过程中的观测噪声；
1)初始化粒子和权值：t＝0时，由先验概率p(x0)采样得到i＝1,2,…,N，每个粒子权值即以目标质心点(x0,y0)为中心初始化N个等权值的目标粒子；
2)系统状态转移：t＝t+1时刻，进入下一帧声纳图像，采样计算重要性权值
式中为重要性密度函数，为了便于实现，选取先验概率密度作为重要性密度函数，即则有重要性权值
3)根据将每个粒子权值进行归一化处理；
4)进行粒子滤波重采样：随着粒子滤波迭代次数的增加会导致粒子权重集中于少数粒子上，大量计算浪费在更新对整个系统帮助很小的小权值粒子上；此时需要进行粒子滤波重采样，复制权值较大的粒子，去除权值较小的粒子；定义有效采样尺度设定有效样本数作为重采样阀值，当Nef<Nth时，采用分类重采样算法进行重采样，主要包括粒子的挑选和复制两个过程，具体步骤如下：
(1)选取作为参考权值，将每个粒子权值与参考权值相比较，保留大于等于参考权值的粒子，去除小于参考权值的粒子；
(2)统计出挑选出的粒子个数，记为m，并将这些粒子按照权值大小排序，求取这m个粒子的权值和及平均权值，分别记为sum、ave；
(3)选取作为粒子复制的门限值，复制过程为从权值最大的粒子开始进行复制直到粒子总数达到N，当时，每个粒子的复制次数为小于等于h＝1,2,…,m的最大整数；当时，每个粒子的复制次数为大于等于h＝1,2,…,m的最小整数；对于复制之后粒子总数仍未达到N的特殊情况，按照粒子权值从大到小的顺序依次复制，直到粒子总数达到N；
(4)将重采样之后所有粒子的权值设定为即令原有的带权样本采样为等权样本
5)输出目标的状态估计
6)再次进入t+1时刻，判断是否是声纳图像结束帧，如果是则跟踪结束，如果不是则继续下一次跟踪过程，转至步骤2。
5.如权利要求4所述自主水下机器人控制系统的声纳目标跟踪方法，其特征在于，所述步骤1的6)，腐蚀运算的实现方式表示如下：
<mrow> <mi>E</mi> <mrow> <mo>(</mo> <mi>X</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>X</mi> <mi>&amp;Theta;</mi> <mi>S</mi> <mo>=</mo> <mo>{</mo> <mi>x</mi> <mo>,</mo> <mi>y</mi> <mo>|</mo> <msub> <mi>S</mi> <mrow> <mi>x</mi> <mi>y</mi> </mrow> </msub> <mo>&amp;SubsetEqual;</mo> <mi>X</mi> <mo>}</mo> </mrow>
S为结构元素，这里取半径为4个像素的圆作为结构元素，X为目标图像，Θ为腐蚀运算的表示符，其代表的是用结构元素S对目标图像X进行腐蚀操作；Sxy代表圆心坐标为(x,y)的结构元素S；E(X)为腐蚀运算后的目标图像，此式的含义是当结构元素S原点移动到点(x,y)处时，若S完全被X包含，则新的图像上该点为1，否则为0；腐蚀运算主要作用是消除目标图像的边界点；
膨胀运算的实现方式表示如下：
D(X)＝X⊕S＝{x,y|Sxy∩X≠0}
S为结构元素，这里取半径为4个像素的圆作为结构元素；X为目标图像，⊕为膨胀运算的表示符，其代表的是用结构元素S对目标图像X进行膨胀操作；Sxy代表圆心坐标为(x,y)的结构元素S；D(X)为膨胀运算后的目标图像，此式的含义是当结构元素S原点移动到点(x,y)处时，若S与X有任何一点同时为1，则新的图像上该点为1，否则为0；若S与X完全没有相交，新的图像上点(x,y)为0，膨胀运算主要作用是去除图像内部的细小空洞；
开运算的实现方式表示如下
L(X)＝XOS＝(XΘS)⊕S
S为结构元素，这里取半径为4个像素的圆作为结构元素；X为目标图像，O为开运算的表示符，其代表的是用结构元素S对目标图像X进行开操作；L(X)为开运算后的目标图像，采用数学形态学开运算，通过先腐蚀运算后膨胀运算的过程达到去除声纳目标图像中的斑点噪声，平滑目标图像边缘，在不明显改变目标图像面积和特征的前提下获得较为平滑的目标图像。
CN201711096917.6A 2017-11-09 2017-11-09 自主水下机器人控制系统及声纳目标跟踪方法 Expired - Fee Related CN107730539B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711096917.6A CN107730539B (zh) 2017-11-09 2017-11-09 自主水下机器人控制系统及声纳目标跟踪方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711096917.6A CN107730539B (zh) 2017-11-09 2017-11-09 自主水下机器人控制系统及声纳目标跟踪方法

Publications (2)

Publication Number Publication Date
CN107730539A true CN107730539A (zh) 2018-02-23
CN107730539B CN107730539B (zh) 2020-07-14

Family

ID=61214639

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711096917.6A Expired - Fee Related CN107730539B (zh) 2017-11-09 2017-11-09 自主水下机器人控制系统及声纳目标跟踪方法

Country Status (1)

Country Link
CN (1) CN107730539B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109032178A (zh) * 2018-08-06 2018-12-18 江苏科技大学 全驱动auv回收控制系统及自主回收方法
CN109460061A (zh) * 2018-12-12 2019-03-12 国家海洋局第二海洋研究所 一种自主水下机器人和地质取样设备的并行作业方法
CN110209208A (zh) * 2019-05-08 2019-09-06 山东航天电子技术研究所 一种基于粒子群优化的光电设备跟踪控制方法
CN111754455A (zh) * 2020-05-15 2020-10-09 华能国际电力股份有限公司海门电厂 一种火电厂漏水检测方法和系统

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20060286931A1 (en) * 2005-06-15 2006-12-21 Mark Rhodes Underwater communications system and method
CN102052924A (zh) * 2010-11-25 2011-05-11 哈尔滨工程大学 一种小型水下机器人组合导航定位方法
CN104199447A (zh) * 2014-08-18 2014-12-10 江苏科技大学 水下结构物检测机器人控制系统及运动控制方法
CN104820993A (zh) * 2015-03-27 2015-08-05 浙江大学 一种联合粒子滤波和跟踪置前检测的水下弱目标跟踪方法
CN105046258A (zh) * 2015-09-08 2015-11-11 中国电子科技集团公司第三研究所 一种小目标探测声呐图像的目标检测方法及装置
CN205438623U (zh) * 2016-03-07 2016-08-10 江苏帝一集团有限公司 一种水下检测和作业机器人的水面监控系统
CN106094819A (zh) * 2016-06-17 2016-11-09 江苏科技大学 水下机器人控制系统及基于声纳图像目标识别的航向控制方法
CN106296725A (zh) * 2015-06-12 2017-01-04 富泰华工业（深圳）有限公司 运动目标实时检测与跟踪方法及目标检测装置

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20060286931A1 (en) * 2005-06-15 2006-12-21 Mark Rhodes Underwater communications system and method
CN102052924A (zh) * 2010-11-25 2011-05-11 哈尔滨工程大学 一种小型水下机器人组合导航定位方法
CN104199447A (zh) * 2014-08-18 2014-12-10 江苏科技大学 水下结构物检测机器人控制系统及运动控制方法
CN104820993A (zh) * 2015-03-27 2015-08-05 浙江大学 一种联合粒子滤波和跟踪置前检测的水下弱目标跟踪方法
CN106296725A (zh) * 2015-06-12 2017-01-04 富泰华工业（深圳）有限公司 运动目标实时检测与跟踪方法及目标检测装置
CN105046258A (zh) * 2015-09-08 2015-11-11 中国电子科技集团公司第三研究所 一种小目标探测声呐图像的目标检测方法及装置
CN205438623U (zh) * 2016-03-07 2016-08-10 江苏帝一集团有限公司 一种水下检测和作业机器人的水面监控系统
CN106094819A (zh) * 2016-06-17 2016-11-09 江苏科技大学 水下机器人控制系统及基于声纳图像目标识别的航向控制方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
J. KIM, S.C. YU: "Convolutional Neural Network-based Real-time ROV Detection Using Forward-looking Sonar Image", 《2016 IEEE/OES AUTONOMOUS UNDERWATER VEHICLES (AUV)》 *
刘加: "基于背景差分和粒子滤波的运动目标检测与跟踪研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *
宋振文: "水下结构检测与作业机器人ROV研制及声纳图像识别研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *
胡旭: "彩色空间下运动目标检测与跟踪问题研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *
马珊 等: "前视声呐多特征自适应融合跟踪方法", 《哈尔滨工程大学学报》 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109032178A (zh) * 2018-08-06 2018-12-18 江苏科技大学 全驱动auv回收控制系统及自主回收方法
CN109032178B (zh) * 2018-08-06 2021-08-24 江苏科技大学 全驱动auv回收控制系统及自主回收方法
CN109460061A (zh) * 2018-12-12 2019-03-12 国家海洋局第二海洋研究所 一种自主水下机器人和地质取样设备的并行作业方法
CN109460061B (zh) * 2018-12-12 2020-08-25 自然资源部第二海洋研究所 一种自主水下机器人和地质取样设备的并行作业方法
CN110209208A (zh) * 2019-05-08 2019-09-06 山东航天电子技术研究所 一种基于粒子群优化的光电设备跟踪控制方法
CN111754455A (zh) * 2020-05-15 2020-10-09 华能国际电力股份有限公司海门电厂 一种火电厂漏水检测方法和系统
CN111754455B (zh) * 2020-05-15 2024-03-19 华能国际电力股份有限公司海门电厂 一种火电厂漏水检测方法和系统

Also Published As

Publication number Publication date
CN107730539B (zh) 2020-07-14

Similar Documents

Publication Publication Date Title
CN107730539B (zh) 自主水下机器人控制系统及声纳目标跟踪方法
Han et al. Marine organism detection and classification from underwater vision based on the deep CNN method
Wang et al. Real-time underwater onboard vision sensing system for robotic gripping
CN109859202B (zh) 一种基于usv水面光学目标跟踪的深度学习检测方法
CN112146654B (zh) 基于关键约束帧的前视成像声呐水下定位与导航方法
Bovcon et al. A water-obstacle separation and refinement network for unmanned surface vehicles
Ji-Yong et al. Design and vision based autonomous capture of sea organism with absorptive type remotely operated vehicle
CN104766312A (zh) 一种基于双目光视觉引导的智能水下机器人自主对接方法
Mu et al. Path following for podded propulsion unmanned surface vehicle: Theory, simulation and experiment
Wu et al. Intelligent control method of underwater inspection robot in netcage
Aqthobilrobbany et al. HSV Based Robot Boat Navigation System
Mishra et al. Autonomous vision-guided object collection from water surfaces with a customized multirotor
CN116246139A (zh) 一种无人艇航行环境基于多传感器融合的目标识别方法
Alla et al. Vision-based Deep Learning algorithm for Underwater Object Detection and Tracking
Yao et al. LiDAR based navigable region detection for unmanned surface vehicles
CN113822297B (zh) 一种海上舰艇目标识别装置及方法
Tarekegn et al. Underwater Object Detection using Image Enhancement and Deep Learning Models
Wu et al. An overview of developments and challenges for unmanned surface vehicle autonomous berthing
CN117454680A (zh) 一种海洋搜索集群设计系统及方法
Lan et al. Based on Deep Reinforcement Learning to path planning in uncertain ocean currents for Underwater Gliders
Feng et al. Real-time riverbank line detection for USV system
Ercan et al. Underwater target detection using deep learning
CN115951693B (zh) 一种欠驱动水下机器人鲁棒轨迹跟踪控制方法
Vidyarthi et al. Fish-Like Robots And Applications Of Sensor-A Review
Hu et al. Underwater target following with a vision-based autonomous robotic fish

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
EE01 Entry into force of recordation of patent licensing contract
EE01 Entry into force of recordation of patent licensing contract

Application publication date: 20180223

Assignee: Center for technology transfer Jiangsu University of Science and Technology

Assignor: JIANGSU University OF SCIENCE AND TECHNOLOGY

Contract record no.: X2021980006173

Denomination of invention: Autonomous underwater vehicle control system and sonar target tracking method

Granted publication date: 20200714

License type: Common License

Record date: 20210714

EC01 Cancellation of recordation of patent licensing contract
EC01 Cancellation of recordation of patent licensing contract

Assignee: Center for technology transfer Jiangsu University of Science and Technology

Assignor: JIANGSU University OF SCIENCE AND TECHNOLOGY

Contract record no.: X2021980006173

Date of cancellation: 20210826

CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20200714