CN112904719A - 一种适用于水下机器人位置环形区域跟踪控制方法 - Google Patents

Abstract

本发明属于水下机器人控制技术领域，具体涉及一种适用于水下机器人位置环形区域跟踪控制方法。本发明从环形区域跟踪的任务需求出发，利用给定的环形期望区域边界和位置跟踪误差构造特殊的Lyapunov函数；再针对控制器设计过程中存在的建模误差以及辨识误差，结合水下机器人动力学近似模型及其全状态量测信息，构建状态观测器，以实现估计误差的有限时间收敛特性：最后，结合艇体速度信息以及虚拟控制律，计算环形区域跟踪控制律。本发明可实现水下机器人位置跟踪误差矢量的幅值一直维持在事先设定的环形期望区域范围内，而艏向角度误差一直维持在事先设定的圆形期望区域范围内。

Description

一种适用于水下机器人位置环形区域跟踪控制方法

技术领域

本发明属于水下机器人控制技术领域，具体涉及一种适用于水下机器人位置环形区域跟踪控制方法。

背景技术

水下机器人在执行类似于管道跟踪、水下探测等任务时，结合任务需求以及复杂海洋环境的特点，只需水下机器人跟踪误差满足用户事先设定的范围即可。因此区域跟踪控制的概念应运而生，即：将传统的期望目标点改为期望区域。

现有的区域跟踪控制方法主要从两方面进行研究。第一类是仅关注每个自由度方向上稳态跟踪误差是否维持在期望圆形区域范围内；另一类则是从全过程角度(包括暂态性能和稳态性能)研究每个自由度方向上跟踪误差的区域跟踪控制。第一类区域跟踪控制可通过基于势能函数的区域跟踪控制方法和基于分段且连续Lyapunov函数的区域跟踪控制方法来实现；而第二类区域跟踪控制主要基于预设性能控制方法及其变体来实现。然而，对某些水下任务如管道探测时，我们更应该关注位置跟踪误差矢量的幅值，即当前水下机器人的位置与期望位置点之间的距离，而方位上则需要各姿态角的跟踪误差单独满足圆形期望区域；此外，在管道跟踪任务中受传感器量量测广度的限制，往往要求水下机器人位置到期望点的距离还需大于一个基本值以维持较好的量测广度。此时需要控制系统能实现水下机器人位置跟踪误差矢量的幅值一直维持在事先设定的环形期望区域范围内，而艏向角度误差一直维持在事先设定的圆形期望区域范围内。现有的这些区域跟踪控制方法不能满足此任务需求。因此，针对此应用情况，研究位置跟踪误差矢量幅值的环形区域跟踪控制是很有意义的。

发明内容

本发明的目的在于提供可实现水下机器人位置跟踪误差矢量的幅值一直维持在事先设定的环形期望区域范围内，且艏向角度误差一直维持在事先设定的圆形期望区域范围内的一种适用于水下机器人位置环形区域跟踪控制方法。

本发明的目的通过如下技术方案来实现：包括以下步骤：

步骤1：获取水下机器人当前t时刻位置信息

艏向角ξ₂＝φ以及速度信息

(x,y,z)为水下机器人在大地坐标系下的坐标；

J₁为旋转变换矩阵；u、v、w为水下机器人艇体坐标系下的纵向、横向以及垂向的线速度；

r为水下机器人艇体坐标系下的艏向角速度；

步骤2：根据任务需求获取期望轨迹

设定期望环形区域的上边界ε_1a和下边界ε_1b以及期望圆形区域的边界ε_2a；其中，

为期望路径点；ξ_d2＝φ_d为期望艏向角；ε_1b设定为常数，ε_1b＞0；ε_1a(t)与ε_2a(t)均设定为指数衰减函数，ε_1a(t)＞ε_1b，ε_2a(t)＞0；

步骤3：计算虚拟控制量

其中，z_a1＝ξ₁-ξ_d1，

z_b1＝ξ₂-ξ_d2；

步骤4：根据上一时刻的跟踪控制律u(t-1)＝[F_x(t-1)F_y(t-1)F_z(t-1)M_φ(t-1)]^T，由状态观测器获取建模误差以及辨识误差的估计值

其中，F_x、F_y、F_z分别为纵向、横向、垂向自由度上所需的力，M_φ为转艏力矩；

M(ξ)为质量矩阵；F(ξ,q)为水动力项；

为水下机器人艇体坐标系下线速度及艏向角速度的估计值；k₁、k₂、k₃、k₄为设计参数，且满足

k₂＞0，k₃＞ρ_H，

ρ_H为

的上边界，

