CN110027678A - 一种基于海浪主动补偿的混联登乘机构运动规划方法 - Google Patents

Abstract

本发明公开了一种基于海浪主动补偿的混联登乘机构运动规划方法。本发明通过对混联登乘系统的结构分析，并进行分解，分别建立六自由度并联平台和三自由度串联舷梯运动学模型，通过雅克比矩阵伪逆法对两个子系统进行运动量分配，结合当前任务情况和子系统的约束情况，采用梯度投影法优化系统的运动量，避免系统的奇异位形出现。该方法有效解决混联机构自由度冗余的运动学多解问题，同时综合考虑关节限位、机构奇异位形等因素，极大限度地优化了混联机构的空间运动分配，为混联登乘系统控制提供输入参数，满足实际工作情况下的运维需要。

Description

一种基于海浪主动补偿的混联登乘机构运动规划方法

技术领域

本发明提供一种混联登乘机构运动规划方法，特别是一种基于海浪主动补偿的混联登乘机构运动规划方法，是一种基于多任务运动规划的主动海浪补偿分析方法，海洋工程技术领域。

背景技术

传统能源高污染高排放对环境造成不可逆转的伤害，这一严峻现状使人们更加关注清洁能源、绿色能源的发展和使用。海洋资源蕴藏丰富，不但包含大量传统能源诸如石油天然气，还包含丰富的可再生能源。风力发电是目前可再生能源开发中技术最成熟、最具开发条件且最有发展前景的领域之一。

然而，在海洋风力资源大力发展的同时也存在许多问题。海上风电场的环境条件比起陆地来说更加艰巨，同时由于海上潮汐、台风、气流和闪电等多种环境因素的影响，海上风电机组经常出现故障，需要风电运维船将维修人员安全平稳地运送到风机平台以进行日常的维修和维护。但海上风场风浪较大，风电运维船受到风浪的影响很难保持船体平稳，以及运送过程中维修人员的安全，因此需要稳定的登乘系统进行辅助工作。

目前基于混联机构的主动海浪补偿研究在国外已经比较成熟，但并不完全适用于国内状况。针对混联机构的运动学分析是主动海浪补偿系统的重点研究内容，运动学求解分为运动学正解和运动学逆解，其中求解运动学正解目前主要有数值法和封闭法。高建设等以能耗最小姿态为最优姿态，基于矢量法求解了串并混联腿的运动学正逆解。韩方元等提出了一种求解3-PRS并联机构正解的快速数值解法，使得计算速度比普通算法快1.5倍。Sung以正运动学方程为约束条件，用优化理论来求最优解，但实时应用还有待在优化理论和软硬件实现方面做进一步探讨。Joey用遗传算法来完成轨迹规划，但这种迭代变量(样本)随机产生的迭代方法的收敛性无法保证。Wolovich提出了基于运动学正解的动态方法来求非冗余度机器人的运动学逆解，但对计算机的配置要求较高而难以实现。刘晶晶等设计了一种串并混联7-DOF仿人机械臂，并通过对并联机构进行运动学分析和D-H建模，建立了仿人机械臂正运动学的数学模型。基于运动学逆解研究运动规划，主要有雅克比矩阵广义逆解法、投影梯度法和扩展空间法等。蒋俊香提出了新一代串并联机床LINKS-EXE700，并对其逆运动学作了详细介绍，先利用姿态矩阵求解串联部分运动学逆解，然后利用杆长不变建立约束方程，求解并联部分运动学逆解。纪校娟对Tripod机构进行了运动学分析，采用简洁的数值搜索方法进行求解，省去了方程求解的繁琐过程。由于混联冗余机器人的运动学解不唯一，在求解过程中还需进一步运动规划。毕树生等提出了一种新颖的用于微动操作的串并联机构，利用矢量运算的方法对混联机构进行了正逆运动学求解，但此种方法计算速度较慢，不能保证获得全部解，且最终的结果与初值选取有关。

本文借鉴其他方法的研究经验，以具有运动学冗余特性的混联机构为研究对象，提出一种基于多任务加权最小范数解的运动学分析方法，从逆运动学的角度对各关节合理分配运动量，同时考虑到运动中的约束问题，解决混联机构整体运动学高效合理求解问题，为登乘系统提供实时控制输入量。

发明内容

