CN112286227B - 基于微分跟踪器和扰动观测器的水下机器人轨迹控制方法及系统 - Google Patents

Abstract

本发明公开了一种基于微分跟踪器和扰动观测器的水下机器人轨迹控制方法及系统，包括以下步骤：步骤1，构建获得水下机器人动力学模型；步骤2，构建获得水下机器人速度观测器；步骤3，构建获得水下机器人扰动观测器；步骤4，基于步骤2获得的水下机器人速度观测器和步骤3获得的水下机器人扰动观测器，构建获得水下机器人轨迹跟踪控制器和水下机器人位置控制器；基于获得的水下机器人轨迹跟踪控制器和水下机器人位置控制器实现水下机器人轨迹控制。本发明能够有效抑制水下机器人的位置控制中的扰动，提高轨迹跟踪系统的鲁棒性；能够使得控制更加精确。

Description

基于微分跟踪器和扰动观测器的水下机器人轨迹控制方法及 系统

技术领域

本发明属于水下机器人控制技术领域，涉及水下机器人的位置和轨迹控制领域，特别涉及一种基于微分跟踪器和扰动观测器的水下机器人轨迹控制方法及系统。

背景技术

海洋资源在人类发展过程中发挥越来越重要的作用，水下机器人由于其导航定位精度高、能源密度高、信息处理方式智能、运动控制与规划决策实时性高，取代了人工潜水，为人类进行深海资源的研究与开发提供了强有力的工具；水下机器人是融合了人工智能、信息识别、智能处理、自主决策、姿态位置控制的嵌入式系统。水下机器人的总体结构、流体性能、动力系统、控制与通讯方式之间存在强耦合，模型的非线性度也很高；此外，受压力、水密、负载和速度影响，水下机器人易受外界条件影响。

综上，亟需一种新的水下机器人轨迹控制方法及系统。

发明内容

本发明的目的在于提供一种基于微分跟踪器和扰动观测器的水下机器人轨迹控制方法及系统，以解决上述存在的一个或多个技术问题。本发明能够有效抑制水下机器人的位置控制中的扰动，提高轨迹跟踪系统的鲁棒性；能够使得控制更加精确。

为达到上述目的，本发明采用以下技术方案：

本发明的一种基于微分跟踪器和扰动观测器的水下机器人轨迹控制方法，包括以下步骤：

步骤1，构建获得水下机器人动力学模型；

步骤2，基于步骤1获得的动力学模型，构建获得水下机器人速度观测器；

步骤3，基于步骤1获得的动力学模型，构建获得水下机器人扰动观测器；

步骤4，基于步骤2获得的水下机器人速度观测器和步骤3获得的水下机器人扰动观测器，构建获得水下机器人轨迹跟踪控制器和水下机器人位置控制器；基于获得的水下机器人轨迹跟踪控制器和水下机器人位置控制器实现水下机器人轨迹控制。

本发明的进一步改进在于，步骤1具体包括：

水下机器人在大地坐标下的位置量为

其中，x、y和z是水下机器人的位置坐标，φ、θ和

是水下机器人的姿态角度；

机体坐标系下，水下机器人的速度量为ν＝[μ υ ω p q r]^T；其中，μ、υ和ω为水下机器人的沿机体坐标轴的线速度，p、q和r是水下机器人绕机体坐标轴的角速度，方向满足右手螺旋定理；

为沿大地坐标轴的速度向量，

与ν的转换关系：

对水下机器人运用动量与动量矩定理，建立如下的运动方程：

其中，M(η)是模型的惯性矩阵，C(ν)是科氏力矩阵，D(ν)代表系统阻尼矩阵，g(η)是负浮力，τ_C表示控制量；

系统(1)和系统(2)联立获得水下机器人动力学模型：

