CN112947067A - 一种水下机器人三维轨迹精确跟踪控制方法 - Google Patents

Abstract

本发明公开了一种水下机器人三维轨迹精确跟踪控制方法，包括以下步骤：建立水下机器人数学模型；构建跟踪误差系统方程；设计非奇异终端滑模面；设计有限时间扰动观测器；设计控制器。本发明设计了有限时间扰动观测器可实现对外界时变扰动的准确估计，为了补偿扰动，本发明提出了一种基于有限时间观测器的非奇异终端滑模控制方法，不仅补偿了时变扰动对水下机器人的影响，并且所设计的控制器也在有限时间内使跟踪误差镇定至零。同时采用幂次趋近律，有效降低了该控制方法所产生的抖振。本发明提出了基于有限时间扰动观测器的非奇异终端滑模控制策略，解决了复杂多维度时变扰动下的水下机器人精确跟踪问题。

Description

一种水下机器人三维轨迹精确跟踪控制方法

技术领域

本发明属于水下机器人运动控制领域，尤其涉及一种复杂扰动下采用非奇异终端滑模控制策略的水下机器人三维轨迹精确跟踪控制方法。

背景技术

水下机器人高精度轨迹跟踪控制是完成水下任务的前提，但是由于水下机器人是一个多输入多输出的非线性系统，各自由度间交叉耦合，并且在水中也会受到周围环境的干扰，因此设计性能良好的三维轨迹跟踪控制器尤为重要。

为了解决时变干扰对水下机器人的三维轨迹跟踪控制问题，学者QIAO L,ZHANGW.发表了Double-loop chattering-free adaptive integral sliding mode controlfor underwater vehicles[C]//MTS/IEEE Oceans 16,Shanghai China.IEEE,2016，在建立水下机器人运动学动力学方程的基础上，在控制器设计环节提出了一种自适应非奇异积分终端滑模控制策略，该策略消除了传统滑模存在的奇异性问题，在完成控制器设计以后，建立速度跟踪误差方程和位置跟踪误差方程，让前者在有限时间内局部收敛到零，让后者能够局部指数收敛到零。

外界时变干扰是影响水下机器人精确轨迹跟踪的重要因素，时变干扰具有随机性，无法对其进行预测，严重的时变干扰甚至会导致水下机器人偏离期望轨迹。

发明内容

为解决现有技术存在的上述问题，本发明要设计一种不仅能补偿时变扰动对水下机器人的影响，并且能在有限时间内使跟踪误差镇定至零，同时能有效降低抖振的水下机器人三维轨迹精确跟踪控制方法。

为了实现上述目的，本发明的基本思路如下：

首先根据水下机器人的惯性坐标系和附体坐标系建立其运动学和动力学方程，根据上述方程进一步建立位姿及速度误差跟踪方程。为了保证系统有限时间收敛的特性，根据误差方程设计了非奇异终端滑模控制律，在此基础上，为了降低抖振，采用了幂次趋近律，在上述设计过程中，由于水下复杂时变扰动仍是未知的，因此设计有限时间扰动观测器用于得到扰动的估计值，实现了在有限时间内扰动实际值等于观测值。最后，利用Lyapunov函数证明了系统在该控制律下的稳定性。通过MATLAB仿真实验，结果表明本发明能够实现三维轨迹的精确跟踪。

本发明的技术方案如下：一种水下机器人三维轨迹精确跟踪控制方法，包括以下步骤：

A、建立水下机器人数学模型

在水下机器人惯性坐标系和附体坐标系下使用动力学建模方法得到六自由度数学模型，考虑水下机器人在运动过程中横滚角变化较小，因此忽略其对水下机器人的影响，建立水下机器人五自由度数学模型如下：

式中：

在惯性坐标系下，η＝[x,y,z,θ,ψ]^T，x为水下机器人纵向位移，y为横向位移，z为垂向位移，θ为俯仰角，ψ为偏航角；

在附体坐标系下，υ＝[u,v,w,q,r]^T，u为纵向速度，v为横向速度，w为垂向速度，q为俯仰角速度，r为偏航角速度；

τ＝[τ₁,τ₂,τ₃,τ₄,τ₅]^T，τ₁、τ₂、τ₃分别为纵向控制力、横向控制力、垂向控制力，τ₄为俯仰角控制力矩，τ₅为偏航角控制力矩。

