水下自主航行器深度和纵向解耦控制方法及系统

摘要

本发明公开了一种基于模型预测控制的水下自主航行器深度和纵向解耦耦控制方法及系统。本发明的基于模型预测控制的水下自主航行器深度和纵向解耦控制方法和系统，将深度和纵向进行解耦，大大提高了计算效率，降低计算负担，显著提高了控制的实时性。

权利要求

1.一种基于模型预测控制的水下自主航行器深度和纵向解耦控制方法，其特征在于，包括建模步骤S0，所述建模步骤S0对水下自主航行器进行动力学建模，使得所述模型符合水下自主航行器的动力学特性，且适于采用于模型预测控制，所述建模步骤S0具体包括：步骤S01，对于水下自主航行器建立深度和纵向速度耦合的系统模型如下：各符号表示的意义如下：

z——深度；

u——纵向速度；

θ——俯仰角；

q——俯仰角速度；

X|u|u——计算流体阻力的系统水动力系数；

——计算横向惯性力矩的加速度水动力系数；

——计算纵向惯性力的加速度水动力系数；

Mq——俯仰角速度水动力系数；

Xwq——垂向速度俯仰角速度水动力系数；

Mw——垂向速度水动力系数；

——舵角水动力系数；

Iy——沿横向轴的转动惯量；

zg——重心在垂直方向的位置；

zb——浮力中心在垂直方向的位置；

W——重力；

B——浮力；

δs——与深度控制相关的舵角，即垂直舵角；

T——纵向推力；

m——水下自主航行器的质量；

步骤S02，将上述系统模型分解为纵向速度控制子系统模型和深度控制子系统模型，纵向速度控制子系统模型为：深度控制子系统模型为：

步骤S03，确定水下自主航行器系统状态向量y、水下自主航行器的深度控制子系统状态向量x，水下自主航行器的速度控制子系统状态向量u，其中，水下自主航行器的系统状态向量y＝[z,θ,q,u]，水下自主航行器的深度控制子系统状态向量x＝[z,θ,q]，水下自主航行器的速度控制子系统状态向量u＝[u]，用x(k)，y(k)，u(k)分别表示上述状态向量在任意离散时刻k的值，步骤S04，用采样时间Δt离散化上述状态向量，步骤S04包括下述步骤：步骤S041，将深度控制子系统状态向量用采样时间Δt离散化，得到：即：x(k+1)＝f[x(k),u(k),δs(k)]，步骤S042，将速度控制子系统状态向量用采样时间Δt离散化，得到：即：u(k+1)＝g[x(k),u(k),T(k)]，步骤S043，将系统状态向量用采样时间Δt离散化，得到：即：y(k+1)＝h[x(k),u(k),δs(k),T(k)]。

2.如权利要求1所述的基于模型预测控制的水下自主航行器深度和纵向解耦控制方法，其特征在于，利用建模步骤S0中建立的模型以及离散化的状态向量表达式，对解耦的纵向速度控制子系统模型和深度控制子系统模型，分别以舵角δs和纵向推力T为控制输入量，构建优化问题，分别获取舵角δs和纵向推力T的最优控制输入，并以所述最优控制输入控制水下自主航行器。

3.如权利要求1所述的基于模型预测控制的水下自主航行器深度和纵向解耦控制方法，其特征在于，

采样水下自主航行器的参数，基于采样参数和离散公式，获取水下自主航行器在未来最大预测步长Np个时刻内的预测状态，把上一离散时刻求得的预测最优状态序列内的纵向速度序列作为深度控制子系统中纵向速度的值，对深度控制子系统进行优化来求解未来Np个时刻内的最优控制舵角输入序列，

同时，把上一离散时刻求得的预测最优状态序列内的俯仰角序列作为纵向速度控制子系统中的对应量的值，对纵向速度控制子系统进行优化来求解未来Np个时刻内的最优控制纵向推力输入序列，

以所述最优控制舵角输入序列的第一个值、所述最优控制纵向推力输入序列的第一个值来控制水下自主航行器，输入序列的其他值用于生成水下自主航行器在后续未来Np个时刻的预测最优状态序列，并重新采样再获取预测最优状态序列，如此循环，直至完成控制任务。

4.如权利要求1‑3中任一项所述的基于模型预测控制的水下自主航行器深度和纵向解耦控制方法，其特征在于，包括下述的控制步骤：步骤S1，在t＝t0的起始时刻，采样水下自主航行器的状态，并对水下自主航行器系统进行初始化，

采样的参数如下：

y(0)＝[z(0),θ(0),q(0),u(0)]，或者，表示为y(t)＝[z(t),θ(t),q(t),u(t)]，其中，t＝t0，对水下自主航行器系统的初始化包括：获取水下自主航行器在未来Np个时刻内的预测最优状态 及

其中，Np为最大预测步长，为大于等于1的自然数，所述Np个时刻为t＝t0+1*Δt,t0+2*Δt,…,t0+k*Δt,…,t0+(Np‑1)*Δt,t0+Np*Δt，Δt为采样周期或采样时间间隔，表示在时刻t对时刻t之后k步的预测最优值，或(t+k*Δt)时刻的预测最优值，

其中， 分别为水下自主航行器的深度、俯仰角、俯仰角速度、和纵向速度的预测最优值，

步骤S2，对深度控制子系统和纵向速度控制子系统分别构建优化问题，即构建对深度控制子系统的最优控制优化问题Pd(t)，以及构建对纵向速度控制子系统的最优控制优化问题Pl(t)，

对于Pd(t)，把 作为深度控制子模型中时刻t之后NP个离散时刻纵向速度的预测值，以舵角输入作为变量，以舵角最小、与预测最优状态 的状态偏离最小、以及/或者与预设约束状态之间的偏差最小为优化目的，以便获取舵角最优控制输入序列，对于Pl(t)，把 中的俯仰角序列作为纵向速度控制子模型中时刻t之后NP个离散时刻俯仰角的预测值，以纵向推力输入作为变量，以纵向推力最小、与预测最优状态 的状态偏离最小、以及/或者与预设约束状态(参考轨迹)之间的偏差最小为优化目的，以便获取纵向推力最优控制输入序列，步骤S3，求解优化问题，获取最优控制输入序列，具体地，通过求解优化问题Pd(t)，获取舵角最优控制输入序列，获取的舵角最优控制输入序列＝[δs(0|t),δs(1|t),…,δs(k|t),…,δs(Np‑1|t)]，通过求解优化问题Pl(t)，获取纵向推力最优控制输入序列，获取的纵向推力最优控制输入序列＝[T(0|t),T(1|t),…,T(k|t),…,T(Np‑1|t)]，步骤S4，将步骤S3获取的舵角最优控制输入序列的第一个值δs(0|t)和纵向推力最优控制输入序列的第一个值T(0|t)作为真实控制量作用于水下自主航行器，基于步骤S3获取的本采样周期的最优控制舵角输入值序列的值，确定下一采样周期的舵角预设序列如下，

* * * *

