本发明的目的在于提供用于控制水下数据采集、深海作业和船体检测等领域机器人的
一种遥控水下机器人水面控制方法。
本发明的目的是这样实现的:
本发明通过水下机器人装有的CCD和前视声纳获取水下被跟踪物体、场景图像和水下障碍物的信息,根据获取的信息采用改进的Sage-Husa自适应卡尔曼滤波
算法获得水下机器人实际的艏向和深度,根据获得的水下机器人实际的艏向和深度、通过递归的以太神经网络DPRFNN算法自动对水下机器人发送运动控制指令。
本发明的一种遥控水下机器人水面控制方法还包括:
1、所述的改进的Sage-Husa自适应卡尔曼滤波算法是在基本的KALMAN滤波
基础上增加系统噪声统计的估计值和量测噪声统计的估计值的调整:
系统干扰均值
系统干扰方差矩阵
量测噪声均值
量测噪声方差矩阵
其中是状态X(k)的估计,Φ(k,k-1)为t(k-1)时刻至t(k)时刻的一步转移阵,H(k)为量测阵,为系统噪声序列的方差阵Q(k)的估计,为量测噪声序列方差阵R(k)的估计,为新息矩阵,新息包含有一步预测的误差,对其作适当加权处理将分离出来修正b是遗忘因子它对滤波的发散和
精度起着至关重要的作用,并通过调整Pk+1|k控制滤波增益阵Kk+1来防止
滤波器的发散,当不成立,则按修正Pk+1|k,其中γ≥1为事先确定可调系数,Sk+1是自适应系数,将系统干扰均值、系统干扰方差矩阵、量测噪声均值、量测噪声方差矩阵和卡尔曼滤波结合就构成了改进的Sage-Husa自适应卡尔曼滤波算法。
2、所述的以太神经网络DPRFNN算法包括
输入层i、隶属度层j、petri以太层、规则层k层和
输出层o层五层结构,递归反馈通过在隶属度层嵌入反馈连接来实现,
信号的传播和每一层的基本函数表示如下:
(1)第一层是输入层,输入层的每一个
节点直接把输入变量xi(i=1,2,3,4)直接传递到下一层,输入节点为深度和
角度的和下潜速度与
角速度误差,neti1是第一层输出:
(2)第二层是隶属度层,层内的每一个节点都经过隶属度函数,隶属度输入为
n是训练次数,αij是代表自反馈循环的权值,μij(n-1)是上一次训练第二层的
输出信号,它通过高斯型隶属函数来定义:
mij和σij分别是第i个输入变量的第j个模糊集合的高斯型隶属函数的均值和标准差,它们都是可调参数,j=1,2,…,nj,nj是每个输入的语义变量的数量,μij[netj(rij)]是第二层输出;
(3)第三层是Petri层,它提供令牌,使用如下的竞争规则
其中tij是转换值,dth是动态变化的
阈值、随着系统响应误差的而变化;
(4)第四层是规则层,每个节点k由∏表示,即连乘输入输入并输出结果:
式中ωjik是Petri网和规则层的权值、为恒定的值,φk(k=1,2,...ny)第k层输出,ny是规则总数,φk是第四层输出;
(5)第五层是输出层:
其中,连接权值ωko是和第k条规则相关的第0个输出的输出强度,yo是第五层输出,输出值即为推力器的控制
电压值;
DPRFNN使用的在线学习算法是监督-斜坡算法,该算法为首先其
能量函数E定义为:
式中h和θ是控制中遥控水下机器人的实时深度值和艏向值,hd和θd分别是h和θ的期望值,eh=hd-h、eθ=θd-θ分别是深度和艏向的误差,动态变化阈值dth由下式来调整:
式中α和β是正常数;
在输出层,误差反向传播值为:
连接权值ωko按照下式来更新:
ηω连接权值学习率,下一时刻ωko为:
规则层的权值是恒定的,则该层的误差为:
Petri层中误差如下计算
模糊化层各参数的更新规则为:
下一时刻模糊化层各参数为:
式中ηm,ηw,ησ,ηα是高斯函数的学习率参数,ε是正常数。
本发明的优势在于:简单、灵活,功能强大、适应性强等。
附图说明
图1是本发明的控制方法总图;
图2是本发明的无线操纵杆手操控制方法体系
框图;
图3是本发明的改进的Sage-Husa自适应KALMAN滤波
流程图;
图4是本发明的遥控水下机器人FNN深度和艏向控制体系框图;
图5是本发明的自动控制遥控水下机器人的五层DPRFNN结构图;
图6是本发明的遥控水下机器人跟踪运动控制流程图;
图7是本发明的遥控水下机器人避碰运动控制流程图;
图8是PC/104计算机与水面主控计算机的联系SOCKET通讯过程图。
下面结合附图举例对本发明做更详细地描述:
结合图1~8,本实施方式的主体是水面遥控制方法。其中水面遥控制方法
硬件通过光纤和ROV相连,实现水下机器人的控制。水下机器人自动控制方法水面控制机采用WindowsXP
操作系统,利用VisualC++图形进行可视编程,建立主控程序。
结合图1遥控水下机器人的水面控制方法由水面遥控平台和水下
机器人本体两部分来实现,水面遥控平台的硬件设备由一台水面主控计算机,光端机,
液晶显示器和一个机器人无线控制操纵
手柄组成。水面遥控平台的
软件由水下机器人的操纵杆手操控制,声视觉识别和避障算法,视觉规划跟踪算法,传感器滤波算法与水下PC/104的SOCKET通讯模块组成。水面遥控平台通过水下机器人装有的环境感知设备和运动感知设备获取作业(包括视频、声图像)、
姿态和深度信息(经过滤波处理),控制者可以通过操纵杆手操,FNN定向和定深控制,跟踪和避碰控制,发送机器人的运动控制命令,该命令通过SOCKET通讯模块和光端机传递给水下PC/104,从而控制机器人的
导管螺旋桨完成动作。
结合图2无线操纵杆手操控制方法为操纵者实时提供水下机器人的当前深度;艏向角,各推力器电压信息;运动轨迹和当前
位置,所采集到的视频和前视声纳(SONAR)信号等信息,操纵者根据这些信息,给水下机器人的六个导管螺旋桨
推进器(包括两个主推,两个侧推,两个垂推)发送控制电压,控制机器人沿目标
位姿和方向前进。其中,艏向角和深度由罗经和深度计经过野值剔除和改进的Sage-Husa自适应KALMAN滤波得到;手操同时对设备进行故障检测和诊断,以保证设备正常运行。
结合图3采用改进的Sage-Husa自适应卡尔曼滤波获得水下机器人实际的艏向和深度。
其实现方案就是在基本的KALMAN滤波基础上增加了系统噪声统计q(k)、Q(k)和量测噪声统计r(k)、R(k)的调整:
系统干扰均值
系统干扰方差矩阵
量测噪声均值
量测噪声方差矩阵
其中是状态X(k)的估计,Φ(k,k-1)为t(k-1)时刻至t(k)时刻的一步转移阵,H(k)为量测阵,为系统噪声序列的方差阵Q(k)的估计,为量测噪声序列方差阵R(k)的估计,为新息矩阵,新息包含有一步预测的误差,对其作适当加权处理就能将分离出来修正b是遗忘因子它对滤波的发散和精度起着至关重要的作用。
并通过调整Pk+1|k控制滤波增益阵Kk+1来防止滤波器的发散,当不成立,则按修正Pk+1|k,其中γ≥1为事先确定可调系数,Sk+1是自适应系数。
将上面的公式和卡尔曼滤波结合交替计算就构成了改进的Sage-Husa自适应卡尔曼滤波算法。
结合图4、5水下机器人自动控制方法:图4中采用改进的Sage-Husa自适应KALMAN滤波的方法对数据进行最优估计。机器人
控制器采用模糊神经网络控制器,控制器根据收到的姿态信息进行对机器人本体在自动控制时进行深度或目标位姿的控制。
结合水动力参数,对机器人本体和仿真同时进行,以保证控制的实时和准确性。同时对设备进行故障检测和诊断,以保证设备正常运行。
图5中描述了自动控制遥控水下机器人使用的一个五层DPRFNN结构。包含输入层i,隶属度层j,petri以太层,规则层k层,和输出层o层,如图所示。递归反馈通过在隶属度层嵌入反馈连接来实现。信号的传播和每一层的基本函数表示如下:
第一层是输入层。输入层的每一个节点直接把输入变量xi(i=1,2,3,4)直接传递到下一层,本专利中,输入节点为深度和角度的和下潜速度与角速度误差,neti1是第一层输出。
第二层是模糊化(隶属度)层。层内的每一个节点都经过隶属度函数,隶属度输入为
这里n是训练次数,αij是代表自反馈循环的权值,μij(n-1)是上一次训练第二层的输出信号,它通过高斯型隶属函数来定义:
mij和σij分别是第i个输入变量的第j个模糊集合的高斯型隶属函数的均值和标准差,它们都是可调参数。j=1,2,…,nj,nj是每个输入的语义变量的数量,μij[netj(rij)]是第二层输出。
第三层是Petri层。该层的目的为了提供令牌,以使用下面的竞争规则:
其中tij是转换值,dth是动态变化的阈值,它将随着系统响应误差的而变化
第四层是规则层。每个节点k由∏表示,即连乘输入输入并输出结果。
式中ωjik是Petri网和规则层的权值,是恒定的值,φk(k=1,2,...ny)第k层输出,ny是规则总数,φk是第四层输出。
第五层是输出层。
其中,连接权值ωko是和第k条规则相关的第O个输出的输出强度,yo是第五层输出。本专利中输出值即为推力器的控制电压值。
DPRFNN使用的在线学习算法是监督-斜坡算法,首先其能量函数E定义为
式中h和θ是控制中遥控水下机器人的实时深度值和艏向值,hd和θd分别是h和θ的期望值。eh=hd-h,eθ=θd-θ分别是深度和艏向的误差。式(9)的动态变化阈值由下式来调整。
式中α和β是正常数。这意味着误差越大阈值越小,即误差越大阈值减小,以使尽量多的控制规则投入使用。
在输出层,误差反向传播值为
连接权值ωko按照下式来更新
ηω连接权值学习率,下一时刻ωko为:
由于规则层的权值是恒定的,该层的误差为:
Petri层中误差如下计算
模糊化层各参数的更新规则为:
下一时刻模糊化层各参数为:
其中ηm,ηw,ησ,ηα是高斯函数的学习率参数。
式中ε是正常数。这样能量函数
结合图6遥控水下机器人的跟踪控制,通过水下机器人装有的CCD获取水下被跟踪物体及场景图像,通过灰度处理和分割、使用神经网络分类器结合
知识库和逻辑推理机制进行特征提取、分类和识别,采用势场法对机器人跟踪路径进行规划。采用图4的方法,对水下机器人发送运动控制指令,从而控制机器人的导管螺旋桨使其完成跟踪的动作。
结合图7遥控水下机器人的避碰控制,通过水下机器人装有的前视声纳获取水下障碍物信息,通过形态膨胀学对声图像进行分割和处理,得到安全区域,采用势场法对机器人避碰路径进行规划,采用图4的方法,对水下机器人发送运动控制指令,从而控制机器人的导管螺旋桨使其完成跟踪的动作。
结合图8,PC/104嵌入式程序采用上位机SOCKET触发机器人嵌入式处理器的方式进行
节拍控制。机器人嵌入式处理器建立SOCKET
服务器端,绑定端口,开始监听等待。水面控制机
请求握手连接。若成功,触发SOCKET事件,PC/104发出控制指令,把
传感器数据通过SOCKET回传给水面控制机。以后每0.1s水面控制机发一次控制指令给PC/104,SOCKET触发PC/104:输出控制指令给执行器;处理传感器信息并通过SOCKET回传给水面控制机,完成一个控制的闭环。若不成功,输出错误信息,进行出错处理。
图8中,水下嵌入式软件通过PC/104总线通信。包括SOCKET通讯模块,
模拟信号电压采集程序,数字/模拟(D/A)转换及电压发送程序,
数字信号采集程序。其中,SOCKET通讯模块用于水面控制机的网络通信;模拟信号电压采集程序负责采集深度计反馈回来的电压值;D/A转换及电压发送程序控制导管螺旋桨的转速;数字信号采集程序监测水密舱内的漏水报警器。
PC/104中采用了实时嵌入式操作系统(VxWorks)。由于VxWorks的嵌入式操作系统提供了Pentium3的BSP,对BSP进行简单的
修改即可使用。主要就是对于Compact Flash Card卡(CF)的支持。可把CF卡当作一块
硬盘来处理。而对于网卡,采用的是Intel 82559ER网卡,这是VxWorks默认的网卡,驱动都是可以直接用的。这样PC/104就可以通过开机自检CF卡启动VxWorks。默认的VxWorks
内核支持CUP板上自带的两个串口″/tyCo/0″和″/tyCo/1″。由于采用串口板进行光纤罗经数据采集,因此必须在VxWorks下驱动串口板。
通过接收到机器人的位姿(深度Depth(即z值),姿态角(Yaw,Pith,Roll)和是否漏水信息),CCD拍摄到的视频和前视声纳获得和处理的信息,机器人控制手柄或控制器向水面控制机发送控制指令,水面控制机通过SOCKET将推力器指令由PC/104发送给导管螺旋桨。