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

摘要

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

说明书

技术领域

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

背景技术

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

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

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

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

发明内容

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

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

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

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

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

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

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

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

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

其中，ρ

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

其中，ρ

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

X

X

其中，h

所述的步骤2)中，采集的状态信息包括无人艇的速度信息、位置信息、推力输入信息以及水下无人潜器的速度信息、位置信息、推力输入信息。

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

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

β

其中，

所述的步骤4)中，基于模型预测控制思想的优化具体包括以下步骤：

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

42)构建约束条件；

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

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

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

其中，

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

系统初始状态约束：

X

X

其中，X

系统状态方程约束：

X

X

速度约束：

λ

其中，λ

非负深度约束：

z

其中，z

推力幅值约束：

τ

τ

其中，τ

抗扰动推力的幅值约束：

其中，

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

约束条件为：

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

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

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

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

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

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

附图说明

图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)水下无人潜器的动力学模型具体为：

ρ

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

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

其中ρ

(13)将水下无人潜器与无人艇的上述连续模型以Δt＝0.1秒为采样步长进行离散化可得离散模型如下：

X

X

其中h

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

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

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

β

其中

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

其中

(42)建立约束。

X

X

X

X

λ

z

τ

τ

前两行表示系统初始状态约束，第三四行表示系统状态方程约束，第五行表示速度约束，第六行表示非负深度约束，七八行表示推力幅值约束。最后两行表示抗扰动推力的幅值约束。其中F＝diag(1，1，1，0，0，0)为速度约束矩阵。λ

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

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

Subject to

X

X

X

X

λ

z

τ

τ

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

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

subject to

其中

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

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

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

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

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

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

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

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

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

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

在对接任务的基础上，根据不同的需求和成本考量，设计不同的控制器参数。Q

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

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