τ_δ＝MJ^-1(η)δ(t)表示在附体坐标系下各自由度所受到的外界扰动；其中，δ(t)＝[δ₁,δ₂,δ₃,δ₄,δ₅]^T，代表外界扰动是随时间变化的，t是时间参数自变量，外界扰动作用在水下机器人五个自由度上，δ₁、δ₂、δ₃、δ₄和δ₅分别为纵向扰动、横向扰动、垂向扰动、绕y轴扰动和绕z轴扰动；

M为质量与附加质量矩阵，且有M＝M^T＞0，C(υ)为科氏向心力矩阵，D(υ)表示阻尼矩阵，g(η)为恢复力矩阵，J(η)表示惯性坐标系与附体坐标系转换矩阵，具体描述如下：

D(υ)＝diag(X_u+X_u|u|u|,Y_v+Y_v|v|v|,Z_w+Z_w|w|w|,M_q+M_q|q|q|,N_r+N_r|r|r) (4)

g(η)＝diag((W-B)sinθ,-(W-B)cosθsinθ,0,-z_BBsinθ,0) (5)

式中：m为水下机器人的质量，I_y、I_z分别为附体坐标系下y轴的转动惯量、z轴的转动惯量，

分别为横向、纵向、垂向、俯仰角和航向角五个自由度的水动力导数，X_u、Y_v、Z_w、M_q、N_r分别为横向、纵向、垂向、俯仰角和航向角五个自由度的一阶阻尼系数，X_uu、Y_vv、Z_ww、M_qq、N_rr分别为横向、纵向、垂向、俯仰角和航向角五个自由度的二阶阻尼系数，W和B分别为水下机器人所受到的重力和浮力，z_B为附体坐标系下浮心在z轴的坐标，即浮心的高度。

B、构建跟踪误差系统方程

设计期望轨迹数学模型如下：

式中：η_d＝[x_d,y_d,z_d,θ_d,ψ_d]^T表示惯性坐标系下的期望纵向位移、横向位移、垂向位移以及期望俯仰角、航向角，υ_d＝[u_d,v_d,w_d,q_d,r_d]^T表示附体坐标系下期望纵向速度、横向速度、垂向速度以及期望俯仰角速度、偏航角速度，τ_d＝[τ_d1,τ_d2,τ_d3,τ_d4,τ_d5]^T表示在期望轨迹下的纵向控制力、横向控制力、垂向控制力、俯仰角控制力矩和偏航角控制力矩，C_d(υ_d)表示期望速度下的科氏向心力矩阵，D_d(υ_d)表示期望速度下的阻尼矩阵，g_d(η_d)为期望位姿下的恢复力矩阵，J_d(η_d)表示期望位姿下的惯性坐标系与附体坐标系转换矩阵。

为了便于说明控制器的设计过程，定义以下状态变量：

式中：

其中

分别为水下机器人在惯性坐标系下的纵向速度、横向速度、垂向速度与俯仰角速度及航向角角速度的实际值，

分别为水下机器人在惯性坐标系下期望轨迹对应的纵向速度、横向速度、垂向速度与俯仰角速度及航向角角速度的期望值。

结合状态变量方程，将式(1)改写为：

式中：

相同的，将式(7)改写为：

式中：

结合式(9)和式(10)，得跟踪误差系统方程：

式中：Ω_e＝Ω-Ω_d-J(η_d)M^-1τ_d，

C、设计非奇异终端滑模面

针对水下机器人跟踪误差系统方程(11)，设计非奇异终端滑模面如下：

式中：k＞0为常数，p、q均为正奇数，且有q/p∈(1,2)。

对式(12)两端求导，并结合式(11)得：

式中：求导后为保证维度正确，将

改写为

其中，diag(·)表示对角矩阵。

D、设计有限时间扰动观测器

假设1：外界扰动δ满足下式：

式中：H_δ＞0且为常数。

针对外界多维度时变扰动，基于假设1设计有限时间扰动观测器，如下式所示：

式中：

式中：z₀表示速度误差的估计值，z₁示外界扰动的观测值，z₃表示外界扰动一阶导数的估计值，其中z_i∈R^5x1，i＝0,1,2；增益系数：μ_i＞0，i＝1,2,3；有限时间扰动观测器参数：N＝diag(n₁,n₂,n₃,n₄,n₅)；另有sig^θ(t)＝|t|^θsign(t)。

E、设计控制器

根据所设计的滑模面(12)和有限时间扰动观测器(15)，控制器设计为：

