[发明专利]一种基于被动补偿的混联机构系统及其运动分解方法

权利要求书

1.一种基于被动补偿的混联机构系统运动分解方法，其特征是，首先针对并联平台和串联舷梯的各机构位置分别建立运动坐标系，根据关节运动特点求解出齐次变换矩阵；在笛卡尔空间下推导出并联平台和串联舷梯的雅克比矩阵，再将两者融合得到任务空间下的混联机构运动学模型；然后利用雅可比矩阵伪逆法对并联平台和串联舷梯各个关节运动量进行基本分配，结合各个关节的极限位置在原有雅可比矩阵中加入权重系数矩阵及零空间项，再结合投影梯度法完成了对舷梯奇异位形的避免，具体包括如下步骤：

步骤1：建立混联机构运动学模型；

步骤2：基于混联机构的运动学模型，设计运动规划方法；

所述步骤1具体为：

(1.1)建立混联机构整体模型

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

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

(1.2)建立串联舷梯雅克比矩阵

基座标系下，三自由度串联舷梯运动方程为

其中为舷梯在基座标系下的位姿向量，q为舷梯三个关节在关节坐标系下的转角或位移向量；J_s将关节空间运动速度转换为笛卡尔空间的运动速度，通过它来把笛卡尔空间的位姿量转化成关节空间的速度量；

(1.3)建立并联平台雅克比矩阵

根据动、静平台几何结构可以得到各个支腿与平台连接的铰点在相应坐标系中的坐标，上平台各铰点在动坐标系的坐标为^AA，下平台各铰点在静坐标系下的坐标为^BB，动平台各铰点在静坐标系下的坐标为：

则各个支腿在静坐标系下的向量表示为：

^BL＝[^BL₁^BL₂^BL₃^BL₄^BL₅^BL₆]

^BL＝^BA-^BB

为求各支腿工作时的速度，对上式两端求导，可得上平台各铰点速度向量为：

令q＝[x y z α β γ]^T表示上平台运动的广义坐标，有：

(1.4)建立任务空间下混联机构雅克比矩阵

定义运动坐标系下混联机构各关节的速度为ξ＝[x y z α θ γ θ₁ θ₂ d₃]^T，惯性坐标系下的机械手末端位姿向量为

舷梯末端从运动坐标系变换到惯性坐标系的齐次变换矩阵为

其中包含舷梯末端的位置矩阵和姿态矩阵；

对舷梯末端在惯性坐标系下的位置求导可得

其中[ⁱw_v×]为斜对称矩阵，即对于ⁱw_v＝[w_x w_y w_z]^T有

同时推导中用到[x×]y＝-[y×]x和x×y＝-y×x两条运算法则，x,y为矩阵；

对舷梯末端在惯性坐标系下的姿态矩阵求导可得

其中ⁱw_e＝ⁱw_v+ⁱR_v^vw_e；

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

其中J_task为混联机构在任务空间下的雅克比矩阵；

所述步骤2具体为：

(2.1)雅克比矩阵伪逆解

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

其中是雅克比矩阵的加号逆，也称为伪逆或摩尔-彭诺斯逆，是舷梯末端的期望速度轨迹，也就是对于海浪干扰的补偿量；

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

(2.2)多任务优先级法：

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

式中N是整个混联机构的自由度，是任意一个机构或关节的速度、角速度矢量，是一个雅克比矩阵零空间中的关节矢量项；将表示成雅克比矩阵的形式可得多任务优先级规划算法为：

式中k为次要任务个数；代表一系列次要任务；为相应次要任务运动方程中的雅克比矩阵；

在对各个关节速度积分获得位置时可能会引起数值漂移问题，所以引入一个期望值和规划值之间的误差闭环：

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

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

其次定义串联舷梯部分的权重系数为：

次要任务二设定为奇异位形约束，因为并联平台的奇异性遍布整个工作空间，只能通过关节限位避免较大奇异，所以本文采用限制串联机构可操作度的方法避免舷梯的奇异位形；可操作度用来描述机构离奇异位形状态的距离，其定义为

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

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

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

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

其中表示舷梯末端期望轨迹，为舷梯末端期望轨迹与规划轨迹的误差值，K_e为相应增益，表示加入关节限位约束后的雅克比矩阵；

(2.4)并联平台逆运动学求解

通过以上运动规划方法分配得到的混联机构运动量，表现形式为串联舷梯三个关节位移或转动量和并联平台末端六自由度位置姿态量，其中需要进一步对并联平台进行逆运动学求解的到六个连杆对应的伸缩量；

并联平台逆运动学求解，已知上平台中心相对下平台中心的转换矩阵目标求出各个杆长伸缩量；上平台坐标系中任意向量R_a可以通过坐标变换的方式变换成下平台坐标系中的R_b，P是动坐标系原点A在固定坐标系B-x_by_bz_b中的位置矢量；

并联平台齐次变换矩阵为：

式中：

P＝{X_p Y_p Z_p}^T