一种基于全阶终端滑模的船载机械手末端轨迹跟踪协调控制方法

技术领域

本发明涉及船载机械手末端轨迹跟踪协调控制技术，具体而言，尤其涉及一种基于全阶终端滑模的船载机械手末端轨迹跟踪协调控制方法。

背景技术

精确的跟踪控制算法依赖科学的系统模型，针对水面无人船与串联机械手的模型特性，需要参考科学的建模方法，并考虑该结构区别于一般移动式机械手模型的特征。传统的移动机械手控制思路是将移动平台和机械手系统视为两个分立的子系统，当移动平台运动时，机械手作为静态负载，移动平台运动到某目标点后固定下来，控制机械手运动从而跟踪末端的目标轨迹。这种独立建模与控制方法忽略了两个结构之间的关联与协调性。

船载机械手是将机械手安装在水面无人船(USV)上的一种新型移动式机械手的应用，一般移动式机械手是以移动平台和多关节机械手组成的集成机器人，目前应用较为广泛的移动式机械手领域主要分布于地面、水下和空间，而水面无人船是一种智能水面机器人，具有运动灵活、自主性高的优势。船载机械手结合了二者的优势，可以实现特殊水域目标跟踪、抓取等繁琐的、复杂的甚至危险的任务，极大地扩展了水面无人船的任务空间与实际功能。作为一种非线性多智能体组合，因为模型的特殊性和子系统间的强耦合动力学问题，阻碍了船载机械手末端的轨迹跟踪控制的发展和应用。

发明内容

根据上述提出的控制模型与子系统间存在强耦合的技术问题，而提供一种基于全阶终端滑模的船载机械手末端轨迹跟踪协调控制方法。本发明可以有效反映系统的完整动态，并保证两个子系统之间的协调性和有限时间稳定性。

本发明采用的技术手段如下：

一种基于全阶终端滑模的船载机械手末端轨迹跟踪协调控制方法，包括：

构建船载机械手数学模型；

基于所述船载机械手数学模型，获取惯性坐标系下的旋转矩阵、船舶模型的惯性矩阵、科里利奥向心力矩阵和阻尼矩阵，结合环境干扰信号获取三自由度无人船运动模型；

对所述船载机械手数学模型和三自由度无人船运动模型受到的不确定干扰进行整合，获得最终的船载机械手动力学模型；

基于所述船载机械手动力学模型设计基于FOTSM的轨迹跟踪控制器，包括设计基于FOTSM的无人船子系统轨迹跟踪控制器和基于FOTSM的机械手子系统轨迹跟踪控制器；

应用获得的无人船子系统轨迹跟踪控制器和机械手子系统轨迹跟踪控制器对船载机械手进行跟踪控制。

进一步地，所述构建船载机械手数学模型，包括：

以全驱动水面无人船为移动基座，在质心位置装载三自由度机械臂；

考虑惯性坐标系下，以无人船质心为原点建立第一体坐标系，并分别以机械臂的肩、大臂和小臂关节为原点分别建立第二体坐标系、第三体坐标系和第四体坐标系；

根据刚体D-H变换法则与牛顿-欧拉迭代法则，基于所述第一体坐标系，第二体坐标系、第三体坐标系和第四体坐标系得到船载机械手完整的数学模型。

进一步地，所述设计基于FOTSM的无人船子系统轨迹跟踪控制器，包括：

基于包含外界干扰与耦合非线性的复合扰动变量对对无人船子系统进行变形；

设计有限时间扰动观测器以实现对复合扰动的精确估计；

对无人船子系统的耦合误差进行镇定，所述耦合误差基于惯性坐标系下的位置和速度跟踪误差确定；

定义全阶终端滑模面以保证在控制器中不存在带有分数次幂项的状态量导数，且系统可以在有限时间内达到抗干扰稳定；

基于所述全阶终端滑模面获取基于全阶终端滑模的轨迹跟踪控制器。