[δs (0|t+Δt),δs (1|t+Δt),…,δs (k|t+Δt),…,δs (Np‑1|t+Δt)]＝[δs(1|t),…,δs(k|t),…,δs(Np‑1|t),δs(Np|t)＝设定舵角值]，基于步骤S3获取的本采样周期的最优控制纵向推力输入值序列的值，确定下一采样周期的纵向推力预设序列如下，* * * *

[T(0|t+Δt),T (1|t+Δt),…,T (k|t+Δt),…,T(Np‑1|t+Δt)]＝[T(1|t),…,T(k|t),…,T(Np‑1|t),T(Np|t)＝设定纵向推力值]，步骤S5，经过一个采样周期后，t＝t+Δt，再次采样水下自主航行器状态，得到采样的参数y(t)＝[z(t),θ(t),q(t),u(t)]，基于采样的参数、当前采样周期的预设控制输入序列，利用离散公式y(k+1)＝h[x(k),u(k),δs(k),T(k)]，获取水下自主航行器在未来Np个时刻内的预测最优状态 及然后，转步骤S2。

5.如权利要求4所述的基于模型预测控制的水下自主航行器深度和纵向解耦控制方法，其特征在于，

在步骤S1中，直接给定与起始时刻对应的采样周期的预设控制输入序列，然后，基于采样的参数、直接给定的预设控制输入序列，利用离散公式y(k+1)＝h[x(k),u(k),δs(k),T(k)]，获取水下自主航行器在未来Np个时刻内的预测最优状态 及 或者

在步骤S1中，根据经验值，或者根据所要跟踪的轨迹，直接给定水下自主航行器在未来Np个时刻内的预测最优状态 及

6.如权利要求4所述的基于模型预测控制的水下自主航行器深度和纵向解耦控制方法，其特征在于，

在步骤S2中，构建对深度控制子系统的最优控制优化问题Pd(t)如下：其中， 为待优化的代价函数，为待优化的代价函数的表达式，为预测舵角输入，在最优控制优化问题Pd(t)的优化过程中为待优化的变量，为t时刻之后的第k个预测舵角输入，是待优化的变量，通过变化 的取值，从而计算最小的代价函数，xp为深度控制子系统状态向量的预测值，在优化过程中，其值根据 而变化，xa为深度控制子系统状态向量的假设值或比较基准值，xa为深度控制子系统状态向量的假设值或比较基准值，xr为所述控制方法所跟踪的跟踪轨迹yr的子集，跟踪轨迹yr(t)＝[zr(t),θr(t),qr(t),ur(t)]＝[xr(t),ur(t)],跟踪轨迹yr(t)包含了每个时刻水下自主航行器的理想状态，其中，xr(k|t)＝xr(t+k)，ur(k|t)＝ur(t+k)，上述优化问题Pd(t)应满足：xp(0|t)＝x(t),其中，

为舵角输入约束，

为水下自主航行器的垂向状态约束，代表对控制输入的惩罚，

代表对预测状态与参考状态误差的惩罚，代表预测状态与跟踪轨迹误差的惩罚Rd、Fd和Gd为权重系数，对于矩阵X，定义P范数：

7.如权利要求4所述的基于模型预测控制的水下自主航行器深度和纵向解耦控制方法，其特征在于，

在步骤S2中，构建对纵向速度控制子系统的最优控制优化问题Pl(t)如下：其中， 为待优化的代价函数，p

ll(up,T ,ua,ur；k|t)为待优化的代价函数的表达式，p

T为预测纵向推力输入，在最优控制优化问题Pl(t)的优化过程中为待优化的变量，p p

T (k|t)为纵向速度控制子系统在t时刻之后的第k个预测纵向推力输入，T (k|t)是待p

优化的变量，通过变化T(k|t)的取值，从而计算最小的代价函数，p

up为纵向速度控制子系统状态向量的预测值，在优化过程中，其值根据T而变化，ua为纵向速度控制子系统状态向量的假设值或比较基准值，ur为所述控制方法所跟踪的跟踪轨迹yr的子集，跟踪轨迹yr(t)＝[zr(t),θr(t),qr(t),ur(t)]＝[xr(t),ur(t)],跟踪轨迹yr(t)包含了每个时刻水下自主航行器的理想状态，其中，xr(k|t)＝xr(t+k)，ur(k|t)＝ur(t+k)，上述优化问题Pl(t)应满足：up(0|t)＝u(t),其中，

为纵向速度控制子系统的输入约束，为水下自主航行器纵向状态约束，代表对控制输入的惩罚，

代表对预测状态与参考状态误差的惩罚，代表预测状态与跟踪轨迹误差的惩罚，Rl、Gl和Fl是权重系数，对于矩阵或参数X，定义P范数：

8.一种基于模型预测控制的水下自主航行器深度和纵向解耦控制系统，其特征在于，所述基于模型预测控制的水下自主航行器深度和纵向解耦控制系统包括数据解耦计算单元，

所述数据解耦计算单元采用的纵向速度控制子系统模型为：深度控制子系统模型为：

各符号表示的意义如下：

z——深度；

u——纵向速度；

θ——俯仰角；

q——俯仰角速度；

X|u|u——计算流体阻力的系统水动力系数；

——计算横向惯性力矩的加速度水动力系数；

——计算纵向惯性力的加速度水动力系数；

Mq——俯仰角速度水动力系数；

Xwq——垂向速度俯仰角速度水动力系数；

Mw——垂向速度水动力系数；

——舵角水动力系数；

Iy——沿横向轴的转动惯量；

zg——重心在垂直方向的位置；

zb——浮力中心在垂直方向的位置；

W——重力；

B——浮力；

δs——与深度控制相关的舵角，即垂直舵角；

T——纵向推力；

m——水下自主航行器的质量；

水下自主航行器的系统状态向量y＝[z,θ,q,u]，水下自主航行器的深度控制子系统状态向量x＝[z,θ,q]，水下自主航行器的速度控制子系统状态向量u＝[u]，用x(k)，y(k)，u(k)分别表示上述状态向量在任意离散时刻k的值；

所述数据解耦计算单元具体还用于用采样时间Δt进行离散化，具体包括下述步骤：步骤S041，将深度控制子系统状态向量用采样时间Δt离散化，得到：即：x(k+1)＝f[x(k),u(k),δs(k)]，步骤S042，将速度控制子系统状态向量用采样时间Δt离散化，得到：即：u(k+1)＝g[x(k),u(k),T(k)]，步骤S043，将系统状态向量用采样时间Δt离散化，得到：即：y(k+1)＝h[x(k),u(k),δs(k),T(k)]。

9.如权利要求8所述的基于模型预测控制的水下自主航行器深度和纵向解耦控制系统，其特征在于，采用权利要求1‑7中任一项所述的方法来进行深度与纵向解耦控制。

10.如权利要求8所述的基于模型预测控制的水下自主航行器深度和纵向解耦控制系统，其特征在于，进一步包括水平舵角控制单元，用于控制水平舵角。

说明书

水下自主航行器深度和纵向解耦控制方法及系统

技术领域

