一种水下特种机器人艇-手多体动力学建模方法及装置与流程

本申请涉及水下机器人技术领域，尤其涉及水下特种机器人艇-手多体动力学建模方法及装置。

背景技术：

随着海洋经济的进一步发展，使用水下机器人-机械臂系统(underwatervehicle-manipulatorsystem-uvms)自主从事海洋工程和海洋科学考察中较精细的工作，如在海底通过挂钩、拔销、拆卸螺栓、按按钮、使用简单的工具进行潜艇和大型载人潜水器的在线维护、简单维修、故障回收和救援工作等，以及完成剪缆、打开阀门、自主采样、水中对接和海洋工程的支持工作等，必将较大地节约海洋工程的花费，节省操作人员的宝贵时间，降低操作人员的负担，创造更大的经济价值。

uvms是强耦合、具有高自由度冗余的多体系统。在水下的特殊环境中，水动力会对其动力学特性产生重大影响。动力学模型的精确性对后期规划以及控制有着重要。

技术实现要素：

本申请提供一种水下特种机器人艇-手多体动力学建模方法，所述水下特种机器人艇-手包括艇体和机械手，所述建模方法包括：建立运动学模型和动力学模型；

所述建立运动学模型，包括：

将水下特种机器人艇-手系统抽象为连杆模型；

根据所述连杆模型建立所述机械手末端执行器与所述水下特种机器人艇-手系统的自由度之间的运动关系函数；

对运动关系函数进行求导，得到所述水下特种机器人艇-手机械手末端执行器的加速度运动式；

所述建立动力学模型，包括：

基于拉格朗日方法对所述水下特种机器人艇-手系统受到外力以及所述加速度运动式进行处理，得到了所述水下特种机器人艇-手系统的动力学方程。

在一些实施例中，所述动力学方程用于在参考系下计算所述机械手末端执行器的位置矢量、、速度、角速度以及艇体的位姿。

在一些实施例中，水下特种机器人艇-手的动力学方程为：

其中，

x∈r⁶⁺ⁿ为uvms系统广义坐标，h(x)为uvms系统的惯性矩阵，ha为uvms系统的附加惯性矩阵，为uvms系统的科氏力矩阵，ca为uvms系统的附加科氏力矩阵，g(x)∈r⁶⁺ⁿ为恢复力矩，表示流体阻尼矩阵，fb为所述水下机器人-机械臂系统所受外力主要包括机器人进行位置、姿态控制的螺旋桨推动力/力矩，τm为机械臂关节驱动力矩。在一些实施例中，所述机械手末端执行器的位置矢量为：

其中，在惯性参考系下，pe为机械手末端执行器的位置向量，pv为艇体固连参考系的位置向量，ii为第i段臂杆从臂杆起始指向臂杆终止位置的向量。

在一些实施例中，，所述水下特种机器人艇-手机械手系统的机械手末端执行器的加速度为：所述水下特种机器人艇-手机械手系统的机械手末端执行器的运动式包括速度和角速度，所述速度的关系式为：

所述角速度的关系式为：

其中，vv和ωv分别表示所述艇体本体的坐标系原点的速度和角速度，pv和pe分别表示在惯性参考系下，所述艇体固连参考系的位置向量和所述机器手末端执行器的位置向量，pe和pi分别表示在惯性参考系下，所述机器手末端执行器的位置向量，和第i段连杆参考系坐标原点向量，表示表示机械臂关节旋转速度向量，ki为常量。

本申请还提供一种水下特种机器人艇-手多体动力学建模装置，所述水下特种机器人艇-手包括艇体和机械手，所述建模装置包括：建立运动学模型模块和动力学模型模块；

所述建立运动学模型模块，包括：

模型抽象单元，用于将水下特种机器人艇-手系统抽象为连杆模型；

运动式计算单元，用于根据所述连杆模型建立所述机械手末端执行器与所述水下特种机器人艇-手系统的自由度之间的运动关系函数；根据运动关系函数得到所述水下特种机器人艇-手机械手末端执行器的运动式；

所述建立动力学模型模块，包括：

动力方程获取单元，用于基于拉格朗日方法对所述水下特种机器人艇-手系统受到外力以及所述运动式进行处理，得到了所述水下特种机器人艇-手系统的动力学方程。

