CN116551696A - 一种海底设施运维多关节变结构机器人控制方法及系统 - Google Patents

Abstract

本发明涉及一种海底设施运维多关节变结构机器人控制方法及系统，该方法包括：S1、建立多关节变结构体机器人模型，获取不同关节角度下各个推进器对于不同自由度运动的推力‑自由度分量矩阵；S2、采集海底设施运维多关节变结构机器人的传感器数据，获取当前机器人控制信号误差，实时解算成期望的运动控制信号；S3、基于期望的运动控制信号以及当前机器人的运动模式，采用推力‑自由度分量矩阵解算出推进器推力；S4、根据动力学模型以及水动力模型，结合推进器推力‑自由度分量矩阵解算出补偿水动力影响所需的各推进器推力，得到最终的推进器推力，推进器输出相应的力以响应运动信号。与现有技术相比，本发明具有稳定性高、抗干扰能力强的优点。

Description

一种海底设施运维多关节变结构机器人控制方法及系统

技术领域

本发明涉及海洋机器人技术领域，尤其是涉及海底设施运维多关节变结构机器人控制方法及系统。

背景技术

海底油气生产系统、海底观测网等海底能源和信息基础设施装备的内部空间狭窄复杂，常规机器人只能进行周界简单巡检，难以进入内部狭窄空间开展巡检维护。

水下多关节机器人运动灵活，结构可变，具有较高的空间可通过性，适用于海底设施运维巡检。这类机器人的关节角度变化将导致自身结构发生改变，其搭载的推进器位置也会发生相应变化，影响不同自由度下的运动方式，因此水下变结构体机器人的运动控制关键在于根据变化的结构确定合适的控制参数。

目前常规研究更多在于固定结构体的水下机器人运动控制，缺少对于水下变结构体机器人运动控制的相关研究，变结构体系统需要建立特殊的数学模型，运用特殊的控制方法。

因此，水下变结构体机器人精确稳定的运动控制是实现海底设施智能巡检的关键。

发明内容

本发明的目的就是为了克服上述现有技术存在的缺陷而提供了一种稳定性高、抗干扰能力强的海底设施运维多关节变结构机器人控制方法及系统。

本发明的目的可以通过以下技术方案来实现：

根据本发明的第一方面，提供了一种海底设施智能运维多关节变结构机器人控制方法，该方法包括以下步骤：

步骤S1、建立多关节变结构体机器人模型，获取不同关节角度下各个推进器对于不同自由度运动的推力-自由度分量矩阵；

步骤S2、采集海底设施运维多关节变结构机器人的传感器数据，获取当前机器人控制信号误差，实时解算成期望的运动控制信号；

步骤S3、基于期望的运动控制信号以及当前机器人的运动模式，采用推力-自由度分量矩阵解算出推进器推力；

步骤S4、根据动力学模型以及水动力模型，结合推进器推力-自由度分量矩阵解算出补偿水动力影响所需的各推进器推力F_w，得到最终的推进器推力F＝F_o+F_w，推进器输出相应的力以实现对于运动信号的响应。

优选地，所述步骤S1包括以下子步骤：

步骤S11、将多关节变结构体机器人模型简化为多连杆铰接模型，在各连杆中部和关节处建立各局部坐标系；

步骤S12、根据各局部坐标系间的DH参数，包括连杆长度、连杆扭转角、连杆偏距和关节转角，获取不同坐标系间的旋转矩阵R及齐次变换矩阵A，建立各坐标系间的转换关系；

步骤S13、根据运动学和动力学模型，获取不同关节角度下对应的推力-自由度分量矩阵A_DOF并离散化。

优选地，所述在各连杆中部和关节处建立各局部坐标系，具体为：

基于双自由度的机器人关节，在关节处建立两个距离为l_j的局部坐标系和/>关节j_i在b_i端进行x_iy_i平面的转动模拟偏航关节，在b_i-1端进行与之垂直平面的转动模拟俯仰关节，在每个连杆质心处，建立用于描述该连杆上坐标及向量的局部坐标系O_i。

优选地，所述推力-自由度分量矩阵A_DOF为6×6的矩阵，一个推力-自由度分量矩阵对应某个关节角度下不同推进器对于不同自由度的影响因子，矩阵中每行对应一个推进器对于六自由度的影响因子，每列对应六个推进器对于某个自由度的影响因子。

优选地，对推力-自由度分量矩阵A_DOF离散化，具体为：根据不同范围的关节角度确定对应的推力-自由度分量矩阵，关节角度每变化设定度数，对应一个不同的推力-自由度分量矩阵。