[0001] 本发明涉及水下自主航行器技术领域，特别是涉及一种基于模型预测控制的水下自主航行器深度和纵向解耦耦控制方法及系统。

背景技术

[0002] 水下自主航行器(Autonomous Underwater Vehicle)，简称AUV，是一种具有自主能力的水下智能运载装备，被广泛应用于海上救援、海底探测和海底管道铺设等场景，具有
深远的应用价值与研究意义。

[0003] 对于AUV的运动控制，可以分为深度、纵向和航向控制三个维度。由于AUV在航行时，需要避开海底的障碍，并且需要根据实际情况调整纵向速度，所以AUV的深度和纵向速
度控制成为人们研究的热点问题。

[0004] 检索现有专利，关于AUV运动控制方法的专利数量不多。浙江大学的专利“基于不确定海流扰动下的多AUV分布式协同跟踪控制方法”(专利号：CN201810142114.8)提出了一
种基于神经网络的控制方法来解决不确定海流扰动带来的控制难题；清华大学的专利“基
于深度强化学习的自主水下航行器轨迹跟踪控制方法”(专利号：CN201810535773.8)提出
了一种基于深度强化学习的轨迹跟踪控制方法，可以使AUV不受到历史轨迹的影响；西北工
业大学的专利“基于分层分布式模型预测控制的AUV编队协同控制方法”(专利号：
CN201611163767.1)提出了一种基于分层分布式的模型预测控制方法，将AUV分为运动学模
型和动力学模型两层，从而实现分层的多AUV分布式控制。

[0005] 检索现有论文，Ge Guo和Zhenyu Gao在论文“Fixed‑time sliding mode formation control of AUVs based on a disturbance observer”中提出了一种有扰动
情况下AUV滑模控制算法，使其在有限时间内，在多重扰动下实现全局稳定。Haoliang Wang
等在论文“Adaptive Cooperative Diving of Saucer‑Type Underwater Gliders 
Subject to Model Uncertainties and Input Constraints”中提出了一种处理模型不确
定性的自适应方法。除此之外，基于神经网络、模糊控制和模型预测控制等的方法也被应用
到了AUV的运动控制中。Chao Shen等在论文“Model Predictive Control for an AUV 
with Dynamic Path Planning”中提出了一种动态路径规划和模型预测控制共同作用的方
法，来解决复杂环境下AUV规划和控制的问题。

[0006] 但是，由于模型预测控制计算负担较重，当模型具有强非线性、强耦合性时，控制的实时性较难得到保证。

发明内容

[0007] 本发明的目的在于提供一种基于模型预测控制的水下自主航行器深度和纵向解耦控制方法和系统，以期提高计算效率来降低计算负担，提高控制的实时性。

[0008] 为实现上述目的，本发明的实施例提供一种基于模型预测控制的水下自主航行器深度和纵向解耦控制方法，所述方法包括建模步骤S0，所述建模步骤S0对水下自主航行器
进行动力学建模，使得所述模型符合水下自主航行器的动力学特性，且适于采用于模型预
测控制，所述建模步骤S0具体包括：

[0009] 步骤S01，对于水下自主航行器建立深度和纵向速度耦合的系统模型如下：

[0010]

[0011] 各符号表示的意义如下：

[0012] z——深度；

[0013] u——纵向速度；

[0014] θ——俯仰角；

[0015] q——俯仰角速度；

[0016] X|u|u——计算流体阻力的系统水动力系数；

[0017] ——计算横向惯性力矩的加速度水动力系数；

[0018] ——计算纵向惯性力的加速度水动力系数；

[0019] Mq——俯仰角速度水动力系数；

[0020] Xwq——垂向速度俯仰角速度水动力系数；

[0021] Mw——垂向速度水动力系数；

[0022] ——舵角水动力系数；

[0023] Iy——沿横向轴的转动惯量；

[0024] zg——重心在垂直方向的位置；

[0025] zb——浮力中心在垂直方向的位置；

[0026] W——重力；

[0027] B——浮力；

[0028] δs——与深度控制相关的舵角，即垂直舵角；在本发明中，为了简化表述，除非特别指明，舵角都是指即垂直舵角；

[0029] T——纵向推力；

[0030] m——水下自主航行器的质量；

[0031] 步骤S02，将上述系统模型分解为纵向速度控制子系统模型和深度控制子系统模型，

[0032] 纵向速度控制子系统模型为：

[0033]

[0034] 深度控制子系统模型为：

[0035]

[0036] 步骤S03，确定水下自主航行器系统状态向量y、水下自主航行器的深度控制子系统状态向量x，水下自主航行器的速度控制子系统状态向量u，

[0037] 其中，水下自主航行器的系统状态向量y＝[z,θ,q,u]，

[0038] 水下自主航行器的深度控制子系统状态向量x＝[z,θ,q]，

[0039] 水下自主航行器的速度控制子系统状态向量u＝[u]，

[0040] 用x(k)，y(k)，u(k)分别表示上述状态向量在任意离散时刻k的值，

[0041] 步骤S04，用采样时间Δt离散化上述状态向量，步骤S04包括下述步骤：

[0042] 步骤S041，将深度控制子系统状态向量用采样时间Δt离散化，得到：

[0043]

[0044] 即：x(k+1)＝f[x(k),u(k),δs(k)]，

[0045] 步骤S042，将速度控制子系统状态向量用采样时间Δt离散化，得到：

[0046] 即：u(k+1)＝g[x(k),u(k),T(k)]，

[0047] 步骤S043，将系统状态向量用采样时间Δt离散化，得到：

[0048]

[0049] 即：y(k+1)＝h[x(k),u(k),δs(k),T(k)]。

[0050] 优选地，根据本发明实施例的方法利用建模步骤S0中建立的模型以及离散化的状态向量表达式，对解耦的纵向速度控制子系统模型和深度控制子系统模型，分别以舵角δs
和纵向推力T为控制输入量，构建优化问题，分别获取舵角δs和纵向推力T的最优控制输入，
并以所述最优控制输入控制水下自主航行器。

[0051] 优选地，在根据本发明实施例的方法中，采样水下自主航行器的参数，基于采样参数和离散公式，获取水下自主航行器在未来最大预测步长Np个时刻内的预测状态，

[0052] 把上一离散时刻求得的预测最优状态序列内的纵向速度序列作为深度控制子系统中纵向速度的值，对深度控制子系统进行优化来求解未来Np个时刻内的最优控制舵角输
入序列，

[0053] 同时，把上一离散时刻求得的预测最优状态序列内的俯仰角序列作为纵向速度控制子系统中的对应量的值，对纵向速度控制子系统进行优化来求解未来Np个时刻内的最优
控制纵向推力输入序列，

[0054] 以所述最优控制舵角输入序列的第一个值、所述最优控制纵向推力输入序列的第一个值来控制水下自主航行器，输入序列的其他值用于生成水下自主航行器在后续未来Np
个时刻的预测最优状态序列，并重新采样再获取预测最优状态序列，如此循环，直至完成控
制任务。

[0055] 优选地，根据本发明实施例的方法包括下述的控制步骤：