步骤5：根据虚拟控制量

和建模误差以及辨识误差的估计值

计算当前时刻的跟踪控制律u(t)＝[F_x(t)F_y(t)F_z(t)M_φ(t)]^T；

其中，

z_a2＝q₁-α₁，z_b2＝q₂-α₂；β为大于1/4的参数；

步骤6：水下机器人根据跟踪控制律u(t)执行环形区域跟踪控制；

步骤7：判断是否到达终点；若未到达终点，则返回步骤1，继续执行环形区域跟踪。

本发明还可以包括：

所述的步骤2中

其中，ρ_∞1、ρ_∞2、ρ₀₁、ρ₀₂、λ₁、λ₂为设计参数且均大于零。

本发明的有益效果在于：

现有的区域跟踪控制方法大都关注各自由度方向上跟踪误差是否维持在期望区域范围内，而本发明专利从实际需求出发关注三维位置跟踪误差矢量幅值是否维持在期望区域范围内，方位上则是关注各姿态角度误差是否维持在期望区域范围内。另外，在管道跟踪任务中受传感器量测距离以及量测广度的限制，往往要求水下机器人位置到期望点的距离能一直保持在一个环形期望区域范围内，现有区域跟踪控制方法不能满足此要求，与现有区域跟踪控制方法只是实现圆形区域跟踪的技术路线不同，本发明设计了一种位置跟踪误差矢量幅值的环形区域跟踪控制方法，实现了位置跟踪误差矢量幅值的环形期望区域跟踪、艏向角度误差的圆形期望区域跟踪。

本发明从环形区域跟踪的任务需求出发，利用给定的环形期望区域边界和位置跟踪误差构造特殊的Lyapunov函数；再针对控制器设计过程中存在的建模误差以及辨识误差，结合水下机器人动力学近似模型及其全状态量测信息，构建状态观测器，以实现估计误差的有限时间收敛特性：最后，结合艇体速度信息以及虚拟控制律，计算环形区域跟踪控制律。本发明可实现水下机器人位置跟踪误差矢量的幅值一直维持在事先设定的环形期望区域范围内，而艏向角度误差一直维持在事先设定的圆形期望区域范围内。

附图说明

图1为本发明中环形区域跟踪的概念图。

图2为本发明中各自由度跟踪误差的结果图。

图3为本发明中位置跟踪误差矢量幅值的环形区域跟踪效果图。

图4为本发明中艏向角度跟踪误差的区域跟踪效果图。

图5为本发明中各自由度方向上的控制量信号图。

具体实施方式

下面结合附图对本发明做进一步描述。

本发明涉及一种适用于水下机器人位置环形区域跟踪控制方法，属于水下机器人控制技术领域。本发明针对环形区域跟踪的特殊性，提供了一种控制策略可确保水下机器人当前位置点到相对应期望点之间的距离可始终维持在事先设定的环形区域之内，而艏向角度跟踪误差可始终保持在以零为中心的圆形期望区域范围内。

一种适用于水下机器人位置环形区域跟踪控制方法，包括以下步骤：

步骤1：获取水下机器人当前t时刻位置信息

艏向角ξ₂＝φ以及速度信息

(x,y,z)为水下机器人在大地坐标系下的坐标；

J₁为旋转变换矩阵；u、v、w为水下机器人艇体坐标系下的纵向、横向以及垂向的线速度；

r为水下机器人艇体坐标系下的艏向角速度；

步骤2：根据任务需求获取期望轨迹

设定期望环形区域的上边界ε_1a和下边界ε_1b以及期望圆形区域的边界ε_2a；其中，

为期望路径点；ξ_d2＝φ_d为期望艏向角；ε_1b设定为常数，ε_1b＞0；ε_1a(t)设定为指数衰减函数，

ε_2a(t)设定为指数衰减函数，

其中，ρ_∞1、ρ_∞2、ρ₀₁、ρ₀₂、λ₁、λ₂为设计参数且均大于零；ε_1a(t)＞ε_1b，ε_2a(t)＞0；

步骤3：计算虚拟控制量

其中，z_a1＝ξ₁-ξ_d1，

z_b1＝ξ₂-ξ_d2；

步骤4：根据上一时刻的跟踪控制律u(t-1)＝[F_x(t-1)F_y(t-1)F_z(t-1)M_φ(t-1)]^T，由状态观测器获取建模误差以及辨识误差的估计值

其中，F_x、F_y、F_z分别为纵向、横向、垂向自由度上所需的力，M_φ为转艏力矩；

M(ξ)为质量矩阵；F(ξ,q)为水动力项；

