CN112925332B - 无人艇与水下无人潜器联合系统的协同交会对接控制方法 - Google Patents

Abstract

本发明涉及一种无人艇与水下无人潜器联合系统的协同交会对接控制方法，包括以下步骤：1)建立外界低频扰动下的水下无人潜器动力学模型与无人艇动力学模型并进行离散化；2)采集当前k时刻无人艇与水下无人潜器的状态信息；3)对外界扰动进行估计得到估计值；4)通过基于模型预测控制思想的优化计算得到从当前时刻到k+N时刻的一系列标称推力和扰动补偿力；5)根据得到的推力序列，将得到的第一个推力与补偿力相加应用于系统作为当前时刻的控制输入从而进行水下无人潜器和无人艇的对接控制。与现有技术相比，本发明能够实现在复杂扰动海洋环境中实现无人艇与水下无人潜器的动态对接，具有提高对接精度等优点。

Description

无人艇与水下无人潜器联合系统的协同交会对接控制方法

技术领域

本发明涉及多海洋机器人协同交会对接控制技术，尤其是涉及一种无人艇与水下无人潜器联合系统的协同交会对接控制方法。

背景技术

随着对海洋资源的日益重视，海洋探索技术已经成为实现资源探索、海洋科考等任务的重中之重。而在海洋探索技术中，以无人艇与水下无人潜器为代表的海洋机器人的相关控制技术是提高探索效率的关键，传统海洋机器人技术主要集中在单体的控制上，如单体的轨迹跟踪，定速巡航等。但由于水下无人潜器受体积容积的限制，其续航能力和数据存储能力受到极大的限制，因此，单一的海洋机器人很难执行长时间的连续工作，其必须要返回岸边进行能源补给，这对海洋搜索任务的工作效率有很大的影响。

针对此问题，利用多机器人协同控制技术，可将水下无人潜器与无人艇相结合，以无人艇作为移动充电站，为执行工作的水下无人潜器提供移动充电服务，构建无人艇与水下无人潜器的联合系统可极大地提高整个系统的工作效率，减少往返于固定充电站所消耗的时间，这其中的关键技术便是二者的交会对接技术。

作为具有高度非线性与强耦合性的机器人系统，无论是水下无人潜器还是无人艇在高精度控制上具有相当的难度。而两者相结合的联合系统其复杂性更高，面临着跨域的风浪流复杂扰动，实现相对静止的交会对接协同控制非常困难。

美国在近二十年来针对相关领域进行了一系列研究，如MIT团队和德州农机大学等但大多停留在无人艇与水下无人潜器的协同路径规划，鲜有深入到控制层面的研究；欧洲一些团队，比如瑞典皇家理工学院针对无人艇与无人机进行了相关的动态对接研究；我国在海洋领域起步相对较晚，上海交通大学针对无人机与无人艇的对接研究做了一些工作，而在无人艇与水下无人潜器的对接控制领域研究较少。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种无人艇与水下无人潜器联合系统的协同交会对接控制方法。

本发明的目的可以通过以下技术方案来实现：

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

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

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

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

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

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

所述的步骤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(·)三个三角函数。

所述的步骤1)中，无人艇动力学模型的表达式为：

其中，ρ_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)中，采集的状态信息包括无人艇的速度信息、位置信息、推力输入信息以及水下无人潜器的速度信息、位置信息、推力输入信息。

所述的步骤3)中，对外界扰动进行估计具体为：

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

β_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)中，基于模型预测控制思想的优化具体包括以下步骤：

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

42)构建约束条件；

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

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

和

作为此刻待执行的补偿项。

所述的步骤41)中，系统的优化目标函数J的表达式为：

其中，

为水下无人潜器的三维位置信息在水平面上的投影位置，X_d为水下无人潜器预定执行的任务轨迹信息，

表示在k时刻预测的k+i时刻的X_A信息，矩阵K₁，K₂，K₃，N₁，N₂，N₃和I₁，I₂分别为选定的权矩阵，N为设定的优化步长，

为无人艇与水下无人潜器相对位置误差在水平面的投影，

