CN105700356A - 基于鲁棒约束模型预测控制的uuv对线控位回收方法 - Google Patents

Abstract

基于鲁棒约束模型预测控制的UUV对线控位回收方法，涉及一种水下母船背驮式搭载UUV的自主回收方法。为了解决现有水下回收UUV的方法主要是针对静止的回收平台对UUV进行回收的问题以及回收过程中由于超调过大、调节时间过长等不稳定因素会使得UUV与回收平台发生碰撞的问题；本发明首先获得UUV与母船状态信息，构建误差预测模型，并计算局部时刻预测误差动态方程的系统矩阵和局部时刻状态预测误差；在约束条件下进行滚动优化，求解优化问题；然后计算控制量并新预测模型、重复滚动优化过程，直至实现与母船的对线控位过程。本发明适用于水下母船背驮式搭载UUV的自主回收。

Description

基于鲁棒约束模型预测控制的UUV对线控位回收方法

技术领域

本发明涉及一种水下母船背驮式搭载UUV的自主回收方法。

背景技术

UUV技术目前已经作为探索海洋空间资源的最重要手段之一，UUV虽然具有隐蔽性、深潜性能、安全性等优点，但是由于UUV受到自身携带的能源限制，使其工作范围和时间受到严格的限制，因此要通过UUV水下回收实现在水下完成能源补给和上传下载数据，这可以大大提高UUV的工作范围和时间。

由于鲁棒约束模型预测控制方法能够有效的处理较复杂的状态约束和控制约束以及对参数不确定性系统有非常好的控制性能，因此本发明采用基于鲁棒约束模型预测控制算法的精确对线控位方法对UUV进行水下回收。

哈尔滨工程大学的张伟等人在《船舶工程》(2013年第05期)上发表了《基于对线控位策略的UUV回收运动控制研究》，该文章对于水下坞舱回收UUV的过程提出了一种基于对线控位策略的回收方法，并设计UUV回收的位置和姿态的灰色预测PID控制器，减小了UUV回收控制中的超调量和调整时间，但是该方法针对的是水下静止平台，而非移动平台，存在一定的局限性。

发明内容

本发明是为了解决现有水下回收UUV的方法主要是针对静止的回收平台对UUV进行回收的问题以及回收过程中由于超调过大、调节时间过长等不稳定因素会使得UUV与回收平台发生碰撞的问题。

基于鲁棒约束模型预测控制的UUV对线控位回收方法，包括以下步骤：

步骤1、通过对线控位方法得到UUV与母船的位置偏差值△x,△y,△z、航向偏差值△ψ和纵倾角偏差值△θ；并将△x,△y,△z，△ψ，△θ作为初始化值；

步骤2、获取当前时刻UUV状态：

通过UUV自身的一系列传感器等得到当前时刻状态：u、v、w分别为纵向、横向和垂向速度(m/s)，p、q、r分别为横摇角、纵摇角、艏摇角速度(rad/s)，ξ、η、ζ分别为UUV相对于固定坐标系的位置(m)，θ、ψ为横摇角、纵摇角、艏摇角(rad)；

步骤3、构建误差预测模型，并计算局部时刻预测误差动态方程的系统矩阵Ω_k和局部时刻状态预测误差

利用UUV六自由度动力学方程和运动学方程构造动态方程：

$\stackrel{\·}{x}\left(t\right)=f\left(x\right(t),\mathrm{\τ}(t),\mathrm{\ω}(t\left)\right)$

y(t)＝h(x(t),τ(t))

式中，x(t)是UUV状态向量，y(t)是输出向量，τ(t)是UUV控制输入向量，ω(t)是干扰向量；为x(t)的一阶导数；t表示时间；

x(t)＝[u(t)v(t)w(t)p(t)q(t)r(t)]^T

τ(t)＝[X_prop(t)Y_prop(t)Z_prop(t)K_prop(t)M_prop(t)N_prop(t)]^T

其中，X_prop,Y_prop,Z_prop为UUV受到的纵向、横向、垂向的作用力，K_prop,M_prop,N_prop为UUV受到的纵向、横向、垂向作用力的力矩；u(t)、v(t)、w(t)、p(t)、q(t)、r(t)、ξ(t)、η(t)、ζ(t)、θ(t)、ψ(t)、X_prop(t)、Y_prop(t)、Z_prop(t)、K_prop(t)、M_prop(t)、N_prop(t)均表示对应参数为时间t的函数；

定义UUV期望状态

x_d(t)＝[u_d(t)v_d(t)w_d(t)p_d(t)q_d(t)r_d(t)]^T

其中，所有带角标d的参数均表示对应参数的期望值；

则实际和期望之间误差表示为

$x_e\left(t\right)=x\left(t\right)-x_d\left(t\right)=\left[\begin{array}{c}u\left(t\right)-u_d\left(t\right)\\ v\left(t\right)-v_d\left(t\right)\\ w\left(t\right)-w_d\left(t\right)\\ p\left(t\right)-p_d\left(t\right)\\ q\left(t\right)-q_d\left(t\right)\\ r\left(t\right)-r_d\left(t\right)\end{array}\right],$

ψ(t)-ψ_d(t)＝△ψ，θ(t)-θ_d(t)＝△θ，

因此

建立误差动态方程

${\stackrel{\·}{x}}_e\left(t\right)=A\left(t\right)x_e\left(t\right)+B_1\left(t\right)\mathrm{\ω}\left(t\right)+B_2\left(t\right)\mathrm{\τ}\left(t\right)$

y_e(t)＝C(t)x_e(t)+D(t)τ(t)(1)

式中，A(t)、B₁(t)、B₂(t)、C(t)、D(t)均为系数矩阵；

方程(1)的系统矩阵为：

