[发明专利]无人艇与水下无人潜器联合系统的协同交会对接控制方法

权利要求书

1.一种无人艇与水下无人潜器联合系统的协同交会对接控制方法，其特征在于，包括以下步骤：

1)建立外界低频扰动下的水下无人潜器动力学模型与无人艇动力学模型并进行离散化，水下无人潜器动力学模型的表达式为：

其中，ρ_A为水下无人潜器的位置状态向量，且ρ_A＝[x_A，y_A，z_A，φ_A，θ_A，ψ_A]^T，v_A为水下无人潜器的速度状态向量，且v_A＝[u_A，v_A，w_A，p_A，q_A，r_A]^T，R_A为坐标转换矩阵，R_T为坐标转换矩阵中关于线速度相关的转换矩阵，R_R为坐标转换矩阵中关于角速度相关的转换矩阵，x_A为水下无人潜器的位置在惯性坐标系下x轴方向上的投影，y_A为水下无人潜器的位置在惯性坐标系下y轴方向上的投影，z_A为水下无人潜器的位置在惯性坐标系下z轴方向上的投影，φ_A为水下无人潜器的滚转角度，θ_A为俯仰角度，ψ_A为航向角度，u_A为前向速度，v_A为横荡速度，w_A为垂荡速度，p_A为滚转角速度，q_A为俯仰角速度，r_A为航向转动角速度，M_A为质量矩阵，C_A为科里奥利矩阵，D_A为阻尼矩阵，g为恢复力向量，τ_A为水下无人潜器的推力输入，c(·)，s(·)，t(·)分别代表cos(·)，sin(·)，tan(·)三个三角函数；

无人艇动力学模型的表达式为：

其中，ρ_U为无人艇的位置状态向量，且ρ_U＝[x_U，y_U，ψ_U]^T，v_U为无人艇的速度状态向量，且v_U＝[u_U，v_U，r_U]^T，x_U为无人艇的位置在惯性坐标系下x轴方向上的投影，y_U为无人艇的位置在惯性坐标系下y轴方向上的投影，ψ_U为无人艇的航向角度，R_U为平面坐标系转换矩阵，u_U为无人艇前向速度，v_U为无人艇横荡速度，r_U为无人艇转向角速度，M_U为质量矩阵，C_U为科里奥利矩阵，D_U为阻尼矩阵，τ_U为无人艇的推力输入；

以离散化采样时间Δt＝0.1秒为采样步长分别对水下无人潜器动力学模型与无人艇动力学模型进行离散化得到的离散模型如下：

X_A(k+1)＝h_A，d(X_A(k)，τ_A(k))+β₁(k)

X_U(k+1)＝h_U，d(X_U(k)，τ_U(k))+β₂(k)

其中，h_A，d和h_U，d为离散化后的水下无人潜器与无人艇的离散动力学向量函数，β₁(k)和β₂(k)分别为水下无人潜器与无人艇在k时刻受到的外界扰动，X_A(k)为水下无人潜器在k时刻的状态向量，X_U(k)为无人艇在k时刻的状态向量，τ_A(k)为水下无人潜器在k时刻的推力输入，τ_U(k)为无人艇在k时刻的推力输入；

2)采集当前k时刻无人艇与水下无人潜器的状态信息；

3)根据采集到的k时刻的状态信息与k-1时刻的信息进行处理，对外界扰动进行估计得到估计值对外界扰动进行估计具体为：

通过后验得到的上一时刻的准确扰动信息作为当前时刻扰动的估计值，则有：

β_i(k-1)＝X_i(k)-h_i，d(X_i(k-1)，τ_i(k-1))

其中，为对k时刻扰动的估计值，βi(k-1)为k-1时刻的准确扰动值，下标i的取值为A或U，分别表示水下无人潜器和无人艇；

4)根据k时刻得到的无人艇与水下无人潜器的状态信息、与预设轨迹的偏差信息以及对当前时刻扰动的估计，通过基于模型预测控制思想的优化计算得到从当前时刻到k+N时刻的一系列标称推力和扰动补偿力，基于模型预测控制思想的优化具体包括以下步骤：

41)设计系统的优化目标函数，主要指标包括对接误差、水下无人潜器与任务轨迹的跟踪误差以及无人艇与水下无人潜器的输入控制量；

42)构建约束条件；

43)根据系统的优化目标函数和约束条件构建模型预测控制优化问题；

44)设计扰动补偿控制，并分别求解无人艇和水下无人潜器的模型预测控制优化问题，得到关于无人艇与水下无人潜器对应的最优抗扰动补偿干扰输入和作为此刻待执行的补偿项；

5)根据步骤4)中得到的推力序列，将得到的第一个推力与补偿力相加应用于系统作为当前时刻的控制输入从而进行水下无人潜器和无人艇的对接控制。