一种基于模型预测的欠驱动水下航行器精确控制方法

本发明涉及水下航行器控制，特别涉及一种基于模型预测的欠驱动水下航行器精确控制方法。

背景技术：

1、水下航行器在水下环境中的运动控制通常涉及位置、姿态、速度和推进器输出等多个环节。由于水下环境复杂，航行器在运动过程中容易受到海流扰动、模型参数不确定、推进器输出受限以及多自由度耦合等因素影响，因此需要控制系统能够在保证运动精度的同时，兼顾控制稳定性和推进器输出平稳性。现有水下航行器的定点控制、轨迹跟踪控制方法主要包括以下几类：

2、pid控制方法：pid控制方法结构简单、计算量小、工程实现方便，常用于水下航行器的航向控制、深度控制和速度控制。该方法通常根据目标值与当前状态之间的误差计算控制量，再通过推进器输出实现运动控制。

3、但是，传统pid控制方法通常依赖固定控制参数，面对海流扰动、目标距离变化较大或多自由度耦合明显的情况时，控制效果容易下降。例如，在三维目标点接近过程中，水平位置和深度方向的响应速度可能不一致，出现水平位置已经接近目标而深度仍然变化较慢的情况。同时，当误差较大时，pid输出可能产生较大的推力波动，导致推进器输出不够平稳。

4、模型预测控制方法：模型预测控制方法能够根据航行器运动模型，在一定预测时域内对未来运动状态进行预测，并通过优化目标函数获得较优控制量。与传统pid控制相比，模型预测控制能够处理速度约束、控制输入约束等问题，适合用于水下航行器的多变量控制任务。

5、但是，现有模型预测控制方法在应用于水下航行器时，仍然存在一定不足。一方面，部分方法未充分考虑海流扰动对预测模型的影响，导致预测轨迹与实际运动之间产生偏差；另一方面，控制权重和速度约束通常采用固定参数，难以根据远距离接近、中距离调整和接近目标阶段的不同控制需求进行动态调节，容易造成接近目标时超调、振荡或收敛速度较慢。

6、推力分配方法：水下航行器通常由多个推进器共同产生运动所需推力。控制器计算出的期望运动指令，需要进一步转换为各推进器的实际推力输出。现有方法中常采用固定比例分配或经验式推力分配方式，该类方法实现简单，但较难同时考虑推进器最大推力、最小推力、推力变化率以及多个推进器之间的协调关系。

7、当航行器姿态变化较快或控制误差较大时，简单推力分配方法可能导致推进器输出突变，使推力曲线出现较大波动。该问题不仅会影响航行器运动稳定性，也不利于实际水下推进器的长期可靠工作。

8、综上所述，现有水下航行器控制方法在三维目标点接近和定点控制过程中，仍存在海流扰动补偿不足、不同方向响应速度不协调、接近目标阶段控制策略单一、控制参数难以自适应调整以及推进器输出波动较大等问题。

技术实现思路

1、为此，本发明的一个目的在于提出一种基于模型预测的欠驱动水下航行器精确控制方法，其能够在海流扰动环境下实现较稳定的三维运动控制，并使水平位置、深度和姿态响应更加协调，同时降低推进器输出突变，提高航行器接近目标点和稳定驻留的控制效果。

2、为了实现上述目的，本发明提供一种基于模型预测的欠驱动水下航行器精确控制方法，包括：

3、获取欠驱动水下航行器水下航行器的当前位姿、当前速度、目标状态以及海流流速；

4、根据所述当前位姿与所述目标状态计算三维距离，并基于预设的远距离阈值和接近目标阈值确定当前控制阶段，所述控制阶段包括远距离接近阶段、中距离调整阶段和接近目标阶段；

5、基于所述海流流速构建带海流补偿的非线性模型预测控制nmpc预测模型，对所述模型进行离散化处理，并基于所述当前控制阶段自适应调整nmpc优化问题中的状态权重矩阵和控制输入权重矩阵，以及控制输入的下限和上限；

6、求解nmpc优化问题，得到期望速度向量，其中为期望纵向速度，为期望垂向速度，为期望俯仰角速度，为期望偏航角速度；

7、对所述期望速度向量进行三轴协调修正，得到协调后的期望速度向量；

8、基于所述协调后的期望速度向量与当前速度的误差，通过pid控制器计算广义力；

9、构建推力优化分配目标函数，结合推进器推力上下限约束及推力变化率约束，求解得到各推进器的实际推力输出，用以驱动水下航行器运动。

10、作为优选，定义基础状态误差权重矩阵：，其中，分别为北向位置、东向位置、深度误差权重的基础值，分别为横滚角、俯仰角、航向角误差权重的基础值；

11、定义基础控制输入权重矩阵：，其中，分别为期望纵向速度、期望垂向速度控制输入权重的基础值，分别为期望俯仰角速度、期望偏航角速度控制输入权重的基础值；

12、在当前控制阶段下，状态权重矩阵，其中分别为北向位置误差权重、东向位置误差权重、深度误差权重，分别为横滚角误差权重、俯仰角误差权重、航向角误差权重；

13、控制输入权重矩阵，其中，分别为期望纵向速度权重、期望垂向速度权重，分别为期望俯仰角速度权重和期望偏航角速度权重；

14、所述基于所述当前控制阶段自适应调整nmpc优化问题中的状态权重矩阵和控制输入权重矩阵，以及控制输入的下限和上限包括：

