一种遥控水下机器人水面控制方法

1.一种遥控水下机器人水面控制方法，其特征是：通过水下机器人装有的CCD和前视声纳获取水下被跟踪物体、场景图像和水下障碍物的包括艏向和深度的信息，根据获取的信息采用改进的Sage-Husa自适应卡尔曼滤波算法获得水下机器人实际的艏向和深度，根据获得的水下机器人实际的艏向和深度、通过递归的以太神经网络DPRFNN算法自动对水下机器人发送运动控制指令；
所述的改进的Sage-Husa自适应卡尔曼滤波算法是在基本的KALMAN滤波基础上增加的调整，其中 为系统噪声序列的方差阵Q(k)的估计，
为量测噪声序列方差阵R(k)的估计， 为系统噪声统计的估计值， 为量测噪声统计的估计值：
其中 是状态X(k)的估计，φ(k，k-1)为t(k-1)时刻至t(k)时刻的一步转移阵，H(k)为量测阵， 为新息矩阵，新息包含有一步预
测的误差，对其作适当加权处理将 分离出来修正 b是遗忘因子，
它对滤波的发散和精度起着至关重要的作用，并通过调整
Pk+1k控制滤波增益阵Kk+1来防止滤波器的发散，当 不成立，
则按 修正Pl+1k，其中γ≥1为事先确定可调系数，Sk+1是自适应系
数，将系统噪声序列的方差阵Q(k)的估计 量测噪声序列方差阵R(k)的估计系统噪声统计的估计值 量测噪声统计的估计值 和卡尔曼滤波结合就构成改进的Sage-Husa自适应卡尔曼滤波算法；
所述的以太神经网络DPRFNN算法包括输入层i、隶属度层j、petri层、规则层k层和输出层o层五层结构，递归反馈通过在隶属度层嵌入反馈连接来实现，信号的传播和每一层的基本函数表示如下：
(1)第一层是输入层，输入层的每一个节点直接把输入变量xi(i＝1，2，3，4)直接传递到下一层，输入节点为深度误差、角度误差、下潜速度误差和角速度误差， 是第一层输出：
(2)第二层是隶属度层，层内的每一个节点都经过隶属度函数，隶属度输入为n是训练次数， 是代表自反馈循环的权值， 是上一次训练第二层的输出信号，它通过高斯型隶属函数来定义：
和 分别是第i个输入变量的第j个模糊集合的高斯型隶属函数的均值和际准差，它们都是可调参数，j=1，2，…，nj，nj是每个输入的语义变量的数量， 是第二层输出；
(3)第三层是Petri层，它提供令牌，使用如下的竞争规则
其中 是转换值，dth是动态变化的阈值、随着系统响应误差的变化而变化；
(4)第四层是规则层，每个节点k由Π表示，即连乘输入并输出结果：
式中 是petri层和规则层的权值、为恒定的值，φk，k=1，2，...ny，ny是规则总数，φk是第四层输出；
(5)第五层是输出层
其中，连接权值 是和第k条规则相关的第o个输出的输出强度，yo是第五层输出，输出值即为推力器的控制电压值；
DPRFNN使用的在线学习算法是监督-斜坡算法，该算法为首先其能量函数E定义为ed=hd-h、eθ=θd-θ分别是深度和艏向的误差；
上式中，h和θ是控制中遥控水下机器人的实时深度值和艏向值，hd和θd分别是h和θ的期望值，动态变化阈值dth由下式来调整：
式中α和β是正常数；
在输出层，误差反向传播值为
连接权值 按照下式来更新：
ηω连接权值学习率，下一时刻 为：
规则层的权值是恒定的，则该层的误差为：
Petri层中误差如下计算：
隶属度层各参数的更新规则为：
下一时刻隶属度层各参数为：
式中ηm，ηs，ηα是高斯函数的学习率参数，ε是正常数。

