專利名稱：：基于動態(tài)響應(yīng)譜的關(guān)節(jié)含間隙機器人軌跡規(guī)劃方法
技術(shù)領(lǐng)域：
：本發(fā)明涉及一種基于動態(tài)響應(yīng)譜的關(guān)節(jié)含間隙機器人軌跡規(guī)劃方法。
背景技術(shù)：
：機器人關(guān)節(jié)(即運動副)連接相鄰的兩個構(gòu)件，并使構(gòu)件之間能夠產(chǎn)生相對運動。關(guān)節(jié)元素之間裝配時必然留有間隙，隨著機器人長時間的運轉(zhuǎn)，關(guān)節(jié)間隙有增大的趨勢。關(guān)節(jié)間隙的存在影響了機器人的運動精度，并使關(guān)節(jié)元素之間發(fā)生沖擊碰撞，產(chǎn)生噪音，并加劇關(guān)節(jié)磨損，因此，對于機器人之類的精密機器，避免運動過程中關(guān)節(jié)元素發(fā)生分離，對于提高機器人操作的準確性與動態(tài)性能有重要的意義。目前，人們主要從含間隙機構(gòu)學(xué)的角度，研究一般連桿機構(gòu)運動過程運動副避免分離方法。例如，Park于1987年在《MechanismandMachineTheory》(22(6):549-556)的論文"Counterweightoptimizationforreducingdynamiceffectsofclearanceatarevolutejoint"中采用附加配重的方法提出了含間隙關(guān)節(jié)元素始終保持接觸的方法。Li于1992年在《MechanismandMachineTheory》(27(5):535-541)的論文"Optim咖balancingoflinkageswithclearances"中采用附加彈簧的方法使機構(gòu)運動過程中含間隙關(guān)節(jié)元素不發(fā)生分離。Feng于2002年在《JournalofMechanicalDesign》(124(1):68-73)的論文"Anewoptimizationmethodfordynamicdesignofplanarlinkagewithclearancesatjoints—Optimizingthemassdistributionoflinkstoreducethechangeofjointforces"中采用改變連桿形狀的方法避免含間隙關(guān)節(jié)元素在平面連桿機構(gòu)運動過程中發(fā)生分離。這些方法的缺點是需要附加彈簧或配重等硬件物體。對于機器人操作臂，原動件由伺服電機驅(qū)動。機器人操作臂運動的位置、速度、加速度是可控的，加速度的可控意味著機構(gòu)的慣性力是可控的。在考慮各關(guān)節(jié)速度、加速度和加速度變化約束的基礎(chǔ)上，對機器人在關(guān)節(jié)空間進行不同的軌跡規(guī)劃，機器人運動過程含間隙關(guān)節(jié)的接觸狀態(tài)也不相同。因此，軌跡規(guī)劃是避免機器人運動過程中關(guān)節(jié)元素發(fā)生分離的有效方法。這種方法的優(yōu)點是無需附加硬件物體，也無需改變機器人硬件，可通過軟件方法實現(xiàn)。
發(fā)明內(nèi)容本發(fā)明的目的在于提供一種基于動態(tài)響應(yīng)譜的關(guān)節(jié)含間隙機器人軌跡規(guī)劃方法，來避免機器人運動過程中關(guān)節(jié)元素發(fā)生分離。本發(fā)明采用的技術(shù)方案包括以下步驟(1)為機器人主動關(guān)節(jié)選擇運動多項式函數(shù)在機器人關(guān)節(jié)空間中進行軌跡規(guī)劃，設(shè)笛卡爾空間中有P+1個路徑點，通過反向運動學(xué)求解，把這P+l個路徑點映射到機器人關(guān)節(jié)空間，設(shè)機器人有K個主動關(guān)節(jié)，每個主動關(guān)節(jié)的運動軌跡用P段多項式函數(shù)描述qk,p(t)=Ak,p,HtH+Ak,p,H—、…+Ak,p,hth+…+Ak,p,^+Ak,p,。(式1)tG[O，Tp]，h=0，1，...，H，p=l，2，...，P，k=l，2，…，K其中qk,p(t)表示第k個主動關(guān)節(jié)的第p段運動軌跡，Ak,p,h為第k個主動關(guān)節(jié)第P段軌跡的h次項系數(shù)，Tp為第p段軌跡的時間長度，t表示時間，H為運動多項式階次，對(式1)求導(dǎo)獲得關(guān)節(jié)速度的函數(shù)，再次求導(dǎo)將獲得關(guān)節(jié)加速度的函數(shù)；(2)計算運動多項式函數(shù)的待定系數(shù)取運動多項式階次H的初值為5，根據(jù)主動關(guān)節(jié)的各段運動軌跡上路徑起點與終點的位置、速度與加速度的約束條件，求解五次運動多項式函數(shù)含有的6個待定系數(shù)；(3)求解含間隙關(guān)節(jié)的關(guān)節(jié)反力設(shè)機器人有C個含間隙關(guān)節(jié)，則每個含間隙關(guān)節(jié)中驅(qū)動構(gòu)件受力P也是P段多項式函數(shù)+、p-u=0，1，…，U，i=1，2，…，C其中《表示第i個含間隙關(guān)節(jié)中驅(qū)動構(gòu)件在第p段運動軌跡中所受的力，多項式函數(shù)的階次U取決于各主動關(guān)節(jié)運動軌跡函數(shù)的階次；[Wi,p,J為第i個含間隙關(guān)節(jié)的關(guān)節(jié)反力在第P段運動軌跡上的多項式函數(shù)的u次項系數(shù)，這是一個3行1列的矩陣，每行元素是運動軌跡qk,p(t)的系數(shù)Ak,p,h的函數(shù)；(4)根據(jù)含間隙關(guān)節(jié)的關(guān)節(jié)反力構(gòu)造動態(tài)響應(yīng)譜關(guān)節(jié)反力的模是Tp和t的函數(shù)，其中tG，動態(tài)響應(yīng)譜是關(guān)節(jié)反力的模隨時間和軌跡時間長度改變而發(fā)生變化的圖譜；(5)從動態(tài)響應(yīng)譜中選擇合適的軌跡時間變量，使得關(guān)節(jié)連接的兩構(gòu)件在運動過程中始終保持接觸軌跡規(guī)劃中軌跡時間變量Tp的取值依據(jù)為，Tp的取值必須滿足對任意tG[O，Tp]，各關(guān)節(jié)反力的模大于O;(6)若動態(tài)響應(yīng)譜中沒有符合要求的軌跡時間變量時，則增加運動多項式函數(shù)的階次H，返回步驟(1);否則，若找到了合適的軌跡時間變量，則軌跡規(guī)劃完成。根據(jù)步驟(4)構(gòu)造的關(guān)節(jié)反力的動態(tài)響應(yīng)譜是一個三維曲面，有兩個自變量Tp和t，其中tG[O，Tp]，函數(shù)值為關(guān)節(jié)反力的模。根據(jù)步驟(5)，軌跡時間變量直接從動態(tài)響應(yīng)譜中選取，使含間隙關(guān)節(jié)元素始終保持接觸狀態(tài)。本發(fā)明具有的有益效果是1)不需要附加彈簧或配重等硬件物體，只需設(shè)計軟件算法即可取得效果；2)計算量較??；3)通過構(gòu)造軌跡時間長度連續(xù)的動態(tài)響應(yīng)譜，可以指導(dǎo)設(shè)計人員如何選擇軌跡時間長度以避免關(guān)節(jié)元素發(fā)生分離。圖1是基于動態(tài)響應(yīng)譜的關(guān)節(jié)含間隙機器人軌跡規(guī)劃的流程圖。圖2是5R機器人機構(gòu)示意圖。圖3是第三關(guān)節(jié)5的動態(tài)響應(yīng)譜。圖4是軌跡時間變量Tp=0.3時的第三關(guān)節(jié)5的關(guān)節(jié)反力響應(yīng)曲線。圖5是軌跡時間變量Tp=1.0時的第三關(guān)節(jié)5的關(guān)節(jié)反力響應(yīng)曲線。圖中1、第一關(guān)節(jié)，2、第一桿件，3、第二關(guān)節(jié)，4、第二桿件，5、第三關(guān)節(jié)，6、第三桿件，7、第四關(guān)節(jié)，8、第四桿件，9、第五關(guān)節(jié)。具體實施例方式下面結(jié)合5R機器人對本發(fā)明作進一步說明。基于動態(tài)響應(yīng)譜的關(guān)節(jié)含間隙機器人軌跡規(guī)劃的流程圖見圖1所示，包括以下關(guān)鍵步驟(1)為機器人主動關(guān)節(jié)選擇的運動多項式函數(shù)；(2)計算多項函數(shù)的待定系數(shù)；(3)求解含間隙關(guān)節(jié)的關(guān)節(jié)反力；(4)根據(jù)含間隙關(guān)節(jié)的關(guān)節(jié)反力構(gòu)造動態(tài)響應(yīng)譜；(5)從動態(tài)響應(yīng)譜中選擇合適的軌跡時間變量，使得運動副始終保持接觸；(6)若動態(tài)響應(yīng)譜中沒有符合要求的軌跡時間變量時，則增加軌跡規(guī)劃多項式函數(shù)的階次，返回步驟(1);否則，若找到了合適的軌跡時間變量，則軌跡規(guī)劃完成。以圖2所示的5R并聯(lián)機器人為例。第一關(guān)節(jié)1、第二關(guān)節(jié)3是主動關(guān)節(jié)，設(shè)第三關(guān)節(jié)5含有間隙。(1)選擇5R機器人主動關(guān)節(jié)的運動多項式函數(shù)為五次多項式。設(shè)各主動關(guān)節(jié)運動軌跡的五次多項式函數(shù)為<formula>formulaseeoriginaldocumentpage5</formula>其中k=1，2。(2)計算多項函數(shù)的待定系數(shù)。設(shè)機器人運動軌跡的起始點在關(guān)節(jié)坐標系中描述為(A。，q2。，q3。)，終點為(q^，q^q3e)，且起點和終點間僅含一段軌跡。在本方法中，主動關(guān)節(jié)的各段軌跡消耗的時間Tp是軌跡時間長度，它將由軌跡規(guī)劃算法確定。則運動多項式的系數(shù)為<formula>formulaseeoriginaldocumentpage5</formula>(3)求解含間隙關(guān)節(jié)的關(guān)節(jié)反力。機器人機構(gòu)的動力學(xué)參數(shù)的設(shè)定值如表1所示，其中l(wèi)al為第一桿件2的長度，la2為第二桿件4的長度，la2。為第二桿件4質(zhì)心到第二關(guān)節(jié)3的長度，m《為第二桿件4的質(zhì)量，Ja2。為第二桿件4的轉(zhuǎn)動慣量，lbl為第四桿件8的長度，lb2為第三桿件6的長度，lb2e為第三桿件6質(zhì)心到第四關(guān)節(jié)7的長度，mb2為第三桿件6的質(zhì)量，Jb2。為第三桿件6的轉(zhuǎn)動慣量，lab為第一關(guān)節(jié)1到第五關(guān)節(jié)9的長度，g為重力加速度，qA1。為第二關(guān)節(jié)4的起始角度，qAle為第二關(guān)節(jié)4的終止角度，qB1。為第四關(guān)節(jié)8的起始角度，q^為第四關(guān)節(jié)8的終止角度。采用多體動力學(xué)的牛頓-歐拉方程求解含間隙關(guān)節(jié)即第三關(guān)節(jié)5的關(guān)節(jié)反力。表15R機器人的動力學(xué)參數(shù)<table>tableseeoriginaldocumentpage6</column></row><table>(4)根據(jù)含間隙關(guān)節(jié)的關(guān)節(jié)反力構(gòu)造動態(tài)響應(yīng)譜。第三關(guān)節(jié)5的關(guān)節(jié)反力模的動態(tài)響應(yīng)譜如圖3所示，其中，坐標Tp表示軌跡時間長度，坐標t表示時間，坐標H。表示第三關(guān)節(jié)5關(guān)節(jié)反力的模。(5)從動態(tài)響應(yīng)譜中選擇合適的軌跡時間變量，使得運動副始終保持接觸。圖4、圖5分別描述了從動態(tài)響應(yīng)譜中切取的Tp=0.3和Tp=1.0時的第三關(guān)節(jié)5關(guān)節(jié)反力模的曲線，圖中關(guān)節(jié)反力模的數(shù)值超出350N的曲線被截去了。從圖5可以看出，Tp=1.0時曲線存在關(guān)節(jié)反力模等于0的情形，關(guān)節(jié)連接的兩構(gòu)件在機器人運動過程中會發(fā)生分離。從圖4可以看出，Tp=0.3時關(guān)節(jié)反力模的最小值離0最遠，即曲線距發(fā)生分離的邊緣最遠，關(guān)節(jié)連接的兩構(gòu)件在機器人運動過程中不會發(fā)生分離，因此，選擇軌跡時間變量Tp=0.3。(6)軌跡規(guī)劃完成。權(quán)利要求一種基于動態(tài)響應(yīng)譜的關(guān)節(jié)含間隙機器人軌跡規(guī)劃方法，其特征在于該方法的步驟如下(1)為機器人主動關(guān)節(jié)選擇運動多項式函數(shù)在機器人關(guān)節(jié)空間中進行軌跡規(guī)劃，設(shè)笛卡爾空間中有P+1個路徑點，通過反向運動學(xué)求解，把這P+1個路徑點映射到機器人關(guān)節(jié)空間，設(shè)機器人有K個主動關(guān)節(jié)，每個主動關(guān)節(jié)的運動軌跡用P段多項式函數(shù)描述qk，p(t)＝Ak，p，HtH+Ak，p，H-1tH-1+…+Ak，p，hth+…+Ak，p，1t+Ak，p，0(式1)t∈，h＝0，1，…，H，p＝1，2，…，P，k＝1，2，…，K其中qk，p(t)表示第k個主動關(guān)節(jié)的第p段運動軌跡，Ak，p，h為第k個主動關(guān)節(jié)第p段軌跡的h次項系數(shù)，Tp為第p段軌跡的時間長度，t表示時間，H為運動多項式階次，對(式1)求導(dǎo)獲得關(guān)節(jié)速度的函數(shù)，再次求導(dǎo)將獲得關(guān)節(jié)加速度的函數(shù)；(2)計算運動多項式函數(shù)的待定系數(shù)取運動多項式階次H的初值為5，根據(jù)主動關(guān)節(jié)的各段運動軌跡上路徑起點與終點的位置、速度與加速度的約束條件，求解五次運動多項式函數(shù)含有的6個待定系數(shù)；(3)求解含間隙關(guān)節(jié)的關(guān)節(jié)反力設(shè)機器人有C個含間隙關(guān)節(jié)，則每個含間隙關(guān)節(jié)中驅(qū)動構(gòu)件受力也是P段多項式函數(shù)<mrow><mover><msub><mi>F</mi><mrow><mi>i</mi><mo>,</mo><mi>p</mi></mrow></msub><mo>→</mo></mover><mo>=</mo><mo>[</mo><msub><mi>W</mi><mrow><mi>i</mi><mo>,</mo><mi>p</mi><mo>,</mo><mi>U</mi></mrow></msub><mo>]</mo><msup><mi>t</mi><mi>U</mi></msup><mo>+</mo><mo>[</mo><msub><mi>W</mi><mrow><mi>i</mi><mo>,</mo><mi>p</mi><mo>,</mo><mi>U</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>]</mo><msup><mi>t</mi><mrow><mi>U</mi><mo>-</mo><mn>1</mn></mrow></msup><mo>+</mo><mo>.</mo><mo>.</mo><mo>.</mo><mo>+</mo><mo>[</mo><msub><mi>W</mi><mrow><mi>i</mi><mo>,</mo><mi>p</mi><mo>,</mo><mi>u</mi></mrow></msub><mo>]</mo><msup><mi>t</mi><mi>u</mi></msup><mo>+</mo><mo>.</mo><mo>.</mo><mo>.</mo><mo>+</mo><mo>[</mo><msub><mi>W</mi><mrow><mi>i</mi><mo>,</mo><mi>p</mi><mo>,</mo><mn>1</mn></mrow></msub><mo>]</mo><mi>t</mi><mo>+</mo><mo>[</mo><msub><mi>W</mi><mrow><mi>i</mi><mo>,</mo><mi>p</mi><mo>,</mo><mn>0</mn></mrow></msub><mo>]</mo></mrow>u＝0，1，…，U，i＝1，2，…，C其中表示第i個含間隙關(guān)節(jié)中驅(qū)動構(gòu)件在第p段運動軌跡中所受的力，多項式函數(shù)的階次U取決于各主動關(guān)節(jié)運動軌跡函數(shù)的階次；[Wi，p，u]為第i個含間隙關(guān)節(jié)的關(guān)節(jié)反力在第p段運動軌跡上的多項式函數(shù)的u次項系數(shù)，這是一個3行1列的矩陣，每行元素是運動軌跡qk，p(t)的系數(shù)Ak，p，h的函數(shù)；(4)根據(jù)含間隙關(guān)節(jié)的關(guān)節(jié)反力構(gòu)造動態(tài)響應(yīng)譜關(guān)節(jié)反力的模是Tp和t的函數(shù)，其中t∈，動態(tài)響應(yīng)譜是關(guān)節(jié)反力的模隨時間和軌跡時間長度改變而發(fā)生變化的圖譜；(5)從動態(tài)響應(yīng)譜中選擇合適的軌跡時間變量，使得關(guān)節(jié)連接的兩構(gòu)件在運動過程中始終保持接觸軌跡規(guī)劃中軌跡時間變量Tp的取值依據(jù)為，Tp的取值必須滿足對任意t∈，各關(guān)節(jié)反力的模大于0；(6)若動態(tài)響應(yīng)譜中沒有符合要求的軌跡時間變量時，則增加運動多項式函數(shù)的階次H，返回步驟(1)；否則，若找到了合適的軌跡時間變量，則軌跡規(guī)劃完成。F200910154130XC00011.tif,F200910154130XC00013.tif2.根據(jù)權(quán)利要求1所述的一種基于動態(tài)響應(yīng)譜的關(guān)節(jié)含間隙機器人軌跡規(guī)劃方法，其特征在于根據(jù)步驟(4)構(gòu)造的關(guān)節(jié)反力的動態(tài)響應(yīng)譜是一個三維曲面，有兩個自變量Tp和t，其中tG[O，Tp]，函數(shù)值為關(guān)節(jié)反力的模。3.根據(jù)權(quán)利要求1所述的一種基于動態(tài)響應(yīng)譜的關(guān)節(jié)含間隙機器人軌跡規(guī)劃方法，其特征在于根據(jù)步驟(5)，軌跡時間變量直接從動態(tài)響應(yīng)譜中選取，使含間隙關(guān)節(jié)元素始終保持接觸狀態(tài)。全文摘要本發(fā)明公開了一種基于動態(tài)響應(yīng)譜的關(guān)節(jié)含間隙機器人軌跡規(guī)劃方法。這種方法針對特定的機器人任務(wù)規(guī)劃，在路徑關(guān)鍵點之間以運動多項式函數(shù)進行機器人關(guān)節(jié)空間軌跡規(guī)劃，把機器人每段運動軌跡的軌跡時間長度作為軌跡規(guī)劃變量，獲得關(guān)節(jié)反力隨時間和軌跡規(guī)劃變量改變而發(fā)生變化的動態(tài)響應(yīng)譜。從動態(tài)響應(yīng)譜中可以便捷地選擇合理的軌跡時間長度，使機器人運動過程中含間隙關(guān)節(jié)連接的兩構(gòu)件始終不發(fā)生分離。這種方法不需要附加彈簧或配重等硬件物體，只需通過設(shè)計軟件算法控制機器人軌跡即可取得效果。通過構(gòu)造軌跡時間長度連續(xù)的動態(tài)響應(yīng)譜，可以指導(dǎo)設(shè)計人員如何選擇軌跡時間長度以避免關(guān)節(jié)元素發(fā)生分離。文檔編號B25J9/16GK101733750SQ200910154130公開日2010年6月16日申請日期2009年11月5日優(yōu)先權(quán)日2009年11月5日發(fā)明者劉振宇,卜王輝,譚建榮申請人:浙江大學(xué)