针对上述现有技术，本发明要解决的技术问题是提供一种基于海浪主动补偿的混联登乘机构运动规划方法，该方法基于多任务运动规划，在合理解决混联机构冗余问题的同时最大限度优化了空间运动分配。利用串联舷梯和并联平台在运动轨迹之间的协同作用，实现对海浪的主动补偿，为主动海浪补偿系统提供重要控制参数。

本发明提供一种基于海浪主动补偿的混联登乘机构运动规划方法，混联登乘机构包括一个六自由度并联机构和三自由度串联舷梯，其中六自由度并联机构由六个液压缸、上平台和基座组成；三自由度串联舷梯由回转机构、俯仰机构和伸缩机构组成，三自由度串联机构与六自由度并联机构之间通过铰支座组成，基座将混联登乘机构固定在运维船甲板上；

六自由度并联机构由六台铰支座与六台伺服油缸构成，六自由度并联机构主要执行机构为六台伺服油缸，六台伺服油缸以对称的空间几何结构支撑着上平台，油缸两端通过十字轴承、铰支座与上平台和基座固联；六台伺服油缸的缸杆端与上铰支座铰接，上铰支座与上平台下表面固连，伺服油缸缸底与下铰支座铰接，下铰支座与基座上表面固连，基座通过地脚螺栓与甲板固连，为运动补偿提供安全可靠的支撑。

三自由度串联机构自下而上依次为回转机构、俯仰机构和伸缩功能的廊桥；回转机构补偿廊桥航向角度上的偏差；俯仰机构补偿廊桥俯仰角度的偏差；伸缩功能的廊桥补偿空间位移上的偏差；回转机构与上平台连接关节为转动关节，回转机构与俯仰机构连接关节为摆动关节，俯仰机构与伸缩功能的廊桥连接关节为伸缩关节，包括以下步骤：

步骤1：建立混联登乘机构坐标系：在并联机构上平台和基座上分别建立坐标系，基座上的坐标系为基坐标系，基坐标系x_by_bz_b的原点B位于基座下平台中心且固定不动，以基坐标系为参考，运动坐标系x_ay_az_a的原点A也位于上平台的中心位置；当上平台处于中位时，整个并联平台也处于中位，z_a轴和z_b轴位于同一条直线上，其中，B_i(i＝1,2,…,6)表示下平台(静平台)铰点，A_i(i＝1,2,…,6)表示上平台铰点；

在三自由度串联舷梯的转动关节、摆动关节和伸缩关节处分别建立转动关节坐标系O-x₀y₀z₀、摆动关节坐标系O₁-x₁y₁z₁和伸缩关节坐标系O₂-x₂y₂z₂，在廊桥末端建立坐标系O₃-x₃y₃z₃；三自由度串联舷梯的转动关节与并联机构的上平台共轴，运动坐标系的原点A与转动关节坐标系原点O重合，转动关节绕转动关节坐标系O-x₀y₀z₀的z₀轴转动；摆动关节绕摆动关节坐标系O₁-x₁y₁z₁的y₁轴上下摆动，摆动关节坐标系原点O₁为z₀轴与y₁轴的交点，O₁到O的距离设为d₁，O₁到伸缩关节坐标系原点O₂的距离为d₂；伸缩关节沿伸缩关节坐标系O₂-x₂y₂z₂的x₂轴做伸缩运动，伸缩关节的伸缩长度为d₃，d₃是O₂到廊桥末端坐标系原点O₃的距离。

步骤2：建立混联登乘机构运动学模型和任务空间下的雅克比矩阵：

设上平台相对基座的三个姿态角分别为α、β、γ，上平台中心在基座标系中的位置向量为则并联机构齐次变换矩阵为：

其中是上平台相对基座的旋转矩阵，满足：

设串联舷梯转动关节旋转角为θ₁，摆动关节摆动角为θ₂，第三个关节伸缩量为d₃，根据D-H法得串联舷梯齐次变换矩阵为：

根据该齐次变换矩阵得到基座标系下三自由度串联舷梯运动方程为：

其中为三自由度串联舷梯在基座标系下的位姿向量，q为三自由度串联舷梯三个关节在各自关节坐标系下的转角或位移向量；J_s是舷梯对应的雅克比矩阵；

针对六自由度并联机构上平台的六自由度运动，采用欧拉角法建立雅克比矩阵：设上平台在基坐标系下的位置矩阵为η₁＝[x,y,z]^T，姿态矩阵为η₂＝[α,β,γ]^T，则上平台广义六自由度运动量为：

