[发明专利]基于鲁棒约束模型预测控制的UUV对线控位回收方法

权利要求书

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

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

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

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

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

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

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的参数均表示对应参数的期望值；

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

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

因此

建立误差动态方程

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

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

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

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

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

方程(2)的系统矩阵

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

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

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