15、在远距离接近阶段：

16、；

17、在中距离调整阶段：

18、；

19、在接近目标阶段，根据距离因子动态提高北向位置误差权重、东向位置误差权重、深度误差权重：

20、；

21、；

22、；

23、且：

24、；

25、；

26、所述控制输入的下限和上限根据阶段缩放系数进行缩放。

27、作为优选，所述带海流补偿的非线性模型预测控制nmpc预测模型的运动学方程表示为：

28、；

29、；

30、；

31、；

32、；

33、；

34、其中，表示海流在北向方向上的速度分量，表示海流在东向方向上的速度分量，表示海流在深度方向上的速度分量。

35、作为优选，所述对所述期望速度向量进行三轴协调修正，具体包括：

36、计算剩余水平距离和深度误差，当剩余水平距离和深度误差均大于设定阈值时，计算公共速度比例因子；

37、基于所述公共速度比例因子修正期望纵向速度和期望垂向速度，得到初步修正值和；

38、计算航向误差，并根据所述航向误差计算速度缩放系数，利用速度缩放系数对进行二次修正，得到最终期望纵向速度。

39、作为优选，所述速度缩放系数如下：

40、；

41、；

42、其中，为航向误差，为航向误差阈值。

43、作为优选，所述通过pid控制器计算广义力的过程中，对积分项进行抗饱和限幅处理：

44、；

45、其中，表示第个控制周期内第个速度通道的积分项，表示第个控制周期内第个速度通道的速度误差，表示控制器的采样周期，表示积分项允许的最大幅值，表示积分项允许的最小幅值。

46、作为优选，所述构建推力优化分配目标函数，具体为：

47、；

48、其中，为广义力跟踪误差权重，表示推进器推力到广义力的映射矩阵，为水下航行器的四个推进器的实际推力输出，表示左水平推进器推力，表示右水平推进器推力，表示前垂向推进器推力，表示后垂向推进器推力，表示期望广义力向量，为推力平滑权重，表示上一控制周期的推进器推力向量。

49、作为优选，所述求解得到各推进器的实际推力输出采用投影梯度迭代求解：

50、；

51、其中，表示第次迭代时的推进器推力向量，表示梯度下降步长，表示目标函数在处对推力向量的梯度。

52、作为优选，还包括：

53、实时计算水下航行器的当前速度幅值及三维距离；

54、当满足且，并连续保持个控制周期时，判定水下航行器已到达目标点并稳定驻留，此时将各推进器推力输出置零；

55、其中，为判断水下航行器是否到达目标点的水下航行器与目标点的距离，为判断水下航行器是否到达目标点的水下航行器的速度，为稳定判定所需的连续控制周期数。

56、作为优选，所述确定当前控制阶段具体为：

57、。

58、作为优选，还包括目标航向角的自适应确定：

59、当所述当前控制阶段为远距离接近阶段或中距离调整阶段时，所述目标航向角按照如下公式计算：

60、；

61、；

62、；

63、其中，、分别表示欠驱动水下航行器的目标点在北向、东向上的位置，、分别为水下航行器当前的北向、东向位置；

64、当所述当前控制阶段为接近目标阶段时，所述目标航向角等于水下航行器的当前实际航向角：

65、。

66、与现有技术相比，本发明所具有的优点和有益效果为：

67、本发明的一种基于模型预测的欠驱动水下航行器精确控制方法，通过在非线性模型预测控制中引入海流速度项并进行离散化，使预测模型能够提前补偿海流造成的位置漂移，解决了现有模型预测控制方法未充分考虑海流扰动导致预测轨迹与实际运动产生偏差的问题，从而在有流环境下提高了水下航行器接近目标点的控制精度与运动稳定性。

68、本发明根据航行器与目标点的三维距离将控制过程划分为远距离接近、中距离调整和接近目标三个阶段，并依据当前阶段自适应调整状态误差权重矩阵与控制输入权重矩阵，同时对速度约束边界进行动态缩放，解决了现有技术中控制权重和速度约束采用固定参数、难以适应不同控制阶段需求的问题，使航行器在远距离阶段能够快速接近目标，在接近目标阶段自动降低速度、增大控制权重以抑制超调与振荡，实现了控制过程的高效性与稳态精度的统一。

69、本发明通过计算剩余水平距离与深度误差的公共速度比例因子，并对纵向与垂向期望速度进行协调修正，同时引入基于航向误差的速度缩放系数，解决了现有技术中水平位置与深度方向响应速度不一致以及航向未对准时易产生水平绕行的问题，使北向、东向和深度方向能够以协调的节奏到达目标点，改善了多自由度耦合下的运动同步性。

70、本发明在推力分配环节构建了包含广义力跟踪误差与推力变化平滑项的优化目标函数，并严格约束推进器的推力上下限及推力变化率，采用投影梯度迭代法求解，解决了现有固定比例或经验式推力分配方式难以兼顾推进器物理约束、易导致输出突变的缺陷，降低了推力曲线的波动幅度，减少了推进器磨损，提升了系统长期工作的可靠性与工程可实现性。

71、本发明通过分阶段切换目标航向角计算策略，在接近目标阶段将目标航向锁定为当前实际航向，避免了因水平距离极小导致目标航向角计算值跳变而引发的原地急转，进一步提升了航行器在末端控制阶段的姿态平稳性与驻留稳定性。

72、本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。