CN105511480A - 一种基于人工蜂群优化的模型预测的欠驱动uuv深度控制方法 - Google Patents

Abstract

一种基于人工蜂群优化的模型预测的欠驱动UUV深度控制方法，本发明涉及欠驱动UUV深度控制方法。本发明是要解决存在复杂水平舵角约束条件时UUV深度控制问题，而提出的一种基于人工蜂群优化的模型预测的欠驱动UUV深度控制方法。该方法是通过一、得到欠驱动UUV增量型的垂直面预测模型；二、将水平舵角的控制输入约束条件统一处理为:H△U(k)≤γ；三、通过模型预测控制将UUV深度控制问题转化为约束条件下的二次规划问题；四、求得蜜源的全局最优位置，五、得到k时刻的控制输入；六、确保UUV达到指定UUV下潜的深度R(k+1)完成下潜作业等步骤实现的。本发明应用于UUV深度控制领域。

Description

一种基于人工蜂群优化的模型预测的欠驱动UUV深度控制方法

技术领域

本发明涉及欠驱动UUV深度控制方法，特别涉及基于人工蜂群优化的模型预测的欠驱动UUV深度控制方法。

背景技术

无人水下航行器(UnmannedUnderwaterVehicle,UUV)具有活动范围大、潜水深、机动性好、安全、智能化、运行和维护费用低等优点，作为人类在海洋活动中，特别是深海活动中的重要替代者和执行者,已被广泛应用于科学考察,深海作业,打捞救生等领域，其在作业时需要保持在一定的深度，因此，切实有效的深度控制方法对UUV的性能有着重要的作用。

目前，国内外学者针对UUV深度控制问题做了大量的研究，相应提出了很多控制方法，如自抗扰法，反步法等等。自抗扰法是一种利用特殊的非线性效应，结合实际的非线性工程算法,是一种自动补偿对象模型的新型控制方法，其抗干扰能力强，运用此方法可以解决海洋环境下UUV近水面垂直面运动的控制扰动问题。反步法计算简单，实时性好，响应快，对传感器要求低，在工程应用中得到了广泛的应用，但该方法中存在虚拟控制量的高阶导数问题。但大多都未能够充分考虑UUV在复杂约束条件下的深度控制问题，因此，将此因素引入UUV深度控制中对其能够安全可靠地执行各种作业任务具有重要的意义。

2014年5月ProceedingsoftheInstitutionofMechanicalEngineersPartM:JournalofEngineeringfortheMaritimeEnvironment第228卷中的“Modelpredictivecontrolofahybridautonomousunderwatervehiclewithexperimentalverification”提出将模型预测控制用于AUV悬停状态时深度控制，但其对UUV艏艉槽道推进器推力变化只进行了简单的卡边处理，在推力约束条件变得更加复杂时，该方法将无法有效处理，这将会影响控制器对UUV深度的控制效果，本发明引入人工蜂群优化算法可以有效的处理较为复杂的约束条件。

发明内容

本发明的目的是为了解决存在复杂水平舵角约束条件时UUV深度控制问题而提出的一种基于人工蜂群优化的模型预测的欠驱动UUV深度控制方法。

上述的发明目的是通过以下技术方案实现的：

步骤一、将UUV垂直面模型整理为状态空间模型，将状态空间模型离散化得到欠驱动UUV增量型的垂直面预测模型；

步骤二、在k时刻，根据水平舵特性建立水平舵角的控制输入约束；所述的水平舵角的控制输入约束包括幅值约束和每一拍动作的增量约束；并将水平舵角的控制输入约束条件统一处理为:

H△U(k)≤γ

其中，△U为水平舵控制输入增量序列；H＝[T^T-T^TΕ^T-Ε^T]^T,γ＝[γ₁γ₂]^T；

γHTγ₁Εγ₂为中间矩阵；水平舵特性包括死区特性和饱和特性；

步骤三、根据步骤一得到的欠驱动UUV增量型的垂直面预测模型和步骤二得到的水平舵角的控制输入约束条件通过模型预测控制将UUV深度控制问题转化为约束条件下的二次规划问题如下：

$\begin{array}{cc}\underset{\mathrm{\Δ}U\left(K\right)}{\min }& J\left(\mathrm{\Δ}U\left(k\right)\right)\\ st& h\left(\mathrm{\Δ}U\right(k\left)\right)\≤0\end{array}---\left(8\right)$

式中，J(△U(k))＝△U(k)^TH△U(k)-G(k+1|k)^T△U(k)，h(△U(k))＝H△U(k)-γ；J(·)为反映△U(k)的性能指标，h(·)为描述函数，G(·)为中间变量， $G(k+1|k)=2S_{\mathrm{\δ}}^T{\mathrm{\Γ}}_y^T{\mathrm{\Γ}}_y{E}_p(k+1|k);$

步骤四、利用基于罚函数的人工蜂群优化算法求解约束条件下的二次规划问题，求得蜜源的全局最优位置，即x_best＝[△δ_s(k),△δ_s(k+1),…,△δ_s(k+m-1)]^T为UUV水平舵角控制输入增量序列△U(k)；其中，△δ_s(k+m-1)为第k+m-1步水平舵角的增量，m为控制时域；

步骤五、在求解出蜜源最终全局最优解x_best后，令△U(k)＝x_best ^T，然后取优化解序列△U(k)的第一个分量，并加上k-1时刻的控制输入作为k时刻的控制输入:

$\begin{array}{c}{\mathrm{\Δ\δ}}_s\left(k\right)=\left[\begin{array}{cccc}{I}_{n_u\×n_u}& 0& ...& 0\end{array}\right]\mathrm{\Δ}U\left(k\right)\\ {\mathrm{\δ}}_s\left(k\right)={\mathrm{\Δ\δ}}_s\left(k\right)+{\mathrm{\δ}}_s\left(k-1\right)\end{array};$

步骤六、在k时刻的控制输入δ_s(k)作用下测得新的状态变量和UUV下潜深度，当UUV未达到指定UUV下潜的深度R(k+1)时，将测得新的状态变量和UUV下潜深度转步骤三，直至当UUV达到指定UUV下潜的深度R(k+1)完成下潜作业；其中，新的状态变量包括UUV状态变量垂向速度、UUV状态变量纵倾角速度、UUV状态变量下潜深度和UUV状态变量纵倾角。

发明效果

本发明提供一种基于人工蜂群优化的约束模型预测的欠驱动UUV深度控制方法。基于人工蜂群优化的约束模型预测控制方法解决的是欠驱动UUV在约束条件下准确的下潜到指定深度的问题。本发明首先将欠驱动UUV深度控制问题通过模型预测控制转化为约束条件下的二次规划问题，然后通过人工蜂群优化算法求解约束条件下的二次规划问题，最后将优化解序列的第一个分量加上前一时刻的控制输入作为欠驱动UUV当前时刻的控制输入。问题的求解过程具备在线执行的特点，可以使UUV有效降低不确定性因素的累积作用。本发明能够为欠驱动UUV存在复杂约束条件时的深度控制提供一种切实有效的方法。在考虑控制约束条件下能够有效的实现深度控制，具有在线优化的特点，可以极大减小外界扰动和系统结构参数变化等不确定性因素对UUV深度控制效果的影响如图1、图2和图3。

本发明提供一种基于人工蜂群优化的约束模型预测的欠驱动UUV深度控制方法，其直接将UUV深度控制问题转换约束条件下的二次规划问题，并通过人工蜂群算法在约束域中寻找优化解，可以很好的解决在存在约束条件时的UUV深度控制。

附图说明

图1为具体实施方式一提出的UUV的垂直面建模图；

图2为具体实施方式一提出的基于人工蜂群优化的模型预测的欠驱动UUV深度控制的流程图；

图3为具体实施方式一提出的基于人工蜂群优化的模型预测的欠驱动UUV深度控制框图；

图4(a)为具体实施方式一提出的UUV深度的曲线图；其中，p为预测时域；m为控制时域；

图4(b)为具体实施方式一提出的UUV纵倾角的曲线图；

图5(a)为具体实施方式一提出的UUV纵倾角角速度的曲线图；

