CN112034865A - 基于优化算法的全驱动水下航行器航迹跟踪控制方法 - Google Patents

Abstract

本发明涉及一种基于优化算法的全驱动水下航行器航迹跟踪控制方法，属于水下航行器航迹跟踪控制技术领域。包括：获得航行器的运动参数，设计其运动姿态约束向量和速度约束向量；然后计算跟踪误差，并根据航行器的六自由度模型得到运动学控制器和动力学控制器；最后由运动学控制器得到期望速度，由动力学控制器推导出个自由度上的力和力矩；直至航迹跟踪完成。解决全驱动航行器在跟踪三维曲线中的冗余问题，同时考虑了航行器在运动过程中的姿态和速度约束问题。由姿态与速度约束构造优化问题为凸优化问题，有多种可靠方法求解，且求解速度可满足实时性的要求；充分考虑了全驱动航行器的运行中的速度饱和问题，实用性强。

Description

基于优化算法的全驱动水下航行器航迹跟踪控制方法

技术领域

本发明涉及水下航行器航迹跟踪控制技术领域，具体地说，涉及一种基于优化算法的全驱动水下航行器航迹跟踪控制方法。

背景技术

水下航行器是海洋工程，如水下管道检查，三维地形测绘，高价值目标搜索中的重要工具，而三维航迹跟踪是水下航行器执行这些任务的基础问题。目前有多种算法已经成功应用于水下航行器的航迹跟踪，例如自适应控制、滑模控制、神经网络控制、模糊控制等，根据适用对象不同可分为欠驱动航行器和全驱动航行器控制算法。

就三维航迹跟踪问题来说，欠驱动航行器，如常见的鱼雷型航行器，可提供纵荡，纵摇和横摇方向上的控制，且这些方向上的控制必须被全部利用以跟踪三维航迹。

而全驱动航行器的优势在于六自由度运动均可控制，因此若给出一条空间三维航迹，且不考虑运行中的姿态，全驱动航行器的控制是存在冗余的。然而，航行器的运行姿态在进行某些任务时至关重要：在执行回坞任务时，错误的姿态会导致航行器无法与泊坞接驳；对于地形测量任务，航行器本身的俯仰或者横滚会影响测量精度。

公布号为CN105527967A的发明专利文献公开了一种AUV反演对接控制方法，AUV的头部安装有USBL收发器，回收站的两侧安装两个USBL应答器B1和B2；在AUV靠近对接站的过程中，AUV通过USBL定位系统获取应答器在体坐标系中的位置,AUV应答器位置测量方程式对时间求导，在AUV体坐标系中定义对接误差。对接过程中AUV处于全驱动航行模式，针对对接误差，设计非线性反演控制器，使AUV沿着期望的直线航迹航行直至抵达对接站。

另有公布号为CN109634307A的发明专利文献公开了一种无人水下航行器复合航迹跟踪控制方法，包括以下步骤：S1：结合运动学模型和动力学模型构建大地坐标系下的无人水下航行器动态模型；S2：在无人水下航行器动态模型的基础上进行变量变换：S3：根据所设计的水下航行器控制器定义Lyapunov函数，采用矩阵Young不等式求出Lyapunov函数关于时间的导数，证明该复合航迹跟踪控制方法有限时间内趋于稳定、航迹跟踪误差有限时间内趋近于零；S4：模拟海洋环境下的风、浪、流扰动环境，设置无人水下航行器期望航迹，设计仿真实验证明所提出控制方法的有效性和优越性。

然而，现有技术没有关注到在航迹跟踪问题中的约束，由于航行器在各自由度上能达到的速度受限于航行器的推力配置与水动力性能，使得约束成为航迹跟踪问题中不容忽视的对象。

发明内容

本发明的目的是提供一种基于优化算法的全驱动水下航行器航迹跟踪控制方法，解决全驱动航行器在跟踪三维曲线中的冗余问题，同时考虑了航行器在运动过程中的姿态和速度约束问题。

为了实现上述目的，本发明提供的基于优化算法的全驱动水下航行器航迹跟踪控制方法，包括：获得航行器的运动参数，设计其运动姿态约束向量和速度约束向量；然后计算跟踪误差，并根据航行器的六自由度模型得到运动学控制器和动力学控制器；最后由运动学控制器得到期望速度，由动力学控制器推导出个自由度上的力和力矩；直至航迹跟踪完成。

具体的，包括以下步骤：

1)采集数据，获得航行器当前在世界坐标系下的坐标η₁＝[x,y,z]^T，姿态η₂＝[φ,θ,ψ]^T，以及体坐标系下的速度

其中线速度v₁＝[u,v,w]^T，角速度v₂＝[p,q,r]^T；

