本发明涉及的是一种欠驱动水下无人航行器的控制方法，特别是涉及一种避免微分爆炸的欠驱动uuv空间轨迹跟踪反步控制方法。
背景技术：
水下无人航行器(unmannedunderwatervehicle,uuv)的空间轨迹跟踪控制能力是实现海洋勘探的保障。所研究的欠驱动系统一方面可以使uuv硬件系统的设计得到相当大程度的简化；同时，当全驱动uuv的部分辅助推进装置出现故障里，uuv变为欠驱动系统，设计的欠驱动uuv轨迹跟踪控制方法依然可以保障任务最大限度地完成，这大大提高了全驱动系统的可靠性。综上，对于欠驱动uuv空间轨迹跟踪控制方法的研究非常重要。
本发明所涉及到的欠驱动uuv在垂向和横向上缺少推进装置，加之uuv空间轨迹为六自由度运动，使得欠驱动uuv模型具有高度耦合性和非线性的特性。主流的控制方法分为两大类：其一是设计解耦的子系统以及线性化处理，但不能保证系统在线性化工作点的全局稳定；其二是通过非线性控制方法进行控制。本发明所涉及的非线性控制方法是基于李雅普诺夫理论的非线性反步控制方法，它能有效实现对欠驱动uuv的直接非线性控制。反步法将一个复杂的非线性系统划分为若干个迭代子系统，通过设计中间虚拟控制函数并逐步计算出中间虚拟控制量的导数，然后逐级的引入后续的子系统，实现对前级子系统的镇定，进而实现整个被控系统的渐进稳定。对中间虚拟控制量的解析求导会导致“微分爆炸”现象和控制器设计复杂问题，不仅会影响控制精度，还会出现执行机构输入饱和问题。针对三维轨迹跟踪控制，文献《spatialpathfollowingforauvsusingadaptiveneuralnetworkcontrollers》(mathematicalproblemsinengineering.2013,第9期.)使用三个神经网络设计控制器，克服参数的不确定性和环境扰动，实现了auv的空间轨迹跟踪，但是神经网络的学习需要时间。文献《position-trackingcontrolofunderactuatedautonomousunderwatervehiclesinthepresenceofunknownoceancurrents》(ietcontroltheoryandapplications.2010,第4卷第11期)通过虚拟速度误差变量的定义简化了传统反步法的计算过程，完成欠驱动auv的平面轨迹跟踪控制。文献《three-dimensionalstraightlinepath-trackingcontrolforunderactuatedunderwatervehicle》(controltheoryandapplications.2013,第30卷第4期)通过合理地选择控制器参数来消除一些非线性项，与传统的反步法设计过程相比，它简化了虚拟控制形式，但仍然需要逐步对虚拟控制量解析求导。文献《three-dimensionalpath-followingcontrolofunderactuatedautonomousunderwatervehiclewithcommandfilteredbackstepping》(自动化学报.2015,第41卷第3期)使用滤波反步法简化了推导过程并消除了高频噪声，实现了欠驱动auv的三维轨迹跟踪控制。在上述反步法中，忽略了时变轨迹对速度和姿态控制的影响。然而，在引入中间虚拟控制变量之后，导数运算将使控制器设计变得非常复杂，影响控制精度，甚至使执行机构产生输入饱和问题。
技术实现要素：
本发明的目的在于提供一种能简化计算量和控制器的复杂性，提高系统的控制性能和鲁棒性，可以实现对欠驱动uuv三维轨迹的精确跟踪控制的避免微分爆炸的uuv轨迹跟踪控制方法。
本发明的目的是这样实现的：
步骤1、初始化，uuv通过所搭载的导航设备和传感器采集数据，获得当前的位置x(t)＝[x(t),y(t),z(t)]t信息、姿态[θ(t),ψ(t)]t信息、速度v(t)＝[u(t),v(t),ω(t),q(t),r(t)]t信息，同时将期望轨迹位置信息给定uuv；
步骤2、利用步骤1中的信息，通过欠驱动uuv的数学模型和轨迹跟踪误差模型，计算出实际轨迹与期望轨迹之间的位置误差和期望姿态，将地面坐标信息转换为船体坐标信息；
步骤3、基于步骤2中计算出的位置误差，采用定义虚拟速度误差变量的方法，将姿态跟踪控制转化为速度控制，计算出纵向速度虚拟控制律ud、纵倾角速度虚拟控制律qd和艏向角速度虚拟控制律rd，并将虚拟期望控制律ud,v1d,w1d用三个生物启发模型的输出量uf,v1f,w1f代替；
步骤4、结合步骤2和步骤3中的数据，通过构造lyapunov函数将位置误差的镇定转移到对速度误差的镇定上，然后分别通过动力模型中的实际控制τu,τr,τq消除速度误差eu,er,eq，通过虚拟控制输入r,q消除虚拟速度误差ev1,ew1；
步骤5、推导避免微分爆炸的欠驱动uuv空间轨迹跟踪控制器，包括纵向控制力矩τu的控制信号、纵倾控制力矩τq和艏摇控制力矩τr的控制信号，计算出外界扰动自适应控制律，实现在外界常值扰动下对欠驱动uuv空间轨迹跟踪控制。
本发明还可以包括：
1、所述欠驱动uuv的数学模型包括运动学模型、动力学模型、位置与姿态误差、三维轨迹跟踪误差模型与期望姿态的表达式分别为:
运动学模型：
动力学模型：
式中：
位置与姿态误差：
三维轨迹跟踪误差模型：
其中，
期望姿态：
2、虚拟速度误差变量v1、w1，纵向速度虚拟控制律ud，纵倾角速度虚拟控制律qd，艏摇角速度虚拟控制律rd和生物启发模型的表达式分别为：
虚拟速度误差变量及其控制律为：
纵向速度虚拟控制律：
ud＝-k1ex/e+vpsinθdsinθ+vtcosθcoseψ
其中，k1,k2,k3为正常数，
艏向角、纵倾角速度虚拟控制律：
其中，k5,k7为正常数；
生物启发模型为：
其中，参数ai,i＝1,2,3是正常数，为模型的输出量的衰减率；正常数bi和di分别为模型的输出量的上限和下限；函数f(·)和g(·)定义为变量a的线性函数f(a)＝max{a,0}和g(a)＝max{-a,0}。
3、lyapunov函数，速度误差和虚拟速度误差的表达式分别为：
lyapunov函数为：
速度误差为：
虚拟速度误差为：
4、lyapunov函数的导数
lyapunov函数的导数为：
其中，
空间轨迹跟踪控制器为：
针对已有技术中存在的问题，本发明提出了一种避免微分爆炸的空间轨迹跟踪反步控制方法。该方法创新性地结合反步控制方法引入生物启发模型，利用生物启发模型的微分输出值，代替反步法控制中对虚拟控制量的实时数值求导，避免了对虚拟控制量的反复求导，简化了计算量和控制器的复杂性，有效的减小了控制输入饱和对系统的影响。本发明的结合生物启发模型的轨迹跟踪反步控制方法，来避免控制器设计时的“微分爆炸”并实现欠驱动uuv的空间轨迹跟踪控制。
本发明方法的有益效果：
1、定义了虚拟速度误差变量，将姿态跟踪控制转化为速度控制，简化了计算过程；
2、利用生物启发模型的微分输出值，代替反步法控制中对虚拟控制量的实时数值求导，避免了对虚拟控制量的反复求导，简化了计算量和控制器的复杂性；
3、提高了系统的控制性能和鲁棒性，满足推进器的推力约束和欠驱动uuv三维轨迹跟踪对位置、速度以及姿态的时间约束要求；
4、在外界常值扰动下可以实现对欠驱动uuv三维轨迹的精确跟踪控制。
附图说明
图1欠驱动水下无人航行器三维轨迹跟踪控制流程图；
图2uuv大地坐标系与船体坐标系；
图3欠驱动uuv三维轨迹跟踪控制系统结构图；
图4欠驱动uuv三维轨迹跟踪效果图；
图5欠驱动uuv三维轨迹跟踪纵向速度及其误差；
图6欠驱动uuv三维轨迹跟踪横向速度及其误差；
图7欠驱动uuv三维轨迹跟踪垂向速度及其误差；
图8欠驱动uuv三维轨迹跟踪纵倾角及其误差；
图9欠驱动uuv三维轨迹跟踪艏向角及其误差；
图10欠驱动uuv三维轨迹跟踪纵倾角速度及其误差；
图11欠驱动uuv三维轨迹跟踪艏向角速度及其误差；
图12欠驱动uuv三维轨迹跟踪控制力及力矩输入。
具体实施方式
下面举例对本发明的避免微分爆炸的uuv轨迹跟踪控制方法做详细描述：
步骤1中uuv根据当前任务，将期望轨迹位置信息给定uuv，并通过所搭载的导航设备和传感器采集数据，获得当前的位置x(t)＝[x(t),y(t),z(t)]t信息、姿态[θ(t),ψ(t)]t信息、速度v(t)＝[u(t),v(t),ω(t),q(t),r(t)]t信息。
然后uuv获取在地面坐标系下的初始位置x(t)＝[x(t),y(t),z(t)]t，uuv初始的艏向角ψ(t)，纵倾角θ(t)，uuv的初始纵向速度ur(t)、横向速度vr(t)和艏摇角速度r(t)，并给定期望轨迹位置xd(t)＝[xd(t),yd(t),zd(t)]t。
步骤2中利用步骤1中的信息，通过欠驱动uuv的数学模型和轨迹跟踪位置误差模型，计算出实际轨迹与期望轨迹之间的位置误差，和期望姿态，将地面坐标信息转换为船体坐标信息。
所涉及的欠驱动uuv的数学模型包括运动学模型和动力学模型、位置与姿态误差、轨迹跟踪误差模型与期望姿态的表达式分别为:
运动学模型：
动力学模型：
式中：
位置与姿态误差：
三维轨迹跟踪误差模型：
其中，
根据给定的期望轨迹位置信息可以得到uuv期望姿态：
所述的将地面坐标信息转换为船体坐标的过程如下：
首先根据期望轨迹获得地面坐标系下的位置误差为：
再经过坐标转换后得到新的船体坐标系下的位置误差为：
至此完成了欠驱动uuv的初始化设置。
欠驱动uuv满足如下的假设：
假设1.1欠驱动uuv的速度和控制输入均有界,即
假设1.2当t＞0时,期望轨迹的变量ud,qd,rd均有界,且它们的导数
假设1.3对
步骤3中按下式分别给出或计算出所定义的虚拟速度误差变量v1、w1，纵向速度虚拟控制律ud，纵倾角速度虚拟控制律qd，艏摇角速度虚拟控制律rd和生物启发模型。
所设计的虚拟速度误差变量及其控制律为：
纵向速度虚拟控制律：
ud＝-k1ex/e+vpsinθdsinθ+vtcosθcoseψ(9)
其中，k1,k2,k3为正常数，
为了避免当艏向角误差eψ＝0时所具有奇异值的问题，设计r的虚拟期望控制律为：
艏向角、纵倾角速度虚拟控制律：
其中，k5,k7为正常数。
生物启发模型为：
其中，参数ai(i＝1,2,3)是正常数，为模型的输出量的衰减率；正常数bi和di分别为模型的输出量的上限和下限；函数f(·)和g(·)定义为变量a的线性函数f(a)＝max{a,0}和g(a)＝max{-a,0}。
步骤4中结合步骤2和步骤3中的数据，通过构造lyapunov函数将位置误差的镇定转移到对速度误差的镇定上：
然后分别通过动力模型中的实际控制τu,τr,τq消除速度误差eu,er,eq，通过虚拟控制输入r,q消除虚拟速度误差
速度误差为：
虚拟速度误差为：
步骤5中推导的避免微分爆炸的欠驱动uuv轨迹跟踪控制器，具体的计算和推到过程如下：
结合式(13)和(14)，对式(12)求导可得：
其中，
根据李雅普诺夫稳定性理论，要使v收敛，需满足条件
其中，
考虑整个控制系统，根据式(12)，设计如下lyapunov函数：
对上式求导可得：
令生物模型中的参数bj＝dj；令ai＝aj+f(id)+g(id)；令bj＝ai，其中(i＝u,v1,w1,r,q；j＝1,2,3,4,5)，则有：
其中：
因此式(16)可以改写为：
即上式又可以变为：
下面利用杨氏不等式
在式(21)中，有：
且满足：
于是，式(21)最终可写为：
同时做出如下定义：
结合式(72)有：
2v1＝||z||2(26)
根据比较原理有：
其中γ＝min{l1,l2,l3,k4,l4,k6,l5,k8,l6,l7,l8,l9,l10}。
再结合(19)可得到：
因此最终可知：
式(27)表明，lyapunov函数v1(t)被界定在(0-δ/2γ)范围内，即跟踪系统误差均是有界的，且收敛在半径为δ/2γ的邻域内，可以通过适当的增大γ使得跟踪系统误差最终收敛到零值附近的一个压缩有界值，即系统误差全局一致最终有界，并且所设计的控制器免去了对ud,qd,rd的求导运算，简化了计算量。
仿真实验验证与分析
为了验证所设计的轨迹跟踪控制器的优点和有效性，对基于虚拟速度代替姿态误差反步法和避免微分爆炸反步法进行了对比仿真实验，并模拟实际工程应用设计了相关下潜空间轨迹，最后对相关结果进行了分析。
其中基于虚拟速度代替姿态误差的反步法控制器设计为：
从式(30)和式(14)的对比中可以看出本发明所设计控制器免去了对ud,qd,rd的求导运算，简化了计算量。
选取期望轨迹为：xd＝t，
uuv初始状态：
x＝-5(m)，y＝10(m)，z＝1(m)，θ＝0(°)，ψ＝0(°)，u＝0(m/s)，v＝0(m/s)，w＝0(m/s)，q＝0(rad/s)，r＝0(rad/s)。
控制器参数：
{k1，k2，k3，k4，k5，k6，k7，k8}＝{1，1.5，1.5，0.3，0.02，0.1，0.01，0.1}。
生物模型参数：
a1＝12,b1＝d1＝10,a2＝13,b2＝d2＝9,a3＝8,b3＝d3＝12,a4＝10,b4＝d4＝12,a5＝10,b5＝d5＝12。
基于matlab的仿真实验结果如图4-图12所示，欠驱动uuv在具有起始误差的情况下，实现了对期望三维空间轨迹的精确跟踪。
图4为欠驱动uuv三维轨迹跟踪的效果图，从图中的对比可以看出基于避免微分爆炸反步法中，由于省去对中间虚拟控制变量的求导，因此其受噪声的干扰较小，在起始点和几个直线拐点处的跟踪效果都好于反步法。
图5-图7分别为欠驱动uuv三维轨迹跟踪x轴、y轴和z轴方向的轨迹及其误差响应曲线，可以看出起始误差为(xe,ye,ze)＝(-5,10,0.4)米；避免微分爆炸反步法在50s内就已经收敛至稳定的状态，且误差都在1米以内；而反步法中到达稳定状态则需要更多的时间，且误差最大超过5米。
图5-图7分别为欠驱动uuv三维轨迹跟踪纵向、横向和垂向速度及其误差变化曲线，反步法的各项速度在误差较大和轨迹拐点时的跳变要比避免微分爆炸反步法要大，且各项速度误差也大于避免微分爆炸反步法；。
图8和图9分别为欠驱动uuv三维轨迹跟踪纵倾角和艏向角及其误差响应曲线，图8中，纵倾角在0°到10°的范围内变化，每次角度的改变避免微分爆炸反步法都会平滑的到达稳态值，而反步法则会出现跳变的现象；图9中，艏向角在0°到25°的范围内变化，可以看出在起始位置误差较大时，反步法出现了短时间内的角度跳变，这样会导致执行机构不能快速的响应，从而引起误差的加大，但避免微分爆炸反步法就很好的克服了该缺点。
图10和图11分别为欠驱动uuv三维轨迹跟踪纵倾角速度和艏向角速度及其误差变化曲线，图10中，避免微分爆炸反步法的纵倾角速度变化快速且平滑，误差仅为反步法的一半；图11中，由于起始的位置误差，反步法中的艏向角速度出现了大且频率较高的跳变。
图12为欠驱动uuv三维轨迹跟踪控制力及力矩输入的变化曲线，直观的反映出所设计控制器实际控制输入的变化情况：反步法中的艏摇控制力矩输入在起始误差较大时出现了抖振的现象，而避免微分爆炸反步法的各项输入却很平滑；反步法中的纵向控制力输入和艏摇控制力矩输入出现了饱和输入的现象。
图4-图12可以看出本发明所设计的控制器在避免传统反步法中微分爆炸现象的同时，还具有控制输入抗饱和控制精度高的特性，实现了在外界常值扰动下对时变三维轨迹的精确跟踪。