图5(b)为具体实施方式一提出的UUV垂向速度的曲线图；

图6(a)为具体实施方式一提出的UUV水平舵角曲线图。

图6(b)为具体实施方式一提出的UUV水平舵角的每一拍增量变化曲线图。

具体实施方式

具体实施方式一：本实施方式的一种基于人工蜂群优化的模型预测的欠驱动UUV深度控制方法，具体是按照以下步骤制备的：

附图2和附图3分别给出了基于人工蜂群优化的模型预测的欠驱动UUV深度控制的流程图和控制框图；

步骤一、将UUV垂直面模型整理为状态空间模型，将状态空间模型离散化得到欠驱动UUV增量型的垂直面预测模型；

步骤二、在k时刻，根据水平舵特性建立水平舵角的控制输入约束；所述的水平舵角的控制输入约束包括幅值约束和每一拍动作的增量约束；并将水平舵角的控制输入约束条件统一处理为：

H△U(k)≤γ

其中，△U为水平舵控制输入增量序列；H＝[T^T-T^TΕ^T-Ε^T]^T,γ＝[γ₁γ₂]^T；

γHTγ₁Εγ₂为中间矩阵；水平舵特性包括死区特性和饱和特性；

步骤三、根据步骤一得到的欠驱动UUV增量型的垂直面预测模型和步骤二得到的水平舵角的控制输入约束条件通过模型预测控制将UUV深度控制问题转化为约束条件下的二次规划问题如下：

$\begin{array}{cc}\underset{\mathrm{\Δ}U\left(K\right)}{\min }& J\left(\mathrm{\Δ}U\left(k\right)\right)\\ st& h\left(\mathrm{\Δ}U\right(k\left)\right)\≤0\end{array}---\left(8\right)$

式中，J(△U(k))＝△U(k)^TH△U(k)-G(k+1|k)^T△U(k)，h(△U(k))＝H△U(k)-γ；J(·)为反映△U(k)的性能指标，h(·)为描述函数，G(·)为中间变量， $G(k+1|k)=2S_{\mathrm{\δ}}^T{\mathrm{\Γ}}_y^T{\mathrm{\Γ}}_y{E}_p(k+1|k);$

步骤四、利用基于罚函数的人工蜂群优化算法求解约束条件下的二次规划问题，求得蜜源的全局最优位置，即x_best＝[△δ_s(k),△δ_s(k+1),…,△δ_s(k+m-1)]^T为UUV水平舵角控制输入增量序列△U(k)；其中，△δ_s(k+m-1)为第k+m-1步水平舵角的增量，m为控制时域；

步骤五、在求解出蜜源最终全局最优解x_best后，令△U(k)＝x_best ^T，然后取优化解序列△U(k)的第一个分量，并加上k-1时刻的控制输入作为k时刻的控制输入:

${\mathrm{\Δ\δ}}_s\left(k\right)=\left[\begin{array}{cccc}{I}_{n_u\×n_u}& 0& ...& 0\end{array}\right]\mathrm{\Δ}U\left(k\right)$

δ_s(k)＝△δ_s(k)+δ_s(k-1)

但只取控制输入序列的第一个分量△δ_s(k)作用于系统；

步骤六、在k时刻的控制输入δ_s(k)作用下测得新的状态变量和UUV下潜深度，当UUV未达到指定UUV下潜的深度R(k+1)时，将测得新的状态变量和UUV下潜深度转步骤三，直至当UUV达到指定UUV下潜的深度R(k+1)完成下潜作业；其中，新的状态变量包括UUV状态变量垂向速度、UUV状态变量纵倾角速度、UUV状态变量下潜深度和UUV状态变量纵倾角；

仿真分析：

结合图4(a)～图6(b)仿真结果，从图4(a)和图4(b)可以看出：在m一定时，随着p的增大，超调量明显减小，但响应的时间变慢了；在一定时，随着m的增大，同样，有超调量量减小，响应时间变缓，但伴随着震荡加剧；可以看出，不管是p还是m增大了，在一定程度上都会提高控制效果，但由于p和m的增大，将会增大QP问题求解的计算量，这将使响应变得迟缓；从图5(a)和图5(b)可以看出，随着m和p的增大，纵倾角角速度和垂向速度的超调量都有所降低，但伴有震荡加剧特点；从图6(a)和图6(b)中可以看出，水平舵在±30°之间变化，同时对每一拍的水平舵角增量可以进行严格控制，这里限制为每拍±20°之间，可以避免舵角大范围变动，同样随着p和m的增大，水平舵的舵角和舵角增量震荡加剧；在实际工程应用中，对于不同的作业要求，可以通过对控制参数的调节来达到要求的性能。

本实施方式效果：

本实施方式提供一种基于人工蜂群优化的约束模型预测的欠驱动UUV深度控制方法。基于人工蜂群优化的约束模型预测控制方法解决的是欠驱动UUV在约束条件下准确的下潜到指定深度的问题。本实施方式首先将欠驱动UUV深度控制问题通过模型预测控制转化为约束条件下的二次规划问题，然后通过人工蜂群优化算法求解约束条件下的二次规划问题，最后将优化解序列的第一个分量加上前一时刻的控制输入作为欠驱动UUV当前时刻的控制输入。问题的求解过程具备在线执行的特点，可以使UUV有效降低不确定性因素的累积作用。本实施方式能够为欠驱动UUV存在复杂约束条件时的深度控制提供一种切实有效的方法。在考虑控制约束条件下能够有效的实现深度控制，具有在线优化的特点，可以极大减小外界扰动和系统结构参数变化等不确定性因素对UUV深度控制效果的影响如图1、图2和图3。

本实施方式提供一种基于人工蜂群优化的约束模型预测的欠驱动UUV深度控制方法，其直接将UUV深度控制问题转换约束条件下的二次规划问题，并通过人工蜂群算法在约束域中寻找优化解，可以很好的解决在存在约束条件时的UUV深度控制。

具体实施方式二：本实施方式与具体实施方式一不同的是：步骤一中将UUV垂直面模型整理为状态空间模型，将状态空间模型离散化得到欠驱动UUV增量型的垂直面预测模型具体过程为：

步骤一一、结合图1，建立了UUV的坐标系统，其中，UUV的坐标系统包括固定坐标系E-ξηζ和运动坐标系O-XYZ；

步骤一二、取状态变量水平舵的舵角δ_s(t)和t时刻UUV下潜深度y(t)建立UUV垂直面的状态空间方程即连续的状态空间模型:

$\begin{array}{c}\stackrel{\·}{x}\left(t\right)=A_{c}x\left(t\right)+B_{c\mathrm{\δ}}{\mathrm{\δ}}_s\left(t\right)+B_{cd}d\left(t\right)\\ y\left(t\right)=C_{c}x\left(t\right)\end{array}---\left(1\right)$

式中， $A_c={\left[\begin{array}{cccc}\hat{a}& \hat{b}& 0& 0\\ \hat{c}& \hat{d}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]}^{-1}\left[\begin{array}{cccc}\hat{e}& \hat{f}& 0& 0\\ \hat{g}& \hat{h}& 0& 0\\ 0& 1& 0& 0\\ 1& 0& -u& 0\end{array}\right],B_{c\mathrm{\δ}}={\left[\begin{array}{cccc}\hat{a}& \hat{b}& 0& 0\\ \hat{c}& \hat{d}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]}^{-1}\left[\begin{array}{c}0\\ \hat{k}\\ 0\\ 0\end{array}\right],B_{cd}=\left[\begin{array}{cc}1& 0\\ 0& 1\\ 0& 0\\ 0& 0\end{array}\right],$ C_c＝[0001]； $\hat{a}=m-\frac{1}{2}{\mathrm{\ρL}}^3{Z}_{\stackrel{\·}{w}}^{\′},\hat{b}=\frac{1}{2}{\mathrm{\ρL}}^4{Z}_{\stackrel{\·}{q}}^{\′},\hat{c}=\frac{1}{2}{\mathrm{\ρL}}^4{M}_{\stackrel{\·}{w}}^{\′},\hat{d}=J_y-\frac{1}{2}{\mathrm{\ρL}}^5{M}_{\stackrel{\·}{q}}^{\′},$ $\hat{e}=\frac{1}{2}{\mathrm{\ρL}}^2{Z}_{uw}^{\′}u,\hat{f}=mu+\frac{1}{2}{\mathrm{\ρL}}^3{Z}_{uq}^{\′}u,\hat{g}=\frac{1}{2}{\mathrm{\ρL}}^3{M}_{uw}^{\′}u,\hat{h}=\frac{1}{2}{\mathrm{\ρL}}^4{M}_{uq}^{\′}u,\hat{k}=\frac{1}{2}{\mathrm{\ρL}}^3{u}^2{M}_{{\mathrm{\δ}}_s},$ d(t)＝[ω₁ω₂]^T；ρ为流体密度，L为UUV长度；