为水下无人潜器部分状态向量，||·||²分别表示关于权矩阵的二范数平方运算。

所述的步骤42)中，约束条件包括：

系统初始状态约束：

X_A(k|k)＝X_A(k)

X_U(k|k)＝X_U(k)

其中，X_A(k|k)为在k时刻对k时刻水下无人潜器状态的预测值，X_U(k|k)为在k时刻对k时刻无人艇状态的预测值；

系统状态方程约束：

X_A(k+i+1|k)＝h_A，d(X_A(k+i|k)，τ_A(k+i|k))

X_U(k+i+1|k)＝h_U，d(X_U(k+i|k)，τ_U(k+i|k))

速度约束：

λ_min≤Fv_A(k+i+1|k)≤λ_max

其中，λ_min和λ_max为水下无人潜器允许的最小与最大速度，F为速度约束矩阵，且F＝diag(1，1，1，0，0，0)，v_A(k+i+1|k)为水下无人潜器在k时刻对k+i+1时刻水下无人潜器速度的预测值；

非负深度约束：

z_A(k+i+1|k)＞＝0

其中，z_A(k+i+1|k)为水下无人潜器在k时刻对k+i+1时刻水下无人潜器深度的预测值；

推力幅值约束：

τ_A，lo≤τ_A(k+i|k)≤τ_A，up

τ_U，lo≤τ_U(k+i|k)≤τ_U，up

其中，τ_A，lo、τ_U，lo分别为水下无人潜器与无人艇的最小推力限制，τ_A，up、τ_U，up为水下无人潜器与无人艇的最大推力限制；

抗扰动推力的幅值约束：

其中，τ _c，A，τ _c，U(k)分别为水下无人潜器与无人艇在k时刻用于扰动补偿的推力最小限制值，

为水下无人潜器与无人艇在k时刻用于扰动补偿的推力最大限制值，τ_A(k|k)为水下无人潜器在k时刻对k时刻其推力的计算值，τ_U(k|k)为无人艇在k时刻对k时刻其推力的计算值。

所述的步骤44)中，通过设计扰动抵消措施，设计求解出额外的控制量从而最大程度减小外界扰动对系统的干扰，通过求解以下最优问题实现：

约束条件为：

其中，γ为估计误差对速度的最大影响值，且

为扰动估计和真实值之间的误差，M指代无人艇或水下无人潜器的质量矩阵，τ_c(k)为扰动补偿输入，τ _c(k)和

为扰动补偿输入的最大和最小输入约束值，Δt为采样时间，

为对k时刻扰动的估计值。

与现有技术相比，本发明具有以下优点：

一、本发明填补了目前在海洋环境中无人艇与水下无人潜器对接控制的空白，同时解决现有对接技术性能较差，难以克服外界扰动从而导致对接精度不高的问题。

二、本发明通过采用基于滚动时域优化与实时控制的对接方法，在每一时刻动态调整自己的策略并进行控制，极大地提高了系统的对接准确度。

三、本发明中水下无人潜器可以在与无人艇进行对接充电的同时，尽可能地执行既定的轨迹跟踪任务，相比于其他现有静态对接技术，本方法解决了动态对接问题，显著提高了效率和适用性。

四、本方法设计了针对外界扰动进行补偿的环节，可以自适应产生抗扰动补偿力，提高了系统的抗扰动性。

附图说明

图1为本发明中水下无人潜器坐标示意图。

图2为本发明中无人艇坐标示意图。

图3为本发明中对接控制算法流程图。

图4为利用本发明得到的部分对接误差与无扰动补偿方法的对比图，其中，图(4a)为无人艇与水下无人潜器在x方向上的对接误差，图(4b)为无人艇与水下无人潜器在y方向上的对接误差，图(4c)为无人艇与水下无人潜器前向速度上的相对误差，图(4d)为无人艇与水下无人潜器横荡速度的相对误差。