2)设航行器的运动姿态约束向量为η_2h＝[φ_max,θ_max,ψ_max]^T和η_2l＝[φ_min,θ_min,ψ_min]^T，其元素分别代表横滚、俯仰和偏航角允许的最大值和最小值；速度约束向量为v_h＝[u_max,v_max,w_max,p_max,q_max,r_max]^T和v_l＝[u_min,v_min,w_min,p_min,q_min,r_min]^T，其元素表示各自由度上的线速度和角速度限制；

3)根据航行器当前坐标与期望的三维航迹计算跟踪误差e；

4)根据给定的全驱动航行器六自由度模型得到运动学控制器，求得期望速度v_d；

5)根据给定的全驱动航行器六自由度模型得到动力学控制器，推导出各自由度上的力和力矩；

6)判断航迹是否跟踪完成，若期望航迹中规划的时间已到，则认为跟踪完成，否则返回步骤2)。

上述技术方案中，通过建立航行器六自由度运动学和动力学模型以反步控制方法构造反馈控制器，选择速度约束构造优化问题的可行域，利用姿态角度构造加权函数，导出运动学优化问题。求解此优化问题得到虚拟控制，即期望速度；再根据反步控制方法和动力学模型得到动力学反馈控制律，使实际速度收敛到期望速度，并在建立的航行器模型上进行模拟。结果表明，针对同一航行器跟踪同一航迹并设置不同的速度和姿态约束，航行器每次运行模式不同，且都满足设置的约束。同时，航行器的空间位置也收敛到所在航迹上。

可选地，步骤3)中，跟踪误差e的计算公式如下：

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

其中，η_1d是期望的三维航迹，

可选地，步骤4)中，利用所述运动姿态约束向量得到优化权重，利用所述速度约束向量得到优化可行域，求解凸优化问题得到所述的运动学控制器，即期望速度v_d，使跟踪误差e收敛。

可选地，步骤4)中所述的凸优化问题为：

其中，

I_3×3是单位矩阵，

K₁是反馈增益矩阵，W是权重矩阵，ξ是优化器得出的结果，通过Q矩阵可得到期望速度v_d，有v_d＝Q^-1ξ，v_l，v_h分别表示步骤2)中所述的速度约束的下界和上界。

可选地，凸优化问题权重矩阵W表示为：

其中，n＝3，权重w_i为：

η_2i表示航行器姿态向量η₂的第i项，H(η₂)是关于航行器姿态的指标函数。

所述的凸优化问题权重矩阵中的指标函数H(η₂)定义为：

η_2h，η_2l是运动姿态约束向量，分别表示约束上界和下界。

可选地，步骤5)中基于反步法设计各自由度上的力和力矩，包括以下步骤：

5-1)选择李雅普诺夫函数：

其中，v_e是实际速度v和步骤4)中得到的期望速度v_d的差；

5-2)设计的力与力矩向量τ为

其中，σ＝[e₁,e₂,e₃,0,0,0]^T，K_v是设计的反馈增益，M，C(v)，D(v)，g(η)由全驱动航行器的模型导出，其中M是6×6的惯性矩阵，由航行器自身质量引发的惯性矩阵和航行器周围水体引发的虚拟惯性矩阵组成；C(v)是6×6的科里奥利和离心力矩阵；D(v)是6×6的阻尼矩阵，表示航行器收到的各种阻力，g(η)是6×1的恢复力向量。

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

本发明提出的控制器具有两层结构，基于优化算法的运动学控制器保证了在速度误差收敛的情况下，水下航行器的航迹收敛到期望航迹上，而动力学学控制器则可以保证速度误差收敛。相较于传统的反步法控制器设计方法，基于优化的控制器考虑了航行器本身的速度约束，符合航行器应用实际；同时在控制器设计中考虑姿态约束，一方面使航行器在进行三维航迹跟踪时满足一系列任务的姿态要求，另一方面避免了传统航行器控制器设计中俯仰角在±π/2的奇异问题；

由姿态与速度约束构造优化问题为凸优化问题，有多种可靠方法求解，且求解速度可满足实时性的要求；充分考虑了全驱动航行器的运行中的速度饱和问题，实用性强。

附图说明

图1为本发明实施例中航行器的位置与姿态示意图，其中(a)表示任务a，(b)表示任务b，(c)表示任务c；

图2为任务b中航行器的横滚、俯仰和航向角度；

图3为任务b中航行器的横荡、纵荡和垂荡速度；

图4为任务c中航行器的横滚、俯仰和航向角度；

图5为任务c中航行器的横荡、纵荡和垂荡速度。

具体实施方式

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

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

实施例

本实施例的基于优化算法的全驱动水下航行器航迹跟踪控制方法包括以下步骤：

S100，传感器数据采集，利用航行器搭载的惯性导航系统、水声定位系统获得航行器当前在世界坐标系下的坐标、姿态和体坐标系下的速度。