为在垂直加速度状态下测得的UUV无因次水动力系数；

为在纵倾角加速度状态下测得的UUV无因次水动力系数；

为在垂直加速度状态下测得的UUV无因次水动力系数；

为在纵倾角加速度状态下测得的UUV无因次水动力系数；

Z′_uw为在纵向速度u和垂向速度w状态下测得的UUV无因次水动力系数；

Z′_uq为在纵向速度u和纵倾角速度q状态下测得的UUV无因次水动力系数；

M′_uw为在纵向速度u和垂向速度w状态下测得的UUV无因次水动力系数；

M′_uq为在纵向速度u和纵倾角速度q状态下测得的UUV无因次水动力系数；

为UUV水平舵的舵效系数(下标δ_s代表水平舵)；

w为UUV状态变量垂向速度；

q为UUV状态变量纵倾角速度；

z为UUV状态变量下潜深度；

θ为UUV状态变量纵倾角；

下角标中c代表连续；

t为时间变量；

δ为水平舵角；

d为系统不确定因素及外界扰动；

A_c为连续状态空间模型的系统矩阵

B_cδ为连续状态空间模型的水平舵角δ的参数矩阵

B_cd为连续状态空间模型的在系统不确定因素及外界扰动d下的参数矩阵

C_c为连续状态空间模型的输出矩阵；

d(t)＝[ω₁ω₂]^T为t时刻系统不确定因素及外界扰动；ω₁为系统在垂向运动上不确定因素及外界扰动，ω₂为系统在纵倾运动上不确定因素及外界扰动；

步骤一三、控制器设计是建立在离散系统模型上，根据工程经验，取采样时间为T_s，将连续状态空间模型转换为离散状态空间模型：

$\begin{array}{c}x\left(k+1\right)=Ax\left(k\right)+B_{\mathrm{\δ}}{\mathrm{\δ}}_s\left(k\right)+B_{d}d\left(k\right)\\ y\left(k\right)=Cx\left(k\right)\end{array}---\left(2\right)$

其中， $A=e^{A_c{T}_s},B_{\mathrm{\δ}}={\∫}_0^{T_S}{e}^{A_c\mathrm{\τ}}{B}_{c\mathrm{\δ}}d\mathrm{\τ},B_d={\∫}_0^{T_S}{e}^{A_c\mathrm{\τ}}{B}_{cd}d\mathrm{\τ},C=C_c$

A、B_δ、B_d和C为离散状态空间模型参数矩阵；

A为离散状态空间模型系统矩阵

C为离散状态空间模型输出矩阵

τ为积分变量，

B_δ为离散化后系统的参数矩阵，

B_d为离散化后系统的参数矩阵，

T_s为采样时间，T_s为0.1s～1.2s，e为自然常数，k为离散系统的时刻；

步骤一四、给出如下定义：

△x(k)＝x(k)-x(k-1)

△δ_s(k)＝δ_s(k)-δ_s(k-1)

△d(k)＝d(k)-d(k-1)

△δ_s(·)为UUV控制输入水平舵角增量；

△d(·)为系统不确定因素及外界扰动增量；

△x(·)为离散的状态变量增量；

δ_s(·)为UUV控制输入水平舵角，

x(·)为离散的状态变量，

y(·)为UUV下潜的深度，

d(·)系统不确定因素和外界扰动

通过离散系统整理获得欠驱动UUV增量型的垂直面预测模型：

$\begin{array}{c}\mathrm{\Δ}x(k+1)=A\mathrm{\Δ}x\left(k\right)+B_{\mathrm{\δ}}{\mathrm{\Δ\δ}}_s\left(k\right)+B_d\mathrm{\Δ}d\left(k\right)\\ y\left(k\right)=C\mathrm{\Δ}x\left(k\right)+y(k-1)\end{array}---\left(3\right)$

其中，y(·)为UUV下潜的深度。

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

具体实施方式三：本实施方式与具体实施方式一或二不同的是：步骤一三中的T_s为0.5s。其它步骤及参数与具体实施方式一或二相同。

具体实施方式四：本实施方式与具体实施方式一至三之一不同的是：步骤二中将水平舵角的控制输入约束具体为：

$\begin{array}{c}{\mathrm{\δ}}_{sMIN}\≤{\mathrm{\δ}}_s\left(k+i\right)\≤{\mathrm{\δ}}_{sMAX},i=0,1,...,m-1\\ {\mathrm{\Δ\δ}}_{sMIN}\≤{\mathrm{\Δ\δ}}_s\left(k+i\right)\≤{\mathrm{\Δ\δ}}_{sMAX},i=0,1,...,m-1\end{array}---\left(4\right)$

δ_s(k+i)为第k+i时刻水平舵角；

δ_sMIN为δ_s(k+i)中舵角最小值；

δ_sMAX为δ_s(k+i)中舵角最大值；

△δ_s(k+i)为第k+i时刻水平舵角的增量；

△δ_sMIN为△δ_s(k+i)中水平舵角增量的最小值；

△δ_sMAX为△δ_s(k+i)中水平舵角增量的最大值；

m为控制时域；

将公式(4)中表示的约束条件转换为：

$\left[\begin{array}{c}T\\ -T\end{array}\right]\mathrm{\Δ}U\left(k\right)\≤{\mathrm{\γ}}_1,\left[\begin{array}{c}E\\ -E\end{array}\right]\mathrm{\Δ}U\left(k\right)\≤{\mathrm{\γ}}_2---\left(5\right)$

式中

$T={\left[\begin{array}{cccc}{I}_{n_u\×n_u}& 0& ...& 0\\ 0& I_{n_u\×n_u}& ...& 0\\ \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}\\ \\ \end{array}& \begin{array}{c}.\\ .\\ .\end{array}\\ 0& 0& ...& I_{n_u\×n_u}\end{array}\right]}_{m\×m},E={\left[\begin{array}{cccc}{I}_{n_u\×n_u}& 0& ...& 0\\ I_{n_u\×n_u}& I_{n_u\×n_u}& ...& 0\\ \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}\\ \\ \end{array}& \begin{array}{c}.\\ .\\ .\end{array}\\ I_{n_u\×n_u}& I_{n_u\×n_u}& ...& I_{n_u\×n_u}\end{array}\right]}_{m\×m}$

${\mathrm{\γ}}_1=\left[\begin{array}{c}{\mathrm{\Δ\δ}}_{sMAX}\left(k\right)\\ \begin{array}{c}.\\ .\\ .\end{array}\\ {\mathrm{\Δ\δ}}_{sMAX}\left(k+m-1\right)\\ -{\mathrm{\Δ\δ}}_{sMIN}\left(k\right)\\ \begin{array}{c}.\\ .\\ .\end{array}\\ -{\mathrm{\Δ\δ}}_{sMIN}\left(k+m-1\right)\end{array}\right],{\mathrm{\γ}}_2=\left[\begin{array}{c}{\mathrm{\δ}}_{sMAX}\left(k\right)-{\mathrm{\δ}}_s\left(k-1\right)\\ \begin{array}{c}.\\ .\\ .\end{array}\\ {\mathrm{\δ}}_{sMAX}\left(k+m-1\right)-{\mathrm{\δ}}_s\left(k-1\right)\\ {\mathrm{\δ}}_s\left(k-1\right)-{\mathrm{\δ}}_{sMIN}\left(k\right)\\ \begin{array}{c}.\\ .\\ .\end{array}\\ {\mathrm{\δ}}_s\left(k-1\right)-{\mathrm{\δ}}_{sMIN}\left(k+m-1\right)\end{array}\right]$

