一种带连杆的六自由度液压运动平台位姿控制方法与流程

本发明涉及一种机械领域的机器人运动学控制方法。具体的说是一种带连杆的六自由度液压运动平台的位姿控制方法。

背景技术：

现有的多数六自由度液压运动平台，都是液压缸通过铰链和上、下两个平台直接相连。下平台和地面固定，通过液压缸的伸缩运动来驱动上平台的运动，从而模拟六个自由度的运动。可广泛的应用在航天、汽车、建筑等领域。随着科技的进步，各个领域对运动平台控制精度的要求越来越高。传统的六自由度液压运动平台，在较高的频率下，液压缸会产生较大的惯性力，不利于对运动平台的精确控制。

带连杆的六自由度液压运动平台可有效解决液压缸缸体在高频振动时产生的横向振动，提高了运动平台的可靠性。采用轻量连杆，不但有效减少了驱动力用于克服液压缸本身重量所做的功，而且有效减少了克服铰接摩擦力所做的功。但目前通用六自由度液压运动平台的控制方法不适用于带连杆的六自由度液压运动平台。

同时采用位姿正解算法与jacobian矩阵，即可实现带连杆的六自由度液压运动平台的运动控制。位姿运动学正解算法主要用于由六个液压缸的位移信号求取上平台的位姿信号。现有的位姿运动学正解算法没有涉及带连杆的运动平台的。

对于带连杆的六自由度液压运动平台，水平液压缸通过连杆与上平台连接，连杆的上、下虎克铰随上平台一起运动。而现有的位姿正解方法中，均假定所有液压缸的下铰点是静止不动的。若将现有的位姿正解方法应用于带连杆的六自由度液压运动平台，将极大降低控制精度，无法实现六自由度指令信号的准确再现。

本发明涉及的参考文献如下：

[1]plummera,motioncontrolforoverconstrainedparallelservohydraulicmechanisms[c].the10thscandinavianinternationalconferenceonfluidpower,sicfp’07,tampere,finland,may2007。

[2]plummera,ageneralco-ordinatetransformationframeworkformulti-axismotioncontrolwithapplicationsinthetestingindustry[j].controlengineeringpractice,2010,18(6)：598-607。

[3]spillmannj,teschnerm,corde:cosseratrodelementsforthedynamicsimulationofone-dimensionalelasticobjects[c].acmsiggraph/eurographicssymposiumoncomputeranimation,2007,6(4)：63-72。

技术实现要素：

为解决现有技术存在的上述问题，本发明要设计一种带连杆的六自由度液压运动平台位姿控制方法，将位姿正解运算与比例积分控制器及jacobian矩阵相结合，实现带连杆六自由度液压运动平台的高精度控制。

为了实现上述目的，本发明的技术方案如下：一种带连杆的六自由度液压运动平台的控制方法，所述的六自由度液压运动平台包括：下平台、上平台、三个水平向液压缸、三个垂直向液压缸和三个水平向连杆；所述的三个水平向液压缸分别为1号液压缸、2号液压缸和3号液压缸；所述的三个垂直向液压缸分别为4号液压缸、5号液压缸和6号液压缸；所述的三个水平向连杆分别为1号连杆、2号连杆和3号连杆；所述的1号液压缸、2号液压缸和3号液压缸分别通过1号液压缸支座、2号液压缸支座和3号液压缸支座固定在下平台上；所述的1号连杆、2号连杆和3号连杆的末端通过各自的下虎克铰分别与1号液压缸、2号液压缸和3号液压缸连接，1号连杆、2号连杆和3号连杆的首端通过各自的上虎克铰与上平台连接；所述的4号液压缸、5号液压缸和6号液压缸的下端分别通过各自的下虎克铰与下平台连接，4号液压缸、5号液压缸和6号液压缸的上端分别通过各自的上虎克铰与上平台连接；

具体的控制方法，包括以下步骤：

a、设定上平台的六自由度位姿指令信号；

b、利用位姿指令信号减去位姿正解计算模块的输出信号，将二者之差作为比例积分控制器模块的输入信号；

c、将比例积分控制器模块的输出信号作为jacobian矩阵的输入信号；

所述的jacobian矩阵为：

式中，inv(hh)表示对矩阵hh进行求逆运算，r为4号液压缸、5号液压缸和6号液压缸各自的下虎克铰铰点中心所在分布圆的半径，d1为1号连杆上虎克铰铰点中心a2到2号连杆上虎克铰铰点中心a3的距离。设jacobian矩阵的输入信号为x，则输出信号y为y＝jacobian·x；