其中，M_η(η)＝J^-T(η)MJ^-1(η)，C_η(ν,η)＝J^-T(η)[C(ν)-MJ^-1(η)]J^-1(η)，g_η(η)＝J^-T(η)g(η)，D_η(ν,η)＝J^-T(η)D(ν)J^-1(η)和τ_C＝J^-T(η)τ。

本发明的进一步改进在于，步骤2中，所述水下机器人速度观测器包括：微分跟踪器和扩张状态观测器；

所述微分跟踪器为：

其中，R>0是一个正值，f(·)满足如下约束：

f(z₁(t),z₂(t))＝-mz₁(t)-2αz₂(t)

所述扩张状态观测器为：

所述水下机器人速度观测器产生的速度观测值记为

和

二维速度迭代公式为：

。

本发明的进一步改进在于，步骤3中，所述水下机器人扰动观测器为：

其中，

是d_j(t)的估计，ξ_j(t)是一个中间变量，k是一个正数，kl(e_λj(t))是反馈增益系数矩阵；

其中，π₁和π₂是两个系数，并且满足π₁＞π₂＞0；

反馈增益矩阵

。

本发明的进一步改进在于，步骤4中，所述水下机器人轨迹跟踪控制器为：

其中，k_P＞0是一个可调参数；j为第j个跟随者。

本发明的进一步改进在于，步骤4中，所述水下机器人位置控制器中，

x轴方向控制时，令

其中，

是当前位置和目标位置的偏差，

虑是当前的平移速度和目标平移速度，τ_1i(t)是x轴方向的虚拟控制量；

位置子系统记为：

虚拟控制率τ_1i(t)为：

其中，α_1i，k_1i，l_1i和β_1i是四个可调的正参数。

本发明的进一步改进在于，步骤4的所述水下机器人位置控制器中，x、y、z轴方向控制方式相同。

本发明的一种基于微分跟踪器和扰动观测器的水下机器人轨迹控制系统，包括：

动力学模型获取模块，用于构建获得水下机器人动力学模型；

速度观测器获取模块，用于根据动力学模型获取模块获得的动力学模型，构建获得水下机器人速度观测器；

扰动观测器获取模块，用于根据动力学模型获取模块获得的动力学模型，构建获得水下机器人扰动观测器；

控制器获取模块，用于根据获得的水下机器人速度观测器和水下机器人扰动观测器，构建获得水下机器人轨迹跟踪控制器和水下机器人位置控制器；基于获得的水下机器人轨迹跟踪控制器和水下机器人位置控制器实现水下机器人轨迹控制。

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

本发明提出了一种基于微分跟踪器和扰动观测器的水下机器人轨迹控制方法，研究了用于水下机器人控制的轨迹规划控制器和位置跟踪控制器。首先，本发明设计了一种跟踪微分器，对由领航者和跟随者组成的水下机器人进行速度信号的生成；其次，发明设计了一种扰动观测器，实时观测抑制位置扰动；最后，本发明基于领航者与跟随者之间的状态误差，利用非线性扰动观测器的轨迹跟踪控制器被设计，领航者与跟随者的位置控制器采用了反三角函数的饱和律。综上，本发明的方法利用基于扰动观测器的轨迹跟踪控制器，有效的抑制水下机器人的位置控制中的扰动，提高了轨迹跟踪系统的鲁棒性；速度观测器，分考虑了模型信息，使得控制更加精确，便于工程实现；位置虚拟控制率使用了三角函数，满足工程实际中“小偏差，大增益；大偏差，小增益”的调参经验，便于工程实现。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面对实施例或现有技术描述中所需要使用的附图做简单的介绍；显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来说，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例的一种基于微分跟踪器和扰动观测器的水下机器人轨迹控制方法的流程示意图。

具体实施方式

为使本发明实施例的目的、技术效果及技术方案更加清楚，下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述；显然，所描述的实施例是本发明一部分实施例。基于本发明公开的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的其它实施例，都应属于本发明保护的范围。

