自主水下机器人控制系统及声纳目标跟踪方法
技术领域
本发明涉及一种自主水下机器人,尤其涉及一种自主水下机器人的控制系统及声纳目标跟踪方法,属于机器人技术领域。
背景技术
地球上海洋空间非常广阔,蕴藏着丰富的资源。随着陆地资源的不断减少,人类逐渐向海洋资源开发进军。合理开发利用海洋资源,保护海洋生物与环境对人类的发展与稳定具有重大意义。由于海洋环境具有复杂性与不确定性,通过载人探测进行海洋开发的方式极其危险,水下机器人在这样的背景下便应用而生。无人自主水下机器人作为一种无缆的自主水下机器人,与装载母船无连接,通过本身携带的高能量电池等能源来实现自主航行,可以实时将水下信息上传给母船。无人自主水下机器人具有较强的环境适应性,较大的活动范围和灵活性,在海洋资源勘探、海底工程作业、科研考察等诸多方面都有着广泛的应用,是人类最优秀的水下作业工具之一。
由于水对光电等信号的吸收比率比在空气中要高得多,而对声波信号的吸收比率较低且声波信号在水中的传输速度比在空气中快得多,这就导致基于声纳的探测手段是水下环境特别是深海环境中进行探测的主要手段。声纳系统作为无人自主水下机器人的眼睛,对于其实现避障,导航,水下信息探测等功能都有着不可取代的作用。除此之外,由于水下运动物体有可能是我们感兴趣的目标或者成为水下机器人的潜在威胁,因而对运动物体进行准确有效的跟踪对水下机器人执行对应的探测任务和确保水下机器人的安全航行都有着非常重要的意义。
本发明结合研制的自主水下机器人,首先引入一种基于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分量, I
n(i,j)为当前帧图像在像素点(i,j)处的像素值,
为更新前的背景帧图像在像素点(i,j)处的RGB分量,C
n-1(i,j)为更新前的背景帧图像在像素点(i,j)处的像素值,C
n(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)处的二值图像RI
n(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(x
0)采样得到
i=1,2,…,N,每个粒子权值
即以目标质心点(x
0,y
0)为中心初始化N个等权值的目标粒子;
2)系统状态转移:t=t+1时刻,进入下一帧声纳图像,采样
计算重要性权值
式中
为重要性密度函数,为了便于实现,选取先验概率密度
作为重要性密度函数,即
则有重要性权值
4)进行粒子滤波重采样:随着粒子滤波迭代次数的增加会导致粒子权重集中于少数粒子上,大量计算浪费在更新对整个系统帮助很小的小权值粒子上;此时需要进行粒子滤波重采样,复制权值较大的粒子,去除权值较小的粒子;定义有效采样尺度
设定有效样本数
作为重采样阀值,当N
eff<N
th时,采用分类重采样算法进行重采样,主要包括粒子的挑选和复制两个过程,具体步骤如下:
(1)选取
作为参考权值,将每个粒子权值
与参考权值
相比较,保留大于等于参考权值的粒子,去除小于参考权值的粒子;
(2)统计出挑选出的粒子个数,记为m,并将这些粒子按照权值大小排序,求取这m个粒子的权值和及平均权值,分别记为sum、ave;
(3)选取
作为粒子复制的门限值,复制过程为从权值最大的粒子开始进行复制直到粒子总数达到N,当
时,每个粒子的复制次数为小于等于
h=1,2,…,m的最大整数;当
时,每个粒子的复制次数为大于等于
h=1,2,…,m的最小整数;对于复制之后粒子总数仍未达到N的特殊情况,按照粒子权值从大到小的顺序依次复制,直到粒子总数达到N;
(4)将重采样之后所有粒子的权值设定为
即令原有的带权样本
采样为等权样本
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进行膨胀操作; S
xy代表圆心坐标为(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分量, I
n(i,j)为当前帧图像在点(i,j)处的像素值,
为更新前的背景帧图像在像素点(i,j)处的RGB分量,C
n-1(i,j)为更新前的背景帧图像在点(i,j)处的像素值,C
n(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)处的二值图像RI
n(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进行膨胀操作; S
xy代表圆心坐标为(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(x
0)采样得到
i=1,2,…,N,每个粒子权值
即以目标质心点(x
0,y
0)为中心初始化N个等权值的目标粒子。
2)系统状态转移:t=t+1时刻,进入下一帧声纳图像,采样
计算重要性权值
式中
为重要性密度函数,为了便于实现,选取先验概率密度
作为重要性密度函数,即
则有重要性权值
4)进行粒子滤波重采样:随着粒子滤波迭代次数的增加会导致粒子权重集中于少数粒子上。大量计算浪费在更新对整个系统帮助很小的小权值粒子上。此时需要进行粒子滤波重采样,复制权值较大的粒子,去除权值较小的粒子。定义有效采样尺度
设定有效样本数
作为重采样阀值,当N
eff<N
th时,采用分类重采样算法进行重采样,主要包括粒子的挑选和复制两个过程,具体步骤如下:
4.1)选取
作为参考权值,将每个粒子权值
与参考权值
相比较,保留大于等于参考权值的粒子,去除小于参考权值的粒子。
4.2)统计出挑选出的粒子个数,记为m,并将这些粒子按照权值大小排序。求取这m个粒子的权值和及平均权值,分别记为sum、ave。
4.3)选取
作为粒子复制的门限值,复制过程为从权值最大的粒子开始进行复制直到粒子总数达到N。当
时,每个粒子的复制次数为小于等于
h=1,2,…,m的最大整数;当
时,每个粒子的复制次数为大于等于
h=1,2,…,m的最小整数。对于复制之后粒子总数仍未达到N的特殊情况,按照粒子权值从大到小的顺序依次复制,直到粒子总数达到N。
4.4)将重采样之后所有粒子的权值设定为
即令原有的带权样本
采样为等权样本
6)再次进入t+1时刻,判断是否是声纳图像结束帧,如果是则跟踪结束,如果不是则继续下一次跟踪过程,转至步骤2。
除上述实施例外,本发明还可以有其他实施方式,凡采用等同替换或等效变换形成的技术方案,均落在本发明要求的保护范围内。