CN112558620A - 一种欠驱动自主水下航行器的航向修正路径跟踪方法 - Google Patents

Abstract

本专利提出了一种欠驱动自主水下航行器的航向修正路径跟踪法方法，通过修正水下航行器的航向角提供准确的理想航向，并设计欠驱动自主水下航行器的运动控制律，可有效实现精确的路径跟踪。该方法首先依据参考路径的切向修正水下航行器的理想航向，消除侧滑角导致的跟踪误差。然后构造水下航行器的跟踪误差，建立水下航行器的动力学模型。在此基础上，利用Lyapunov方程求解水下航行器的运动控制律，计算运动控制量并实现水下航行器的路径跟踪。该方法通过修正参考路径的切向提供更加准确的理想航向角，将路径跟踪转化为理想航向角速度跟踪，不仅简化了路径跟踪控制器设计，更加保障了系统的稳定性和控制效果。

Description

一种欠驱动自主水下航行器的航向修正路径跟踪方法

技术领域

本发明设计一种水下航行器的路径跟踪控制方法，具体涉及一种欠驱动自主水下航行器的航向修正路径跟踪法方法。

背景技术

水下航行器是进行海洋探测的重要设备，在水下探测，搜索等领域发挥着重要作用。水下航行器的路径跟踪控制是水下航行器规避障碍物、完成探测任务提供重要方法和先决性基础，被广泛的应用于水下航行器的运动控制，并取得了良好效果。然而，水下航行器具有欠驱动特性，且缺乏准确的航向角生成方法，难以有效的控制水下航行器，降低了路径跟踪效果。目前水下航行器路径跟踪通常以参考路径的切向方向作为航向，设计运动控制算法消除航向误差和侧向偏差，实现水下航行器的路径跟踪。由于水下航行器侧滑角的影响，导致路径跟踪误差。在跟踪曲线路径过程中，水下航行器需要进行转向，导致其重心速度方向与航向并不完全一致，进而产生侧滑角。由于侧滑角的存在，传统的理想航向仅沿重心速度方向，不可避免的使路径跟踪的存在稳态误差。因此，利用侧滑角进行修正理想航向，消除路径跟踪过程中产生的稳态误差。而且，水下航行器具有欠驱动特性，难以实现准确的路径跟踪控制。

发明内容

本发明解决的技术问题是：本发明旨在客服上述现有技术的缺点，提供了一种欠驱动自主水下航行器的航向修正路径跟踪控制方法。该方法结合水下航行器的航向修正方法和运动控制技术，修正理想航向并设计路径跟踪运动控制律，实现了水下航行器的路径跟踪控制。

本发明的技术方案是：一种欠驱动自主水下航行器的航向修正路径跟踪法方法，其特征在于，包括以下步骤：

步骤1：对理想航向进行修正，包括以下子步骤：

步骤1.1：计算欠驱动自主水下航行器的侧滑角：β＝v_y/v_x，其中v_x是水下航行器质心的纵向速度，v_y是水下航行器质心的侧向速度；

步骤1.2：利用侧滑角对参考路径的切线方向进行修正，得到修正后的理想航向；ψ_d＝ψ-β，其中ψ是已知的参考路径切线方向；

步骤2：构造水下航行器相对参考路径的跟踪误差，根据跟踪误差求解理想航向角速度，包括以下子步骤：

步骤2.1：定义水下航行器的实际航向角为ψ_h，通过实际航向角与理想航向角的差值求解航向角偏差：ψ_e＝ψ_h-ψ_d，同时定义水下航行器质心到参考路径的侧向偏差为y_e；

步骤2.2：计算理想航向角速度：ω_d＝v_x/r-k₂(ψ_e+k₁y_e)，其中，r为参考路径的曲率半径，v_x为纵向速度，k₁和k₂为正数参数，且满足k₂＞k₁v_x；

步骤3：建立水下航行器的动力学模型：定义水下航行器的运动状态为x，则x＝[v_yω]^T，其中v_y为实际侧向速度和ω为实际航向角速度。水下航行器的状态空间动力学模型可表述为

