CN111880409A

CN111880409A - 一种基于数据驱动的自主水下航行器轨迹跟踪控制方法

Info

Publication number: CN111880409A
Application number: CN202010784100.3A
Authority: CN
Inventors: 马书根; 李潇涵; 任超
Original assignee: Tianjin University
Current assignee: Tianjin University
Priority date: 2020-08-06
Filing date: 2020-08-06
Publication date: 2020-11-03
Anticipated expiration: 2040-08-06
Also published as: CN111880409B

Abstract

本发明公开了一种基于数据驱动的自主水下航行器轨迹跟踪控制方法，所述方法采用双闭环控制架构，包括：基于运动学模型设计外环控制器，得到自主水下航行器在本体坐标系下的期望速度，用于内环控制器的设计；对本体坐标系下动力学模型进行离散化处理、转化，并给出AUV的动态线性化数据模型，得到离散化动力学模型与动态线性化数据模型的对应关系；基于改进投影算法设计PJM估计器，实时估计动态线性化数据模型中的PJMΦ_c(k)，并给出PJM估计初值的直观设计方法；设计离散扩张状态观测器，观测动态线性化数据模型中的总扰动增量项f(k)；设计基于MFAC与PD控制算法相结合，实时补偿ESO所观测的系统模型估计误差的内环控制器，实现对机器人本体坐标系下期望速度的跟踪。

Description

一种基于数据驱动的自主水下航行器轨迹跟踪控制方法

技术领域

本发明涉及自主水下航行器跟踪控制技术领域，尤其涉及一种数据驱动的基于模型估计误差补偿的自主水下航行器轨迹跟踪自适应控制方法。

背景技术

自主水下航行器(Autonomous Underwater Vehicle,AUV)作为执行海洋任务不可或缺的工具之一，因其无人无缆，能够自主完成任务，相比载人潜水器和遥控水下机器人，具有安全灵活、作业范围广、不依赖于外部平台等优点，广泛应用于海洋资源勘探、海底地貌观测、海洋环境监测、海油及天然气管道检测、水下救援、雷区搜索等领域。AUV应用研究的不断深入，对AUV的精确轨迹跟踪能力提出了更高的要求。

AUV的轨迹跟踪属于AUV跟踪控制研究的重要方面，要求AUV跟踪一条以时间为参数的轨迹，具有时间条件约束，即AUV必须在指定时刻运动到指定位置。目前AUV轨迹跟踪控制研究仍有很多难点问题亟待解决。首先，AUV是一种具有强不确定性、强耦合特征的复杂非线性系统，难以建立用于控制器设计的精确机理模型。此外，由于海洋环境复杂多变，海流、风浪对AUV运动产生不确定性干扰作用，并且加剧了AUV水动力学参数等模型参数的不确定性，使AUV精确轨迹跟踪控制面临很大困难。

目前国内外对AUV轨迹跟踪控制的研究多集中于基于模型的控制。为解决上述不确定性和外扰问题，自适应控制、鲁棒控制、基于观测器的控制等方案在AUV领域得以广泛应用。Sarhadi等人采用自适应的方案实现了AUV在外部干扰下的跟踪控制^[1]；Guo-chengZhang等人针对AUV系统的不确定性和时变外扰问题，基于自适应二阶滑模控制器实现了AUV的跟踪控制^[2]；杨泽文等人研究了高增益观测器结合反步控制的方法，实现了欠驱动AUV水平面轨迹跟踪控制，并通过数值仿真验证了该方法的控制效果^[3]。

然而，上述基于模型的控制方案在实际应用中存在局限性，即必须已知AUV的水动力学参数等模型信息。众所周知，AUV水动力学参数的辨识过程是非常复杂、高成本、耗时的，并且由于水动力学参数会随着水环境密度、温度梯度等实时变化，因而水动力学参数还需要实时修正，加剧了基于模型控制的困难程度。基于此，国内外少数学者开始研究AUV的基于数据驱动的无模型轨迹跟踪控制方案。Jian Gao等人采用自适应神经网络方法实现了全驱动水下机器人的视觉伺服控制^[4]；哈尔滨工程大学汤莉利用BP神经网络，提高了AUV控制系统抑制海流等环境干扰的能力^[5]。

