CN109050835B - 全驱动自主水下机器人结构及回收三维路径跟踪方法 - Google Patents

Abstract

本发明公开了一种全驱动自主水下机器人结构及回收三维路径跟踪方法，提供一种三段式回收三维路径跟踪方法，以及一种用于机器人回收直线归位阶段与直线跟踪阶段的改进的非奇异终端模糊滑模控制算法。所采用的三维路径跟踪方法，将回收路径分为三段，将每段曲线转化为空间离散点序列对其跟踪控制。所采用的改进的非奇异终端模糊滑模算法，在滑模控制中，设计分段切换函数，实现多个滑动模态，并通过模糊算法对滑模增益进行实时优化，使其能快速、平稳地追踪目标点，增强回收三维路径跟踪性能，提高回收成功率。

Description

全驱动自主水下机器人结构及回收三维路径跟踪方法

技术领域

本发明涉及自主水下机器人，尤其涉及全驱动自主水下机器人结构及回收三维路径跟踪阶段方法，属于机器人控制技术领域。

背景技术

随着信息技术的不断发展，自主式水下无人航行器(AUV)作为探索海洋空间的有力工具之一，在军事和科学研究方面起着越来越重要的作用。未来的AUV需要更长的水下工作时间、更大的自主性、更隐蔽的情报收集能力、更高速的数据分析速度以及更强大的通信能力。而这些目标的实现主要受到自身携带能源和水下通信两个因素的限制。由于几十年来电池技术一直没有获得突破性进展，AUV的水下工作时间仍然很短，需要频繁的回收和布放。此外水声通信速率较低，AUV难以及时回传所收集到的数据。两者均极大增加了AUV的使用成本，降低了其工作效率。因此，对AUV的回收对接研究显得尤为重要。在回收过程中，AUV路径跟踪控制非常关键，尤其在复杂的海洋环境下，探究有效的回收路径跟踪控制方法，完成回收任务，成为国内外研究的热点，而且在军事、科研等方面具有现实意义。

全驱动自主水下机器人的执行机构一般采用多推进器操纵的设计方式，通过改变多推进器的推力来实现自主水下机器人的纵向运动，转艏运动以及升沉运动。由于全驱动自主水下机器人自身运动具有高度的非线性以及强耦合性，并且在水下航行时容易受到海流以及信号传输干扰等问题，这些因素给运动控制带来很大难度。这些都成为全驱动AUV三维空间跟踪控制器设计的难点。

目前，国内外就三维路径的跟踪问题研究的较少，研究多针对于水平面、垂直面的二维平面控制。本发明首先将三维路径分解为均匀的离散点序列，与传统的航路点跟踪方法相比，本方法对于曲线点的规划及选取更加快捷、平滑。在控制器的选择上，由于全驱动自主水下机器人模型的高度非线性与不确定性的特点，本发明采用改进的非奇异终端模糊滑模控制算法。目前应用于自主水下机器人三维路径跟踪控制多为模糊控制、滑模控制等智能控制算法。模糊控制算法虽然具有鲁棒性强，抗干扰性强等特点，但是模糊控制器参数难以直接确定；滑模控制算法可以不依赖系统模型，但系统本身所带来的抖振问题难以消除，影响了系统的控制精度。申请号为“201210490435.X”的专利文献公开了一种“用于喷药移动机器人的轨迹跟踪滑模控制系统和控制方法”，滑模控制带来的抖振问题没有得到解决，跟踪精度较差；申请号为“201610835447.X”的专利文献公开了“一种基于自适应滑模控制的UUV路径跟踪方法”，其将滑模控制与自适应控制相结合，但控制器仍然存在对机器人惯量及不确定性的依赖。

发明内容

本发明的目的在于提供一种全驱动自主水下机器人结构及回收三维路径跟踪方法，应用于全驱动自主水下机器人回收控制过程，提供全驱动自主水下机器人回收直线归位与直线跟踪阶段的三维路径跟踪控制方法，以及提供一种全驱动自主水下机器人回收直线归位阶段与直线跟踪阶段的改进的非奇异终端模糊滑模控制算法。