其中A是水下航行器的系统矩阵；u是水下航行器的的控制输入，即舵角产生的力矩，B是水下航行器的控制输入矩阵；。系统矩阵A和控制矩阵B可以表述为

其中，m₁₁，m₂₂，m₃₃是包含附加质量的惯性系数，d₂₂和d₃₃是水动力阻尼系数，v_x是水下航行器的纵向速度。

步骤4：设计欠驱动自主水下航行器的运动控制律：基于水下航行器动力学模型，建立Lyapunov方程(A+BK)^TP+P(A+BK)＝-W，其中P和W是使用者设定的正定矩阵，使矩阵P和W的所有特征值均为正数，K是系统的反馈控制律；

步骤5：利用步骤4建立的运动控制律求解水下航行器的反馈控制输入：u＝Kx，其中水下航行器的运动状态x由多普勒测速仪获得，利用反馈控制输入公式可求得舵角产生的力矩，作为水下航行器的控制输入，使水下航行器的实际航向角速度ω收敛于理想航向角速度ω_d，实现水下航行器的路径跟踪控制。

本发明进一步的技术方案是：所述步骤4中的反馈控制律K由Lyapunov方程求解获得。

本发明进一步的技术方案是：所述反馈控制律K使(A+BK)为负定矩阵。

发明效果

本发明的技术效果在于：该方法综合了航向修正、欠驱动自主水下航行器建模、路径跟踪控制的优势，其算法流程如图1所示。相较于传统与之，有如下显著优势：(1)在航向修正方面，根据权力要求书的步骤1，对参考轨迹的切线方向进行补偿，修正理想航向，避免路径跟踪的误差。(2)考虑水下航行器的欠驱动特性，将路径跟踪转化为跟踪理想航向角速度，设计运动控制律并实现路径跟踪。(3)整体上，该方法综合了航向修正、欠驱动自主水下航行器建模、路径跟踪控制的优势优点，修正了水下航行器航向，根据权力要求书的步骤5，实现欠驱动自主水下航行器的运动控制，使路径跟踪更加准确。

附图说明

图1欠驱动自主水下航行器航向修正路径跟踪控制流程图

图2水下航行器路径跟踪示意图

图3水下航行器的航行轨迹和参考路径

图4水下航行器的轨迹跟踪误差

图中，相比于未修正航向的跟踪误差，进行航向修正后，水下航行器的路径跟踪误差减小

图5水下航行器的修正航向角和实际航向角

图中，水下航行器的实际航向能够收敛于参考航向

图6水下航行器的航向角误差

图中，进行航向修正后，水下航行器的航向误差可快速收敛于零，且航向误差小于未修正时的航向误差。

具体实施方式

在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”、“顺时针”、“逆时针”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

参见图1—图6，本发明提出了一种欠驱动自主水下航行器的航向修正路径跟踪法方法，结合水下航行器的航向修正方法和运动控制技术，修正理想航向并设计路径跟踪运动控制律，实现了水下航行器的路径跟踪控制，本发明采用的技术方案是：首先依据参考路径生成理想航向，不同于传统的理想航向角生成方法，该方法利用侧滑角修正参考轨迹的切线方向，获取理想航向角以消除稳态误差。得到修正后的理想航向后，建立水下航行器的动力学模型，明确控制输入和状态输出。然后设计水下航行器的运动控制律，通过状态反馈实现路径跟踪控制。最后，结合所设计的控制律和测量的系统状态反馈求解控制输入量，实现水下航行器的路径跟踪控制。航向修正是为了确定水下航行器的理想航向角，以便设计水下航行器的运动控制，实现路径跟踪的同时保持理想航向。

一种欠驱动自主水下航行器的航向修正路径跟踪法方法，其特征在于修正水下航行器的理想航向，实现准确路径跟踪控制。包括以下步骤：

(1)依据参考路径修正理想航向。由于侧滑角的影响，水下航行器的重心速度方向和航向存在偏差。因此，求解参考路径的切线方向以及水下航行器的侧滑角，利用水下航行器的侧滑角对切线方向进行修正，得到理想航向。航向修正可表示为ψ_d＝ψ-β，其中ψ是参考路径的切线方向，β是水下航行器的侧滑角，ψ_d是修正后的理想航向。

