CN111857165B - 一种水下航行器的轨迹跟踪控制方法 - Google Patents

Abstract

本发明涉及一种水下航行器的轨迹跟踪控制方法，属于水下机器人控制技术领域。包括：1)从导航系统中得到航行器的当前位置和航向角度，并计算航行器当前的位置误差η_1e；2)通过位置误差η_1e和航行角度得到航行器当前的期望速度v_d；3)根据期望速度v_d得到航行器的控制向量τ^*；4)根据控制向量τ^*控制航行器内部驱动机构的运行参数，从而控制航行器的行驶状态。在未知模型参数和外部扰动的前提下，本发明提出了一种由开关函数组成的鲁棒性非平滑控制器，以保证模型在不确定因素与有约束的未知外部扰动下的速度跟踪误差的收敛性。

Description

一种水下航行器的轨迹跟踪控制方法

技术领域

本发明涉及水下机器人控制技术领域，具体地说，涉及一种水下航行器的轨迹跟踪控制方法。

背景技术

深海海底科学的发展高度依赖于深海探测技术和装备，由于深海环境复杂、条件极端，目前主要采用深海作业型自主水下航行器代替或辅助人对深海进行探测、观察和采样。而针对海洋资源探索、海底调查和海洋测绘等人类无法到达现场操作的任务场景，保证AUV水下运动的自主性和可控性是一项最基本且重要的功能要求，是实现各项复杂作业任务的前提。

然而，AUV的许多离岸应用(例如轨迹跟踪控制、目标跟踪控制等)极具挑战性，这种挑战性主要由AUV系统以下三方面的特性导致。第一，AUV作为一种多输入多输出系统，其动力学和运动学模型(以下简称模型)复杂，具有高度非线性、强耦合、存在输入或状态约束和时变等特点；第二，模型参数或水动力环境存在不确定性，导致AUV系统建模较为困难；第三，当前大部分AUV属于欠驱动系统，即自由度大于独立执行器的数量(各独立执行器分别对应一个自由度)。通常，通过数学物理机理推导、数值模拟和实物实验相结合的方法来确定AUV的模型及参数，并合理刻画模型中的不确定部分。

随着AUV应用场景的不断扩展，人们对其运动控制的精度、稳定性都提出更高的要求。有许多任务需要做到位置和姿态方向均调整为设定值，即所谓的姿态敏感型轨迹跟踪(OSTT)任务。例如，对接就是一个典型的OSTT问题。由于光学或电磁传感器通常装于航行器的纵轴，且对接过程通常使航行器受限于管状通道中，因此对接端应该与对接目标严格保持一条直线。由于需要保证正确的位置以实现航行器与对接目标的接近，同时姿态的配合是航行器与对接目标连接的必要条件，所以位置和姿态必须同时考虑。

对于常见的水下航行器，存在不能直接控制横向运动的严重问题。在这种情况下，当航行器在三维空间中追踪轨迹时，通过调整控制器，对纵向的力和浪涌、偏航、俯仰的转矩进行独立的控制，从而使航行器的姿态发生改变。因此，这些航行器不能同时跟踪定向在OSTT任务中给出的姿态和位置。基于此，许多研究者通常关注的是一种非传统的驱动系统，即试图通过增加额外的推进器来提高其在OSTT任务中的机动性，然而并没有解决欠驱动水下航行器的OSTT问题。此外，在水下航行器的高度非线性耦合动力学、水下动力学不确定性和未知的外部时变扰动等方面也存在困难。

发明内容

本发明的目的是提供一种水下航行器的轨迹跟踪控制方法，可以解决三维空间中的一类常见的欠驱动水下航行器的OSTT问题。

为了实现上述目的，本发明提供的水下航行器的轨迹跟踪控制方法，包括以下步骤：

1)从导航系统中得到航行器的当前位置和航向角度，并计算航行器当前的位置误差η_1e；

2)通过位置误差η_1e和航行角度得到航行器当前的期望速度v_d；

3)根据期望速度v_d得到航行器的控制向量τ^*；

4)根据控制向量τ^*控制航行器内部驱动机构的运行参数，从而控制航行器的行驶状态。

步骤1)中通过以下方式计算航行器当前的位值误差η_1e：

1-1)定义以航行器的重心为原点的世界坐标框架{N}＝(x_n，y_n，z_n)和体坐标系{B}＝(x_b，y_b，z_b)，x_b，y_b，z_b分别代表纵向轴、横向轴和法向轴的轴；