在一些实施例中，所述动力学方程用于在参考系下计算所述机械手末端执行器的位置矢量、、速度、角速度以及艇体的位姿。

在一些实施例中，水下特种机器人艇-手的动力学方程为：

其中

x∈r⁶⁺ⁿ为uvms系统广义坐标，h(x)为uvms系统的惯性矩阵，ha为uvms系统的附加惯性矩阵，为uvms系统的科氏力矩阵，ca为uvms系统的附加科氏力矩阵，g(x)∈r⁶⁺ⁿ为恢复力矩，表示流体阻尼矩阵，fb为所述水下机器人-机械臂系统所受外力主要包括机器人进行位置、姿态控制的螺旋桨推动力/力矩，τm为机械臂关节驱动力矩。在一些实施例中，所述机械手末端执行器的位置矢量为：

其中，在惯性参考系下，pe为机械手末端执行器的位置向量，pv为艇体固连参考系的位置向量，ii为第i段臂杆从臂杆起始指向臂杆终止位置的向量。

在一些实施例中，所述水下特种机器人艇-手机械手系统的机械手末端执行器的运动式包括速度和角速度，所述速度的关系式为：

所述角速度的关系式为：

其中，vv和ωv分别表示所述艇体本体的坐标系原点的速度和角速度，pv和pe分别表示在惯性参考系下，所述艇体固连参考系的位置向量和所述机器手末端执行器的位置向量，pe和pi分别表示在惯性参考系下，所述机器手末端执行器的位置向量，和第i段连杆参考系坐标原点向量，表示表示机械臂关节旋转速度向量，ki为常量。

应用本申请实施例运动学建模方法，以刚体运动学为基础，将uvms系统抽象为连杆模型，通过坐标变换的方法建立了末端执行器与整个系统自由度之间的运动关系，求解得到运动学雅可比矩阵。同时为了在接下来在加速度层应用轨迹规划方法，对运动学方程进行求导，得到了系统加速度运动方程。在系统的动力学建模方面，使用拉格朗日方法，从系统能量的角度入手，并考虑系统所处的水下环境，将系统受到的流体静力、粘性力以及附加质量力加入到拉格朗日方程中，最终得到了uvms系统的完整动力学方程，为接下来的uvms系统轨迹规划工作做好了准备。应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，而非限制本公开。

附图说明

为了更清楚地说明本申请实施例或背景技术中的技术方案，下面将对本申请实施例或背景技术中所需要使用的附图进行说明。

此处的附图被并入说明书中并构成本说明书的一部分，这些附图示出了符合本公开的实施例，并与说明书一起用于说明本公开的技术方案。

图1为本申请实施例提供的水下特种机器人艇-手多体动力学建模方法的流程示意图；

图2为本申请实施例提供的建模的坐标系变换示意图；

图3为本申请实施例提供的一种水下特种机器人艇-手系统坐标系示意图；

图4为本申请实施例提供的一种水下特种机器人艇-手多体动力学建模装置的结构示意图。

具体实施方式

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

本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别不同对象，而不是用于描述特定顺序。此外，术语“包括”和“具有”以及它们任何变形，意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其他步骤或单元。

在本文中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本文所描述的实施例可以与其它实施例相结合。

水下特种机器人艇-手(underwatervehicle-manipulatorsystem-uvms系统)系统是一个强耦合、非线性的多体动力学系统。对uvms系统的轨迹规划以及优化首先需要得到其准确的运动学和动力学模型。本发明实施例中提出一种水下特种机器人艇-手多体建模方法，用于建立uvms系统的运动学和动力学模型。

运动学是在忽略物体本身的物理性质和加在物体上的外力的情况下，从几何的角度，描述和研究物体位置随时间的变化规律。uvms系统运动学是典型的刚体运动学。刚体运动学主要研究刚体本身的平动、转动过程，建立系统的速度、加速度、角速度、角加速度等复杂的运动关系。uvms最重要的运动学特征是在其进行抓取作业过程中的末端执行器的运动学变量。在本章中，通过使用齐次变换方法，建立末端执行器与uvms系统的广义位姿向量之间的几何关系。之后通过数学手段得到末端执行器的速度、加速度雅可比矩阵，确定系统广义速度、加速度与末端执行器速度、加速度之间的运动学关系。