优选地，所述步骤S2包括以下子步骤：

步骤S21、根据规划的路径或者输入的控制指令，获取期望的控制目标，包括位置、角度、速度、角速度、加速度和角加速度；

步骤S22、获取机器人传感器的实时运动数据，计算得到各参数的控制误差；

步骤S23、对于控制误差进行解算并归一化，获取运动控制信号。

优选地，对于不同的控制信号，所述步骤S23中对于控制误差进行解算，具体为：

对于位置角度、速度角速度、加速度、角加速度进行控制，控制系统总体为双环设计，外环对于各轴方向位置或角度进行控制，内环对于各轴方向速度或角速度进行控制；

当给定的控制信号为位置、角度信号时，通过控制器外环对于各轴方向位置、角度误差进行积分分离环节、比例环节以及微分环节处理，计算得到各轴方向速度、角速度的控制信号；

对于外环解算或者系统输入的各轴方向速度角速度误差信号，通过内环比例环节计算得到各轴方向加速度、角加速度的控制信号；

对于内环得到的各轴方向加速度、角加速度控制信号，归一化得到最终的各轴方向运动信号。

优选地，所述步骤S3中的推力解算的表达式为：

F_o＝DOF/INF

式中，DOF为不同自由度的期望运动信号，INF为六自由度推力-自由度分量矩阵。

优选地，所述步骤S4包括以下子步骤：

步骤S41、通过Fluent流体力学软件，对于简化的机器人模型进行水动力仿真并获取在不同速度下系统各方向的水动力大小及力矩大小F_D；

步骤S42、获取各推进器单位推力在各方向的分力以及分力矩F_t；

步骤S43、根据解算公式F_w＝-F_D/F_t，获取各推进器的补偿推力F_w；

步骤S44、获取最终的推进器推力F＝F_o+F_w。

根据本发明的第二方面，提供了一种海底设施智能运维多关节变结构机器人控制系统，采用任一项所述的方法，所述系统包括：

路径规划模块，用于产生位置、角度的控制目标信号；

运动信号解算模块，用于根据传感器数据实时解算出期望的运动信号；

运动控制基础模块，用于期望的运动信号以及当前机器人的运动模式，采用推力-自由度分量矩阵解算出推进器推力；

水动力补偿模块，用于根据水动力模型估计水动力，并解算补偿所需的推进器推力；

推力合成模块，用于将运动控制基础模块以及水动力补偿模块的结果结合获取最终推力并输出

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

1)本发明通过离散化的推力-自由度分量矩阵确定变结构体系统不同关节角度下的各推进器推力与不同自由度运动的关系，提高了控制稳定性、抗干扰能力；

2)本发明基于水动力模型获取当前角度下机器人所受的水动力，对推进器推力进行补偿，提高了水下控制的精准性，可应用于水下多关节变结构体系统；

3)采用多关节变结构体机器人模型简化为多连杆铰接模型，在各连杆中部和关节处建立各局部坐标系，降低了推力-自由度分量矩阵的构建难度。

附图说明

图1为本发明的方法流程示意图；

图2为本发明的离散化推力-自由度分量矩阵计算流程示意图；

图3为本发明的各级控制信号计算流程示意图；

图4为本发明的推进器推力计算流程示意图；

图5为本发明的实施例中系统结构图。

具体实施方式

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

实施例

本发实施例给出了一种海底设施运维多关节变结构机器人控制方法，通过离散化的推力-自由度分量矩阵确定变结构体系统不同关节角度下的各推进器推力与不同自由度运动的关系，以此作为控制系统的核心。首先基于变结构体简化模型建立坐标转换关系以及运动学模型，基于Fluent流体力学仿真软件获取机器人的水动力模型建立动力学模型，获取不同角度下的推力-自由度分量矩阵，对其进行离散化并在控制系统中进行储存；其次，基于变结构体系统各轴方向上的运动参数控制误差，经过积分分离环节、比例环节、微分环节获取运动控制信号；其次，基于运动信号以及推力-自由度分量矩阵计算得出所需推进器推力；最后，基于水动力模型获取当前角度下机器人所受的水动力，通过推进器进行补偿。接下来对本发明的方法实施例进行详细介绍，如图1所示，该方法包括以下步骤：

步骤S1、建立多关节变结构体机器人的简化模型以及各局部坐标系，获取DH参数并基于DH法获取坐标系间的转换关系。基于坐标转换关系及动力学模型获取不同关节角度下各个推进器对于不同自由度运动的推力-自由度分量矩阵A_DOF，对于不同的角度范围进行离散化处理、存储。