但是，基于神经网络的非线性自适应控制方案在实际应用中是存在困难的，算法需要大量的训练过程和丰富的离线输入输出数据，尤其对于AUV系统而言，这是不现实的；并且，为保证神经网络算法的实时性，AUV系统硬件也需要达到很高的要求，这些都使得基于神经网络的自适应控制方案难以实用化。另一方面，传统的无模型自适应控制(Model-Free Adaptive Control,MFAC)架构首先基于动态线性化方法建立动态数据模型，然后利用在线输入输出数据实时估计数据模型的伪雅克比矩阵(Pseudo Jacobian Matrix,PJM)，以实现对实际系统模型的实时估计。不同于基于神经网络的控制方法，该方法具有计算量小、鲁棒性较强、易于实现等优势，仅依赖于被控系统的实时量测数据，不需要任何外在测试信号或训练过程，更适于在AUV实际系统中应用^[6]。

然而，传统的MFAC方法存在模型估计误差，尤其当系统存在速度等状态突变、较大的随机外扰和参数摄动时，模型估计效果变差，从而导致更大的模型估计误差，严重影响传统MFAC方案的控制效果。

参考文献：

[1]Sarhadi P.,Noei A R.,Khosravi A..Model reference adaptiveautopilot with anti-windup compensator for an autonomous underwater vehicle:Design and hardware in the loop implementation results[J].Applied OceanResearch,2017,62:27-36.

[2]Guo-cheng Zhang,Hai Huang,Hong-de Qin,et al.A novel adaptivesecond order sliding mode path following control for a portable AUV[J].OceanEngineering,2018,151:82-92.

[3]杨泽文,贾鹤鸣,宋文龙,等.基于高增益观测器的AUV水平面轨迹跟踪控制[J].计算机工程与应用,2017,53(11):26-30.

[4]Jian Gao,Pu-guo Wu,Bo Yang,et al.Adaptive neural network controlfor visual servoing of underwater vehicles with pose estimation[J].Journal ofMarine Science and Technology,2017,22:470–478.

[5]汤莉.AUV神经网络水平面航迹跟踪控制研究[D].哈尔滨：哈尔滨工程大学,硕士学位论文,2009.

[6]Zhong-sheng Hou,Shang-tai Jin.Data-driven model-free adaptivecontrol for a class of MIMO nonlinear discrete-time systems[J].IEEETransactions on Neural Networks,2011,22(12):2173-2188.

发明内容

为避免现有控制技术的不足之处，以及填补MFAC应用于多入多出旋转机器人系统的空白，本发明的目的在于提供一种具备模型估计误差在线实时补偿的，具有抗扰、抗不确定性优势，面向AUV轨迹跟踪控制的新型无模型自适应控制器设计方案，以及PJM初值等关键参数的直观设计方法，实现在不需要任何系统模型参数信息的情况下AUV在复杂海洋环境中的精确轨迹跟踪，提高跟踪精度和控制系统的鲁棒性，详见下文描述：

一种基于数据驱动的自主水下航行器轨迹跟踪控制方法，所述方法采用双闭环控制架构，包括：

基于运动学模型设计外环控制器，得到自主水下航行器在本体坐标系下的期望速度，用于内环控制器的设计；

对本体坐标系下动力学模型进行离散化处理、转化，并给出AUV的动态线性化数据模型，得到离散化动力学模型与动态线性化数据模型的对应关系；

基于改进投影算法设计PJM估计器，实时估计动态线性化数据模型中的PJMΦ_c(k)，并给出PJM估计初值的直观设计方法；

设计离散扩张状态观测器，观测动态线性化数据模型中的总扰动增量项f(k)；

设计基于MFAC与PD控制算法相结合，实时补偿ESO所观测的系统模型估计误差的内环控制器，实现对机器人本体坐标系下期望速度的跟踪。

本发明提供的技术方案的有益效果是：

针对AUV强不确定性、强耦合、复杂外界扰动等特征带来的精确轨迹跟踪控制难题，本发明设计了一种数据驱动的基于扩张状态观测器实现模型估计误差补偿的新型无模型自适应控制方法，本发明具有如下特点：

