CN108446425B - 基于混联机构的海浪主动补偿系统的运动学求解方法 - Google Patents

Abstract

本发明提供的是一种基于混联机构的海浪主动补偿系统的运动学求解方法。逆运动学过程通过上平台末端目标点的位姿参数，求解出动平台在空间的位置和姿态，求各个杆长即各移动副的位移，将所得到的关节运动量输入到补偿平台控制器实现运动控制；正运动学过程通过六个输入杆长度、杆长的约束方程，联立方程组求解出动坐标系在静坐标系中的方向余弦矩阵和动坐标系在静坐标系中的位置向量，求得上平台舷梯末端的齐次变换矩阵，将所得到的舷梯末端在空间的位姿输入到补偿平台控制器实现运动控制。本发明可以使计算过程较为迅速、简单和准确且高效率、应用方便，为海浪主动补偿系统的运动控制提供了依据，能够满足工程上主动补偿系统的工作需求。

Description

基于混联机构的海浪主动补偿系统的运动学求解方法

技术领域

本发明涉及的是一种运动学求解方法，具体地说是一种基于混联机构的海浪主动补偿系统的运动学求解方法。

背景技术

随着科学技术快速发展及人民生活水平日益提高，清洁能源、绿色能源的发展和使用得到了广泛的关注。广袤的海洋占据了地球约70％的面积，海上风能资源丰富，海上风能的开发和利用早已进入各国视野，风能发电站的安装数量大幅增长，各国纷纷视其为新能源战略中最重要的组成部分。在我国，300多万平方公里的蓝色领土更是蕴藏着巨大的风力资源和发电条件，福建沿海海域安装了大量的海上风能发电站并持续不断地提供清洁能源，为我国沿海经济的发展做出了重要贡献。因此，海上风力发电已成为新能源开发中蕴藏量最大、开发技术最成熟且最有开发前景的领域之一。

然而，在风力资源大力发展的同时也存在许多问题。海上风电场要面对风浪流等多重载荷的考验，环境条件比起陆地来说更加的艰巨而复杂，同时由于海上潮汐、台风、气流和闪电等多种环境因素，海上风电机组容易出现故障且导致人员从船上到风机平台上十分困难，具有很大的危险性。想要保持廊桥的稳定从而保持人员的安全性，基于混联机构的海浪主动补偿系统的研究就变得十分重要。其中，运动学求解方法的研究是重中之重。

混联机构当中，求运动学正解是最为困难的，反之求逆解则相对容易。其建模求解过程与单独使用并联平台进行海浪补偿的运动学建模相似。各国学者对运动学建模方法进行了深入研究和探索。GRIG J J提出了Denavit-Hartenberg的运动学建模方法，为达到船用起重机的补偿目的，王生海、陈海泉、孙玉清等人在《一种新型船用起重机综合补偿系统原理及运动学模型》中利用该方法，对综合补偿系统进行运动学建模，而后进行运动学正反解。对于并联机构来说，解决位置正解问题主要方法有数值法和解析法两种。数值法中典型的有非线性方程组消元搜索算法，神经网络算法等。哈尔滨工业大学的赵杰等运用空间几何学及矢量代数的方法求解Delta机器人的正解。但此种方法计算速度较慢，不能保证获得全部解，且最终的结果与初值选取有关。解析法是通过消元法消去机构约束方程中的未知数，从而获得输入输出方程中仅含一个未知数的多项式。国内外学者求解正解的解析解，都是从特殊构型到一般构型的思路进行。北京邮电大学梁崇高教授等人对于Stewart三角平台型6腿SPS并联机械手作了复杂的正解求解；黄真教授则利用求解非线性方程组的方法求解，这些方法的最大缺点或是较为费时，或是对计算机的配置要求较高而难以实现。Han K、Chuang W等人提出增设多个传感器的方法以降低求解的难度并加快求解速度，但这样却增加了结构设计的难度，也带来误差问题。

在各国学者的研究基础上，我们认识到由于并联机构的高耦合性，导致平台独立运动性差，并且有它自身的运动极限。

发明内容

本发明的目的在于提供一种能够实现海浪补偿系统的控制要求，为运动控制的研究提供依据的基于混联机构的海浪主动补偿系统的运动学求解方法。

本发明的目的是这样实现的：

逆运动学求解时，包括目标点在空间的位姿参数，上下平台合理坐标系建立。正运动学求解时，包括输入杆长度。

简单来说，已知上平台末端目标点的位姿参数可求解出动平台在空间的位置和姿态，从而求各个杆长即各移动副的位移。

给定六个输入杆长度，已知杆长的约束方程，联立方程组求解出动坐标系在静坐标系中的方向余弦矩阵和动坐标系在静坐标系中的位置向量，从而求得上平台舷梯末端的齐次变换矩阵。

由于是混联机构，故逆运动学求解较为简单，正运动学的求解复杂，存在多解性的问题即没有唯一的解。由于非线性方程组所含未知数数目较多，故计算速度较慢。求解的关键在于实际求解过程中，我们需要对方程组进一步化简，从而减少未知数的个数，以达到提高计算机的求解速度的目的。另一关键问题是要根据方向余弦不得大于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₁₁，