如图2所示，步骤S1包括以下子步骤：

步骤S11、将多关节变结构体机器人的结构简化为多连杆铰接的模型，并在各连杆中部、关节处建立各局部坐标系，由于机器人关节为双自由度关节，因此在关节处建立两个距离为l_j的局部坐标系和/>关节j_i在b_i端进行x_iy_i平面的转动(偏航关节)，在b_i-1端进行与之垂直平面的转动(俯仰关节)。在每个连杆质心处，建立用于描述该连杆上坐标及向量的局部坐标系O_i。

步骤S12、根据各局部坐标系间的DH参数，包括连杆长度、连杆扭转角、连杆偏距、关节转角，获取不同坐标系间的旋转矩阵R及齐次变换矩阵A，建立各坐标系间的转换关系。具体DH参数以及转换矩阵如表1所示：

表1

其中，l₀为头部连杆长度，l₁为中部连杆长度，l₂为尾部连杆长度；a为连杆长度，α为连杆扭转角，d为连杆偏距，θ为连杆转角。

其中，表示坐标系O_j到O_i的旋转矩阵，/>表示坐标系O_j到O_i的齐次变换矩阵。

步骤S13、根据运动学动力学模型，获取不同关节角度下对应的推力-自由度分量矩阵A_DOF并进行离散化。具体推力-自由度分量矩阵选择过程为：

1)机器人推进器位置坐标在所在机身局部坐标系下坐标为P，产生单位推力为F，通过旋转矩阵以及齐次变换矩阵转换到中部坐标系下表示；

2)获取中部坐标系表示下的推进器推力在各轴方向上的分力，表达式为：

其中，x，y,z是O₁坐标系下的三轴单位向量。

3)构造力臂为连杆b₁指向力/>的向量在O₁三轴上的投影矢量，则俯仰、偏航、滚转运动的作用力矩表示为：

4)全部推进器对于六自由度运动的合力和合力矩表示为：

5)推力-自由度分量矩阵INF表示为一个6×6的矩阵，第j行表示编号i的推进器对于六个自由度运动的影响，第i行矩阵表示为：

6)由于嵌入式平台通常受到算力限制，因此对于矩阵进行离散化。关节角度每变化10°，对应一个推力-自由度分量矩阵，储存到嵌入式控制平台中。

步骤S2、根据机器人搭载的陀螺仪及加速度计等传感器数据，获取当前机器人位置、角度、速度、角速度、加速度、角加速度与期望控制信号的误差，实时解算成期望的归一化运动信号，归一化运动信号是机器人在各轴方向上的加速度以及角加速度归一化误差。如图3所示，具体过程为：

1)获取机器人期望的控制信号Q_e。控制信号可以是位置角度、速度角速度、加速度角加速度各组信号量或其组合。控制信号可以来自系统输入、操作员输入以及上一级控制系统给定，例如位置、角度控制量通常由路径规划模块给出，而速度、角速度控制量由外环位置、角度模块计算得出，也可以通过操作员直接输入给定。

2)获取机器人实际的运动参数Q_r。运动参数是对应于控制信号的机器人实际的运动情况，通过对于惯性导航、陀螺仪、加速度计等传感器数据进行处理获得。

3)计算得到机器人控制误差并计算各轴方向的控制信号及子控制信号。

对于外环位置角度控制量，首先计算获得机器人各轴方向位置、角度的控制误差，Q_err＝Q_e-Q_r，最后通过积分分离环节K_i，比例环节K_p以及微分环节K_d获取各轴方向速度、角速度的控制信号，表达式为：

β为积分分离环节的开关系数，当误差小于一定范围时才会启用积分环节，ε为积分分离阈值系数，r是当前航段被控量的期望值，而v(0)是初始切换到该航段时的系统输出。

对于内环速度、角速度控制量，首先计算获得机器人各轴方向位置、角度的控制误差，Q_err＝Q_e-Q_r，最后通过比例环节K_p获取各轴方向加速度、角加速度的控制信号，表达式为：u(t)＝K_pQerr。

对于加速度、角加速度信号，归一化获取机器人在各轴方向上的运动控制信号。

步骤S3、根据关节角度选取合适的推力-自由度分量矩阵。根据输入或系统解算出的期望的运动信号以及当前机器人的运动模式，通过推力-自由度分量矩阵解算出推进器推力F_o。如提4所示，具体步骤为：

