基于鲁棒约束模型预测控制的uuv对线控位回收方法
【技术领域】
[0001 ]本发明涉及一种水下母船背驮式搭载UUV的自主回收方法。
【背景技术】
[0002] UUV技术目前已经作为探索海洋空间资源的最重要手段之一，UUV虽然具有隐蔽 性、深潜性能、安全性等优点，但是由于UUV受到自身携带的能源限制，使其工作范围和时间 受到严格的限制，因此要通过UUV水下回收实现在水下完成能源补给和上传下载数据，这可 以大大提高UUV的工作范围和时间。
[0003] 由于鲁棒约束模型预测控制方法能够有效的处理较复杂的状态约束和控制约束 以及对参数不确定性系统有非常好的控制性能，因此本发明采用基于鲁棒约束模型预测控 制算法的精确对线控位方法对UUV进行水下回收。
[0004] 哈尔滨工程大学的张伟等人在《船舶工程》（2013年第05期)上发表了《基于对线控 位策略的UUV回收运动控制研究》，该文章对于水下坞舱回收UUV的过程提出了一种基于对 线控位策略的回收方法，并设计UUV回收的位置和姿态的灰色预测PID控制器，减小了 UUV回 收控制中的超调量和调整时间，但是该方法针对的是水下静止平台，而非移动平台，存在一 定的局限性。
【发明内容】
[0005] 本发明是为了解决现有水下回收UUV的方法主要是针对静止的回收平台对UUV进 行回收的问题以及回收过程中由于超调过大、调节时间过长等不稳定因素会使得UUV与回 收平台发生碰撞的问题。
[0006] 基于鲁棒约束模型预测控制的UUV对线控位回收方法，包括以下步骤：
[0007] 步骤1、通过对线控位方法得到UUV与母船的位置偏差值Λχ，Ay，Λζ、航向偏差值 ΛΦ和纵倾角偏差值Λθ;并将Λχ，Ay，Λζ，ΛΦ，Λθ作为初始化值；
[0008] 步骤2、获取当前时刻UUV状态：
[0009] 通过UUV自身的一系列传感器等得到当前时刻状态:u、ν、w分别为纵向、横向和垂 向速度(m/s)，p、q、r分别为横摇角、纵摇角、艏摇角速度(rad/s)，ξ、η、ζ分别为UUV相对于固 定坐标系的位置(πι)，％θ、φ为横摇角、纵摇角、艏摇角( rad);
[0010] 步骤3、构建误差预测模型，并计算局部时刻预测误差动态方程的系统矩阵 局部时刻状态预测误差元
[0011] 利用UUV六自由度动力学方程和运动学方程构造动态方程：
[0012] i(，).二 /(λ.(，), r(,，)，w⑴）
[0013] y(t)=h(x(t) ,τ(?))
[0014] 式中，x(t)是UUV状态向量，y(t)是输出向量，τ(〇是UUV控制输入向量，co(t)是干 扰向量;为x(t)的一阶导数;t表示时间；
[0015] x(t) = [u(t) v(t) w(t) p(t) q(t) r(t)]T
[0016] n{t) C(〇 φ{() θ{?
[0017] T ( t) = [Xprop ( t) Yprop(t) Zprop(t) Kprop(t) Mprop(t) Nprop(t)]
[0018] 其中，Xprcip，Yprcip，Zprot^UUV受到的纵向、横向、垂向的作用力，K prcip，Mprcip，Npr。!^ UUV受到的纵向、横向、垂向作用力的力矩;u(t)、v(t)、w(t)、p(t)、q(t)、r(t)、C(t)、n(t)、G (t)、0(?)、.9(t)、iKt)、X prQp(t)、YprQp(t)、ZprQp(t)、KprQp(t)、M prQp(t)、NprQp(t)均表示对应参 数为时间t的函数；
[0019] 定义UUV期望状态
[0020] xd(t) = [ud(t) vd(t) wd(t) pa(t) qa(t) ra(t)]T
[0021] ν,(〇=[^(0 %(0 (0 φ,,{? ^ (0 ψλ (0]γ
[0022] 其中，所有带角标d的参数均表示对应参数的期望值；
[0023] 则实际和期望之间误差表示为
[0029] 建立误差动态方程
[0030] ,ν, (/) = A (/).v. (/) + /i, (/)〇.){ l j -l· B, (/) r (/)
[0031] ye(t) = C(t)xe(t)+D(t)x(t) (1)
[0032] 式中，△(〇、81(〇、82(〇、以〇、0(〇均为系数矩阵；
[0033]方程(1)的系统矩阵为：
[0036] 在局部时刻[tk，tk+1)的预测误差动态方程为：
[0037] t (〇 = A-K (〇 + Bu,^(0 + Byj(〇
[0038] 兑（?）= (? (/) + D, r(/) (2)
[0039] 式中，所有带角标k参数的均表示局部时刻的对应参数，:^.(/)、；Γ:, (/)表示xjthye (t)局部时刻的预测值；
[0040] 方程(2)的系统矩阵
[0042] 由于预测模型采用了线性化误差系统模型近似描述非线性系统的局部特征，因此 在每个采样时刻需要更新系统矩阵Ω k，获得新的预测模型完成预测过程；
[0043]
，在约束条件下进行滚动优化，求解 优化问题，求可行解（Tk，Qk，Yk);
[0044] 步骤5、针对优化问题有可行解（丫15，(^，￥1〇，根据下式计算控制输入向量1(〇，
[0045] Kk :YkQ-kl
[0046] r(/) = X, (/), V/ e [/,,,,:), A > 0
[0047] 将控制输入向量作用于方程(2)的系统，然后令k = k+l，返回步骤2更新预测模型、 重复滚动优化过程，直至实现UUV对线控位回收。
[0048]本发明具有以下效果：
[0049] 本发明采用的鲁棒约束模型预测控制方法不仅能够针对静止回收平台对UUV进行 回收，还能针对低速运动的回收平台对UUV进行回收，通过鲁棒约束模型预测控制，使得回 收过程中超调量大大减小，调节时间加快，使得UUV能够更加稳定、安全的进行回收。
【附图说明】
[0050] 图1为母船运动状态下与UUV的相对初始位置示意图；
[0051 ] 图2为UUV对接示意图；
[0052]图3为鲁棒约束模型预测控制结构框图；
[0053]图4为鲁棒模型预测控制实现流程；
[0054]图5为仿真实验中UUV与母船的位置误差曲线图。
【具体实施方式】
[0055]【具体实施方式】一：
[0056]基于鲁棒约束模型预测控制的UUV对线控位回收方法，包括以下步骤：
[0057]步骤1、通过对线控位方法得到UUV与母船的位置偏差值Λχ，Ay，Λζ、航向偏差值 ΛΦ和纵倾角偏差值Λθ;并将Λχ，Ay，Λζ，ΛΦ，Λθ作为初始化值；
[0058] 步骤2、获取当前时刻UUV状态：
[0059]通过UUV自身的一系列传感器等得到当前时刻状态:u、ν、w分别为纵向、横向和垂 向速度(m/s)，p、q、r分别为横摇角、纵摇角、艏摇角速度(rad/s)，ξ、η、ζ分别为UUV相对于固 定坐标系的位置(m)，Ρ、θ、φ为横摇角、纵摇角、艏摇角(rad);
[0060]步骤3、构建误差预测模型，并计算局部时刻预测误差动态方程的系统矩阵 局部时刻状态预测误差t
[0061 ] 利用UUV六自由度动力学方程和运动学方程构造动态方程：
[0062] f(/) = /(.'-(')，r(r)，iy(/))
[0063] y(t)=h(x(t) ,τ(?))
[0064] 式中，x(t)是UUV状态向量，y(t)是输出向量，τ(〇是UUV控制输入向量，co(t)是干 扰向量Μ (/)为X (t)的一阶导数;t表示时间；
[0065] x(t) = [u(t) v(t) w(t) p(t) q(t) r(t)]T
[0066] v(r) = [^(r) η{? C[t) φ{?) θ{?) ^(r)]r
[0067] T ( t) = [Xprop ( t) Yprop(t) Zprop(t) Kprop(t) Mprop(t) Nprop(t)]
[0068] 其中，Xpr〇P，Ypr〇P，ZprciI^UUV受到的纵向、横向、垂向的作用力，K pr〇P，M