一种补偿UUV运动学和动力学干扰的空间轨迹跟踪控制方法与流程

本发明属于无人水下航行器自主控制领域，尤其涉及一种补偿uuv运动学和动力学干扰的空间轨迹跟踪控制方法。

背景技术：

水下无人航行器(unmannedunderwatervehicle，uuv)的出现，为进行海洋探索和开发提供了非常重要的手段，已经成为目前公认的最有效的海洋开发工具。uuv是一种自带能源、自主导航与控制、能够不受监控的自主的执行众多的海洋使命的水下无人航行器。

欠驱动自主水下航行器的反馈控制在近几年吸引力了大批控制和海洋工程领域人员的关注。与全驱动uuv运动控制相比，欠驱动uuv控制器设计过程中主要考虑的问题是uuv独立的执行机构数量少于自由度的个数。这种结构增加了非线性控制器设计的难度。本发明就是针对欠驱动uuv进行轨迹跟踪控制。

在对uuv进行轨迹跟踪控制过程中，一般我们会先对轨迹进行规划，当uuv沿着期望轨迹航行时，由于外界及uuv自身条件的影响，使得uuv的实际运动轨迹与期望的运动轨迹存在着偏差，于是我们需要进行合理的控制，使得uuv能够更好的沿着期望轨迹航行，完成回收对接。现有技术中曹永辉、石秀华《水下航行器轨迹跟踪控制与仿真》针对uuv的水平面运动提出了一种基于滑模控制的将横向轨迹误差法和视线法组合的轨迹跟踪控制方法。首先分别建立横向轨迹误差法的滑模控制器和视线法的滑模控制器，当航向角偏差较大时采用视线法，当航向偏差小于一个定值时采用横向轨迹误差法。高剑、徐德民、严卫生等人《自主水下航行器回坞路径规划与跟踪控制》同样针对uuv的水平面运动，提出了一包含位置跟踪和航向角跟踪的级联系统的轨迹跟踪控制方法。根据反步法设计位置跟踪控制器，并且保证了轨迹跟踪误差控制全局一致渐近稳定性。但是现有技术中大多是研究uuv的水面轨迹跟踪控制问题，对于三维空间的轨迹跟踪问题一般也是基于反步法设计，并且数学复杂性高。

技术实现要素：

本发明的目的是提供一种控制精度高的，能够补偿uuv运动学和动力学干扰的空间轨迹跟踪控制方法。

本发明是通过以下方案实现的：

一种补偿uuv运动学和动力学干扰的空间轨迹跟踪控制方法，包括以下几个步骤，

步骤一：给定平滑有界的期望轨迹yd；

步骤二：通过uuv搭载的惯性导航仪、深度计、姿态传感器和多普勒计程仪采集uuv当前时刻的位姿信息和速度信息；

其中，位姿信息η＝[x,y,z,θ,ψ]^t，包括纵向位移x、横向位移y、垂向位移z、纵摇角θ和艏摇角ψ；速度信息包括直接驱动速度矢量υ＝[u,q,r]^t和间接驱动速度矢量w＝[v,w]^t，包括纵向速度u、横向速度v、垂向速度w、纵摇角速度q和艏摇角速度r；

步骤三：选取uuv前端的虚拟控制点的位置；

步骤四：建立轨迹跟踪误差e，对轨迹跟踪误差e进行滤波处理，得到滤波后的轨迹跟踪误差ef；

步骤五：利用具有l个节点的两层rbf神经网络估计uuv运动学和动力学干扰项f(α)，得到uuv的运动学和动力学干扰项估计值利用滤波后的轨迹跟踪误差ef得到神经网络自适应控制律

步骤六：根据神经网络自适应控制律得到轨迹跟踪控制信号τan，进一步得到执行机构控制信号τa＝[τu,τq,τr]^t，其中τu是由uuv主推产生的纵向推力，τq为纵倾控制力矩，τr为转艏控制力矩；

步骤七：判断uuv前端的虚拟控制点的位置是否到达给定期望轨迹的终点，如果是，则结束运行；否则返回步骤二。

