本发明属于无人水下机器人运动控制技术领域，尤其涉及一种基于虚拟锚泊的欠驱动AUV三维控制区域镇定控制方法。
背景技术：
欠驱动AUV是指独立控制输入维数少于系统自由度的水下机器人，其最大的特点在于用较少的推进器控制多自由度的运动方向，它是我国实施深海资源开发战略不可或缺的重大技术装备之一。相比较全驱动AUV，欠驱动AUV的优势在于：(1)，欠驱动水下机器人在成本方面有所降低，多推进装置的配置不仅代表了设备成本的提高，也使得对于水下机器人的能源消耗的大幅增加，同时也加重了机器人的重量。(2)在水下机器人的纵向航速不断增加的情况，全驱动水下机器人的侧向推进器和垂向推进器的产生推力的作用将越来越弱，甚至消失。当全驱动水下机器人的摸个推进器出现故障，无法正常工作时，水下机器人将由全驱动变为欠驱动，需要通过欠驱动控制算法对水下机器人进行有效控制，这也是控制系统可靠性的要求。(3)仅仅针对增加系统可靠性而单纯增加推进器在个自由度方向上的布置数量，并不是很符合经济性，通过用较少的执行机构同样能够达到期望的控制要求。这也是研究欠驱动系统最本质的意义。
欠驱动AUV的镇定控制是实现欠驱动AUV控制系统在水下复杂环境的精确定位的关键技术之一，也是欠驱动AUV在水下干扰情况下的水下稳定对接的极为重要保证之一。在欠驱动水下机器人的镇定控制当中，机器人的位置和姿态都要达到收敛状态，其控制难度大大增加，对于理论探索和发展的重要性使得欠驱动水下机器人的镇定控制问题成为国内学者研究的重点。目前对于欠驱动系统都是基于系统运动学模型(以速度为输入)，采用非线性状态反馈的思想对其进行镇定，主要方法有齐次方法,σ变换法和反步法。不足之处在于：(1)这些研究都是基于系统无干扰且为无漂非完整约束系统；(2)在环境干扰强烈的实际海洋环境中稳定性无法保证；(3)在欠驱动AUV的实际应用几乎没有；(4)一些抗干扰能力强的镇定算法也无法完全摆脱模型的依赖。
技术实现要素：
本发明的目的在于公开抗干扰能力强，速度快的一种基于虚拟锚泊的欠驱动AUV三维控制区域镇定控制方法。
本发明的目的是这样实现的：
一种基于虚拟锚泊的欠驱动AUV三维控制区域镇定控制方法，包含如下步骤：
步骤(1)：建立AUV六自由度运动学方程，AUV六自由度动力学方程；
建立欠驱动AUV的六自由度运动方程:
上式中，M为质量矩阵，V为AUV在随艇坐标系的速度向量，C(V)为科氏向心力矩阵，D(V)为流体阻尼矩阵，g(η)为重力和浮力产生的恢复力或力矩矩阵，τ为执行机构产生的力或力矩。
在三维空间，执行机构产生的力或力矩向量可表示为：
τ＝[X N M]T；
AUV六自由度运动学方程表示为:
上式中，ν为AUV的横向速度。x是x轴方向的坐标，y是y轴方向的坐标，z是z轴方向的坐标，ψ为艏向角，θ为纵倾角，φ是前向角，u是AUV的纵向速度，v是AUV的横向速度，w是AUV的前向速度。
AUV六自由度动力学方程表示为:
上式中，p为纵向角加速度，q为横向角加速度，r为前向角加速度。
步骤(2)：建立海洋环境影响下的欠驱动AUV的相对速度和AUV的相对加速度的方程：
在海洋环境中，AUV所受到的干扰力较为复杂以及多变，例如海流本身的影响，海水密度、盐度和温度的影响等。本文假定水域为深广水域，只存在海流的影响。
设海流平行于大地坐标系的水平面，海流流速为恒定值，因此AUV的相对速度：
AUV的纵向相对速度：ur＝u-UC cos(θ)cos(αC-ψ)
AUV的横向相对速度：vr＝v-UC sin(αC-ψ)；
AUV的前向相对速度：wr＝w-UC sin(θ)cos(αC-ψ)
上式中，Uc为流速，αc为流向角，ψ为艏向角，θ为纵倾角，u是AUV的纵向速度，v是AUV的横向速度，w是AUV的前向速度；
对相对速度式进行时间的微分后可得到AUV的相对加速度：
AUV的纵向相对加速度：
AUV的横向相对加速度：
AUV的前向相对加速度：
步骤(3)：对虚拟锚泊状态下欠驱动水下机器人进行受力分析，得到纵向力Fxm，横向力Fym，转艏力矩Mm：
在一定深度的海中，AUV将锚链连接的锚抛入海水中，使其沉入水中并使其啮入土中，锚产生的抓力与水底固结起来，使得AUV可以固留在预定区域内。因此对于在水平面上处于虚拟锚泊状态的欠驱动AUV受锚链作用的受力分析：
上式中，Fh为AUV受到锚链作用水平分力，ψ为AUV的艏向角，Le为锚链角，Fxm为纵向力，Fym为横向力，Mm为转艏力矩，lm为锚泊水平分力到作用点的力臂。
对于在水平面上处于虚拟锚泊状态的欠驱动AUV受锚链作用的受力分析：
Nm＝Fvlm cos(θ-Ve)；
上式中，Fv为AUV受到锚链作用垂直分力，θ为AUV的纵倾角，Ve为锚链角，Nm为Fv经过分解后的俯仰力矩。
步骤(4)：通过改进的静态悬链线方程表示悬链曲线张力水平分力方程和悬链曲线张力垂直分力方程：
在没有铺底锚链时，锚点即为目标点，改进的静态悬链线方程为：
悬链曲线张力水平分力方程为：
Fh＝q·a；
悬链曲线张力垂直分力方程为：
步骤(5)：采用改进牛顿法求解水下机器人所受到的悬链曲线张力水平分力和悬链曲线张力垂直分力：
采用改进牛顿法对a进行求解：
第一步：将AUV与目标点之间的水平距离d和AUV到目标点的垂直距离h代入到悬链线方程中，求解方程解的初值a0：
将x＝a代入改进的静态悬链线方程，且令则：
代入x＝d，y＝h，d为AUV与目标点之间的水平距离，h为AUV到目标点的垂直距离，a视情况从某个数开始迭加，当首次f(an)·f(an+1)＜0时求出初值a0；
第二步：将a0带入改进牛顿法的迭代公式：
k为非负整数；
初始化令k＝0，每循环一次，令k＝k+1；
第三步：计算误差e＝ak+1-ak；
第四步：判断是否|e|＜ε，ε＝0.00001，若是则结束并输出a＝ak+1，否则转到第二步；
第五步：将a代入悬链曲线张力水平分力方程求得悬链曲线张力水平分力，将a代入悬链曲线张力垂直分力方程求解悬链曲线张力垂直分力。
步骤(6)：对悬链曲线张力水平分力和悬链曲线张力垂直分力进行分解：
在水平面控制中，将悬链曲线张力水平分力按照AUV的随艇坐标系进行分解，分为纵向力Fxm和横向力Fym以及转艏力矩Mm；对悬链曲线张力垂直分力进行分解，分为俯仰力矩Nm。
步骤(7)：将纵向力Fxm、转艏力矩Mm和俯仰力矩Nm分配到AUV的执行器，得出主推进器的推力X等于纵向力Fxm、AUV垂直舵产生的转艏力矩等于转艏力矩Mm，AUV垂直舵产生的俯仰力矩等于俯仰力矩Nm。
本发明的有益效果为：
本发明不依赖于模型，控制算法结构较为简单，并且能够让欠驱动AUV在强干扰环境下实现抵抗海流的目标，抗干扰能力强，具有很强的实际应用前景。
附图说明
图1是欠驱动AUV虚拟锚泊控制系统流程示意图；
图2是欠驱动AUV水平面运动控制系统图；
图3是改进牛顿法计算流程图；
图4是欠驱动AUV在干扰环境下的初始状态示意图；
图5是欠驱动AUV在干扰环境下的开始转艏、下潜，准备向目标点运动并寻找顶流方向示意图；
图6是欠驱动AUV在干扰环境下向目标点运动，并在该过程中不断调整自身位姿，寻找顶流方向示意图；
图7是欠驱动AUV在干扰环境下处于镇定状态(控制变量收敛)，并在有效范围内寻找到了顶流方向示意图；
图8是欠驱动AUV的基于虚拟锚泊的三维空间区域镇定控制流程图；
具体实施方式
下面结合附图来进一步描述本发明：
一种基于虚拟锚泊的欠驱动AUV三维控制区域镇定控制方法，包含如下步骤：
步骤(1)：建立AUV六自由度运动学方程，AUV六自由度动力学方程；
建立欠驱动AUV的六自由度运动方程:
上式中，M为质量矩阵，V为AUV在随艇坐标系的速度向量，C(V)为科氏向心力矩阵，D(V)为流体阻尼矩阵，g(η)为重力和浮力产生的恢复力或力矩矩阵，τ为执行机构产生的力或力矩。
在三维空间，执行机构产生的力或力矩向量可表示为：
τ＝[X N M]T；
AUV六自由度运动学方程表示为:
上式中，ν为AUV的横向速度。x是x轴方向的坐标，y是y轴方向的坐标，z是z轴方向的坐标，ψ为艏向角，θ为纵倾角，φ是前向角，u是AUV的纵向速度，v是AUV的横向速度，w是AUV的前向速度。
AUV六自由度动力学方程表示为:
上式中，p为纵向角加速度，q为横向角加速度，r为前向角加速度。
步骤(2)：建立海洋环境影响下的欠驱动AUV的相对速度和AUV的相对加速度的方程：
在海洋环境中，AUV所受到的干扰力较为复杂以及多变，例如海流本身的影响，海水密度、盐度和温度的影响等。本文假定水域为深广水域，只存在海流的影响。
设海流平行于大地坐标系的水平面，海流流速为恒定值，因此AUV的相对速度：
AUV的纵向相对速度：ur＝u-UC cos(θ)cos(αC-ψ)
AUV的横向相对速度：vr＝v-UC sin(αC-ψ)；
AUV的前向相对速度：wr＝w-UC sin(θ)cos(αC-ψ)
上式中，Uc为流速，αc为流向角，ψ为艏向角，θ为纵倾角，u是AUV的纵向速度，v是AUV的横向速度，w是AUV的前向速度；
对相对速度式进行时间的微分后可得到AUV的相对加速度：
AUV的纵向相对加速度：
AUV的横向相对加速度：
AUV的前向相对加速度：
步骤(3)：对虚拟锚泊状态下欠驱动水下机器人进行受力分析，得到纵向力Fxm，横向力Fym，转艏力矩Mm：
在一定深度的海中，AUV将锚链连接的锚抛入海水中，使其沉入水中并使其啮入土中，锚产生的抓力与水底固结起来，使得AUV可以固留在预定区域内。因此对于在水平面上处于虚拟锚泊状态的欠驱动AUV受锚链作用的受力分析：
上式中，Fh为AUV受到锚链作用水平分力，ψ为AUV的艏向角，Le为锚链角，Fxm为纵向力，Fym为横向力，Mm为转艏力矩，lm为锚泊水平分力到作用点的力臂。
对于在水平面上处于虚拟锚泊状态的欠驱动AUV受锚链作用的受力分析：
Nm＝Fvlm cos(θ-Ve)；
上式中，Fv为AUV受到锚链作用垂直分力，θ为AUV的纵倾角，Ve为锚链角，Nm为Fv经过分解后的俯仰力矩。
步骤(4)：通过改进的静态悬链线方程表示悬链曲线张力水平分力方程和悬链曲线张力垂直分力方程：
在没有铺底锚链时，锚点即为目标点，改进的静态悬链线方程为：
悬链曲线张力水平分力方程为：
Fh＝q·a；
悬链曲线张力垂直分力方程为：
步骤(5)：采用改进牛顿法求解水下机器人所受到的悬链曲线张力水平分力和悬链曲线张力垂直分力：
采用改进牛顿法对a进行求解：
第一步：将AUV与目标点之间的水平距离d和AUV到目标点的垂直距离h代入到悬链线方程中，求解方程解的初值a0：
将x＝a代入改进的静态悬链线方程，且令则：
代入x＝d，y＝h，d为AUV与目标点之间的水平距离，h为AUV到目标点的垂直距离，a视情况从某个数开始迭加，当首次f(an)·f(an+1)＜0时求出初值a0；
第二步：将a0带入改进牛顿法的迭代公式：
k为非负整数；
初始化令k＝0，每循环一次，令k＝k+1；
第三步：计算误差e＝ak+1-ak；
第四步：判断是否|e|＜ε，ε＝0.00001，若是则结束并输出a＝ak+1，否则转到第二步；
第五步：将a代入悬链曲线张力水平分力方程求得悬链曲线张力水平分力，将a代入悬链曲线张力垂直分力方程求解悬链曲线张力垂直分力。
步骤(6)：对悬链曲线张力水平分力和悬链曲线张力垂直分力进行分解：
在水平面控制中，将悬链曲线张力水平分力按照AUV的随艇坐标系进行分解，分为纵向力Fxm和横向力Fym以及转艏力矩Mm；对悬链曲线张力垂直分力进行分解，分为俯仰力矩Nm。
步骤(7)：将纵向力Fxm、转艏力矩Mm和俯仰力矩Nm分配到AUV的执行器，得出主推进器的推力X等于纵向力Fxm、AUV垂直舵产生的转艏力矩等于转艏力矩Mm，AUV垂直舵产生的俯仰力矩等于俯仰力矩Nm。
在实际使用过程中，在目标位置处抛下一只锚，锚通过锚链与AUV连接。在海中，当AUV受到环境力作用离开目标位置时，锚链产生张力，抵抗环境力的作用，从而使AUV保持在目标位置附近。虚拟锚链张力由AUV推力器提供，虚拟锚链最大张力即为AUV能够输出的最大控制力。
如图2所示，欠驱动AUV的动力学方程，可输出随艇坐标系中的AUV六自由度运动状态，包括速度向量[u v r p q r]T，然后将动力学方程所输出的速度向量[u v r p q r]T经过积分处理后输入到AUV六自由度运动学方程，经过积分计算，将其转化为大地坐标系下的AUV六自由度运动变量，得到AUV的位姿向量将位姿向量中AUV位置向量[ξηζ]T输入到锚链角和间距计算模块中，对水平锚链角Le、水平间距d和垂直锚链角Ve、垂直间距h进行计算，与姿态角向量一起输入到欠驱动AUV虚拟锚泊区域镇定控制器当中，求取出AUV各执行器的数值，包括主推推力X、转艏力矩N和俯仰力矩M。接着执行器产生的X、N和M输入到欠驱动AUV的动力学方程中，实现控制AUV运动和位姿按照期望命令进行变化。这是一次控制过程，通过这一次次控制过程，对欠驱动AUV进行有效控制，最终实现镇定目标。欠驱动AUV虚拟锚泊区域镇定控制器的控制流程是是这样实现的：根据所输入的水平锚链角Le和水平间距d，垂直锚链角Ve和垂直间距h，计算出a的初值a0，然后采用改进牛顿法进行对a的精确求解，不断的对a进行迭代，直到求解出符合要求(|e|＜ε)的a值，如图3，计算出锚链张力的水平分力Fh和垂直分力Fv，对水平分力Fh按照欠驱动AUV的随艇坐标进行分解，分为纵向力Fxm和横向力Fym以及转艏力矩Mm，将垂直分力分为俯仰力矩Nm其中，纵向力Fxm为AUV所要产生的纵向推力，转艏力矩Mm为AUV所要产生的转艏力矩，俯仰力矩Nm为AUV所要产生的俯仰力矩，如图1。实际海洋环境下基于虚拟锚泊的欠驱动水下机器人三维空间区域镇定控制方法的基本控制过程(从水平面运动和垂直面运动加以解释)：AUV刚开始启动时速度为零，远小于海流的速度，导致AUV在海流的作用下被带离起始点，该阶段海流对AUV的作用占主导位置；当AUV的速度在主推推力的作用下不断增大，抵抗住了海流，在转艏力矩和俯仰力矩的作用下朝着目标点方向运动，不断地转艏和下潜来调整AUV的位姿，同时搜索顶流的方向；当AUV的艏向角与海流的角度呈180deg差值，纵倾角与海流呈180deg差值时，表示AUV开始顶流运动，转艏力矩、艏向角、主推推力、俯仰力矩、纵倾角将会收敛，转艏力矩和俯仰力矩将趋于零，只有主推推力在作用，使得AUV朝着目标点前进；在有效作用范围内以及顶流运动的情况下，当AUV距离目标点一定距离时，主推推力的海流对AUV的作用力达到平衡，AUV将停止运动，如图4，图5，图6，图7。其中，当欠驱动AUV完成方向搜索无转艏力矩作用，朝着目标前进时，突然改变海流的流向，例如从220deg到180deg，AUV运动产生了变化，在本身推力和力矩再加上海流的影响下，偏离了目标点，后在虚拟锚链系统的调解下重新搜索方向，最后找到方向(顶流方向)，在有效作用半径内实现镇定。整体的基于虚拟锚泊的欠驱动AUV三维控制区域镇定控制的控制流程是这样实现的：首先外部工作人员根据任务的需要通过控制台输入目标位置的控制指令(ξd ηd ζd)期望位置，控制台将控制命令发送给AUV载体，AUV根据自身所携带的设备，包括DVL、磁罗经、惯导、超短基线等，计算出自身与目标点的水平距离d和垂直距离h，以及水平锚链角Le和垂直锚链角Ve。通过判断水平间距和垂直间距是否在有效范围内，并且AUV的控制变量是否处于收敛状态，决定是否运行虚拟锚泊控制算法。首先判断AUV是否在目标点的有效方围内(即d<30，h<30的范围)，不在则运行虚拟锚泊的控制算法。在控制算法中，根据所输入的水平间距d和垂直间距h，通过改进牛顿法计算得到a值，再通过悬链线张力方程得到锚泊张力的水平分力Fh和垂直分力Fv，将水平分力Fh和垂直分力Fv进行分解，得到纵向力Fxm、转艏力矩Mm和俯仰力矩Nm。纵向力Fxm即为纵向推进器产生的推力X，转艏力矩Mm即为AUV垂直舵产生的转艏力矩N，俯仰力矩Nm即为AUV水平舵产生的俯仰力矩M。AUV执行器工作并作用于AUV，改变AUV的运动，使得AUV向目标点运动，并搜索顶流方向，AUV接着对位姿进行判定，输出执行器里和力矩，不断调整AUV的运动状态，直到AUV在目标点得有效范围内，并且控制变量处于收敛，实现区域镇定的目标，如图8。
本发明不依赖于模型，控制算法结构较为简单，并且能够让欠驱动AUV在强干扰环境下实现抵抗海流的目标，抗干扰能力强，具有很强的实际应用前景。
以上所述并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。