请参阅图1，本发明实施例的一种基于微分跟踪器和扰动观测器的水下机器人轨迹控制方法，其构建了速度微分跟踪器，设计水下机器人的扰动观测器，设计非线性位置控制器通过以下步骤实现：

步骤1，构建水下机器人动力学模型，包括：

水下机器人在大地坐标下的位置量为

其中x、y、z是水下机器人的位置坐标，φ、θ、

是水下机器人的姿态角度；机体坐标系下，水下机器人的速度量为ν＝[μ υ ω p q r]^T，其中，μ、υ、ω为机器人的沿机体坐标轴的线速度，p、q、r是机器人绕机体坐标轴的角速度，方向满足右手螺旋定理；

为沿大地坐标轴的速度向量，通过转换矩阵J(η)，可以获得

与ν的转换关系：

对水下机器人运用动量与动量矩定理，可以建立如下的运动方程：

其中，M(η)是该模型的惯性矩阵，C(ν)是科氏力矩阵，D(ν)代表系统阻尼矩阵，g(η)是负浮力，τ_C表示控制量。

系统(1)和系统(2)联立可得：

其中，M_η(η)＝J^-T(η)MJ^-1(η)，C_η(ν,η)＝J^-T(η)[C(ν)-MJ^-1(η)]J^-1(η)，g_η(η)＝J^-T(η)g(η)，C_η(ν,η)＝J^-T(η)[C(ν)-MJ^-1(η)]J^-1(η)，D_η(ν,η)＝J^-T(η)D(ν)J^-1(η)和τ_C＝J^-T(η)τ；

本发明实施例中，相关的参数定义如下：

r_B＝[x_B,y_B,z_B]^T＝[0,0,0]^T，r_G＝[x_G,y_G,z_G]^T＝[0,0,0.05]^T，m＝125，

C(ν)＝C_A(ν)+C_RB(ν)，

D(ν)＝diag{48|μ|,48|υ|,48|ω|,110|p|,110|q|,110|r|}，

其中，x_B，y_B和z_B是浮心坐标，x_G，y_G和z_G表示质心坐标，m表示质量，I₀是转动惯量矩阵，ν₁＝[μ υ ω]^T和ν₂＝[p q r]^T是速度ν的平移速度和角速度分量，C_A(ν)和C_RB(ν)分别是所排流体的运动所具有的柯氏矩阵和柯氏矩阵。

步骤2，构建水下机器人速度观测器，包括：

针对系统(3)设计的微分跟踪器如下：

其中，R>0是一个正值，f(·)满足如下约束：

f(z₁(t),z₂(t))＝-mz₁(t)-2αz₂(t)

其中，扩张状态观测器设计为：

速度观测器(4)-(7)产生的速度观测值记为

和

进一步，设计的二维速度迭代公式如下：

本发明实施例中，相关的可调参数设置为：

R＝100，m＝10，α＝10。

步骤3，构建水下机器人扰动观测器，包括：

设计一个非线性扰动观测器：

其中，

是d_j(t)的估计，ξ_j(t)是一个中间变量，k是一个正数，kl(e_λj(t))是反馈增益系数矩阵，p(e_λj(t))设计如下：

其中，π₁和π₂是两个系数，并且满足π₁＞π₂＞0。

反馈增益矩阵kl(e_λj(t))设计如下：

本发明实施例中，相关的可调参数设置为：k＝0.2，π₁＝9.5，π₂＝3.6。

步骤4，构建水下机器人轨迹跟踪微分器，包括

针对第j个跟随者设计跟踪微分器：

其中，k_P＞0是一个可调参数。

本发明实施例中，相关的可调参数设置为：k_P＝400。

步骤5，构建水下机器人位置控制器，包括：

以x轴方向控制为例，考虑状态方程(3)，令

x_i(t)＝x_1i(t)

其中，

是当前位置和目标位置的偏差，

虑是当前的平移速度和目标平移速度，τ_1i(t)是x轴方向的虚拟控制量；