本发明的目的通过以下技术方案予以实现：

一种全驱动自主水下机器人结构，包括艏段2，艏部推进段7，电子舱段12，艉部推进段15和主推进段18五大部分；整体采用鱼雷形流线型结构，艏段2与艏部推进段7、艏部推进段7与电子舱段12，电子舱段12与艉部推进段15、艉部推进段15与主推进段18同心密封相连；艏段2包括探测仪1、避碰声纳4和艏段壳体3，探测仪1通过密封安装于艏段2前部，避碰声呐4通过水密接插件安装于艏段2上部，艏段壳体3采用铝合金6061制造并进行表面硬质氧化处理，艏段壳体3采用半球形外形以保证水下机器人具有良好的流线型并兼顾艏部的安装空间；艏部推进段7由艏部侧向推进器5、艏部垂向推进器8和艏部推进段壳体6组成，侧向推进器5水平放置，安装于艏部推进段7前侧方，垂向推进器8垂直放置，安装于艏部推进段7后上方，驱动电机与推进螺旋桨之间采用磁耦合联轴器连接；电子舱段12由锂电池9、抛载10、天线导流罩13、电子舱段壳体11组成，天线导流罩13包括超短基线接收器和天线，锂电池9环绕相连于电子舱段12前部，下部通过电磁铁将抛载10垂直固定，后上部通过水密接插件与天线导流罩13相连；艉部推进段15包括艉部垂向推进器14、艉部侧向推进器17和艉部推进段壳体16组成，垂向推进器14垂直放置，安装于艉部推进段15前上方，侧向推进器17水平放置，安装于艉部推进段15后侧方，驱动电机与推进螺旋桨之间采用磁耦合联轴器连接；主推进段14由主推进器19组成，主推进器19采用桨后舵设计。

一种全驱动自主水下机器人的回收三维路径跟踪方法，回收控制过程包括：

过程1：直线归位阶段20，这个阶段是指回收装置24用超短基线定位到AUV开始,进入对接中轴线21的跟踪过程；利用超短基线提供的相对位置、姿态信息来调整AUV与对接装置的位置，使得在消耗较短对接中轴线距离的情况下，将AUV航行至中轴线上，使AUV的姿态与中轴线一致，利于进一步实时对接；

过程2：直线跟踪阶段22，这个阶段指是指AUV重心进入中轴线21开始直到距离对接口3-5米的阶段，在这个阶段要保证AUV沿中轴线航行，且艏向角指向对接口；

过程3：艏向调整阶段23，这个阶段是指过程2结束到对接成功的阶段，进入此阶段需要满足两个条件：一是AUV与对接装置轴线距离小于5米，二是相对位置、姿态偏差超出预设值；如果直线跟踪阶段保持的位置、姿态偏差一直未超过预设值，则可以略过此阶段，直接完成对接，即只有在出现足够误差时，才会触发艏向调整阶段。

本发明的目的还可以采用以下技术措施进一步实现：

前述全驱动自主水下机器人的回收三维路径跟踪方法，其中机器人回收直线归位阶段20与直线跟踪阶段22的三维路径跟踪控制方法包括以下步骤：

步骤1：将回收路径三维曲线转化为空间点序列；

步骤2：将空间点序列描述为自主水下航行器的目标跟踪点，将其转化为自主水下航行器的惯性坐标系下的坐标，对其进行目标跟踪；

步骤3：全驱动控制形式的自主水下航行器，对单个目标点进行跟踪控制，将水下机器人的位置和艏向角与目标点位置和期望艏向值进行对比得到跟踪误差，采用改进的非奇异终端模糊滑模算法，输出为各推进器的推力；

步骤4：进行步骤2所述目标跟踪点的更替，并依次运行步骤3完成每个目标点的跟踪，最终完成空间点序列的跟踪，实现回收三维路径跟踪。

