CN109283941B - 基于扰动观测器的预设性能海底飞行节点轨迹跟踪控制方法 - Google Patents

Abstract

基于扰动观测器的预设性能海底飞行节点轨迹跟踪控制方法，本发明涉及预设性能海底飞行节点轨迹跟踪控制方法。本发明为了解决现有方法没有考虑建模不确定性以及海洋环境扰动与推进器故障对OBFN的影响的问题。本发明包括：一：建立Fossen大纲六自由度非线性动力学模型；二：对步骤一建立的非线性动力学模型进行OBFN的动力学模型变换，得到OBFN的动力学模型，根据OBFN的动力学模型确定OBFN的跟踪误差方程；三：建立性能函数；四：将步骤三的跟踪误差进行误差变换，得到变换后的误差；五：根据步骤四得到的变换后的误差，设计OBFN系统总不确定性观测器与预设性能轨迹跟踪控制器。本发明用于轨迹跟踪控制领域。

Description

基于扰动观测器的预设性能海底飞行节点轨迹跟踪控制方法

技术领域

本发明涉及预设性能海底飞行节点轨迹跟踪控制方法。

背景技术

随着材料、能源、自动控制等领域的技术进步，自主式水下航行器(Autonomousunderwater vehicle，AUV)正逐渐向模块化、多功能和高可靠性的方向发展，进而衍生出各种海洋作业应用。例如海底管道的检修、海洋环境特征参数的采集、海底油气资源勘探等等。海底飞行节点(Ocean bottom flying node，OBFN)是将当前海底节点地震勘探技术(Ocean bottom node，OBN)与AUV相结合的产物，如图1—图3所示。OBFN由母船释放后自主航行至海底指定位置，可长期坐底采集海底地震数据，用于海洋油气资源勘探。

为了达到海底石油勘探所需的精确度，通常需要数以千计的OBFN自主布放在海底指定位置，因此，不仅需要克服复杂海洋环境造成扰动，跟踪期望的轨迹完成部署，还需要控制轨迹跟踪误差的收敛过程，避免在大规模部署中出现OBFN之间相互碰撞等情况。

建模不确定性、复杂海洋环境的扰动与推进器故障是影响AUV控制常见的因素。常见的研究思路是假设AUV在工作过程中不出现故障情况，或是为推进器故障单独设计一套故障诊断处理模型，再设计其他的控制算法处理AUV的建模不确定性和海洋环境的扰动影响，其处理过程过于复杂。

传统的预设性能方法均采用一种指数收敛形式的性能函数，这种形式令性能函数的参数与实际的收敛速率之间难以建立明确的数学关系，且性能函数参数的选取缺乏一个合理的规则。

因此，本发明针对OBFN的轨迹跟踪控制问题，考虑建模不确定性、海洋环境扰动与推进器故障对OBFN的影响，提出一种预设性能的OBFN轨迹跟踪控制方法。

发明内容

本发明的目的是为了解决现有方法没有考虑建模不确定性以及海洋环境扰动与推进器故障对OBFN的影响的问题，而提出基于扰动观测器的预设性能海底飞行节点轨迹跟踪控制方法。

基于扰动观测器的预设性能海底飞行节点轨迹跟踪控制方法包括以下步骤：

步骤一：建立Fossen大纲六自由度非线性动力学模型；

步骤二：对步骤一建立的Fossen大纲六自由度非线性动力学模型进行OBFN的动力学模型变换，得到OBFN的动力学模型，根据OBFN的动力学模型确定OBFN的跟踪误差方程；所述OBFN为海底飞行节点；

所述OBFN的动力学模型为：

其中

为v的一阶导数，M^-1为M的逆，M为质量惯性矩阵，B₀代表OBFN推力分配矩阵B的标称值，u代表OBFN推进器的控制输出，C_v0代表C_v的标称值，C_v为OBFN的科氏力和向心力矩阵，D_v0代表D_v的标称值，D_v为OBFN的水动力阻尼矩阵，g_η0代表g_η的标称值，g_η为OBFN重力和浮力产生的力和力矩向量，v＝[u′,a,w,p,q,r]^T表示OBFN在运动坐标系下的速度与角速度，u′为OBFN在运动坐标系下纵荡速度，a为OBFN在运动坐标系下横荡速度，w为OBFN在运动坐标系下垂荡速度，p为OBFN在运动坐标系下横倾角速度，q为OBFN在运动坐标系下纵倾角速度，r为OBFN在运动坐标系下摇首角速度，T为转置；F表示系统的总不确定度；

步骤三：建立性能函数，利用性能函数将跟踪误差表示为：

其中e_i(t)为OBFN位置与姿态角误差，i＝1,2,3,4,5,6；e_i(0)为初始时间OBFN位置与姿态角误差，ρ_i(t)为为第i个自由度的性能函数，δ_i为辅助变量，满足0≤δ_i≤1；

根据性能函数(9)和式(2)的形式可知，如果跟踪误差初值满足0≤||e_i(0)||≤ρ_i(0)，则参数k_i限制了跟踪误差的最小收敛速率，而ρ_i∞给定了允许的稳态跟踪误差的上界，同时系统响应的超调不会超过δ_iρ_i(t)。

步骤四：将步骤三的跟踪误差进行误差变换，得到变换后的误差s；

步骤五：根据步骤四得到的变换后的误差s，设计OBFN系统总不确定性观测器与预设性能轨迹跟踪控制器。使用观测器观测飞行节点在运动中的系统总不确定性并将观测值代入控制器，然后使用控制器控制海底飞行节点的推进器输出，即可实现轨迹跟踪。

本发明的有益效果为：