[0056] 步骤S1，在t＝t0的起始时刻，采样水下自主航行器的状态，并对水下自主航行器系统进行初始化，

[0057] 采样的参数如下：

[0058] y(0)＝[z(0),θ(0),q(0),u(0)]，或者，表示为

[0059] y(t)＝[z(t),θ(t),q(t),u(t)]，其中，t＝t0，

[0060] 对水下自主航行器系统的初始化包括：获取水下自主航行器在未来Np个时刻内的预测最优状态 及

[0061] 需要指出的是，“预测最优状态”仅仅是一个假定的状态，或者是一个设定的状态，并非是实际的运行状态。“预测最优状态”根据上一离散时刻求得的最优状态而来或者直接
给定，仅仅用于给两个子系统的耦合变量赋值，从而解耦两个子系统，进而求取最优的舵角
输入及推力输入。

[0062] 其中，Np为最大预测步长，为大于等于1的自然数，

[0063] 所述Np个时刻为t＝t0+1*Δt,t0+2*Δt,…,t0+k*Δt,…,t0+(Np‑1)*Δt,t0+Np*Δt，Δt为采样周期或采样时间间隔，

[0064]

[0065]

[0066]

[0067]

[0068] 表示在时刻t对时刻t之后k步的预测最优值，或(t+k*Δt)时刻的预测最优值，

[0069] 其中， 分别为水下自主航行器的深度、俯仰角、俯仰角速度、和纵向速度的预测最优值，

[0070] 步骤S2，对深度控制子系统和纵向速度控制子系统分别构建优化问题，即构建对深度控制子系统的最优控制优化问题Pd(t)，以及构建对纵向速度控制子系统的最优控制
优化问题Pl(t)，

[0071] 对于Pd(t)，把 作为深度控制子模型中时刻t之后NP个离散时刻纵向速度的预测值，以舵角输入作为变量，以舵角最小、与预测最优状态 的状态偏离最小、以及/
或者与预设约束状态之间的偏差最小为优化目的，以便获取舵角最优控制输入序列，

[0072] 对于Pl(t)，把 中的俯仰角序列作为纵向速度控制子模型中时刻t之后NP个离散时刻俯仰角的预测值，以纵向推力输入作为变量，以纵向推力最小、与预测最优状态
的状态偏离最小、以及/或者与预设约束状态之间的偏差最小为优化目的，以便获取
纵向推力最优控制输入序列，

[0073] 需要指出的是，此处的预设约束状态可以是参考轨迹或跟踪轨迹，但不限于参考轨迹或跟踪轨迹，还可以是其他预设约束状态。例如可以是仅仅在某些时间段具有参考轨
迹；或者可以是要求在某些时间段中，处于设定的深度，或者处于设定的位置，或者而处于
设定的纵向速度，或者要求在特定的时间段或特定的区域纵向推力小于设定值。

[0074] 所述其他预设约束状态具体通过优化问题的代价函数中代表预测状态与其他预设约束状态之间误差的惩罚来体现。在不存在其他预设约束状态，或者在某些时段不存在
其他预设约束状态的情况下，所述惩罚取值为0。

[0075] 在不能计算其他预设约束状态、或不能计算预测状态与其的误差的情况下；或者，在某些时段不能计算存在其他预设约束状态、或不能计算预测状态与其的误差的情况下，
所述惩罚取值为0。

[0076] 步骤S3，求解优化问题，获取最优控制输入序列，

[0077] 具体地，通过求解优化问题Pd(t)，获取舵角最优控制输入序列，获取的舵角最优控制输入序列＝[δs(0|t),δs(1|t),…,δs(k|t),…,δs(Np‑1|t)]，

[0078] 通过求解优化问题Pl(t)，获取纵向推力最优控制输入序列，获取的纵向推力最优控制输入序列＝[T(0|t),T(1|t),…,T(k|t),…,T(Np‑1|t)]，

[0079] 步骤S4，将步骤S3获取的舵角最优控制输入序列的第一个值δs(0|t)和纵向推力最优控制输入序列的第一个值T(0|t)作为真实控制量作用于水下自主航行器，

[0080] 基于步骤S3获取的本采样周期的最优控制舵角输入值序列的值，确定下一采样周期的舵角预设序列如下，

[0081] [δs*(0|t+Δt),δs*(1|t+Δt),…,δs*(k|t+Δt),…,δs*(Np‑1|t+Δt)]＝[δs(1|t),…,δs(k|t),…,δs(Np‑1|t),δs(Np|t)＝设定舵角值]，设定舵角值可以根据需要设置，例
如δs(Np|t)设置为等于零，以便于快速实现优化收敛，以较快的计算速度或效率实现舵角最
小的优化目标；或者δs(Np|t)设置为等于已经确定的δs(Np‑1|t)，从而，有利于增进运行中
舵角的稳定。需要指出的是，δs(Np|t)的值仅仅是一个给出的假定初始值，最终的输出舵角
取决于优化的结果。

[0082] 基于步骤S3获取的本采样周期的最优控制纵向推力输入值序列的值，确定下一采样周期的纵向推力预设序列如下，

[0083] [T*(0|t+Δt),T*(1|t+Δt),…,T*(k|t+Δt),…,T*(Np‑1|t+Δt)]＝[T(1|t),…,T(k|t),…,T(Np‑1|t),T(Np|t)＝设定纵向推力值]，设定纵向推力值可以根据需要
设置，例如T(Np|t)可以设置为等于零，以便于快速实现优化收敛，以较快的计算速度或效
率实现纵向推力最小的优化目标；或者T(Np|t)设置为等于已经确定的T(Np‑1|t)，从而，有
利于增进运行中纵向推力的稳定。需要指出的是，T(Np|t)的值仅仅是一个给出的假定初始
值，最终的输出纵向推力取决于优化的结果。

[0084] 步骤S5，经过一个采样周期后，t＝t+Δt，再次采样水下自主航行器状态，得到采样的参数y(t)＝[z(t),θ(t),q(t),u(t)]，

[0085] 基于采样的参数、当前采样周期的预设控制输入序列，利用离散公式y(k+1)＝h[x(k),u(k),δs(k),T(k)]，获取水下自主航行器在未来Np个时刻内的预测最优状态 及

[0086] 然后，转步骤S2。

[0087] 在本发明的一些实施例中，为了结束控制，设置有结束判断步骤，在所述结束判断步骤中，判断是否满足结束对水下自主航行器的深度和纵向解耦控制的条件，如果满足，则
结束控制；否则继续进行。

[0088] 所述结束判断步骤可以设置在步骤S1至S5的任何阶段，或者可以设置为一个中断触发入口，在满足相信的触发条件后，即中断上述控制方法的进行。

[0089] 中断的条件可以根据需要设置，例如，可以是完成控制任务，如到达目的地；或运行了设定时长；或者，可以是已经无法执行签署控制方法，例如卸载的能量低于设定阈值，
或者在设定的时长内，无法完成优化而求解出相应的最优控制输入序列。