1、更具有实用价值。本发明采用数据驱动设计，不需要任何AUV系统的模型参数信息，且不需要任何外在测试信号或训练过程，仅依赖于在线输入输出量测数据，即可实现AUV的精确轨迹跟踪控制。该方法省去了复杂、高成本、耗时的AUV水动力学参数辨识及修正过程，相比传统的基于模型的控制方法以及基于神经网络的自适应方法，更适于在AUV实际系统中应用。

2、跟踪精度高，鲁棒性强。传统的MFAC方法存在模型估计误差，尤其当系统出现速度等状态突变、较大的随机外扰和参数摄动时，模型估计效果变差，从而导致更大的模型估计误差，会严重影响传统MFAC方案的控制效果。本发明通过采用扩张状态观测器实时估计并补偿MFAC架构下的系统模型估计误差，从而大幅提高了AUV无模型轨迹跟踪控制的精度，以及面向状态突变、参数不确定、时变外扰和测量噪声下的算法鲁棒性，并减少了PJM的重置次数。

3、填补MFAC应用于旋转机器人系统的空白。MFAC方法要求PJM中所有元素符号始终保持不变，这使得MFAC方法不能直接应用于带有旋转的机器人系统，并且目前国内外并无相关研究。本发明通过采用双闭环控制架构，在非惯性机器人本体坐标系下设计MFAC方法，解决了因机器人旋转带来的MFAC设计难题。

4、本发明给出了针对AUV系统的、直观的、带有明确物理意义的PJM估计初值设置方法，也对其他系统PJM估计初值给定提供了参考。PJM初值设置对MFAC的控制性能至关重要，不恰当的初始设置会使控制系统无法工作并且非常不稳定。然而目前PJM初始设置很困难，往往依靠经验和大量的调试。本发明通过对AUV机理模型的离散化处理、转化，得到AUV机理模型与动态线性化数据模型的对应关系，并结合受力分析，最终给出了PJM初值设置方法。

附图说明

图1是本发明中自主水下航行器示意图及坐标系描述示意图；

图2是本发明所设计的控制系统的结构框图；

图3是自主水下航行器正方形轨迹跟踪控制仿真效果图；

图中：

(a)是正方形参考轨迹及轨迹跟踪曲线；(b)是各方向轨迹跟踪误差变化曲线；

(c)是各方向机器人本体坐标系中速度跟踪曲线；(d)是控制输入变化曲线；

(e)是变结构扩张状态观测器对总扰动增量的估计值变化曲线；(f)是PJM估计值变化曲线。

图4是突然加入持续随机外扰条件下，自主水下航行器双纽线轨迹跟踪控制仿真效果图。

图中：

(a)是双纽线参考轨迹及轨迹跟踪曲线；(b)是各方向轨迹跟踪误差变化曲线；

(e)是变结构扩张状态观测器对总扰动增量的估计值变化曲线；(f)是实际外部扰动变化曲线。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面对本发明实施方式作进一步地详细描述。

本发明采用的无模型自适应控制(Model-Free Adaptive Control,MFAC)方法，具有计算量小、鲁棒性较强、易于实现等优势，仅依赖于被控系统的实时量测数据，不需要任何外在测试信号或训练过程，更适于在AUV实际系统中应用^[6]。但是，传统的MFAC方法存在模型估计误差，尤其当系统存在速度等状态突变、较大的随机外扰和参数摄动时，模型估计效果变差，从而导致更大的模型估计误差，严重影响传统MFAC方案的控制效果。

针对此问题，本发明通过采用扩张状态观测器(Extended State Observer,ESO)，实时估计并补偿MFAC架构下的系统模型估计误差，从而大幅提高了AUV无模型轨迹跟踪控制的精度，以及面向状态突变、参数不确定、时变外扰和测量噪声下的算法鲁棒性，并减少了PJM的重置次数。此外，MFAC方法要求PJM矩阵中所有元素符号始终保持不变，这使得MFAC方法不能直接应用于带有旋转的机器人系统，并且目前国内外并无相关研究。