$\left(\begin{array}{ccc}A\left(t\right)& B_1\left(t\right)& B_2\left(t\right)\end{array}\right)=\left(\begin{array}{ccc}\frac{\∂f}{\∂x}& \frac{\∂f}{\∂\mathrm{\ω}}& \frac{\∂f}{\∂\mathrm{\τ}}\end{array}\right)|\left(x_d\right(t),0,\mathrm{\τ}(t\left)\right)$

$\left(\begin{array}{cc}C\left(t\right)& D\left(t\right)\end{array}\right)=\left(\begin{array}{cc}\frac{\∂h}{\∂x}& \frac{\∂h}{\∂\mathrm{\τ}}\end{array}\right)|\left(x_d\right(t),\mathrm{\τ}(t\left)\right)$

在局部时刻[t_k,t_k+1)的预测误差动态方程为：

${\stackrel{\·}{\stackrel{\‾}{x}}}_e\left(t\right)=A_k{\stackrel{\‾}{x}}_e\left(t\right)+B_{1k}\mathrm{\ω}\left(t\right)+B_{2k}\mathrm{\τ}\left(t\right)$

${\stackrel{\‾}{y}}_e\left(t\right)=C_k{\stackrel{\‾}{x}}_e\left(t\right)+D_k\mathrm{\τ}\left(t\right)---\left(2\right)$

式中，所有带角标k参数的均表示局部时刻的对应参数，表示x_e(t)、y_e(t)局部时刻的预测值；

方程(2)的系统矩阵

$\left(\begin{array}{ccc}{A}_k& B_{1k}& B_{2k}\\ C_k& D_k& 0\end{array}\right)=\left(\begin{array}{ccc}A\left(t_k\right)& B_1\left(t_k\right)& B_2\left(t_k\right)\\ C\left(t_k\right)& D\left(t_k\right)& 0\end{array}\right)={\mathrm{\Ω}}_k$

由于预测模型采用了线性化误差系统模型近似描述非线性系统的局部特征，因此在每个采样时刻需要更新系统矩阵Ω_k，获得新的预测模型完成预测过程；

步骤4、针对滚动优化问题在约束条件下进行滚动优化，求解优化问题，求可行解(γ_k,Q_k,Y_k)；

步骤5、针对优化问题有可行解(γ_k,Q_k,Y_k)，根据下式计算控制输入向量τ(t)，

$K_k=Y_k{Q}_k^{-1}$

$\mathrm{\τ}\left(t\right)=K_k{\stackrel{\‾}{x}}_e\left(t\right),\∀t\∈\[t_k,t_{k+1}),k\≥0$

将控制输入向量作用于方程(2)的系统，然后令k＝k+1，返回步骤2更新预测模型、重复滚动优化过程，直至实现UUV对线控位回收。

本发明具有以下效果：

本发明采用的鲁棒约束模型预测控制方法不仅能够针对静止回收平台对UUV进行回收，还能针对低速运动的回收平台对UUV进行回收，通过鲁棒约束模型预测控制，使得回收过程中超调量大大减小，调节时间加快，使得UUV能够更加稳定、安全的进行回收。

附图说明

图1为母船运动状态下与UUV的相对初始位置示意图；

图2为UUV对接示意图；

图3为鲁棒约束模型预测控制结构框图；

图4为鲁棒模型预测控制实现流程；

图5为仿真实验中UUV与母船的位置误差曲线图。

具体实施方式

具体实施方式一：

基于鲁棒约束模型预测控制的UUV对线控位回收方法，包括以下步骤：

步骤1、通过对线控位方法得到UUV与母船的位置偏差值△x,△y,△z、航向偏差值△ψ和纵倾角偏差值△θ；并将△x,△y,△z，△ψ，△θ作为初始化值；

步骤2、获取当前时刻UUV状态：

通过UUV自身的一系列传感器等得到当前时刻状态：u、v、w分别为纵向、横向和垂向速度(m/s)，p、q、r分别为横摇角、纵摇角、艏摇角速度(rad/s)，ξ、η、ζ分别为UUV相对于固定坐标系的位置(m)，θ、ψ为横摇角、纵摇角、艏摇角(rad)；

步骤3、构建误差预测模型，并计算局部时刻预测误差动态方程的系统矩阵Ω_k和局部时刻状态预测误差

利用UUV六自由度动力学方程和运动学方程构造动态方程：

$\stackrel{\·}{x}\left(t\right)=f\left(x\right(t),\mathrm{\τ}(t),\mathrm{\ω}(t\left)\right)$

y(t)＝h(x(t),τ(t))

式中，x(t)是UUV状态向量，y(t)是输出向量，τ(t)是UUV控制输入向量，ω(t)是干扰向量；为x(t)的一阶导数；t表示时间；

x(t)＝[u(t)v(t)w(t)p(t)q(t)r(t)]^T

τ(t)＝[X_prop(t)Y_prop(t)Z_prop(t)K_prop(t)M_prop(t)N_prop(t)]^T

其中，X_prop,Y_prop,Z_prop为UUV受到的纵向、横向、垂向的作用力，K_prop,M_prop,N_prop为UUV受到的纵向、横向、垂向作用力的力矩；u(t)、v(t)、w(t)、p(t)、q(t)、r(t)、ξ(t)、η(t)、ζ(t)、θ(t)、ψ(t)、X_prop(t)、Y_prop(t)、Z_prop(t)、K_prop(t)、M_prop(t)、N_prop(t)均表示对应参数为时间t的函数；

定义UUV期望状态

x_d(t)＝[u_d(t)v_d(t)w_d(t)p_d(t)q_d(t)r_d(t)]^T

其中，所有带角标d的参数均表示对应参数的期望值；

则实际和期望之间误差表示为

$x_e\left(t\right)=x\left(t\right)-x_d\left(t\right)=\left[\begin{array}{c}u\left(t\right)-u_d\left(t\right)\\ v\left(t\right)-v_d\left(t\right)\\ w\left(t\right)-w_d\left(t\right)\\ p\left(t\right)-p_d\left(t\right)\\ q\left(t\right)-q_d\left(t\right)\\ r\left(t\right)-r_d\left(t\right)\end{array}\right],$