(2)构造跟踪误差并求解理想航向角速度。定义水下航行器的实际航向角为ψ_h，通过实际航向角与理想航向角的差值求解航向角偏差ψ_e，可表述为ψ_e＝ψ_h-ψ_d，同时定义水下航行器质心到参考路径的侧向偏差为y_e；将路径跟踪转化为对理想航向角速度的跟踪，以便设计欠驱动自主水下航行器的运动控制律。

(3)建立水下航行器的动力学模型，定义水下航行器的运动状态为x，则x＝[v_yω]^T，其中v_y为侧向速度和ω为航向角速度。水下航行器的状态空间动力学模型可表述为

其中A是系统矩阵；u是系统的控制输入，B是控制输入矩阵。

(4)设计欠驱动自主水下航行器的运动控制律，基于水下航行器动力学模型，建立Lyapunov方程(A+BK)^TP+P(A+BK)＝-W，其中P和W是正定矩阵，K是系统的控制律。设计系统的控制律K，使(A+BK)为负定矩阵，以保证系统稳定性和控制效果。

(5)利用所设计的运动控制律求解水下航行器的反馈控制输入，结合水下航行器的状态反馈信号求解控制输入量，将控制输入量作用于水下航行器实现路径跟踪控制。

2.根据权力要求1所述的欠驱动自主水下航行器的航向修正路径跟踪法方法，其特征在于依据参考路径的水下航行器航向修正方法，步骤(1)中ψ_d是修正后的理想航向。通过水下航行器质心的纵向速度和侧向速度求解侧滑角，则侧滑角可表述为β＝v_y/v_x，其中v_x是水下航行器质心的纵向速度，v_y是水下航行器质心的侧向速度。

3.根据权力要求1所述的欠驱动自主水下航行器的航向修正路径跟踪法方法，其特征在于将路径跟踪转化为航向角速度跟踪。理想航向角速度可计算为ω_d＝v_x/r-k₂(ψ_e+k₁y_e)。

其中，r为参考路径的曲率半径，v_x为纵向速度，k₁和k₂为正数参数，且满足k₂＞k₁v_x。

4.根据权力要求1所述的欠驱动自主水下航行器的航向修正路径跟踪控制方法，其特征为欠驱动自主水下航行器的运动控制方法，步骤(5)中，水下航行器的控制输入可表述为u＝Kx，其中反馈控制律K由Lyapunov方程求解获得，系统状态x由多普勒测速仪获得。

本发明所述的欠驱动自主水下航行器的航向修正路径跟踪控制方法，包括以下步骤：

(1)依据参考路径修正理想航向。获取水下航行器的参考路径，并求解参考路径的切线方向。由于侧滑角的影响，水下航行器的重心速度方向和航向存在偏差。因此，利用水下航行器的侧滑角对切线方向进行修正，得到理想航向。航向修正可表示为ψ_d＝ψ-β，其中ψ是参考路径的切线方向，β是水下航行器的侧滑角，ψ_d是修正后的理想航向。

(2)构造跟踪误差并求解理想航向角速度。定义水下航行器的实际航向角为ψ_h，通过实际航向角与理想航向角的差值求解航向角偏差ψ_e，可表述为ψ_e＝ψ_h-ψ_d，同时定义水下航行器质心到参考路径的侧向偏差为y_e；将路径跟踪转化为对理想航向角速度的跟踪，以便设计欠驱动自主水下航行器的运动控制律。

(3)建立水下航行器的动力学模型，定义水下航行器的运动状态为x，则x＝[v_yω]^T，其中v_y为侧向速度和ω为航向角速度。水下航行器的状态空间动力学模型可表述为

其中A是系统矩阵；u是系统的控制输入，B是控制输入矩阵。

(4)设计欠驱动自主水下航行器的运动控制律，基于水下航行器动力学模型，建立Lyapunov方程(A+BK)^TP+P(A+BK)＝-W，其中P和W是正定矩阵，K是系统的控制律。设计系统的控制律K，使(A+BK)为负定矩阵，以保证系统稳定性和控制效果。