进一步，位置子系统记为：

考虑公式(8)(9)，虚拟控制率τ_1i(t)设计为：

其中，α_1i，k_1i，l_1i和β_1i是四个可调的正参数。

本发明实施例中，α_ix＝2.0，β_ix＝0.6，k_ix＝0.5，l_ix＝0.3；α_iy＝2.0，β_iy＝0.6，k_iy＝0.5，l_iy＝0.3。

跟踪微分器通过设置一个过渡过程来减少超调和保持快速响应。跟踪微分器可以提取任意分段光滑非线性信号的微分。线性跟踪微分器能够在线估计非正弦信号的噪声扰动频率。利用基于泰勒展开式的高增益跟踪微分器，能够有效地减少时间滞后现象。扰动观测器对外界干扰和模型失配具有较强的鲁棒性，且具有较强的计算能力和无需较高的控制增益。扰动观测器根据标称模型的知识和扰动的极限来估计扰动，水下机器人对外界环境很敏感，由于忽略了外部干扰，可能导致一般控制器的鲁棒性较差。由于扰动观测器固有的解耦功能，是控制水下机器人的一种合适方法。在此基础上，提出了一种基于领航者和跟随者的非线性轨迹跟踪控制器。基于扰动观测器的水下机器人非线性轨迹跟踪控制器，被用于水下机器人在有扰动的情况下规划目标轨迹。其次还设计了一种具有反三角函数的位置控制器，用于领航者和跟随者的位置。综上，本发明利用基于扰动观测器的轨迹跟踪控制器，有效的抑制水下机器人的位置控制中的扰动，提高了轨迹跟踪系统的鲁棒性；速度观测器，分考虑了模型信息，使得控制更加精确，便于工程实现；位置虚拟控制率使用了三角函数，满足工程实际中“小偏差，大增益；大偏差，小增益”的调参经验，便于工程实现。

本发明实施例的一种基于微分跟踪器和扰动观测器的水下机器人轨迹控制系统，包括：

动力学模型获取模块，用于构建获得水下机器人动力学模型；

速度观测器获取模块，用于根据动力学模型获取模块获得的动力学模型，构建获得水下机器人速度观测器；

扰动观测器获取模块，用于根据动力学模型获取模块获得的动力学模型，构建获得水下机器人扰动观测器；

控制器获取模块，用于根据获得的水下机器人速度观测器和水下机器人扰动观测器，构建获得水下机器人轨迹跟踪控制器和水下机器人位置控制器；基于获得的水下机器人轨迹跟踪控制器和水下机器人位置控制器实现水下机器人轨迹控制。

综上，本发明公开了一种基于微分跟踪器和扰动观测器的水下机器人轨迹控制方法及系统，涉及水下机器人的位置和轨迹控制方法，属于水下机器人控制领域；本发明的方法首先对水下机器人运用动量和矩定理并利用大地坐标系和机体坐标系间的转换关系，建立动力学模型；设计了水下机器人的速度跟踪微分器，生成由领航者和跟随者组成的水下机器人的速度信号。跟踪微分器避免了系统响应的突变，解决了快速性和超调的矛盾；其次，设计了一种扰动观测器，实时观测抑制位置扰动。最后，基于领航者与跟随者之间的状态误差，设计了利用非线性扰动观测器的轨迹跟踪控制器。领航者与跟随者的位置控制器采用了具有反三角函数的饱和控制律，适于工程应用。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员依然可以对本发明的具体实施方式进行修改或者等同替换，这些未脱离本发明精神和范围的任何修改或者等同替换，均在申请待批的本发明的权利要求保护范围之内。

Claims (3)

1.一种基于微分跟踪器和扰动观测器的水下机器人轨迹控制方法，其特征在于，包括以下步骤：

步骤1，构建获得水下机器人动力学模型；

步骤2，基于步骤1获得的动力学模型，构建获得水下机器人速度观测器；