本发明实施例的uvms对象由艇体和具有自由度的机械臂两部分构成。机械臂由旋转关节和机械臂连杆组成，其末端搭载末端执行器，可以在海洋环境中根据工况进行作业。由此，末端执行器的位置精度就有着重要意义。潜水器的末端执行器的位姿由系统的自由度共同决定。在uvms系统中，其末端执行器的位姿由艇体的移动、转动自由度，以及机械臂各个关节的转角决定。也就是，末端执行器的位姿由艇体广义坐标以及机械臂关节运动广义坐标共同决定。

本发明实施例的uvms系统不考虑变形，即可以视为刚体系统。刚体之间的空间位置关系一般使用的方法主要包括齐次变换法、矢量法、旋量法、欧拉角表示法、四元素法等。uvms系统包含连杆系统，非常适合使用齐次坐标变换来描述其空间位置关系。

在一些实施例中，如图1所示，所述建模方法包括：建立运动学模型和动力学模型；

步骤101，所述建立运动学模型，包括：

将水下特种机器人艇-手系统抽象为连杆模型；

根据连杆模型建立所述机械手末端执行器与所述水下特种机器人艇-手系统的自由度之间的运动关系函数；

对运动关系函数进行求导，得到所述水下特种机器人艇-手机械手末端执行器的加速度运动式；

在图2中，σa和σb为两个任意坐标系，^bp是在坐标系σb中的点的向量。向量^ap是在坐标系σa中向量^bp的表示。^ap向量是由坐标系σa指向坐标系σb的过渡向量。向量^ap和向量^bp有如下的过渡关系为：

其中，矩阵是由坐标系σa到坐标系σa的旋转变换矩阵，该矩阵可由绕坐标系x、y、z轴的基本旋转矩阵连续相乘得到，3个基本的旋转矩阵分别为：

为了方便表示向量在不同坐标系下的表示形式，可以将式(1)写成齐次形式

其中，令矩阵

称为齐次变换矩阵，通过齐次变换矩阵可以实现向量在不同坐标系中的平移、旋转表示，实现了向量在不同坐标系中的自由转换。

为了建立其运动学方程，建立坐标系如图3所示。惯性参考系与地面固连，在该参考系下计算末端、艇体位姿以及速度。艇体参考系与艇体固连，其位姿坐标在惯性参考系中即表示艇体的广义坐标及速度。根据潜水器坐标系一般规定，艇体固连坐标系一般取z向下方向为正方向，以下为各个物理变量的解释。

σ0：惯性参考系，

σv：机器人艇体固连参考系，

pv：在惯性参考系下，艇体固连参考系的位置向量，

pe：在惯性参考系下，末端执行器的位置向量，

pi：在惯性参考系下，第i段连杆参考系坐标原点向量，

rg：在惯性参考系下，艇体质心位置向量，，

rb：在惯性参考系下，艇体浮心位置向量，，

rgi：在惯性参考系下，第i段臂杆的质心位置向量，，

rbi：在惯性参考系下，第i段臂杆的浮心位置向量，；

ii：在惯性参考系下，第i段臂杆从臂杆起始指向臂杆终止位置的向量。

在图3中，机械手末端执行器的位置矢量pe∈r³可以表示为：

式中上标为2表示本发明实施例的机械臂为两段式三自由度机械臂。对式(5)两边对时间求导可以得到：

其中vv和ωv分别表示机器人本体的坐标系原点速度和角速度，变矢量ii(i＝0,1,2)的求导公式为：

其中，表示矢量在其固连坐标系中长度的变化，矢量ii表示刚体内部固定矢量故该项为0。ωi表示与矢量ii固连的坐标系角速度，故得到式(7)中结果。又由连杆质心角速度的递推关系式：

可以推出机械臂末端的速度递推关系式为：

以及角速度关系式

将式(9)和式(10)合并为矩阵形式可以得到：

其中向量就为末端执行器的广义速度向量。表示向量的叉乘矩阵，即若有向量a和b与矩阵b，满足关系式ba＝b×a，就称矩阵b为向量a和b的叉乘矩阵，且叉乘矩阵b有如下形式：