[0090] 在本发明的一些实施例中，在步骤S1中，直接给定与起始时刻对应的采样周期的预设控制输入序列，然后，基于采样的参数、直接给定的预设控制输入序列，利用离散公式y
(k+1)＝h[x(k),u(k),δs(k),T(k)]，获取水下自主航行器在未来Np个时刻内的预测最优状
态 及 或者

[0091] 在步骤S1中，根据经验值，或者根据所要跟踪的轨迹，直接给定水下自主航行器在未来Np个时刻内的预测最优状态 及

[0092] 在本发明的一些实施例中，在步骤S2中，构建对深度控制子系统的最优控制优化问题Pd(t)如下：

[0093]

[0094] 其中， 为待优化的代价函数，

[0095] ld为待优化的代价函数的表达式，

[0096] 为预测舵角输入，在最优控制优化问题Pd(t)的优化过程中为待优化的变量，

[0097] 为t时刻之后的第k个预测舵角输入，其值在本优化周期中，或者更具体地，在优化问题Pd(t)的求解过程中，在一定的阈值内变化，以得到优化的结果，

[0098] 是待优化的变量，通过变化 的取值，从而计算最小的代价函数，

[0099] xp为深度控制子系统状态向量的预测值，在优化过程中，其值根据 而变化，

[0100] xa为深度控制子系统状态向量的假设值或比较基准值，

[0101]

[0102] xa为深度控制子系统状态向量的假设值或比较基准值，

[0103] xr为所述控制方法所跟踪的跟踪轨迹yr的子集，

[0104] 跟踪轨迹yr(t)＝[zr(t),θr(t),qr(t),ur(t)]＝[xr(t),ur(t)],跟踪轨迹yr(t)包含了每个时刻水下自主航行器的理想状态，其中，xr(k|t)＝xr(t+k)，ur(k|t)＝ur(t+k)，上
述优化问题Pd(t)满足：

[0105] xp(0|t)＝x(t),

[0106]

[0107]

[0108]

[0109] 其中，

[0110] 为舵角输入约束，例如，所述舵角输入约束具体为：舵角的取值，应该确保俯仰角处于设定的范围之内，或者舵角的变化速度应该处于设定的范围之内等等，当然也可以
是其他与舵角相关的约束。

[0111] 为水下自主航行器的垂向状态约束，例如，所述垂向状态约束具体为纵向速度的取值，应该确保深度或者深度变化速度处于设定的范围之内等等，当然也可以是其他与
与舵角相关的约束。

[0112] 可以根据具体的优化目的确定，在一个实施例中，其定义为下式：

[0113]

[0114] 其中， 代表对控制输入的惩罚，

[0115] 代表对预测状态与参考状态误差的惩罚，

[0116] 代表预测状态与跟踪轨迹误差的惩罚

[0117] Rd、Fd和Gd为权重系数，

[0118] 对于矩阵X，定义P范数：

[0119] 在本发明的一些实施例中，在步骤S2中，构建对纵向速度控制子系统的最优控制优化问题Pl(t)如下：

[0120]

[0121] 其中， 为待优化的代价函数，

[0122] ll为待优化的代价函数的表达式，

[0123] Tp为预测纵向推力输入，在最优控制优化问题Pl(t)的优化过程中为待优化的变量，

[0124] Tp(k|t)为纵向速度控制子系统在t时刻之后的第k个预测纵向推力输入，Tp(k|t)p
是待优化的变量，通过变化T(k|t)的取值，从而计算最小的代价函数，

[0125] up为纵向速度控制子系统状态向量的预测值，在优化过程中，其值根据Tp而变化，

[0126] ua为纵向速度控制子系统状态向量的假设值或比较基准值，

[0127]

[0128] ur为所述控制方法所跟踪的跟踪轨迹yr的子集，

[0129] 跟踪轨迹yr(t)＝[zr(t),θr(t),qr(t),ur(t)]＝[xr(t),ur(t)],跟踪轨迹yr(t)包含了每个时刻水下自主航行器的理想状态，其中，xr(k|t)＝xr(t+k)，ur(k|t)＝ur(t+k)，

[0130] 上述优化问题Pl(t)满足：

[0131] up(0|t)＝u(t),

[0132]

[0133]

[0134]

[0135] 其中，

[0136] 为纵向速度控制子系统的输入约束，

[0137] 为水下自主航行器纵向状态约束，

[0138] ll(up,Tp,ua；k|t)可以根据具体的优化目的确定，在一个实施例中，其定义为下式：

[0139]

[0140] 其中， 代表对控制输入的惩罚，

[0141] 代表对预测状态与参考状态误差的惩罚，

[0142] 代表预测状态与跟踪轨迹误差的惩罚，

[0143] Rl、Gl和Fl是权重系数，

[0144] 对于矩阵或参数X，定义P范数：

[0145] 本发明的实施例还提供一种基于模型预测控制的水下自主航行器深度和纵向解耦控制系统，所述基于模型预测控制的水下自主航行器深度和纵向解耦控制系统包括：状
态传感器单元、数据解耦计算单元、优化控制计算单元和控制执行单元等，

[0146] 其中，状态传感器单元与优化控制计算单元和数据解耦计算单元连接，优化控制计算单元与数据解耦计算单元和控制执行单元连接，

[0147] 状态传感器单元以设定的采样周期探测水下自主航行器的状态，所述状态包括：水下自主航行器的深度、俯仰角、俯仰角速度、纵向速度，所述状态的值被传输至优化控制
计算单元和数据解耦计算单元，

[0148] 数据解耦计算单元根据所接受的状态和上一时刻接受于最优控制计算单元的最优控制序列，计算未来的预测最优状态，从而解耦水下自主航行器的纵向速度控制子模型
和深度控制子模型，并生成两个子系统的最优化问题，

[0149] 优化控制计算单元接受数据解耦计算单元发送的最优化问题，基于所述状态的值，以及预测状态的值，得到最优控制输入，以得到的最优控制输入来控制执行单元驱动
AUV运动，并将得到的最优控制输入传输到控制执行单元和数据解耦计算单元中，用于下一
周期的预测及优化。

[0150] 在本发明的一些实施例中，所述的基于模型预测控制的水下自主航行器深度和纵向解耦控制系统，采用如上任一项所述的方法来进行深度与纵向解耦控制。

[0151] 在本发明的一些实施例中，所述的基于模型预测控制的水下自主航行器深度和纵向解耦控制系统进一步包括水平舵角控制单元，用于控制水平舵角。水平舵角控制单元的
控制方法，可以采用任何适当的控制方法，例如可以借鉴在路面上行驶的车辆的横向运动
控制方法。

[0152] 本发明实施例的基于模型预测控制的水下自主航行器深度和纵向解耦控制方法和系统，将深度和纵向进行解耦，大大提高了计算效率，降低计算负担，显著提高了控制的
实时性。

附图说明

[0153] 图1是根据本发明一实施例的水下自主航行器深度和纵向解耦耦控制系统的示意图。

具体实施方式

[0154] 在附图中，使用相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面结合附图对本发明的实施例进行详细说明。