本发明通过采用双闭环控制架构，在非惯性机器人坐标系下设计MFAC方法，解决了因机器人旋转带来的MFAC设计难题。PJM初值设置对MFAC的控制性能至关重要，然而PJM初始设置很困难，目前主要依靠经验和大量的调试，国内外研究中也并没有对于PJM初值设置方法的讨论。基于此，本发明利用AUV机理模型结构转化和受力分析，给出了针对AUV系统的、直观的、带有明确物理意义的PJM初值设置方法，解决MFAC方法在具体实施中的参数整定难题，也对其他系统PJM初值的给定方案提供了参考。

本发明的主要目的是提供一种具备模型估计误差在线实时观测并补偿的，具有抗扰、抗不确定性优势的，面向AUV精确轨迹跟踪控制的新型无模型自适应控制器设计方法。实现在不需要任何系统模型参数信息情况下，AUV在复杂海洋环境中的精确轨迹跟踪，提高跟踪精度和控制系统的鲁棒性；填补MFAC应用于多入多出旋转机器人系统的研究空白；并给出针对AUV系统的、直观的、带有明确物理意义的PJM初值设计方法，解决具体实施中的参数整定难题。

本发明采用的技术方案是一种具有模型估计误差补偿特征的、扩张状态观测器与无模型自适应控制器结合的自主水下航行器轨迹跟踪控制方法，具体步骤如下：

步骤1)建立AUV的运动学和动力学模型；

定义世界惯性坐标系{W}和机器人本体坐标系{B}，通过构造坐标变换矩阵，得到AUV的运动学模型为：

其中，η＝[x,y,ψ]^T表示世界坐标系{W}下AUV的位置和姿态向量，

表示世界坐标系下AUV的线速度和偏航角速度向量，v＝[u,v,r]^T表示本体坐标系{B}下AUV的线速度和偏航角速度向量，如图1所示。J(η)表示从本体坐标系到世界坐标系的坐标变换矩阵。x,y表示AUV在世界坐标系下的位置，ψ表示AUV的偏航角，u,v表示本体坐标系下AUV的线速度，r表示AUV的偏航角速度。

AUV的动力学模型为：

其中，M表示惯性质量矩阵，C(v)表示科氏力和向心力矩阵，D(v)表示水动力阻尼矩阵，G(η)表示由重力和浮力产生的恢复力项，d(η,v)表示由海流、风浪等外扰引起的扰动项，

表示本体坐标系{B}下AUV的线加速度和偏航角加速度向量，τ＝[F_u,F_v,M_I]^T表示AUV的控制输入，即本体坐标系{B}下的牵引力及偏航力矩。

其中，F_u表示本体坐标系下AUV纵荡方向的牵引力，F_v表示本体坐标系下横荡方向的牵引力，M_I表示艏摇方向的偏航力矩。

假设AUV为质量均匀的刚体，且重心与浮心重合，三个平面均对称，忽略横滚和俯仰运动，则系统动力学模型(2)可进一步简化为：

其中，H(v)＝C(v)+D(v),

其中，m为AUV的质量，I_z为转动惯量，

为AUV本体坐标系下纵荡、横荡、艏摇三个方向的附加质量和转动惯量，X_u,Y_v,N_r为AUV本体坐标系下三个方向的线性阻力系数，D_u,D_v,D_r为AUV本体坐标系下三个方向的二次阻力系数。式(3)将作为AUV的动力学模型进行后续步骤3)中的模型转化。

步骤2)采用双闭环控制架构设计AUV轨迹跟踪控制器，基于AUV的运动学模型(1)设计外环控制器，得到AUV在本体坐标系下的期望速度，即内环控制器的控制期望，用于步骤3)内环控制器设计；

具体的，可以采用基于PD的外环控制器：

其中，v_d表示内环期望的本体坐标系下的速度，e_η(k)＝η_d(k)-η(k)，

K_p1＝diag[k_p1,k_p1,k_p1]，K_d1＝diag[k_d1,k_d1,k_d1]。

其中，η_d(k)为k时刻世界坐标系下AUV期望的位置和姿态向量，即轨迹追踪的期望。ψ_d(k+1)为k+1时刻AUV期望的偏航角，η(k)为k时刻世界坐标系下AUV实际的位置和姿态向量，

为k时刻世界坐标系下AUV期望的线速度和偏航角速度向量，