同时，式中矩阵jte∈r^3×n与jre∈r^3×n分别为机械臂平动雅可比矩阵和转动雅可比矩阵：

jte＝[k1×(pe-p1),k2×(pe-p2),…](13)

jre＝[k1,k2,…]

若用向量xv∈r⁶表示艇体在惯性坐标系的广义坐标，向量xe∈r⁶表示末端执行器在惯性坐标系的广义坐标，表示机械臂关节旋转速度向量，则可以得到uvms系统末端执行器的运动学表达式为

其中矩阵je为末端执行器运动学方程的雅可比矩阵。对式(14)两边求导可以得到末端加速度表达式。

其中表示向量vv-ve的叉乘矩阵，矩阵和矩阵分别是雅可比矩阵和对时间的导数，其具体形式为：

和

步骤102，所述建立动力学模型，包括：

基于拉格朗日方法对所述水下特种机器人艇-手系统受到外力以及所述加速度运动式进行处理，得到了所述水下特种机器人艇-手系统的动力学方程。拉格朗日方法与牛顿-欧拉方法不同，它对系统整体进行建模，忽略系统内部的相互作用力，直接建立系统与外界作用的动力学关系，尤其适合系统的整体动力学建模。利用拉格朗日方法对水下机器人-机械手一体化系统进行动力学建模。定义拉格朗日函数为l＝t-v，其中t为系统动能，v为系统的势能。在惯性系下拉格朗日方程可写为：

其中为uvms系统的广义坐标，q为非保守主动力所对应的广义力。第i连杆相对于惯性系的动能为：

式中，mi为连杆i的质量，ji为连杆i相对于连杆坐标系的惯量阵，且均为常值。特别的，i＝0表示仅有水下机器人艇体自己。在式(22)中，各个连杆的广义速度与系统广义速度的运动学关系为：

其中将式(23)代入式(22)可得到水下机器人系统的总动能为：

式中h(x)为系统的广义惯量阵。其中hv为艇体的惯量阵，hm为机械臂系统惯量阵，hvm为机器人艇体和机械臂惯量阵的耦合项。机器人在水下环境中受到浮力影响，重力势能可忽略不计，将式(24)代入式(21)可以得到：

令则上式可以改写为式中为与广义加速度成正比的惯性项，中各元素为广义速度的二次型，包括离心力和哥氏力。

水下机器人-机械臂系统所受外力主要包括机器人进行位置、姿态控制的螺旋桨推动力/力矩fb，机械臂关节驱动力矩τm，水下机器人-机械臂系统所受到的水动力fflt。结合广义力的表达式，系统的动力学方程可以写为：

进一步的，动力学方程中矩阵表达式如下所述：

系统中第i个连杆上的任意质点在惯性系中的齐次位置坐标可由系统的齐次变换得到：

其中，矩阵为由惯性坐标系到第i连杆固连坐标系的其次变换矩阵，向量ⁱr为第i连杆固连坐标系中任意质点的位置向量。特别地，当i＝0时代表水下机器人艇体本体。对式(27)两边对时间求导可以得到该点的齐次速度表达式：

可以求得该质点的动能表达式为

对式(29)两边积分可以得到连杆i的动能为：

其中常矩阵ji为第i连杆的惯性矩阵。由此可得系统的总动能表达式为

其中，矩阵h(x)是uvms系统的惯性矩阵，是广义位置向量的函数。求得惯性矩阵后，可以根据式(25)求得矩阵为：

经过上述一系列演算过程，动力学方程中的惯性矩阵和科氏力矩阵的表达式求解完毕。

另外，刚体在水下环境中主要受到附加质量力、流体拖曳力、流体升力以及洋流冲击力等。由于流体力学的精确计算非常复杂，本工作中抓住几个重要的水动力系数进行分析，通过调整方程中的参数可以达到近似的效果。