步骤3，基于步骤1获得的动力学模型，构建获得水下机器人扰动观测器；

步骤4，基于步骤2获得的水下机器人速度观测器和步骤3获得的水下机器人扰动观测器，构建获得水下机器人轨迹跟踪控制器和水下机器人位置控制器；基于获得的水下机器人轨迹跟踪控制器和水下机器人位置控制器实现水下机器人轨迹控制；

其中，步骤1具体包括：

水下机器人在大地坐标下的位置量为

其中，x、y和z是水下机器人的位置坐标，φ、θ和

是水下机器人的姿态角度；

机体坐标系下，水下机器人的速度量为ν＝[μ υ ω p q r]^T；其中，μ、υ和ω为水下机器人的沿机体坐标轴的线速度，p、q和r是水下机器人绕机体坐标轴的角速度，方向满足右手螺旋定理；

为沿大地坐标轴的速度向量，

与ν的转换关系：

对水下机器人运用动量与动量矩定理，建立如下的运动方程：

其中，M(η)是模型的惯性矩阵，C(ν)是科氏力矩阵，D(ν)代表系统阻尼矩阵，g(η)是负浮力，τ_C表示控制量；

系统(1)和系统(2)联立获得水下机器人动力学模型：

其中，M_η(η)＝J^-T(η)MJ^-1(η)，C_η(ν,η)＝J^-T(η)[C(ν)-MJ^-1(η)]J^-1(η)，g_η(η)＝J^-T(η)g(η)，D_η(ν,η)＝J^-T(η)D(ν)J^-1(η)和τ_C＝J^-T(η)τ；

步骤2中，所述水下机器人速度观测器包括：微分跟踪器和扩张状态观测器；

所述微分跟踪器为：

其中，R>0是一个正值，f(·)满足如下约束：

f(z₁(t),z₂(t))＝-mz₁(t)-2αz₂(t)

所述扩张状态观测器为：

所述水下机器人速度观测器产生的速度观测值记为

和

二维速度迭代公式为：

；

步骤3中，所述水下机器人扰动观测器为：

其中，

是d_j(t)的估计，ξ_j(t)是一个中间变量，k是一个正数，kl(e_λj(t))是反馈增益系数矩阵；

其中，π₁和π₂是两个系数，并且满足π₁＞π₂＞0；

反馈增益矩阵

；

步骤4中，所述水下机器人轨迹跟踪控制器为：

其中，k_P＞0是一个可调参数；j为第j个跟随者；

步骤4中，所述水下机器人位置控制器中，

x轴方向控制时，令

其中，

是当前位置和目标位置的偏差，

是当前的平移速度和目标平移速度的偏差，τ_1i(t)是x轴方向的虚拟控制量；

位置子系统记为：

虚拟控制率τ_1i(t)为：

其中，α_1i，k_1i，l_1i和β_1i是四个可调的正参数。

2.根据权利要求1所述的一种基于微分跟踪器和扰动观测器的水下机器人轨迹控制方法，其特征在于，步骤4的所述水下机器人位置控制器中，x、y、z轴方向控制方式相同。

3.一种基于微分跟踪器和扰动观测器的水下机器人轨迹控制系统，其特征在于，基于权利要求1所述的方法，所述系统包括：

动力学模型获取模块，用于构建获得水下机器人动力学模型；

速度观测器获取模块，用于根据动力学模型获取模块获得的动力学模型，构建获得水下机器人速度观测器；

扰动观测器获取模块，用于根据动力学模型获取模块获得的动力学模型，构建获得水下机器人扰动观测器；

控制器获取模块，用于根据获得的水下机器人速度观测器和水下机器人扰动观测器，构建获得水下机器人轨迹跟踪控制器和水下机器人位置控制器；基于获得的水下机器人轨迹跟踪控制器和水下机器人位置控制器实现水下机器人轨迹控制。

Images