前述全驱动自主水下机器人的回收三维路径跟踪方法，其中改进的非奇异终端模糊滑模控制算法包括以下步骤：

步骤1：全驱动自主水下机器人的运动学数学模型表示为：

式中，η＝[x y z θ ψ]^T，为水下机器人相对于惯性坐标系中的位置和姿态角，其中θ为俯仰角，ψ为航向角，此处不考虑横滚角对机器人的影响；v＝[u v w q r]^T，为水下机器人在载体坐标系内的线速度和角速度，其中u,v,w分别为x,y,z坐标轴方向的线速度，q,r为y,z坐标轴方向的角速度；J(η)为坐标转换矩阵；M为包含附加质量的惯量矩阵且满足对称性M＝M^T＞0，

C(v)v为包含了质量矩阵和附加质量矩阵引起的科氏力与向心力且满足反对称性；D(v)为黏性水动力系数矩阵且D(v)＞0；v为去除环境干扰力时水下机器人的速度；g(η)是恢复力和力矩；τ是各自由度的力和力矩；ξ为环境干扰；

步骤2：改进的非奇异终端模糊滑模控制器根据当前水下机器人的位置和艏向角与目标点位置和期望艏向值得到位置跟踪误差和艏向跟踪误差，通过模糊控制器对滑模增益进行优化，得出各推进器的推力；

位置跟踪误差为：

其中，e_p为位置跟踪误差，x_d，y_d，z_d分别为目标点三轴方向上的位置坐标，x,y,z分别为当前水下机器人三轴方向上的位置坐标；

艏向跟踪误差为：

e_ψ＝ψ_d-ψ (3)

其中，e_ψ为艏向跟踪误差，ψ_d为期望艏向值，ψ为当前水下机器人的艏向角；

定义n＝[x y z θ ψ]^T，与η等同，是机器人实时的位置和姿态，则

表示三轴方向上线速度和角速度，

表示三轴方向上线加速度和角加速度，令常数n_d为目标点，那么跟踪误差表示为n_e＝n-n_d，算法实现目标为寻找适当的控制律，使水下机器人的位置n能够尽快地到达期望值n_d，即n_e要在有限时间内快速收敛到零；

定义运算

其中，sgn为符号函数，x₁,…x₅为各状态变量，γ₁,…γ₅为状态变量系数；

设计非奇异终端滑模面：

为了使连接点处的速度连续，即n_e＝ε时

要相等，所以有

ε＝β^q/p-q (6)

式中，ε＝diag{ε₁,…ε₅}，β＝diag{β₁,…β₅}，β_i∈R⁺，s＝[s₁,…s₅]^T，n_e,

p_i,q_i∈N⁺，i＝1,2…5且1＜p_i/q_i＜2；

以|s|和d|s|/dt作为模糊控制器的输入，滑模增益优化项Δk作为模糊控制器的输出，设计一个双输入单输出的模糊控制器；其中|s|，d|s|/dt，均为三角形隶属度函数；

描述输入输出变量的子集分别为d|s|＝[NB＝负大，NM＝负中，NS＝负小，ZO＝零，PS＝正小，PM＝正中，PB＝正大]，|s|＝[S0,S1,S2,S3,S4,S5,S6],Δk＝[K0,K1,K2,K3,K4,K5,K6],其中S0,S1,S2,S3,S4,S5,S6,K0,K1,K2,K3,K4,K5,K6均为模糊数；

采取的控制规则为：

如果d|s|是A,|s|是B,则Δk是C；

优化后开关函数的增益为：

k＝η×(1+Δk)+d (7)

采用如下终端吸引子作为趋近率：

其中，

k₁＝diag(k₁₁,…k₁₅),k₂＝diag(k₂₁,…,k₂₅),k_1i，k_2i∈R⁺,i＝1,2,…5；

式(5)的一阶导数表示如下：

其中，I∈R^5×5，i＝1,2,···,5,