当刚体在流体中运动时，由于刚体本体的加速运动而产生的周围流体的附加惯性力是必须考虑的。在陆地工业机器人的附加质量影响可以忽略不计因为空气密度比机械系统的密度要小很多。但是在水下环境中附加质量力是不能不考虑的。在水下刚体运动过程中，该刚体周围的液体随着刚体本身进行加速运动。这样，流体的加速运动会在刚体上施加一个反作用力，这个反作用力就是刚体运动时所受到的附加质量力。该附加质量并不是单纯的给刚体本身增加额外的质量，附加质量矩阵是一个更为复杂的与刚体表面几何结构有关的惯量矩阵，因此附加质量矩阵不一定是正定矩阵。在惯性坐标系中，沿x轴方向的线性加速度被定义为：

其中，

用向量[xyzkmn...]^t表示系统的广义力向量，向量表示系统的广义速度向量。同样地，使用与上式相同的形式，可以定义附加质量矩阵ma的其他元素。当刚体处于完全浸没状态，刚体运动速度不大同时不考虑洋流干扰因素的情况下，可以认为附加质量矩阵具有性质

附加科氏力矩阵可以被证明具有以下性质

向量v是系统的广义速度向量。对于完全浸没的水下刚体来说，其附加质量矩阵ma和附加科氏力矩阵ca具有以下形式：

其中矩阵mav为艇体的附加质量矩阵，mam为机械手的附加质量矩阵，

mamv为附加质量耦合项。同理，cav是艇体的附加科氏力矩阵，cam是机械手的科氏力矩阵，camv是附加科氏力矩阵耦合项。一般地，艇体的附加质量矩阵mav和附加科氏力矩阵cav具有如下形式：

式中参数可以根据几何构型理论计算得到，但是在大多数情况下由实验得到。

粘性流体作用力主要分为拖曳力(dragforce)和升力(liftforce)。通常对于粘性作用力的计算只考虑其线性项和二次项。在动力学方程中流体阻尼项矩阵drb可以表示为：

drb(v)＝-diag{xu,yv,zw,kp,mq,nr}

＝-diag{xu|u||u|,yv|v||v|,zw|w||w|,kp|p||p|,mq|q||q|,nr|r||r|}(38)

恢复力矩是水下机器人受到的液体静力，其大小和系统的质心与浮心的位置有关，同时与系统的位姿也有非常紧密的关系。广义的恢复力矩g(x)∈r⁶⁺ⁿ可以表示为如下形式：

式中，向量fbj∈r³表示第j连杆所受浮力，向量fgj∈r³表示第j连杆所受重力，向量rbj∈r³和rgj∈r³表示第j连杆的浮心与重心在惯性坐标系中的位置向量，向量ni∈r³表示第连杆旋转轴方向向量。综合动力学表达式以及外力计算公式，uvms系统的完整动力学方程可以补充为：

其中，

本申请实施例还提出一种水下特种机器人艇-手多体动力学建模装置400，所述水下特种机器人艇-手包括艇体和机械手，所述建模装置包括：建立运动学模型模块401和动力学模型模块402；

所述建立运动学模型模块401，包括：

模型抽象单元403，用于将水下特种机器人艇-手系统抽象为连杆模型；

运动式计算单元404，用于所述连杆模型建立所述机械手末端执行器与所述水下特种机器人艇-手系统的自由度之间的运动关系函数；根据运动关系函数得到所述水下特种机器人艇-手机械手末端执行器的运动式；

所述建立动力学模型模块402，包括：

动力方程获取单元405，用于基于拉格朗日方法对所述水下特种机器人艇-手系统受到外力以及所述运动式进行处理，得到了所述水下特种机器人艇-手系统的动力学方程。

上述实施例的水下特种机器人艇-手多体动力学建模装置400的具体实施方式请参考前述实施例的方法部分，这里不再追述。上述的水下特种机器人艇-手多体动力学建模方法和装置在运动学建模过程中，以刚体运动学为基础，将uvms系统抽象为连杆模型，通过坐标变换的方法建立了末端执行器与整个系统自由度之间的运动关系，求解得到运动学雅可比矩阵。同时为了在接下来在加速度层应用轨迹规划方法，对运动学方程进行求导，得到了系统加速度运动方程。在系统的动力学建模方面，使用拉格朗日方法，从系统能量的角度入手，并考虑系统所处的水下环境，将系统受到的流体静力、粘性力以及附加质量力加入到拉格朗日方程中，最终得到了uvms系统的完整动力学方程，为uvms系统轨迹规划工作做好了准备。