η＝[η₁ ^T,η₂ ^T]^T

上平台相对基座标系沿x_by_bz_b三轴方向的位移和速度转换关系为：

其中v₁是上平台沿基坐标系三个坐标轴平移的速度矩阵，J₁是与欧拉角相关的速度转换矩阵，根据旋转欧拉定理，J₁表达式为：

同理，上平台相对基座标系绕x_by_bz_b三轴转动的角度与角速度的转换关系为：

其中v₂是上平台绕x_by_bz_b三个坐标轴转动的角速度矩阵，J₂是角速度转换矩阵，根据旋转欧拉定理，J₂表达式为：

其中，假设纵倾角满足

得到并联机构六自由度雅克比矩阵如下：

在任务空间下建立雅克比矩阵：定义运动坐标系下混联机构各关节的速度为惯性坐标系下的舷梯末端位姿向量为其中^BP_C是廊桥末端在基坐标系下的位置矩阵，^Br_C是廊桥末端在基坐标系下的姿态矩阵；

结合位置矩阵^BP_C和姿态矩阵^Br_C的导数可得到混联机构的运动学模型：

其中J_task为混联机构在任务空间下的雅克比矩阵，^AP_C是廊桥末端在运动坐标系下的位置矩阵；[w×]为斜对称矩阵，对于w＝[w_x w_y w_z]^T有

步骤3：采用多任务优先级法对混联机构各关节的补偿量进行分配，具体为：

首先，求出混联机构任务空间下的运动学模型的极小范数最小二乘解为：

式中是雅克比矩阵的加号逆，是舷梯末端的期望速度轨迹，即对于海浪干扰的补偿量；N是整个混联机构的自由度，是任意一个机构或关节的速度、角速度矢量，是一个雅克比矩阵零空间中的关节矢量项；将表示成雅克比矩阵的形式得到多任务优先级规划算法为：

式中为主要任务项，为次要任务项；k为次要任务个数；代表一系列次要任务；为相应次要任务运动方程中的雅克比矩阵；e_e和e_a分别是主次要任务与规划值之间的误差，K_e和K_a是对应的参数；

主要任务为保证廊桥末端达到期望位姿，根据混联机构任务空间下运动学方程式，对其求逆可得：

该式对混联机构各关节量进行基本分配；

其次，考虑关节最大转动和奇异位形限制：

次要任务一为关节限位约束：对于串联舷梯以三个关节为约束对象，对于并联机构以上平台位姿作为约束对象，首先定义一个目标优化函数：

其中C_i＞0是一个常系数，表示第i个关节的限位作用强度；q_i、q_imax和q_imin分别是第i个关节的角度或位置的边界值、最大转动或移动的边界值和最小转动或移动边界值；

其次定义各个关节的权重系数W_i,i为：

其中,

给混联机构各关节速度加入一个加权范数矩阵，得到加权雅各比矩阵加号逆：

次要任务二为奇异位形约束，对于串联机构采用限制可操作度的方法避免舷梯的奇异位形，可操作度用来描述机构离奇异位形状态的距离，定义为：

其中J是对应机构的雅克比矩阵；

对于串联舷梯的可操作度以三个关节角度或位置变量求梯度得：

最后，基于加权最小范数法和投影梯度法，确定基于投影梯度法的多任务加权最小范数法，具体表达式为：

其中表示加入关节限位约束后的雅克比矩阵。

步骤4：通过上面的步骤得到混联机构各关节的速度量，通过积分运算转换得到三自由度舷梯各关节分量θ₁、θ₂和d₃和并联机构上平台的位姿量α、β、γ、x、y、z，在针对并联机构上平台的位姿量通过并联机构逆运动学求解得到并联机构六个杆长伸缩量l_i(i＝1,2,3,4,5,6)：

已知上平台中心相对基座中心的转换矩阵，并联机构齐次变换矩阵为：

上平台坐标系中任意向量R_a通过坐标变换的方式变换成下平台坐标系中的R_b，具体为：

其中，是运动坐标系原点A在固定坐标系B-x_by_bz_b中的位置矢量；

缸杆的伸缩量可以表示为：

其中，

令

根据上述推导得到并联机构的六个缸杆伸缩量，完成混联机构的整体自由度分配以及运动规划；