由式(4)～(9)可得，对于AUV的非线性动力学模型公式(1)，如果变量非奇异终端滑模选取式(5)，趋近率选取式(8)，采用如下控制率：

u＝u₁+u₂ (12)

其中，为了减轻当|n_e|＞ε时，滑模控制器中的颤振现象，利用饱和函数sat(s/Δ)替代滑模控制器中的指数趋近率的符号函数sig(s)，并取边界层Δ＝0.01，α＝diag{β₁,···β₅}，α_i∈R⁺。

前述全驱动自主水下机器人的回收三维路径跟踪方法，改进的非奇异终端模糊滑模控制算法中模糊控制器的滑模增益值选取，具体为：

控制器运动点远离滑模面，即|s|＞0时，滑模增益选取为30，提高控制器运动点的趋近速度；在靠近滑模面的区域内，即s→0时，滑模增益不应超过25，以此减弱因为s的正负性频繁变化造成的高频抖振。

与现有技术相比，本发明的有益效果是：

1.本发明的用于回收的全驱动自主水下机器人结构具有易拆装、模块化特点。采用多推进器设计，控制灵活。

2.将回收路径规划为三段式，分别对三段曲线进行三维路径跟踪，针对性设计不同阶段的跟踪控制策略，提高回收成功率。

3.将每段回收空间曲线规划成点序列进行跟踪控制，可满足回收高精度跟踪性能的需求，提高回收成功率。

4.本发明在回收三维路径跟踪中采用的改进的非奇异终端模糊滑模算法，在滑模控制基础上，设计分段切换函数，实现多个滑动模态，并通过模糊算法对滑模增益进行实时优化，使其能快速、平稳地追踪目标点，增强回收三维路径跟踪性能，提高回收成功率。

附图说明

图1是本发明的机器人外形结构框图；

图2是本发明的机器人回收对接流程框图；

图3是本发明的机器人回收对接三维路径跟踪流程框图；

图4是本发明的机器人回收三维路径跟踪改进的非奇异终端模糊滑模算法流程框图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明。

如图1所示，全驱动自主水下机器人结构，包括艏段2，艏部推进段7，电子舱段12，艉部推进段15和主推进段18五大部分；整体采用鱼雷形流线型结构，艏段2与艏部推进段7、艏部推进段7与电子舱段12，电子舱段12与艉部推进段15、艉部推进段15与主推进段18同心密封相连；艏段2包括探测仪1、避碰声纳4和艏段壳体3，探测仪1通过密封安装于艏段2前部，避碰声呐4通过水密接插件安装于艏段2上部，艏段壳体3采用铝合金6061制造并进行表面硬质氧化处理，艏段壳体3采用半球形外形以保证水下机器人具有良好的流线型并兼顾艏部的安装空间；艏部推进段7由艏部侧向推进器5、艏部垂向推进器8和艏部推进段壳体6组成，侧向推进器5水平放置，安装于艏部推进段7前侧方，垂向推进器8垂直放置，安装于艏部推进段7后上方，驱动电机与推进螺旋桨之间采用磁耦合联轴器连接取代以往的动密封结构来提供系统的可靠性；电子舱段12由锂电池9、抛载10、天线导流罩13、电子舱段壳体11组成，天线导流罩13包括超短基线接收器和天线，锂电池9环绕相连于电子舱段12前部，下部通过电磁铁将抛载10垂直固定，后上部通过水密接插件与天线导流罩13相连；艉部推进段15包括艉部垂向推进器14、艉部侧向推进器17和艉部推进段壳体16组成，垂向推进器14垂直放置，安装于艉部推进段15前上方，侧向推进器17水平放置，安装于艉部推进段15后侧方，驱动电机与推进螺旋桨之间采用磁耦合联轴器连接取代以往的动密封结构来提供系统的可靠性；主推进段14由主推进器19组成，主推进器19采用桨后舵设计，避免了传统十字舵或X字舵的缺点，防止水下机器人在收放过程中，碰伤舵板。