进一步地，所述设计基于FOTSM的机械手子系统轨迹跟踪控制器，包括：

获取机械手子系统一阶逆运动学虚拟关节角速度作为参考信号，对械手数学模型进行变形；

设计有限时间扰动观测器以实现对复合扰动的精确估计；

对机械手子系统的耦合误差进行镇定，所述耦合误差基于状态误差变量与耦合跟踪误差变量确定；

定义全阶终端滑模面以保证滑动模面的连续性；

基于扰动观测器，设计机械手全阶终端滑模控制器。

较现有技术相比，本发明具有以下优点：

1、本发明在建立船载机械手模型的基础上，提出了一种基于全阶终端滑模的轨迹跟踪控制策略，分别控制无人船与机械手两个系统，实现各系统耦合轨迹跟踪误差的有限时间收敛。

2、本发明结合为机械手子系统提出的逆运动学虚拟输入，可以实现子系统之间的协调运动。针对系统受到的复合扰动，设计有限时间扰动观测器来进行精确观测。

相比于传统的移动机械手独立控制策略，本文所提出的协调控制方法更好地体现了两个结构的关联性，具有实际意义。相比于传统的滑模控制方法，本发明所提出的全阶终端滑模控制方法具有有限时间稳定性，克服了收敛时间较长与滑模抖振的缺点。通过仿真实验验证了所提方法的有效性，可以为后期船载机械手的实物研究提供一定理论支撑。

附图说明

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

图1为本发明跟踪协调控制方法流程图。

图2为实施例中无人船轨迹跟踪效果图。

图3为实施例中无人船位置与航向角跟踪误差效果图。

图4为实施例中无人船速度跟踪误差效果图。

图5为实施例中无人船所受扰动观测效果图。

图6为实施例中无人船耦合误差效果图。

图7为实施例中船载机械手末端轨迹跟踪效果图。

图8为实施例中船载机械手末端空间位置跟踪效果图。

图9为实施例中机械手关机角速度跟踪效果图。

图10为实施例中机械手所受扰动观测效果图。

图11为实施例中机械手耦合误差效果图。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。

需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

本文建立了船载机械手系统完整的动力学模型与末端执行器运动学表达式，考虑了机械手与无人船平台之间的动力学耦合和受到的外界干扰。在此基础上，提出了一种基于全阶终端滑模的轨迹跟踪协调控制策略，并设计有限时间扰动观测器对复合扰动部分进行观测，这种控制方法可以有效反映系统的完整动态，并保证两个子系统之间的协调性和有限时间稳定性。最后，利用有限时间稳定性原理分析并证明了闭环系统的稳定性。

如图1所示，本发明提供了一种基于全阶终端滑模的船载机械手末端轨迹跟踪协调控制方法，包括：

S1、构建船载机械手数学模型。具体包括：

S110、以全驱动水面无人船为移动基座，在质心位置装载三自由度机械臂；

S120、考虑惯性坐标系下，以无人船质心为原点建立第一体坐标系，并分别以机械臂的肩、大臂和小臂关节为原点分别建立第二体坐标系、第三体坐标系和第四体坐标系；

S130、根据刚体D-H变换法则与牛顿-欧拉迭代法则，基于所述第一体坐标系，第二体坐标系、第三体坐标系和第四体坐标系得到船载机械手完整的数学模型。