为水下机器人艇体坐标系下线速度及艏向角速度的估计值；k₁、k₂、k₃、k₄为设计参数，且满足

k₂＞0，k₃＞ρ_H，

ρ_H为

的上边界，

步骤5：根据虚拟控制量

和建模误差以及辨识误差的估计值

计算当前时刻的跟踪控制律u(t)＝[F_x(t)F_y(t)F_z(t)M_φ(t)]^T；

其中，

z_a2＝q₁-α₁，z_b2＝q₂-α₂；β为大于1/4的参数；

步骤6：水下机器人根据跟踪控制律u(t)执行环形区域跟踪控制；

步骤7：判断是否到达终点；若未到达终点，则返回步骤1，继续执行环形区域跟踪。

本发明的优点在于：现有的区域跟踪控制方法大都关注各自由度方向上跟踪误差是否维持在期望区域范围内，而本发明专利从实际需求出发关注三维位置跟踪误差矢量幅值是否维持在期望区域范围内，方位上则是关注各姿态角度误差是否维持在期望区域范围内。另外，在管道跟踪任务中受传感器量测距离以及量测广度的限制，往往要求水下机器人位置到期望点的距离能一直保持在一个环形期望区域范围内，现有区域跟踪控制方法不能满足此要求，与现有区域跟踪控制方法只是实现圆形区域跟踪的技术路线不同，本发明设计了一种位置跟踪误差矢量幅值的环形区域跟踪控制方法，实现了位置跟踪误差矢量幅值的环形期望区域跟踪、艏向角度误差的圆形期望区域跟踪。本发明可实现水下机器人位置跟踪误差矢量的幅值一直维持在事先设定的环形期望区域范围内，而艏向角度误差一直维持在事先设定的圆形期望区域范围内。

本发明从环形区域跟踪的任务需求出发，利用给定的环形期望区域边界和位置跟踪误差构造特殊的Lyapunov函数；再针对控制器设计过程中存在的建模误差以及辨识误差，结合水下机器人动力学近似模型及其全状态量测信息，构建状态观测器，以实现估计误差的有限时间收敛特性：最后，结合艇体速度信息以及虚拟控制律，推导环形区域跟踪控制律。

图1为本发明专利的AUV区域跟踪控制的流程图。结合图1，一种位置跟踪误差矢量幅值的环形区域跟踪控制方法的具体实现步骤如下：

步骤(1)：结合任务需求，构造可实现位置误差距离上环形区域跟踪的Lyapunov函数以及艏向角度误差上圆形区域跟踪的Lyapunov函数。

由于某些水下机器人结构设计等原因，其横滚角和俯仰角可忽略，不需进行控制。因此，本专利仅考虑了水下机器人四个自由度，即：三个位置和一个艏向角度。

x，y，z分为为水下机器人的位置信息，φ为艏向角度。

定义期望轨迹

假定该期望轨迹是连续且光滑的。其跟踪误差可表示为

为让位置跟踪误差的距离可始终保持在环形期望区域范围内，本专利构造如下Lyapunov函数。

其中，ε_1b＞0为期望环形区域的下边界，一般设计成常数；ε_1a＞ε_1b为期望环形区域的上边界，一般设计成指数衰减函数，例如

ρ_∞1，ρ₀₁，λ₁为设计参数且均大于零；

Lyapunov函数V₁对||z_a1||²求偏导，可得：

其中，

Lyapunov函数V₁对||z_a1||²的偏导函数

满足以下不等式：

Lyapunov函数V₁对ε_1a求偏导，可得：

为了使艏向角度跟踪误差始终保持在期望的圆形区域范围内，构造以下Lyapunov函数V₂。

其中，ε_2a＞0为期望圆形区域的边界，一般设定为指数衰减函数，例如：

ρ_∞2，ρ₀₂，λ₂为设计参数且均大于零。

同样，Lyapunov函数V₂分别对

和ε_2a求偏导，得到如下公式：

同样，函数

满足以下不等式：

综上，若能证明这两个Lyapunov函数V₁和V₂均是有界的，即可说明位置跟踪误差的距离始终保持在期望的环形区域范围内，以及艏向角度跟踪误差始终保持在圆形期望区域范围内。

步骤(2)：结合步骤(1)给出的Lyapunov函数以及水下机器人运动学方程，根据反演控制设计思路，推导适用于水下机器人位置误差的距离区域跟踪虚拟控制量，以及适用于艏向角度误差的自由度区域跟踪虚拟控制量；

水下机器人的运动学方程可描述为：

其中，

J₁为旋转变换矩阵，满足