ψ(t)-ψ_d(t)＝△ψ，θ(t)-θ_d(t)＝△θ，

因此

建立误差动态方程

${\stackrel{\·}{x}}_e\left(t\right)=A\left(t\right)x_e\left(t\right)+B_1\left(t\right)\mathrm{\ω}\left(t\right)+B_2\left(t\right)\mathrm{\τ}\left(t\right)$

y_e(t)＝C(t)x_e(t)+D(t)τ(t)(1)

式中，A(t)、B₁(t)、B₂(t)、C(t)、D(t)均为系数矩阵；

方程(1)的系统矩阵为：

$\left(\begin{array}{ccc}A\left(t\right)& B_1\left(t\right)& B_2\left(t\right)\end{array}\right)=\left(\begin{array}{ccc}\frac{\∂f}{\∂x}& \frac{\∂f}{\∂\mathrm{\ω}}& \frac{\∂f}{\∂\mathrm{\τ}}\end{array}\right)|\left(x_d\right(t),0,\mathrm{\τ}(t\left)\right)$

$\left(\begin{array}{cc}C\left(t\right)& D\left(t\right)\end{array}\right)=\left(\begin{array}{cc}\frac{\∂h}{\∂x}& \frac{\∂h}{\∂\mathrm{\τ}}\end{array}\right)|\left(x_d\right(t),\mathrm{\τ}(t\left)\right)$

在局部时刻[t_k,t_k+1)的预测误差动态方程为：

${\stackrel{\·}{\stackrel{\‾}{x}}}_e\left(t\right)=A_k{\stackrel{\‾}{x}}_e\left(t\right)+B_{1k}\mathrm{\ω}\left(t\right)+B_{2k}\mathrm{\τ}\left(t\right)$

${\stackrel{\‾}{y}}_e\left(t\right)=C_k{\stackrel{\‾}{x}}_e\left(t\right)+D_k\mathrm{\τ}\left(t\right)---\left(2\right)$

式中，所有带角标k参数的均表示局部时刻的对应参数，表示x_e(t)、y_e(t)局部时刻的预测值；

方程(2)的系统矩阵

$\left(\begin{array}{ccc}{A}_k& B_{1k}& B_{2k}\\ C_k& D_k& 0\end{array}\right)=\left(\begin{array}{ccc}A\left(t_k\right)& B_1\left(t_k\right)& B_2\left(t_k\right)\\ C\left(t_k\right)& D\left(t_k\right)& 0\end{array}\right)={\mathrm{\Ω}}_k$

由于预测模型采用了线性化误差系统模型近似描述非线性系统的局部特征，因此在每个采样时刻需要更新系统矩阵Ω_k，获得新的预测模型完成预测过程；

步骤4、针对滚动优化问题在约束条件下进行滚动优化，求解优化问题，求可行解(γ_k,Q_k,Y_k)；

步骤5、针对优化问题有可行解(γ_k,Q_k,Y_k)，根据下式计算控制输入向量τ(t)，

$K_k=Y_k{Q}_k^{-1}$

$\mathrm{\τ}\left(t\right)=K_k{\stackrel{\‾}{x}}_e\left(t\right),\∀t\∈\[t_k,t_{k+1}),k\≥0$

将控制输入向量作用于方程(2)的系统，然后令k＝k+1，返回步骤2更新预测模型、重复滚动优化过程，直至实现UUV对线控位回收。

具体实施方式二：

本实施方式的步骤4的具体步骤如下：

滚动优化问题满足如下约束：

视线约束：A_LC_LX(l)≤d_L，

其中， $A_L=\left[\begin{array}{ccc}{a}_1& b_1& c_1\\ \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}.\\ .\\ .\end{array}\\ a_{N_L}& b_{N_L}& c_{N_L}\end{array}\right],d_L=\left[\begin{array}{c}{d}_1\\ .\\ .\\ .\\ d_{NL}\end{array}\right],C_L=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right];$

避碰约束： $\left\{\begin{array}{c}{A}_o{C}_{o}X\left(l\right)\≤d_0+{\mathrm{Hs}}_o\\ \left|\right|s_o|{|}_1\≤N_o-1\end{array}\right.,$

其中 $A_o=\left[\begin{array}{ccc}{a}_{o1}& b_{o1}& c_{o1}\\ \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}.\\ .\\ .\end{array}\\ a_{{\mathrm{oN}}_o}& b_{{\mathrm{oN}}_o}& c_{{\mathrm{oN}}_o}\end{array}\right],d_o=\left[\begin{array}{c}{d}_{o1}\\ .\\ .\\ .\\ d_{{\mathrm{oN}}_o}\end{array}\right],C_o=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right],s_o=\left[\begin{array}{c}{s}_{o1}\\ .\\ .\\ .\\ s_{{\mathrm{oN}}_o}\end{array}\right],$ N_o为半空间个数；

干扰约束：Hω≤I,ω∈R³；

其中，H∈R^n×3为常值矩阵，I∈Rⁿ为常值列向量，n为线性约束个数；

不等式约束： $\left(\begin{array}{ccc}{A}_k{Q}_k+Q_k{A}_k^T+B_{2k}{Y}_k+{Y_k}^T{B}_{2k}^T& B_{1k}& {(C_k{Q}_k+D_k{Y}_k)}^T\\ B_{1k}^T& -{{\mathrm{\&gamma;}}_k}^{2}I& 0\\ C_k{Q}_k+D_k{Y}_k& 0& -I\end{array}\right)<0,$

$\left(\begin{array}{cc}{p}_0-p_{k-1}+x_e{\left(t_k\right)}^T{P}_{k-1}{x}_e\left(t_k\right)& x_e{\left(t_k\right)}^T\\ x_e\left(t_k\right)& Q_k\end{array}\right)\&GreaterEqual;0,$