1)根据当前关节角度，对应选取离散化的推力-自由度分量矩阵；

2)根据控制信号以及推力-自由度分量矩阵计算得到推进器推力Fo，具体为：

DOF＝[D A]＝[F L T R P Y]

Fo＝DOF/INF

其中，F，L，T，R，P，Y分别为机器人在进退、横移、垂向、滚转、偏航、俯仰方向的运动控制信号，而INF则为选取的推力-自由度分量矩阵。

步骤S4、根据动力学模型以及水动力模拟仿真获得的水动力模型，结合推进器推力-自由度分量矩阵解算出补偿水动力影响所需的各推进器推力F_w。最终的推进器推力F＝F_o+F_w，推进器输出相应的力以实现对于运动信号的响应。具体步骤为：

1)根据Fluent流体力学软件仿真获取得到机器人在不同关节角度、速度下的水动力模型。

2)根据水动力模型以及传感器数据计算得到机器人当前所受的水动力及力矩估计F_D，表达式为：

F_D＝[F_Dx F_Dy F_Dz τ_Dx τ_Dy τ_Dz]

其中机器人所受水动力为[F_Dx,F_Dy,F_Dz]，分别为中部机身坐标系O₁下的三轴方向的水动力，具体表达式为：

f_Dik为机器人机身i在其附体坐标系中所受水动力在轴k方向的分量。f_ik为理论部分对于机身i的k轴轴向的水动力的计算公式，v_ik为机身i沿附体坐标系k轴轴向的速度。F_Di为机器人机身i在其附体坐标系中所受水动力的向量表示。为机器人机身i在O₁坐标系中所受水动力向量；/>表示机器人所受的水动力在O₁坐标系下的轴k方向的分量；x，y，z分别为各轴向的单位向量。

机器人所受水动力力矩为[τ_Dx,τ_Dy,τ_Dz]，分别为中部机身坐标系下的三轴方向的水动力力矩分量，具体计算步骤为：

首先，构造水动力作用于O₁坐标系下各轴的力臂：

其次，计算水动力对于O₁坐标系下各轴的力矩:

最终，获得所受水动力在O₁坐标系下的各轴向合力矩τ_D：

3)根据各推进器单位推力在O1坐标系下各轴方向的分力，计算得到补偿水动力所需的各推进器的推力，具体步骤为。

已知6个推进器单位推力及力矩在O₁坐标系下的各轴向的分量所组成的矩阵F_t为：

在步骤S13中已经给出各元素的计算表达式。

则每个推进器所需补偿力大小F_w可以计算为：

F_w＝-F_D/F_t

4)综合推进器推力以及水动力补偿力，计算得到最终推进器推力F＝F_o+F_w。推进器输出推力对于控制信号进行响应。

5)根据传感器数据判断机器人是否达成控制目标，达成则切换下一个控制目标，跳转步骤S2。

下面给出本发明的系统实施例，如图5所示一种海底设施智能运维多关节变结构机器人控制系统，该系统包括：

路径规划模块，用于产生位置、角度的控制目标信号；

位置、角度外环控制模块，用于计算各轴方向上的速度、角速度控制信号；

速度、角速度内环控制模块，用于计算各轴方向上的加速度、角加速度控制信号；

六自由度运动控制基础模块，对于加速度、角加速度控制信号，解算推进器推力；

水动力补偿模块，根据水动力模型估计水动力，并解算补偿所需的推进器推力；

推力合成模块，将六自由度运动控制基础模块以及水动力补偿模块的结果结合获取最终推力并输出。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (10)

1.一种海底设施智能运维多关节变结构机器人控制方法，其特征在于，该方法包括以下步骤：

步骤S1、建立多关节变结构体机器人模型，获取不同关节角度下各个推进器对于不同自由度运动的推力-自由度分量矩阵；

步骤S2、采集海底设施运维多关节变结构机器人的传感器数据，获取当前机器人控制信号误差，实时解算成期望的运动控制信号；

步骤S3、基于期望的运动控制信号以及当前机器人的运动模式，采用推力-自由度分量矩阵解算出推进器推力；

步骤S4、根据动力学模型以及水动力模型，结合推进器推力-自由度分量矩阵解算出补偿水动力影响所需的各推进器推力F_w，得到最终的推进器推力F＝F_o+F_w，推进器输出相应的力以响应运动信号。

2.根据权利要求1所述的一种海底设施智能运维多关节变结构机器人控制方法，其特征在于，所述步骤S1包括以下子步骤：

步骤S11、将多关节变结构体机器人模型简化为多连杆铰接模型，在各连杆中部和关节处建立各局部坐标系；