如图2所示，全驱动自主水下机器人回收对接流程框图。回收分为3个过程：

过程1：直线归位阶段20，这个阶段是指回收装置24用超短基线定位到AUV开始,进入对接中轴线21的跟踪过程；利用超短基线提供的相对位置、姿态信息来调整AUV与对接装置的位置，使得在消耗较短对接中轴线距离的情况下，将AUV航行至中轴线上，使AUV的姿态与中轴线一致，利于进一步实时对接；

过程2：直线跟踪阶段22，这个阶段指是指AUV重心进入中轴线21开始直到距离对接口3-5米的阶段，在这个阶段要保证AUV沿中轴线航行，且艏向角指向对接口；

过程3：艏向调整阶段23，这个阶段是指过程2结束到对接成功的阶段，进入此阶段需要满足两个条件：一是AUV与对接装置轴线距离小于5米，二是相对位置、姿态偏差超出预设值；如果直线跟踪阶段保持的位置、姿态偏差一直未超过预设值，则可以略过此阶段，直接完成对接，即只有在出现足够误差时，才会触发艏向调整阶段。

如图3所示，全驱动自主水下机器人回收对接三维路径跟踪流程框图，具体步骤如下：

步骤1：将回收路径三维曲线转化为空间点序列；

步骤2：将空间点序列描述为自主水下航行器的目标跟踪点，将其转化为自主水下航行器的惯性坐标系下的坐标，对其进行目标跟踪；

步骤3：全驱动控制形式的自主水下航行器，对单个目标点进行跟踪控制，首先将位置偏差转化为参考艏向角，然后将期望艏向角和期望深度作为输入，采用改进的非奇异终端模糊滑模算法，输出为各推进器的推力；

步骤4：进行步骤2目标跟踪点的更替，并依次运行步骤3完成每个目标点的跟踪，最终完成空间点序列的跟踪，实现回收三维路径跟踪。

如图4所示，全驱动自主水下机器人回收三维路径跟踪改进的非奇异终端模糊滑模算法流程框图，其特征是：

步骤1：全驱动自主水下机器人的运动学数学模型可以表示为：

式中，η＝[x y z θ ψ]^T，为水下机器人相对于惯性坐标系中的位置和姿态角，其中θ为俯仰角，ψ为航向角，此处不考虑横滚角对机器人的影响；v＝[u v w q r]^T，为水下机器人在载体坐标系内的线速度和角速度，其中u,v,w分别为x,y,z坐标轴方向的线速度，q,r为y,z坐标轴方向的角速度；J(η)为坐标转换矩阵；M为包含附加质量的惯量矩阵且满足对称性M＝M^T＞0，

C(v)v为包含了质量矩阵和附加质量矩阵引起的科氏力与向心力且满足反对称性；D(v)为黏性水动力系数矩阵且D(v)＞0；v为去除环境干扰力时水下机器人的速度；g(η)是恢复力和力矩；τ是各自由度的力和力矩；ξ为环境干扰；

步骤2：多模态、多变量非奇异终端模糊滑模控制器根据当前水下机器人的位置和艏向角与目标点位置和期望艏向值进行对比得到位置跟踪误差和艏向跟踪误差，通过模糊控制器对滑模增益进行优化，得到各推进器的推力；

位置跟踪误差为：

其中，e_p为位置跟踪误差，x_d，y_d，z_d分别为目标点三轴方向上的位置坐标，x,y,z分别为当前水下机器人三轴方向上的位置坐标；

艏向跟踪误差为：

e_ψ＝ψ_d-ψ (3)

其中，e_ψ为艏向跟踪误差，ψ_d为期望艏向值，ψ为当前水下机器人的艏向角；

定义n＝[x y z θ ψ]^T，与η等同，是机器人实时的位置和姿态，则

表示三轴方向上线速度和角速度，