d、将jacobian矩阵的输出信号作为六个伺服阀的驱动信号，输入到伺服阀中，由伺服阀控制液压缸运动，由液压缸驱动上平台实现六自由度运动；

e、采集六个液压缸的位移信号，作为位姿正解计算模块的输入信号。位姿正解模块的输出信号为上平台的位姿信号。位姿正解模块通过位姿正解计算，由六个液压缸的位移求取上平台的位姿信号；带连杆的六自由度液压运动平台的位姿正解计算，通过迭代计算的方法求取上平台位姿，每次迭代中需要求解线性方程组以获取上平台位姿增量，当精度满足要求时，迭代过程结束并输出上平台位姿。因此，正解计算中，首先给出线性方程组的具体实现形式，然后求解上平台位姿增量，最后求得上平台位姿。

所述的正解计算包括以下步骤：

e1、设定坐标矩阵a、b：

取1号连杆、2号连杆、3号连杆、4号液压缸、5号液压缸、6号液压缸的上虎克铰铰点中心的坐标矩阵a和下虎克铰铰点中心的坐标矩阵b分别为

式中，h1为1号连杆上虎克铰铰点中心a1到平台中心o的水平距离；h2为平台中心o到2号连杆上虎克铰铰点中心a2与3号连杆上虎克铰铰点中心a3的连线的水平距离；hv为4号液压缸、5号液压缸和6号液压缸各自的上虎克铰铰点中心a4、a5和a6组成的平面到平台中心o的垂直距离；l1为1号连杆下虎克铰铰点中心b1到上虎克铰铰点中心a1的初始距离；l2为2号连杆下虎克铰铰点中心b2到上虎克铰铰点中心a2的初始距离；l3为3号连杆下虎克铰铰点中心b3到上虎克铰铰点中心a3的初始距离；dv为4号液压缸、5号液压缸和6号液压缸各自的下虎克铰铰点中心b4、b5和b6组成的平面到平台中心o的垂直距离；δl1为1号液压缸的位移；δl2为2号液压缸的位移；δl3为3号液压缸的位移；

e2、赋予q0初始值

设上平台六个自由度位姿信号为：

q＝[q1q2q3q4q5q6]^t

式中,上标t表示转置运算，q1-横摇角；q2-纵摇角；q3-偏航角；q4-沿ox平移量；q5-沿oy平移量；q6-沿oz平移量。设初始值为：

q0＝[q10q20q30q40q50q60]^t,

q0为6行1列的向量，q10＝0，q20＝0，q30＝0，q40＝0，q50＝0，q60＝0；

e3、计算矩阵t

矩阵t为4行4列矩阵，式中，c表示求余弦符号cos，例如，cq2表示cos(q2)，其余类同；s表示求正弦符号sin，例如，sq3表示sin(q3)，其余类同。

e4、计算f：

f为6行1列的向量。定义l4为4号液压缸下虎克铰铰点中心b4到上虎克铰铰点中心a4的初始距离；l5为5号液压缸下虎克铰铰点中心b5到上虎克铰铰点中心a5的初始距离；l6为6号液压缸下虎克铰铰点中心b6到上虎克铰铰点中心a6的初始距离；δl4为4号液压缸的位移；δl5为5号液压缸的位移；δl6为6号液压缸的位移。

定义符号“.*”为两个矩阵的元素相乘，例如：

则

定义符号“csum(dd)”为矩阵dd的列向量相加，例如：

则csum(dd)＝[a+d+gb+e+hc+f+i]

则f为：

式中，上标“t”表示转置运算。

e5、计算矩阵j：

j为6行6列矩阵。

首先计算下式：

式中，mi为6行1列的向量，表示求矩阵t对qi的偏导数；

然后得到矩阵j＝[m1m2m3m4m5m6]。

e6、求解δqi：

δqi为6行1列的向量。基于步骤e4求得的f及步骤e5求得的j组成方程组：j·δqi＝-f。利用qr分解方法，求取方程组的解δqi。

e7、计算上平台位姿qi：

qi＝q0+δqi

e8、检验解算精度是否满足要求：

判断δqi的2范数是否小于求解精度ε，若小于ε，则转至步骤e10，否则，转至步骤e9。

e9、q0重新赋值：

令q0＝qi，转至步骤e3；

e10、上平台位姿输出

将qi作为上平台位姿，由正解计算模块输出。

进一步地，所述的求解精度ε在[1×10^-10，1×10^-6]区间内取值。

与现有技术相比，本发明具有以下有益效果：