1-2)建立航行器在三维空间中运动的运动学和动力学模型分别为：

其中，

η₁＝[x y z]^T表示在世界坐标系下x_n、y_n、z_n三个方向的位移，即航行器的位置，η₂＝[φ θ ψ]^T表示世界坐标系下滚动、俯仰和偏航的角度；

v₁＝[u v w]^T，v₂＝[p q r]^T表示体坐标系下x_b、y_b、z_b三个方向的速度与滚动，俯仰和偏航的角速度，g(η)是恢复力和力矩矢量，M为刚体惯性质量和力矩M_RB与流体力学附加质量和力矩M_A之和，M_RB＝diag(m，m，m，I_x，I_y，I_z)，

C(v)为科里奥利矩阵，D为阻尼矩阵，D＝diag(X_u，Y_v，Z_w，K_p，M_q，N_r)，J(η)为旋转矩阵，

其中R＝R_z，ψR_y，θR_x，φ，

且有：

1-3)定义以航行器的中心为原点的体坐标系，在体坐标系下定义位置误差η_1e：

η_1e＝R^T(η₁-η_1d)

其中，η_1d(t)：[0→∞)，为三维实数向量，表示在世界坐标系下航行器期望的位置。

步骤2)包括：

将位置误差η_1e对时间求导，即

定义速度追踪误差v_e＝v-v_d；以此求得航行器当前的期望速度v_d。

步骤1-3)中，在体坐标系下定义位置误差η_1e时，包括ψ_e＝ψ-ψ_d，ψ_d∈[0，2π)，表示目标偏航角度。

步骤1-3)中还包括：将误差对时间求导得到

其中，

步骤2)还包括：

设ζ＝η_1e-Δ，其中Δ＝[0 0 δ]^T，δ为任意小的标量；

将ζ对时间求导，有：

其中，B＝diag(1，δ，1)；

选择李雅普诺夫函数

令

其中，K₁＝diag(k₁，k₂，k₃)，且k₁，k₂，k₃，k₄均为大于0的标量。

步骤3)包括：

3-1)修正动力学模型为

其中，

v^*＝[u w p r]^T，

M^*＝diag(m₁₁，m₃₃，m₄₄，m₆₆)，

D^*＝-diag(X_u，Z_w，K_p，N_r)，

g^*(η)由g(η)的第1，3，4，6行组成，τ^*＝[τ_u τ_w τ_p τ_r]^T，d^*＝[d_u d_w d_p d_r]^T，

m₁₂＝m₁₁-m₂₂，m₂₃＝m₂₂-m₃₃，m₄₅＝m₄₄-m₄₅，，m₅₆＝m₅₅-m₆₆；

3-2)设李雅普诺夫函数

其中

V₂的导数为：

控制向量τ^*满足：

其中，K₂＝diag(k₅，k₆，k₇，k₈)，k₅，k₆，k₇，k₈均为正的标量，(v，v_d，ζ^*)满足：

其中，

满足：

sign(α)满足sign(α)＝diag(sign(α₁)，sign(α₂)，...，sign(α_n))，sign(·)为开关函数，α表示任意的n×1向量。

步骤4)中，V₂＞0且

即系统存在渐进平衡点

意为追踪误差可以收敛为0，从而实现对航行器位置与姿态的同时控制。

与现有技术相比，本发明的有益之处在于：

通过航行器本身为浪涌、摇摆、滚动和偏航提供独立的力或力矩，本发明所提出的控制器设计遵循标准的反步控制程序。在本发明中，浪涌、摇摆、滚动和偏航的速度被视为运动学阶段的虚拟控制，因此，翻滚推力不仅可以用于深度控制，还可以在滚动角非零时用于侧向移动。同时，在未知模型参数和外部扰动的前提下，本发明提出了一种由开关函数组成的鲁棒性非平滑控制器，以保证模型在不确定因素与有约束的未知外部扰动下的速度跟踪误差的收敛性。

附图说明

图1为本发明实施例的控制系统的结构示意图；

图2为本发明实施例中世界坐标系与体坐标系的符号约定与示意；

图3为本发明实施例中对接任务仿真的3D示意图；

图4为本发明实施例中对接任务仿真中位置跟踪误差(x_e，y_e，z_e)变化示意图；

图5为本发明实施例中对接任务仿真中偏航角跟踪误差(ψ_e)变化示意图；