u，v，w为水下机器人艇体坐标系下的线速度，r为水下机器人艇体坐标系下的艏向角速度。

定义速度跟踪误差变量：

其中

为将要设计的虚拟控制量，且z_a2＝q₁-α₁；z_b2＝q₂-α₂。

对Lyapunov函数V₁和V₂分别求时间导数并带入

和

的具体表达式，得：

为了让

和

尽可能小于零，虚拟控制量α设计成如下形式：

其中，

和

分别为期望线速度和期望艏向角速度。

进而

和

可进一步简化为：

步骤(3)：结合水下机器人动力学近似模型以及水下机器人全状态量测信息，构建状态观测器以及广义不确定性项的估计律。

水下机器人动力学模型往往通过一个二阶动力学方程来描述，在这一过程中不可避免存在建模误差，再加上基于传感器数据进行模型辨识的过程中因海流干扰以及量测噪声的存在，也不可避免地导致辨识出的动力学参数存在不确定性误差。因此，在设计区域跟踪控制律的过程中，必须要考虑到上述建模误差以及辨识误差。在本专利中，假定上述建模误差以及辨识误差的幅值以及变化率是有界的，在现有的许多水下机器人控制方法中也存在这一假设。

综上，多推进器驱动的水下机器人动力学模型一般可描述为以下二阶方程：

其中，Μ(ξ)是质量矩阵；J是艇体坐标系与大地坐标系之间速度转换矩阵，

u是各自由度上的力(位置方向上)或力矩(艏向上)；F(ξ,q)是水动力项；d(t)是建模误差以及辨识误差的总和，为未知项。

为了能获得建模误差以及辨识误差d(t)的估计值，结合水下机器人动力学近似模型以及水下机器人全状态量测信息，构建如下状态观测器：

其中，

为水下机器人艇体坐标系速度的估计值，

是建模误差以及辨识误差d(t)的估计值，

k_i(i＝1,2,3,4)为设计参数，需满足以下条件：

其中，ρ_H为

的上边界，即：

且ρ_H已知。

接下来论证

的有限时间收敛特性。为此，根据构建的状态观测器(15)以及水下机器人动力学模型(14)可以得到以下误差动态方程：

根据现有成果，公式(17)以及k_i(i＝1,2,3,4)满足的不等式条件(16)，可证明出

和

能在有限时间内收敛于零，证明过程在此忽略。

通过公式(15)，得到了广义不确定性项(建模误差以及辨识误差)的估计律。

步骤(4)：结合步骤(3)给出的广义不确定性项的估计结果以及水下机器人近似动力学模型，推导区域跟踪控制律，并验证基于所设计的控制律作用下，水下机器人位置跟踪误差能始终保持在事先设定的环形期望区域范围内，而艏向跟踪误差始终保持在圆形期望区域范围内。

为推导可实现位置跟踪误差幅值的环形区域跟踪以及艏向角度跟踪误差的圆形期望区域跟踪的控制律，设计如下Lyapunov函数：

综合步骤(1)-(3)的结果，Lyapunov函数V_T的时间导数可描述为：

为了使V_T有界，设计控制律u满足以下形式：

其中，β为大于1/4的参数。

接下来论证V_T的有界性。将设计的控制律u带入公式(19)得到

根据Lyapunov稳定性理论，公式(21)表明V_T是有界的，进而说明V₁和V₂均有界。因此，根据Lyapunov函数V₁和V₂的表达式，在本专利设计的控制律作用下，水下机器人位置跟踪误差矢量的幅值一直维持在事先设定的环形期望区域范围内，而艏向角度误差一直维持在事先设定的圆形期望区域范围内。

应用案例

为验证本发明专利所设计的一种水下机器人位置跟踪误差矢量幅值环形期望区域跟踪控制方法的有效性，在Matlab/Simulink环境下进行仿真验证实验，其中选择全驱动水下机器人ODIN AUV作为被控对象。定义如下期望轨迹：

ξ_d＝[x_d y_d z_d φ_d]^T (21)

其中，

在仿真实验验证过程中，建模误差以及辨识误差按被控对象动力学模型的30％模拟，模拟的海流均值为0.33、标准偏差为0.079。本实施例设定的位置环形期望区域边界为：下边界ε_1b＝0.2²；上边界ε_1a＝(2.4e^-0.1t+0.6)²；艏向角度的圆形期望区域边界为：ε_2a＝(0.4e^-0.1t+0.1)²。水下机器人的初始状态需在设定的期望区域范围内，本仿真实验中，水下机器人的初始位置/姿态为[1,1,-1,π/9]^T；水下机器人的初始速度为0.01×[4,4,4,2]^T。