为k+1时刻世界坐标系下AUV期望的线速度和偏航角速度向量。J(ψ(k))为坐标转换矩阵，v(k)为k时刻本体坐标系下AUV实际的线速度和偏航角速度向量。k_p1为外环控制器的比例控制增益，k_d1为外环控制器的微分控制增益，K_p1为外环控制器的比例控制增益矩阵，K_d1为外环控制器的微分控制增益矩阵，e_η(k)为k时刻的轨迹追踪误差。

采用双闭环控制架构的优势在于，该方案使得MFAC在机器人非惯性本体坐标系下设计，首次解决了MFAC不能直接应用于带有旋转的机器人系统的设计难题。填补了MFAC应用于旋转机器人系统的空白。具体解释详见步骤3)环节1。

步骤3)设计扩张状态观测器与无模型自适应控制器相结合的数据驱动内环控制器。

该部分是本发明的重点内容，具体分为以下五个环节：

(1)环节1.

对本体坐标系下AUV动力学模型(3)进行离散化处理、转化，并给出AUV的动态线性化数据模型，得到AUV离散化动力学模型与动态数据模型的对应关系，用于后续MFAC架构设计以及PJM估计初值参数的调节。

首先对AUV动力学模型(3)进行离散化，具体推导步骤如下：

将时间t离散成时间点：

利用前向差分法，

h表示采样时间，得到AUV离散化模型为：

v(k+1)＝hM^-1τ(k)+(I-hM^-1H(v(k)))v(k)-hM^-1d(k). (6)

然后，为了得到AUV动力学模型式(3)与MFAC架构中的动态数据模型^[6]的对应关系，将AUV离散化模型式(6)转化为类似动态线性化数据模型的形式，具体推导如下：

Δv(k+1)＝v(k+1)-v(k)＝[hM^-1τ(k)+(I-hM^-1H(v(k)))v(k)-hM^-1d(k)]-[hM^-1τ(k-1)+(I-hM^-1H(v(k-1)))v(k-1)-hM^-1d(k-1)]

＝hM^-1Δτ(k)+(I-hM^-1H(v(k)))v(k)-(I-hM^-1H(v(k-1)))v(k-1)-hM^-1Δd(k).

令

得到：

其中，ζ(k)、d₁(k)、

均为模型转化过程中的中间参量。Δ在本发明中均代表两个时刻物理量的变化量。

AUV系统可等价表示为紧格式动态线性化数据模型^[6](本领域公知的技术术语)：

Δv(k+1)＝Φ_c(k)Δτ(k)+f(k). (8)

其中，AUV系统的PJM

为对角占优矩阵，满足

为一个上界参数，且Φ_c(k)中所有元素符号始终保持不变。f(k)定义为总扰动增量，即模型估计误差增量。

值得注意的是，通过将AUV动力学模型与动态线性化数据模型相对应，可以发现动态线性化数据模型式(8)中的PJMΦ_c(k)与AUV离散动力学模型(7)中的hM^-1近似对应。由AUV动力学模型得知，hM^-1是一个对角常数矩阵，因此机器人本体坐标系下的AUV系统满足MFAC架构中对于PJMΦ_c(k)为对角占优矩阵以及Φ_c(k)中所有元素符号始终保持不变的假设，即可以应用MFAC算法。此外，PJMΦ_c(k)描述了Δτ(k)到Δv(k+1)的映射关系，根据受力分析可得，v与τ方向一致，即τ对v始终是正向作用，也可以说明Φ_c(k)中所有元素符号始终保持不变，且对角线元素符号为正。因而对于旋转机器人系统而言，采用双闭环控制架构，在机器人本体坐标系下设计MFAC内环控制器的思路合理，本发明填补了MFAC应用于多入多出旋转机器人系统的空白。

将AUV离散化动力学模型与动态线性化数据模型相对应的优势在于，一是证明了对于旋转AUV系统而言，在本体坐标系下设计MFAC内环控制器的思路合理，满足假设；同时，为PJM估计初值

的调参提供了指导，具体设计详见环节2。综上，本发明融合了基于模型和数据驱动两种方案的优点，改善了纯数据驱动方案在设计和参数调节中的局限性。

(2)环节2.

基于改进投影算法设计PJM估计器，实时估计动态线性化数据模型中的PJMΦ_c(k)，并给出PJM估计初值的直观设计方法，为控制器在实际应用中的调参提供指导。