τ＝τ_eq+τ_s (17)

其中，等效控制项为：

鲁棒控制项为：

τ_s＝-MJ^-1(η)κ|s|^βsign(s) (19)

式中：为保证有限时间内到达滑模面，选取幂次趋近律：

其中，κ＝diag(κ₁,κ₂,κ₃,κ₄,κ₅)为常数对角矩阵，κ_i＞0，i＝1,2,3,4,5；β为常数，且0＜β＜1；sign(s)＝[sign(s₁),sign(s₂),sign(s₃),sign(s₄),sign(s₅)]^T，sign(·)表示符号函数，并且有以下性质：

下面对本发明进行系统稳定性分析：

引理1：考虑非线性系统：

式中：x(t)＝[x₁,…,x_n]^T为系统n维的状态向量，f(·)为原点邻域上的非线性系统，且f(0)＝0。若存在函数V(x,t)满足：

(1)V(x,t)整定，

负定，则系统在原点处为渐进稳定的；

(2)V(x,t)正定，

半负定，除原点之外，

不恒为零，则系统在原点处是渐进稳定的；

(3)V(x,t)正定，

半负定，且当||x||→∞时，V(x,t)→∞，则系统在原点处是大范围渐进稳定的。

引理2：考虑以下系统：

如果系统满足上述条件，则其有限时间稳定，其中β_i＞0(i＝0,1,...,n)，L＞0。

定理1：针对假设1下的多维度时变复杂扰动，设计的有限时间扰动观测器实现对扰动的观测，补偿扰动对系统的影响，提高鲁棒性。

现证明如下：

对于所设计的有限时间扰动观测器(15)，定义观测误差方程，即：

式(23)等号两端求导，结合式(15)和式(16)得：

式(24)还可以写成：

由引理2知，式(22)有限时间稳定，即所设计的有限时间扰动观测器可以在有限时间内对扰动进行观测。并且在有限时间内有：

因此可以得到观测误差z₁-δ≡0。

证毕。

定理2：考虑复杂多维度时变扰动下水下机器人三维轨迹跟踪，在控制律τ作用下，系统可以在有限时间内到达滑动面s(t)＝0，使位姿跟踪误差被镇定到零。

现证明如下：

结合定理1，将式(17)-式(19)代入式(13)整理后得：

定义Lyapunov函数：

对式(29)求导，并将式(28)代入得：

令：

当

时，由于q/p-1＞0，k＞0，β＞0且q，p为正奇数，可以得到Q为正定矩阵，即λ_min(Q)≥0。那么式(30)可以改写为：

根据引理1可知，该系统渐进稳定。进一步证明系统有限时间稳定，令：

ρ＝2^(β+1)/2λ_min(Q) (33)

结合式(29)与式(32)-(33)，整理后得：

由于0＜β＜1，得1/2＜(β+1)/2＜1，根据引理2可知，系统有限时间收敛。当

时，根据式(17-19)和式(11)，有：

由上式可得，当s_i＞0，

快速减小；当s_i＜0时，

快速增大。因此，当

时，在有限时间内有s(t)＝0。即，跟踪误差η_e和速度跟踪误差

在有限时间内到达滑模面。

根据上述证明可知，本发明所设计的基于有限时间观测器的非奇异终端滑模控制方法(non-singular terminal sliding mode control based on finite-timedistribute observer，FTDO-NTSMC)可以使水下机器人在有限时间实现三维轨迹的精确跟踪，系统跟踪误差可以在有限时间内被镇定到零。

与现有技术相比，本发明具有以下有益效果：

1、本发明设计了有限时间扰动观测器可实现对外界时变扰动的准确估计，为了补偿扰动，本发明提出了一种基于有限时间观测器的非奇异终端滑模控制方法(non-singular terminal sliding mode control based on finite-time distributeobserver，FDO-NTSMC)，不仅补偿了时变扰动对水下机器人的影响，并且所设计的控制器也在有限时间内使跟踪误差镇定至零。同时采用幂次趋近律，有效降低了该控制方法所产生的抖振。

2、本发明提出了基于有限时间扰动观测器的非奇异终端滑模(FDO-NTSMC)控制策略，解决了复杂多维度时变扰动下的水下机器人精确跟踪问题。通过有限时间扰动观测器的设计，实现了对外界时变扰动的估计并对其进行了补偿。用幂次趋近律取代以往使用的等速趋近律，降低了非奇异终端滑模NTSMC所产生的抖振，使得跟踪误差在有限时间内镇定到零。