$\left(\begin{array}{cc}\frac{{\mathrm{\&tau;}}_{j,\max }^2}{r_c}& e_j^T{Y}_k\\ Y_k^T{e}_j& Q_k\end{array}\right)\&GreaterEqual;0,j=1,2,...,m_2,$

$\left(\begin{array}{cc}a& x_e{\left(t_k\right)}^T\\ x_e\left(t_k\right)& Q_k\end{array}\right)\&GreaterEqual;0,$

a≤a_c(1+σ)；

式中，e_j表示空间的一个基函数，m₂为空间维数，p₀＝x_e(t₀)^TP₀x_e(t₀)表示t₀时刻的误差状态向量x_e(t₀)与椭圆心P₀的距离，p_k-1＝x_e(t_k-1)^TP_k-1x_e(t_k-1)表示t_k-1时刻的误差状态向量x_e(t_k-1)与椭圆心P_k-1的距离，x_e(t_k)表示t_k时刻UUV的误差状态向量，τ_j,max表示在t_k时刻的j维的干扰力和力矩的最大值，γ_k为拉格朗日算子，表示性能指标，Q_k为对称矩阵，K_k为预测的状态反馈增益，Y_k＝K_kQ_k，q₁、q₂是优化加权系数，a是要优化的椭圆域的半径，a_c是椭圆域半径的给定值，σ为控制参数，初始值取0；

控制输入满足|τ_j(t)|≤τ_j,max＝τ_max，j＝1,2,...,m₂表示空间维数；

τ_max＝[X_prop,maxY_prop,maxZ_prop,maxK_prop,maxM_prop,maxN_prop,max]^T

其中，带有角标max的参数均表示对应参数的最大值；

针对以上约束条件对优化问题求可行解(γ_k,Q_k,Y_k)。

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

具体实施方式三：

本实施方式步骤4所述的视线约束A_LC_LX(l)≤d_L是通过以下步骤得到的：

视线约束属于凸约束，采用多面体的形式进行表示，设其半空间表示为：a_jξ+b_jη+c_jζ-d_j≤0，j＝1,2,…,N_L，N_L个半空间相交形成多面体，因此视线约束可以采用如下形式表示

$\left\{\begin{array}{c}{a}_1\mathrm{\&xi;}\left(l\right)+b_1\mathrm{\&eta;}\left(l\right)+c_1\mathrm{\&zeta;}\left(l\right)\&le;d_1\\ a_2\mathrm{\&xi;}\left(l\right)+b_2\mathrm{\&eta;}\left(l\right)+c_2\mathrm{\&zeta;}\left(l\right)\&le;d_2\\ \begin{array}{c}.\\ .\\ .\end{array}\\ a_{N_L}\mathrm{\&xi;}\left(l\right)+b_{N_L}\mathrm{\&eta;}\left(l\right)+c_{N_L}\mathrm{\&zeta;}\left(l\right)\&le;d_{N_L}\end{array}\right.;l=1,...,N_F$

其中N_F为多面体面数，将上式转换为矩阵型式如下：

A_LC_LX(l)≤d_L，

其中， $A_L=\left[\begin{array}{ccc}{a}_1& b_1& c_1\\ \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}.\\ .\\ .\end{array}\\ a_{N_L}& b_{N_L}& c_{N_L}\end{array}\right],d_L=\left[\begin{array}{c}{d}_1\\ .\\ .\\ .\\ d_{NL}\end{array}\right],C_L=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right],$ X(l)表示转换为矩阵型式后的待求解矩阵。

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

具体实施方式四：