首先设计参数估计准则函数如下：

其中，μ＞0为权重因子，用于惩罚PJM估计值的过大变化，

表示k-1时刻系统PJMΦ_c(k-1)的估计矩阵。

然后将Φ_c(k)Δτ(k-1)在

处作一阶泰勒展开，得到：

利用改进投影算法极小化准则函数，得到PJM最优估计为：

其中，

为k时刻系统PJMΦ_c(k)的估计矩阵，I表示单位矩阵。

由于上述算法包含矩阵求逆运算，会带来耗费计算资源、增大系统延时等问题。基于此，通过简化，得到不含矩阵求逆运算的PJM估计算法为：

其中，η∈(0,2]是步长因子，

为总扰动增量f(k-1)的估计值。

为了使PJM估计算法具有更强的对时变参数的跟踪能力，设计

重置机制如下：

如果

或

或

如果

或

其中，

是

的初值，

是

的初值，

表示PJM估计矩阵

的对角线元素，

表示PJM估计矩阵

的非对角线元素。α,b₁,b₂均为设计参数，用于限制

的范围，满足α≥1,b₂＞2b₁(2α+1)。sign(·)表示符号函数。

下面给出针对AUV系统的PJM估计初值

的设置方法：

环节1中指出，动态线性化数据模型中的PJMΦ_c(k)与AUV实际动力学模型中的hM^-1近似对应。由AUV动力学模型得知，hM^-1是一个对角矩阵，因此PJM初值

可以设计为对角矩阵。此外，PJMΦ_c(k)描述了Δτ(k)到Δv(k+1)的映射关系，根据受力分析可得，v与τ方向一致，即τ对v是正向作用，因此PJM初值

可以设计为正定对角矩阵。

(3)环节3.

设计离散扩张状态观测器，观测动态线性化数据模型中的总扰动增量项f(k)。本发明给出两种离散ESO设计，一种是传统线性ESO，一种是变结构ESO。

首先介绍针对AUV动态线性化模型的传统线性ESO的设计，具体设计如下：

定义状态变量z₁(k)＝v(k),z₂(k)＝f(k)，则AUV系统的状态空间描述为：

其中，f(k)为动态线性化数据模型(8)中的总扰动增量项。

则传统ESO的表达式为：

其中，

表示传统ESO观测的z_i(k)的估计值，观测误差

β_0i(i＝1,2)为传统ESO的增益矩阵，

ω_o为观测器的带宽，且ω_o＞0，是扩张状态观测器仅有的一个需要调节的参数。模型估计误差增量f(k)的估计值为

为抑制传统ESO的初始微分峰值现象，避免系统调节过程中出现振荡，缩短调节时间，提高动态性能，本发明给出一种面向AUV动态线性化模型的离散变结构ESO设计。具体设计如下：

面向AUV动态线性化模型的变结构ESO的表达式为：

其中，

β_i(i＝1,2)为变结构ESO的增益矩阵，具体设计为：

其中，ω_o为观测器的带宽，且ω_o＞0,hω_o＜2。模型估计误差增量f(k)的估计值为

下面给出e_o2(k)和观测增益矩阵β_i的详细推导过程。

e_o2(k)的具体推导过程如下：

因为

根据AUV状态空间表达式和变结构ESO表达式，得到：

z₁(k+1)＝z₁(k)+Φ_c(k)Δτ(k)+z₂(k)， (19)

最后化简得：

观测增益矩阵β_i的具体推导过程如下：

定义PJM的估计误差为

则变结构ESO的误差状态方程如下：

即：

令e_o(k)＝[e_o1(k),e_o2(k)]^T，则：

其中，

式中，I表示单位向量，0表示零向量。

然后对误差状态方程进行单通道分析，

的特征多项式为：

p(λ)＝λ²+(β₁+β₂)λ+β₁β₂＝(β₁+λ)(β₂+λ). (25)

将闭环传递函数的极点都配置在-ω_o，即：p(λ)＝(β₁+λ)(β₂+λ)＝(λ+ω_o)².λ表示矩阵A_es的特征值。得到β₁＝ω_o,β₂＝ω_o.即：

(4)环节4.