步骤5：根据以上步骤获得的三自由度舷梯各关节分量θ₁、θ₂和d₃和并联机构六个杆长伸缩量l_i(i＝1,2,3,4,5,6)发送到补偿平台控制器。

本发明有益效果：

本发明通过对混联登乘系统的结构分析，并进行分解，分别建立六自由度并联平台和三自由度串联舷梯运动学模型，通过雅克比矩阵伪逆法对两个子系统进行运动量分配，结合当前任务情况和子系统的约束情况，采用梯度投影法优化系统的运动量，避免系统的奇异位形出现。该方法有效解决混联机构自由度冗余的运动学多解问题，同时综合考虑关节限位、机构奇异位形等因素，极大限度地优化了混联机构的空间运动分配，为混联登乘系统控制提供输入参数，满足实际工作情况下的运维需要。本发明能够根据系统检测到的运维船受海浪影响后的姿态和位移，计算出混联机构各部分相应姿态和位置的目标补偿量，是登乘系统控制的前提和基础。本发明借鉴其他方法的研究经验，以具有运动学冗余特性的混联机构为研究对象，提出一种基于多任务加权最小范数解的运动学分析方法，实现混联机构整体运动学求解，为登乘系统提供控制输入量。

(1)本发明通用性好，可以广泛应用于高海况条件下海浪补偿的海上作业中，具有广泛的应用空间。

(2)本发明利用雅克比矩阵的伪逆解直接得到混联机构各关节的位移(转动)量，解决了混联机构自由度冗余，运动学解不唯一的问题。

(3)本发明对混联机构各关节的机构极限进行约束，保证机构在补偿海浪干扰时在安全工作空间内合理运动。

(4)本方法采用梯度投影法限制关节的可操作度，避免了关节奇异位形的出现，提高了混联机构在运行时的安全性和平稳性。

本发明为高海况情况下的海上平台运维登乘系统控制扫清技术障碍，极大提升海工装备技术水平。

附图说明

图1为海浪主动补偿系统混联机构示意图；

图2为海浪主动补偿系统混联机构模型图；

图3为混联机构运动规划流程图。

具体实施方式

下面结合附图对本发明具体实施方式做进一步说明。

本发明提到的登乘系统是一种主动海浪补偿系统，由一个六自由度并联平台和三自由度串联舷梯混联组成，通过控制九自由度混联机构各关节运动补偿风浪对运维船舶产生的不良影响，保证在海浪干扰下补偿机构末端仍保持平稳，从而实现对海浪的主动补偿。已知运维船受海浪影响后的姿态和位移，通过对混联机构进行运动学分析求出机构各部分的运动量，从而实现主动海浪补偿，保证混联机构末端保持平稳的登乘状态。对混联机构进行运动学分析，需要考虑串并联机构之间互相耦合的关系，本发明公开了一种混联机构的运动规划方法，结合各自机构特点对串并联目标补偿量进行合理分配。

具体的运动规划方法如下，首先针对并联平台和串联舷梯的各机构位置建立模型，分别求解运动学方程。再将两者融合，得到任务空间下混联机构的雅克比矩阵。补偿系统输入量为六自由度位姿补偿量，利用雅可比矩阵伪逆法对并联平台和串联舷梯各个关节运动量进行基本分配。为满足对性能指标的要求，实现对约束的优化，结合各个关节的极限位置在原有雅可比矩阵中加入权重系数矩阵及零空间项，再利用投影梯度法完成了对舷梯奇异位形的避免。最后得到九个自由度关节位移(转动)量，完成对混联机构各关节的补偿量分配，实现运动规划。

如图1所示，该混联机构主要由六自由度并联机构、三自由度串联舷梯组成：六自由度并联机构由六个液压缸、上平台和基座组成，主要补偿运维船受海浪影响的三维姿态(俯仰、横滚、航向)；三自由度串联机构由回转机构、俯仰机构和伸缩机构组成，能够补偿运维船受海浪影响的三维方向位移。三自由度串联机构与六自由度并联机构之间通过铰支座组成，基座用于将混联机构固定在运维船甲板上。