图6为本发明实施例中对接任务仿真中滚动角(φ)变化示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，以下结合实施例及其附图对本发明作进一步说明。显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于所描述的实施例，本领域普通技术人员在无需创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

除非另外定义，本发明使用的技术术语或者科学术语应当为本发明所属领域内具有一般技能的人士所理解的通常意义。本发明中使用的“包括”或者“包含”等类似的词语意指出现该词前面的元件或者物件涵盖出现在该词后面列举的元件或者物件及其等同，而不排除其他元件或者物件。“连接”或者“相连”等类似的词语并非限定于物理的或者机械的连接，而是可以包括电性的连接，不管是直接的还是间接的。“上”、“下”、“左”、“右”等仅用于表示相对位置关系，当被描述对象的绝对位置改变后，则该相对位置关系也可能相应地改变。

实施例

参见图1，为本实施例的控制系统，本实施例的水下航行器的轨迹跟踪控制方法基于该控制系统实现，首先对航行器的运动学和动力学进行分析。其中，运动学只处理运动的几何方面，而动力学分析引起运动的力。参见图2，为了确定运动方程，首先定义世界坐标框架{N}＝(x_n，y_n，z_n)与原点O_n和体坐标系{B}＝(x_b，y_b，z_b)与原点O_b。原点O_b与重力中心(CG)重合，x_b，y_b，z_b分别代表纵向轴、横向轴和法向轴的轴。水下航行器在三维空间中运动的运动学和动力学模型是

其中，

η₁＝[x y z]^T表示在世界坐标系下x_n，y_n，z_n三个方向分别的位移，η₂＝[φ θ ψ]^T表示世界坐标系下滚动，俯仰和偏航的角度。

v₁＝[u v w]^T，v₂＝[p q r]^T表示体坐标系下x_b，y_b，z_b方向的速度与滚动，俯仰和偏航的角速度。g(η)是恢复力和力矩矢量。此外，M为刚体惯性质量和力矩M_RB与流体力学附加质量和力矩M_A之和，其中有：

M_RB＝diag(m，m，m，I_x，I_y，I_z)

C(v)为科里奥利矩阵，阻尼矩阵D表示为

D＝diag(X_u，Y_v，Z_w，K_p，M_q，N_r)

J(η)为旋转矩阵，表示为：

R＝R_z，ψR_y，θR_x，φ，

其中有：

考虑实际系统的控制，对v，q的控制力或力矩是缺失的，因此控制向量τ为：

τ＝[τ_u 0 τ_w τ_p 0 τ_r]

由风力或水波干扰引起的外部的未知扰动d设为：

d＝[d_u d_v d_w d_p d_q d_r]^T

值得注意的是，d是时变的，且被限制在一定范围内。

为了研究OSTT问题，在体坐标系下定义追踪误差η_1e：

η_1e＝R^T(η₁-η_1d)，ψ_e＝ψ-ψ_d

其中，η_1d(t)：[0→∞)为三维实数向量，ψ_d∈[0，2π)表示目标位置与偏航角度。将误差对时间求导，可以得到：

其中有

同样的，我们定义速度追踪误差v_e＝v-v_d，其中v_d表示期望的速度。

接下来，对控制方法进行介绍。控制方法包含两个部分，在第一部分，运动学控制器给出虚拟控制的速度，以保证位置跟踪误差的指数收敛。在第二部分，通过李雅普诺夫控制函数(CLF)得到一个具有鲁棒性的开关控制器，用以处理模型的不确定性和外部干扰，从而提供期望的控制信号，以使速度跟踪误差实现指数收敛。

在控制器第一部分，设：

ζ＝η_1e-Δ

其中Δ＝[0 0 δ]^T，δ为任意小的标量。将ζ对时间求导，有：

其中，B＝diag(1，δ，1)。

选择李雅普诺夫函数

欲使

需令：

其中，K₁＝diag(k₁，k₂，k₃)，且k₁，k₂，k₃，k₄均为大于0的标量。

在控制器的第二部分，保证速度的追踪误差收敛到0。首先，设新的追踪误差为ζ*＝[ζ₁，ζ₂，ζ₃，ψ_e]^T.在没有v，q的动力的情况下，修正后的动力学模型如下：

其中：

v^*＝[u w p r]^T，M^*＝diag(m₁₁，m₃₃，m₄₄，m₆₆)，D^*＝-diag(X_u，Z_w，K_p，N_r)，g^*(η)由g(η)的第1，3，4，6行组成。τ^*＝[τ_u τ_w τ_p τ_r]^T，d^*＝[d_u d_w d_p d_r]^T，同时，有