一种遥控水下机器人水面控制方法\n技术领域\n[0001] 本发明涉及的是一种控制方法，具体地说是应用于水下数据采集、深海作业和船体检测等领域机器人的一种控制方法。\n背景技术\n[0002] 占地球表面积75％的海洋，是一个富饶而远未得到开发的宝库。人类要维持生存繁衍和发展，充分利用地球仅有的这块最后待开发的疆土，将是无可回避的选择。水下机器人由于其行动灵活，动力充足，信息和数据的传递和交换快捷方便、数据量大，能够在水中长时间工作，现已广泛应用于水下工程、海洋石油资源开发、海洋矿物资源调查、海洋生物资源调查、深海打捞、船体检测等方面，因此水下机器人的研制和应用具有重要的战略意义。\n[0003] 申请号为200810064256.3的中国专利文件(公开日：2008年4月8日)中公开的“基于递归模糊神经网络的水下潜器空间变结构控制方法”提供了水下潜器自动舵控制的控制方法。基于递归模糊神经网络的水下潜器空间变结构控制方法虽然与本专利属于同一技术领域，但它是通过设计基于递归模糊神经网络(RFNN)的方向舵控制系统、围壳舵控制系统、尾升舵控制系统，进而构成水下潜器空间运动联合控制系统，与本发明实现的方法不同。\n[0004] 而本发明是通过获得传感器和环境感知信息对水下机器人的运动进行控制，进而实现其避障，跟踪，检测等功能。\n发明内容\n[0005] 本发明的目的在于提供用于控制水下数据采集、深海作业和船体检测等领域机器人的一种遥控水下机器人水面控制方法。\n[0006] 本发明的目的是这样实现的：\n[0007] 本发明通过水下机器人装有的CCD和前视声纳获取水下被跟踪物体、场景图像和水下障碍物的信息，根据获取的信息采用改进的Sage-Husa自适应卡尔曼滤波算法获得水下机器人实际的艏向和深度，根据获得的水下机器人实际的艏向和深度、通过递归的以太神经网络DPRFNN算法自动对水下机器人发送运动控制指令。\n[0008] 本发明的一种遥控水下机器人水面控制方法还包括：\n[0009] 1、所述的改进的Sage-Husa自适应卡尔曼滤波算法是在基本的KALMAN滤波基础上增加系统噪声统计的估计值 和量测噪声统计的估计值 的调整：\n[0010] 系统干扰均值\n[0011] \n[0012] 系统干扰方差矩阵\n[0013] \n[0014] \n[0015] 量测噪声均值\n[0016] \n[0017] 量测噪声方差矩阵\n[0018] \n[0019] 其中 是状态X(k)的估计，Φ(k，k-1)为t(k-1)时刻至t(k)时刻的一步转移阵，H(k)为量测阵， 为系统噪声序列的方差阵Q(k)的估计， 为量测噪声序列方差阵R(k)的估计， 为新息矩阵，新息包含有一步\n预测的误差，对其作适当加权处理将 分离出来修正 b是遗忘因子\n它对滤波的发散和精度起着至关重要的作用，并通过调整\nPk+1|k控制滤波增益阵Kk+1来防止滤波器的发散，当 不成立，\n则按 修正Pk+1|k，其中γ≥1为事先确定可调系数，Sk+1是自适应系\n数，将系统干扰均值、系统干扰方差矩阵、量测噪声均值、量测噪声方差矩阵和卡尔曼滤波结合就构成了改进的Sage-Husa自适应卡尔曼滤波算法。\n[0020] 2、所述的以太神经网络DPRFNN算法包括输入层i、隶属度层j、petri以太层、规则层k层和输出层o层五层结构，递归反馈通过在隶属度层嵌入反馈连接来实现，信号的传播和每一层的基本函数表示如下：\n[0021] (1)第一层是输入层，输入层的每一个节点直接把输入变量xi(i＝1，2，3，4)直\n1\n接传递到下一层，输入节点为深度和角度的和下潜速度与角速度误差，neti 是第一层输出：\n[0022] (2)第二层是隶属度层，层内的每一个节点都经过隶属度函数，隶属度输入为[0023] \n[0024] n是训练次数，αij是代表自反馈循环的权值，μij(n-1)是上一次训练第二层的输出信号，它通过高斯型隶属函数来定义：\n[0025] \n[0026] mij和σij分别是第i个输入变量的第j个模糊集合的高斯型隶属函数的均值和标准差，它们都是可调参数，j＝1，2，…，nj，nj是每个输入的语义变量的数量，μij[netj(rij)]是第二层输出；\n[0027] (3)第三层是Petri层，它提供令牌，使用如下的竞争规则\n[0028] \nj\n[0029] 其中ti 是转换值，dth是动态变化的阈值、随着系统响应误差的而变化；\n[0030] (4)第四层是规则层，每个节点k由∏表示，即连乘输入输入并输出结果：\n[0031] \n[0032] 式中ωjik是Petri网和规则层的权值、为恒定的值，φk(k＝1，2，...ny)第k层输出，ny是规则总数，φk是第四层输出；\n[0033] (5)第五层是输出层：\no\n[0034] 其中，连接权值ωk 是和第k条规则相关的第0个输出的输出强度，yo是第五层输出，输出值即为推力器的控制电压值；\n[0035] DPRFNN使用的在线学习算法是监督-斜坡算法，该算法为首先其能量函数E定义为：\n[0036] 式中h和θ是控制中遥控水下机器人的实时深度值和艏向值，hd和θd分别是h和θ的期望值，eh＝hd-h、eθ＝θd-θ分别是深度和艏向的误差，动态变化阈值dth由下式来调整：\n[0037] 式中α和β是正常数；\n[0038] 在输出层，误差反向传播值为：\n[0039] 连接权值ωko按照下式来更新：\n[0040] ηω连接权值学习率，下一时刻ωko为：\n[0041] 规则层的权值是恒定的，则该层的误差为：\n[0042] Petri层中误差如下计算\n[0043] \n[0044] 模糊化层各参数的更新规则为：\n[0045] \n[0046] \n[0047] 下一时刻模糊化层各参数为：\n[0048] \n[0049] \n[0050] \n[0051] 式中ηm，ηw，ησ，ηα是高斯函数的学习率参数，ε是正常数。\n[0052] 本发明的优势在于：简单、灵活，功能强大、适应性强等。\n附图说明\n[0053] 图1是本发明的控制方法总图；\n[0054] 图2是本发明的无线操纵杆手操控制方法体系框图；\n[0055] 图3是本发明的改进的Sage-Husa自适应KALMAN滤波流程图；\n[0056] 图4是本发明的遥控水下机器人FNN深度和艏向控制体系框图；\n[0057] 图5是本发明的自动控制遥控水下机器人的五层DPRFNN结构图；\n[0058] 图6是本发明的遥控水下机器人跟踪运动控制流程图；\n[0059] 图7是本发明的遥控水下机器人避碰运动控制流程图；\n[0060] 图8是PC/104计算机与水面主控计算机的联系SOCKET通讯过程图。\n具体实施方式\n[0061] 下面结合附图举例对本发明做更详细地描述：\n[0062] 结合图1～8，本实施方式的主体是水面遥控制方法。其中水面遥控制方法硬件通过光纤和ROV相连，实现水下机器人的控制。水下机器人自动控制方法水面控制机采用WindowsXP操作系统，利用VisualC++图形进行可视编程，建立主控程序。\n[0063] 结合图1遥控水下机器人的水面控制方法由水面遥控平台和水下机器人本体两部分来实现，水面遥控平台的硬件设备由一台水面主控计算机，光端机，液晶显示器和一个机器人无线控制操纵手柄组成。水面遥控平台的软件由水下机器人的操纵杆手操控制，声视觉识别和避障算法，视觉规划跟踪算法，传感器滤波算法与水下PC/104的SOCKET通讯模块组成。水面遥控平台通过水下机器人装有的环境感知设备和运动感知设备获取作业(包括视频、声图像)、姿态和深度信息(经过滤波处理)，控制者可以通过操纵杆手操，FNN定向和定深控制，跟踪和避碰控制，发送机器人的运动控制命令，该命令通过SOCKET通讯模块和光端机传递给水下PC/104，从而控制机器人的导管螺旋桨完成动作。\n[0064] 结合图2无线操纵杆手操控制方法为操纵者实时提供水下机器人的当前深度；艏向角，各推力器电压信息；运动轨迹和当前位置，所采集到的视频和前视声纳(SONAR)信号等信息，操纵者根据这些信息，给水下机器人的六个导管螺旋桨推进器(包括两个主推，两个侧推，两个垂推)发送控制电压，控制机器人沿目标位姿和方向前进。其中，艏向角和深度由罗经和深度计经过野值剔除和改进的Sage-Husa自适应KALMAN滤波得到；手操同时对设备进行故障检测和诊断，以保证设备正常运行。\n[0065] 结合图3采用改进的Sage-Husa自适应卡尔曼滤波获得水下机器人实际的艏向和深度。\n[0066] 其实现方案就是在基本的KALMAN滤波基础上增加了系统噪声统计q(k)、Q(k)和量测噪声统计r(k)、R(k)的调整：\n[0067] 系统干扰均值\n[0068] \n[0069] 系统干扰方差矩阵\n[0070] \n[0071] \n[0072] 量测噪声均值\n[0073] \n[0074] 量测噪声方差矩阵\n[0075] \n[0076] 其中 是状态X(k)的估计，Φ(k，k-1)为t(k-1)时刻至t(k)时刻的一步转移阵，H(k)为量测阵， 为系统噪声序列的方差阵Q(k)的估计， 为量测噪声序列方差阵R(k)的估计， 为新息矩阵，新息包含有一步预\n测的误差，对其作适当加权处理就能将 分离出来修正 b是遗忘因\n子 它对滤波的发散和精度起着至关重要的作用。\n[0077] 并通 过 调 整Pk+1|k 控制 滤 波增 益 阵Kk+1来 防止 滤 波 器的 发 散，当不成立，则按 修正Pk+1|k，其中\nγ≥1为事先确定可调系数，Sk+1是自适应系数。\n[0078] 将上面的公式和卡尔曼滤波结合交替计算就构成了改进的Sage-Husa自适应卡尔曼滤波算法。\n[0079] 结合图4、5水下机器人自动控制方法：图4中采用改进的Sage-Husa自适应KALMAN滤波的方法对数据进行最优估计。机器人控制器采用模糊神经网络控制器，控制器根据收到的姿态信息进行对机器人本体在自动控制时进行深度或目标位姿的控制。结合水动力参数，对机器人本体和仿真同时进行，以保证控制的实时和准确性。同时对设备进行故障检测和诊断，以保证设备正常运行。\n[0080] 图5中描述了自动控制遥控水下机器人使用的一个五层DPRFNN结构。包含输入层i，隶属度层j，petri以太层，规则层k层，和输出层o层，如图所示。递归反馈通过在隶属度层嵌入反馈连接来实现。信号的传播和每一层的基本函数表示如下：\n[0081] 第一层是输入层。输入层的每一个节点直接把输入变量xi(i＝1，2，3，4)直接传\n1\n递到下一层，本专利中，输入节点为深度和角度的和下潜速度与角速度误差，neti 是第一层输出。\n[0082] 第二层是模糊化(隶属度)层。层内的每一个节点都经过隶属度函数，隶属度输入为\n[0083] 这里n是训练次数，αij是代表自反馈循环的权值，μij(n-1)是上一次训练第二层的输出信号，它通过高斯型隶属函数来定义：\n[0084] \n[0085] \n[0086] mij和σij分别是第i个输入变量的第j个模糊集合的高斯型隶属函数的均值和标j j\n准差，它们都是可调参数。j＝1，2，…，nj，nj是每个输入的语义变量的数量，μi[netj(ri)]是第二层输出。\n[0087] 第三层是Petri层。该层的目的为了提供令牌，以使用下面的竞争规则：\n[0088] \n[0089] 其中tij是转换值，dth是动态变化的阈值，它将随着系统响应误差的而变化[0090] 第四层是规则层。每个节点k由∏表示，即连乘输入输入并输出结果。\n[0091] \n[0092] 式中ωjik是Petri网和规则层的权值，是恒定的值，φk(k＝1，2，...ny)第k层输出，ny是规则总数，φk是第四层输出。\n[0093] 第五层是输出层。\n[0094] 其中，连接权值ωko是和第k条规则相关的第O个输出的输出强度，yo是第五层输出。本专利中输出值即为推力器的控制电压值。\n[0095] DPRFNN使用的在线学习算法是监督-斜坡算法，首先其能量函数E定义为[0096] 式中h和θ是控制中遥控水下机器人的实时深度值和艏向值，hd和θd分别是h和θ的期望值。eh＝hd-h，eθ＝θd-θ分别是深度和艏向的误差。式(9)的动态变化阈值由下式来调整。\n[0097] 式中α和β是正常数。这意味着误差越大阈值越小，即误差越大阈值减小，以使尽量多的控制规则投入使用。\n[0098] 在输出层，误差反向传播值为\no\n[0099] 连接权值ωk 按照下式来更新\n[0100] ηω连接权值学习率，下一时刻ωko为：\n[0101] 由于规则层的权值是恒定的，该层的误差为：\n[0102] Petri层中误差如下计算\n[0103] 模糊化层各参数的更新规则为：\n[0104] \n[0105] \n[0106] 下一时刻模糊化层各参数为：\n[0107] \n[0108] 其中ηm，ηw，ησ，ηα是高斯函数的学习率参数。\n[0109] \n[0110] \n[0111] 式中ε是正常数。这样能量函数\n[0112] \n[0113] \n[0114] \n[0115] 结合图6遥控水下机器人的跟踪控制，通过水下机器人装有的CCD获取水下被跟踪物体及场景图像，通过灰度处理和分割、使用神经网络分类器结合知识库和逻辑推理机制进行特征提取、分类和识别，采用势场法对机器人跟踪路径进行规划。采用图4的方法，对水下机器人发送运动控制指令，从而控制机器人的导管螺旋桨使其完成跟踪的动作。\n[0116] 结合图7遥控水下机器人的避碰控制，通过水下机器人装有的前视声纳获取水下障碍物信息，通过形态膨胀学对声图像进行分割和处理，得到安全区域，采用势场法对机器人避碰路径进行规划，采用图4的方法，对水下机器人发送运动控制指令，从而控制机器人的导管螺旋桨使其完成跟踪的动作。\n[0117] 结合图8，PC/104嵌入式程序采用上位机SOCKET触发机器人嵌入式处理器的方式进行节拍控制。机器人嵌入式处理器建立SOCKET服务器端，绑定端口，开始监听等待。水面控制机请求握手连接。若成功，触发SOCKET事件，PC/104发出控制指令，把传感器数据通过SOCKET回传给水面控制机。以后每0.1s水面控制机发一次控制指令给PC/104，SOCKET触发PC/104：输出控制指令给执行器；处理传感器信息并通过SOCKET回传给水面控制机，完成一个控制的闭环。若不成功，输出错误信息，进行出错处理。\n[0118] 图8中，水下嵌入式软件通过PC/104总线通信。包括SOCKET通讯模块，模拟信号电压采集程序，数字/模拟(D/A)转换及电压发送程序，数字信号采集程序。其中，SOCKET通讯模块用于水面控制机的网络通信；模拟信号电压采集程序负责采集深度计反馈回来的电压值；D/A转换及电压发送程序控制导管螺旋桨的转速；数字信号采集程序监测水密舱内的漏水报警器。\n[0119] PC/104中采用了实时嵌入式操作系统(VxWorks)。由于VxWorks的嵌入式操作系统提供了Pentium3的BSP，对BSP进行简单的修改即可使用。主要就是对于Compact Flash Card卡(CF)的支持。可把CF卡当作一块硬盘来处理。而对于网卡，采用的是Intel \n82559ER网卡，这是VxWorks默认的网卡，驱动都是可以直接用的。这样PC/104就可以通过开机自检CF卡启动VxWorks。默认的VxWorks内核支持CUP板上自带的两个串口″/tyCo/0″和″/tyCo/1″。由于采用串口板进行光纤罗经数据采集，因此必须在VxWorks下驱动串口板。\n[0120] 通过接收到机器人的位姿(深度Depth(即z值)，姿态角(Yaw，Pith，Roll)和是否漏水信息)，CCD拍摄到的视频和前视声纳获得和处理的信息，机器人控制手柄或控制器向水面控制机发送控制指令，水面控制机通过SOCKET将推力器指令由PC/104发送给导管螺旋桨。