本发明一种补偿uuv运动学和动力学干扰的空间轨迹跟踪控制方法，还可以包括：

1、所述的uuv前端的虚拟控制点的位置为，

其中，是恒定正常数，表示虚拟控制点pl和uuv质心com之间的距离。

2、所述的轨迹跟踪误差e为：

e＝y-yd，

对轨迹跟踪误差e进行滤波处理，得到滤波后的轨迹跟踪误差ef：

其中，q1为增益矩阵，k1和k2为可调系数。

3、所述的神经网络自适应控制律的求取过程为，

(1)利用具有l个节点的两层rbf神经网络，得到估计后的uuv的运动学和动力学干扰项

其中，α＝[η,υ,w,τan]^t，w是神经网络的可调参数矩阵，ξ(α)＝[ξ1(α),，..,ξl(α)^t]是神经网络基函数向量，ξi(α)为高斯函数：

其中，μi＝[μi1,μi2,...μin]^t和βi分别是高斯函数的中心和宽度，向量α和w分别属于紧集u和ω，其中，m1和m2是参数；

ρ^*＝ε(α)+ρ，ε(α)为神经网络的误差，误差||ε(α)||≤bε，bε是给定阈值；干扰矩阵ρ有界||ρ||≤bρ，bρ为给定阈值；τa＝[τu,τq,τr]^t为执行机构控制信号，惯性矩阵是惯性矩阵m1(η)的估计值，m11,m55,m66是uuv的质量和惯性参数；

(2)利用滤波后的轨迹跟踪误差ef得到神经网络自适应控制律

w和ρm的更新规则为：

其中，阈值ρm＝bε+bρ，γw和γρ为自适应增益，σw和σρ为正常数，kp为增益。

4、所述的轨迹跟踪控制信号τan为

其中，

其中，雅克比矩阵

5、所述的uuv运动学和动力学干扰包括：测量仪器不确定性干扰，模型参数不确定性干扰，海流与海浪干扰，载荷动力学干扰。

6、所述uuv前端的虚拟控制点的位置与执行机构控制信号τa＝[τu,τq,τr]^t的关系为，

式中，为运动学和动力学干扰项的估计值。

7、所述神经网络的可调参数矩阵w的最优矩阵为：

本发明具有如下有益效果：

本发明一种补偿uuv运动学和动力学干扰的空间轨迹跟踪控制方法，能够成功地控制uuv跟踪上期望的轨迹，跟踪偏差是收敛到零点附近的一个邻域内的，而且所有的闭环信号是有界的。神经网络的突出优点是表现出平滑的响应。本发明考虑了uuv轨迹跟踪控制过程中测量仪器不确定性干扰，模型参数不确定性干扰，海流与海浪干扰及载荷动力学干扰对uuv控制精度的影响，采用神经网络逼近uuv运动学与动力学扰动，能够有效补偿uuv的运动学与动力学干扰，提高轨迹跟踪控制精度。本发明对轨迹跟踪误差进行有效滤波，采用加权动态滤波方式，有效效地减少了执行机构饱和的风险。

说明书附图

图1为本发明uuv三维空间自适应轨迹跟踪控制方法流程图；

图2为uuv前端的虚拟控制点的位置示意图；

图3为uuv空间轨迹跟踪结果：图3(a)xyz跟踪结果；图3(b)xy跟踪结果；图3(c)yz跟踪结果。

图4为本发明滤波后的轨迹跟踪误差ef(t)滤波效果图，图4(a)为本发明滤波后信号和原信号的对比图，图4(b)为本发明滤波后轨迹跟踪误差ef(t)效果图。

具体实施方式

下面将结合附图对本发明做进一步详细说明。

本发明一种uuv三维空间自适应轨迹跟踪控制方法，如图1所示，包括以下几个步骤：

步骤一：给定平滑有界的期望轨迹yd(t)；

本发明的控制目标是：对于存在运动学与动力学干扰的欠驱动uuv设计一个跟踪控制律，并且使得跟踪误差在三维空间中是一致最终有界的。