进一步

表示为：

H△U(k)≤γ(6)

为单位矩阵，n_u为控制输入维数，这里控制输入只有水平舵角，所以n_u＝1，其它步骤及参数与具体实施方式一至三之一相同。

具体实施方式五：本实施方式与具体实施方式一至四之一不同的是：步骤三中根据步骤一得到的欠驱动UUV增量型的垂直面预测模型和步骤二得到的水平舵角的控制输入约束条件通过模型预测控制将UUV深度控制问题转化为约束条件下的二次规划问题具体过程为：

(1)、根据式(3)所示增量型的垂直面预测模型预测的p步系统输出，由系统预测输出所构成的向量和m步控制输入序列构成的向量为：

$Y_p(k+1|k)\stackrel{def}{=}{\left[\begin{array}{c}y(k+1|k)\\ y(k+2|k)\\ \begin{array}{c}.\\ .\\ .\end{array}\\ y(k+p|k)\end{array}\right]}_{p\×1},\mathrm{\Δ}U\left(k\right)\stackrel{def}{=}{\left[\begin{array}{c}{\mathrm{\Δ\δ}}_s\left(k\right)\\ {\mathrm{\Δ\δ}}_s(k+1)\\ \begin{array}{c}.\\ .\\ .\end{array}\\ {\mathrm{\Δ\δ}}_s(k+m-1)\end{array}\right]}_{m\×1}$

其中，Y_p(·)为预测输出矩阵，下角标p为预测时域；

(2)、将系统预测输出进一步表示为：

Y_p(k+1|k)＝S_x△x(k)+Iy(k)+S_d△d(k)+S_δ△U(k)(7)

其中

$S_x={\left[\begin{array}{c}CA\\ \underset{i=1}{\overset{2}{\Σ}}{\mathrm{CA}}^i\\ \begin{array}{c}.\\ .\\ .\end{array}\\ \underset{i=1}{\overset{p}{\Σ}}{\mathrm{CA}}^i\end{array}\right]}_{p\×1},I={\left[\begin{array}{c}1\\ 1\\ .\\ .\\ .\\ 1\end{array}\right]}_{p\×1},S_d={\left[\begin{array}{c}{\mathrm{CB}}_d\\ \underset{i=1}{\overset{2}{\Σ}}{\mathrm{CA}}^{i-1}{B}_d\\ \begin{array}{c}.\\ .\\ .\end{array}\\ \underset{i=1}{\overset{p}{\Σ}}{\mathrm{CA}}^{i-1}{B}_d\end{array}\right]}_{p\×1}$

式中，

S_x为状态变量△x(k)系数矩阵；

I为UUV下潜深度y(k)系数矩阵；

S_d为系统不确定因素及外界扰动增量△d(k)系数矩阵；

S_δ为控制输入水平舵角增量序列△U(k)系数矩阵；

(3)、设UUV给定控制输出参考序列，即UUV下潜的深度R(k+1)，如下式所示：

R(k+1)＝[r(k+1)r(k+2)…r(k+p)]^T

R(·)为p步控制输出参考序列，r(·)为R(·)中每一步的控制输出参考序列；

(4)、根据MPC算法的特点，引入目标函数：

J(△U(k))＝||Γ_y(Y_p(k+1|k)-R(k+1))||²+||Γ_δ△U(k)||²(6)

其中，Γ_y和Γ_δ是对称正定加权矩阵，并给出如下定义：

Γ_y＝diag{Γ_y,1,Γ_y,2,…,Γ_y,p}_p×p

Γ_δ＝diag{Γ_δ,1,Γ_δ,2,…,Γ_δ,m}_m×m

(5)、定义E_p(k+1|k)＝R(k+1)-S_x△x(k)-Iy(k)-S_d△d(k)，则目标函数表示为：

J(△U(k))＝△U(k)^TH△U(k)-G(k+1|k)^T△U(k)(7)

其中 $H=S_{\mathrm{\δ}}^T{\mathrm{\Γ}}_y^T{\mathrm{\Γ}}_y{S}_{\mathrm{\δ}}+{\mathrm{\Γ}}_{\mathrm{\δ}}^T{\mathrm{\Γ}}_{\mathrm{\δ}},G(k+1|k)=2S_{\mathrm{\δ}}^T{\mathrm{\Γ}}_y^T{\mathrm{\Γ}}_y{E}_p(k+1|k);$

H、E_p(·)和G(·)为中间变量；

(6)、将UUV的深度控制问题转化为二次规划(Quadraticprogramming，QP)问题进行求解，即有约束二次规划问题为：

$\begin{array}{cc}\underset{\mathrm{\Δ}U\left(K\right)}{\min }& J\left(\mathrm{\Δ}U\left(k\right)\right)\\ st& h\left(\mathrm{\Δ}U\right(k\left)\right)\≤0\end{array}---\left(8\right)$

其中J(△U(k))＝△U(k)^TH△U(k)-G(k+1|k)^T△U(k)，h(△U(k))＝H△U(k)-γ。其它步骤及参数与具体实施方式一至四之一相同。

具体实施方式六：本实施方式与具体实施方式一至五之一不同的是：步骤四中利用基于罚函数的人工蜂群优化算法求解约束条件下的二次规划问题，求得蜜源的全局最优位置，即x_best＝[△δ_s(k),△δ_s(k+1),…,△δ_s(k+m-1)]^T为UUV水平舵角控制输入增量序列△U(k)具体为：

步骤四一、应用人工蜂群算法，引入惩罚函数法，将带有约束条件的优化问题转换成无约束的优化问题；

(1)、罚函数法是通过一系列罚因子，求罚函数的极小点来逼近原约束问题的最优点，针对式(8)所描述的约束优化问题，建立如下罚函数以及增广目标函数F(△U(k),σ)：

$\stackrel{\‾}{p}\left(\mathrm{\Δ}U\right(k\left)\right)={\[max\{0,h\left(\mathrm{\Δ}U\right(k\left)\right)\}\]}^2---\left(13\right)$

$F\left(\mathrm{\Δ}U\right(k),\mathrm{\σ})=J\left(\mathrm{\Δ}U\right(k\left)\right)+\mathrm{\σ}\stackrel{\‾}{p}\left(\mathrm{\Δ}U\right(k\left)\right)---\left(14\right)$

其中，σ是一个给定的相对比较大的正数，一般被叫为罚因子；

(2)、当△U(k)在约束域即满足h(△U(k))≤0时，max{0,h(△U(k))}＝0，此时有F(△U(k),σ)＝J(△U(k))；

(3)当△U(k)在约束域外时，max{0,h(△U(k))}＝h(△U(k))，此时有 $F\left(\mathrm{\Δ}U\right(k),\mathrm{\σ})=J\left(\mathrm{\Δ}U\right(k\left)\right)+\mathrm{\σ}\stackrel{\‾}{p}\left(\mathrm{\Δ}U\right(k\left)\right);$ 为罚函数，F(·)为增广目标函数；

(4)、则式(8)的带有约束条件的优化问题转化为如下无约束的优化问题：

$\begin{array}{cc}\underset{\mathrm{\Δ}U\left(K\right)}{\min }& F\left(\mathrm{\Δ}U\left(k\right),\mathrm{\σ}\right)\end{array}---\left(15\right)$

步骤四二、采用人工蜂群优化算法(Artificialbeecolony,ABC)求解式(15)所示的二次规划问题；