设计基于MFAC算法，结合PD控制器，并能实时补偿ESO所观测的系统模型估计误差的内环控制器，实现对机器人本体坐标系下期望速度(由步骤2)外环控制器所得)的跟踪。具体设计如下：

首先设计控制输入准则函数如下：

J(τ(k))＝||v_d(k+1)-v(k+1)||²+λ||τ(k)-τ(k-1)||². (26)

其中，λ＞0为权重因子，用于惩罚控制输入量的过大变化，v_d(k+1)是通过步骤2)得到的内环的控制期望。

然后，求解最优化问题，得到控制输入最优解为：

其中，

表示环节2得到的PJM的估计值，

表示环节3得到的总扰动增量项f(k)的估计值。

由于上述算法也包含矩阵求逆运算，通过简化，得到不含矩阵求逆运算的控制算法：

其中，ρ∈(0,1]为步长因子。

为提高控制系统性能，对传统MFAC引入PD设计，最终得到内环控制器设计具体为：

其中，控制误差e_v(k)＝v_d(k+1)-v(k)，λ＞0,ρ∈(0,1],

其中，k_p2为内环控制器的比例控制增益，k_d2为内环控制器的微分控制增益。

(5)环节5.证明内环控制系统的稳定性，分为三部分：

首先证明PJM估计的收敛性。借鉴传统MFAC架构的稳定性分析方法，可以证明PJM估计值

和PJM估计误差

均有界。

然后证明变结构扩张状态观测器的收敛性。借鉴传统离散ESO的稳定性分析方法，可以证明

hω_o＜2，观测器观测误差e_oi有界，同时模型估计误差增量即总扰动增量项估计值

有界。

最后证明内环控制系统的收敛性。借鉴传统MFAC的稳定性分析方法，可以证明系统跟踪误差序列有界，并且闭环控制系统有界输入有界输出稳定。

步骤4.仿真验证与分析。

为验证本发明所设计的控制算法的有效性，以MATLAB作为仿真平台，采用真实系统Falcon AUV的实验辨识模型及参数，进行AUV轨迹跟踪控制仿真验证，并与传统MFAC控制仿真结果进行对比分析。

下面结合仿真举例和结果附图，在控制系统存在状态突变、随机扰动和测量噪声的条件下，验证本发明所提方法的有效性。

Falcon实验辨识参数，即仿真中模型参数取值如下：AUV的质量m＝116(kg)，AUV偏航转动惯量I_z＝13.1(kg·m²)，纵荡、横荡、艏摇三个方向的附加惯性质量及转动惯量分别为：

水动力参数的线性阻力项分别为X_u＝26.9,Y_v＝35.8,N_r＝3.5,水动力参数的二次阻力项分别为D_u＝241.3,D_v＝503.8,D_r＝76.9.由于输入饱和限制，AUV实际系统控制输入范围F_u,max＝-F_u,min＝2000(N),F_v,max＝-F_v,min＝2000(N),M_I,max＝-M_I,min＝1000(Nm).仿真中的速度、位姿输出数据均加入信噪比90(dB)的加性高斯白噪声。

本发明方法中控制器各参数取值如下：在外环控制器中，比例控制增益K_p1＝diag[1,1,1],微分控制增益K_d1＝diag[0.5,0.5,0.5].

在PJM估计算法中,α＝1.25，b₁＝0.1，b₂＝0.8,μ＝50，η＝1,PJM估计初值

根据步骤3)环节2中给出的PJM初值调参方法设计。在变结构ESO中，观测器带宽ω_o＝15(rad/s).最后在MFAC控制算法中，ρ＝1，λ＝0.5，比例控制增益K_p2＝diag[30,30,30],微分控制增益K_d2＝diag[20,20,20].仿真采样时间h＝0.01(s)。

其中，diag[·]表示对角矩阵。值得注意的是，用于对比的传统MFAC与本发明方法所用控制参数相同，并且两组仿真中控制参数保持不变。

仿真举例1：验证参考速度突变情况下本发明方法的控制效果。

采用40(s)的正方形参考轨迹如下：

AUV的初始位姿和初始速度分别为：

[x(0),y(0),ψ(0)]^T＝[0,0,0]^T,[u(0),v(0),r(0)]^T＝[0.5,0,0]^T.