(5)利用所设计的运动控制律求解水下航行器的反馈控制输入，结合水下航行器的状态反馈信号求解控制输入量，将控制输入量作用于水下航行器实现路径跟踪控制。

步骤(1)中，ψ为已知的参考路径切线方向，侧滑角β通过测量水下航行器质心的纵向速度v_x和侧向速度v_y获得，可表述为β＝v_y/v_x

步骤(2)种，理想航向角速度可计算为ω_d＝v_x/r-k₂(ψe+k₁y_e)，其中，r为参考路径的曲率半径，v_x为纵向速度，k₁和k₂为正数参数，且满足k₂＞k₁v_x。

步骤(5)中，水下航行器的控制输入可表述为u＝Kx，其中反馈控制律K由Lyapunov方程求解获得，系统状态x由多普勒测速仪获得。

下面结合仿真实例和附图对本发明的内容做进一步详细描述：

为验证所发明欠驱动自主水下航行器航向修正路径跟踪方法的有效性，针对路径跟踪控制进行仿真验证：按照发明内容中步骤3所述，首先给出自主水下航行器的特征参数，m₁₁＝32k_g；m₂₂＝66kg，m₃₃＝66.5kg，d₂₂＝-262kg/s，d₃₃＝-188kg·m²/s.自主水下航行器的初始运动状态为v_x(0)＝2m/s，v_y(0)＝0m/s，ω(0)＝0rad/s，u(0)＝0N·m，矩阵P＝[31.5；1.2 3.5]；W＝[2 0；0 2]；其余运动参数均为零。

步骤1中根据给定的参考路径修正理想航向，具体过程包括计算航行器的侧滑角和参考路径的切线方向。通过路径规划获取水下航行器的参考路径，计算参考路径的切线方向作为水下航行器的参考方向。由于侧滑角的影响，水下航行器的重心速度方向和航向存在偏差。需要利用水下航行器的侧滑角对切线方向进行修正，得到理想航向。因此，修正后的水下航行器理想航向可表示为

ψ_d＝ψ-β

其中ψ是已知参考路径的切线方向，ψ_d是修正后的理想航向，β是水下航行器的侧滑角。侧滑角用于修正水下航行器的理想航向，侧滑角的计算公式可表示为

β＝v_y/v_x

侧滑角β通过测量水下航行器质心的纵向速度v_x和侧向速度v_y获得。

步骤2中首先构造水下航行器的路径跟踪误差，然后求解理想航向角速度，以实现路径跟踪控制。图2说明了路径跟踪误差和航行方向的关系。测量水下航行器的实际航向角ψ_h，通过实际航向角与理想航向角的差值求解航向角偏差ψ_e，可表述为

ψ_e＝ψ_h-ψ_d

为了实现路径跟踪，计算水下航行器质心到参考路径的侧向偏差y_e；考虑到水下航行器的欠驱动特性，将路径跟踪转化为对理想航向角速度的跟踪，以便设计欠驱动自主水下航行器的运动控制律。通过侧向偏差y_e和航向角偏差ψ_e，跟踪理想路径所需的理想航向角速度可计算为

ω_d＝v_x/r-k₂(ψ_e+k₁y_e)

其中r为参考路径的曲率半径，v_x为纵向速度，k₁和k₂为正数参数，且满足关系式k₂＞k₁v_x。

步骤3中建立了水下航行器的动力学模型，图2说明了水下航行器的动力学特征参数。将水下航行器的运动状态定义为x，则x＝[v_y ω]^T，其中v_y为水下航行器的侧向速度和ω为水下航行器的航向角速度。水下航行器的状态空间动力学模型可表述为

其中A是已知系统矩阵，B是已知控制输入矩阵，由水下航行器参数计算所得；u是系统的控制输入，由所提出的控制算法计算得到。

步骤4中设计了欠驱动自主水下航行器的运动控制律，基于水下航行器动力学模型，水下航行器运动控制的Lyapunov方程可表述为

