[发明专利]基于混联机构的海浪主动补偿系统的运动学求解方法

权利要求书

1.一种基于混联机构的海浪主动补偿系统的运动学求解方法，包括正运动学和逆运动学两个过程，逆运动学过程通过上平台末端目标点的位姿参数，求解出动平台在空间的位置和姿态，从而求各个杆长即各移动副的位移，将所得到的关节运动量输入到补偿平台控制器实现运动控制；正运动学过程通过六个输入杆长度、杆长的约束方程，联立方程组求解出动坐标系在静坐标系中的方向余弦矩阵和动坐标系在静坐标系中的位置向量，从而求得上平台舷梯末端的齐次变换矩阵，将所得到的舷梯末端在空间的位姿输入到补偿平台控制器实现运动控制；

其特征是逆运动学过程具体包括：

设α、β、γ分别为绕x、y、z轴的转角，x、y、z为A系相对于B系的位置，E系为空间惯性坐标系，q₁为转动关节的转动角度，q₂为摆动关节的俯仰角度，q₃为伸缩长度，首先在动、静平台的中心分别建立动、静坐标系，

(1)下平台Stewart并联平台姿态变换矩阵为齐次变换矩阵为

(2)上平台串联3-DOF机械臂齐次变换矩阵为：

(3)混联平台齐次变换矩阵为：

(4)混联平台逆运动学求解过程：

由于逆运动学求解的输入为目标点的位姿矩阵，故设目标点相对于惯性坐标系的位姿矩阵为T_s，则于是

设，

则，

(5)进行姿态匹配：

(6)式有5个变量，分别为γ、β、α、q₁、q₂，还有三个独立方程，设γ、β为已知量，α、q₁、q₂为未知量，

假设，则由于α∈[-25°,25°]，sinα∈[-0.4226,0.4226]，cosα∈[0.9063,1]，c₂₁＝cosαcosq₁∈(-0.9063,0.9063)，c₃₁＝sinαcosq₁∈(-0.4226,0.4226)，于是c₃₁＝cosγsinβt₁₁+sinγsinβt₂₁+cosβt₃₁∈(-0.4226,0.4226)；

假设x₁＝cosβ，x₂＝cosγ，y₁＝sinβ，y₂＝sinγ，β∈[-25°,25°]，x₁∈[0.9063,1]，y₁∈[-0.4226,0.4226]，γ∈[-30°,30°]，x₂∈[0.5,1]，y₂∈[-0.5,0.5]，于是，

假设则t₃₁≤0.4226，由于t₁₁²+t₂₁²+t₃₁²＝1，于是点的坐标的取值范围为蓝线包围区域，假如t₁₁，t₂₁，t₃₁的范围在上述区域内，则说明解存在，设β＝0，γ＝max(γ)±5°，则q₁＝-arcsinc₁₁，

(7)进行位置匹配：

于是得到三个方程，有四个未知量x，y，z，q₃，因此存在冗余，l₁，l₂，l₃，l₄，l₅，l₆的取值范围为[0,1.5](m),q₃＞0，求得x,y,z后，根据六自由度Stewart平台逆运动学，求l₁，l₂，l₃，l₄，l₅，l₆；

假设q₃＝0:0.01:2(m)，由(10)式求得x,y,z，根据Stewart平台逆运动学求l₁，l₂，l₃，l₄，l₅，l₆，判断l₁，l₂，l₃，l₄，l₅，l₆是否在[0,1.5](m)范围内。

2.根据权利要求1所述的基于混联机构的海浪主动补偿系统的运动学求解方法，其特征是正运动学过程具体包括：

首先以动、静平台的球面副A₁、B₁为原点建立动、静坐标系，并使它们的X轴经过运动副A₂和运动副B₂；

步骤1：计算从坐标系S_A到S_B的变换矩阵其中，P＝(x y z)^T是O_A在坐标系S_B中的位置向量，矩阵R是一个3×3的方向余弦矩阵，其每一列为坐标系S_A的x_a，y_a，z_a轴在坐标系S_B中的方向余弦；

位置的正解就是当6个输入杆的长度给定后，求解上述的矩阵R和P中的12个元素，因此除方程组(12)中的6个方程外，还需要另外6个方程，这6个方程通过6个杆长的约束方程给出；

步骤2：静平台的每个球面副的坐标在S_B坐标系中表示为且式中a₁＝b₁＝b₂＝0，而A_i在坐标系S_A中的坐标为式中p₁＝q₁＝q₂＝0，A_i在坐标系S_B中的坐标采用坐标变换得到，则各杆长表示为由上式看出不含n_x，n_y，n_z，如此一来在位置正解中只需要求解9个未知数；

步骤3：引入中间变量w₁和w₂对方程组进行化简，中间变量的引入使得未知数个数变成11个，

式中，F_i，G_i，H_i和I_i为常数，i＝1,2,3,…,5,通过矩阵运算来求得这些常数,同时，3个未知数z，l_z，m_z表达为：

接下来通过一系列数值迭代，最终m_y，l_x，l_y杯确定,w₁，w₂，x，y，m_x求出，根据l_z＝(w₁-l_xx-l_yy)/z，m_z＝(w₂-m_xx-m_yy)/z求出z，l_z，m_z；

由于得到的关于m_y的20次多项式使m_y有20个可能的解，最后得到Stewart并联平台对应一组给定的杆长最多有40个可能的位形，再根据约束条件找出符合要求的位形；

步骤4：将静平台的坐标原点从球面副转换到平台中心，设静平台六个球面副连成的圆的半径为r_B，则已知假设动平台的各个球面副连接而成的圆的半径为r_A，则最终得到

步骤5：求解上平台三自由度串联机械臂模型，θ₁为转动关节的旋转角，θ₂为摆动关节的摆动角，首先在动平台的中心处建立与O_A重合的坐标系0系，再利用D-H参数法，对三自由度串联机构各关节的齐次变换矩阵进行求解，

所以则从静平台中心点到舷梯末端的齐次变换矩阵为：