[发明专利]一种基于海浪主动补偿的混联登乘机构运动规划方法

摘要

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

主权项

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)发送到补偿平台控制器。