一种无人潜水器模型预测容错跟踪控制方法与流程

本发明涉及水下机器人故障容错技术领域，特别是涉及一种无人潜水器模型预测容错跟踪控制方法。

背景技术：

作为人类探索和开发海洋不可或缺的工具，无人潜水器(unmannedunderwatervehicle，简称uuv)正在起着越来越重要的作用，无人潜水器的前进主要通过推进器进行推动。随着科技的进步，无人潜水器的开发和应用对我国海洋产业、海洋探索和开发具有重大的影响，已成为当今世界海洋工程领域研究的一个热点。

目前，传统的无人潜水器跟踪控制采用如下策略：当uuv在水下作业时，通过传感器获取当前uuv位姿信息，与期望参考轨迹信息相减获取误差值，同时结合速度和加速度信息，构建uuv跟踪控制律，在控制律作用下uuv姿态误差逐渐趋于零并稳定跟踪。但是传统方法一般不会考虑以下问题：(1)uuv自身能源所能提供的最大推力约束；(2)uuv跟踪过程中，推进器长期暴露在外，极有可能遭遇海藻、渔网等缠绕，或者电机故障等导致无法正常运行，此时传统跟踪控制无法解决此类问题，迫切需要一种新型方法去解决以上几点问题。

因此，如何有效的对无人潜水器进行跟踪控制成为亟待解决的技术问题。

技术实现要素：

鉴于以上所述现有技术的缺点，本发明的目的在于提供一种无人潜水器模型预测容错跟踪控制方法，能够在推进器完全故障/部分故障情况下的容错跟踪，确保无人潜水器顺利完成跟踪控制任务。

为实现上述目的及其他相关目的，本发明提供一种无人潜水器模型预测容错跟踪控制方法，所述方法包括步骤：

通过无人潜水器控制系统获得无人潜水器的当前状态参数，其中，所述状态参数至少包括无人潜水器的位置、加速度、速度和推进器的推力值；

确定给定控制电压与对应转速的关系；

如果给定控制电压、转速为0，则判断推进器出现完全故障，并执行步骤：根据推力分配矩阵，删除该故障推进器的对应推力分配矩阵信息，通过伪逆求解得到剩余可布置推进器的第一归一化推力值，其中，推力分配矩阵为所有推进器所组成的矩阵；根据推力分配矩阵和所述第一归一化推力值重构总推力值；

如果实际转速与预设转速大小不一致，则执行步骤：根据故障推进器故障后的转速与原有正常转速的比值，计算推进器故障权系数，通过伪逆求解得到各个推进器归一化推力值，并判断是否存在推进器的推力值大于1，如果存在则采用量子粒子群优化进行推力解空间计算，获得第二归一化后推力值；根据推力分配矩阵和第二归一化后推力值重构总推力值；

根据重构后的总推力值对所述无人潜水器进行轨迹跟踪。

本发明的一种实现方式中，在所述通过无人潜水器控制系统的获得无人潜水器的当前状态参数的步骤之前，所述方法还包括：

(21)通过无人潜水器控制系统获得无人潜水器的当前状态参数；

(22)将无人潜水器的当前状态输入线性误差模型中，获得离散化的预测输出结果，其中，所述线性误差模型为根据通过无人潜水器实际状态和期望状态建立误差模型；

(23)将预先设置的参考轨迹和所述预测输出结果作为目标函数的输入，并对所述目标函数进行求解，获得控制周期内目标函数的求解结果，其中，所述目标函数为预先设置的函数，所述求解结果为所述控制周期的时域内多个控制输入增量值；

(24)选定所述多个控制输入增量值中的第一个作为目标增量值，并将其发送至所述无人潜水器控制系统，驱动无人潜水器进行运动，获得无人潜水器的更新状态参数；

(25)将所述无人潜水器的更新状态作为所述无人潜水器的当前状态参数，并返回步骤(22)。

如上所述，本发明实施例提供的一种无人潜水器模型预测容错跟踪控制方法，能够兼顾推进器完全故障/部分故障情况下的容错跟踪，确保无人潜水器顺利完成跟踪控制任务。

附图说明

图1是本发明实施例无人潜水器模型预测容错跟踪控制方法的一种流程示意图；

图2是本发明实施例无人潜水器模型预测容错跟踪控制方法的另一种流程示意图；

图3是本发明实施例无人潜水器模型预测容错跟踪控制方法的又一种流程示意图；

图4是本发明实施例无人潜水器模型预测容错跟踪控制方法的再一种流程示意图。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。

请参阅附图。需要说明的是，本实施例中所提供的图示仅以示意方式说明本发明的基本构想，遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。

如图1和图2所示，一种无人潜水器模型预测容错跟踪控制方法，包括如下步骤：

s101，通过无人潜水器控制系统的获得无人潜水器的当前状态参数，其中，所述状态参数至少包括无人潜水器的位置、加速度、速度和推力器的推力值。

在作业情况下，无人潜水器通过自身所携带的传感器可以获得其自身当前的位置信息、速度信息、推力器的推力值等。具体的，正常作业情况下，uuv依据自身的传感器信息获得当前的位姿信息以及速度信息，通过求解携带约束目标函数的模型预测控制方法获取并向uuv输出有效跟踪控制量，uuv在控制信号作用下实现对全局参考轨迹的跟踪。

轨迹跟踪控制过程中采用的模型预测控制方法如图3所示，其中，虚线框为mpc控制器的主体，主要由线性误差模型、系统约束以及目标函数构成。具体而言，首先通过无人潜水器uuv实际状态和期望状态建立误差模型，并进行离散化处理；其次，将控制量约束与控制增量约束加入优化目标函数，在每个控制周期内完成对目标函数的最优化求解，得到控制时域内的一系列控制输入增量，将该控制序列中第一个元素作为实际的控制输入增量作用于uuv系统。控制律作用于uuv，生成下一时刻的实际uuv状态。如此循环反复，直至最终跟踪控制完成。