六自由度并联平台由六台铰支座与六台伺服油缸构成。六自由度并联机构主要执行机构为六台伺服油缸，六台伺服油缸以对称的空间几何结构支撑着上平台，油缸两端通过十字轴承、铰支座与上平台和基座固联。六台伺服油缸的缸杆端与上铰支座铰接，上铰支座与上平台下表面固连，伺服油缸缸底与下铰支座铰接，下铰支座与基座上表面固连。基座通过地脚螺栓与甲板固连，为运动补偿提供安全可靠的支撑。在计算机控制下，六台伺服油缸协调同步的运动，驱动平台实现海浪主动补偿，并实时的进行故障检测与安全保护，实时的进行运动参数的测试与传输；

三自由度串联机构自下而上依次为回转机构、俯仰机构和伸缩功能的廊桥。回转机构以液压马达作为执行元件，回转机构具有360°全方位旋转角度，补偿廊桥在航向角度上偏差；俯仰机构采用双液压缸作为执行元件，补偿廊桥在俯仰角度上的偏差；伸缩功能的廊桥能补偿空间位移上的偏差，采用直流无刷电机加双向缆绳方式控制廊桥的伸缩。

结合图2，对混联机构运动学模型进行分析，建立任务空间下混联机构的雅克比矩阵。通过雅克比矩阵将关节空间运动速度转换为笛卡尔空间的运动速度，把笛卡尔空间的位姿量转化成关节空间的速度量。

如图2所示，首先建立混联机构整体模型，在并联平台上平台和基座(下平台)上分别建立坐标系，并在舷梯的三个关节分别建立运动坐标系。基坐标系x_by_bz_b的原点B位于下平台中心，是固定不动的，以它为参考，运动坐标系x_ay_az_a的原点A也位于上平台的中心位置。当上平台处于中位时，整个并联平台也处于中位，z_a轴和z_b轴位于同一条直线上。其中，B_i(i＝1,2,…,6)表示下平台(静平台)铰点，A_i(i＝1,2,…,6)表示上平台铰点。

三自由度串联舷梯的转动关节(第一个关节)与并联平台的上平台共轴，也就是说运动坐标系的原点A与O重合，关节绕z₀轴转动。沿着z₀轴，它与摆动关节的摆动轴的交点到点O的距离设为d₁。摆动关节(第二个关节)绕y₁轴上下摆动，其坐标系原点O₁到伸缩关节坐标系原点O₂的距离为d₂。伸缩关节(第三个关节)沿x₂轴做伸缩运动，伸缩关节的伸缩长度为d₃，也就是伸缩关节原点O₂到舷梯末端坐标系原点O₃的距离。

设上平台相对基座的三个姿态角分别为α、β、γ，上平台中心在基座标系中的位置向量为则并联平台齐次变换矩阵为

其中是上平台相对下平台的旋转矩阵

设串联舷梯第一个关节即转动关节相对自身坐标系的旋转角为θ₁，第二个关节即摆动关节相对自身坐标系的旋转角为θ₂，第三个关节即伸缩关节相对自身坐标系的伸缩量为d₃，根据D-H法得串联舷梯齐次变换矩阵为:

根据该齐次变换矩阵得到基座标系下三自由度串联舷梯运动方程：

其中为舷梯在基座标系下的位姿向量，q为舷梯三个关节在关节坐标系下的转角或位移向量。J_s是舷梯对应的雅克比矩阵。

针对并联平台上平台的六自由度运动，采用欧拉角法建立雅克比矩阵。设上平台在基坐标系下的位置矩阵为η₁＝[x,y,z]^T，姿态矩阵为η₂＝[α,β,γ]^T，则上平台广义六自由度运动量为：

η＝[η₁ ^T,η₂ ^T]^T

上平台相对基座标系沿三轴方向的位移和速度转换关系为

其中v₁是上平台沿三个坐标轴平移的速度矩阵，J₁是与欧拉角相关的速度转换矩阵，结合旋转欧拉定理可得J₁表达式为

同理，上平台相对基座标系绕三轴转动的角度与角速度的转换关系为

其中v₂是上平台绕三个坐标轴转动的角速度矩阵，J₂是角速度转换矩阵，结合旋转欧拉定理可得J₂表达式为

为保证方程有意义、非奇异，假设纵倾角满足

将位移量和角度量结合在一起，可得并联平台总的六自由度雅克比矩阵如下

最后在任务空间下建立雅克比矩阵，定义运动坐标系下混联机构各关节的速度为惯性坐标系下的舷梯末端位姿向量为其中^BP_C是廊桥末端在基坐标系下的位置矩阵，^Br_C是廊桥末端在基坐标系下的姿态矩阵；

