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

技术领域

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

背景技术

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

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

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

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

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

发明内容

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

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

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

各符号表示的意义如下：

z——深度；

u——纵向速度；

θ——俯仰角；

q——俯仰角速度；

X

M

X

M

I

z

z

W——重力；

B——浮力；

δ

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)，δ

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

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

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

即：y(k+1)＝h[x(k)，u(k)，δ

优选地，根据本发明实施例的方法利用建模步骤S0中建立的模型以及离散化的状态向量表达式，对解耦的纵向速度控制子系统模型和深度控制子系统模型，分别以舵角δ

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

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

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

以所述最优控制舵角输入序列的第一个值、所述最优控制纵向推力输入序列的第一个值来控制水下自主航行器，输入序列的其他值用于生成水下自主航行器在后续未来N

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

步骤S1，在t＝t

采样的参数如下：

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

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

对水下自主航行器系统的初始化包括：获取水下自主航行器在未来N

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

其中，N

所述Np个时刻为t＝t

其中，

步骤S2，对深度控制子系统和纵向速度控制子系统分别构建优化问题，即构建对深度控制子系统的最优控制优化问题P

对于P

对于P

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

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

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

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

具体地，通过求解优化问题P

通过求解优化问题P

步骤S4，将步骤S3获取的舵角最优控制输入序列的第一个值δ

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

[δ

＝[δ

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

[T

＝[T(1|t)，…，T(k|t)，…，T(Np-1|t)，T(Np|t)＝设定纵向推力值]，设定纵向推力值可以根据需要设置，例如T(Np|t)可以设置为等于零，以便于快速实现优化收敛，以较快的计算速度或效率实现纵向推力最小的优化目标；或者T(Np|t)设置为等于已经确定的T(N

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

基于采样的参数、当前采样周期的预设控制输入序列，利用离散公式y(k+1)＝h[x(k)，u(k)，δ

然后，转步骤S2。

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

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

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

在本发明的一些实施例中，在步骤S1中，直接给定与起始时刻对应的采样周期的预设控制输入序列，然后，基于采样的参数、直接给定的预设控制输入序列，利用离散公式y(k+1)＝h[x(k)，u(k)，δ

在步骤S1中，根据经验值，或者根据所要跟踪的轨迹，直接给定水下自主航行器在未来N

在本发明的一些实施例中，在步骤S2中，构建对深度控制子系统的最优控制优化问题P

其中，

l

x

x

x

x

跟踪轨迹y

上述优化问题P

x

其中，

其中，

R

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

在本发明的一些实施例中，在步骤S2中，构建对纵向速度控制子系统的最优控制优化问题P

其中，

l

T

T

u

u

u

跟踪轨迹y

上述优化问题P

u

其中，

l

其中，

R

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

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

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

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

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

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

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

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

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

附图说明

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

具体实施方式

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

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

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

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

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

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

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

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

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

各符号表示的意义如下：

z——深度；

u——纵向速度；

θ——俯仰角；

q——俯仰角速度；

X

M

X

M

I

z

z

W——重力；

B——浮力；

δ

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)，δ

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

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

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

即：y(k+1)＝h[x(k)，u(k)，δ

优选地，根据本发明实施例的方法利用建模步骤S0中建立的模型以及离散化的状态向量表达式，对解耦的纵向速度控制子系统模型和深度控制子系统模型，分别以舵角δ

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

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

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

以所述最优控制舵角输入序列的第一个值、所述最优控制纵向推力输入序列的第一个值来控制水下自主航行器，输入序列的其他值用于生成水下自主航行器在后续未来N

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

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

步骤S1为初始步骤，在t＝t

采样的参数如下：

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

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

对水下自主航行器系统的初始化包括：获取水下自主航行器在未来N

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

其中，N

所述Np个时刻为t＝t

其中，

步骤S2为最优控制优化问题构建步骤。在步骤S2中，对深度控制子系统和纵向速度控制子系统分别构建优化问题，即构建对深度控制子系统的最优控制优化问题P

对于P

对于P

通过把

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

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

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

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

具体地，通过求解优化问题P

通过求解优化问题P

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

在步骤S4中，将步骤S3获取的舵角最优控制输入序列的第一个值δ

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

[δ

＝[δ

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

[T

＝[T(1|t)，…，T(k|t)，…，T(Np-1|t)，T(Np|t)＝设定纵向推力值]，设定纵向推力值可以根据需要设置，例如T(Np|t)可以设置为等于零，以便于快速实现优化收敛，以较快的计算速度或效率实现纵向推力最小的优化目标；或者T(Np|t)设置为等于已经确定的T(N

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

基于采样的参数、当前采样周期的预设控制输入序列，利用离散公式y(k+1)＝h[x(k)，u(k)，δ

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

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

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

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

在本发明的一些实施例中，在步骤S1中，直接给定与起始时刻对应的采样周期的预设控制输入序列，然后，基于采样的参数、直接给定的预设控制输入序列，利用离散公式y(k+1)＝h[x(k)，u(k)，δ

在步骤S1中，根据经验值，或者根据所要跟踪的轨迹，直接给定水下自主航行器在未来N

在本发明的一些实施例中，在步骤S2中，构建对深度控制子系统的最优控制优化问题P

其中，

l

x

x

x

x

跟踪轨迹y

上述优化问题P

x

其中，

其中，

R

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

在本发明的一些实施例中，在步骤S2中，构建对纵向速度控制子系统的最优控制优化问题P

其中，

l

T

T

u

u

u

跟踪轨迹y

上述优化问题P

u

其中，

l

其中，

R

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

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

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

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

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

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

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

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

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

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

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