本发明方法引入了扰动观测器技术估计建模不确定性、海洋环境扰动与推进器故障造成的影响。此外，引入一种新颖的性能函数替代传统性能函数，通过合理的选择参数从而确保期望的收敛时间，还可以自由设置误差系统的收敛速度，避免产生过大的控制输出。由仿真实验结果可知，OBFN在跟踪设定的螺旋线轨迹时，其6个自由度轨迹跟踪误差均处于设计的性能函数边界之内，无超调现象，且稳态误差小于性能函数的0.01，实现了跟踪误差收敛至设定的稳态精度0.01，且最大收敛时间小于仿真设定的t_f值，即20s。

如果要实现在海洋环境扰动、模型不确定性和推进器故障综合影响下的海底飞行节点轨迹跟踪的控制要求，除了本发明算法外还有使用PID控制器、滑模控制、自适应神经网络控制等考虑外部扰动的轨迹跟踪控制方案和使用传统性能函数的预设性能控制方案，以下介绍这两种方案，并将它们与本发明算法进行比较。

(1)考虑外部扰动的方案

文献(Gao J，Wu P，Yang B，et al.Adaptive neural network control forvisual servoing of underwater vehicles with pose estimation[J].Journal ofMarine Science&Technology，2017，22(3):470-478)设计了一种自适应神经网络控制器，其利用单隐层神经网络与滑模控制器来保证跟踪误差的收敛，以补偿动态不确定性和外部扰动造成的影响。文献(Sun B，Gan W，Mei M，et al.CASCADED UUV TRAJECTORY TRACKINGCONTROL BASED ON MODEL PREDICTIVE AND SLIDING MODE CONTROL[J].Journal ofMarine Science&Technology，2017，25)提出了一种基于模型预测控制和滑模技术的级联动态轨迹跟踪控制方法，解决了建模不确定性与外部扰动问题。文献(Liu X，Zhang M，YaoF.Adaptive fault tolerant control and thruster fault reconstruction forautonomous underwater vehicle[J].Ocean Engineering，2018，155:10-23)提出了一种基于自适应容错控制的虚拟闭环系统，用于处理理想环境下的初始跟踪误差，避免出现严重的抖振效应，并构造了一种改进的二阶滑模观测器用于估计推进器的故障影响，实现了AUV在海流扰动、建模不确定和推进器故障影响下的轨迹跟踪控制。文献(杨超，郭佳，张铭钧.基于RBF神经网络的作业型AUV自适应终端滑模控制方法及实验研究[J].机器人，2018，40(3):336-345)提出一种基于径向基函数神经网络的AUV自适应终端滑模运动控制方法，该方法对机械手展开引起的AUV动力学性能变化和水流环境干扰进行在线逼近，并结合自适应终端滑模控制器对神经网络权值和AUV控制参数进行自适应在线调节，实现了轨迹跟踪的控制目标。

可以看出，在大部分的AUV控制设计中，海流扰动、建模不确定性和推进器故障等影响控制精度的因素通常被单独考虑，设计过程较为繁琐。因此本发明算法在其基础上进行了改进，将上述三种影响因素并入系统的总不确定性中，通过设计系统总不确定性观测器估计其产生的影响，实现了OBFN的预设性能轨迹跟踪控制。

(2)基于传统性能函数的预设性能控制方案

文献(Bechlioulis CP，Karras GC，Heshmati-Alamdari S，et al.Trajectorytracking with prescribed performance for underactuated underwater vehiclesunder model uncertainties and external disturbances.IEEE Trans.ControlSyst.Technol 2017；25(2):429-440)将预设性能方法应用于一种欠驱动AUV的三维轨迹跟踪控制中，增强对系统不确定性的鲁棒性，并获得了预设的动态和稳态性能。文献(高吉成.具有预设性能的自适应容错控制研究及其在水下机器人的应用[D].扬州大学，2017)提出了一种预设性能自适应容错反步控制方案，在考虑外界干扰和预设性能要求的情况下，通过性能函数与相应的误差变换，将初始系统转换为结构一致的非线性系统，使输出误差满足预先设定的性能标准。

本发明针对传统的预设性能方法所使用的指数形式的性能函数存在参数与实际的收敛速率之间难以建立明确的数学关系的问题，引入了一种新颖的性能函数，其可以通过合理的选择参数从而确保期望的收敛时间，还可以自由设置误差系统的收敛速度，避免产生过大的控制输出，便于预设性能方法的推广使用。

附图说明

图1为海底飞行节点侧视图；

图2为海底飞行节点斜视图；

图3为海底飞行节点俯视图；

图4为OBFN的推进器布置形式图；

图5为推进器突然故障下的纵荡跟踪误差e₁；

图6为推进器突然故障下的横荡跟踪误差e₂；

图7为推进器突然故障下的垂荡跟踪误差e₃；

图8为推进器突然故障下的横滚跟踪误差e₄；

图9为推进器突然故障下的俯仰跟踪误差e₅；

图10为推进器突然故障下的摇首跟踪误差e₆。

具体实施方式

具体实施方式一：基于扰动观测器的预设性能海底飞行节点轨迹跟踪控制方法包括以下步骤：

运动坐标系(G-xyz)：原点G取在OBFN的重心处，x轴、y轴和z轴分别为经过原点的水线面、横剖面和中纵剖面的交线。

惯性坐标系(E-ξηζ)：原点E可选在海面的某一点，Eξ轴和Eη轴置于水平面内且互相垂直，Eξ轴正向指向正北方向。.Eζ.垂直于Eξη平面，正向指向地心。

Fossen大纲六自由度非线性动力学模型：