具体来说，船载机械手本质上是一种多智能体结构的组合，以全驱动水面无人船为移动基座，在质心位置装载三自由度机械臂，以实现水面领域的目标跟踪甚至抓取等任务。面对复杂的洋流干扰与两个结构之间不可忽略的耦合影响，建立完整的船载机械手数学模型是解决轨迹跟踪控制问题的基础。如图1所示，考虑惯性坐标系{O

式中，X

S2、基于所述船载机械手数学模型，获取惯性坐标系下的旋转矩阵、船舶模型的惯性矩阵、科里利奥向心力矩阵和阻尼矩阵，结合环境干扰信号获取三自由度无人船运动模型。

分析无人船作用于机械手系统的耦合力矩与风、浪干扰影响，通过牛顿第三定律，得到无人船所受机械手的耦合作用力，定义以前进、横摇与偏航平面运动为主的三自由度无人船运动模型：

式中，惯性坐标系下无人船位置为η

S3、对所述船载机械手数学模型和三自由度无人船运动模型受到的不确定干扰进行整合，获得最终的船载机械手动力学模型。

鉴于末端执行器的运动轨迹由移动平台和机械手关节运动共同影响，船载机械手末端轨迹跟踪控制目标可以表达为：通过分别设计无人船的速度与偏航控制器τ

式中，

本发明在设计过程中，遵循以下公知准则：

引理1当存在正定的Lyapunov函数V(x)满足如下不等式：

式中，当系数满足λ

引理2符合以下形式的系统参数是有限时间稳定的：

式中L＞0,λ

此外，本发明在设计过程中，做如下假设：

假设1机械手的末端执行器在其操作空间中跟踪的目标轨迹X

假设2系统中复合干扰变量可导且有上界，即

S4、基于所述船载机械手动力学模型设计基于FOTSM的轨迹跟踪控制器，包括设计基于FOTSM的无人船子系统轨迹跟踪控制器和基于FOTSM的机械手子系统轨迹跟踪控制器。

其中，设计基于FOTSM的无人船子系统轨迹跟踪控制器，包括：

S411、基于包含外界干扰与耦合非线性的复合扰动变量对对无人船子系统进行变形；

S412、设计有限时间扰动观测器以实现对复合扰动的精确估计；

S413、对无人船子系统的耦合误差进行镇定，所述耦合误差基于惯性坐标系下的位置和速度跟踪误差确定；

S414、定义全阶终端滑模面以保证在控制器中不存在带有分数次幂项的状态量导数，且系统可以在有限时间内达到抗干扰稳定；

S415、基于所述全阶终端滑模面获取基于全阶终端滑模的轨迹跟踪控制器。

本文考虑在非理想环境下，分别为无人船与机械手两个子系统设计基于全阶终端滑模的轨迹跟踪控制器。不同于常见的降阶滑模控制方法，所设计的全阶终端滑模面可以完整描述系统的所有状态阶数，并克服了滑模动态的抖振问题，所提出的控制策略可以实现船载机械手的抗干扰协调控制。

具体来说，基于FOTSM的无人船子系统轨迹跟踪

首先对无人船子系统进行变形有：

式中，

式中，

为了实现对复合扰动的精确估计，设计如下所示有限时间扰动观测器：

式中，μ

定义第一个误差变量：

式中，η

定义无人船子系统的状态误差变量与耦合跟踪误差变量分量：

e

式(12)、(13)中，e

式中，Λ

式中，系数α＝0.5。对滑动变量s

设计如下基于全阶终端滑模的轨迹跟踪控制器τ

式中，τ

在该步骤中，本发明方法应当遵循以下公知准则：

定理1在所设计的有限时间扰动观测器式(10)的作用下，无人船所受到的系统耦合动态与外界干扰可以在有限时间内被精确观测，在此基础上所设计的全阶终端滑模控制器式(18)，可以使系统的耦合跟踪误差与状态跟踪误差快速稳定地收敛到零域。

设计基于FOTSM的机械手子系统轨迹跟踪控制器，包括：

S421、获取机械手子系统一阶逆运动学虚拟关节角速度作为参考信号，对械手数学模型进行变形；

S422、设计有限时间扰动观测器以实现对复合扰动的精确估计；

S423、对机械手子系统的耦合误差进行镇定，所述耦合误差基于状态误差变量与耦合跟踪误差变量确定；

S424、定义全阶终端滑模面以保证滑动模面的连续性；

S425、基于扰动观测器，设计机械手全阶终端滑模控制器。

具体来说，基于假设1，无人船在水面的参考轨迹与机械手末端在任务空间的参考轨迹已知，需要为机械手子系统设计一阶逆运动学虚拟关节角速度作为参考信号：

式中，ε

式中，

设计如下所示有限时间扰动观测器：

式中，δ

定义机械手子系统的状态误差变量与耦合跟踪误差变量分量：

θ

e

同样，为了保证滑动模面的连续性并消除抖振看，设计如下形式的全阶终端滑模面：

式中，Λ

在扰动观测器的基础上，设计机械手全阶终端滑模控制器:

式中，τ

在该步骤中，本发明方法应当遵循以下公知准则：

定理2所设计的全阶终端控制器，可以保证机械手系统的轨迹跟踪误差式(22)-式(24)在有限时间内收敛到零域。且所受到的复合扰动也可以在有限时间内被精确观测。

S5、应用获得的无人船子系统轨迹跟踪控制器和机械手子系统轨迹跟踪控制器对船载机械手进行跟踪控制。

下面对本发明方法进行稳定性分析，将稳定性分析分为三个模块，首先证明所设计扰动观测的有限时间稳定性，其次证明两个子系统的状态与耦合跟踪误差可以在有限时间内收敛到零域，最后证明完整系统的闭环稳定性。

证明1针对所提出的无人船子系统有限时间扰动观测器式(14)，首先定义一组观测误差：

对误差变量分别求导并将所设计的观测器表达式(10)带入导数中得到：

式中，观测器系数L,λ

证明2对于无人船与机械手子系统的全阶滑模控制策略，分别进行Lyapunov稳定性分析，定义第一个Lyapunov函数：

考虑对式(31)求导并将控制器(18)带入滑模动态式(17)中，得到

式中，指数

定义第二个Lyapunov函数：

在扰动误差可以被有限时间观测的前提下，对V

同理，根据引理1，所设计的全阶终端滑模控制器可以保证滑动变量在有限时间内收敛到零，收敛时间满足

在达到滑动面s

证明3为了保证船载机械手完整系统的稳定性，需要考察进行末端执行器的跟踪效果，定义第三个Lyapunov函数：

对该函数求导得到：

式中，γ

如图2-11所示，为本发明方法的一个应用实例，通过引入反步法、一般积分滑模法控制策略与本文所提出的全阶滑模控制策略进行对比，图1给出了无人船在平面上的轨迹跟踪效果，图2～图3分别给出了无人船运动轨迹与速度的跟踪误差，从图中可以看出，全阶滑模控制策略跟踪稳定性与跟踪速度比其他方法更好。由图4给出的扰动观测效果图可以看出，在2秒左右，所设计的扰动观测器就可以跟踪上实际扰动。图5给出了无人船子系统的耦合误差跟踪效果图，可以看出反步法跟踪稳定性较差，一般积分滑模方法跟踪速度与抖振情况都没有全阶滑模方法好。

图6给出了机械手末端在执行任务时的轨迹跟踪效果，从图中可以看出相较于反步法与积分滑模法，本文提出的控制方法可以保证末端位置跟踪稳定性。图7与图8分别从位置和机械手角速度上验证了跟踪效果。图9给出了机械手关节所受扰动的观测效果图，受外界环境与关节摩擦等多方面影响，通过本文提出的观测器可以有效跟踪上扰动。图10给出了机械手子系统的耦合跟踪误差效果图，可以看出所提出的全阶滑模控制方法可以快速有效地使跟踪误差收敛。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

在本发明的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。

在本申请所提供的几个实施例中，应该理解到，所揭露的技术内容，可通过其它的方式实现。其中，以上所描述的装置实施例仅仅是示意性的，例如所述单元的划分，可以为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接，可以是电性或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可为个人计算机、服务器或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。