期望轨迹yd(t)及其都是有界的，supt≥0||yd(t)||＜bdp，其中bdp、bdv和bda边界常数。

步骤二：通过uuv搭载的传感器采集当前的位姿信息和速度信息，位姿信息η＝[x,y,z,θ，ψ,]^t包括大地坐标系下的纵荡x、横荡y、垂荡z、纵摇角θ和艏摇角ψ，速度信息包括船体坐标系下的纵荡u、横荡v、垂荡w、纵摇q和艏摇速度r，分别记为直接驱动速度矢量υ＝[u,q,r]^t和间接驱动速度矢量w＝[v,w]^t。

欠驱动uuv的5自由度数学模型如下：

其中，τu，τq，τr是由执行机构产生的信号，τwu(t)，τwv(t)，τww(t)，τwq(t)，是有界的时变扰动。mii是uuv的质量和惯性参数，dii是阻尼系数，i＝1，2,3,5,6。ρ为水密度，g为重力加速度，▽为水的容积，gml为纵向稳心高。

运动学模型(1)可以如下表示：

其中，υ＝[u,q,r]^t和w＝[v,w]^t是重新定义的速度矢量，前者是直接驱动的，后者是不能直接驱动的。s(η)和分别是运动学矩阵和运动学干扰矢量阵，如下所示：

航行器直接驱动部分的动力学模型：

其中τa＝[τu,τq,τr]^t为控制输入向量。为惯性矩阵，是科氏向心力，是水动力阻尼矩阵，是重力向量，是由海浪、海流引起的扰动。

航行器不能直接驱动部分的动力学模型：

其中，

其中，是科氏向心力，是水动力阻尼矩阵，是由海浪、海流引起的扰动。

说明：1)航行器的横荡和垂荡速度是被动有界的supt≥0||w(t)||≤bw，bw为边界常数。

2)扰动向量和的界限是：||τw1(t)||≤λw11，其中λw11和是正常数。

3)为了避免稳定性分析中出现奇点，定义纵倾角的界限是：|θ(t)|≤θmax＜π/2。

步骤三：选取uuv前端的虚拟控制点的位置；

因为本发明主要研究uuv三维点跟踪控制，x,y,z方向的坐标应该选大地坐标系下的。一个简化的选择是质心的位置，记为com，如图2所示。然而，这种选择的优点是：(1)基于前面所提出的uuv模型，该控制器不会呈现纵摇和艏摇方向的扰动。(2)质心位置不会受纵摇和艏摇控制输入的影响。因此，引入以下变量变换，包括在各方向结合了uuv动力的所有自由度和所有控制输入。

选取uuv前端的虚拟控制点的位置：

其中，是恒定正常数，表示虚拟控制点pl和uuv质心com之间的距离，如图2所示。

根据uuv当前的位姿信息和速度信息，构建uuv前端虚拟控制点与执行机构控制信号τa＝[τu,τq,τr]^t的关系，即uuv的输入输出模型，具体过程为：

(1)uuv模型状态空间表示

将uuv运动学模型式(3)和动力学模型式(5)结合得到状态空间表示形式：

其中：

状态变量将在下面进行简化状态空间模型(9)，进行控制器设计，稳定性分析。状态反馈控制是：

其中，τa是uuv的控制输入，τan是一个新的控制输入，是m1(η)的近似值。将公式(11)代入公式(9)中，uuv状态空间模型改写为：

其中，为了简化，f(x)和g(x)表示系统的平滑向量场，q(x)表示运动学和动力学扰动。

(2)uuv的输入输出模型

通过uuv运动学模型和uuv输出方程，可以得出：

其中，lfh(x)＝▽hf，lgh(x)＝▽hg，lqh(x)＝▽hq，表示h分别沿着矢量f,g,q方向的导数。▽h是h的梯度(导数)，jδ(η，w)是输入-输出模型对应运动学模型扰动的部分。