本实施方式步骤4所述的避碰约束 $\left\{\begin{array}{c}{A}_o{C}_{o}X\left(l\right)\&le;d_0+{\mathrm{Hs}}_o\\ \left|\right|s_o|{|}_1\&le;N_o-1\end{array}\right.$ 是通过以下步骤得到的：

避碰约束属于非凸约束，根据UUV和母船的外形结构及对接装置，采用多个凸多面体的方法进行近似数学表示，具体表示如下：

$\left\{\begin{array}{c}{a}_{o1}\mathrm{\&xi;}\left(l\right)+b_{o1}\mathrm{\&eta;}\left(l\right)+c_{o1}\mathrm{\&zeta;}\left(l\right)\&le;d_{o1}\\ or\\ a_{o2}\mathrm{\&xi;}\left(l\right)+b_{o2}\mathrm{\&eta;}\left(l\right)+c_{o2}\mathrm{\&zeta;}\left(l\right)\&le;d_{o2}\\ or\\ \begin{array}{c}.\\ .\\ .\end{array}\\ or\\ a_{{\mathrm{oN}}_o}\mathrm{\&xi;}\left(l\right)+b_{{\mathrm{oN}}_o}\mathrm{\&eta;}\left(l\right)+c_{{\mathrm{oN}}_o}\mathrm{\&zeta;}\left(l\right)\&le;d_{{\mathrm{oN}}_o}\end{array}\right.;l=1,...,N_F$

将“or”约束通过采用大数定理和二进制变量进行处理将其转化为“and”约束，引入大数H和二进制变量s_oi(i＝1,…,N_o)得到

$\left\{\begin{array}{c}{a}_{o1}\mathrm{\&xi;}\left(l\right)+b_{o1}\mathrm{\&eta;}\left(l\right)+c_{o1}\mathrm{\&zeta;}\left(l\right)\&le;d_{o1}+{\mathrm{Hs}}_{o1}\\ and\\ a_{o2}\mathrm{\&xi;}\left(l\right)+b_{o2}\mathrm{\&eta;}\left(l\right)+c_{o2}\mathrm{\&zeta;}\left(l\right)\&le;d_{o2}+{\mathrm{Hs}}_{o2}\\ and\\ \begin{array}{c}.\\ .\\ .\end{array}\\ and\\ a_{{\mathrm{oN}}_o}\mathrm{\&xi;}\left(l\right)+b_{{\mathrm{oN}}_o}\mathrm{\&eta;}\left(l\right)+c_{{\mathrm{oN}}_o}\mathrm{\&zeta;}\left(l\right)\&le;d_{{\mathrm{oN}}_o}+{\mathrm{Hs}}_{{\mathrm{oN}}_o}\\ and\\ \underset{i=1}{\overset{N_o}{\&Sigma;}}{s}_{oi}\&le;N_o-1\end{array}\right.;l=1,...,N_F$

转换为矩阵形式如下

$\left\{\begin{array}{c}{A}_o{C}_{o}X\left(l\right)\&le;d_0+{\mathrm{Hs}}_o\\ \left|\right|s_o|{|}_1\&le;N_o-1\end{array}\right.$

其中 $A_o=\left[\begin{array}{ccc}{a}_{o1}& b_{o1}& c_{o1}\\ \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}.\\ .\\ .\end{array}\\ a_{{\mathrm{oN}}_o}& b_{{\mathrm{oN}}_o}& c_{{\mathrm{oN}}_o}\end{array}\right],d_o=\left[\begin{array}{c}{d}_{o1}\\ .\\ .\\ .\\ d_{{\mathrm{oN}}_o}\end{array}\right],C_o=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right],s_o=\left[\begin{array}{c}{s}_{o1}\\ .\\ .\\ .\\ s_{{\mathrm{oN}}_o}\end{array}\right],$ N_o为半空间个数。

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

具体实施方式五：

本实施方式步骤4所述对优化问题求可行解(γ_k,Q_k,Y_k)的过程中，若优化问题无可行解(γ_k,Q_k,Y_k)，则增大σ，再进行滚动优化(即返回步骤4再进行滚动优化)，直到其有解为止。

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

具体实施方式六：

本实施方式步骤1所述的通过对线控位方法得到UUV与母船的位置偏差值△x,△y,△z、航向偏差值△ψ和纵倾角偏差值△θ的具体步骤如下：

在基于母船的坐标系下，如图1所示，母船回收对接平台上的艏艉导引灯的位置坐标分别表示为(X_B,Y_B,Z_B)、(X_S,Y_S,Z_S)，则UUV腹部艏艉摄像头设备的位置坐标分别表示为(X′_B,Y′_B,Z′_B)、(X′_S,Y′_S,Z′_S)；

UUV腹部艏艉摄像头与母船回收平台艏艉导引灯的位置之差，根据其各位置坐标偏差值分别表示为(ex_B,ey_B,ez_B)、(ex_S,ey_S,ez_S)；

具体表达式为：ex_B＝X_B-X′_B，ey_B＝Y_B-Y′_B，ez_B＝Z_B-Z′_B，ex_S＝X_S-X′_S，ey_S＝Y_S-Y′_S，ez_S＝Z_S-Z′_S；

该误差值还不能直接应用于控制器，需要对其进行相应的变换处理， $\mathrm{\&Delta;}y=\frac{{\mathrm{ey}}_B+{\mathrm{ey}}_S}{2},\mathrm{\&Delta;}z=\frac{{\mathrm{ez}}_B+{\mathrm{ez}}_S}{2};$

(ey_B-ey_S)与航向偏差存在近似的比例关系：△ψ＝σ₁(ey_B-ey_S)；

式中，△ψ表示UUV与母船的航向偏差值，σ₁是其比例系数；

根据上述方法，同样可将UUV与母船的纵倾角偏差表示为：△θ＝σ₂(ez_B-ez_S)

式中，△θ表示UUV与母船的纵倾角偏差值，σ₂是比例系数；

并将△x,△y,△z，△ψ，△θ作为初始化值。

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

实施例

设置母船艏摇角ψ在正负3度之间正弦变化，变化周期为120秒，母船与UUV初始位置如图1所示，在该图上可以看出，UUV上的艏艉两部摄像机的安装位置，以及母船上的艏艉引导灯位置。

初始状态△x＝10m，△y＝-10m，△z＝10m，△ψ＝10度，△θ＝0度，母船航速为0.5m/s，UUV航速为0.5m/s，取可调参数a_c＝1,q₁＝2,q₂＝1；

然后按照图4所示的方案进行控制，首先系统初始化相关参数，给定r_c，初始化σ＝0，然后通过传感器测量获得当前UUV的系统状态及母船信息，图2中UUV通过摄像头捕捉导引灯位置而计算出两者的位置误差信息(x_e,y_e)＝(x-x_d,y-y_d)，然后计算预测模型系统矩阵Ω_k及之后通过鲁棒约束模型预测控制器对UUV姿态和位置进行调整，在图3中，给出了鲁棒约束模型预测控制的结构框图，鲁棒模型预测控制主要包括预测模型和滚动优化两部分，刷新预测模型，然后求解优化问题，若有可行解(γ_k,Q_k,Y_k)，根据下式计算控制量τ(t)和p_k；否则增大σ，直到其有解，然后根据下式计算控制量τ(t)和p_k；

$K_k=Y_k{Q}_k^{-1}$

$\mathrm{\&tau;}\left(t\right)=K_k{\stackrel{\&OverBar;}{x}}_e\left(t\right),\&ForAll;t\&Element;\&lsqb;t_k,t_{k+1}),k\&GreaterEqual;0$

将控制量作用于系统，然后k＝k+1，返回刷新预测模型和重复滚动优化过程，直至实现与母船的对线控位过程，从而实现精确地对线控位，使得UUV坐落到母船回收平台上。

得到UUV与母船的位置误差曲线如图5(a)、图5(b)和图5(c)所示，从仿真结果可以看出，鲁棒约束模型预测控制在母船运动情况下能够有效地对UUV进行安全、稳定的回收。

Claims (6)

1.基于鲁棒约束模型预测控制的UUV对线控位回收方法，其特征在于包括以下步骤：

步骤1、通过对线控位方法得到UUV与母船的位置偏差值△x,△y,△z、航向偏差值△ψ和纵倾角偏差值△θ；并将△x,△y,△z，△ψ，△θ作为初始化值；

步骤2、获取当前时刻UUV状态：

通过UUV自身的传感器得到当前时刻状态：u、v、w分别为纵向、横向和垂向速度，p、q、r分别为横摇角、纵摇角、艏摇角速度，ξ、η、ζ分别为UUV相对于固定坐标系的位置，θ、ψ为横摇角、纵摇角、艏摇角(rad)；

步骤3、构建误差预测模型，并计算局部时刻预测误差动态方程的系统矩阵Ω_k和局部时刻状态预测误差

利用UUV六自由度动力学方程和运动学方程构造动态方程：

$\stackrel{\&CenterDot;}{x}\left(t\right)=f\left(x\right(t),\mathrm{\&tau;}(t),\mathrm{\&omega;}(t\left)\right)$

y(t)＝h(x(t),τ(t))

式中，x(t)是UUV状态向量，y(t)是输出向量，τ(t)是UUV控制输入向量，ω(t)是干扰向量；为x(t)的一阶导数；t表示时间；

x(t)＝[u(t)v(t)w(t)p(t)q(t)r(t)]^T

τ(t)＝[X_prop(t)Y_prop(t)Z_prop(t)K_prop(t)M_prop(t)N_prop(t)]^T

其中，X_prop,Y_prop,Z_prop为UUV受到的纵向、横向、垂向的作用力，K_prop,M_prop,N_prop为UUV受到的纵向、横向、垂向作用力的力矩；u(t)、v(t)、w(t)、p(t)、q(t)、r(t)、ξ(t)、η(t)、ζ(t)、θ(t)、ψ(t)、X_prop(t)、Y_prop(t)、Z_prop(t)、K_prop(t)、M_prop(t)、N_prop(t)均表示对应参数为时间t的函数；

定义UUV期望状态

x_d(t)＝[u_d(t)v_d(t)w_d(t)p_d(t)q_d(t)r_d(t)]^T

其中，所有带角标d的参数均表示对应参数的期望值；

则实际和期望之间误差表示为

$x_e\left(t\right)=x\left(t\right)-x_d\left(t\right)=\left[\begin{array}{c}u\left(t\right)-u_d\left(t\right)\\ v\left(t\right)-v_d\left(t\right)\\ w\left(t\right)-w_d\left(t\right)\\ p\left(t\right)-p_d\left(t\right)\\ q\left(t\right)-q_d\left(t\right)\\ r\left(t\right)-r_d\left(t\right)\end{array}\right],$

ψ(t)-ψ_d(t)＝△ψ，θ(t)-θ_d(t)＝△θ，

因此

建立误差动态方程

${\stackrel{\&CenterDot;}{x}}_e\left(t\right)=A\left(t\right)x_e\left(t\right)+B_1\left(t\right)\mathrm{\&omega;}\left(t\right)+B_2\left(t\right)\mathrm{\&tau;}\left(t\right)$

y_e(t)＝C(t)x_e(t)+D(t)τ(t)(1)

式中，A(t)、B₁(t)、B₂(t)、C(t)、D(t)均为系数矩阵；

方程(1)的系统矩阵为：

$\left(\begin{array}{ccc}A\left(t\right)& B_1\left(t\right)& B_2\left(t\right)\end{array}\right)=\left(\begin{array}{ccc}\frac{\&part;f}{\&part;x}& \frac{\&part;f}{\&part;\mathrm{\&omega;}}& \frac{\&part;f}{\&part;\mathrm{\&tau;}}\end{array}\right){|}_{\left(x_d\right(t),0,\mathrm{\&tau;}(t\left)\right)}$

$\left(\begin{array}{cc}C\left(t\right)& D\left(t\right)\end{array}\right)=\left(\begin{array}{cc}\frac{\&part;h}{\&part;x}& \frac{\&part;h}{\&part;\mathrm{\&tau;}}\end{array}\right){|}_{\left(x_d\right(t),\mathrm{\&tau;}(t\left)\right)}$

在局部时刻[t_k,t_k+1)的预测误差动态方程为：

${\stackrel{\&CenterDot;}{\stackrel{\&OverBar;}{x}}}_e\left(t\right)=A_k{\stackrel{\&OverBar;}{x}}_e\left(t\right)+B_{1k}\mathrm{\&omega;}\left(t\right)+B_{2k}\mathrm{\&tau;}\left(t\right)$

${\stackrel{\&OverBar;}{y}}_e\left(t\right)=C_k{\stackrel{\&OverBar;}{x}}_e\left(t\right)+D_k\mathrm{\&tau;}\left(t\right)---\left(2\right)$

式中，所有带角标k参数的均表示局部时刻的对应参数，表示x_e(t)、y_e(t)局部时刻的预测值；

方程(2)的系统矩阵

$\left(\begin{array}{ccc}{A}_k& B_{1k}& B_{2k}\\ C_k& D_k& 0\end{array}\right)=\left(\begin{array}{ccc}A\left(t_k\right)& B_1\left(t_k\right)& B_2\left(t_k\right)\\ C\left(t_k\right)& D\left(t_k\right)& 0\end{array}\right)={\mathrm{\&Omega;}}_k$

步骤4、针对滚动优化问题在约束条件下进行滚动优化，求解优化问题，求可行解(γ_k,Q_k,Y_k)；

步骤5、针对优化问题有可行解(γ_k,Q_k,Y_k)，根据下式计算控制输入向量τ(t)，

$K_k=Y_k{Q}_k^{-1}$

$\mathrm{\&tau;}\left(t\right)=K_k{\stackrel{\&OverBar;}{x}}_e\left(t\right),\&ForAll;t\&Element;\&lsqb;t_k,t_{k+1}),k\&GreaterEqual;0$