式中：M为质量惯性矩阵，η＝[x,y,z,φ,θ,ψ]^T表示OBFN在大地坐标系下的六自由度位置与姿态，v＝[u,v,w,p,q,r]^T表示OBFN在运动坐标系下的速度与角速度，J是固定坐标系与运动坐标系之间的转换矩阵，C_v为OBFN的科氏力和向心力矩阵，D_v为OBFN的水动力阻尼矩阵，g_η为OBFN重力和浮力产生的力和力矩向量，τ为OBFN推进系统产生的控制力和力矩。

预设性能控制方法：是一种通过引入性能函数与误差变换，将原始的“约束”系统转换为等效的“无约束”系统，并用李雅普诺夫理论证明该“无约束”系统的稳定性，从而使收敛速度、超调以及跟踪误差获得预先设定的性能的方法。

扰动观测器：将控制系统外部扰动以及模型参数变化造成的实际对象与标称模型之间的差异等效到控制输入端，即观测出等效干扰。并在控制中引入等效的补偿，进而消除了外部扰动的影响。

参数定义：

η＝[x,y,z,φ,θ,ψ]^T为固定坐标系下OBFN的六自由度位置与姿态值；J为固定坐标系与运动坐标系之间的转换矩阵；v＝[u,a,w,p,q,r]^T为运动坐标系下OBFN的速度与角速度量；M为OBFN的质量惯性矩阵；C_v为OBFN的科氏力和向心力矩阵；D_v为OBFN的水动力阻尼矩阵；g_η为OBFN重力和浮力产生的力和力矩向量；τ为OBFN推进系统产生的控制力和力矩。

本发明专利的核心为设计控制器u使OBFN在存在建模不确定性、海洋环境扰动与推进器故障的情况下，其位置与姿态量η仍然能够跟踪期望值η_d，并使跟踪误差e＝η-η_d具有预先给定的动态性能及稳态响应情况。

步骤一：建立Fossen大纲六自由度非线性动力学模型；

步骤二：对步骤一建立的Fossen大纲六自由度非线性动力学模型进行OBFN的动力学模型变换，得到OBFN的动力学模型，根据OBFN的动力学模型确定OBFN的跟踪误差方程；所述OBFN为海底飞行节点；

所述OBFN的动力学模型为：

其中

为v的一阶导数，M^-1为M的逆，M为质量惯性矩阵，B₀代表OBFN推力分配矩阵B的标称值，u代表OBFN推进器的控制输出，C_v0代表C_v的标称值，C_v为OBFN的科氏力和向心力矩阵，D_v0代表D_v的标称值，D_v为OBFN的水动力阻尼矩阵，g_η0代表g_η的标称值，g_η为OBFN重力和浮力产生的力和力矩向量，v＝[u′,a,w,p,q,r]^T表示OBFN在运动坐标系下的速度与角速度，u′为OBFN在运动坐标系下纵荡速度，a为OBFN在运动坐标系下横荡速度，w为OBFN在运动坐标系下垂荡速度，p为OBFN在运动坐标系下横倾角速度，q为OBFN在运动坐标系下纵倾角速度，r为OBFN在运动坐标系下摇首角速度，T为转置；F表示系统(公式1)的总不确定度；

步骤三：建立性能函数，利用性能函数将跟踪误差表示为：

其中e_i(t)为OBFN位置与姿态角误差，i＝1,2,3,4,5,6；e_i(0)为初始时间OBFN位置与姿态角误差，ρ_i(t)为为第i个自由度的性能函数，δ_i为辅助变量，满足0≤δ_i≤1；

根据性能函数(9)和式(2)的形式可知，如果跟踪误差初值满足0≤||e_i(0)||≤ρ_i(0)，则参数k_i限制了跟踪误差的最小收敛速率，而ρ_i∞给定了允许的稳态跟踪误差的上界，同时系统响应的超调不会超过δ_iρ_i(t)。

步骤四：将步骤三的跟踪误差进行误差变换，得到变换后的误差s；

步骤五：根据步骤四得到的变换后的误差s，设计OBFN系统总不确定性观测器与预设性能轨迹跟踪控制器。使用观测器观测飞行节点在运动中的系统总不确定性并将观测值代入控制器，然后使用控制器控制海底飞行节点的推进器输出，即可实现轨迹跟踪。

理论基础

1、OBFN的动力学模型

OBFN可以采用Fossen大纲六自由度非线性AUV动力学模型：

式中：M为质量惯性矩阵，η＝[x,y,z,φ,θ,ψ]^T表示OBFN在大地坐标系下的六自由度位置与姿态，v＝[u,v,w,p,q,r]^T表示OBFN在运动坐标系下的速度与角速度，J是固定坐标系与运动坐标系之间的转换矩阵，C_v为OBFN的科氏力和向心力矩阵，D_v为OBFN的水动力阻尼矩阵，g_η为OBFN重力和浮力产生的力和力矩向量，τ为OBFN推进系统产生的控制力和力矩。

OBFN的推进器的故障影响可以采用推力分配矩阵形式表示，定义为ΔB。因此，实际的控制力与力矩可以改写为τ+Δτ：

τ+Δτ＝(B₀-KB)u＝(B₀+ΔB)u (22)

式中：B₀代表OBFN推力分配矩阵的标称值，B为OBFN的推力分配矩阵，u代表OBFN推进器的控制输出，K是一个对角矩阵，其元素k_ii∈[0,1]，表示相应的推进器故障程度，其中1代表故障程度最高，推进器完全失效。因此，等式(21)可改写为：

式中：下标0表示对应变量的标称值，C_v0即代表C_v的标称值，以此类推；F表示系统的总不确定度，其表达式如下：

式中：