附图说明

图1是水下机器人坐标系示意图。

图2是三维轨迹跟踪曲线。

图3是纵向位移状态曲线。

图4是横向位移状态曲线。

图5是垂向位移状态曲线。

图6是俯仰角状态曲线。

图7是偏航角状态曲线。

图8是纵向跟踪误差曲线。

图9是横向跟踪误差曲线。

图10是垂向跟踪误差曲线。

图11是俯仰角跟踪误差曲线。

图12是偏航角跟踪误差曲线。

图13是纵向控制力输入曲线。

图14是横向控制力输入曲线。

图15是垂向控制力输入曲线。

图16是俯仰角控制力矩输入曲线。

图17是偏航角控制力矩输入曲线。

图18是纵向速度状态曲线。

图19是横向速度状态曲线。

图20是垂向速度状态曲线。

图21是俯仰角速度状态曲线。

图22是偏航角速度状态曲线。

图23是纵向速度跟踪误差曲线。

图24是横向速度跟踪误差曲线。

图25是垂向速度跟踪误差曲线。

图26是俯仰角速度跟踪误差曲线。

图27是偏航角速度跟踪误差曲线。

图28是纵向扰动及其观测曲线。

图29是横向扰动及其观测曲线。

图30是垂向扰动及其观测曲线。

图31是俯仰扰动力矩及其观测曲线。

图32是偏航扰动力矩及其观测曲线。

具体实施方式

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

本发明先是根据图1所示的水下机器人坐标系建立5自由度的动力学和运动学方程，在完成整体设计以后，设计一条期望三维轨迹曲线(图2虚线)，图2实线为水下机器人根据设计的控制器跟踪图2虚线所形成曲线，图3-7是将图2展开到每一个具体位姿下随时间跟踪期望的效果图。图8-12是在图3-7的基础上，将实际值与给定期望值做差得到的图，在水下机器人跟踪期望路径的过程中，改变控制输入的值即可实现水下机器人轨迹跟踪。图13-17虚线即为相应期望的5个控制力及力矩的值，实线为实际产生的控制力及力矩形成的曲线，图18-22是水下机器人在运动过程中的期望速度及角速度与实际速度及角速度。虚线为期望值。图23-27是实际速度及角速度与期望速度及角速度之间的差形成的图。图28-32中的虚线是水下机器人在各个自由度上受到的干扰力及力矩，实线是采用有限时间扰动观测器以后对各个自由度上的估计值。

本发明不局限于本实施例，任何在本发明披露的技术范围内的等同构思或者改变，均列为本发明的保护范围。

Claims (1)

1.一种水下机器人三维轨迹精确跟踪控制方法，其特征在于：包括以下步骤：

A、建立水下机器人数学模型

在水下机器人惯性坐标系和附体坐标系下使用动力学建模方法得到六自由度数学模型，考虑水下机器人在运动过程中横滚角变化较小，因此忽略其对水下机器人的影响，建立水下机器人五自由度数学模型如下：

式中：

在惯性坐标系下，η＝[x,y,z,θ,ψ]^T，x为水下机器人纵向位移，y为横向位移，z为垂向位移，θ为俯仰角，ψ为偏航角；

在附体坐标系下，υ＝[u,v,w,q,r]^T，u为纵向速度，v为横向速度，w为垂向速度，q为俯仰角速度，r为偏航角速度；

τ＝[τ₁,τ₂,τ₃,τ₄,τ₅]^T，τ₁、τ₂、τ₃分别为纵向控制力、横向控制力、垂向控制力，τ₄为俯仰角控制力矩，τ₅为偏航角控制力矩；

τ_δ＝MJ^-1(η)δ(t)表示在附体坐标系下各自由度所受到的外界扰动；其中，δ(t)＝[δ₁,δ₂,δ₃,δ₄,δ₅]^T，代表外界扰动是随时间变化的，t是时间参数自变量，外界扰动作用在水下机器人五个自由度上，δ₁、δ₂、δ₃、δ₄和δ₅分别为纵向扰动、横向扰动、垂向扰动、绕y轴扰动和绕z轴扰动；

M为质量与附加质量矩阵，且有M＝M^T＞0，C(υ)为科氏向心力矩阵，D(υ)表示阻尼矩阵，g(η)为恢复力矩阵，J(η)表示惯性坐标系与附体坐标系转换矩阵，具体描述如下：