[0155] 在本发明的描述中，术语“中心”、“纵向”、“横向”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关
系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有
特定的方位、以特定的方位构造和操作，因此不能理解为对本发明保护范围的限制。

[0156] 水下自主航行器是一种综合了人工智能和其他先进计算技术的任务控制器，集成了深潜器、传感器、环境效应、计算机软件、能量储存、转换与推进、新材料与新工艺、以及水
下智能武器等高科技，主要用于海洋科学考察及水下搜救工作。军事上则能够用于反潜战、
水雷战、侦察与监视和后勤支援等领域。

[0157] 但是，因为运行轨迹的立体化，水下自主航行器的运行控制极其复杂，尤其是其深度和纵向速度控制，因为彼此耦合在一起，进行控制时，运算极其复杂。

[0158] 模型预测控制(Model Predictive Control)，简称MPC，是一种基于预测未来时刻的系统状态，设计代价函数，求取最优控制输入的方法，在航空航天、汽车等工业控制领域
得到了很好的应用。

[0159] 本发明的实施例，将模型预测控制引入水下自主航行器的控制，通过对水下自主航行器进行动力学建模，并分解所述动力学模型，从而实现深度与纵向的解耦，大大简化了
控制过程中的计算，提高了运算效率，有利于保证控制的实时性。

[0160] 根据本发明一实施例的基于模型预测控制的水下自主航行器深度和纵向解耦控制方法首先包括建模步骤S0。所述建模步骤S0对水下自主航行器进行动力学建模，使得所
述模型符合水下自主航行器的动力学特性，且适于采用于模型预测控制方法来进行控制。

[0161] 所述建模步骤S0具体包括：

[0162] 步骤S01，对于水下自主航行器建立深度和纵向速度耦合的系统模型如下：

[0163]

[0164] 各符号表示的意义如下：

[0165] z——深度；

[0166] u——纵向速度；

[0167] θ——俯仰角；

[0168] q——俯仰角速度；

[0169] X|u|u——计算流体阻力的系统水动力系数；

[0170] ——计算横向惯性力矩的加速度水动力系数；

[0171] ——计算纵向惯性力的加速度水动力系数；

[0172] Mq——俯仰角速度水动力系数；

[0173] Xwq——垂向速度俯仰角速度水动力系数；

[0174] Mw——垂向速度水动力系数；

[0175] ——舵角水动力系数；

[0176] Iy——沿横向轴的转动惯量；

[0177] zg——重心在垂直方向的位置；

[0178] zb——浮力中心在垂直方向的位置；

[0179] W——重力；

[0180] B——浮力；

[0181] δs——与深度控制相关的舵角，即垂直舵角；在本发明中，为了简化表述，除非特别指明，舵角都是指即垂直舵角；

[0182] T——纵向推力；

[0183] m——水下自主航行器的质量；

[0184] 步骤S02，将上述系统模型分解为纵向速度控制子系统模型和深度控制子系统模型，

[0185] 纵向速度控制子系统模型为：

[0186]

[0187] 深度控制子系统模型为：

[0188]

[0189] 步骤S03，确定水下自主航行器系统状态向量y、水下自主航行器的深度控制子系统状态向量x，水下自主航行器的速度控制子系统状态向量u，

[0190] 其中，水下自主航行器的系统状态向量y＝[z,θ,q,u]，

[0191] 水下自主航行器的深度控制子系统状态向量x＝[z,θ,q]，

[0192] 水下自主航行器的速度控制子系统状态向量u＝[u]，

[0193] 用x(k)，y(k)，u(k)分别表示上述状态向量在任意离散时刻k的值，

[0194] 步骤S04，用采样时间Δt离散化上述状态向量，步骤S04包括下述步骤：

[0195] 步骤S041，将深度控制子系统状态向量用采样时间Δt离散化，得到：

[0196] 即：x(k+1)＝f[x(k),u(k),δs(k)]，

[0197] 步骤S042，将速度控制子系统状态向量用采样时间Δt离散化，得到：

[0198]

[0199] 即：u(k+1)＝g[x(k),u(k),T(k)]，

[0200] 步骤S043，将系统状态向量用采样时间Δt离散化，得到：

[0201]

[0202] 即：y(k+1)＝h[x(k),u(k),δs(k),T(k)]。

[0203] 优选地，根据本发明实施例的方法利用建模步骤S0中建立的模型以及离散化的状态向量表达式，对解耦的纵向速度控制子系统模型和深度控制子系统模型，分别以舵角δs
和纵向推力T为控制输入量，构建优化问题，分别获取舵角δs和纵向推力T的最优控制输入，
并以所述最优控制输入控制水下自主航行器。

[0204] 优选地，在根据本发明实施例的方法中，采样水下自主航行器的参数，基于采样参数和离散公式，获取水下自主航行器在未来最大预测步长Np个时刻内的预测状态，

[0205] 把上一离散时刻求得的预测最优状态序列内的纵向速度序列作为深度控制子系统中纵向速度的值，对深度控制子系统进行优化来求解未来Np个时刻内的最优控制舵角输
入序列，

[0206] 同时，把上一离散时刻求得的预测最优状态序列内的俯仰角序列作为纵向速度控制子系统中的对应量的值，对纵向速度控制子系统进行优化来求解未来Np个时刻内的最优
控制纵向推力输入序列，

[0207] 以所述最优控制舵角输入序列的第一个值、所述最优控制纵向推力输入序列的第一个值来控制水下自主航行器，输入序列的其他值用于生成水下自主航行器在后续未来Np
个时刻的预测最优状态序列，并重新采样再获取预测最优状态序列，如此循环，直至完成控
制任务。

[0208] 本发明的实施例具有以下优点：1、将复杂的AUV模型分成两个部分，有助于提高计算效率和控制器的实时性；2、两个系统的最优化计算互不干扰，在实际控制中可以分开调
节系统参数。

[0209] 具体地，根据本发明一实施例的方法包括下述的控制步骤S1至步骤S5。步骤S1至步骤S5形成一个循环，反复地运行。

[0210] 步骤S1为初始步骤，在t＝t0的起始时刻，采样水下自主航行器的状态，并对水下自主航行器系统进行初始化，

[0211] 采样的参数如下：

[0212] y(0)＝[z(0),θ(0),q(0),u(0)]，或者，表示为

[0213] y(t)＝[z(t),θ(t),q(t),u(t)]，其中，t＝t0，

[0214] 对水下自主航行器系统的初始化包括：获取水下自主航行器在未来Np个时刻内的预测最优状态 及

[0215] 需要指出的是，“预测最优状态”仅仅是一个假定的状态，或者是一个设定的状态，并非是实际的运行状态。“预测最优状态”仅仅用于求取舵角输入及推力输入。

[0216] 其中，Np为最大预测步长，为大于等于1的自然数。最大预测步长可以根据需要设置，最大预测步长越大，越能够满足控制要求，但是计算量也越大。