表示海流扰动对OBFN造成的影响；符号Δ表示对应的变量的不确定值，ΔM即代表OBFN质量惯性矩阵M的不确定值，以此类推。

为了有效控制轨迹跟踪的误差收敛，在OBFN的动力学模型(23)的基础上，给出OBFN的跟踪误差方程：

式中：

代表OBFN在固定坐标系下轨迹跟踪误差的二阶导数，v_e＝v-v_d代表OBFN在运动坐标系下速度与角速度的跟踪误差，

表OBFN在运动坐标系下速度与角速度的跟踪误差的二阶导数，令

D＝-J(η)F，其中G、H、D均为简化变量。等式(25)可简写如下：

本发明专利的核心为设计控制器u使OBFN在存在建模不确定性、海洋环境扰动与推进器故障的情况下，其位置与姿态量η仍然能够跟踪期望值η_d，并使跟踪误差e＝η-η_d具有预先给定的动态性能及稳态响应情况。为实现这一目标，结合实际工程背景，本发明给出以下2个假设：

假设1位置与姿态角η和速度与角速度v可测。

假设2期望的位置与姿态角η_d与其一阶、二阶导数均已知而且有界。

2、性能函数的建立与对比

对于光滑函数ρ(t):R₊→R，如果其满足如下条件：

(1)ρ(t)单调递减且恒为正；

(2)

则称ρ(t)为一个性能函数。

首先给出一种传统的性能函数，其表达式如下所示：

ρ(t)＝(ρ₀-ρ_∞)e^-kt+ρ_∞ (27)

式中：ρ₀、ρ_∞和k为预先给定的正常数。利用性能函数可以将跟踪误差表示为：

式中：e_i(t),i＝1,2,3,4,5,6为OBFN位置与姿态角误差，0≤δ_i≤1。根据性能函数(27)和式(28)的形式可知，如果跟踪误差初值满足0≤||e_i(0)||≤ρ_i(0)，则参数k_i限制了跟踪误差的最小收敛速率，而ρ_i∞给定了允许的稳态跟踪误差的上界，同时系统响应的超调不会超过δ_iρ_i(t)。

但是，传统的性能函数(27)的收敛速率取决于指数项e^-kt，这种指数收敛的形式难以将常数k与实际的收敛速度建立一个明确的数学关系。此外，常数k的选取缺乏一个合理的规则。综合考虑上述因素，本发明提出了一种新颖的性能函数用于替代传统的性能函数(27)，其表达式如下所示：

式中：a₁、a₂、a₃、a₄为待设计参数，k和ρ_tf＝ρ_∞的定义与传统的性能函数(27)相同，预设参数t_f定义了性能函数(29)到达ρ_∞的截止时间。

本发明通过下述两个步骤确保式(29)是符合性能函数的一种性能函数。

步骤1：通过下述的限制条件计算参数a₁、a₂、a₃、a₄。

这种新颖的性能函数(29)的初始和终端条件与传统性能函数(27)相同，可以被表述为ρ(0)＝ρ₀，ρ(t_f)＝ρ_tf，其中，ρ(t)关于时间的一阶和二阶导数均为连续函数，即

与

通过上述条件即可计算四个未知的参数a₁、a₂、a₃、a₄。定义a₀＝2t_fk/π，我们可得：

步骤2：验证ρ(t)是一个单调递减且恒为正的函数。

注意到ρ(0)＝ρ₀＞0且

若

被证明对所有t∈[0,t_f)均成立，则ρ(t)满足单调递减且恒为正的条件。

本发明通过严谨的数学推导验证上述2个步骤能有效的保证式(29)符合性能函数的定义。

令等式(29)在区间t∈[0,t_f)对时间t求导并代入a₀,...,a₄的具体表达式，可得：

通过计算可知ka₄＞0。因此，问题的证明可转化为验证函数y＜0在区间[0,t_f)上成立，其中：

令c＝t_fk，x＝t/t_f，等式(32)可改写为

依据变量y(x)的初始值y(0)＝2c/π-e^c＜0和y(1)＝0，求取y(x)关于x的一阶导数

依据变量

的初始值

和

进一步求y(x)对x的二阶导数

显然，

即

为单调递减函数。因为

大于0且

等于0，可得

在定义区间上不小于0，即y(x)为单调递增函数。又y(0)小于0且y(1)等于0，可得y(x)在区间上不大于0。综上所述，

在区间0≤t≤t_f(当且仅当t＝t_f时，

)成立，即ρ(t)是一个单调递减且恒为正的函数。

因此，等式(29)可作为一种性能函数，其参数设置如式(30)所示。在上述的分析过程中，步骤2证明了参数t_f和k的选取均会影响性能函数(29)的收敛速度，且两者之间没有约束关系，可自由选取。与传统的性能函数(27)相比，这种新颖的性能函数(29)具有以下重要的特点：

(1)最大收敛时间t_f可以被预先设定；

(2)对于一个给定的稳态收敛时间，可以通过修改参数k来调整性能函数(29)的收敛速度。

3、误差变换

为解决由式(28)表示的预设性能控制问题，本发明采用一种误差变换方式将约束下的跟踪控制问题转化为无约束的稳定控制问题。定义函数S_i(ε_i)，具有以下性质：

(1)S_i(ε_i)光滑且严格单调递增；

(2)

(3)

其中ε_i∈(-∞,+∞)称为变换误差。满足上述条件的一个函数S_i(ε_i)由下式给出：

根据S_i(ε_i)的特性，式(28)可等价表示为

e_i(t)＝ρ_i(t)S_i(ε_i) (37)

因为S_i(ε_i)是严格单调递增的，所以存在反函数