表示三轴方向上线加速度和角加速度，令常数n_d为目标点，那么跟踪误差表示为n_e＝n-n_d，算法实现目标为寻找适当的控制律，使水下机器人的位置n能够尽快地到达期望值n_d，即n_e要在有限时间内快速收敛到零；

定义运算

其中，sgn为符号函数，x₁,…x₅为各状态变量，γ₁,…γ₅为状态变量系数；

设计非奇异终端滑模面：

为了使连接点处的速度连续，即n_e＝ε时

要相等，所以有

ε＝β^q/p-q (6)

式中，ε＝diag{ε₁,…ε₅}，β＝diag{β₁,…β₅}，β_i∈R+，s＝[s₁,…s₅]^T，n_e,

p_i,q_i∈N⁺，i＝1,2…5且1＜p_i/q_i＜2；

以|s|和d|s|/dt作为模糊控制器的输入，滑模增益优化项Δk作为模糊控制器的输出，设计一个双输入单输出的模糊控制器。其中|s|，d|s|/dt，均为三角形隶属度函数；

控制器运动点远离滑模面，即|s|＞0时，滑模增益选取为30，提高控制器运动点的趋近速度；在靠近滑模面的区域内，即s→0时，滑模增益不应超过25，以此减弱因为s的正负性频繁变化造成的高频抖振。为了达到以上效果，设计出了模糊推理规则：

其中,描述输入输出变量的子集分别为d|s|＝[NB＝负大，NM＝负中，NS＝负小，ZO＝零，PS＝正小，PM＝正中，PB＝正大]，|s|＝[S0,S1,S2,S3,S4,S5,S6],Δk＝[K0,K1,K2,K3,K4,K5,K6],其中S0,S1,S2,S3,S4,S5,S6,K0,K1,K2,K3,K4,K5,K6为模糊数；

采取的模糊规则为：

如果d|s|是A,|s|是B,则Δk是C；

优化后开关函数的增益为：

k＝η×(1+Δk)+d (7)

采用如下终端吸引子作为趋近率：

其中，

k₁＝diag(k₁₁,…k₁₅),k₂＝diag(k₂₁,…,k₂₅),k_1i，k_2i∈R⁺,i＝1,2,…5；

式(5)的一阶导数表示如下：

其中，I∈R^5×5，i＝1,2,···,5,

由式(4)～(9)可得，对于AUV的非线性动力学模型公式(1)，如果变量FNTSM选取式(5)，趋近率选取式(8)，采用如下控制率：

u＝u₁+u₂ (12)

其中，为了减轻当|n_e|＞ε时，滑模控制器中的颤振现象，利用饱和函数sat(s/Δ)替代滑模控制器中的指数趋近率的符号函数sig(s)，并取边界层Δ＝0.01，α＝diag{β₁,···β₅}，α_i∈R⁺。

除上述实施例外，本发明还可以有其他实施方式，凡采用等同替换或等效变换形成的技术方案，均落在本发明要求的保护范围内。

Claims (4)

1.一种全驱动自主水下机器人结构的回收三维路径跟踪方法，全驱动自主水下机器人结构包括艏段(2)，艏部推进段(7)，电子舱段(12)，艉部推进段(15)和主推进段(18)五大部分；整体采用鱼雷形流线型结构，艏段(2)与艏部推进段(7)、艏部推进段(7)与电子舱段(12)，电子舱段(12)与艉部推进段(15)、艉部推进段(15)与主推进段(18)同心密封相连；艏段(2)包括探测仪(1)、避碰声呐(4)和艏段壳体(3)，探测仪(1)通过密封安装于艏段(2)前部，避碰声呐(4)通过水密接插件安装于艏段(2)上部，艏段壳体(3)采用铝合金6061制造并进行表面硬质氧化处理，艏段壳体(3)采用半球形外形以保证水下机器人具有良好的流线型并兼顾艏部的安装空间；艏部推进段(7)由艏部侧向推进器(5)、艏部垂向推进器(8)和艏部推进段壳体(6)组成，侧向推进器(5)水平放置，安装于艏部推进段(7)前侧方，垂向推进器(8)垂直放置，安装于艏部推进段(7)后上方，驱动电机与推进螺旋桨之间采用磁耦合联轴器连接；电子舱段(12)由锂电池(9)、抛载(10)、天线导流罩(13)、电子舱段壳体(11)组成，天线导流罩(13)包括超短基线接收器和天线，锂电池(9)环绕相连于电子舱段(12)前部，下部通过电磁铁将抛载(10)垂直固定，后上部通过水密接插件与天线导流罩(13)相连；艉部推进段(15)包括艉部垂向推进器(14)、艉部侧向推进器(17)和艉部推进段壳体(16)组成，垂向推进器(14)垂直放置，安装于艉部推进段(15)前上方，侧向推进器(17)水平放置，安装于艉部推进段(15)后侧方，驱动电机与推进螺旋桨之间采用磁耦合联轴器连接；主推进段(18)由主推进器(19)组成，主推进器(19)采用桨后舵设计；其特征在于，回收控制过程包括以下3个过程：

