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所述的基于模型预测控制的水下自主航行器深度和纵向解耦控制系统,其特征在于,进一步包括水平舵角控制单元,用于控制水平舵角。