專利名稱：一種冗余度機(jī)械臂的新型加速度層重復(fù)運(yùn)動(dòng)規(guī)劃方法
技術(shù)領(lǐng)域：
本發(fā)明涉及冗余度機(jī)械臂運(yùn)動(dòng)規(guī)劃及控制領(lǐng)域，具體涉及一種冗余度機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解方法。
背景技術(shù)：
冗余度機(jī)械臂具有自由度大于任務(wù)空間所需最少自由度的特性，廣泛應(yīng)用于裝備制造、產(chǎn)品加工、機(jī)器作業(yè)等國(guó)民經(jīng)濟(jì)生產(chǎn)活動(dòng)中。冗余度機(jī)械臂的逆運(yùn)動(dòng)學(xué)問題是指已知機(jī)械臂末端位姿，確定機(jī)械臂的關(guān)節(jié)角問題。當(dāng)冗余度機(jī)械臂末端任務(wù)為一個(gè)封閉曲線時(shí)，其各個(gè)關(guān)節(jié)可能回不到初始位置，這種現(xiàn)象叫做關(guān)節(jié)角偏差現(xiàn)象，或稱非重復(fù)運(yùn)動(dòng)問題，如果冗余度機(jī)械臂不能實(shí)現(xiàn)重復(fù)運(yùn)動(dòng)，則在運(yùn)行過程中會(huì)產(chǎn)生不能預(yù)料甚至是危險(xiǎn)有損機(jī)械臂與周圍人員安全的問題；而重復(fù)運(yùn)動(dòng)規(guī)劃方案就是要設(shè)計(jì)適當(dāng)?shù)男阅苤笜?biāo)，使得機(jī)械臂末端執(zhí)行完封閉曲線任務(wù)時(shí)，各個(gè)關(guān)節(jié)角都能夠回到其初始位置。以往的重復(fù)運(yùn)動(dòng)解析方法大多都是在速度層上進(jìn)行，得到的結(jié)果為速度值，這不能滿足某些采用非速度控制的冗 余度機(jī)械臂的要求，且速度層上的求解方法并不考慮加速度極限，機(jī)械臂在運(yùn)動(dòng)過程中就有可能因超越加速度極限而產(chǎn)生物理?yè)p壞。本發(fā)明人團(tuán)隊(duì)之前的發(fā)明《一種冗余度機(jī)械臂重復(fù)運(yùn)動(dòng)規(guī)劃的方法》(申請(qǐng)?zhí)?201010264141.6)則為一種加速度層的重復(fù)運(yùn)動(dòng)解析方法，可以克服上述速度層控制的冗余度機(jī)械臂的缺陷，但是，該發(fā)明方案單一，并不能滿足實(shí)際應(yīng)用中對(duì)不同類型加速度層重復(fù)運(yùn)動(dòng)方案參數(shù)選擇的需求。本發(fā)明專利提出一種新型的加速度層重復(fù)運(yùn)動(dòng)規(guī)劃方案，有效地克服了以往加速度層重復(fù)運(yùn)動(dòng)規(guī)劃中參數(shù)選擇的局限，具有新穎性與實(shí)用性。
發(fā)明內(nèi)容
本發(fā)明所要解決的技術(shù)問題是提供一種新型的、計(jì)算量小、普遍用于加速度控制的冗余度機(jī)械臂重復(fù)運(yùn)動(dòng)規(guī)劃方法。為解決上述技術(shù)問題，本發(fā)明通過以下技術(shù)方案予以實(shí)現(xiàn):1、一種冗余度機(jī)械臂的新型加速度層重復(fù)運(yùn)動(dòng)規(guī)劃方法包括如下步驟:I)通過上位機(jī)采用二次型優(yōu)化方案在加速度層上對(duì)機(jī)械臂的重復(fù)運(yùn)動(dòng)進(jìn)行逆運(yùn)動(dòng)學(xué)求解，設(shè)計(jì)的最小化性能指標(biāo)為重復(fù)運(yùn)動(dòng)，受約束于加速度雅可比等式、障礙物躲避不等式、關(guān)節(jié)角極限、關(guān)節(jié)速度極限和關(guān)節(jié)加速度極限；2)將步驟I)的二次型優(yōu)化方案轉(zhuǎn)化為QP (二次規(guī)劃)；3)將步驟2)的QP用QP求解器進(jìn)行求解；4)將步驟3)的求解結(jié)果傳遞給下位機(jī)控制器驅(qū)動(dòng)機(jī)械臂運(yùn)動(dòng)。2、冗余度機(jī)械臂加速度層重復(fù)運(yùn)動(dòng)方案設(shè)計(jì)為:最小化的W/2 + /々，受約束于J^ = ra ^A0<b^ θ~ <θ<θ+ ^ θ~ <θ<θ+ ^,其中沒表示關(guān)節(jié)加速度，W為單位陣，= + 叫 0)]，ρτ 表示 P 的轉(zhuǎn)置，α>0 和 β>0 且 α2<4β 是用來(lái)控制關(guān)節(jié)位移幅值和收斂率的設(shè)計(jì)參數(shù)，等式約束4表示機(jī)械臂末端運(yùn)動(dòng)軌跡，J表示機(jī)械臂的雅可比矩陣，j是其時(shí)間導(dǎo)數(shù)，Θ表示關(guān)節(jié)角表示關(guān)節(jié)速度，表示機(jī)械臂末端執(zhí)行器加速度矢量，θ_< θ <分別表示關(guān)節(jié)角極限、關(guān)節(jié)速
度極限和關(guān)節(jié)加速度極限，θ 土表示關(guān)節(jié)角上下限，&表示關(guān)節(jié)速度上下限，滬表示關(guān)節(jié)加速度上下限，A和b是障礙物躲避參數(shù)。3、上述的二次型優(yōu)化方案可以轉(zhuǎn)化為一個(gè)QP，即最小化XTWX/2+pTX，受約束于Cx = d, Ax ^ b, ζ ^ X ^ ζ+，其中，= ，W 為單位矩陣，/ = α6 + α[ _ (0)]，C = J,
d = ra=r-:W, ζ +和ζ_分別表示合成雙端約束的上下極限，它們第i個(gè)元素分別定義為
權(quán)利要求
1.一種冗余度機(jī)械臂的新型加速度層重復(fù)運(yùn)動(dòng)規(guī)劃方法，其特征在于包括如下步驟: 1)通過上位機(jī)采用新型二次型優(yōu)化方案在加速度層上對(duì)機(jī)械臂的重復(fù)運(yùn)動(dòng)進(jìn)行冗余度解析，設(shè)計(jì)的新型最小化性能指標(biāo)為重復(fù)運(yùn)動(dòng)，受約束于加速度雅可比等式、障礙物躲避不等式、關(guān)節(jié)角極限、關(guān)節(jié)速度極限和關(guān)節(jié)加速度極限，即為最小化
2.根據(jù)權(quán)利要求1所述的冗余度機(jī)械臂加速度層重復(fù)運(yùn)動(dòng)規(guī)劃方法，其特征在于所述步驟2)的新型二次型優(yōu)化轉(zhuǎn)化為一個(gè)QP，設(shè)計(jì)其性能指標(biāo)為最小化xTWX/2+pTX，受約束于 Cx = d, Ax < b, ζ ^ X ^ ζ+，其中，x ，W 為單位矩陣
3.根據(jù)權(quán)利要求2所述的冗余度機(jī)械臂加速度層重復(fù)運(yùn)動(dòng)規(guī)劃方法，其特征在于所述步驟3)的QP求解器求解。
4.根據(jù)權(quán)利要求3所述的冗余度機(jī)械臂加速度層重復(fù)運(yùn)動(dòng)規(guī)劃方法，其特征在于所述步驟3)的QP求解器求解結(jié)果轉(zhuǎn)化為電機(jī)驅(qū)動(dòng)所需要的控制信號(hào)，從而驅(qū)動(dòng)各關(guān)節(jié)電機(jī)使機(jī)械臂重復(fù)運(yùn)動(dòng)。
全文摘要
本發(fā)明提供了一種冗余度機(jī)械臂的新型加速度層重復(fù)運(yùn)動(dòng)規(guī)劃方法，包括如下步驟1)設(shè)計(jì)新型二次型形式的加速度層重復(fù)運(yùn)動(dòng)性能指標(biāo)，生成二次型優(yōu)化冗余度解析方案；2)將步驟1)的二次型優(yōu)化冗余度解析方案轉(zhuǎn)化為QP(二次規(guī)劃)；3)將步驟2)的QP運(yùn)用QP求解器進(jìn)行求解；4)將步驟3)的求解結(jié)果傳遞給下位機(jī)控制器驅(qū)動(dòng)機(jī)械臂運(yùn)動(dòng)。本發(fā)明突破了以往參數(shù)選擇中的局限，可以較大范圍地選取正的設(shè)計(jì)參數(shù)，使得冗余度重復(fù)運(yùn)動(dòng)規(guī)劃可選擇的方案顯著增加，這在工程應(yīng)用領(lǐng)域具有良好前景。
文檔編號(hào)B25J13/00GK103231381SQ201310160659
公開日2013年8月7日 申請(qǐng)日期2013年5月3日 優(yōu)先權(quán)日2013年5月3日
發(fā)明者張雨濃, 金龍, 王英 申請(qǐng)人:中山大學(xué)