步骤S12、根据各局部坐标系间的DH参数，包括连杆长度、连杆扭转角、连杆偏距和关节转角，获取不同坐标系间的旋转矩阵R及齐次变换矩阵A，建立各坐标系间的转换关系；

步骤S13、根据运动学和动力学模型，获取不同关节角度下对应的推力-自由度分量矩阵A_DOF并离散化。

3.根据权利要求2所述的一种海底设施智能运维多关节变结构机器人控制方法，其特征在于，所述在各连杆中部和关节处建立各局部坐标系，具体为：

基于双自由度的机器人关节，在关节处建立两个距离为l_j的局部坐标系和/>关节j_i在b_i端进行x_iy_i平面的转动模拟偏航关节，在b_i-1端进行与之垂直平面的转动模拟俯仰关节，在每个连杆质心处，建立用于描述该连杆上坐标及向量的局部坐标系O_i。

4.根据权利要求3所述的一种海底设施智能运维多关节变结构机器人控制方法，其特征在于，所述推力-自由度分量矩阵A_DOF为6×6的矩阵，一个推力-自由度分量矩阵对应某个关节角度下不同推进器对于不同自由度的影响因子，矩阵中每行对应一个推进器对于六自由度的影响因子，每列对应六个推进器对于某个自由度的影响因子。

5.根据权利要求2所述的一种海底设施智能运维多关节变结构机器人控制方法，其特征在于，对推力-自由度分量矩阵A_DOF离散化，具体为：根据不同范围的关节角度确定对应的推力-自由度分量矩阵，关节角度每变化设定度数，对应一个不同的推力-自由度分量矩阵。

6.根据权利要求1所述的一种海底设施智能运维多关节变结构机器人控制方法，其特征在于，所述步骤S2包括以下子步骤：

步骤S21、根据规划的路径或者输入的控制指令，获取期望的控制目标，包括位置、角度、速度、角速度、加速度和角加速度；

步骤S22、获取机器人传感器的实时运动数据，计算得到各参数的控制误差；

步骤S23、对于控制误差进行解算并归一化，获取运动控制信号。

7.根据权利要求6所述的一种海底设施智能运维多关节变结构机器人控制方法，其特征在于，对于不同的控制信号，所述步骤S23中对于控制误差进行解算，具体为：

对于位置角度、速度角速度、加速度、角加速度进行控制，控制系统总体为双环设计，外环对于各轴方向位置或角度进行控制，内环对于各轴方向速度或角速度进行控制；

当给定的控制信号为位置、角度信号时，通过控制器外环对于各轴方向位置、角度误差进行积分分离环节、比例环节以及微分环节处理，计算得到各轴方向速度、角速度的控制信号；

对于外环解算或者系统输入的各轴方向速度角速度误差信号，通过内环比例环节计算得到各轴方向加速度、角加速度的控制信号；

对于内环得到的各轴方向加速度、角加速度控制信号，归一化得到最终的各轴方向运动信号。

8.根据权利要求1所述的一种海底设施智能运维多关节变结构机器人控制方法，其特征在于，所述步骤S3中的推力解算的表达式为：

F_o＝DOF/INF

式中，DOF为不同自由度的期望运动信号，INF为六自由度推力-自由度分量矩阵。

9.根据权利要求1所述的一种海底设施智能运维多关节变结构机器人控制方法，其特征在于，所述步骤S4包括以下子步骤：

步骤S41、通过Fluent流体力学软件，对于简化的机器人模型进行水动力仿真并获取在不同速度下系统各方向的水动力大小及力矩大小F_D；

步骤S42、获取各推进器单位推力在各方向的分力以及分力矩F_t；

步骤S43、根据解算公式F_w＝-F_D/F_t，获取各推进器的补偿推力F_w；

步骤S44、获取最终的推进器推力F＝F_o+F_w。

10.一种海底设施智能运维多关节变结构机器人控制系统，其特征在于，采用权利要求1～9任一项所述的方法，所述系统包括：

路径规划模块，用于产生位置、角度的控制目标信号；

运动信号解算模块，用于根据传感器数据实时解算出期望的运动信号；

运动控制基础模块，用于期望的运动信号以及当前机器人的运动模式，采用推力-自由度分量矩阵解算出推进器推力；

水动力补偿模块，用于根据水动力模型估计水动力，并解算补偿所需的推进器推力；

推力合成模块，用于将运动控制基础模块以及水动力补偿模块的结果结合获取最终推力并输出。