D(υ)＝diag(X_u+X_u|u||u|,Y_v+Y_v|v||v|,Z_w+Z_w|w||w|,M_q+M_q|q||q|,N_r+N_r|r||r|) (4)

g(η)＝diag((W-B)sinθ,-(W-B)cosθsinθ,0,-z_BBsinθ,0) (5)

式中：m为水下机器人的质量，I_y、I_z分别为附体坐标系下y轴的转动惯量、z轴的转动惯量，

分别为横向、纵向、垂向、俯仰角和航向角五个自由度的水动力导数，X_u、Y_v、Z_w、M_q、N_r分别为横向、纵向、垂向、俯仰角和航向角五个自由度的一阶阻尼系数，X_u|u|、Y_v|v|、Z_w|w|、M_q|q|、N_r|r|分别为横向、纵向、垂向、俯仰角和航向角五个自由度的二阶阻尼系数，W和B分别为水下机器人所受到的重力和浮力，z_B为附体坐标系下浮心在z轴的坐标，即浮心的高度；

B、构建跟踪误差系统方程

设计期望轨迹数学模型如下：

式中：η_d＝[x_d,y_d,z_d,θ_d,ψ_d]^T表示惯性坐标系下的期望纵向位移、横向位移、垂向位移以及期望俯仰角、航向角，υ_d＝[u_d,v_d,w_d,q_d,r_d]^T表示附体坐标系下期望纵向速度、横向速度、垂向速度以及期望俯仰角速度、偏航角速度，τ_d＝[τ_d1,τ_d2,τ_d3,τ_d4,τ_d5]^T表示在期望轨迹下的纵向控制力、横向控制力、垂向控制力、俯仰角控制力矩和偏航角控制力矩，C_d(υ_d)表示期望速度下的科氏向心力矩阵，D_d(υ_d)表示期望速度下的阻尼矩阵，g_d(η_d)为期望位姿下的恢复力矩阵，J_d(η_d)表示期望位姿下的惯性坐标系与附体坐标系转换矩阵；

为了便于说明控制器的设计过程，定义以下状态变量：

式中：

其中

分别为水下机器人在惯性坐标系下的纵向速度、横向速度、垂向速度与俯仰角速度及航向角角速度的实际值，

分别为水下机器人在惯性坐标系下期望轨迹对应的纵向速度、横向速度、垂向速度与俯仰角速度及航向角角速度的期望值；

结合状态变量方程，将式(1)改写为：

式中：

相同的，将式(7)改写为：

式中：

结合式(9)和式(10)，得跟踪误差系统方程：

式中：

C、设计非奇异终端滑模面

针对水下机器人跟踪误差系统方程(11)，设计非奇异终端滑模面如下：

式中：k＞0为常数，p、q均为正奇数，且有q/p∈(1,2)；

对式(12)两端求导，并结合式(11)得：

式中：求导后为保证维度正确，将

改写为

其中，diag(·)表示对角矩阵；

D、设计有限时间扰动观测器

假设1：外界扰动δ满足下式：

式中：H_δ＞0且为常数；

针对外界多维度时变扰动，基于假设1设计有限时间扰动观测器，如下式所示：

式中：

式中：z₀表示速度误差的估计值，z₁示外界扰动的观测值，z₃表示外界扰动一阶导数的估计值，其中z_i∈R^5x1，i＝0,1,2；增益系数：μ_i＞0，i＝1,2,3；有限时间扰动观测器参数：N＝diag(n₁,n₂,n₃,n₄,n₅)；另有sig^θ(t)＝|t|^θsign(t)；

E、设计控制器

根据所设计的滑模面(12)和有限时间扰动观测器(15)，控制器设计为：

τ＝τ_eq+τ_s (17)

其中，等效控制项为：

鲁棒控制项为：

τ_s＝-MJ^-1(η)κ|s|^βsign(s) (19)

式中：为保证有限时间内到达滑模面，选取幂次趋近律：

其中，κ＝diag(κ₁,κ₂,κ₃,κ₄,κ₅)为常数对角矩阵，κ_i＞0，i＝1,2,3,4,5；β为常数，且0＜β＜1；sign(s)＝[sign(s₁),sign(s₂),sign(s₃),sign(s₄),sign(s₅)]^T，sign(·)表示符号函数，并且有以下性质：

Images