1、本发明通过进行局部坐标变换，利用水平液压缸的位移获取连杆下虎克铰铰点中心坐标的表达式，通过求解方程组，由液压缸位移信号求得上平台的位姿信号，实现了带连杆六自由度运动平台的位姿正解运算，提高了由六个液压缸位移信号到运动平台的六自由度位姿信号的转换精度。结合jacobian矩阵，给出带连杆的六自由度液压运动平台的位姿控制方法，明显提高了带连杆的六自由度液压运动平台的控制精度。

2、本发明可以通过软件编程实现。在cpu为intelpd2.6g、内存为1g的advantech工控机ipc-610上测试，算法的运行周期小于0.5ms，能够满足运动控制系统实时性要求，所以本发明易于采用计算机数字控制实现。

附图说明

图1是带连杆的六自由度液压运动平台的结构示意图。

图2是图1的侧视图。

图3是图1的俯视图。

图4是本发明的流程图。

图5是位姿正解算法的流程图。

图中：1、1号液压缸，2、2号液压缸，3、3号液压缸，4、4号液压缸，5、5号液压缸，6、6号液压缸，7、下平台，8、上平台，9、1号连杆，10、2号连杆，11、3号连杆。

具体实施方式

下面结合附图对本发明进行进一步地描述。如图1-5所示，一种带连杆的六自由度液压运动平台的控制方法，所述的六自由度液压运动平台包括：下平台7、上平台8、三个水平向液压缸、三个垂直向液压缸和三个水平向连杆；所述的三个水平向液压缸分别为1号液压缸1、2号液压缸2和3号液压缸3；所述的三个垂直向液压缸分别为4号液压缸4、5号液压缸5和6号液压缸6；所述的三个水平向连杆分别为1号连杆9、2号连杆10和3号连杆11；所述的1号液压缸1、2号液压缸2和3号液压缸3分别通过1号液压缸支座、2号液压缸支座和3号液压缸支座固定在下平台7上；所述的1号连杆9、2号连杆10和3号连杆11的末端通过各自的下虎克铰分别与1号液压缸1、2号液压缸2和3号液压缸3连接，1号连杆9、2号连杆10和3号连杆11的首端通过各自的上虎克铰与上平台8连接；所述的4号液压缸4、5号液压缸5和6号液压缸6的下端分别通过各自的下虎克铰与下平台7连接，4号液压缸4、5号液压缸5和6号液压缸6的上端分别通过各自的上虎克铰与上平台8连接；

具体的控制方法，包括以下步骤：

a、设定上平台8的六自由度位姿指令信号；

b、利用位姿指令信号减去位姿正解计算模块的输出信号，将二者之差作为比例积分控制器模块的输入信号；

c、将比例积分控制器模块的输出信号作为jacobian矩阵的输入信号；

所述的jacobian矩阵为：

式中，inv(hh)表示对矩阵hh进行求逆运算，r为4号液压缸4、5号液压缸5和6号液压缸6各自的下虎克铰铰点中心所在分布圆的半径，d1为1号连杆9上虎克铰铰点中心a2到2号连杆10上虎克铰铰点中心a3的距离。设jacobian矩阵的输入信号为x，则输出信号y为y＝jacobian·x；

d、将jacobian矩阵的输出信号作为六个伺服阀的驱动信号，输入到伺服阀中，由伺服阀控制液压缸运动，由液压缸驱动上平台8实现六自由度运动；

e、采集六个液压缸的位移信号，作为位姿正解计算模块的输入信号。位姿正解模块的输出信号为上平台8的位姿信号。位姿正解模块通过位姿正解计算，由六个液压缸的位移求取上平台8的位姿信号；带连杆的六自由度液压运动平台的位姿正解计算，通过迭代计算的方法求取上平台8位姿，每次迭代中需要求解线性方程组以获取上平台8位姿增量，当精度满足要求时，迭代过程结束并输出上平台8位姿。因此，正解计算中，首先给出线性方程组的具体实现形式，然后求解上平台8位姿增量，最后求得上平台8位姿。

所述的正解计算包括以下步骤：

e1、设定坐标矩阵a、b：

取1号连杆9、2号连杆10、3号连杆11、4号液压缸4、5号液压缸5、6号液压缸6的上虎克铰铰点中心的坐标矩阵a和下虎克铰铰点中心的坐标矩阵b分别为