利用Matlab/Simulink仿真平台得到的仿真结果分别如图2-5所示。

图3给出的位置跟踪误差矢量幅值，也就是水下机器人当前位置与相对应期望点之间的距离，一直维持在事先设定的上下边界之中。从三维空间角度来看，水下机器人的中心点一直维持在以当前期望点为中心的环形区域内。图4展示了在本专利所设计的控制律下艏向角度跟踪误差一直维持在以零为中心的圆形期望区域范围内。结合图3和图4，仿真实验结果说明了当水下机器人初始位置误差矢量的幅值和艏向角度初始误差分别在给定的期望区域范围内的前提下，本专利所设计的控制方法能够让位置跟踪误差矢量的幅值始终维持在事先设定的环形期望区域内，同时艏向角度误差也能一直保持在预先设定的期望区域范围内。图5是本专利所设计的控制方法给出的各自由度方向上的控制量信号。图2则是水下机器人各自由度上的跟踪误差。结合图2，图3和图5可以看出：本专利设计的区域跟踪控制方法关注的是位置跟踪误差矢量的幅值是否在设定的环形期望区域范围内，而不关注单个自由度的跟踪精度。

综上，现有区域跟踪控制方法研究每个自由度上跟踪误差的圆形期望区域跟踪问题，而未从距离的概念研究区域跟踪控制问题。为此，本发明专利提出了一种水下机器人位置跟踪误差矢量幅值环形期望区域跟踪控制方法，基于该方法，可确保水下机器人当前位置点到相对应期望点之间的距离可始终维持在事先设定的上下边界之内，而艏向角度跟踪误差可始终保持在以零为中心的圆形期望区域范围内。因此本专利特别适合于此类区域跟踪任务。本专利可进一步扩展为水下机器人位置距离误差的环形期望区域跟踪以及方位角度上的圆形期望区域跟踪，同时，若实际任务中需要强化某位置自由度的贡献，可引入正定对角阵来作为加权矩阵，如将

改为

其中P为正定对角阵，进而以此推导强化某自由度后的环形区域跟踪控制律。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (2)

1.一种适用于水下机器人位置环形区域跟踪控制方法，其特征在于，包括以下步骤：

步骤1：获取水下机器人当前t时刻位置信息

艏向角ξ₂＝φ以及速度信息

(x,y,z)为水下机器人在大地坐标系下的坐标；

J₁为旋转变换矩阵；u、v、w为水下机器人艇体坐标系下的纵向、横向以及垂向的线速度；

r为水下机器人艇体坐标系下的艏向角速度；

步骤2：根据任务需求获取期望轨迹

设定期望环形区域的上边界ε_1a和下边界ε_1b以及期望圆形区域的边界ε_2a；其中，

为期望路径点；ξ_d2＝φ_d为期望艏向角；ε_1b设定为常数，ε_1b＞0；ε_1a(t)与ε_2a(t)均设定为指数衰减函数，ε_1a(t)＞ε_1b，ε_2a(t)＞0；

步骤3：计算虚拟控制量

其中，z_a1＝ξ₁-ξ_d1，

z_b1＝ξ₂-ξ_d2；

步骤4：根据上一时刻的跟踪控制律u(t-1)＝[F_x(t-1)F_y(t-1)F_z(t-1)M_φ(t-1)]^T，由状态观测器获取建模误差以及辨识误差的估计值

其中，F_x、F_y、F_z分别为纵向、横向、垂向自由度上所需的力，M_φ为转艏力矩；

M(ξ)为质量矩阵；F(ξ,q)为水动力项；

为水下机器人艇体坐标系下线速度及艏向角速度的估计值；k₁、k₂、k₃、k₄为设计参数，且满足

k₂＞0，k₃＞ρ_H，

ρ_H为

的上边界，

步骤5：根据虚拟控制量

和建模误差以及辨识误差的估计值

计算当前时刻的跟踪控制律u(t)＝[F_x(t) F_y(t) F_z(t) M_φ(t)]^T；

其中，

z_a2＝q₁-α₁，z_b2＝q₂-α₂；β为大于1/4的参数；

步骤6：水下机器人根据跟踪控制律u(t)执行环形区域跟踪控制；

步骤7：判断是否到达终点；若未到达终点，则返回步骤1，继续执行环形区域跟踪。

2.根据权利要求1所述的一种适用于水下机器人位置环形区域跟踪控制方法，其特征在于：所述的步骤2中

其中，ρ_∞1、ρ_∞2、ρ₀₁、ρ₀₂、λ₁、λ₂为设计参数且均大于零。

Images