将控制输入向量作用于方程(2)的系统，然后令k＝k+1，返回步骤2更新预测模型、重复滚动优化过程，直至实现UUV对线控位回收。

2.根据权利要求1所述的基于鲁棒约束模型预测控制的UUV对线控位回收方法，其特征在于步骤4的具体步骤如下：

滚动优化问题满足如下约束：

视线约束：A_LC_LX(l)≤d_L，

其中， $A_L=\left[\begin{array}{ccc}{a}_1& b_1& c_1\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ a_{N_L}& b_{N_L}& c_{N_L}\end{array}\right],d_L=\left[\begin{array}{c}{d}_1\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ d_{NL}\end{array}\right],C_L=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right];$

避碰约束： $\left\{\begin{array}{c}{A}_o{C}_{o}X\left(l\right)\&le;d_0+H{s}_o\\ \left|\right|s_o|{|}_1\&le;N_o-1\end{array}\right.,$

其中 $A_o=\left[\begin{array}{ccc}{a}_{o1}& b_{o1}& c_{o1}\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ a_{{\mathrm{oN}}_o}& b_{{\mathrm{oN}}_o}& c_{{\mathrm{oN}}_o}\end{array}\right],d_o=\left[\begin{array}{c}{d}_{o1}\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ d_{{\mathrm{oN}}_o}\end{array}\right],C_o=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right];s_o=\left[\begin{array}{c}{s}_{o1}\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ s_{{\mathrm{oN}}_o}\end{array}\right],$ N_o为半空间个数；