S200，设计航行器运动姿态约束向量，其元素分别代表横滚，俯仰和偏航角允许的最大和最小角度；速度约束向量，其元素表示各自由度上的线速度和角速度限制。

S300，根据步骤S100中的航行器当前坐标与期望的三维航迹计算跟踪误差。

S400，利用步骤S200的姿态约束向量设计优化权重，速度约束向量设计优化可行域，求解凸优化问题得到虚拟控制器，即期望速度，使航迹跟踪误差收敛；期望速度由求解优化问题获得。

S500，根据给定的全驱动航行器六自由度模型设计动力学控制器，即推导出各自由度上的力和力矩。

S600，判断设计航迹是否跟踪完成，若期望航迹中规划的时间已到，则认为跟踪完成，否则返回步骤S200。

为说明控制器设计方法并验证方法的有效性，先给出具有一般性的六自由度航行器运动学模型与动力学模型分别为：

其中，

τ分别表示航行器的位置角度、速度角速度和控制量输入，M，C(v)，D(v)，g(η)是模型参数，J(η)＝diag(R,T)是变换矩阵，用以描述从体坐标系到世界坐标系的变换关系。

按照步骤S200给出姿态和速度约束：航行器运动姿态约束向量η_2h，η_2l，其元素分别代表横滚，俯仰和偏航角允许的最大和最小角度；速度约束向量v_h，v_l，其元素表示各自由度上的线速度和角速度限制；

按照步骤S300定义位置跟踪误差e＝R^T(η₁-η_1d)，η_1d是期望的三维航迹，那么对其求导得到误差动态：

其中S(v₂)是关于v₂中元素的反对称矩阵。

按照步骤S400构造优化问题：

其中，

I_3×3是单位矩阵，

K₁是反馈增益矩阵，W是权重矩阵，

其中，n＝3，权重w_i设计为：

指标函数H(η₂)定义为：

使用内点法解上述优化问题，得到期望速度v_d。

按照步骤S500设计动力学控制器，即：

其中，σ＝[e₁,e₂,e₃,0,0,0]^T，K₂是设计的反馈增益矩阵。

下面对其进行稳定性证明：

选取李雅普诺夫函数

由凸优化问题(3)中的等式约束有：

将航行器动力学模型(1)和动力学控制器(7)带入式(8)有：

因此，若K₁，K₂设计为正定矩阵

即航迹跟踪误差和速度跟踪误差收敛。

下面说明使用上述控制器的系统满足步骤(2)设计的约束：速度约束v_h，v_l已在优化问题(3)考虑；由式(6)可知，当航行器的姿态接近约束时，

会趋向于无穷，那么对应的旋转自由度权重也将趋向于无穷。由此，运动学控制器给出的期望速度将趋向于0，这避免了航行器旋转角度的继续变化。

下面给出航行器利用本实施例的控制方法在三个具有不同约束的任务中跟踪同一航迹的仿真结果。

设计的航迹为

其中，α＝10，β＝0.002π，t是运行时间，终止时间设为2000秒。三种任务描述如下：

任务a、不考虑航行器的姿态约束，航行器以最小速度代价v^Tv跟踪设计航迹；

任务b、考虑航行器的姿态约束|φ,θ,ψ|≤0.001，不考虑速度约束，仿真航行器在某些姿态敏感任务下的航迹跟踪情况；

任务c、既考虑航行器的姿态约束|φ|≤0.001，

又考虑速度约束，限制航行器横荡和纵荡速度|v,w|≤0.001，仿真鱼雷形航行器的航迹跟踪情况。

仿真结果参见图1，椭球体和三角形部分表示航行器，实线表示期望的轨迹，由图1可知，三种情况下，航行器的位置均跟踪了给定的期望轨迹，任务a航行器是自由姿态，以最小速度代价跟踪，任务b航行器的姿态由于约束基本保持不变，任务c由于侧向和垂直运动速度受到约束，航行器需要不断调整姿态以跟踪给定的轨迹。

图2中上下两条粗黑线表示任务描述中的约束上界和约束下界，由图2可知，航行器的横滚，俯仰和航向角度均被约束在-0.001到0.001之间，其中可以看到角度有一些抖动，这是由于在仿真中添加了外部扰动。

由于航行器的角度都被约束在了很小的范围内，所以可以认为航行器的体坐标系和世界坐标系的三轴方向平行，那么，航行器体坐标系x，y，z轴的速度应该和世界坐标系下期望的三轴速度

一致。由图3可知，航行器的横荡、纵荡和垂荡速度都为正弦，且幅值，相位，频率都与世界坐标系下的期望速度一致。

