专利名称：浮游式水下机器人运动的s面控制方法
技术领域：
本发明涉及浮游式水下机器人运动的S面控制方法。
背景技术：
浮游式水下机器人的动力学系统是非线性的、时变的。各个自由度的运动相互耦合，海流等环境干扰力的影响较大。总之，水下机器人是一个强非线性系统。而且考虑到运动的时变性、环境的复杂性和不确定性，建立精确的水下机器人运动模型是不现实的。所以，水下机器人控制系统的设计必须考虑非线性问题和运动模型的复杂性。同时，从加强机器人的自主性与适应性，改善机器人工作性能的观点来看，控制系统还应该具备良好的自适应与自学能力。水下机器人的运动控制器通常有PID控制器，神经网络控制器和模糊逻辑控制器三种，由于水下机器人运动模型的复杂性，因此更多的研究人员把注意力和精力放在现代控制论的模糊逻辑和神经网络控制技术上。
美国的佛罗里达大西洋大学成功地将模糊逻辑控制应用于智能水下机器人”Ocean Explor”上，并据此完成了一系列海上试验。模糊控制器的设计比较简单实用。然而，模糊控制属于主观性很强的规则控制。对于水下机器人，人们不太可能得到较好的操纵经验，因此其隶属度函数的设定、推理方法和反模糊化方法以及模糊规则的获取存在相当的难度。而这些参数设计的好与坏，又将直接影响到控制的效果。所以，参数调整的复杂度制约了模糊控制技术在水下机器人运动控制中的广泛应用。
神经网络控制的优点是充分考虑到了水下机器人的强非线性和各个自由度之间的耦合，同时其学习机制能够跟踪系统自身或外围环境的缓慢变化。1996年，哈尔滨工程大学海洋综合技术研究中心采用神经网络控制，ZS-II型水下机器人分别在水池和湖中进行了试验。但其缺点是当环境变化比较剧烈，如在波浪中或是涨潮退潮时的港湾中(存在较大的环流)，外界干扰的幅度和周期与水下机器人自身的运动幅度和周期相近时，神经网络的学习就出现明显的滞后现象，控制容易发生振荡。
因此，研制出一种能有效适用于水下机器人复杂的强非线性系统，参数数目少，结构简单，便于调节的控制方法，是水下机器人运动控制的关键性技术问题。而本申请所述的S面控制方法体现了上述优点，适用于水下机器人的运动控制。
目前已经受专利保护或已提出申请的水下机器人控制方法只涉及到柔性机器鱼的控制。但这些运动控制方法都是针对机器鱼柔性尾鳍的控制。与本申请所述的刚体浮游式水下机器人的运动控制方法有极大不同。如中国专利局在2004年3月24日已公开的发明专利(申请号02142836.0)“仿生机器鱼游动方向的模糊控制方法”。
发明内容
本发明的目的是提供一种适合于浮游式水下机器人运动的S面控制方法。可以有效地适应水下机器人强非线性动力学特性和复杂的运动关系，实现精确的运动控制。
为实现上述目的，浮游式水下机器人运动的S面控制方法步骤为基于短基线和深度计获取位置信息，基于多普勒速度计获取速度信息，基于罗经获取姿态角信息，通过控制器解算，由螺旋桨和舵、翼执行运动。
小范围定位时，通过六自由度位置偏差和偏差变化率(速度)作为输入的位置控制及精确动力定位。
大范围航渡时，通过六自由度速度偏差和偏差变化率(加速度)作为输入的速度控制。
通过PD控制的指数化实现对水下机器人的非线性控制。
本发明可有效提高水下机器人运动控制的精度，实际应用于水下机器人控制系统设计，进行水下探测，考古，水下救捞，水下设施的维护和海洋领土的防御等。
图1浮游式水下机器人运动的S面控制方法输入输出关系曲面2浮游式水下机器人运动的S面控制方法解算流程3WXX-I型水下机器人S面控制器精确动力定位响应曲线图具体实施方式
浮游式水下机器人运动控制工作包括当前位置姿态的获取，控制器的解算，执行机构的力学输出。在本发明中，我们基于短基线和深度计获取位置信息，基于多普勒速度计获取速度信息，基于罗经获取姿态角信息，通过控制器解算，由螺旋桨和舵、翼执行运动。
短基线能提供水下平面坐标系中相对于基阵的坐标x，y；深度计提供深度信息，即坐标z；罗经提供航态角(艏向角，纵倾角和横摇角)。多普勒速度计提供速度信息，根据目标与当前的偏差和偏差变化律(若为位置控制，偏差为位置差，偏差变化律为速度；若为速度控制，偏差为速度差，偏差变化律为加速度)决定控制器的输入。通过运动控制器解算，得到系统输出各推力器的应输出推力。进而决定施力装置的电压输出和脉冲输出。
S面控制方法是控制器的解算方法。输入为偏差和偏差变化律，输出为各施力装置的推力。
小范围定位时，通过六自由度位置偏差和偏差变化率(速度)作为输入的位置控制及精确动力定位。
大范围航渡时，通过六自由度速度偏差和偏差变化率(加速度)作为输入的速度控制。
从常规模糊控制器的控制规则(表1)可以看出，控制输出的变化是有规律可循的。取主对角线数值连成折线，可以用一条光滑曲线(是一个Sigmoid函数)来拟合，事实上，光滑曲线可以看作无数条长度趋向于零的折线相连。我们在设计模糊控制器的时候，通常采用的是两边疏松，中间密的形式(即偏差大时采用控制比较粗，而偏差小时控制比较细)，这一点与Sigmoid函数的变化形式是一致的，因此，Sigmoid函数在一定程度上体现了模糊控制的思想。同理，整个模糊控制规则库对应的折线面，也就可以用光滑曲线的曲线面代替。
表1常规模糊控制器的控制规则
一般地，Sigmoid曲线函数为y＝2.0/(1.0+e-kx)-1.0 (1)那么，Sigmoid曲面函数为z=2.0/(1.0+e(-k1x-k2y))-1.0---(2)]]>因此，我们设计的S面控制器的控制模型为
其中，e和&为控制的输入信息(偏差和偏差变化率，通过归一化处理)，u为控制输出，k1和k2分别为对应偏差和偏差变化率的控制参数，可以改变其对应自由度的变化速度。如附图1(浮游式水下机器人运动的S面控制方法输入输出关系曲面图)所示。在这里控制器需要调整的参数只有k1和k2。
人工调整控制器参数k1和k2，使得在一般情况下水下机器人的运动控制满足要求。k1和k2取得越大，响应对小偏差的敏感性越高，但取得太大容易引起振荡。如果超调大了，可以适当减小k1而增加k2，反之，如果收敛速度慢了，则可以适当增加k1而减小k2。通过调整k1和k2，调整偏差和偏差变化率在控制输入中所占的比例，最终达到一个近最优的结果。
对于海流、未知扰动等因素，可以考虑成一段时间内的固定干扰力，也就是说在目标点附近，可能会存在一个固定偏差，我们可以通过增加积分项，调整S面的偏移，来达到消除固定偏差的目的。为此，我们对S面方法进行改进，增加积分项。采用如附图2控制模型。
控制器输出为u(t)=Σi=13ki(t)xi(t)---(5)]]>u0(t)=f(u)=1-e-u(t)1+e-u(t)---(6)]]>ug(t)＝Umaxu0(t) (7)式中
ki——控制器参数Umax——最大控制量。
其中积分项x3的计算我们采用自适应的方式，自适应方式如下①判断机器人的运动速度是否小于一个设定的阈值，若是，转②，若不是，转③；②将该自由度的偏差值赋予一个设定的数组，同时将设定的计数值加1，当该计数值达到一个预定的定值时，转④；③将数组的值往前窜一位，同时计数值减1，转①；④对这一数组的数值加权平均，得到的平均偏差值用于计算控制输出的偏移量，自适应调整控制器的输出，以消除固定偏差，同时将计数值和数组归零，进行下一个循环。
如此就构造了一个简单、实用的水下机器人控制器，在复杂的海洋环境下该控制器控制效果可以满足作业要求。但此时S面控制器的参数调整完全是手工的，我们可以寻找一种自学习的方法，让控制器通过自学习在线的调整参数，使控制器有更高的精度，达到更好的控制效果。
为了给S面控制增加自学习的功能，我们可以借鉴神经网络的误差反传学习算法，定义一个合适的输出误差函数，通过学习算法修改控制器参数，使控制器的输出误差最小。因为水下机器人的运动具有自己的运动意识或则说它能反映人的意识，所以水下机器人有一个期望的运动状态，对控制而言，也就有一个期望的输出。因此我们首先定义输出误差函数为Ep=12(ud-u)2---(8)]]>式中，ud是由输出规划器得到的期望输出，u是前一时刻输出。
通过梯度下降法最优化方法，可用Ep的梯度来修正系数k1和k2。
Δki=-η∂Ep∂ki---(9)]]>式中η是学习率(0＜η＜1.0)∂Ep∂ki=-(ud-u)∂u∂ki]]> 所以系数k1和k2按以下公式来修正ki(t+1)＝ki(t)+Δki 这样就使得S面控制方法具有了自学习的能力，可以通过自学习自动调整参数。
具体实施例方式
本发明提供的浮游式水下机器人运动的S面控制方法应用于哈尔滨工程大学海洋综合技术研究中心研制的WXX-I型水下机器人。该机器人是为探索智能水下机器人小型化和集群化而研制的。其B/L大，回转性好，而航行稳定性差，精确运动控制难度较大。
控制流的过程为短基线系统提供大地坐标系中当前的x，y值，深度计提供z值，磁罗经提供当前姿态角度(艏向角，纵倾角，横摇角)信息。规划系统(软件的一部分)决定目标位姿的x，y，z，艏向角，纵倾角，横摇角。从而得到六自由度控制偏差，差分得偏差变化律(速度)。运用S面控制方法解算六自由度输出力大小，进行推力分配得到施力装置应施加的力。通过光纤传至水下控制机。通过相应电路接口将力数据传至螺旋桨和翼的控制电机，产生控制力，驱动WXX-I完成精确运动。
上述方案作为实施例1，其配比如下
表4自学习后的ke，kv值以位置控制为例，S面控制方法经过参数整定后，试验得到如附图3的响应曲线。可以看出响应较快，且超调很小。能满足工程实际的需要。
本发明的优点是适用于浮游式水下机器人复杂的强非线性系统，参数数目少，结构简单，便于调节，控制精度高。可以应用于通用的浮游式水下机器人控制系统设计，进行水下探测，考古，水下救捞，水下设施的维护和海洋领土的防御等。
权利要求
1.一种浮游式水下机器人运动的S面控制方法，其特征在于它包括步骤基于短基线和深度计获取位置信息，基于罗经获取姿态角信息，基于多普勒速度计获取速度信息，通过控制器解算，由螺旋桨和舵、翼执行运动；小范围定位时，通过六自由度位置偏差和偏差变化率作为输入的位置控制及精确动力定位；大范围航渡时，通过六自由度速度偏差和偏差变化率作为输入的速度控制。
2.如权利要求1所述的浮游式水下机器人运动的S面控制方法，其特征在于其控制器通过PD控制的指数化实现对水下机器人的非线性控制。
全文摘要
本发明浮游式水下机器人运动的S面控制方法，包括步骤基于短基线和深度计获取位置信息，基于罗经获取姿态角信息，基于多普勒速度计获取速度信息，通过控制器解算，通过螺旋桨和舵、翼执行运动。小范围定位时，通过六自由度位置偏差和偏差变化率作为输入的位置控制及精确动力定位。大范围航渡时，通过六自由度速度偏差和偏差变化率作为输入的速度控制。通过PD控制的指数化实现对水下机器人的非线性控制。本发明适用于浮游式水下机器人复杂的强非线性系统，参数数目少，便于调节，控制精度高。可实际应用于浮游式水下机器人控制系统设计，进行水下探测，考古，水下救捞，水下设施的维护和海洋领土的防御等。
文档编号B25J13/00GK1718378SQ20051001011
公开日2006年1月11日 申请日期2005年6月24日 优先权日2005年6月24日
发明者李晔, 万磊, 苏玉民, 庞永杰, 刘学敏 申请人:哈尔滨工程大学