本發(fā)明涉及一種機械領(lǐng)域的機器人運動學(xué)控制方法。具體的說是一種帶連桿的六自由度液壓運動平臺的位姿控制方法。

背景技術(shù)：

現(xiàn)有的多數(shù)六自由度液壓運動平臺，都是液壓缸通過鉸鏈和上、下兩個平臺直接相連。下平臺和地面固定，通過液壓缸的伸縮運動來驅(qū)動上平臺的運動，從而模擬六個自由度的運動。可廣泛的應(yīng)用在航天、汽車、建筑等領(lǐng)域。隨著科技的進步，各個領(lǐng)域?qū)\動平臺控制精度的要求越來越高。傳統(tǒng)的六自由度液壓運動平臺，在較高的頻率下，液壓缸會產(chǎn)生較大的慣性力，不利于對運動平臺的精確控制。

帶連桿的六自由度液壓運動平臺可有效解決液壓缸缸體在高頻振動時產(chǎn)生的橫向振動，提高了運動平臺的可靠性。采用輕量連桿，不但有效減少了驅(qū)動力用于克服液壓缸本身重量所做的功，而且有效減少了克服鉸接摩擦力所做的功。但目前通用六自由度液壓運動平臺的控制方法不適用于帶連桿的六自由度液壓運動平臺。

同時采用位姿正解算法與jacobian矩陣，即可實現(xiàn)帶連桿的六自由度液壓運動平臺的運動控制。位姿運動學(xué)正解算法主要用于由六個液壓缸的位移信號求取上平臺的位姿信號。現(xiàn)有的位姿運動學(xué)正解算法沒有涉及帶連桿的運動平臺的。

對于帶連桿的六自由度液壓運動平臺，水平液壓缸通過連桿與上平臺連接，連桿的上、下虎克鉸隨上平臺一起運動。而現(xiàn)有的位姿正解方法中，均假定所有液壓缸的下鉸點是靜止不動的。若將現(xiàn)有的位姿正解方法應(yīng)用于帶連桿的六自由度液壓運動平臺，將極大降低控制精度，無法實現(xiàn)六自由度指令信號的準(zhǔn)確再現(xiàn)。

本發(fā)明涉及的參考文獻如下：