参见图4，由于航行器在垂直运动和横向运动的速度受限，航行器必须采用类似鱼雷的欠驱动运动模态，即航行器的航向角和俯仰角需要不停调整，以便航行器跟踪给定的轨迹；在此任务中，航行器的横滚角被约束，由图4可见其角度值并未超过约束的上界和下界。

由图5可知，任务c中航行器的横荡和垂荡速度未超过任务要求的约束，纵荡速度没有做约束。

由三种任务的仿真结果可见本发明所提出的控制方法使航行器在进行三维航迹跟踪时可以满足一系列任务的姿态和速度要求。

Claims (8)

1.一种基于优化算法的全驱动水下航行器航迹跟踪控制方法，其特征在于，包括：

获得航行器的运动参数，设计其运动姿态约束向量和速度约束向量；然后计算跟踪误差，并根据航行器的六自由度模型得到运动学控制器和动力学控制器；最后由运动学控制器得到期望速度，由动力学控制器推导出个自由度上的力和力矩；直至航迹跟踪完成。

2.根据权利要求1所述的基于优化算法的全驱动水下航行器航迹跟踪控制方法，其特征在于，包括以下步骤：

1)采集数据，获得航行器当前在世界坐标系下的坐标η₁＝[x，y，z]^T，姿态η₂＝[φ，θ，ψ]^T，以及体坐标系下的速度

其中线速度v₁＝[u，v，w]^T，角速度v₂＝[p，q，r]^T；

2)设航行器的运动姿态约束向量为η_2h＝[φ_max，θ_max，ψ_max]^T和η_2l＝[φ_min，θ_min，ψ_min]^T，其元素分别代表横滚、俯仰和偏航角允许的最大值和最小值；速度约束向量为v_h＝[u_max，v_max，w_max，p_max，q_max，r_max]^T和v_l＝[u_min，v_min，w_min，p_min，q_min，r_min]^T，其元素表示各自由度上的线速度和角速度限制；

3)根据航行器当前坐标与期望的三维航迹计算跟踪误差e；

4)根据给定的全驱动航行器六自由度模型得到运动学控制器，求得期望速度v_d；

5)根据给定的全驱动航行器六自由度模型得到动力学控制器，推导出各自由度上的力和力矩；

6)判断航迹是否跟踪完成，若期望航迹中规划的时间已到，则认为跟踪完成，否则返回步骤2)。

3.根据权利要求2所述的基于优化算法的全驱动水下航行器航迹跟踪控制方法，其特征在于，步骤3)中，跟踪误差e的计算公式如下：

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

其中，η_1d是期望的三维航迹，

4.根据权利要求3所述的基于优化算法的全驱动水下航行器航迹跟踪控制方法，其特征在于，步骤4)中，利用所述运动姿态约束向量得到优化权重，利用所述速度约束向量得到优化可行域，求解凸优化问题得到所述的运动学控制器，即期望速度v_d，使跟踪误差e收敛。

5.根据权利要求4所述的基于优化算法的全驱动水下航行器航迹跟踪控制方法，其特征在于，步骤4)中所述的凸优化问题为：

s.t.χ＝PQ^-1ξ

v_l≤Q^-1ξ≤v_h

其中，

I_3×3是单位矩阵，

K₁是反馈增益矩阵，W是权重矩阵，ξ是优化器得出的结果，通过Q矩阵可得到期望速度v_d，有v_d＝Q^-1ξ，v_l，v_h分别表示步骤2)中所述的速度约束的下界和上界。

6.根据权利要求5所述的基于优化算法的全驱动水下航行器航迹跟踪控制方法，其特征在于，所述的凸优化问题权重矩阵W表示为：

其中，n＝3，权重w_i为：

η_2i表示航行器姿态向量η₂的第i项，H(η₂)是关于航行器姿态的指标函数。

7.根据权利要求6所述的基于优化算法的全驱动水下航行器航迹跟踪控制方法，其特征在于，所述的凸优化问题权重矩阵中的指标函数H(η₂)定义为：

η_2h，η_2l是运动姿态约束向量，分别表示约束上界和下界。

8.根据权利要求2所述的基于优化算法的全驱动水下航行器航迹跟踪控制方法，其特征在于，步骤5)中基于反步法设计各自由度上的力和力矩，包括以下步骤：

5-1)选择李雅普诺夫函数：

其中，v_e是实际速度v和步骤4)中得到的期望速度v_d的差；

5-2)设计的力与力矩向量τ为

其中，σ＝[e₁，e₂，e₃，0，0，0]^T，K_v是设计的反馈增益，M，C(v)，D(v)，g(η)由全驱动航行器的模型导出，其中M是惯性矩阵，C(v)是科里奥利和离心力矩阵，D(v)是阻尼矩阵，表示航行器收到的各种阻力，g(η)是恢复力向量。

Images