m₁₂＝m₁₁-m₂₂，m₂₃＝m₂₂-m₃₃，m₄₅＝m₄₄-m₄₅，m₅₆＝m₅₅-m₆₆满足下述假设。

假设一：在流体力学中，由于周围流体的惯性而产生的附加质量具有差异，且在u上的附加质量应小于在v上的附加质量。这同样适用于与在v和w上的附加质量，即m₁₂＜0，m₂₃＜0。对于φ，θ，ψ的转动惯量，同样地，有m₄₅＜0，m₅₆＞0。

假设二：(1)惯量受如下约束：

(2)衰减系数受如下约束：

(3)外部扰动受如下约束：

在控制器的第二部分，设李雅普诺夫函数

其中

则V₂的导数为：

欲使

控制量τ^*应满足：

其中，K₂＝diag(k₅，k₆，k₇，k₈)，k₅，k₆，k₇，k₈均为正的标量，F(v，v_d，ζ^*)满足：

其中，

λ(v)＝diag(v，-v，-v，-v)，

λ(q)＝diag(q，-q，-q，q)，

P^*＝diag(1，ζ₃+δ，1，cos(φ))，

满足：

sign(α)满足sign(α)＝diag(sign(α₁)，sign(α₂)，...，sign(α_n))，其中sign(·)为开关函数，α表示任意的n×1向量。

可以证明，此时V₂＞0且

即系统存在渐进平衡点

意为追踪误差可以收敛为0，从而实现了对航行器位置与姿态的同时控制。

在公开号为CN105346695A的专利申请所提出的四旋翼式微型水下航行器上实现以上方法。所使用的航行器通过流体力学分析得到的标称参数为：M＝diag(40.76，55.20，79.57，0.21，4.96，2.72)，D＝diag(24.33，77.76，38.12，1.53，2.24，9.64)。在水中受到的重力与浮力分别为392N和400N.重心坐标为(0，0，0)，浮力中心坐标为(0，0，0.5)。仿真中选择的控制器参数为：K₁＝diag(10，10，10)，k₄＝10，K₂＝diag(100，100，100，100)。

航行器停泊的初始点位于世界坐标系的原点，即(x，y，z，φ，θ，ψ)＝(0，0，0，0，0，0)。航行器的初速度与初角速度均设为0，即(u，v，w，p，q，r)＝(0，0，0，0，0，0)。

与通常的对接策略不同，采用本实施例的控制方法，实现了一种不同的对接策略。如图3所示，椭圆形和三角形分别表示水下航行器的主体和尾部，对接目标已经被标出，对接板为与对接目标锥体底部平行的平面，用于提供对接任务的必要信息，例如航行器与对接目标之间的距离。在对接任务中，航行器指向对接板并与其保持垂直，即偏航角ψ保持不变。对接目标位于世界坐标系下的点(15，15，0)，且本次仿真的时间为3分钟。

所设置的轨迹与姿态如下所示：

x_d＝L^-1[u(s)G_x(s)]

y_d＝L^-1[u(s)G_y(s)]

z_d＝0

ψ_d＝0

其中，s为复数频率参数。

仿真结果参见图4～6，可以看出在初始状态下，滚动角为0，随着任务的进行，误差x_e，y_e，ψ_e均收敛为0，同时z_e收敛至δ。在任务过程中，通过滚动角实现航行器将推力分解进而实现侧向移动，当航行器靠近目标时，滚动角收敛至0。

本实施例解决了存在水下动力学不确定性和外部扰动的自主水下航行器的姿态敏感型轨迹跟踪控制器设计问题。位置与姿态控制器根据跟踪误差产生正确的参考速度，速度/角速度鲁棒控制器根据参考速度计算控制力与转矩。在运动学层面，这种控制方法使水下航行器能够通过利用侧倾的角度侧向移动，从而在转向跟踪参考偏航角的同时实现位置跟踪。在动力学层面，这种方法采用鲁棒的非光滑控制器来保证速度跟踪误差的指数收敛。通过对对接任务的仿真，结果说明了该方法的有效性。

Claims (7)

1.一种水下航行器的轨迹跟踪控制方法，其特征在于，包括以下步骤：

1)从导航系统中得到航行器的当前位置和航向角度，并计算航行器当前的位置误差η_1e；