[0217] 所述Np个时刻为t＝t0+1*Δt,t0+2*Δt,…,t0+k*Δt,…,t0+(Np‑1)*Δt,t0+Np*Δt，Δt为采样周期或采样时间间隔，

[0218]

[0219]

[0220]

[0221]

[0222] 表示在时刻t对时刻t之后k步的预测最优值，或(t+k*Δt)时刻的预测最优值，

[0223] 其中， 分别为水下自主航行器的深度、俯仰角、俯仰角速度、和纵向速度的预测最优值。

[0224] 步骤S2为最优控制优化问题构建步骤。在步骤S2中，对深度控制子系统和纵向速度控制子系统分别构建优化问题，即构建对深度控制子系统的最优控制优化问题Pd(t)，以
及构建对纵向速度控制子系统的最优控制优化问题Pl(t)。

[0225] 对于Pd(t)，把 作为深度控制子模型中时刻t之后NP个离散时刻纵向速度的定常参数，以舵角输入作为变量，以舵角最小、与预测最优状态 的状态偏离最小、以
及/或者与预设约束状态之间的偏差最小为优化目的，以便获取舵角最优控制输入序列。

[0226] 对于Pl(t)，把 中的俯仰角序列作为纵向速度控制子模型中时刻t之后NP个离散时刻俯仰角的定常参数，以纵向推力输入作为变量，以纵向推力最小、与预测最优状态
的状态偏离最小、以及/或者与预设约束状态之间的偏差最小为优化目的，以便获取
纵向推力最优控制输入序列。

[0227] 通过把 内纵向速度序列作为深度控制子系统中纵向速度的值，同时把内的俯仰角序列作为纵向速度控制子系统的对应量的值，由此动力学模型解耦成为
独立的两个子模型，可以分别进行优化求解。

[0228] 需要指出的是，此处的预设约束状态可以是参考轨迹或跟踪轨迹，但不限于参考轨迹或跟踪轨迹，还可以是其他预设约束状态。例如可以是仅仅在某些时间段具有参考轨
迹；或者可以是要求在某些时间段中，处于设定的深度，或者处于设定的位置，或者而处于
设定的纵向速度，或者要求在特定的时间段或特定的区域纵向推力小于设定值。

[0229] 所述其他预设约束状态具体通过优化问题的代价函数中代表预测状态与其他预设约束状态之间误差的惩罚来体现。在不存在其他预设约束状态，或者在某些时段不存在
其他预设约束状态的情况下，所述惩罚取值为0。

[0230] 在不能计算其他预设约束状态、或不能计算预测状态与其的误差的情况下；或者，在某些时段不能计算存在其他预设约束状态、或不能计算预测状态与其的误差的情况下，
所述惩罚取值为0。

[0231] 步骤S3为优化求解步骤。在步骤S3中，求解优化问题，获取最优控制输入序列。

[0232] 具体地，通过求解优化问题Pd(t)，获取舵角最优控制输入序列，获取的舵角最优控制输入序列＝[δs(0|t),δs(1|t),…,δs(k|t),…,δs(Np‑1|t)]，以及

[0233] 通过求解优化问题Pl(t)，获取纵向推力最优控制输入序列，获取的纵向推力最优控制输入序列＝[T(0|t),T(1|t),…,T(k|t),…,T(Np‑1|t)]。

[0234] 步骤S4为输出最优控制输入的步骤。

[0235] 在步骤S4中，将步骤S3获取的舵角最优控制输入序列的第一个值δs(0|t)和纵向推力最优控制输入序列的第一个值T(0|t)作为真实控制量作用于水下自主航行器，

[0236] 此外，基于步骤S3获取的本采样周期的最优控制舵角输入值序列的值，确定下一采样周期的舵角预设序列如下，

[0237] [δs*(0|t+Δt),δs*(1|t+Δt),…,δs*(k|t+Δt),…,δs*(Np‑1|t+Δt)]＝[δs(1|t),…,δs(k|t),…,δs(Np‑1|t),δs(Np|t)＝设定舵角值]，设定舵角值可以根据需要设置，例
如δs(Np|t)设置为等于零，以便于快速实现优化收敛，以较快的计算速度或效率实现舵角最
小的优化目标；或者δs(Np|t)设置为等于已经确定的δs(Np‑1|t)，从而，有利于增进运行中
舵角的稳定。需要指出的是，δs(Np|t)的值仅仅是一个给出的假定初始值，最终的输出舵角
取决于优化的结果。

[0238] 基于步骤S3获取的本采样周期的最优控制纵向推力输入值序列的值，确定下一采样周期的纵向推力预设序列如下，

[0239] [T*(0|t+Δt),T*(1|t+Δt),…,T*(k|t+Δt),…,T*(Np‑1|t+Δt)]＝[T(1|t),…,T(k|t),…,T(Np‑1|t),T(Np|t)＝设定纵向推力值]，设定纵向推力值可以根据需要
设置，例如T(Np|t)可以设置为等于零，以便于快速实现优化收敛，以较快的计算速度或效
率实现纵向推力最小的优化目标；或者T(Np|t)设置为等于已经确定的T(Np‑1|t)，从而，有
利于增进运行中纵向推力的稳定。需要指出的是，T(Np|t)的值仅仅是一个给出的假定初始
值，最终的输出纵向推力取决于优化的结果。

[0240] 步骤S5为重新采样步骤，经过一个采样周期后，t＝t+Δt，再次采样水下自主航行器状态，得到采样的参数y(t)＝[z(t),θ(t),q(t),u(t)]，

[0241] 基于采样的参数、当前采样周期的预设控制输入序列，利用离散公式y(k+1)＝h[x(k),u(k),δs(k),T(k)]，获取水下自主航行器在未来Np个时刻内的预测最优状态
及

[0242] 然后，转步骤S2。由此形成一个循环的控制过程。

[0243] 在本发明的一些实施例中，为了结束控制，设置有结束判断步骤，在所述结束判断步骤中，判断是否满足结束对水下自主航行器的深度和纵向解耦控制的条件，如果满足，则
结束控制；否则继续进行。

[0244] 所述结束判断步骤可以设置在步骤S1至S5的任何阶段，或者可以设置为一个中断触发入口，在满足相信的触发条件后，即中断上述控制方法的进行。

[0245] 中断的条件可以根据需要设置，例如，可以是完成控制任务，如到达目的地；或运行了设定时长；或者，可以是已经无法执行签署控制方法，例如卸载的能量低于设定阈值，
或者在设定的时长内，无法完成优化而求解出相应的最优控制输入序列。

[0246] 在本发明的一些实施例中，在步骤S1中，直接给定与起始时刻对应的采样周期的预设控制输入序列，然后，基于采样的参数、直接给定的预设控制输入序列，利用离散公式y
(k+1)＝h[x(k),u(k),δs(k),T(k)]，获取水下自主航行器在未来Np个时刻内的预测最优状
态 及 或者

[0247] 在步骤S1中，根据经验值，或者根据所要跟踪的轨迹，直接给定水下自主航行器在未来Np个时刻内的预测最优状态 及