图5为利用本发明得到的部分对接误差与无扰动补偿方法的对比图，其中，图(5a)为水下无人潜器深度，图(5b)为无人艇与水下无人潜器航向角度误差，图(5c)为水下无人潜器垂荡速度，图(5d)为无人艇与水下无人潜器航向角速度相对误差。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。

实施例

本发明提供一种面向无人艇与水下无人潜器联合系统的协同交会对接控制方法，解决了复杂海洋环境中无人艇与水下无人潜器交会对接问题，提出了切实可行的解决方案且提高了系统的抗扰动能力，该方法具体包括以下步骤：

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

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

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

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

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

步骤1)具体为：

(11)水下无人潜器的动力学模型具体为：

ρ_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，具体说明可参见说明附图1。M_A为质量矩阵，C_A为科里奥利矩阵，D_A为阻尼矩阵，g为恢复力向量，τ_A为输入。其他参数如下：

其中c(·)，s(·)，t(·)分别代表cos(·)，sin(·)，tan(·)三个函数。

(12)无人艇动力学模型具体为：

其中ρ_U＝[x_U，y_U，ψ_U]^T为无人艇的位置状态向量，v_U＝[u_U，v_U，r_U]^T为无人艇的速度状态向量，具体说明可参见说明附图2。M_U为质量矩阵，C_U为科里奥利矩阵，D_U为阻尼矩阵，τ_U为输入。其他参数如下：

(13)将水下无人潜器与无人艇的上述连续模型以Δ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时刻受到的外界扰动。

本发明中低频扰动是指无人艇与水下无人潜器在海况良好的情况下，收到的外界扰动，如风浪流均为低频扰动，且符合实际情况。

本发明步骤(2)中状态信息包括无人艇与水下无人潜器的速度，位置以及推力数据信息。

本发明步骤(3)中对外界扰动进行估计的具体方法如下：

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

其中

为对k时刻扰动的估计值，即以上一时刻的扰动值作为此时刻扰动的估计值，由于海洋扰动是低频的，因此在采样时间小的情况下估计是较为准确的本发明步骤(4)中基于模型预测控制思想的优化具体过程如下：

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

其中

弋表水下无人潜器的三维位置信息在水平面上的投影位置，X_d为水下无人潜器预定执行的任务轨迹信息。

表示在k时刻预测的k+i时刻的X_A信息。矩阵K₁，K₂，K₃，N₁，N₂，N₃和I₁，I₂分别为选定的权矩阵。

(42)建立约束。

X_A(k|k)＝X_A(k)

X_U(k|k)＝X_U(k)

X_A(k+i+1|k)＝h_A，d(X_A(k+i|k)，τ_A(k+i|k))

X_U(k+i+1|k)＝h_U，d(X_U(k+i|k)，τ_U(k+i|k))

λ_min≤Fv_A(k+i+1|k)≤λ_max

z_A(k+i+1|k)＞＝0

τ_A，lo≤τ_A(k+i|k)≤τ_A，up

τ_U，lo≤τ_U(k+i|k)≤τ_U，up

前两行表示系统初始状态约束，第三四行表示系统状态方程约束，第五行表示速度约束，第六行表示非负深度约束，七八行表示推力幅值约束。最后两行表示抗扰动推力的幅值约束。其中F＝diag(1，1，1，0，0，0)为速度约束矩阵。λ_min和λ_max为水下无人潜器允许的最小与最大速度。τ_A，lo，τ_U，lo为水下无人潜器与无人艇的最小推力限制，τ_A，up，τ_U，up为水下无人潜器与无人艇的最大推力限制。τ _c，A，τ _c，U(k)为水下无人潜器与无人艇在k时刻用于扰动补偿的推力最小限制值，

为水下无人潜器与无人艇在k时刻用于扰动补偿的推力最大限制值。

(43)构建模型预测控制优化问题。

根据步骤(41)-(43)，构建出关于无人艇与水下无人潜器的对接系统的模型预测控制优化问题如下：

Subject to

X_A(k|k)＝X_A(k)

X_U(k|k)＝X_U(k)

X_A(k+i+1|k)＝h_A，d(X_A(k+i|k)，τ_A(k+i|k))

