本发明属于无人水下机器人运动控制技术领域，具体涉及基于q学习的欠驱动auv虚拟锚泊三维镇定控制及实现方法。
背景技术：
欠驱动水下机器人(underacutatedautonomousunderwatervehicles)是指独立控制输入维数少于系统自由度的水下机器人，它是我国实施深海资源开发战略不可或缺的重大技术装备之一。由于其欠驱动水下机器人所存在的强耦合性和非线性的特征，并且动力学模型中的一些水动力系数并不是一个稳定的数值，很难建立欠驱动auv的精确模型，因此欠驱动auv的准确控制是一大难题。
运动控制技术是auv技术领域中的关键，实现欠驱动auv在干扰环境下的镇定控制是欠驱动auv进行水下精确对接的重要保证之一，也是完成海底探测和水下搜救的关键技术之一。欠驱动auv的镇定控制的难题在于系统的非完整约束性(不满足brocket必要性条件)，使得线性控制方法和一些现代非线性控制方法，无法直接应用于欠驱动auv的镇定控制当中。目前国内外对于欠驱动机械系统和移动机器人的镇定研究已经比较完整了，但是对于欠驱动水下机器人的镇定控制研究还比较有限，且对于欠驱动系统都是基于系统运动学模型，采用非线性状态反馈的思想对其进行镇定，主要方法有：齐次方法、σ-变换法(σ-processing)和反步法。然而这些方法都是基于模型，并且抗干扰能力差，一些抗干扰能力强的镇定控制算法也是基于不精确模型，无法完全摆脱对模型的依赖。
具有抗干扰能力的镇定控制系统可以实现欠驱动auv在一定干扰环境下的准确定位，它可以根据海流、目标位置与实时位置的差异等信息自主计算出所需的力和力矩，然后交由欠驱动auv的纵向推进器、水平舵和垂直舵等执行器，从而保持在目标位置并完成相应的任务。
文献“欠驱动智能水下机器人的三维轨迹跟踪控制方法研究”，针对欠驱动auv存在高度的非线性和非完整性等特性(简化了其动力学方程和运动学方程)，基于微分同胚变换，从auv的六自由度运动方程中得到一个级联形式的非线性系统，并且证明了原系统中镇定控制问题与所得到的级联系统的稳定问题是等价的，并应用李雅普诺夫函数得到一个较为简单的控制律
该方法虽然经过了仿真的验证，方法也较为简单，但其依赖于精确的模型，并且没有在干扰环境进行仿真验证，因此其实际应用方面将会比较狭小，缺乏实用性。
本发明设计了一种基于q学习的欠驱动auv虚拟锚泊三维镇定控制及实现方法。该镇定控制方法完全不依赖于模型，控制算法结构较为简单，并且能够让欠驱动auv在干扰环境下实现抵抗海流的目标，具有很强的实际应用前景，并且在虚拟锚泊控制的基础上添加了q学习方法，加快了算法的收敛速度，减少其本身的盲目性，当q学习收敛完毕，完成训练后，进行纯q学习，利用q学习本身的随机探索的特性，从而弥补虚拟锚泊控制在接近目标后出现的收敛较慢的问题，两者进行互补。该方法最终可用于欠驱动auv在实际海洋环境中的镇定控制中。
技术实现要素：
本发明的目的在于提供一种控制算法结构较为简单、加快了算法的收敛速度、具有很强的实际应用前景的基于q学习的欠驱动auv虚拟锚泊三维镇定控制及实现方法。
本发明的目的通过如下技术方案来实现：
(1)初始化各变量，设定输入状态变量、奖罚值r和输出动作a，建立q表，并输入控制命令：
设定q学习的输入状态变量s：
s＝{uxbxybyzbz}
式中：u为欠驱动auv的速度，ξ为横向位移和bξ为横向位移偏差，η为纵向位移和bη为纵向向位移偏差，ζ为垂向位移和bζ为垂向向位移偏差；
设定奖罚值r：
设定输出动作a：
a＝{xnm}
式中：x为主推进器的推力，n为转艏力矩，m为俯仰力矩；
设定q值表为多维的空间，以状态s为纵坐标，动作a为横坐标，q值为一个维度，初始化为全零空间；
(2)建立q学习迭代方程，输入各变量：
根据q学习的更新方法，建立q学习迭代方程：
式中：α为学习率，γ为折扣因数，表示对未来利益的重视程度；输入各个变量，例如初始状态s到系统中，进行计算；
(3)根据q迭代的收敛性选择策略，选择动作a：
首先是判断迭代方程的收敛性，是则选择根据ε-greedy策略选择q(s,a)值最大的动作值a(如果在该状态下q值全为零，则随机选择动作a)，否则利用虚拟锚泊控制来产生x、n、m，并以此组成动作a，以该动作来选择q(s,a)值。
(4)动作a作用于auv，得到所需参数，更新q值和q表：
auv执行该动作a，到了s状态所相对的下一时刻的状态s′，并将评价该动作a的好坏，给定奖罚值r(s,a,s′)；将s′在q表中查找值的最大数
(5)进行状态交换，判断auv是否达到目标且保持镇定状态，是则退出迭代循环，在满足espisode次数后，结束系统，否则继续迭代：
将s′状态的变量赋值给s状态，并通过auv是否抵达目标点，且保持镇定状态，是则开始新一轮的循环，也就是q值的迭代，否则退出这个循环，判断auv的espisode(从开始到结束运行的次数)的次数是否大于200，是则退出系统，q学习完成，否则继续训练。
(6)虚拟锚泊控制部分具体内容为：
(6.1)建立欠驱动auv的六自由度动力学方程:
在三维空间，执行机构产生的力/力矩向量可表示为：
τ＝[xnm]t
auv六自由度动力学方程可以表示为：
(6.2)auv六自由度动力学方程可以表示为：
(6.3)建立还有环境影响下的欠驱动auv的相对速度和相对加速度的方程：
在海洋环境中，auv所受到的干扰力较为复杂以及多变，例如海流本身的影响，海水密度、盐度和温度的影响等。本文假定水域为深广水域，只存在海流的影响。
相对流速:
ur＝u-uccos(θ)cos(αc-ψ)
vr＝v-ucsin(αc-ψ)
wr＝w-ucsin(θ)cos(αc-ψ)
其中，uc为流速，αc为流向角，ψ为艏向角，θ为纵倾角，对相对速度式进行时间的微分后可得到相对加速度：
(6.4)根据欠驱动水下机器人在虚拟锚泊状态下受力分析，建立了虚拟锚泊的数学模型：
在一定深度的海中，auv将锚链连接的锚抛入海水中，使其沉入水中并使其啮入土中，锚产生的抓力与水底固结起来，使得auv可以固留在预定区域内。因此对于在水平面和垂直面上处于虚拟锚泊状态的欠驱动auv受锚链作用的受力分析：
fxm＝fhcos(ψ-le)
fym＝fhsin(ψ-le)
mm＝fhlmsin(ψ-le)
nm＝fvlmcos(θ-ve)
其中，fh为auv受到锚链作用水平分力，ψ为auv的艏向角，le为锚链角，fxm为fh经过分解后的纵向力，fym为fh经过分解后的横向力，mm为fh经过分解后的转艏力矩，lm为锚泊水平分力到作用点的力臂，fv为auv受到锚链作用垂直分力，θ为auv的纵倾角，ve为锚链角，nm为fv经过分解后的俯仰力矩。
(6.5)通过改进的静态悬链线方程对水平分力和垂直分力进行表示并求解：
本文对悬链线方程进行修改，在没有铺底锚链且锚点(曲线最低点)即为目标点，已知悬链线方程为：
悬链曲线张力水平分力和垂直分力方程为：
fh＝q#a
采用改进牛顿法对悬链线方程进行求解a：
(6.6)取锚链张力中的水平分力进行按照auv的随艇坐标系进行分解，分为纵向力fxm和横向力fym以及转艏力矩mm，对锚链张力的垂直分力进行分解，分为俯仰力矩nm：
在水平面控制中，将水平分力fh按照auv的随艇坐标系进行分解，分为纵向力fxm和横向力fym以及转艏力矩mm，对锚链张力的垂直分力进行分解，分为俯仰力矩nm。
(6.7)将fxm、mm和nm分配到auv的执行器，得出主推进器的推力x、转艏力矩n和俯仰力矩m：
纵向力fxm为auv所要产生的纵向推力(主推推力)x,由auv的纵向推进器作用产生；转艏力矩mm为auv所要产生的转艏力矩n,由auv的垂直舵作用产生；俯仰力矩nm为auv所要产生的俯仰力矩m，由auv的水平舵作用产生。
该发明的有益效果在于：
基于q学习的欠驱动auv虚拟锚泊三维镇定控制及实现方法完全不依赖于模型，控制算法结构较为简单，并且能够让欠驱动auv在干扰环境下实现抵抗海流的目标，具有很强的实际应用前景，并且在虚拟锚泊控制的基础上添加了q学习方法，加快了算法的收敛速度，减少其本身的盲目性，当q学习收敛完毕，完成训练后，进行纯q学习，利用q学习本身的随机探索的特性，从而弥补虚拟锚泊控制在接近目标后出现的收敛较慢的问题，两者进行互补。该方法最终可用于欠驱动auv在实际海洋环境中的镇定控制中。
附图说明
图1为欠驱动auv基于q学习的虚拟锚泊控制流程框图；
图2为欠驱动auv虚拟锚泊控制系统流程示意图；
图3为改进牛顿法计算流程图；
图4为欠驱动auv在水平面在该算法作用下从初始位置到目标位置的示意图；
图5为欠驱动auv在垂直面在该算法作用下从初始位置到目标位置的示意图。
具体实施方式
下面结合附图对本发明的具体实施方式作进一步说明：
结合图1，基于q学习的欠驱动auv虚拟锚泊控制方法根据auv的位姿信息、周围环境和所设定的变量等对auv载体进行镇定控制，开始部分以虚拟锚泊控制算法作为q学习的参考，当q学习收敛完毕，完成训练后，进行纯q学习。
其具体步骤如下：
(1)系统中个变量的初始化，完成对状态s、奖罚值r和动作a的初始设定，并建立q表，其中q表为一个多维空间，可通过s和a进行索引并选择q(s,a)值。系统初始化完毕，同时输入控制命令，即目标位置(xb,yb,zb)；
(2)判断q迭代的收敛性，如果收敛则选择根据ε-greedy策略选择q(s,a)最大值的动作值a(如果在该状态下q值全为零，则随机选择动作a)，否则则采用虚拟锚泊控制算法进行运算，输出所需的x、n、m，用该数据组成动作a，并在q表中选择相应的q(s,a)值；
(3)在s状态下，将所选择的动作a进行环境的交互，也就是作用于auv的载体，控制执行机构的输出，到达了s状态所相对的下一时刻的状态s′，评价该动作a的好坏，给定奖罚值r(s,a,s′)；将s′在q表中查找值的最大数
(4)将s′状态的变量赋值给s状态，并通过auv是否抵达目标点，且保持镇定状态，是则开始新一轮的循环，也就是q值的迭代，否则退出这个循环，判断auv的espisode(从开始到结束运行的次数)的次数是否大于200，是则退出系统，q学习完成，否则继续训练，转步骤(2)。
步骤(2)中的虚拟锚泊镇定控制方法是本发明的一个核心，结合图1，其主要步骤为：
(a)根据欠驱动auv的控制输入维度，建立auv的六自由度运动方程：
执行机构产生的力和力矩向量为：τ＝[xnm]t，
auv六自由度动力学方程可以表示：
(b)考虑海洋干扰环境因素，建立auv六自由运动方程：
auv六自由度运动学方程可以表示为:
由于在海洋环境中，auv会受到海流、海水密度和盐度等因素的影响，将环境设定为深广水域，只存在海流影响，且海流平行于大地坐标线，因此相对速度：
ur＝u-uccos(θ)cos(αc-ψ)
vr＝v-ucsin(αc-ψ)
wr＝w-ucsin(θ)cos(αc-ψ)
式中，uc为流速，αc为流向角，ψ为艏向角，θ为纵倾角，对相对速度式进行时间的微分后可得到相对加速度：
(c)根据欠驱动水下机器人在虚拟锚泊状态下受力分析，建立了虚拟锚泊的数学模型：在水平面和垂直面对处于虚拟锚泊状态下的欠驱动auv进行受力分析：
fxm＝fhcos(ψ-le)
fym＝fhsin(ψ-le)
mm＝fhlmsin(ψ-le)
nm＝fvlmcos(θ-ve)
(d)通过改进的静态悬链线方程对水平分力和垂直分力进行表示并求解a：在所设定的条件下通过悬链线方程推导出悬链曲线的水平分力和垂直分力的方程为：
fh＝q·a
采用改进牛顿法对a进行求解：
通过不断迭代，到一定误差后，输出a值，并通过公式得出锚链张力的水平分力和垂直分力；
(e)取锚链张力中的水平分力进行按照auv的随艇坐标系进行分解，分为纵向力fxm和横向力fym以及转艏力矩mm，对锚链张力的垂直分力进行分解，分为俯仰力矩nm；
(f)将fxm、mm和nm分配到auv的执行器，得出主推进器的推力x、转艏力矩n和俯仰力矩m。
整体的欠驱动auv虚拟锚泊镇定控制的控制流程是这样实现的：
如图2所示，首先外部工作人员根据任务的需要通过控制台输入目标位置的控制指令(xd,yd,zd)(期望位置)，控制台将控制命令和所处的状态s发送给auv载体，auv根据自身所携带的设备，包括dvl、磁罗经、惯导、超短基线等，计算出自身与目标点的水平距离d和垂直距离h，以及水平锚链角le和垂直锚链角ve。通过判断水平间距和垂直间距是否在有效范围内，并且auv的控制变量是否处于收敛状态，决定是否运行虚拟锚泊控制算法。首先判断auv是否在目标点的有效方围内(即d<30，h<30的范围)，不在则运行虚拟锚泊的控制算法。在控制算法中，根据所输入的水平间距d和垂直间距h，通过改进牛顿法(具体过程见图3)计算得到a值，再通过悬链线张力方程得到锚泊张力的水平分力fh和垂直分力fv，将水平分力fh和垂直分力fv进行分解，得到纵向力fxm、转艏力矩mm和俯仰力矩nm。纵向力fxm即为纵向推进器产生的推力x，转艏力矩mm即为auv垂直舵产生的转艏力矩n，俯仰力矩nm即为auv水平舵产生的俯仰力矩m。auv执行器工作并作用于auv(虚拟锚泊控制作用于载体auv与q学习部分作用于auv的时间具有一致性)，改变auv的运动，使得auv向目标点运动，并搜索顶流方向，auv接着对位姿进行判定，输出执行器里和力矩，不断调整auv的运动状态，直到auv在目标点得有效范围内，并且控制变量处于收敛，实现区域镇定的目标。
采用改进牛顿法对a进行较为精确的求解的是这样实现的：
由悬链线可知：
代入x＝d，y＝h(d为auv与目标点之间的水平距离，h为auv到目标点得垂直距离)，a视情况从某个数开始迭加，当首次f(an)·f(an+1)＜0求出初值a0，如图3。
详细步骤如下：
(1)将水平间距d和垂直间距h带入到悬链线方程中，求解方程解的初值a0；
(2)将a0带入改进牛顿法的迭代公式中(如下式)，求解精确解(初始k＝0，循环一次k+1)；
(3)计算误差e＝ak+1-ak，若|e|＜ε(本文中ε＝0.00001)结束，输出a＝ak+1，否则继续；
(4)转2)，继续计算；
实际海洋环境下欠驱动水下机器人虚拟锚泊镇定控制方法的基本控制过程(从水平面运动和垂直面运动加以解释)：auv刚开始启动时速度为零(即使有主推推力作用下)，远小于海流的速度，导致auv在海流的作用下被带离起始点，该阶段海流对auv的作用占主导位置；虚拟锚链不断拉扯auv向着目标位置前进；当auv的速度在主推推力的作用下不断增大，抵抗住了海流，在转艏力矩和俯仰力矩的作用下朝着目标点方向运动，不断地转艏和下潜来调整auv的位姿，同时搜索顶流的方向；当auv的艏向角与海流的角度呈180deg差值，纵倾角与海流呈180deg差值时，表示auv开始顶流运动(搜索方向完成)，转艏力矩、艏向角、主推推力、俯仰力矩、纵倾角将会收敛，转艏力矩和俯仰力矩将趋于零，只有主推推力在作用，使得auv朝着目标点前进；在有效作用范围内以及顶流运动的情况下，当auv距离目标点一定距离时，主推推力的海流对auv的作用力达到平衡，auv将停止运动，如图4-5，图中虚线l代表虚拟锚链。
其中，当欠驱动auv完成方向搜索(无转艏力矩作用)，朝着目标前进时，突然改变海流的流向，例如从220deg到180deg，auv运动产生了变化，在本身推力和力矩再加上海流的影响下，偏离了目标点，后在虚拟锚链系统的调解下重新搜索方向，最后找到方向(顶流方向)，在有效作用半径内实现镇定。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。