其中，雅克比矩阵与公式(4)中运动学矩阵s(η)相反，j(η)对于所有的θ，没有奇异点。因为公式(13)并不是全部与执行机构控制输入相关，因此再一次变形得：

其中，可得ρ≤bρ。

步骤四：根据给定的期望轨迹yd建立轨迹跟踪误差e＝y-yd。

建立轨迹跟踪误差e：

e＝y-yd，

对轨迹跟踪误差e进行滤波处理，得到滤波后的轨迹跟踪误差：

其中，

tanh(·)为双曲正切函数，(xd,yd,zd)为期望轨迹yd的坐标,q1为增益矩阵，k1和k2为可调系数，

给出一个平滑有界的期望轨迹期望轨迹由开环的运动规划器给出。

其中，期望轨迹状态向量状态变量ηd为期望轨迹位姿信息，υd为期望轨迹速度信息，τand为期望轨迹的控制输入。

步骤五：利用具有l个节点的两层rbf神经网络估计uuv运动学和动力学干扰项f(α)，得到估计后的uuv的运动学和动力学干扰项利用滤波后的轨迹跟踪误差ef得到神经网络自适应控制律能够补偿估计后的uuv运动学与动力学干扰

(1)利用具有l个节点的两层rbf神经网络，得到估计后的uuv的运动学和动力学干扰项

其中，α＝[η,υ,w,τan]^t，w是神经网络的可调参数矩阵，ξ(α)＝[ξ1(α),...,ξl(α)]^t是神经网络基函数向量，ξi(α)为高斯函数：

其中，μi＝[μi1,μi2,...μin]^t和βi分别是高斯函数的中心和宽度，向量α和w分别属于紧集u和ω，其中，m1和m2是参数；

神经网络的可调参数矩阵w的最优矩阵为：

ρ^*＝ε(α)+ρ，ε(α)为神经网络的误差，误差||ε(α)||≤bε，bε是给定阈值；干扰矩阵ρ有界||ρ||≤bρ，bρ为给定阈值；τa＝[τu,τq,τr]^t为执行机构控制信号，惯性矩阵是惯性矩阵m1(η)的估计值，m11,m55,m66是uuv的质量和惯性参数；

考虑uuv实际系统，包括各种传感器的数据采集，空间机动所处介质物理属性，同时结合uuv的五自由度数学模型，uuv运动过程中所受的运动学与动力学干扰包括：测量仪器不确定性干扰，模型参数不确定性干扰，海流与海浪干扰及载荷动力学干扰。

测量仪器不确定性干扰主要指仪器量测中所受噪声干扰，而实际海洋环境复杂多变，加之本身元器件的工艺水平限制，量测系统不可避免将受到各种噪声的污染。如多普勒计程仪利用多普勒效应原理进行测量底跟踪速度或者对流速度，如果测量过程受水中散射体的影响，将给测量得到的速度量引入随机误差。这种情况下，该速度信号属于非平稳信号，其频率随时间而变化。

模型参数不确定性干扰主要指建立uuv动力学模型时，认为水动力系数不变，为定值，实际中水动力系数会随着运动状态的变化产生微小的摄动，此时相关的水动力项在计算值上应附加一个偏移量，同比例缩尺度模型试验研究表明在节航速范围内该偏移量不占主导地位，可视为扰动。

uuv在航速不高的近海面航行时受海流、海浪影响较大，流体流速是空间和时间的一个复杂函数，随水域、深度和时间的变化而改变，将控制器的抗流能力作为运动控制设计的一项指标。

uuv在航行中，所附在的载荷结构或者形状发生变化时，对uuv质量分布会产生影响。

(2)利用滤波后的轨迹跟踪误差ef进行控制器设计

得到神经网络自适应控制律

w和ρm的更新规则为：

其中，阈值ρm＝bε+bρ，γw和γρ为自适应增益，σw和σρ为正常数，kp为增益；

步骤六：根据神经网络自适应控制律得到轨迹跟踪控制信号τan，

进一步得到执行机构控制信号τa＝[τu,τq,τr]^t，其中τu是有uuv主推产生的纵向推力，τq为纵倾控制力矩，τr为转艏控制力矩；