如果能够控制ε_i有界，则可以保证式(28)成立，进在性能函数ρ_i(t)的约束下使跟踪误差达到期望目标。此时系统(26)的跟踪控制问题便转化为以ε_i为变量的闭环系统的稳定控制问题。

考虑S_i(ε_i)取等式(36)的形式，则有

式中：z_i＝e_i(t)/ρ_i(t)

令ε_i分别对时间t求一阶和二阶导数：

式中：

可通过式(39)计算获得

分别表示OBFN实际的位置与姿态角和期望的位置与姿态角。由于

和ρ_i(t)＞0可知r_i恒大于零，且只要误差e_i(t)的轨迹严格限制在式(28)的范围内，则r_i有界即满足

和

为正常数。

取误差变量s∈R⁶为如下形式

式中：ε＝[ε₁,ε₂,ε₃,ε₄,ε₅,ε₆]^T，λ＝diag[λ₁,λ₂,λ₃,λ₄,λ₅,λ₆]＞0为待设计参数。

结合OBFN的简化形式的跟踪误差方程(26)与误差变量(42)，可得：

式中：V＝[v₁,v₂,v₃,v₄,v₅,v₆]^T，

R＝diag[r₁,r₂,r₃,r₄,r₅,r₆]。如果设计控制器u使s有界，则根据式(42)可得ε_i和

有界。

4、OBFN系统总不确定性观测器与预设性能轨迹跟踪控制器设计

本发明将针对误差系统(26)设计对应的控制器。考虑系统的总不确定性不能被直接测量，观测器技术被引入用于估计系统的总不确定性，相应的模型假设也一并给出，如下所示。

假设3：系统的总不确定性D的变化速率有界，即

其中，χ为未知正常数。

系统的总不确定性观测器设计如下：

式中：

代表了总不确定性D的估计值，z_D是观测器的辅助变量，P,K₁和L＝K₁R是观测器的增益矩阵。

基于上述观测器的轨迹跟踪控制器如下所示：

式中：K₂为控制增益矩阵。

考虑OBFN的轨迹跟踪误差系统(26)，通过误差变换(38)化为误差系统(43)，如果将控制器u设计为式(45)的形式并采用式(44)的观测器，且增益矩阵P,K₁,K₂满足下列不等式

式中：

和λ_min分别代表了最大正定和最小的矩阵特征值，μ₁是正定的常数，W＝R^-1。则变换误差ε_i一致最终有界，且跟踪误差e_i满足预设性能约束式(28)。

证明：因为矩阵R是一个对称的正定矩阵，且r_i有界，本发明给出如下Lyapunov备选函数：

式中：

代表系统总不确定性的估计误差，W＝R^-1。对V₁求导并代入式(43)、控制器(45)和观测器(44)可得

式中：

当选择合适的增益矩阵P,K₁,K₂满足条件(46)时，误差s和系统总不确定性观测误差D_e一致最终有界，且分别收敛于集合：

进而有变换误差ε_i一致最终有界，且收敛于

最后，依据函数S_i(ε_i)的性质，可使性能约束等式(28)成立，即OBFN的轨迹跟踪误差e_i获得预先指定的动态性能及稳态响应，证毕。

具体实施方式二：本实施方式与具体实施方式一不同的是：所述步骤一中建立Fossen大纲六自由度非线性动力学模型的具体过程为：

运动坐标系(G-xyz)的原点G取在OBFN的重心处，x轴、y轴和z轴分别为经过原点的水线面、横剖面和中纵剖面的交线；

固定坐标系(E-ξηζ)的原点E选在海面的某一点，Eξ轴和Eη轴置于水平面内且互相垂直，Eξ轴正向指向正北方向，Eζ垂直于Eξη平面，正向指向地心；

Fossen大纲六自由度非线性动力学模型：

式中η＝[x,y,z,φ,θ,ψ]^T表示OBFN在固定坐标系下的六自由度位置与姿态，式中：x为OBFN固定坐标系下Eξ轴方向位移，y为OBFN在固定坐标系下Eη轴方向位移，z为OBFN在固定坐标系下Eζ轴方向位移，φ为为OBFN在固定坐标系下横倾角度，θ为OBFN在固定坐标系下纵倾角度，ψ为OBFN在固定坐标系下摇首角度；J(η)是固定坐标系与运动坐标系之间的转换矩阵，τ为OBFN推进系统产生的控制力和力矩，

为v的一阶导数。

其它步骤及参数与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式一或二不同的是：所述步骤二中对步骤一建立的Fossen大纲六自由度非线性动力学模型进行OBFN的动力学模型变换，得到OBFN的动力学模型，根据OBFN的动力学模型确定OBFN的跟踪误差方程的具体过程为：

OBFN的推进器的故障影响采用推力分配矩阵形式表示，定义为ΔB；因此，实际的控制力与力矩改写为τ+Δτ：

τ+Δτ＝(B₀-KB)u＝(B₀+ΔB)u (5)

式中B为OBFN的推力分配矩阵，τ为OBFN的推进器的实际的控制力，Δτ是推进器故障对推进器推力的影响值，K是一个对角矩阵，其元素k_ii∈[0,1]，表示相应的推进器故障程度，其中1代表故障程度最高，推进器完全失效，u是OBFN的推进器的控制输入，B₀为OBFN的推力分配矩阵的标称值(所有下标带0的均为标称值)；因此，等式(4)改写为公式(1)的形式；

系统的总不确定度F的表达式如下：

式中：

