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

摘要

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

主权项

基于鲁棒约束模型预测控制的UUV对线控位回收方法，其特征在于包括以下步骤：步骤1、通过对线控位方法得到UUV与母船的位置偏差值△x,△y,△z、航向偏差值△ψ和纵倾角偏差值△θ；并将△x,△y,△z，△ψ，△θ作为初始化值；步骤2、获取当前时刻UUV状态：通过UUV自身的传感器得到当前时刻状态：u、v、w分别为纵向、横向和垂向速度，p、q、r分别为横摇角、纵摇角、艏摇角速度，ξ、η、ζ分别为UUV相对于固定坐标系的位置，θ、ψ为横摇角、纵摇角、艏摇角(rad)；步骤3、构建误差预测模型，并计算局部时刻预测误差动态方程的系统矩阵Ω_k和局部时刻状态预测误差利用UUV六自由度动力学方程和运动学方程构造动态方程：$\stackrel{·}{x}\left(t\right)=f\left(x\right(t),\tau (t),\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{·}{x}}_e\left(t\right)=A\left(t\right)x_e\left(t\right)+B_1\left(t\right)\omega \left(t\right)+B_2\left(t\right)\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{\partial f}{\partial x}& \frac{\partial f}{\partial \omega }& \frac{\partial f}{\partial \tau }\end{array}\right){|}_{\left(x_d\right(t),0,\tau (t\left)\right)}$$\left(\begin{array}{cc}C\left(t\right)& D\left(t\right)\end{array}\right)=\left(\begin{array}{cc}\frac{\partial h}{\partial x}& \frac{\partial h}{\partial \tau }\end{array}\right){|}_{\left(x_d\right(t),\tau (t\left)\right)}$在局部时刻[t_k,t_k+1)的预测误差动态方程为：${\stackrel{·}{\stackrel{‾}{x}}}_e\left(t\right)=A_k{\bar{x}}_e\left(t\right)+B_{1k}\omega \left(t\right)+B_{2k}\tau \left(t\right)$${\bar{y}}_e\left(t\right)=C_k{\bar{x}}_e\left(t\right)+D_k\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)={\Omega }_k$步骤4、针对滚动优化问题在约束条件下进行滚动优化，求解优化问题，求可行解(γ_k,Q_k,Y_k)；步骤5、针对优化问题有可行解(γ_k,Q_k,Y_k)，根据下式计算控制输入向量τ(t)，$K_k=Y_k{Q}_k^{-1}$$\tau \left(t\right)=K_k{\bar{x}}_e\left(t\right),\forall t\in [t_k,t_{k+1}),k\ge 0$将控制输入向量作用于方程(2)的系统，然后令k＝k+1，返回步骤2更新预测模型、重复滚动优化过程，直至实现UUV对线控位回收。