预测模型，然后求解优化问题，若有可行解（丫1<，(^，￥1〇，根据下式计算控制量1 (t)和pk;否则增大 〇，直到其有解，然后根据下式计算控制量T(t)和pk;
[0155] Kk =1,0,1
[0156] r(/) = K,；v, (/),V/e. [/,,,,),k > 0
[0157] 将控制量作用于系统，然后k = k+l，返回刷新预测模型和重复滚动优化过程，直至 实现与母船的对线控位过程，从而实现精确地对线控位，使得UUV坐落到母船回收平台上。
[0158] 得到UUV与母船的位置误差曲线如图5(a)、图5(b)和图5(c)所示，从仿真结果可以 看出，鲁棒约束模型预测控制在母船运动情况下能够有效地对UUV进行安全、稳定的回收。
【主权项】
1.基于鲁棒约束模型预测控制的uuv对线控位回收方法，其特征在于包括w下步骤： 步骤1、通过对线控位方法得到UUV与母船的位置偏差值Δχ，Ay，Δζ、航向偏差值ΔΦ 和纵倾角偏差值ΔΘ;并将Δχ，Ay，Δζ，ΔΦ，ΔΘ作为初始化值； 步骤2、获取当前时刻UUV状态： 通过UUV自身的传感器得到当前时刻状态:U、V、W分别为纵向、横向和垂向速度，Ρ、q、r 分别为横摇角、纵摇角、腊摇角速度，ξ、η、ζ分别为UUV相对于固定坐标系的位置，妒θ、φ为横 摇角、纵摇角、腊摇角(rad); 步骤3、构建误差预测模型，并计算局部时刻预测误差动态方程的系统矩阵Qk和局部时 刻状态预测误差兩(0; 利用UUV六自由度动力学方程和运动学方程构造动态方程：y(t)=h(x(t) ,τ(?)) 式中，x(t)是UUV状态向量，y(t)是输出向量，T(t)是UUV控制输入向量，ω (t)是干扰向 量；;-(/)为x(t)的一阶导数;t表示时间；其中，XprDp，YprDp，ZprDp为UUV受到的纵向、横向、垂向的作用力，Vdp，Mprcp，Nprcp为UUV受 到的纵向、横向、垂向作用力的力矩;u(t)、v(t)、w(t)、p(t)、q(t)、;r(t)、C(t)、ri(t)、C(t)、 口(/)、目（t)、lKt)、Xprop(t)、Yprop(t)、Zpr叩（t)、Kprop(t)、Mpr叩（t)、Nprop(t)均表示对应参数为 时间t的函数； 定义UUV期望状态其中，所有带角标d的参数均表示对应参数的期望值； 则实际和期望之间误差表示为Φ(?)-Φ??(?) = ΔΦ,θ(?)-θ(?(?) = ΔΘ, 因此建立误差动态方程ye(t) =C(t)xe(t)+D(t)x(t) (1) 式中，4(〇、81(*)、82(*)、(：4)、0(*)均为系数矩阵； 方程(1)的系统矩阵为：在局部时刻[tk，tk+l)的预测误差动态方程为：式中，所有带角标k参数的均表示局部时刻的对应参数，tiO、巧和)表示Xe(t)、ye(t) 局部时刻的预测值； 方程(2)的系统矩阵步骤4、针对滚动优化问题，在约束条件下进行滚动优化，求解优化 问题，求可行解（丫 k，Qk，Yk); 步骤5、针对优化问题有可行解（丫 k，Qk，&)，根据下式计算控制输入向量T(t)，Φ)=於在(0·々'/E II)，足 >() 将控制输入向量作用于方程(2)的系统，然后令k = k+l，返回步骤2更新预测模型、重复 滚动优化过程，直至实现UUV对线控位回收。2.根据权利要求1所述的基于鲁棒约束模型预测控制的UUV对线控位回收方法，其特征 在于步骤4的具体步骤如下： 滚动优化问题蘭足如下约束： 视线约束:Α^χα)^^，间个数； 干扰约束:Ηω <1，COER3; 其中，为常值矩阵，ler为常值列向量，η为线性约束个数；a < acd+σ)； 式中，e康示iT2空间的一个基函数，m2为空间维数，P0 = Xe ( to ) TPoXe ( to )表示to时刻的误 差状态向量Xe(to)与楠圆屯、Po的距离，Pk-l = Xe(tk-〇Tpk-lXe(tk-l)表示tk-l时刻的误差状态向 量Xe(tk-l)与楠圆屯、Pk-1的距离，Xe(tk)表示tk时刻UUV的误差状态向量，Tj,max表示在tk时刻 的j维的干扰力和力矩的最大值，丫功拉格朗日算子，表示性能指标，Qk为对称矩阵，Kk为预 ii的状态反馈增益，化= KkQk，qi、q2是优化加权系数，a是要优化的楠圆域的半径，ac是楠圆 域半径的给定值，σ为控制参数； 枉制输入細足I Tj (t) I < Tj,max二Tmax, j二1,2 , . . . ,Π12表不全间维数； Tmax- [Xprop,max Yprop,max Zprop,max Kprop,max Mprop,max Nprop,max] 其中，带有角标max的参数均表示对应参数的最大值； 针对W上约束条件对优化问题求可行解（丫 k，Qk，&)。3. 根据权利要求2所述的基于鲁棒约束模型预测控制的UUV对线控位回收方法，其特征 在于步骤4所述的视线约束ALCLX(l)<dL是通过W下步骤得到的： 视线约束属于凸约束，采用多面体的形式进行表示，设其半空间表示为:曰护M+C店-dj <0，j = l，2,…，化，化个半空间相交形成多面体，因此视线约束可W采用如下形式表示?. Np. 其中Nf为多面体面数，将上式转换为矩阵型式如下： AlClXQ) <山， 其中，X(l)表示转换为矩阵 型式后的待求解矩阵。4. 根据权利要求2所述的基于鲁棒约束模型预测控制的UUV对线控位回收方法，其特征 在于步骤4所述的避碰约束曼通过W下步骤得到的： 避碰约束属于非凸约束，根据UUV和母船的外形结构及对接装置，采用多个凸多面体的 方法进行近似数学表示，具体表示如下：将"or"约束通过采用大数定理和二进制变量进行处理将其转化为"and"约束，引入大 数Η和二进制变量SQi(i = l，···，N。)得到转换为矩阵形式如下其中'N。为半空 间个数。5. 根据权利要求1、2、3或4所述的基于鲁棒约束模型预测控制的UUV对线控位回收方 法，其特征在于步骤4所述对优化问题求可行解（丫 k，Qk，化)的过程中，若优化问题无可行解 (丫 k，Qk，化），则增大0,再进行滚动优化，直到其有解为止。6. 根据权利要求5所述的基于鲁棒约束模型预测控制的UUV对线控位回收方法，其特征 在于步骤1所述的通过对线控位方法得到UUV与母船的位置偏差值Δχ，Ay，Δζ、航向偏差 值ΔΦ和纵倾角偏差值Δ Θ的具体步骤如下： 在基于母船的坐标系下，母船回收对接平台上的腊艇导引灯的位置坐标分别表示为 (Χβ，Υβ，Zb )、（Xs，Ys，Zs)，则UUV腹部腊艇摄像头设备的位置坐标分别表示为(X' Β，Υ' Β，Ζ' Β)、 (X's，Y's，Z's); UUV腹部腊艇摄像头与母船回收平台腊艇导引灯的位置之差，根据其各位置坐标偏差 值分别表示为（exB, eyB, ezB)、（exs, eys, ezs); 具体表达式为：exB = XB-X' B，eyB = YB_Y' B，ezB = ZB_Z' B，exs = Xs-X's，eys = Ys_Y' s，ezs = Zs-Z's; 进行变换处理：(巧Β-巧s)与航向偏差存在近似的比例关系：ΔΦ = σι (eyB-eys); 式中，ΔΦ表示UUV与母船的航向偏差值，〇1是其比例系数； 根据上述方法，将UUV与母船的纵倾角偏差表示为:Δ Θ = 02 (ezB-ezs) 式中，ΛΘ表示UUV与母船的纵倾角偏差值，02是比例系数； 并将Δχ，Ay，Δζ，ΔΦ，Δθ作为初始化值。
【专利摘要】基于鲁棒约束模型预测控制的UUV对线控位回收方法，涉及一种水下母船背驮式搭载UUV的自主回收方法。为了解决现有水下回收UUV的方法主要是针对静止的回收平台对UUV进行回收的问题以及回收过程中由于超调过大、调节时间过长等不稳定因素会使得UUV与回收平台发生碰撞的问题；本发明首先获得UUV与母船状态信息，构建误差预测模型，并计算局部时刻预测误差动态方程的系统矩阵和局部时刻状态预测误差；在约束条件下进行滚动优化，求解优化问题；然后计算控制量并新预测模型、重复滚动优化过程，直至实现与母船的对线控位过程。本发明适用于水下母船背驮式搭载UUV的自主回收。
【IPC分类】G05B13/04
【公开号】CN105700356
【申请号】CN201610104470
【发明人】张伟, 滕彦斌, 陈海田, 陈涛, 李娟 , 严浙平
【申请人】哈尔滨工程大学
【公开日】2016年6月22日
【申请日】2016年2月25日