(A+BK)^TP+P(A+BK)＝-W

其中P和W是正定矩阵，由算法使用者设定；K是系统的控制律，通过求解Lyapunov方程得到。设计系统的控制律K，使闭环控制矩阵(A+BK)为负定矩阵，以保证系统稳定性和控制效果。

步骤5中利用所设计的运动控制律求解水下航行器的反馈控制输入，结合水下航行器的状态反馈信号求解控制输入量，将控制输入量作用于水下航行器实现路径跟踪控制。水下航行器的控制输入可表述为

u＝Kx

其中反馈控制律K由Lyapunov方程求解获得，水下航行器的状态x数值可由多普勒测速仪获得。

利用所求解的控制输入对水下航行器进行控制，实现水下航行器的路径跟踪控制。路径跟踪控制效果如图3-图6所示，利用所提出的航向修正路径跟踪方法可使水下航行器跟踪参考路径。所提出的方法通过航向修正提高路径跟踪精度，减小路径跟踪误差。相比于未修正航向的路径跟踪控制方法，所提出的方法能有效减少路径跟踪误差和航向角跟踪误差。

Claims (3)

1.一种欠驱动自主水下航行器的航向修正路径跟踪法方法，其特征在于，包括以下步骤：

步骤1：对理想航向进行修正，包括以下子步骤：

步骤1.1：计算欠驱动自主水下航行器的侧滑角：β＝v_y/v_x，其中v_x是水下航行器质心的纵向速度，v_y是水下航行器质心的侧向速度；

步骤1.2：利用侧滑角对参考路径的切线方向进行修正，得到修正后的理想航向；ψ_d＝ψ-β，其中ψ是已知的参考路径切线方向；

步骤2：构造水下航行器相对参考路径的跟踪误差，根据跟踪误差求解理想航向角速度，包括以下子步骤：

步骤2.1：定义水下航行器的实际航向角为ψ_h，通过实际航向角与理想航向角的差值求解航向角偏差：ψ_e-ψ_h-ψ_d，同时定义水下航行器质心到参考路径的侧向偏差为y_e；

步骤2.2：计算理想航向角速度：ω_d＝v_x/r-k₂(ψ_e+k₁y_e)，其中，r为参考路径的曲率半径，v_x为纵向速度，k₁和k₂为正数参数，且满足k₂＞k₁v_x；

步骤3：建立水下航行器的动力学模型：定义水下航行器的运动状态为x，则x＝[v_y ω]^T，其中v_y为实际侧向速度和ω为实际航向角速度。水下航行器的状态空间动力学模型可表述为

其中A是水下航行器的系统矩阵；u是水下航行器的的控制输入，即舵角产生的力矩，B是水下航行器的控制输入矩阵；。系统矩阵A和控制矩阵B可以表述为

其中，m₁₁，m₂₂，m₃₃是包含附加质量的惯性系数，d₂₂和d₃₃是水动力阻尼系数，v_x是水下航行器的纵向速度。

步骤4：设计欠驱动自主水下航行器的运动控制律：基于水下航行器动力学模型，建立Lyapunov方程(A+BK)^TP+P(A+BK)＝-W，其中P和W是使用者设定的正定矩阵，使矩阵P和W的所有特征值均为正数，K是系统的反馈控制律；

步骤5：利用步骤4建立的运动控制律求解水下航行器的反馈控制输入：u＝Kx，其中水下航行器的运动状态x由多普勒测速仪获得，利用反馈控制输入公式可求得舵角产生的力矩，作为水下航行器的控制输入，使水下航行器的实际航向角速度ω收敛于理想航向角速度ω_d，实现水下航行器的路径跟踪控制。

2.如权利要求1所述的一种欠驱动自主水下航行器的航向修正路径跟踪法方法，其特征在于，所述步骤4中的反馈控制律K由Lyapunov方程求解获得。

3.如权利要求2所述的一种欠驱动自主水下航行器的航向修正路径跟踪法方法，其特征在于，所述反馈控制律K使(A+BK)为负定矩阵。

Images