结合位置矩阵^BP_C和姿态矩阵^Br_C的导数可得到混联机构的运动学模型：

其中J_task为混联机构在任务空间下的雅克比矩阵，^AP_C是廊桥末端在运动坐标系下的位置矩阵。[w×]为斜对称矩阵，即对于w＝[w_x w_y w_z]^T有

结合附图3，基于上面建立的混联机构运动学模型，提出一种基于多任务运动规划的混联登乘系统主动海浪补偿分析方法。基于混联机构的运动学模型设计运动规划方法：利用基于投影梯度法的多任务加权最小范数解法，对混联机构进行运动量分配。对于多任务优先级法来说，主要任务为利用雅克比矩阵的伪逆矩阵对补偿量进行分配保证末端姿态稳定，次要任务分别为限制关节极限和避免奇异位形。其中关节限位采用加权最小范数法实现，奇异位形避免通过投影梯度法限制机构可操作度实现。

(1)多任务优先级法：

首先，求出混联机构任务空间下的运动学模型的极小范数最小二乘解为

式中是雅克比矩阵的加号逆，也称为伪逆或摩尔-彭诺斯逆，是舷梯末端的期望速度轨迹，也就是对于海浪干扰的补偿量。N是整个混联机构的自由度，是任意一个机构或关节的速度、角速度矢量，是一个雅克比矩阵零空间中的关节矢量项。将表示成雅克比矩阵的形式可得多任务优先级规划算法为：

式中为主要任务项，为次要任务项；k为次要任务个数；代表一系列次要任务；为相应次要任务运动方程中的雅克比矩阵。在对各个关节速度积分获得位置时可能会引起数值漂移问题，所以引入一个期望值和规划值之间的误差闭环，其中e_e和e_a分别是主次要任务与规划值之间的误差，K_e和K_a是相应的参数。

(2)主要任务为保证廊桥末端达到期望位姿

根据混联机构任务空间下运动学方程式，对其求逆可得：

其中是雅克比矩阵的加号逆，也称为伪逆或摩尔-彭诺斯逆。该式对混联机构各关节量进行基本分配，理论上可以实现对海浪的补偿，但实际作业时还要考虑关节最大转动(位移量)以及奇异位形等条件限制。

(3)次要任务一为关节限位约束

对于串联舷梯来说三个关节为约束对象，对于并联平台把它的动平台位姿作为约束对象。首先定义一个目标优化函数

其中C_i＞0是一个常系数，用来定义第i个关节的限位作用强度；q_i、q_imax和q_imin分别是第i个关节的角度(位置)、最大转动(移动)边界值和最小转动(移动)边界值。

其次定义各个关节的权重系数为：

给串并联机构各关节速度加入一个加权范数矩阵，得到加权雅各比矩阵加号逆：

(4)次要任务二为奇异位形约束

因为并联平台的奇异性遍布整个工作空间，只能通过关节限位避免较大奇异，对于串联机构采用限制可操作度的方法避免舷梯的奇异位形。可操作度用来描述机构离奇异位形状态的距离，其定义为

其中J是对应机构的雅克比矩阵。

对于串联舷梯的奇异位形避免，需对它的可操作度以各关节角度(位置)为变量求梯度得

(5)基于投影梯度法的多任务加权最小范数解

本文最终规划方法基于加权最小范数法和投影梯度法，确定为基于投影梯度法的多任务加权最小范数解，具体表达式为

其中表示加入关节限位约束后的雅克比矩阵，次要任务项中采用了归一化梯度值，均衡次要任务比重。

(6)通过上面的步骤得到混联机构各关节的速度量，通过积分运算转换得到三自由度舷梯各关节分量θ₁、θ₂和d₃和并联机构上平台的位姿量α、β、γ、x、y、z，在针对并联机构上平台的位姿量通过并联机构逆运动学求解得到并联机构六个杆长伸缩量l_i(i＝1,2,3,4,5,6)。

并联平台逆运动学求解，已知上平台中心相对下平台中心的转换矩阵目标求出各个杆长伸缩量。并联平台齐次变换矩阵为：

上平台坐标系中任意向量R_a可以通过坐标变换的方式变换成下平台坐标系中的R_b，是动坐标系原点A在固定坐标系B-x_by_bz_b中的位置矢量。

杆长的伸缩量可以表示为

令

