[发明专利]自主水下机器人控制系统及声纳目标跟踪方法

权利要求书

1.一种自主水下机器人控制系统的声纳目标跟踪方法，自主水下机器人控制系统包括水面控制系统和水下控制系统，所述水面控制系统包括笔记本、第一光电转换器、光纤、第一数传电台；所述第一光电转换器与笔记本相连，所述光纤与第一光电转换器相连，第一数传电台与笔记本相连；所述水下控制系统包括第二光电转换器、云台摄像头、PC104主控板、电源管理系统、电池模块、stm32从控板、第二数传电台、传感器模块、水下灯、水平舵机、垂直舵机、电机模块、漏水检测模块、下载口、抛载模块；所述第二光电转换器和第二数传电台与PC104主控板相连，接收水面控制系统控制指令并将水下控制系统采集到的数据传至水面控制系统；所述电池模块通过电源管理系统给整个水下控制系统供电；电机模块接收stm32从控板发出的控制指令，驱动螺旋桨转动；水平舵机接收stm32从控板的信号，改变自主水下机器人的运行方向，垂直舵机接收stm32从控板的信号，控制自主水下机器人的上浮下潜；水下灯接收stm32从控的控制信号；电源管理系统和漏水检测模块检测电池状态信息和机器人内部环境信息发送给stm32从控板；传感器模块采集位置、姿态、深度、声纳数据传回PC104主控板；PC104主控板通过stm32从控板采集水下控制系统各单元数据，并通过第二光电转换器或第二数传电台发至水面控制系统；自主水下机器人控制系统的声纳目标跟踪方法，其特征在于，包括以下步骤：

步骤1：基于RGB色彩空间的声纳目标检测算法，通过以下几个步骤实现：

1)从声纳采集到的连续H帧图像序列f₁、f₂、...、f_H中抽取L帧图像，记为I₁、I₂、...、I_L；当目标运动速度大于0.1m/s时，L＝H，当目标运动速度小于0.1m/s时，采用间隔3帧抽取图像的方式对L进行取值；

2)采用来表示第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)：

式中，RI_n(i,j)为声纳目标在像素点(i,j)处的二值图像，TH2为目标检测阀值，取为125；

6)受图像中噪声影响，运动物体内部会出现空洞或是图像不连续的现象，采用数学形态学开运算来对差分后的二值图像进行处理，通过先腐蚀运算后膨胀运算的过程达到去除声纳目标图像中的斑点噪声，平滑目标图像边缘，在不明显改变目标图像面积和特征的前提下获得较为平滑的目标图像；

步骤2：提取运动目标的质心点：

运动目标图像的质心点可由下式求出：

式中，f(x,y)表示M×N的数字图像，M和N分别代表X方向和Y方向的像素点个数，x_o，y_o分别为目标质心点的横坐标、纵坐标；

步骤3：进行基于分类重采样的粒子滤波声纳目标跟踪，

假设目标跟踪的数学模型为：

式中，x_t表示t时刻的目标状态值，z_t表示t时刻的目标观测值，f(x_t-1,w_t-1)表示系统状态转移函数，h(x_t,v_t)表示系统观测函数，w_t-1表示跟踪过程中的系统噪声，v_t表示跟踪过程中的观测噪声；

1)初始化粒子和权值：t＝0时，由先验概率p(x₀)采样得到每个粒子权值即以目标质心点(x₀,y₀)为中心初始化N个等权值的目标粒子；

2)系统状态转移：t＝t+1时刻，进入下一帧声纳图像，采样计算重要性权值

式中为重要性密度函数，为了便于实现，选取先验概率密度作为重要性密度函数，即则有重要性权值

3)根据将每个粒子权值进行归一化处理；

4)进行粒子滤波重采样：随着粒子滤波迭代次数的增加会导致粒子权重集中于少数粒子上，大量计算浪费在更新对整个系统帮助很小的小权值粒子上；此时需要进行粒子滤波重采样，复制权值较大的粒子，去除权值较小的粒子；定义有效采样尺度设定有效样本数作为重采样阀值，当N_eff＜N_th时，采用分类重采样算法进行重采样，主要包括粒子的挑选和复制两个过程，具体步骤如下：

(1)选取作为参考权值，将每个粒子权值与参考权值相比较，保留大于等于参考权值的粒子，去除小于参考权值的粒子；

(2)统计出挑选出的粒子个数，记为m，并将这些粒子按照权值大小排序，求取这m个粒子的权值和及平均权值，分别记为sum、ave；

(3)选取作为粒子复制的门限值，复制过程为从权值最大的粒子开始进行复制直到粒子总数达到N，当时，每个粒子的复制次数为小于等于的最大整数；当时，每个粒子的复制次数为大于等于的最小整数；对于复制之后粒子总数仍未达到N的特殊情况，按照粒子权值从大到小的顺序依次复制，直到粒子总数达到N；

(4)将重采样之后所有粒子的权值设定为即令原有的带权样本采样为等权样本

5)输出目标的状态估计

6)再次进入t+1时刻，判断是否是声纳图像结束帧，如果是则跟踪结束，如果不是则继续下一次跟踪过程，转至步骤2。