过程1：直线归位阶段，这个阶段是指回收装置用超短基线定位到AUV开始,进入对接中轴线跟踪过程,这个过程重点在于利用超短基线提供的相对位置/姿态信息来调整AUV与对接装置的位置，使得在消耗较短对接中轴线距离的情况下，将AUV航行至中轴线上，同时要考虑到AUV的姿态尽可能的与中轴线一致，利于进一步实时对接；

过程2：直线跟踪阶段，这个阶段指是指AUV重心进入中轴线开始直到距离对接口3到5米的阶段，在这个阶段要保证AUV沿中轴线航行，且艏向角指向对接口；

过程3：艏向调整阶段，这个阶段是指过程2结束到对接成功的阶段，进入此阶段需要满足两个条件：一是AUV与对接装置轴线距离小于5米，二是相对位置/姿态偏差超出预设值，如果直线跟踪阶段保持的位置/姿态偏差一直未超过预设值，则略过此阶段，直接完成对接，即只有在出现足够误差时，才会触发艏向调整阶段。

2.如权利要求1所述的全驱动自主水下机器人结构的回收三维路径跟踪方法，其特征在于，其中机器人回收直线归位阶段与直线跟踪阶段的三维路径跟踪控制方法包括以下步骤：

步骤1：将回收路径三维曲线转化为空间点序列；

步骤2：将空间点序列描述为自主水下航行器的目标跟踪点，将其转化为自主水下航行器的惯性坐标系下的坐标，对其进行目标跟踪；

步骤3：全驱动控制形式的自主水下航行器，对单个目标点进行跟踪控制，将水下机器人的位置和艏向角与目标点位置和期望艏向值进行对比得到跟踪误差，采用改进的非奇异终端模糊滑模算法，输出为各推进器的推力；

步骤4：进行步骤2目标跟踪点的更替，并依次运行步骤3完成每个目标点的跟踪，最终完成空间点序列的跟踪，实现回收三维路径跟踪。

3.如权利要求2所述的全驱动自主水下机器人结构的回收三维路径跟踪方法，其特征在于，改进的非奇异终端模糊滑模控制算法包括以下步骤：

步骤1：全驱动自主水下机器人的运动学数学模型表示为：

式中，η＝[x y z θ ψ]^T，为水下机器人相对于惯性坐标系中的位置和姿态角，其中θ为俯仰角，ψ为航向角，此处不考虑横滚角对机器人的影响；v＝[u v w q r]^T，为水下机器人在载体坐标系内的线速度和角速度，其中u,v,w分别为x,y,z坐标轴方向的线速度，q,r为y,z坐标轴方向的角速度；J(η)为坐标转换矩阵；M为包含附加质量的惯量矩阵且满足对称性M＝M^T＞0，

C(v)v为包含了质量矩阵和附加质量矩阵引起的科氏力与向心力且满足反对称性；D(v)为黏性水动力系数矩阵且D(v)＞0；v为去除环境干扰力时水下机器人的速度；g(η)是恢复力和力矩；τ是各自由度的力和力矩；ξ为环境干扰；