表示海流扰动对OBFN的干扰，符号Δ表示对应的变量的不确定值，ΔM即代表OBFN质量惯性矩阵M的不确定值，ΔB为B的不确定值，ΔC_v为C_v的不确定值，ΔD_v为D_v的不确定值，Δg_η为g_η的不确定值，C_Aη为C_A的导出变量，即C_Aη＝C_A(v_r)J^-1，式中：C_A为OBFN附加质量的科氏力和向心力矩阵，D_η为D的导出变量，即D_η＝D(v_r)J^-1，式中：D为水动力阻尼矩阵，η_r为OBFN在固定坐标系下相对于海流的位移向量；

不确定值在仿真中可以人为设定一个值，用于证明所提出的方法可以有效的克服这种不确定性。比方说A，他的真实值是A，标称值是A0，真实值A就等于标称值A0加上A的不确定值。

为了有效控制轨迹跟踪的误差收敛，在OBFN的动力学模型(1)的基础上，给出OBFN的跟踪误差方程：

式中

代表OBFN在固定坐标系下轨迹跟踪误差的二阶导数，

表示OBFN实际的位置与姿态角，

表示OBFN期望的位置与姿态角，v_e＝v-v_d代表OBFN在运动坐标系下速度与角速度的跟踪误差，v_d代表OBFN在运动坐标系下期望的速度与角速度，

表OBFN在运动坐标系下速度与角速度的跟踪误差的一阶导数，令

D＝-J(η)F，其中G、H、D均为简化变量；等式(7)简写如下：

其它步骤及参数与具体实施方式一或二相同。

具体实施方式四：本实施方式与具体实施方式一至三之一不同的是：所述步骤三中建立性能函数的具体过程为：

一种新性能函数的定义如下所示：

式中a₁、a₂、a₃和a₄为待设计参数，k和ρ_tf＝ρ_∞为预先给定的正常数，预设参数t_f定义了性能函数(9)到达ρ_∞的截止时间，ρ_∞为根据OBFN稳态的控制精度而定的正常数(取决于控制者希望OBFN的轨迹跟踪控制系统最后能达到什么样的精度，这个值就是最终的精度值)；参数t_f和k的选取均会影响性能函数(8)的收敛速度，且两者之间没有约束关系，可自由选取。

其它步骤及参数与具体实施方式一至三之一相同。

具体实施方式五：本实施方式与具体实施方式一至四之一不同的是：所述步骤四中将步骤三的跟踪误差进行误差变换，得到变换后的误差s的具体过程为：

本发明采用一种误差变换方式将约束下的跟踪控制问题转化为无约束的稳定控制问题。定义函数S_i(ε_i)：

其中ε_i∈(-∞,+∞)为变换误差；

其具有以下性质：

(1)S_i(ε_i)光滑且严格单调递增；

(2)

(3)

其中，ε_i∈(-∞,+∞)称为变换误差。

根据S_i(ε_i)的特性，式(9)等价表示为：

e_i(t)＝ρ_i(t)S_i(ε_i) (11)

因为S_i(ε_i)是严格单调递增的，所以存在反函数：

其中

为S_i函数的反函数；

此时系统(5)的跟踪控制问题便转化为以ε_i为变量的闭环系统的稳定控制问题。

考虑S_i(ε_i)取等式(10)的形式，则有：

式中z_i＝e_i(t)/ρ_i(t)，z_i为第i个自由度的辅助变量；令ε_i对时间t求取一阶导数

和二阶导数

式中

通过式(13)计算获得；r_i为第i个自由度的辅助变量，

为r_i的一阶导数，e_i为OBFN在固定坐标系下的位置与姿态角误差，

为e_i的一阶导数，

为第i个自由度性能函数的一阶导数，

为第i个自由度性能函数的二阶导数；

表示第i自由度OBFN实际的位置与姿态角，

表示第i自由度OBFN期望的位置与姿态角，i＝1,2,3,4,5,6；

取误差变量s∈R⁶为如下形式：

式中：ε＝[ε₁,ε₂,ε₃,ε₄,ε₅,ε₆]^T，

为ε的一阶导数；λ＝diag[λ₁,λ₂,λ₃,λ₄,λ₅,λ₆]＞0为待设计参数；ε₁,ε₂,ε₃,ε₄,ε₅,ε₆分别代表第1自由度至第6自由度的误差变量，ε₁代表第1自由度的误差变量，λ₁,λ₂,λ₃,λ₄,λ₅,λ₆分别代表第1自由度至第6自由度的待设计参数；

结合OBFN的简化形式的跟踪误差方程(7)与误差变量(16)，得到：

式中：

为s的一阶导数，

为ε的一阶导数，

为ε的二阶导数；V＝[v₁,v₂,v₃,v₄,v₅,v₆]^T，

R＝diag[r₁,r₂,r₃,r₄,r₅,r₆]，V和R为中间变量，v_i为第i个自由度的中间变量，

为第i个自由度的辅助变量r_i的一阶导数；如果设计控制器u使s有界，则根据式(16)可得ε_i和

有界。

其它步骤及参数与具体实施方式一至四之一相同。

具体实施方式六：本实施方式与具体实施方式一至五之一不同的是：所述步骤五中根据步骤四得到的变换后的误差s，设计OBFN系统总不确定性观测器与预设性能轨迹跟踪控制器的具体过程为：

OBFN系统的总不确定性观测器设计如下：

式中

代表了总不确定性D的估计值，z_D是观测器的辅助变量，

是观测器的辅助变量的一阶导数，R^-1是中间变量R的逆矩阵，P,K₁和L＝K₁R是观测器的增益矩阵；

基于上述观测器的轨迹跟踪控制器如下所示：

式中K₂为增益矩阵，H^-1为简化变量H的逆矩阵。

其它步骤及参数与具体实施方式一至五之一相同。