根据上述推导可得到并联平台的六个杆长神缩量，完成混联机构的整体自由度分配以及运动规划。

Claims (1)

1.一种基于海浪主动补偿的混联登乘机构运动规划方法，所述混联登乘机构包括一个六自由度并联机构和三自由度串联舷梯，其中六自由度并联机构由六个液压缸、上平台和基座组成；三自由度串联舷梯由回转机构、俯仰机构和伸缩机构组成，三自由度串联机构与六自由度并联机构之间通过铰支座组成，基座将混联登乘机构固定在运维船甲板上；

六自由度并联机构由六台铰支座与六台伺服油缸构成，六自由度并联机构主要执行机构为六台伺服油缸，六台伺服油缸以对称的空间几何结构支撑着上平台，油缸两端通过十字轴承、铰支座与上平台和基座固联；六台伺服油缸的缸杆端与上铰支座铰接，上铰支座与上平台下表面固连，伺服油缸缸底与下铰支座铰接，下铰支座与基座上表面固连，基座通过地脚螺栓与甲板固连，为运动补偿提供安全可靠的支撑。

三自由度串联机构自下而上依次为回转机构、俯仰机构和伸缩功能的廊桥；回转机构补偿廊桥航向角度上的偏差；俯仰机构补偿廊桥俯仰角度的偏差；伸缩功能的廊桥补偿空间位移上的偏差；回转机构与上平台连接关节为转动关节，回转机构与俯仰机构连接关节为摆动关节，俯仰机构与伸缩功能的廊桥连接关节为伸缩关节，其特征在于，包括以下步骤：

步骤1：建立混联登乘机构坐标系：在并联机构上平台和基座上分别建立坐标系，基座上的坐标系为基坐标系，基坐标系x_by_bz_b的原点B位于基座下平台中心且固定不动，以基坐标系为参考，运动坐标系x_ay_az_a的原点A也位于上平台的中心位置；当上平台处于中位时，整个并联平台也处于中位，z_a轴和z_b轴位于同一条直线上，其中，B_i(i＝1,2,…,6)表示下平台(静平台)铰点，A_i(i＝1,2,…,6)表示上平台铰点；

在三自由度串联舷梯的转动关节、摆动关节和伸缩关节处分别建立转动关节坐标系O-x₀y₀z₀、摆动关节坐标系O₁-x₁y₁z₁和伸缩关节坐标系O₂-x₂y₂z₂，在廊桥末端建立坐标系O₃-x₃y₃z₃；三自由度串联舷梯的转动关节与并联机构的上平台共轴，运动坐标系的原点A与转动关节坐标系原点O重合，转动关节绕转动关节坐标系O-x₀y₀z₀的z₀轴转动；摆动关节绕摆动关节坐标系O₁-x₁y₁z₁的y₁轴上下摆动，摆动关节坐标系原点O₁为z₀轴与y₁轴的交点，O₁到O的距离设为d₁，O₁到伸缩关节坐标系原点O₂的距离为d₂；伸缩关节沿伸缩关节坐标系O₂-x₂y₂z₂的x₂轴做伸缩运动，伸缩关节的伸缩长度为d₃，d₃是O₂到廊桥末端坐标系原点O₃的距离。

步骤2：建立混联登乘机构运动学模型和任务空间下的雅克比矩阵：

设上平台相对基座的三个姿态角分别为α、β、γ，上平台中心在基座标系中的位置向量为则并联机构齐次变换矩阵为：

其中是上平台相对基座的旋转矩阵，满足：

设串联舷梯转动关节旋转角为θ₁，摆动关节摆动角为θ₂，第三个关节伸缩量为d₃，根据D-H法得串联舷梯齐次变换矩阵为：

根据该齐次变换矩阵得到基座标系下三自由度串联舷梯运动方程为：

其中为三自由度串联舷梯在基座标系下的位姿向量，q为三自由度串联舷梯三个关节在各自关节坐标系下的转角或位移向量；J_s是舷梯对应的雅克比矩阵；

针对六自由度并联机构上平台的六自由度运动，采用欧拉角法建立雅克比矩阵：设上平台在基坐标系下的位置矩阵为η₁＝[x,y,z]^T，姿态矩阵为η₂＝[α,β,γ]^T，则上平台广义六自由度运动量为：

η＝[η₁ ^T,η₂ ^T]^T