[0248] 在本发明的一些实施例中，在步骤S2中，构建对深度控制子系统的最优控制优化问题Pd(t)如下：

[0249]

[0250] 其中， 为待优化的代价函数，

[0251] ld为待优化的代价函数的表达式，

[0252] 为预测舵角输入，在最优控制优化问题Pd(t)的优化过程中为待优化的变量，

[0253] 为t时刻之后的第k个预测舵角输入，其值在本优化周期中，或者更具体地，在优化问题Pd(t)的求解过程中，在一定的阈值内变化，以得到优化的结果，

[0254] 是待优化的变量，通过变化 的取值，从而计算最小的代价函数，

[0255] xp为深度控制子系统状态向量的预测值，在优化过程中，其值根据 而变化，

[0256] xa为深度控制子系统状态向量的假设值或比较基准值，

[0257]

[0258] xa为深度控制子系统状态向量的假设值或比较基准值，

[0259] xr为所述控制方法所跟踪的跟踪轨迹yr的子集，

[0260] 跟踪轨迹yr(t)＝[zr(t),θr(t),qr(t),ur(t)]＝[xr(t),ur(t)],跟踪轨迹yr(t)包含了每个时刻水下自主航行器的理想状态，其中，xr(k|t)＝xr(t+k)，ur(k|t)＝ur(t+k)，

[0261] 上述优化问题Pd(t)满足：

[0262] xp(0|t)＝x(t),

[0263]

[0264]

[0265]

[0266] 其中，

[0267] 为舵角输入约束，例如，所述舵角输入约束具体为：舵角的取值，应该确保俯仰角处于设定的范围之内，或者舵角的变化速度应该处于设定的范围之内等等，当然也可以
是其他与舵角相关的约束。

[0268] 为水下自主航行器的垂向状态约束，例如，所述垂向状态约束具体为纵向速度的取值，应该确保深度或者深度变化速度处于设定的范围之内等等，当然也可以是其他与
与舵角相关的约束。

[0269] 可以根据具体的优化目的确定，在一个实施例中，其定义为下式：

[0270]

[0271] 其中， 代表对控制输入的惩罚，

[0272] 代表对预测状态与参考状态误差的惩罚，

[0273] 代表预测状态与跟踪轨迹误差的惩罚

[0274] Rd、Fd和Gd为权重系数，

[0275] 对于矩阵X，定义P范数：

[0276] 在本发明的一些实施例中，在步骤S2中，构建对纵向速度控制子系统的最优控制优化问题Pl(t)如下：

[0277]

[0278] 其中， 为待优化的代价函数，

[0279] ll为待优化的代价函数的表达式，

[0280] Tp为预测纵向推力输入，在最优控制优化问题Pl(t)的优化过程中为待优化的变量，

[0281] Tp(k|t)为纵向速度控制子系统在t时刻之后的第k个预测纵向推力输入，Tp(k|t)p
是待优化的变量，通过变化T(k|t)的取值，从而计算最小的代价函数，

[0282] up为纵向速度控制子系统状态向量的预测值，在优化过程中，其值根据Tp而变化，

[0283] ua为纵向速度控制子系统状态向量的假设值或比较基准值，

[0284]

[0285] ur为所述控制方法所跟踪的跟踪轨迹yr的子集，

[0286] 跟踪轨迹yr(t)＝[zr(t),θr(t),qr(t),ur(t)]＝[xr(t),ur(t)],跟踪轨迹yr(t)包含了每个时刻水下自主航行器的理想状态，其中，xr(k|t)＝xr(t+k)，ur(k|t)＝ur(t+k)，

[0287] 上述优化问题Pl(t)满足：

[0288] up(0|t)＝u(t),

[0289]

[0290]

[0291]

[0292] 其中，

[0293] 为纵向速度控制子系统的输入约束，

[0294] 为水下自主航行器纵向状态约束，

[0295] ll(up,Tp,ua；k|t)可以根据具体的优化目的确定，在一个实施例中，其定义为下式：

[0296]

[0297] 其中， 代表对控制输入的惩罚，

[0298] 代表对预测状态与参考状态误差的惩罚，

[0299] 代表预测状态与跟踪轨迹误差的惩罚，

[0300] Rl、Gl和Fl是权重系数，

[0301] 对于矩阵或参数X，定义P范数：

[0302] 本发明的实施例还提供一种基于模型预测控制的水下自主航行器深度和纵向解耦控制系统，所述基于模型预测控制的水下自主航行器深度和纵向解耦控制系统包括：状
态传感器单元、数据解耦计算单元、优化控制计算单元和控制执行单元等，

[0303] 其中，状态传感器单元与优化控制计算单元和数据解耦计算单元连接，优化控制计算单元与数据解耦计算单元和控制执行单元连接，

[0304] 状态传感器单元以设定的采样周期探测水下自主航行器的状态，所述状态包括：水下自主航行器的深度、俯仰角、俯仰角速度、纵向速度，所述状态的值被传输至优化控制
计算单元和数据解耦计算单元，

[0305] 数据解耦计算单元根据所接受的状态和上一时刻接受于最优控制计算单元的最优控制序列，计算未来的预测最优状态，从而解耦水下自主航行器的纵向速度控制子模型
和深度控制子模型，并生成两个子系统的最优化问题，

[0306] 优化控制计算单元接受数据解耦计算单元发送的最优化问题，基于所述状态的值，以及预测状态的值，得到最优控制输入，以得到的最优控制输入来控制执行单元驱动
AUV运动，并将得到的最优控制输入传输到控制执行单元和数据解耦计算单元中，用于下一
周期的预测及优化。

[0307] 也就是说，就本发明一实施例的基于模型预测控制的水下自主航行器深度和纵向解耦控制系统包括状态传感器单元、数据解耦计算单元、优化控制计算单元和控制执行单
元等。状态传感器单元与优化控制计算单元连接，优化控制计算单元与数据解耦计算单元
和控制执行单元连接。状态传感器探测此时的AUV状态，输入到优化控制计算单元，优化控
制计算单元将得到的最优控制输入传输到控制执行单元和数据解耦计算单元中，控制执行
单元驱动AUV运动，数据解耦计算单元根据最优控制输入计算未来的预测状态，从而解耦两
个子系统。该系统可支持上述方法的具体实施。

[0308] 在本发明的一些实施例中，所述的基于模型预测控制的水下自主航行器深度和纵向解耦控制系统，采用如上任一项所述的方法来进行深度与纵向解耦控制。

[0309] 在本发明的一些实施例中，所述的基于模型预测控制的水下自主航行器深度和纵向解耦控制系统进一步包括水平舵角控制单元，用于控制水平舵角。水平舵角控制单元的
控制方法，可以采用任何适当的控制方法，例如可以借鉴在路面上行驶的车辆的横向运动
控制方法。

[0310] 本发明实施例的基于模型预测控制的水下自主航行器深度和纵向解耦控制方法和系统，将深度和纵向进行解耦，大大提高了计算效率，降低计算负担，显著提高了控制的
实时性。

[0311] 最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对
其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本
发明各实施例技术方案的精神和范围。