实施例一：

(1)仿真准备

为了验证本发明所提出控制方法的有效性，将其应用到一种OBFN模型中进行仿真验证。OBFN模型对应的水动力系数、惯性系数分别如表1和2所示。

表1 OBFN的水动力系数

表2 OBFN的惯性系数

螺旋式下潜是AUV航行过程中的一种常见的航行模式，由于其涵盖了多个自由度的运动，具有一定的代表性。因此，本发明假设OBFN期望的轨迹为一种螺旋线，具体表达式如下所示：

η_d＝[2sin(0.1t)；2cos(0.1t)+2；-0.5144t；0；0；0] (51)

海流通常由重力，风和海水的摩擦力以及不同位置海水的密度差所产生，在仿真中难以被精确的描述。因此，有必要对海流扰动模型进行适当简化。本发明假设在OBFN轨迹跟踪的局部区域内，海流的方向是固定的，与地球坐标系下与x轴正方向相同，海流流速的表达式如下:

为了便于仿真分析，本发明将建模不确定性量化处理。考虑将模型标称值的20％作为建模误差，并将其作为扰动的一部分并入仿真模块。

由于OBFN的推进器布置采用全驱动模式，各向布置基本相同，如图4所示。因此在仿真中，仅考虑某一固定的推进器出现故障，即可代表任一推进器的故障情况。本发明假设1号推进器为故障推进器，其故障模式用式(53)表示。

针对每一个控制自由度，本发明所期望的OBFN的轨迹跟踪控制性能设计为：(1)稳态跟踪误差不超过0.01；(2)最大收敛时间不超过20s；(3)系统响应无超调。据此可以确定性能函数ρ_i(t)和δ_i的取值，如表3所示，与之相对应的轨迹跟踪控制器与系统总不确定性观测器的参数一并给出，如表4所示。

表3性能函数的参数值

表4控制器与观测器的参数值

(2)仿真结果

在该仿真分析中，推进器故障模式基于等式(53)，并考虑建模不确定性与海流扰动对OBFN的影响。图5-图10给出了OBFN 6个自由度轨迹跟踪误差曲线。其中，实线代表应用本发明所提出的控制方法作用下的轨迹跟踪误差曲线；虚线曲线代表预设的性能边界。

从图5-图10可以看出，本发明所提出的总不确定性观测器与预设性能控制方法可以使轨迹跟踪误差保持在由性能函数预先设定的边界内，并在规定的时间内收敛到预设的稳态控制精度。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (4)

1.基于扰动观测器的预设性能海底飞行节点轨迹跟踪控制方法，其特征在于：所述方法包括以下步骤：

步骤一：建立Fossen大纲六自由度非线性动力学模型；

步骤二：对步骤一建立的Fossen大纲六自由度非线性动力学模型进行OBFN的动力学模型变换，得到OBFN的动力学模型，根据OBFN的动力学模型确定OBFN的跟踪误差方程；所述OBFN为海底飞行节点；

所述OBFN的动力学模型为：

其中

为v的一阶导数，M^-1为M的逆，M为质量惯性矩阵，B₀代表OBFN推力分配矩阵B的标称值，u代表OBFN推进器的控制输出，C_v0代表C_v的标称值，C_v为OBFN的科氏力和向心力矩阵，D_v0代表D_v的标称值，D_v为OBFN的水动力阻尼矩阵，g_η0代表g_η的标称值，g_η为OBFN重力和浮力产生的力和力矩向量，v＝[u′,a,w,p,q,r]^T表示OBFN在运动坐标系下的速度与角速度，u′为OBFN在运动坐标系下纵荡速度，a为OBFN在运动坐标系下横荡速度，w为OBFN在运动坐标系下垂荡速度，p为OBFN在运动坐标系下横倾角速度，q为OBFN在运动坐标系下纵倾角速度，r为OBFN在运动坐标系下摇首角速度，T为转置；F表示系统的总不确定度；

步骤三：建立性能函数，利用性能函数将跟踪误差表示为：

其中e_i(t)为OBFN位置与姿态角误差，i＝1,2,3,4,5,6；e_i(0)为初始时间OBFN位置与姿态角误差，ρ_i(t)为为第i个自由度的性能函数，δ_i为辅助变量，满足0≤δ_i≤1；

步骤四：将步骤三的跟踪误差进行误差变换，得到变换后的误差s；

步骤五：根据步骤四得到的变换后的误差s，设计OBFN系统总不确定性观测器与预设性能轨迹跟踪控制器；

所述步骤一中建立Fossen大纲六自由度非线性动力学模型的具体过程为：

运动坐标系的原点G取在OBFN的重心处，x轴、y轴和z轴分别为经过原点的水线面、横剖面和中纵剖面的交线；

固定坐标系的原点E选在海面的某一点，Eξ轴和Eη轴置于水平面内且互相垂直，Eξ轴正向指向正北方向，Eζ垂直于Eξη平面，正向指向地心；

Fossen大纲六自由度非线性动力学模型：

式中η＝[x,y,z,φ,θ,ψ]^T表示OBFN在固定坐标系下的六自由度位置与姿态，式中：x为OBFN固定坐标系下Eξ轴方向位移，y为OBFN在固定坐标系下Eη轴方向位移，z为OBFN在固定坐标系下Eζ轴方向位移，φ为OBFN在固定坐标系下横倾角度，θ为OBFN在固定坐标系下纵倾角度，ψ为OBFN在固定坐标系下摇首角度；J(η)是固定坐标系与运动坐标系之间的转换矩阵，τ为OBFN推进系统产生的控制力和力矩，

为v的一阶导数；