每经过10秒，正方形轨迹会有一个直角拐点，此时AUV的期望速度和运动方向会发生突变。正方形轨迹跟踪仿真结果如图3(a)、3(b)、3(c)、3(d)、3(e)、3(f)所示。从图3(a)、3(b)、3(c)可以看出，在AUV系统状态突变的条件下，采用本发明的控制方案比传统MFAC有更高的跟踪精度、更快的收敛速度和更强的鲁棒性，尤其是在正方形轨迹的四个拐角处，对比效果非常明显。图3(d)显示了控制输入随时间的变化曲线。图3(e)说明变结构扩张状态观测器可以实时观测模型估计误差增量；图3(f)显示了PJM估计值随时间的变化曲线。

仿真举例2：验证随机扰动情况下本发明方法的控制效果。

采用40(s)的双纽线参考轨迹如下：

在仿真第10秒时，AUV本体坐标系下纵荡、横荡、艏摇三个通道分别同时加入三种不同类型的扰动：d＝[d_u,d_v,d_r]。其中，d_u设计为低频随机扰动，幅值范围[-400(N),400(N)]；d_v设计为较大幅值的正弦扰动，d_v＝500sin(t)(N)；d_r设计为定常扰动，d_r＝400(N)。AUV的初始位姿和初始速度分别为：

[x(0),y(0),ψ(0)]^T＝[0,0,0]^T,[u(0),v(0),r(0)]^T＝[0.3,0.6,π/15]^T.

双纽线轨迹跟踪仿真结果如图4(a)、4(b)、4(c)、4(d)、4(e)、4(f)所示。从图4(a)、4(b)、4(c)可以看出，在AUV系统突然受到持续随机外部扰动的条件下，采用本发明的控制方案比传统MFAC有更高的跟踪精度、更快的收敛速度和更强的鲁棒性，尤其是在第10秒突然加入外扰时，对比效果非常明显。图4(d)显示了控制输入随时间的变化曲线。图4(e)和4(f)对比，说明变结构扩张状态观测器可以实时且准确地估计系统总扰动增量，从而有效提高系统的控制性能。

本发明实施例对各器件的型号除做特殊说明的以外，其他器件的型号不做限制，只要能完成上述功能的器件均可。

本领域技术人员可以理解附图只是一个优选实施例的示意图，上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims

1.一种基于数据驱动的自主水下航行器轨迹跟踪控制方法，其特征在于，所述方法采用双闭环控制架构，包括：

2.根据权利要求1所述的一种基于数据驱动的自主水下航行器轨迹跟踪控制方法，其特征在于，所述基于运动学模型设计外环控制器具体为：

其中，v_d表示内环期望的AUV在本体坐标系下的速度，e_η(k)＝η_d(k)-η(k)，

K_p1＝diag[k_p1,k_p1,k_p1]，K_d1＝diag[k_d1,k_d1,k_d1]。

η_d(k)为k时刻世界坐标系下AUV期望的位置和姿态向量，即轨迹追踪的期望，ψ_d(k+1)为k+1时刻AUV期望的偏航角，η(k)为k时刻世界坐标系下AUV实际的位置和姿态向量，

为k时刻世界坐标系下AUV期望的线速度和偏航角速度向量，J(ψ(k))为坐标转换矩阵，v(k)为k时刻本体坐标系下AUV实际的线速度和偏航角速度向量，k_p1为外环控制器的比例控制增益，k_d1为外环控制器的微分控制增益，K_p1为外环控制器的比例控制增益矩阵，K_d1为外环控制器的微分控制增益矩阵，e_η(k)为k时刻的轨迹追踪误差。

3.根据权利要求1所述的一种基于数据驱动的自主水下航行器轨迹跟踪控制方法，其特征在于，所述离散化动力学模型与动态线性化数据模型的对应关系为：

动态线性化数据模型中的PJMΦ_c(k)与AUV离散化动力学模型中的hM^-1近似对应，hM^-1是对角常数矩阵，Φ_c(k)满足对角占优矩阵，且所有元素符号始终保持不变。

4.根据权利要求1所述的一种基于数据驱动的自主水下航行器轨迹跟踪控制方法，其特征在于，所述给出PJM估计初值的直观设计方法具体为：

将PJM估计初值