(6)进行位置匹配：

于是可以得到三个方程，有四个未知量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)范围内。

本发明还可以包括：在实际应用海浪补偿系统的过程中，我们要进行实时监控以保证机构的安全性，故求正解的过程我们要保证高速高效和实时性就变得十分复杂。

进行正运动学求解时，由于运动学的正解具有多解性，我们运用解析法求出机构的全部封闭解，再利用限制条件对所有位置解进行筛选优化，寻求合理的位置解。

与逆运动学求解不同，为了方便计算，在正运动学求解分析过程中，我们首先以动、静平台的球面副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,…,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参数法，对三自由度串联机构各关节的齐次变换矩阵进行求解，可知

所以

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

传统使用并联机构进行海浪补偿的系统补偿范围有限，故本发明以九自由度海浪主动补偿平台为研究对象，首先建立逆运动学模型，再运用解析法建立正运动学模型，解决海浪主动补偿系统正逆运动学通用高效快速求解问题，为海浪主动补偿系统提供控制参数，扩大补偿范围，船保持稳定的可能性也相应增大。

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

(1)本发明弥补了六自由度平台因为耦合性高而存在的运动极限，补偿性能提高，由于海浪的不确定性而导致的舷梯运动剧烈且不稳定的情况也大大降低。

(2)本发明采用解析法求运动学正解，虽然数学模型较为复杂且数学推导运算量大，但是通过该方法可以得到机构全部的位置解，且最终的结果与初值选取无关。

(3)本发明通用性好，可广泛应用于海浪主动补偿系统中，使船体稳定性提升。

(4)本发明的正运动学求解方法克服了传统数值解法速度慢、效率低并且无法求出全部解的缺陷。

(5)本发明中的海浪补偿系统使用混联机构，综合了串联机构和并联机构的优点，运动空间加大，稳定性提高。

(6)本发明对于海浪的横摇、纵摇等各个方向的影响都有很好的补偿作用，为风机的维修和维护提供了方便。

附图说明

图1是海浪主动补偿系统机构示意图。

图2是海浪主动补偿系统逆运动学模型图。

图3是海浪主动补偿系统正运动学模型图。

图4是海浪主动补偿系统逆运动学流程图。

图5是海浪主动补偿系统正运动学流程图。

图6是闭环反馈控制框图。

具体实施方式

本发明的目的是针对现有海浪主动补偿系统运动学求解方法的不足，提供一种基于混联机构的海浪主动补偿系统的运动学求解方法，为海浪主动补偿系统的运动控制研究提供基础和依据，用于实现海浪补偿。

本发明采用的技术方案是：

结合图1，该混联机构由一个上平台即三自由度串联平台2和一个下平台即六自由度Stewart并联平台1组成。上平台三自由度串联平台由具有伸缩功能的舷梯3、俯仰机构、回转机构和操作平台组成。下平台六自由度Stewart并联平台由六台铰支座与六台伺服油缸构成。

六自由度Stewart并联平台主要执行机构为六台伺服油缸，六台伺服油缸的缸杆端与上铰支座铰接，上铰支座与运动平台下表面固连，伺服油缸缸底与下铰支座铰接，下铰支座与静平台上表面固连。静平台通过地脚螺栓与甲板固连，为运动补偿提供安全可靠的支撑。

结合图2，逆运动学求解，将坐标系建立在Stewart并联平台动、静平台的中心位置。再根据串联机构的伸缩、俯仰、回转来建立上平台舷梯的坐标系。利用矩阵变换，下平台Stewart并联平台姿态变换矩阵为

齐次变换矩阵表示为

结合图3，正运动学求解，由于解析法求解过程涉及到许多未知数，故为了方便计算，将坐标系建立在动、静平台球面副的中心，并让X轴通过运动副A₂和运动副B₂。

结合图4，具体求解过程。

我们首先在动、静平台的中心分别建立动、静坐标系。

(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₁₁，

(6)进行位置匹配：

于是可以得到三个方程，有四个未知量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)范围内。

结合图5，具体求解过程。

在实际应用海浪补偿系统的过程中，我们要进行实时监控以保证机构的安全性，故求正解的过程我们要保证高速高效和实时性就变得十分复杂。

进行正运动学求解时，由于运动学的正解具有多解性，我们运用解析法求出机构的全部封闭解，再利用限制条件对所有位置解进行筛选优化，寻求合理的位置解。

与逆运动学求解不同，为了方便计算，在正运动学求解分析过程中，我们首先以动、静平台的球面副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,…,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参数法，对三自由度串联机构各关节的齐次变换矩阵进行求解，可知

所以

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

结合图6可知，正逆运动学求解的结果的差值作为运动控制与补偿的依据，作为输入给补偿器进行运动的补偿。

Claims (2)

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参数法，对三自由度串联机构各关节的齐次变换矩阵进行求解，

所以

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

Images