得到下述闭环动力误差方程：

其中，是宽度估计误差。

步骤七：判断uuv前端的虚拟控制点的位置是否到达给定期望轨迹的终点，如果是，则结束运行；否则返回步骤二。

在本发明中，λmax(·)(λmin(·))定义为矩阵最大的(最小的)特征值。定义为向量的欧几里得范数。矩阵a的诱导范数为矩阵a的frobenius范数为：其中tr{·}表示求迹运算。矩阵in表示n维单位阵。还定义了以下符号tanh(x)＝[tanh(x1),...,tanh(xn)]^t，sech²(x)＝diag[sech²(x1),...,sech²(xn)]^t。其中diag[·]表示对角阵，tanh(·)为双曲正切函数，sech(·)＝1/cosh(·)为双曲正割函数，cosh(·)为双曲余弦函数。

本发明中uuv运动学和动力学干扰项f(α)＝d(α)+ρ，利用神经网络的逼近性质，可以近似未知函数d(α)：

神经网络的误差：

此处，w^*是w的估计值，定义估计误差非线性不确定性d改写为：d(x)＝w^*ξ(x)+ε使得||ε||≤bε。因此，公式(16)可以改写为：

其中，ρ^*(t)＝ε(t)+ρ(t)的界限其中

下面给出本发明的一个实验验证本发明方法的有效性：

利用randn(.)函数在uuv输出的测量中加入了高斯白噪声来建模位置测量系统。所有的仿真都利用时间步长是20ms的欧拉解算法来完成。uuv配置有螺旋桨来提供纵向力、纵倾和偏航力矩。对于采用的实际uuv的模型，用到的模型参数为：

m11＝25kg,m22＝17.5kg,m33＝30kg,m55＝22.5kgm²,m66＝15kgm²,d11＝30kgs^-1,d22＝30kgs^-1,d33＝30kgs^-1,d55＝20kgm²s^-1,d66＝20kgm²s^-1，ρg▽gml＝5。不过，在实际中来确定这些参数的实际值是非常困难的，因此uuv是具有参数不确定性的。另外，通过以下方式加入环境干扰：

τw1(t)＝0.5sgn(υ)+2[sin(0.1t),sin(0.1t),sin(0.1t)]^t

控制参数的选择如下：kp＝10i3,q＝10i3,γp＝1,σp＝0.005,εt＝1。设定控制信号是限值为|τai|≤100nm,i＝1,2,，3来建模执行机构的饱和特性。实验中uuv的初始位姿为x(0)＝5m,y(0)＝5m,z(0)＝0m,θ(0)＝0rad,ψ(0)＝0rad。uuv的参考轨迹yd(t)通过一个开环的运动规划器产生。参考轨迹的初始位姿和控制信号设置为

x(0)＝5m,y(0)＝5m,z(0)＝0m,θd(0)＝0rad,ψd(0)＝0rad,τad＝[7.5,1.5,3]^tnm。

此外，采用一个具有6个隐含节点(l＝6)、三个输出节点的rbf神经网络来建模和逼近uuv的运动学与动力学干扰。rbf神经网络的参数为γw＝10,σw＝0.04,μi＝[-3,-2,-1,1,2,3]^t,βi＝10。w是一个初始值为0的3×6的矩阵。图3给出了跟踪的结果，包括uuv和参考轨迹的xyz、xy和yz三个轨迹图。从图中可以看出，uuv成功的跟踪上了期望的轨迹，跟踪偏差是收敛到零点附近的一个邻域内的。而且所有的闭环信号是有界的。神经网络的突出优点是表现出平滑的响应。本发明产生的控制信号都位于可接受的饱和限值之内。图4(a)为本发明滤波后信号和原信号的对比图，可见本发明滤波效果非常好，本发明滤波后轨迹跟踪误差ef信号，其中k1＝0.1,k2＝1。图4(b)中对比滤波信号函数为时滤波效果很差，没有本发明滤波效果好。