2)通过位置误差η_1e和航行角度得到航行器当前的期望速度v_d；

3)根据期望速度v_d得到航行器的控制向量τ^*；具体包括：

3-1)修正动力学模型为

其中，

v^*＝[u w p r]^T，

M^*＝diag(m₁₁，m₃₃，m₄₄，m₆₆)，

D^*＝-diag(X_u，Z_w，K_p，N_r)，

g(η)是恢复力和力矩矢量，g^*(η)由g(η)的第1，3，4，6行组成，τ^*＝[τ_u τ_w τ_p τ_r]^T，d^*＝[d_u d_w d_p d_r]^T，

m₁₂＝m₁₁-m₂₂，m₂₃＝m₂₂-m₃₃，m₄₅＝m₄₄-m₄₅，m₅₆＝m₅₅-m₆₆；

3-2)设李雅普诺夫函数

其中

V₂的导数为：

控制向量τ^*满足：

其中，K₂＝diag(k₅，k₆，k₇，k₈)，k₅，k₆，k₇，k₈均为正的标量，(v，v_d，ζ^*)满足：

其中，

满足：

sign(α)满足sign(α)＝diag(sign(α₁)，sign(α₂)，...，sign(α_n))，sign(·)为开关函数，α表示任意的n×1向量；

4)根据控制向量τ^*控制航行器内部驱动机构的运行参数，从而控制航行器的行驶状态。

2.根据权利要求1所述的水下航行器的轨迹跟踪控制方法，其特征在于，步骤1)中通过以下方式计算航行器当前的位值误差η_1e：

1-1)定义以航行器的重心为原点的世界坐标框架{N}＝(x_n，y_n，z_n)和体坐标系{B}＝(x_b，y_b，z_b)，x_b，y_b，z_b分别代表纵向轴、横向轴和法向轴的轴；

1-2)建立航行器在三维空间中运动的运动学和动力学模型分别为：

其中，

η₁＝[x y z]^T表示在世界坐标系下x_n、y_n、z_n三个方向的位移，即航行器的位置，η₂＝[φ θ ψ]^T表示世界坐标系下滚动、俯仰和偏航的角度；

v₁＝[u v w]^T，v₂＝[p q r]^T表示体坐标系下x_b、y_b、z_b三个方向的速度与滚动，俯仰和偏航的角速度，g(η)是恢复力和力矩矢量，M为刚体惯性质量和力矩M_RB与流体力学附加质量和力矩M_A之和，M_RB＝diag(m，m，m，I_x，I_y，I_z)，

C(v)为科里奥利矩阵，D为阻尼矩阵，D＝diag(X_u，Y_v，Z_w，K_p，M_q，N_r)，J(η)为旋转矩阵，

其中R＝R_z，ψR_y，θR_x，φ，

且有：

1-3)定义以航行器的中心为原点的体坐标系，在体坐标系下定义位置误差η_1e：

η_1e＝R^T(η₁-η_1d)

其中，η_1d(t)：[0→∞)，表示在世界坐标系下航行器期望的位置。

3.根据权利要求2所述的水下航行器的轨迹跟踪控制方法，其特征在于，步骤2)包括：

将位置误差η_1e对时间求导，即

定义速度追踪误差v_e＝v-v_d；以此求得航行器当前的期望速度v_d。

4.根据权利要求2所述的水下航行器的轨迹跟踪控制方法，其特征在于，步骤1-3)中，在体坐标系下定义位置误差η_1e时，包括ψ_e＝ψ-ψ_d，ψ_d∈[0，2π)，表示目标偏航角度。

5.根据权利要求4所述的水下航行器的轨迹跟踪控制方法，其特征在于，步骤1-3)中还包括：将误差对时间求导得到

其中，

6.根据权利要求3所述的水下航行器的轨迹跟踪控制方法，其特征在于，步骤2)还包括：

设ζ＝η_1e-Δ，其中Δ＝[0 0 δ]^T，δ为任意小的标量；

将ζ对时间求导，有：

其中，B＝diag(1，δ，1)；

选择李雅普诺夫函数

令

其中，K₁＝diag(k₁，k₂，k₃)，且k₁，k₂，k₃，k₄均为大于0的标量。

7.根据权利要求1所述的水下航行器的轨迹跟踪控制方法，其特征在于，步骤4)中，V₂＞0且

即系统存在渐进平衡点

意为追踪误差可以收敛为0，从而实现对航行器位置与姿态的同时控制。

Images