干扰约束：Hω≤I,ω∈R³；

其中，H∈R^n×3为常值矩阵，I∈Rⁿ为常值列向量，n为线性约束个数；

不等式约束： $\left(\begin{array}{ccc}{A}_k{Q}_k+Q_k{A}_k^T+B_{2k}{Y}_k+Y_k^T{B}_{2k}^T& B_{1k}& {(C_k{Q}_k+D_k{Y}_k)}^T\\ B_{1k}^T& -{{\mathrm{\&gamma;}}_k}^{2}I& 0\\ C_k{Q}_k+D_k{Y}_k& 0& -I\end{array}\right)<0,$

$\left(\begin{array}{cc}{p}_0-p_{k-1}+x_e{\left(t_k\right)}^T{P}_{k-1}{x}_e\left(t_k\right)& x_e{\left(t_k\right)}^T\\ x_e\left(t_k\right)& Q_k\end{array}\right)\&GreaterEqual;0,$

$\left(\begin{array}{cc}\frac{{\mathrm{\&tau;}}_{j,max}^2}{r_c}& e_j^T{Y}_k\\ Y_k^T{e}_j& Q_k\end{array}\right)\&GreaterEqual;0,j=1,2,...,m_2,$

$\left(\begin{array}{cc}a& x_e{\left(t_k\right)}^T\\ x_e\left(t_k\right)& Q_k\end{array}\right)\&GreaterEqual;0,$

a≤a_c(1+σ)；

式中，e_j表示空间的一个基函数，m₂为空间维数，p₀＝x_e(t₀)^TP₀x_e(t₀)表示t₀时刻的误差状态向量x_e(t₀)与椭圆心P₀的距离，p_k-1＝x_e(t_k-1)^TP_k-1x_e(t_k-1)表示t_k-1时刻的误差状态向量x_e(t_k-1)与椭圆心P_k-1的距离，x_e(t_k)表示t_k时刻UUV的误差状态向量，τ_j,max表示在t_k时刻的j维的干扰力和力矩的最大值，γ_k为拉格朗日算子，表示性能指标，Q_k为对称矩阵，K_k为预测的状态反馈增益，Y_k＝K_kQ_k，q₁、q₂是优化加权系数，a是要优化的椭圆域的半径，a_c是椭圆域半径的给定值，σ为控制参数；

控制输入满足|τ_j(t)|≤τ_j,max＝τ_max，j＝1,2,...,m₂表示空间维数；

τ_max＝[X_prop,maxY_prop,maxZ_prop,maxK_prop,maxM_prop,maxN_prop,max]^T

其中，带有角标max的参数均表示对应参数的最大值；

针对以上约束条件对优化问题求可行解(γ_k,Q_k,Y_k)。

3.根据权利要求2所述的基于鲁棒约束模型预测控制的UUV对线控位回收方法，其特征在于步骤4所述的视线约束A_LC_LX(l)≤d_L是通过以下步骤得到的：

视线约束属于凸约束，采用多面体的形式进行表示，设其半空间表示为：a_jξ+b_jη+c_jζ-d_j≤0，j＝1,2,…,N_L，N_L个半空间相交形成多面体，因此视线约束可以采用如下形式表示

$\left\{\begin{array}{c}{a}_1\mathrm{\&xi;}\left(l\right)+b_1\mathrm{\&eta;}\left(l\right)+c_1\mathrm{\&zeta;}\left(l\right)\&le;d_1\\ a_2\mathrm{\&xi;}\left(l\right)+b_2\mathrm{\&eta;}\left(l\right)+c_2\mathrm{\&zeta;}\left(l\right)\&le;d_2\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ a_{N_L}\mathrm{\&xi;}\left(l\right)+b_{N_L}\mathrm{\&eta;}\left(l\right)+c_{N_L}\mathrm{\&zeta;}\left(l\right)\&le;d_{N_L}\end{array}\right.;l=1,...,N_F$

其中N_F为多面体面数，将上式转换为矩阵型式如下：

A_LC_LX(l)≤d_L，

其中， $A_L=\left[\begin{array}{ccc}{a}_1& b_1& c_1\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ a_{N_L}& b_{N_L}& c_{N_L}\end{array}\right],d_L=\left[\begin{array}{c}{d}_1\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ d_{NL}\end{array}\right],C_L=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right],$ X(l)表示转换为矩阵型式后的待求解矩阵。