[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。

技術(shù)實現(xiàn)要素：

為解決現(xiàn)有技術(shù)存在的上述問題，本發(fā)明要設(shè)計一種帶連桿的六自由度液壓運動平臺位姿控制方法，將位姿正解運算與比例積分控制器及jacobian矩陣相結(jié)合，實現(xiàn)帶連桿六自由度液壓運動平臺的高精度控制。

為了實現(xiàn)上述目的，本發(fā)明的技術(shù)方案如下：一種帶連桿的六自由度液壓運動平臺的控制方法，所述的六自由度液壓運動平臺包括：下平臺、上平臺、三個水平向液壓缸、三個垂直向液壓缸和三個水平向連桿；所述的三個水平向液壓缸分別為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、設(shè)定上平臺的六自由度位姿指令信號；

b、利用位姿指令信號減去位姿正解計算模塊的輸出信號，將二者之差作為比例積分控制器模塊的輸入信號；

c、將比例積分控制器模塊的輸出信號作為jacobian矩陣的輸入信號；

所述的jacobian矩陣為：

式中，inv(hh)表示對矩陣hh進行求逆運算，r為4號液壓缸、5號液壓缸和6號液壓缸各自的下虎克鉸鉸點中心所在分布圓的半徑，d1為1號連桿上虎克鉸鉸點中心a2到2號連桿上虎克鉸鉸點中心a3的距離。設(shè)jacobian矩陣的輸入信號為x，則輸出信號y為y＝j(luò)acobian·x；

d、將jacobian矩陣的輸出信號作為六個伺服閥的驅(qū)動信號，輸入到伺服閥中，由伺服閥控制液壓缸運動，由液壓缸驅(qū)動上平臺實現(xiàn)六自由度運動；

e、采集六個液壓缸的位移信號，作為位姿正解計算模塊的輸入信號。位姿正解模塊的輸出信號為上平臺的位姿信號。位姿正解模塊通過位姿正解計算，由六個液壓缸的位移求取上平臺的位姿信號；帶連桿的六自由度液壓運動平臺的位姿正解計算，通過迭代計算的方法求取上平臺位姿，每次迭代中需要求解線性方程組以獲取上平臺位姿增量，當(dāng)精度滿足要求時，迭代過程結(jié)束并輸出上平臺位姿。因此，正解計算中，首先給出線性方程組的具體實現(xiàn)形式，然后求解上平臺位姿增量，最后求得上平臺位姿。

所述的正解計算包括以下步驟：

e1、設(shè)定坐標(biāo)矩陣a、b：

取1號連桿、2號連桿、3號連桿、4號液壓缸、5號液壓缸、6號液壓缸的上虎克鉸鉸點中心的坐標(biāo)矩陣a和下虎克鉸鉸點中心的坐標(biāo)矩陣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初始值

設(shè)上平臺六個自由度位姿信號為：

q＝[q1q2q3q4q5q6]^t

式中,上標(biāo)t表示轉(zhuǎn)置運算，q1-橫搖角；q2-縱搖角；q3-偏航角；q4-沿ox平移量；q5-沿oy平移量；q6-沿oz平移量。設(shè)初始值為：

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為：

式中，上標(biāo)“t”表示轉(zhuǎn)置運算。

e5、計算矩陣j：

j為6行6列矩陣。

首先計算下式：

式中，mi為6行1列的向量，表示求矩陣t對qi的偏導(dǎo)數(shù)；

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

e6、求解δqi：

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

e7、計算上平臺位姿qi：

qi＝q0+δqi

e8、檢驗解算精度是否滿足要求：

判斷δqi的2范數(shù)是否小于求解精度ε，若小于ε，則轉(zhuǎn)至步驟e10，否則，轉(zhuǎn)至步驟e9。

e9、q0重新賦值：

令q0＝qi，轉(zhuǎn)至步驟e3；

e10、上平臺位姿輸出

將qi作為上平臺位姿，由正解計算模塊輸出。

進一步地，所述的求解精度ε在[1×10^-10，1×10^-6]區(qū)間內(nèi)取值。

與現(xiàn)有技術(shù)相比，本發(fā)明具有以下有益效果：

1、本發(fā)明通過進行局部坐標(biāo)變換，利用水平液壓缸的位移獲取連桿下虎克鉸鉸點中心坐標(biāo)的表達式，通過求解方程組，由液壓缸位移信號求得上平臺的位姿信號，實現(xiàn)了帶連桿六自由度運動平臺的位姿正解運算，提高了由六個液壓缸位移信號到運動平臺的六自由度位姿信號的轉(zhuǎn)換精度。結(jié)合jacobian矩陣，給出帶連桿的六自由度液壓運動平臺的位姿控制方法，明顯提高了帶連桿的六自由度液壓運動平臺的控制精度。

2、本發(fā)明可以通過軟件編程實現(xiàn)。在cpu為intelpd2.6g、內(nèi)存為1g的advantech工控機ipc-610上測試，算法的運行周期小于0.5ms，能夠滿足運動控制系統(tǒng)實時性要求，所以本發(fā)明易于采用計算機數(shù)字控制實現(xiàn)。

附圖說明

圖1是帶連桿的六自由度液壓運動平臺的結(jié)構(gòu)示意圖。

圖2是圖1的側(cè)視圖。

圖3是圖1的俯視圖。

圖4是本發(fā)明的流程圖。

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

圖中：1、1號液壓缸，2、2號液壓缸，3、3號液壓缸，4、4號液壓缸，5、5號液壓缸，6、6號液壓缸，7、下平臺，8、上平臺，9、1號連桿，10、2號連桿，11、3號連桿。

具體實施方式

下面結(jié)合附圖對本發(fā)明進行進一步地描述。如圖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、設(shè)定上平臺8的六自由度位姿指令信號；

b、利用位姿指令信號減去位姿正解計算模塊的輸出信號，將二者之差作為比例積分控制器模塊的輸入信號；

c、將比例積分控制器模塊的輸出信號作為jacobian矩陣的輸入信號；

所述的jacobian矩陣為：

式中，inv(hh)表示對矩陣hh進行求逆運算，r為4號液壓缸4、5號液壓缸5和6號液壓缸6各自的下虎克鉸鉸點中心所在分布圓的半徑，d1為1號連桿9上虎克鉸鉸點中心a2到2號連桿10上虎克鉸鉸點中心a3的距離。設(shè)jacobian矩陣的輸入信號為x，則輸出信號y為y＝j(luò)acobian·x；

d、將jacobian矩陣的輸出信號作為六個伺服閥的驅(qū)動信號，輸入到伺服閥中，由伺服閥控制液壓缸運動，由液壓缸驅(qū)動上平臺8實現(xiàn)六自由度運動；

e、采集六個液壓缸的位移信號，作為位姿正解計算模塊的輸入信號。位姿正解模塊的輸出信號為上平臺8的位姿信號。位姿正解模塊通過位姿正解計算，由六個液壓缸的位移求取上平臺8的位姿信號；帶連桿的六自由度液壓運動平臺的位姿正解計算，通過迭代計算的方法求取上平臺8位姿，每次迭代中需要求解線性方程組以獲取上平臺8位姿增量，當(dāng)精度滿足要求時，迭代過程結(jié)束并輸出上平臺8位姿。因此，正解計算中，首先給出線性方程組的具體實現(xiàn)形式，然后求解上平臺8位姿增量，最后求得上平臺8位姿。

所述的正解計算包括以下步驟：

e1、設(shè)定坐標(biāo)矩陣a、b：

取1號連桿9、2號連桿10、3號連桿11、4號液壓缸4、5號液壓缸5、6號液壓缸6的上虎克鉸鉸點中心的坐標(biāo)矩陣a和下虎克鉸鉸點中心的坐標(biāo)矩陣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初始值

設(shè)上平臺8六個自由度位姿信號為：

q＝[q1q2q3q4q5q6]^t

式中,上標(biāo)t表示轉(zhuǎn)置運算，q1-橫搖角；q2-縱搖角；q3-偏航角；q4-沿ox平移量；q5-沿oy平移量；q6-沿oz平移量。設(shè)初始值為：

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為：

式中，上標(biāo)“t”表示轉(zhuǎn)置運算。

e5、計算矩陣j：

j為6行6列矩陣。

首先計算下式：

式中，mi為6行1列的向量，表示求矩陣t對qi的偏導(dǎo)數(shù)；

然后得到矩陣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范數(shù)是否小于求解精度ε，若小于ε，則轉(zhuǎn)至步驟e10，否則，轉(zhuǎn)至步驟e9。

e9、q0重新賦值：

令q0＝qi，轉(zhuǎn)至步驟e3；

e10、上平臺8位姿輸出

將qi作為上平臺8位姿，由正解計算模塊輸出。

進一步地，所述的求解精度ε在[1×10^-10，1×10^-6]區(qū)間內(nèi)取值。

本發(fā)明的實施例如下：

取圖1所示六自由度液壓運動平臺的結(jié)構(gòu)參數(shù)為：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米。

設(shè)上平臺8六自由度位姿指令信號為[0度0度5度1×10^-2米0米0米]，定義液壓缸伸出方向為位移正方向。當(dāng)六個液壓缸的位移信號分別為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米]，與位姿指令信號一致。應(yīng)用現(xiàn)有的位姿正解算法，可求得上平臺8的輸出位姿信號為[-3.564×10^-3度-4.11×10^-3度5.003度1.074×10^-2米9.121×10^-4米0米]。應(yīng)用本發(fā)明提出的帶連桿的位姿正解算法，可求得上平臺8的輸出位姿為[0度0度5度1×10^-2米0米0米]，與實際輸出位姿一致。對比上平臺8實際輸出位姿與現(xiàn)有位姿正解算法得出的位姿信號及本發(fā)明提出的帶連桿的位姿正解算法得出的位姿信號可知，現(xiàn)有位姿正解算法存在偏差，本發(fā)明提出的位姿正解算法能夠?qū)崿F(xiàn)由六個液壓缸位移信號到上平臺8六自由度位姿信號的高精度轉(zhuǎn)換。應(yīng)用本發(fā)明提出的帶連桿的位姿控制方法，能夠明顯提高六自由度位姿指令的再現(xiàn)精度。

本發(fā)明不局限于本實施例，任何在本發(fā)明披露的技術(shù)范圍內(nèi)的等同構(gòu)思或者改變，均列為本發(fā)明的保護范圍。