X_U(k+i+1|k)＝h_U，d(X_U(k+i|k)，τ_U(k+i|k))

λ_min≤Fv_A(k+i+1|k)≤λ_max

z_A(k+i+1|k)＞＝0

τ_A，lo≤τ_A(k+i|k)≤τ_A，up

τ_U，lo≤τ_U(k+i|k)≤τ_U，up

在每一时刻，求解上述关于N个对接预测步长的优化问题，得到优化后的N-1个时刻的推力序列

(44)设计扰动补偿控制。通过设计扰动抵消措施，设计求解出额外的控制量从而最大程度减小外界扰动对系统的干扰，可通过求解下列最有问题实现：

subject to

其中

为扰动估计和真实值之间的误差，M指代无人艇或水下无人潜器的质量矩阵，τ_c(k)为扰动补偿输入，τ _c(k)和

为扰动补偿输入的最大和最小输入约束值。

分别为无人艇和水下无人潜器求解上述优化问题，得到关于无人艇与水下无人潜器对应的最优抗扰动补偿干扰输入

和

作为此刻待执行的补偿项。

本发明步骤(5)中具体控制过程为在步骤(1)至步骤(4)的基础上，在当前t＝k时刻，将步骤(4)中得到的

分别与

相加，得到标称控制和扰动补偿控制之和的最终控制量，并在此刻将此控制量发送给执行机构进行执行。

当每次执行完成后，重新测量当前状态信息，进行下一轮计算与控制，不断重复上述优化与控制的循环，直到任务时间结束或对接完成。整个方法流程可参见附图3。

按照上述步骤具体实施时，系统主要由传感器模块、控制中心、通讯模块与执行器四大部分协同完成。

传感器模块包括安装在无人艇与水下无人潜器上的惯性元件IMU，用于获取各自位置信息的GPS，通过传感器可以获取相应的位置速度等状态信息用于后续处理。通讯模块包括安装在无人艇与水下无人潜器上的水声通讯设备，可以将各自的位置速度信息互相传递，从而实现两个个体之间的协同工作。控制中心是指机载的计算处理单元，如嵌入式处理器，可进行传感器信息的收集，控制算法的处理，以及控制指令的下发。执行器包括安装在无人艇与水下无人潜器上的推进器，螺旋桨等动力推进装置，用于执行控制指令，实现个体的运动。

进一步地，在上述控制方法的执行步骤与系统架构下，可以根据任务需求与实际情况的不同，对系统的核心控制中心模块进行有针对性的重点设计。该设计具体步骤为：

(1)控制器约束的设计。

在实际情况中，可能会采用不同型号的无人艇或水下无人潜器，其驱动方式，应用场景，执行任务环境等都会有所不同。针对这种情况，设计不同约束条件可以使得该方法的使用更加有效。在本方法中基本约束包括速度约束，非负位深度约束，推力约束等。这些约束分别保证了无人艇与水下无人潜器的运动速度符合实际限制，不会超过系统本身的物理上限，水下无人潜器上浮过程始终在水面及以下，不会出现潜器浮到水面以上与无人艇相撞的计算结果，以及二者的推力在推进器物理限制之内。

可根据实际选用设备与对接任务场景不同进行约束的增减，如增加机器人的在其他运动方向上的速度限制。

(2)控制器参数的设定。

在对接任务的基础上，根据不同的需求和成本考量，设计不同的控制器参数。Q₁，直接影响潜器在执行对接任务时脱离原预定轨迹的偏离程度，Q₂影响无人艇与潜器对接时的对接精度，Q₃影响潜器在对接时自身滚转俯仰等方向上的稳定性。R₁，R₂直接影响水下无人潜器和无人艇能源消耗的多少。当考量因素较为重要时可通过调大相应参数来实现任务的适配。

通过以上步骤和设计方法，本发明可有效解决无人艇与水下无人潜器在低频风浪流扰动环境中的动态对接问题，提高了对接精度，对接结果与对接误差见附图4，5。