4.根据权利要求2所述的基于鲁棒约束模型预测控制的UUV对线控位回收方法，其特征在于步骤4所述的避碰约束 $\left\{\begin{array}{c}{A}_o{C}_{o}X\left(l\right)\&le;d_0+H{s}_o\\ \left|\right|s_o|{|}_1\&le;N_o-1\end{array}\right.$ 是通过以下步骤得到的：

避碰约束属于非凸约束，根据UUV和母船的外形结构及对接装置，采用多个凸多面体的方法进行近似数学表示，具体表示如下：

$\left\{\begin{array}{c}{a}_{o1}\mathrm{\&xi;}\left(l\right)+b_{o1}\mathrm{\&eta;}\left(l\right)+c_{o1}\mathrm{\&zeta;}\left(l\right)\&le;d_{o1}\\ or\\ a_{o2}\mathrm{\&xi;}\left(l\right)+b_{o2}\mathrm{\&eta;}\left(l\right)+c_{o2}\mathrm{\&zeta;}\left(l\right)\&le;d_{o2}\\ or\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ or\\ a_{{\mathrm{oN}}_o}\mathrm{\&xi;}\left(l\right)+b_{{\mathrm{oN}}_o}\mathrm{\&eta;}\left(l\right)+c_{{\mathrm{oN}}_o}\mathrm{\&zeta;}\left(l\right)\&le;d_{{\mathrm{oN}}_o}\end{array}\right.;l=1,...,N_F$

将“or”约束通过采用大数定理和二进制变量进行处理将其转化为“and”约束，引入大数H和二进制变量s_oi(i＝1,…,N_o)得到

$\left\{\begin{array}{c}{a}_{o1}\mathrm{\&xi;}\left(l\right)+b_{o1}\mathrm{\&eta;}\left(l\right)+c_{o1}\mathrm{\&zeta;}\left(l\right)\&le;d_{o1}+{\mathrm{Hs}}_{o1}\\ and\\ a_{o2}\mathrm{\&xi;}\left(l\right)+b_{o2}\mathrm{\&eta;}\left(l\right)+c_{o2}\mathrm{\&zeta;}\left(l\right)\&le;d_{o2}+{\mathrm{Hs}}_{o2}\\ and\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ and\\ a_{{\mathrm{oN}}_o}\mathrm{\&xi;}\left(l\right)+b_{{\mathrm{oN}}_o}\mathrm{\&eta;}\left(l\right)+c_{{\mathrm{oN}}_o}\mathrm{\&zeta;}\left(l\right)\&le;d_{{\mathrm{oN}}_o}+{\mathrm{Hs}}_{{\mathrm{oN}}_o}\\ and\\ \underset{i=1}{\overset{N_o}{\&Sigma;}}{s}_{oi}\&le;N_o-1\end{array}\right.;l=1,...,N_F$

转换为矩阵形式如下

$\left\{\begin{array}{c}{A}_o{C}_{o}X\left(l\right)\&le;d_0+H{s}_o\\ \left|\right|s_o|{|}_1\&le;N_o-1\end{array}\right.$

其中 $A_o=\left[\begin{array}{ccc}{a}_{o1}& b_{o1}& c_{o1}\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ a_{{\mathrm{oN}}_o}& b_{{\mathrm{oN}}_o}& c_{{\mathrm{oN}}_o}\end{array}\right],d_o=\left[\begin{array}{c}{d}_{o1}\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ d_{{\mathrm{oN}}_o}\end{array}\right],C_o=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right];s_o=\left[\begin{array}{c}{s}_{o1}\\ \&CenterDot;\\ \&CenterDot;\\ \&CenterDot;\\ s_{{\mathrm{oN}}_o}\end{array}\right],$ N_o为半空间个数。

5.根据权利要求1、2、3或4所述的基于鲁棒约束模型预测控制的UUV对线控位回收方法，其特征在于步骤4所述对优化问题求可行解(γ_k,Q_k,Y_k)的过程中，若优化问题无可行解(γ_k,Q_k,Y_k)，则增大σ，再进行滚动优化，直到其有解为止。

6.根据权利要求5所述的基于鲁棒约束模型预测控制的UUV对线控位回收方法，其特征在于步骤1所述的通过对线控位方法得到UUV与母船的位置偏差值△x,△y,△z、航向偏差值△ψ和纵倾角偏差值△θ的具体步骤如下：

在基于母船的坐标系下，母船回收对接平台上的艏艉导引灯的位置坐标分别表示为(X_B,Y_B,Z_B)、(X_S,Y_S,Z_S)，则UUV腹部艏艉摄像头设备的位置坐标分别表示为(X′_B,Y′_B,Z′_B)、(X′_S,Y′_S,Z′_S)；

UUV腹部艏艉摄像头与母船回收平台艏艉导引灯的位置之差，根据其各位置坐标偏差值分别表示为(ex_B,ey_B,ez_B)、(ex_S,ey_S,ez_S)；

具体表达式为：ex_B＝X_B-X′_B，ey_B＝Y_B-Y′_B，ez_B＝Z_B-Z′_B，ex_S＝X_S-X′_S，ey_S＝Y_S-Y′_S，ez_S＝Z_S-Z′_S；

进行变换处理， $\mathrm{\&Delta;}x=\frac{{\mathrm{ex}}_B+{\mathrm{ex}}_S}{2},\mathrm{\&Delta;}y=\frac{{\mathrm{ey}}_B+{\mathrm{ey}}_S}{2},\mathrm{\&Delta;}z=\frac{{\mathrm{ez}}_B+{\mathrm{ez}}_S}{2};$

(ey_B-ey_S)与航向偏差存在近似的比例关系：△ψ＝σ₁(ey_B-ey_S)；

式中，△ψ表示UUV与母船的航向偏差值，σ₁是其比例系数；

根据上述方法，将UUV与母船的纵倾角偏差表示为：△θ＝σ₂(ez_B-ez_S)

式中，△θ表示UUV与母船的纵倾角偏差值，σ₂是比例系数；

并将△x,△y,△z，△ψ，△θ作为初始化值。