所述步骤二中对步骤一建立的Fossen大纲六自由度非线性动力学模型进行OBFN的动力学模型变换，得到OBFN的动力学模型，根据OBFN的动力学模型确定OBFN的跟踪误差方程的具体过程为：

OBFN的推进器的故障影响采用推力分配矩阵形式表示，定义为ΔB；实际的控制力与力矩改写为τ+Δτ：

τ+Δτ＝(B₀-KB)u＝(B₀+ΔB)u (5)

式中B为OBFN的推力分配矩阵，τ为OBFN的推进器的实际的控制力，Δτ是推进器故障对推进器推力的影响值，K是一个对角矩阵，其元素k_ii∈[0,1]，表示相应的推进器故障程度，其中1代表故障程度最高，推进器完全失效，u是OBFN的推进器的控制输入，B₀为OBFN的推力分配矩阵的标称值；等式(4)改写为公式(1)的形式；

系统的总不确定度F的表达式如下：

式中：

表示海流扰动对OBFN的干扰，ΔM即代表OBFN质量惯性矩阵M的不确定值，ΔB为B的不确定值，ΔC_v为C_v的不确定值，ΔD_v为D_v的不确定值，Δg_η为g_η的不确定值，C_Aη为C_A的导出变量，即C_Aη＝C_A(v_r)J^-1，式中：C_A为OBFN附加质量的科氏力和向心力矩阵，D_η为D的导出变量，即D_η＝D(v_r)J^-1，式中：D为水动力阻尼矩阵，η_r为OBFN在固定坐标系下相对于海流的位移向量；

在OBFN的动力学模型(1)的基础上，给出OBFN的跟踪误差方程：

式中

代表OBFN在固定坐标系下轨迹跟踪误差的二阶导数，

表示OBFN实际的位置与姿态角，

表示OBFN期望的位置与姿态角，v_e＝v-v_d代表OBFN在运动坐标系下速度与角速度的跟踪误差，v_d代表OBFN在运动坐标系下期望的速度与角速度，

表OBFN在运动坐标系下速度与角速度的跟踪误差的一阶导数，令

D＝-J(η)F，其中G、H、D均为简化变量；等式(7)简写如下：

2.根据权利要求1所述基于扰动观测器的预设性能海底飞行节点轨迹跟踪控制方法，其特征在于：所述步骤三中建立性能函数的具体过程为：

性能函数的定义如下所示：

式中a₁、a₂、a₃和a₄为待设计参数，k和ρ_tf＝ρ_∞为预先给定的正常数，预设参数t_f定义了性能函数(9)到达ρ_∞的截止时间，ρ_∞为根据OBFN稳态的控制精度而定的正常数。

3.根据权利要求2所述基于扰动观测器的预设性能海底飞行节点轨迹跟踪控制方法，其特征在于：所述步骤四中将步骤三的跟踪误差进行误差变换，得到变换后的误差s的具体过程为：

定义函数S_i(ε_i)：

其中，ε_i∈(-∞,+∞)称为变换误差；

根据S_i(ε_i)的特性，式(9)等价表示为：

e_i(t)＝ρ_i(t)S_i(ε_i) (11)

存在反函数：

S_i(ε_i)取等式(10)的形式，则有：

式中z_i＝e_i(t)/ρ_i(t)，z_i为第i个自由度的辅助变量；令ε_i对时间t求取一阶导数

和二阶导数

式中

通过式(13)计算获得；r_i为第i个自由度的辅助变量，

为r_i的一阶导数，e_i为OBFN在固定坐标系下的位置与姿态角误差，

为e_i的一阶导数，

为第i个自由度性能函数的一阶导数，

为第i个自由度性能函数的二阶导数；

表示第i自由度OBFN实际的位置与姿态角，

表示第i自由度OBFN期望的位置与姿态角，i＝1,2,3,4,5,6；

取误差变量s∈R⁶为如下形式：

式中：ε＝[ε₁,ε₂,ε₃,ε₄,ε₅,ε₆]^T，

为ε的一阶导数；λ＝diag[λ₁,λ₂,λ₃,λ₄,λ₅,λ₆]＞0为待设计参数；ε₁,ε₂,ε₃,ε₄,ε₅,ε₆分别代表第1自由度至第6自由度的误差变量，λ₁,λ₂,λ₃,λ₄,λ₅,λ₆分别代表第1自由度至第6自由度的待设计参数；

结合OBFN的简化形式的跟踪误差方程(7)与误差变量(16)，得到：

式中：

为s的一阶导数，

为ε的一阶导数，

为ε的二阶导数；V＝[v₁,v₂,v₃,v₄,v₅,v₆]^T，

R＝diag[r₁,r₂,r₃,r₄,r₅,r₆]，V和R为中间变量，v_i为第i个自由度的中间变量，i＝1,2,…,6，

为第i个自由度的辅助变量r_i的一阶导数；如果设计控制器u使s有界，则根据式(16)得到ε_i和

有界。

4.根据权利要求3所述基于扰动观测器的预设性能海底飞行节点轨迹跟踪控制方法，其特征在于：所述步骤五中根据步骤四得到的变换后的误差s，设计OBFN系统总不确定性观测器与预设性能轨迹跟踪控制器的具体过程为：

OBFN系统的总不确定性观测器设计如下：

式中

代表了总不确定性D的估计值，z_D是观测器的辅助变量，

是观测器的辅助变量的一阶导数，R^-1是中间变量R的逆矩阵，P,K₁和L＝K₁R是观测器的增益矩阵；

基于上述观测器的轨迹跟踪控制器如下所示：

式中K₂为增益矩阵，H^-1为简化变量H的逆矩阵。

Images