上述实施方式仅为例举，不表示对本发明范围的限定。这些实施方式还能以其它各种方式来实施，且能在不脱离本发明技术思想的范围内作各种省略、置换、变更。

Claims (5)

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)中得到的推力序列，将得到的第一个推力与补偿力相加应用于系统作为当前时刻的控制输入从而进行水下无人潜器和无人艇的对接控制。

2.根据权利要求1所述的一种无人艇与水下无人潜器联合系统的协同交会对接控制方法，其特征在于，所述的步骤2)中，采集的状态信息包括无人艇的速度信息、位置信息、推力输入信息以及水下无人潜器的速度信息、位置信息、推力输入信息。

3.根据权利要求1所述的一种无人艇与水下无人潜器联合系统的协同交会对接控制方法，其特征在于，所述的步骤41)中，系统的优化目标函数J的表达式为：

其中，

为水下无人潜器的三维位置信息在水平面上的投影位置，X_d为水下无人潜器预定执行的任务轨迹信息，

表示在k时刻预测的k+i时刻的X_A信息，矩阵K₁，K₂，K₃，N₁，N₂，N₃和I₁，I₂分别为选定的权矩阵，N为设定的优化步长，

为无人艇与水下无人潜器相对位置误差在水平面的投影，

为水下无人潜器部分状态向量，‖·‖²分别表示关于权矩阵的二范数平方运算。

4.根据权利要求3所述的一种无人艇与水下无人潜器联合系统的协同交会对接控制方法，其特征在于，所述的步骤42)中，约束条件包括：

系统初始状态约束：

X_A(k∣k)＝X_A(k)

X_U(k∣k)＝X_U(k)

其中，X_A(k∣k)为在k时刻对k时刻水下无人潜器状态的预测值，X_U(k∣k)为在k时刻对k时刻无人艇状态的预测值；

系统状态方程约束：

X_A(k+i+1|k)＝h_A，d(X_A(k+i|k)，τ_A(k+i|k))

X_U(k+i+1|k)＝h_U，d(X_U(k+i|k)，τ_U(k+i|k))

速度约束：

λ_min≤Fv_A(k+i+1|k)≤λ_max

其中，λ_min和λ_max为水下无人潜器允许的最小与最大速度，F为速度约束矩阵，且F＝diag(1，1，1，0，0，0)，v_A(k+i+1|k)为水下无人潜器在k时刻对k+i+1时刻水下无人潜器速度的预测值；

非负深度约束：

z_A(k+i+1|k)＞＝0

其中，z_A(k+i+1|k)为水下无人潜器在k时刻对k+i+1时刻水下无人潜器深度的预测值；

推力幅值约束：

τ_A，lo≤τ_A(k+i|k)≤τ_A，up

τ_U，lo≤τ_U(k+i|k)≤τ_U，up

其中，τ_A，lo、τ_U，lo分别为水下无人潜器与无人艇的最小推力限制，τ_A，up、τ_U，up为水下无人潜器与无人艇的最大推力限制；

抗扰动推力的幅值约束：

其中，τ _c，A，τ _c，U(k)分别为水下无人潜器与无人艇在k时刻用于扰动补偿的推力最小限制值，

为水下无人潜器与无人艇在k时刻用于扰动补偿的推力最大限制值，τ_A(k|k)为水下无人潜器在k时刻对k时刻其推力的计算值，τ_U(k|k)为无人艇在k时刻对k时刻其推力的计算值。

5.根据权利要求4所述的一种无人艇与水下无人潜器联合系统的协同交会对接控制方法，其特征在于，所述的步骤44)中，通过设计扰动抵消措施，设计求解出额外的控制量从而最大程度减小外界扰动对系统的干扰，通过求解以下最优问题实现：

约束条件为：

其中，γ为估计误差对速度的最大影响值，且

为扰动估计和真实值之间的误差，M指代无人艇或水下无人潜器的质量矩阵，τ_c(k)为扰动补偿输入，τ _c(k)和

为扰动补偿输入的最大和最小输入约束值，Δt为采样时间，

为对k时刻扰动的估计值。

Images