s102，确定给定控制电压与对应转速的关系。

s103，如果给定控制电压、转速为0，则判断推进器出现完全故障，执行步骤s104，如果实际转速与预设转速大小不一致，执行步骤s105。

具体的，预设转速可以为预先计算出来的理论转速，计算过后进行存储，当需要进行比较的时候，则取出与实际所测量得到的转速进行比较，确认是否一致，具体的判断是否一致的标准可以是：预设转速可以是一个数值区间，例如，位于第一数值和第二数值之间的任意一个区间；针对每一个实际转速，判断是否落入该数值区间中，如果是，则表示一致，否则，则为不一致。

s104，根据推力分配矩阵，删除该故障推进器的对应推力分配矩阵信息，通过伪逆求解得到剩余可布置推进器的第一归一化推力值，其中，推力分配矩阵为所有推进器所组成的矩阵；根据推力分配矩阵和所述第一归一化推力值重构总推力值。

如图4所示，示例性的，当有第i个推进器ti出现完全故障情形时，根据推力分配矩阵，删除对应第i个推进器的对应推力分配矩阵信息，通过伪逆求解得到剩余可布置推进器的推力分布，即得到归一化后的第一归一化推力值ti，实现推进器完全故障下的容错跟踪。

s105，根据故障推进器故障后的转速与原有正常转速的比值，计算推进器故障权系数，通过伪逆求解得到各个推进器归一化推力值，并判断是否存在推进器的推力值大于1，如果存在则采用量子粒子群优化进行推力解空间计算，获得第二归一化后推力值；根据推力分配矩阵和第二归一化后推力值重构总推力值。

假设，当有推进器ti出现部分故障，即推进器仍能输出部分推力，根据各个推进器故障后的转速与原有正常转速的比值，推导出推进器故障权系数wi，如果有多个推进器部分故障，则依次获得各个推进器故障权系数，扩展得到推进器故障权系数矩阵w＝diag[w1,w2,wi,…wj…]。各个推进器推力值为矩阵t，推力分配矩阵为b，考虑推进器部分故障情况下，归一化的推进器推力值与合力/力矩之间的关系可以表示如下：

首先，通过伪逆求解得到各个推进器归一化推力值，如果所有推力值都小于1，则可以直接作用于无人潜水器进行跟踪控制，如果有任一推进器ti的推力值超出最大值1，则此时采用量子粒子群优化进行推力解空间的寻优。将所需求解的推力值矩阵t作为待求解的解空间，确定粒子群的规模n、粒子的维数d(与推进器个数相同)和最大迭代次数等初始参数，计算每个粒子的适应度值，适应度函数采取无穷范数形式保证多个推进器中的最大推力值最小。对于每个粒子，将其适应值与所经历过的最好位置的适应值进行比较。如果更好，则将其作为粒子的个体历史最优值，用当前位置更新个体历史最好位置。对于每个粒子，比较它的适应度值和群体所经历的最好位置的适应度值。若更好，更新最好位置。

根据迭代更新公式调整粒子的位置。

x(t+1)＝pi-β*(mbest-xt)*ln(1/z)ifz≥0.5

x(t+1)＝pi+β*(mbest-xt)*ln(1/z)ifz＜0.5

其中，x(t),x(t+1)分别对应t和t+1时刻的粒子位置信息，mbest为个体最优平均值，β为收缩扩张因子，pbest和gbest分别为个体最优和群体最优，z为区间(0,1)上的随机数，n为搜索空间维数，为(0,1)间的系数值。

直至达到结束条件(推力值已在约束范围内或最大迭代次数，以此优化后的推力值作用于无人潜水系统进行部分故障情况下的跟踪控制。

s106，根据重构后的总推力值对所述无人潜水器进行轨迹跟踪。

具体的，进行无人潜水器的轨迹跟踪过程可以采用现有的方案执行，本发明实施例在此不做赘述。

另外，本发明实施例还提供了一种进行轨迹跟踪的具体方式，步骤包括：

(21)通过无人潜水器控制系统获得无人潜水器的当前状态参数；

(22)将无人潜水器的当前状态输入离散化的线性误差模型中，获得离散化的预测输出结果，其中，所述线性误差模型为根据通过无人潜水器实际状态和期望状态建立误差模型；

具体的，一种实现方式中，线性误差模型为：其中，k是采样时刻，是状态误差，是控制量差值，a和b分别对应状态误差的转换矩阵和控制量差值的转换矩阵。

(23)将预先设置的参考轨迹和所述预测输出结果作为目标函数的输入，并对所述目标函数进行求解，获得控制周期内目标函数的求解结果，其中，所述目标函数为预先设置的函数，所述求解结果为所述控制周期的时域内多个控制输入增量值；

示例性的，目标函数的表达可以为如下所示，

且，该目标函数可以根据需要预先设置和调整，因此，模型预测控制在每一步的约束优化求解问题都等价于求解如下的二次规划问题：其中δv(t)为带约束增量值，ht和gt为对应的转换矩阵。在每个控制周期内完成等式求解后，得到了控制时域内的一系列控制输入增量：

(24)选定所述多个控制输入增量值中的第一个作为目标增量值，并将其发送至所述无人潜水器控制系统，驱动无人潜水器进行运动，获得无人潜水器的更新状态参数；

(25)将所述无人潜水器的更新状态作为所述无人潜水器的当前状态参数，并返回步骤(22)。

上述实施例仅例示性说明本发明的原理及其功效，而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。