设在d维空间里，人工蜂群优化方法取△U(水平舵控制输入增量序列)作为蜜源地址，即第l个蜜源位置记为X_l＝(x_l1,x_l2,…,x_ld)，(每个蜜源代表优化问题的一个优化解，为最佳控制输入序列△U(k)；(△U(k)即可以通过求解式(15)获得，这里又将△U(k)与蜜源位置对应，即为X_l＝(x_l1,x_l2,…,x_ld))；将SN个蜜源与采蜜蜂初始位置一一对应，则2×SN个蜜源中每一个解的适应度值按下式求得：

${\mathrm{fit}}_l=\left\{\begin{array}{c}1/(1+f_l),f_l\&GreaterEqual;0\\ 1+abs\left(f_l\right),f_l<0\end{array}\right.---\left(11\right)$

其中，fit_l代表第l个蜜源的适应度，其越大代表着拥有更多的蜂蜜，也表明优化解质量越高，被选中的概率越高，f_l＝F(△U(k),σ)，△U(k)为UUV水平舵角控制输入增量序列；根据式(11)清晰地看出，f_l的值越小，所对应的蜜源适应度fit_l越大；abs(·)为绝对值函数；人工蜂群种群包括采蜜蜂、观察蜂和侦查蜂，x_ld为第l个蜜源位置中第d个分量；人工蜂群的数目为2×SN，采蜜蜂和观察蜂均为SN；2×SN个人工蜂群蜜源包括SN个采蜜蜂蜜源和SN个观察蜂蜜源；根据式(11)求得SN个采蜜蜂蜜源中每一个解的适应度值；l＝1,2，3，…，2×SN；2×SN为人工蜂群蜜源的总个数；

步骤四三、设置人工蜂群优化方法迭代求解二次规划问题的次数为Cycle＝1；

步骤四四、当执行第n＝1次步骤一到步骤六时，将步骤四二中2×SN个初始解的适应度值最大的解记为全局最优初始解x_best；当执行第n>1次步骤三到步骤六时，为了改善了人工蜂群算法的求解效率，取n-1次步骤五中△U(k)剩余的m-1步控制输入分量作为人工蜂群全局最优位置的全局最优初始解，即：

x_best＝[△δ_s(k+1),…,△δ_s(k+m-1),△δ_s(k+m-1)]；

步骤四五、在ABC优化算法搜索最优蜜源的过程中，采蜜蜂根据式(9)搜索新的采蜜蜂蜜源：

v_lj＝x_lj+r₁(x_lj-x_hj)+r₂(x_best,j-x_lj)(9)

其中，h是一个随机产生的整数，h∈1,2,…,SN,h≠i，j∈1,2,…,d，r₁∈[-1,1]也是一个随机数，r₂∈[0,1]，x_best,j为全局最优解的第j个元素；v_lj第l个采蜜蜂蜜源位置中第j个分量的更新值；x_lj第l个采蜜蜂蜜源位置中第j个分量；x_hj第h个采蜜蜂蜜源位置中第j个分量；

按式(11)求解新采蜜蜂蜜源的适应度值，在新采蜜蜂蜜源的适应度值和步骤四二采蜜蜂的适应度值之间实施贪婪选择，若新采蜜蜂蜜源适应度值比步骤四二采蜜蜂的适应度值大，则第l个蜜源位置中第j个分量根据式(9)求得更新值v_lj，否则，保留步骤四二采蜜蜂蜜源；

步骤四六、在步骤四五得到采蜜蜂蜜源的位置上，观察蜂按轮盘赌方法以选择概率P_l对采蜜蜂的蜜源进行选择得到观察蜂的蜜源；其中概率P_l具体为：

$P_l=\frac{{\mathrm{fit}}_l}{\underset{j=1}{\overset{SN}{\&Sigma;}}{\mathrm{fit}}_j}---\left(10\right)$

根据观察蜂的蜜源按式(9)搜索新的观察峰蜜源，并按式(11)求出新的观察峰蜜源的适应度值和观察蜂的蜜源适应度值，在新的观察峰蜜源的适应度值和观察蜂的蜜源的适应度值之间实施贪婪选择，若新的观察峰蜜源的适应度值比观察蜂的蜜源的适应度值大，则第l个蜜源位置中第j个分量为根据式(9)求得更新值v_lj，否则，保留观察峰蜜源的位置；

步骤四七、将步骤四五得到的最终的采蜜蜂蜜源适应度值和步骤四六得到的最终的观察峰蜜源的适应度值进行比较得到适应度值最大的蜜源作为全局最优解x_best；若此x_best对应的最大适应度值在0.998～1.002之间，则输出全局最优解x_best作为最终全局最优解，若此x_best对应的最大适应度值不在0.98～1.02之间，该全局最优解x_best作为当前全局最优解，同时将采蜜蜂蜜源转至步骤四八；

步骤四八、如果采蜜蜂的蜜源连续经过5～20次的步骤四一到四七后，蜜源质量依旧得不到改善，即该蜜源所对应的适应度值在所有采蜜蜂中不是最大，且两次相邻的数值变化在-0.001～0.001之间，那么采蜜蜂将会丢弃此处蜜源，并将采蜜蜂转变为侦查蜂，并根据式(12)随机产生侦查蜂的蜜源：

x_lj＝△δ_sMIN+rand(0,1)(△δ_sMAX-△δ_sMIN)(12)

其中，△δ_sMIN为变量x_lj的下边界；△δ_sMAX为变量x_lj的上边界；

侦查蜂的蜜源添加到丢弃后的采蜜蜂的蜜源中得到最终的采蜜蜂的蜜源并转至步骤四九；

步骤四九、Cycle＝Cycle+1，如果Cycle小于设定次数500，将最终的采蜜蜂的蜜源转步骤四五；反之，如果Cycle大于等于设定次数500，输出全局最优解x_best作为最终全局最优解。其它步骤及参数与具体实施方式一至五之一相同。

Claims (6)

1.一种基于人工蜂群优化的约束模型预测的欠驱动UUV深度控制方法，其特征在于：一种基于人工蜂群优化的约束模型预测的欠驱动UUV深度控制方法具体是按照以下步骤进行的：

步骤一、将UUV垂直面模型整理为状态空间模型，将状态空间模型离散化得到欠驱动UUV增量型的垂直面预测模型；

步骤二、在k时刻，根据水平舵特性建立水平舵角的控制输入约束；所述的水平舵角的控制输入约束包括幅值约束和每一拍动作的增量约束；并将水平舵角的控制输入约束条件统一处理为:

H△U(k)≤γ

其中，△U为水平舵控制输入增量序列；H＝[T^T-T^TΕ^T-Ε^T]^T,γ＝[γ₁γ₂]^T；

γ、H、T、γ₁、Ε和γ₂为中间矩阵；

步骤三、根据步骤一得到的欠驱动UUV增量型的垂直面预测模型和步骤二得到的水平舵角的控制输入约束条件通过模型预测控制将UUV深度控制问题转化为约束条件下的二次规划问题如下：

$\begin{array}{cc}\underset{\mathrm{\&Delta;}U\left(K\right)}{\min }& J\left(\mathrm{\&Delta;}U\right(k\left)\right)\end{array}$

sth(△U(k))≤0(8)

式中，J(△U(k))＝△U(k)^TH△U(k)-G(k+1|k)^T△U(k)，h(△U(k))＝H△U(k)-γ；J(·)为反映△U(k)的性能指标，h(·)为描述函数，G(·)为中间变量， $G(k+1|k)=2S_{\mathrm{\&delta;}}^T{\mathrm{\&Gamma;}}_y^T{\mathrm{\&Gamma;}}_y{E}_p(k+1|k);$

步骤四、利用基于罚函数的人工蜂群优化算法求解约束条件下的二次规划问题，求得蜜源的全局最优位置，即x_best＝[△δ_s(k),△δ_s(k+1),…,△δ_s(k+m-1)]^T为UUV水平舵角控制输入增量序列△U(k)；其中，△δ_s(k+m-1)为第k+m-1步水平舵角的增量，m为控制时域；

步骤五、在求解出蜜源最终全局最优解x_best后，令△U(k)＝x_best ^T，然后取优化解序列△U(k)的第一个分量，并加上k-1时刻的控制输入作为k时刻的控制输入:

$\begin{array}{c}{\mathrm{\&Delta;\&delta;}}_s\left(k\right)=\left[\begin{array}{cccc}{I}_{n_u\&times;n_u}& 0& \mathrm{...}& 0\end{array}\right]\mathrm{\&Delta;}U\left(k\right)\\ {\mathrm{\&delta;}}_s\left(k\right)={\mathrm{\&Delta;\&delta;}}_s\left(k\right)+{\mathrm{\&delta;}}_s(k-1)\end{array};$

步骤六、在k时刻的控制输入δ_s(k)作用下测得新的状态变量和UUV下潜深度，当UUV未达到指定UUV下潜的深度R(k+1)时，将测得新的状态变量和UUV下潜深度转步骤三，直至当UUV达到指定UUV下潜的深度R(k+1)完成下潜作业；其中，新的状态变量包括UUV状态变量垂向速度、UUV状态变量纵倾角速度、UUV状态变量下潜深度和UUV状态变量纵倾角。

2.根据权利要求1所述一种基于人工蜂群优化的约束模型预测的欠驱动UUV深度控制方法，其特征在于：步骤一中将UUV垂直面模型整理为状态空间模型，将状态空间模型离散化得到欠驱动UUV增量型的垂直面预测模型具体过程为：

步骤一一、建立了UUV的坐标系统，其中，UUV的坐标系统包括固定坐标系E-ξηζ和运动坐标系O-XYZ；

步骤一二、取状态变量水平舵的舵角δ_s(t)和t时刻UUV下潜深度y(t)建立UUV垂直面的状态空间方程即连续的状态空间模型:

$\begin{array}{c}\stackrel{\&CenterDot;}{x}\left(t\right)=A_{c}x\left(t\right)+B_{c\mathrm{\&delta;}}{\mathrm{\&delta;}}_s\left(t\right)+B_{cd}d\left(t\right)\\ y\left(t\right)=C_{c}x\left(t\right)\end{array}---\left(1\right)$

式中， $A_c={\left[\begin{array}{cccc}\hat{a}& \hat{b}& 0& 0\\ \hat{c}& \hat{d}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]}^{-1}\left[\begin{array}{cccc}\hat{e}& \hat{f}& 0& 0\\ \hat{g}& \hat{h}& 0& 0\\ 0& 1& 0& 0\\ 1& 0& -u& 0\end{array}\right],$ $B_{c\mathrm{\&delta;}}={\left[\begin{array}{cccc}\hat{a}& \hat{b}& 0& 0\\ \hat{c}& \hat{d}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]}^{-1}\left[\begin{array}{c}0\\ \hat{k}\\ 0\\ 0\end{array}\right],$ $B_{cd}=\left[\begin{array}{cc}1& 0\\ 0& 1\\ 0& 0\\ 0& 0\end{array}\right],$ C_c＝[0001]； $\hat{a}=m-\frac{1}{2}{\mathrm{\&rho;L}}^3{Z}_{\stackrel{\&CenterDot;}{w}}^{\&prime;},$ $\hat{b}=\frac{1}{2}{\mathrm{\&rho;L}}^4{Z}_{\stackrel{\&CenterDot;}{q}}^{\&prime;},$ $\hat{c}=\frac{1}{2}{\mathrm{\&rho;L}}^4{M}_{\stackrel{\&CenterDot;}{w}}^{\&prime;},$ $\hat{d}=J_y-\frac{1}{2}{\mathrm{\&rho;L}}^5{M}_{\stackrel{\&CenterDot;}{q}}^{\&prime;},$ $\hat{e}=\frac{1}{2}{\mathrm{\&rho;L}}^2{Z}_{uw}^{\&prime;}u,$ $\hat{f}=mu+\frac{1}{2}{\mathrm{\&rho;L}}^3{Z}_{uq}^{\&prime;}u,$ $\hat{g}=\frac{1}{2}{\mathrm{\&rho;L}}^3{M}_{uw}^{\&prime;}u,$ $\hat{h}=\frac{1}{2}{\mathrm{\&rho;L}}^4{M}_{uq}^{\&prime;}u,$ $\hat{k}=\frac{1}{2}{\mathrm{\&rho;L}}^3{u}^2{M}_{{\mathrm{\&delta;}}_s},$ d(t)＝[ω₁ω₂]^T；ρ为流体密度，L为UUV长度；

为在垂直加速度状态下测得的UUV无因次水动力系数；

为在纵倾角加速度状态下测得的UUV无因次水动力系数；

为在垂直加速度状态下测得的UUV无因次水动力系数；

为在纵倾角加速度状态下测得的UUV无因次水动力系数；

Z′_uw为在纵向速度u和垂向速度w状态下测得的UUV无因次水动力系数；

Z′_uq为在纵向速度u和纵倾角速度q状态下测得的UUV无因次水动力系数；

M′_uw为在纵向速度u和垂向速度w状态下测得的UUV无因次水动力系数；

M′_uq为在纵向速度u和纵倾角速度q状态下测得的UUV无因次水动力系数；

为UUV水平舵的舵效系；

w为UUV状态变量垂向速度；

q为UUV状态变量纵倾角速度；

z为UUV状态变量下潜深度；

θ为UUV状态变量纵倾角；

下角标中c代表连续；

t为时间变量；

δ为水平舵角；

d为系统不确定因素及外界扰动；

A_c为连续状态空间模型的系统矩阵

B_cδ为连续状态空间模型的水平舵角δ的参数矩阵

B_cd为连续状态空间模型的在系统不确定因素及外界扰动d下的参数矩阵

C_c为连续状态空间模型的输出矩阵；

d(t)＝[ω₁ω₂]^T为t时刻系统不确定因素及外界扰动；ω₁为系统在垂向运动上不确定因素及外界扰动，ω₂为系统在纵倾运动上不确定因素及外界扰动；

步骤一三、取采样时间为T_s，将连续状态空间模型转换为离散状态空间模型：

$\begin{array}{c}x(x+1)=Ax\left(k\right)+B_{\mathrm{\&delta;}}{\mathrm{\&delta;}}_s\left(k\right)+B_{d}d\left(k\right)\\ y\left(k\right)=Cx\left(k\right)\end{array}---\left(2\right)$

其中， $A=e^{A_c{T}_s},$ $B_{\mathrm{\&delta;}}={\&Integral;}_0^{T_S}{e}^{A_c\mathrm{\&tau;}}{B}_{c\mathrm{\&delta;}}d\mathrm{\&tau;},$ $B_d={\&Integral;}_0^{T_S}{e}^{A_c\mathrm{\&tau;}}{B}_{cd}d\mathrm{\&tau;},$ C＝C_c

A、B_δ、B_d和C为离散状态空间模型参数矩阵；

A为离散状态空间模型系统矩阵

C为离散状态空间模型输出矩阵

τ为积分变量，

B_δ为离散化后系统的参数矩阵，

B_d为离散化后系统的参数矩阵，

T_s为采样时间，T_s为0.1s～1.2s，e为自然常数，k为离散系统的时刻；

步骤一四、给出如下定义：

△x(k)＝x(k)-x(k-1)

△δ_s(k)＝δ_s(k)-δ_s(k-1)

△d(k)＝d(k)-d(k-1)

△δ_s(·)为UUV控制输入水平舵角增量；

△d(·)为系统不确定因素及外界扰动增量；

△x(·)为离散的状态变量增量；

δ_s(·)为UUV控制输入水平舵角，

x(·)为离散的状态变量，

y(·)为UUV下潜的深度，

d(·)系统不确定因素和外界扰动

通过离散系统整理获得欠驱动UUV增量型的垂直面预测模型：

$\begin{array}{c}\mathrm{\&Delta;}x(k+1)=A\mathrm{\&Delta;}x\left(k\right)+B_{\mathrm{\&delta;}}{\mathrm{\&Delta;\&delta;}}_s\left(k\right)+B_d\mathrm{\&Delta;}d\left(k\right)\\ y\left(k\right)=C\mathrm{\&Delta;}x\left(k\right)+y(k-1)\end{array}---\left(3\right)$

其中，y(·)为UUV下潜的深度。

3.根据权利要求2所述一种基于人工蜂群优化的约束模型预测的欠驱动UUV深度控制方法，其特征在于：步骤一三中的T_s为0.5s。

4.根据权利要求3所述一种基于人工蜂群优化的约束模型预测的欠驱动UUV深度控制方法，其特征在于：步骤二中将水平舵角的控制输入约束具体为：

$\begin{array}{c}{\mathrm{\&delta;}}_{sMIN}\&le;{\mathrm{\&delta;}}_s(k+i)\&le;{\mathrm{\&delta;}}_{sMAX},i=0,1,\mathrm{...},m-1\\ {\mathrm{\&Delta;\&delta;}}_{sMIN}\&le;{\mathrm{\&Delta;\&delta;}}_s(k+i)\&le;{\mathrm{\&Delta;\&delta;}}_{sMAX},i=0,1,\mathrm{...},m-1\end{array}---\left(4\right)$

δ_s(k+i)为第k+i时刻水平舵角；

δ_sMIN为δ_s(k+i)中舵角最小值；

δ_sMAX为δ_s(k+i)中舵角最大值；

△δ_s(k+i)为第k+i时刻水平舵角的增量；

△δ_sMIN为△δ_s(k+i)中水平舵角增量的最小值；

△δ_sMAX为△δ_s(k+i)中水平舵角增量的最大值；

m为控制时域；

将公式(4)中表示的约束条件转换为：

$\left[\begin{array}{c}T\\ -T\end{array}\right]\mathrm{\&Delta;}U\left(k\right)\&le;{\mathrm{\&gamma;}}_1,\left[\begin{array}{c}E\\ -E\end{array}\right]\mathrm{\&Delta;}U\left(k\right)\&le;{\mathrm{\&gamma;}}_2---\left(5\right)$

式中

$T={\left[\begin{array}{cccc}{I}_{n_u\&times;n_u}& 0& \mathrm{...}& 0\\ 0& I_{n_u\&times;n_u}& \mathrm{...}& 0\\ .& .& & .\\ .& .& & .\\ .& .& & .\\ 0& 0& \mathrm{...}& I_{n_u\&times;n_u}\end{array}\right]}_{m\&times;m},$ $E={\left[\begin{array}{cccc}{I}_{n_u\&times;n_u}& 0& \mathrm{...}& 0\\ I_{n_u\&times;n_u}& I_{n_u\&times;n_u}& \mathrm{...}& 0\\ .& .& & .\\ .& .& & .\\ .& .& & .\\ I_{n_u\&times;n_u}& I_{n_u\&times;n_u}& \mathrm{...}& I_{n_u\&times;n_u}\end{array}\right]}_{m\&times;m}$

${\mathrm{\&gamma;}}_1=\left[\begin{array}{c}{\mathrm{\&Delta;\&delta;}}_{sMAX}\left(k\right)\\ .\\ .\\ .\\ {\mathrm{\&Delta;\&delta;}}_{sMAX}(k+m-1)\\ -{\mathrm{\&Delta;\&delta;}}_{sMIN}\left(k\right)\\ .\\ .\\ .\\ -{\mathrm{\&Delta;\&delta;}}_{sMIN}(k+m-1)\end{array}\right],$ ${\mathrm{\&gamma;}}_2=\left[\begin{array}{c}{\mathrm{\&delta;}}_{sMAX}\left(k\right)-{\mathrm{\&delta;}}_s(k-1)\\ .\\ .\\ .\\ {\mathrm{\&delta;}}_{sMAX}(k+m-1)-{\mathrm{\&delta;}}_s(k-1)\\ {\mathrm{\&delta;}}_s(k-1)-{\mathrm{\&delta;}}_{sMIN}\left(k\right)\\ .\\ .\\ .\\ {\mathrm{\&delta;}}_s(k-1)-{\mathrm{\&delta;}}_{sMIN}(k+m-1)\end{array}\right]$

进一步

表示为：

H△U(k)≤γ(6)

为单位矩阵，n_u为控制输入维数，n_u＝1，

5.根据权利要求4所述一种基于人工蜂群优化的约束模型预测的欠驱动UUV深度控制方法，其特征在于：步骤三中根据步骤一得到的欠驱动UUV增量型的垂直面预测模型和步骤二得到的水平舵角的控制输入约束条件通过模型预测控制将UUV深度控制问题转化为约束条件下的二次规划问题具体过程为：

(1)、根据式(3)所示增量型的垂直面预测模型预测的p步系统输出，由系统预测输出所构成的向量和m步控制输入序列构成的向量为：

$Y_p(k+1|k)\underset{\&OverBar;}{\underset{\&OverBar;}{def}}{\left[\begin{array}{c}y(k+1|k)\\ y(k+2|k)\\ .\\ .\\ .\\ y(k+p|k)\end{array}\right]}_{p\&times;1},$ $\mathrm{\&Delta;}U\left(k\right)\underset{\&OverBar;}{\underset{\&OverBar;}{def}}{\left[\begin{array}{c}{\mathrm{\&Delta;\&delta;}}_s\left(k\right)\\ {\mathrm{\&Delta;\&delta;}}_s(k+1)\\ .\\ .\\ .\\ {\mathrm{\&Delta;\&delta;}}_s(k+m-1)\end{array}\right]}_{m\&times;1}$

其中，Y_p(·)为预测输出矩阵，下角标p为预测时域；

(2)、将系统预测输出进一步表示为：

Y_p(k+1|k)＝S_x△x(k)+Iy(k)+S_d△d(k)+S_δ△U(k)(7)

其中

$S_x={\left[\begin{array}{c}CA\\ \underset{i=1}{\overset{2}{\mathrm{\&Sigma;}}}{\mathrm{CA}}^i\\ .\\ .\\ .\\ \underset{i=1}{\overset{p}{\&Sigma;}}{\mathrm{CA}}^i\end{array}\right]}_{p\&times;1},$ $I={\left[\begin{array}{c}1\\ 1\\ .\\ .\\ .\\ 1\end{array}\right]}_{p\&times;1},$ $S_d={\left[\begin{array}{c}{\mathrm{CB}}_d\\ \underset{i=1}{\overset{2}{\&Sigma;}}{\mathrm{CA}}^{i-1}{B}_d\\ .\\ .\\ .\\ \underset{i=1}{\overset{p}{\&Sigma;}}{\mathrm{CA}}^{i-1}{B}_d\end{array}\right]}_{p\&times;1}$

式中，

S_x为状态变量△x(k)系数矩阵；

I为UUV下潜深度y(k)系数矩阵；

S_d为系统不确定因素及外界扰动增量△d(k)系数矩阵；

S_δ为控制输入水平舵角增量序列△U(k)系数矩阵；

(3)、设UUV给定控制输出参考序列，即UUV下潜的深度R(k+1)，如下式所示：

R(k+1)＝[r(k+1)r(k+2)…r(k+p)]^T

R(·)为p步控制输出参考序列，r(·)为R(·)中每一步的控制输出参考序列；

(4)、根据MPC算法的特点，引入目标函数：

J(△U(k))＝||Γ_y(Y_p(k+1|k)-R(k+1))||²+||Γ_δ△U(k)||²(6)

其中，Γ_y和Γ_δ是对称正定加权矩阵，并给出如下定义：

Γ_y＝diag{Γ_y,1,Γ_y,2,…,Γ_y,p}_p×p

Γ_δ＝diag{Γ_δ,1,Γ_δ,2,…,Γ_δ,m}_m×m

(5)、定义E_p(k+1|k)＝R(k+1)-S_x△x(k)-Iy(k)-S_d△d(k)，则目标函数表示为：

J(△U(k))＝△U(k)^TH△U(k)-G(k+1|k)^T△U(k)(7)

其中 $H=S_{\mathrm{\&delta;}}^T{\mathrm{\&Gamma;}}_y^T{\mathrm{\&Gamma;}}_y{S}_{\mathrm{\&delta;}}+{\mathrm{\&Gamma;}}_{\mathrm{\&delta;}}^T{\mathrm{\&Gamma;}}_{\mathrm{\&delta;}},$ $G(k+1|k)=2S_{\mathrm{\&delta;}}^T{\mathrm{\&Gamma;}}_y^T{\mathrm{\&Gamma;}}_y{E}_p(k+1|k);$

H、E_p(·)和G(·)为中间变量；

(6)、将UUV的深度控制问题转化为二次规划问题进行求解，即有约束二次规划问题为：

$\begin{array}{cc}\underset{\mathrm{\&Delta;}U\left(K\right)}{\min }& J\left(\mathrm{\&Delta;}U\right(k\left)\right)\\ st& h\left(\mathrm{\&Delta;}U\right(k\left)\right)\&le;0\end{array}---\left(8\right)$

其中J(△U(k))＝△U(k)^TH△U(k)-G(k+1|k)^T△U(k)，h(△U(k))＝H△U(k)-γ。

6.根据权利要求5所述一种基于人工蜂群优化的约束模型预测的欠驱动UUV深度控制方法，其特征在于：步骤四中利用基于罚函数的人工蜂群优化算法求解约束条件下的二次规划问题，求得蜜源的全局最优位置，即x_best＝[△δ_s(k),△δ_s(k+1),…,△δ_s(k+m-1)]^T为UUV水平舵角控制输入增量序列△U(k)具体为：

步骤四一、应用人工蜂群算法，引入惩罚函数法，将带有约束条件的优化问题转换成无约束的优化问题；

(1)、针对式(8)所描述的约束优化问题，建立如下罚函数以及增广目标函数F(△U(k),σ)：

$\stackrel{\&OverBar;}{p}\left(\mathrm{\&Delta;}U\right(k\left)\right)={\&lsqb;max\{0,h\left(\mathrm{\&Delta;}U\right(k\left)\right)\}\&rsqb;}^2---\left(13\right)$

$F\left(\mathrm{\&Delta;}U\right(k),\mathrm{\&sigma;})=J\left(\mathrm{\&Delta;}U\right(k\left)\right)+\mathrm{\&sigma;}\stackrel{\&OverBar;}{p}\left(\mathrm{\&Delta;}U\right(k\left)\right)---\left(14\right)$

其中，σ为罚因子；

(2)、当△U(k)在约束域即满足h(△U(k))≤0时，max{0,h(△U(k))}＝0，此时有F(△U(k),σ)＝J(△U(k))；

(3)当△U(k)在约束域外时，max{0,h(△U(k))}＝h(△U(k))，此时有 $F\left(\mathrm{\&Delta;}U\right(k),\mathrm{\&sigma;})=J\left(\mathrm{\&Delta;}U\right(k\left)\right)+\mathrm{\&sigma;}\stackrel{\&OverBar;}{p}\left(\mathrm{\&Delta;}U\right(k\left)\right);$ 为罚函数，F(·)为增广目标函数；

(4)、则式(8)的带有约束条件的优化问题转化为如下无约束的优化问题：

$\begin{array}{cc}\underset{\mathrm{\&Delta;}U\left(K\right)}{\min }& F\left(\mathrm{\&Delta;}U\right(k),\mathrm{\&sigma;})\end{array}---\left(15\right)$

步骤四二、采用人工蜂群优化算法求解式(15)所示的二次规划问题；

设在d维空间里，人工蜂群优化方法取△U作为蜜源地址，即第l个蜜源位置记为X_l＝(x_l1,x_l2,…,x_ld)；将SN个蜜源与采蜜蜂初始位置一一对应，则2×SN个蜜源中每一个解的适应度值按下式求得：

${\mathrm{fit}}_l=\left\{\begin{array}{c}1/(1+f_l),f_l\&GreaterEqual;0\\ 1+abs\left(f_l\right),f_l<0\end{array}\right.---\left(11\right)$

其中，fit_l代表第l个蜜源的适应度，△U(k)为UUV水平舵角控制输入增量序列；

abs(·)为绝对值函数；人工蜂群种群包括采蜜蜂、观察蜂和侦查蜂，x_ld为第l个蜜源位置中第d个分量；人工蜂群的数目为2×SN，采蜜蜂和观察蜂均为SN；2×SN个人工蜂群蜜源包括SN个采蜜蜂蜜源和SN个观察蜂蜜源；l＝1,2，3，…，2×SN；2×SN为人工蜂群蜜源的总个数；

步骤四三、设置人工蜂群优化方法迭代求解二次规划问题的次数为Cycle＝1；

步骤四四、当执行第n＝1次步骤一到步骤六时，将步骤四二中2×SN个初始解的适应度值最大的解记为全局最优初始解x_best；当执行第n>1次步骤三到步骤六时，取n-1次步骤五中△U(k)剩余的m-1步控制输入分量作为人工蜂群全局最优位置的全局最优初始解，即：

x_best＝[△δ_s(k+1),…,△δ_s(k+m-1),△δ_s(k+m-1)]；

步骤四五、在ABC优化算法搜索最优蜜源的过程中，采蜜蜂根据式(9)搜索新的采蜜蜂蜜源：

v_lj＝x_lj+r₁(x_lj-x_hj)+r₂(x_best,j-x_lj)(9)

其中，h是一个随机产生的整数，h∈1,2,…,SN,h≠i，j∈1,2,…,d，r1∈[-1,1]也是一个随机数，r₂∈[0,1]，x_best,j为全局最优解的第j个元素；v_lj第l个采蜜蜂蜜源位置中第j个分量的更新值；x_lj第l个采蜜蜂蜜源位置中第j个分量；x_hj第h个采蜜蜂蜜源位置中第j个分量；

按式(11)求解新采蜜蜂蜜源的适应度值，在新采蜜蜂蜜源的适应度值和步骤四二采蜜蜂的适应度值之间实施贪婪选择，若新采蜜蜂蜜源适应度值比步骤四二采蜜蜂的适应度值大，则第l个蜜源位置中第j个分量根据式(9)求得更新值v_lj，否则，保留步骤四二采蜜蜂蜜源；

步骤四六、在步骤四五得到采蜜蜂蜜源的位置上，观察蜂按轮盘赌方法以选择概率P_l对采蜜蜂的蜜源进行选择得到观察蜂的蜜源；其中概率P_l具体为：

$P_l=\frac{{\mathrm{fit}}_l}{\underset{j=1}{\overset{SN}{\&Sigma;}}{\mathrm{fit}}_j}---\left(10\right)$

根据观察蜂的蜜源按式(9)搜索新的观察峰蜜源，并按式(11)求出新的观察峰蜜源的适应度值和观察蜂的蜜源适应度值，在新的观察峰蜜源的适应度值和观察蜂的蜜源的适应度值之间实施贪婪选择，若新的观察峰蜜源的适应度值比观察蜂的蜜源的适应度值大，则第l个蜜源位置中第j个分量为根据式(9)求得更新值v_lj，否则，保留观察峰蜜源的位置；

步骤四七、将步骤四五得到的最终的采蜜蜂蜜源适应度值和步骤四六得到的最终的观察峰蜜源的适应度值进行比较得到适应度值最大的蜜源作为全局最优解x_best；若此x_best对应的最大适应度值在0.998～1.002之间，则输出全局最优解x_best作为最终全局最优解，若此x_best对应的最大适应度值不在0.98～1.02之间，该全局最优解x_best作为当前全局最优解，同时将采蜜蜂蜜源转至步骤四八；

步骤四八、如果采蜜蜂的蜜源连续经过5～20次的步骤四一到四七后，即该蜜源所对应的适应度值在所有采蜜蜂中不是最大，且两次相邻的数值变化在-0.001～0.001之间，那么采蜜蜂将会丢弃此处蜜源，并将采蜜蜂转变为侦查蜂，并根据式(12)随机产生侦查蜂的蜜源：

x_lj＝△δ_sMIN+rand(0,1)(△δ_sMAX-△δ_sMIN)(12)

其中，△δ_sMIN为变量x_lj的下边界；△δ_sMAX为变量x_lj的上边界；

侦查蜂的蜜源添加到丢弃后的采蜜蜂的蜜源中得到最终的采蜜蜂的蜜源并转至步骤四九；

步骤四九、Cycle＝Cycle+1，如果Cycle小于设定次数500，将最终的采蜜蜂的蜜源转步骤四五；反之，如果Cycle大于等于设定次数500，输出全局最优解x_best作为最终全局最优解。