步骤2：改进的非奇异终端模糊滑模控制器根据当前水下机器人的位置和艏向角与目标点位置和期望艏向值进行对比得到位置跟踪误差和艏向跟踪误差，通过模糊控制器对滑模增益进行优化，得到各推进器的推力；

位置跟踪误差为：

其中，e_p为位置跟踪误差，x_d，y_d，z_d分别为目标点三轴方向上的位置坐标，x,y,z分别为当前水下机器人三轴方向上的位置坐标；

艏向跟踪误差为：

e_ψ＝ψ_d-ψ (3)

其中，e_ψ为艏向跟踪误差，ψ_d为期望艏向值，ψ为当前水下机器人的艏向角；

定义n＝[x y z θ ψ]^T，与η等同，是机器人实时的位置和姿态，则

表示三轴方向上线速度和角速度，

表示三轴方向上线加速度和角加速度，令常数n_d为目标点，那么跟踪误差表示为n_e＝n-n_d，算法实现目标为寻找适当的控制律，使水下机器人的位置n能够尽快地到达期望值n_d，即n_e要在有限时间内快速收敛到零；

定义运算

其中，sgn为符号函数，x₁,…x₅为各状态变量，γ₁,…γ₅为状态变量系数；

设计非奇异终端滑模面：

为了使连接点处的速度连续，即n_e＝ε时

要相等，所以有

ε＝β^q/p-q (6)

式中，ε＝diag{ε₁,…ε₅}，β＝diag{β₁,…β₅}，β_i∈R⁺，s＝[s₁,…s₅]^T，

p_i,q_i∈N⁺，i＝1,2…5且1＜p_i/q_i＜2；

以|s|和d|s|/dt作为模糊控制器的输入，滑模增益优化项Δk作为模糊控制器的输出，设计一个双输入单输出的模糊控制器；其中|s|，d|s|/dt，均为三角形隶属度函数；

描述输入输出变量的子集分别为d|s|＝[NB＝负大，NM＝负中，NS＝负小，ZO＝零，PS＝正小，PM＝正中，PB＝正大]，|s|＝[S0,S1,S2,S3,S4,S5,S6],Δk＝[K0,K1,K2,K3,K4,K5,K6],其中S0,S1,S2,S3,S4,S5,S6,K0,K1,K2,K3,K4,K5,K6为模糊数；

采取的控制规则为：

如果d|s|是A,|s|是B,则Δk是C；

优化后开关函数的增益为：

k＝η×(1+Δk)+d (7)

采用如下终端吸引子作为趋近率：

其中，

k₁＝diag(k₁₁,…k₁₅),k₂＝diag(k₂₁,…,k₂₅),k_1i，k_2i∈R⁺,i＝1,2,…5；

式(5)的一阶导数表示如下：

其中，I∈R^5×5，i＝1,2,···,5,

由式(4)～(9)可得，对于AUV的非线性动力学模型公式(1)，如果变量非奇异终端滑模选取式(5)，趋近率选取式(8)，采用如下控制率：

u＝u₁+u₂ (12)

其中，为了减轻当|n_e|＞ε时，滑模控制器中的颤振现象，利用饱和函数sat(s/Δ)替代滑模控制器中的指数趋近率的符号函数sig(s)，并取边界层Δ＝0.01，α＝diag{β₁,···β₅}，α_i∈R⁺。

4.如权利要求3所述的全驱动自主水下机器人结构的回收三维路径跟踪方法，其特征在于，改进的非奇异终端模糊滑模控制算法中模糊控制器的滑模增益值选取为：

控制器运动点远离滑模面，即|s|＞0时，滑模增益选取为30，提高控制器运动点的趋近速度；在靠近滑模面的区域内，即s→0时，滑模增益不应超过25，以此减弱因为s的正负性频繁变化造成的高频抖振。

Images