式中，h1为1号连杆9上虎克铰铰点中心a1到平台中心o的水平距离；h2为平台中心o到2号连杆10上虎克铰铰点中心a2与3号连杆11上虎克铰铰点中心a3的连线的水平距离；hv为4号液压缸4、5号液压缸5和6号液压缸6各自的上虎克铰铰点中心a4、a5和a6组成的平面到平台中心o的垂直距离；l1为1号连杆9下虎克铰铰点中心b1到上虎克铰铰点中心a1的初始距离；l2为2号连杆10下虎克铰铰点中心b2到上虎克铰铰点中心a2的初始距离；l3为3号连杆11下虎克铰铰点中心b3到上虎克铰铰点中心a3的初始距离；dv为4号液压缸4、5号液压缸5和6号液压缸6各自的下虎克铰铰点中心b4、b5和b6组成的平面到平台中心o的垂直距离；δl1为1号液压缸1的位移；δl2为2号液压缸2的位移；δl3为3号液压缸3的位移；

e2、赋予q0初始值

设上平台8六个自由度位姿信号为：

q＝[q1q2q3q4q5q6]^t

式中,上标t表示转置运算，q1-横摇角；q2-纵摇角；q3-偏航角；q4-沿ox平移量；q5-沿oy平移量；q6-沿oz平移量。设初始值为：

q0＝[q10q20q30q40q50q60]^t,

q0为6行1列的向量，q10＝0，q20＝0，q30＝0，q40＝0，q50＝0，q60＝0；

e3、计算矩阵t

矩阵t为4行4列矩阵，式中，c表示求余弦符号cos，例如，cq2表示cos(q2)，其余类同；s表示求正弦符号sin，例如，sq3表示sin(q3)，其余类同。

e4、计算f：

f为6行1列的向量。定义l4为4号液压缸4下虎克铰铰点中心b4到上虎克铰铰点中心a4的初始距离；l5为5号液压缸5下虎克铰铰点中心b5到上虎克铰铰点中心a5的初始距离；l6为6号液压缸6下虎克铰铰点中心b6到上虎克铰铰点中心a6的初始距离；δl4为4号液压缸4的位移；δl5为5号液压缸5的位移；δl6为6号液压缸6的位移。

定义符号“.*”为两个矩阵的元素相乘，例如：

则

定义符号“csum(dd)”为矩阵dd的列向量相加，例如：

则csum(dd)＝[a+d+gb+e+hc+f+i]

则f为：

式中，上标“t”表示转置运算。

e5、计算矩阵j：

j为6行6列矩阵。

首先计算下式：

式中，mi为6行1列的向量，表示求矩阵t对qi的偏导数；

然后得到矩阵j＝[m1m2m3m4m5m6]。

e6、求解δqi：

δqi为6行1列的向量。基于步骤e4求得的f及步骤e5求得的j组成方程组：j·δqi＝-f。利用qr分解方法，求取方程组的解δqi。

e7、计算上平台8位姿qi：

qi＝q0+δqi

e8、检验解算精度是否满足要求：

判断δqi的2范数是否小于求解精度ε，若小于ε，则转至步骤e10，否则，转至步骤e9。

e9、q0重新赋值：

令q0＝qi，转至步骤e3；

e10、上平台8位姿输出

将qi作为上平台8位姿，由正解计算模块输出。

进一步地，所述的求解精度ε在[1×10^-10，1×10^-6]区间内取值。

本发明的实施例如下：

取图1所示六自由度液压运动平台的结构参数为：r＝3.5×＝3^-1米，d1＝6×1＝^-1米，l1＝7.2×7.^-1米，l2和l3的值与l1相同，dv＝1.23米，h1＝6.2×6.^-1米，h2的值与h1相同，hv＝1.5×1.^-1米。

设上平台8六自由度位姿指令信号为[0度0度5度1×10^-2米0米0米]，定义液压缸伸出方向为位移正方向。当六个液压缸的位移信号分别为1.439×10^-2米、-2.083×10^-2米、3.126×10^-2米、7.284×10^-4米、4.655×10^-4米和2.394×10^-4米时，上平台8的实际输出位姿为[0度0度5度1×10^-2米0米0米]，与位姿指令信号一致。应用现有的位姿正解算法，可求得上平台8的输出位姿信号为[-3.564×10^-3度-4.11×10^-3度5.003度1.074×10^-2米9.121×10^-4米0米]。应用本发明提出的带连杆的位姿正解算法，可求得上平台8的输出位姿为[0度0度5度1×10^-2米0米0米]，与实际输出位姿一致。对比上平台8实际输出位姿与现有位姿正解算法得出的位姿信号及本发明提出的带连杆的位姿正解算法得出的位姿信号可知，现有位姿正解算法存在偏差，本发明提出的位姿正解算法能够实现由六个液压缸位移信号到上平台8六自由度位姿信号的高精度转换。应用本发明提出的带连杆的位姿控制方法，能够明显提高六自由度位姿指令的再现精度。

本发明不局限于本实施例，任何在本发明披露的技术范围内的等同构思或者改变，均列为本发明的保护范围。