上平台相对基座标系沿x_by_bz_b三轴方向的位移和速度转换关系为：

其中v₁是上平台沿基坐标系三个坐标轴平移的速度矩阵，J₁是与欧拉角相关的速度转换矩阵，根据旋转欧拉定理，J₁表达式为：

同理，上平台相对基座标系绕x_by_bz_b三轴转动的角度与角速度的转换关系为：

其中v₂是上平台绕x_by_bz_b三个坐标轴转动的角速度矩阵，J₂是角速度转换矩阵，根据旋转欧拉定理，J₂表达式为：

其中，假设纵倾角满足

得到并联机构六自由度雅克比矩阵如下：

在任务空间下建立雅克比矩阵：定义运动坐标系下混联机构各关节的速度为惯性坐标系下的舷梯末端位姿向量为其中^BP_C是廊桥末端在基坐标系下的位置矩阵，^Br_C是廊桥末端在基坐标系下的姿态矩阵；

结合位置矩阵^BP_C和姿态矩阵^Br_C的导数可得到混联机构的运动学模型：

其中J_task为混联机构在任务空间下的雅克比矩阵，^AP_C是廊桥末端在运动坐标系下的位置矩阵；[w×]为斜对称矩阵，对于w＝[w_x w_y w_z]^T有

步骤3：采用多任务优先级法对混联机构各关节的补偿量进行分配，具体为：

首先，求出混联机构任务空间下的运动学模型的极小范数最小二乘解为：

式中是雅克比矩阵的加号逆，是舷梯末端的期望速度轨迹，即对于海浪干扰的补偿量；N是整个混联机构的自由度，是任意一个机构或关节的速度、角速度矢量，是一个雅克比矩阵零空间中的关节矢量项；将表示成雅克比矩阵的形式得到多任务优先级规划算法为：

式中为主要任务项，为次要任务项；k为次要任务个数；代表一系列次要任务；为相应次要任务运动方程中的雅克比矩阵；e_e和e_a分别是主次要任务与规划值之间的误差，K_e和K_a是对应的参数；

主要任务为保证廊桥末端达到期望位姿，根据混联机构任务空间下运动学方程式，对其求逆可得：

该式对混联机构各关节量进行基本分配；

其次，考虑关节最大转动和奇异位形限制：

次要任务一为关节限位约束：对于串联舷梯以三个关节为约束对象，对于并联机构以上平台位姿作为约束对象，首先定义一个目标优化函数：

其中C_i＞0是一个常系数，表示第i个关节的限位作用强度；q_i、q_imax和q_imin分别是第i个关节的角度或位置的边界值、最大转动或移动的边界值和最小转动或移动边界值；

其次定义各个关节的权重系数W_i,i为：

其中,

给混联机构各关节速度加入一个加权范数矩阵，得到加权雅各比矩阵加号逆：

次要任务二为奇异位形约束，对于串联机构采用限制可操作度的方法避免舷梯的奇异位形，可操作度用来描述机构离奇异位形状态的距离，定义为：

其中J是对应机构的雅克比矩阵；

对于串联舷梯的可操作度以三个关节角度或位置变量求梯度得：

最后，基于加权最小范数法和投影梯度法，确定基于投影梯度法的多任务加权最小范数法，具体表达式为：

其中表示加入关节限位约束后的雅克比矩阵。

步骤4：通过上面的步骤得到混联机构各关节的速度量，通过积分运算转换得到三自由度舷梯各关节分量θ₁、θ₂和d₃和并联机构上平台的位姿量α、β、γ、x、y、z，在针对并联机构上平台的位姿量通过并联机构逆运动学求解得到并联机构六个杆长伸缩量l_i(i＝1,2,3,4,5,6)：

已知上平台中心相对基座中心的转换矩阵，并联机构齐次变换矩阵为：

上平台坐标系中任意向量R_a通过坐标变换的方式变换成下平台坐标系中的R_b，具体为：

其中，是运动坐标系原点A在固定坐标系B-x_by_bz_b中的位置矢量；

缸杆的伸缩量可以表示为：

其中，

令

根据上述推导得到并联机构的六个缸杆伸缩量，完成混联机构的整体自由度分配以及运动规划；

步骤5：根据以上步骤获得的三自由度舷梯各关节分量θ₁、θ₂和d₃和并联机构六个杆长伸缩量l_i(i＝1,2,3,4,5,6)发送到补偿平台控制器。