一種冗余度機(jī)械臂加速度層的容錯(cuò)規(guī)劃方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及冗余度機(jī)械臂容錯(cuò)規(guī)劃及控制領(lǐng)域，具體涉及一種冗余度機(jī)械臂的逆 運(yùn)動(dòng)學(xué)解析方法。
【背景技術(shù)】
[0002] 冗余度機(jī)械臂，具有關(guān)節(jié)自由度數(shù)目大于任務(wù)空間所需最少自由度的特性，即：完 成某一特定任務(wù)時(shí)，機(jī)械臂具有多余的自由度。其應(yīng)用范圍十分廣泛，大到深海和宇宙空 間的探索，小到產(chǎn)品加工和家庭服務(wù)等不同領(lǐng)域都可以發(fā)現(xiàn)其身影。冗余度機(jī)械臂的逆運(yùn) 動(dòng)學(xué)問(wèn)題是指已知機(jī)械臂末端位姿，確定機(jī)械臂的關(guān)節(jié)角度的問(wèn)題。由于具備冗余的自由 度，冗余度機(jī)械臂的逆運(yùn)動(dòng)學(xué)問(wèn)題具有無(wú)窮多解，即保證機(jī)械臂末端在完成所要求任務(wù)的 同時(shí)，還能根據(jù)不同的任務(wù)要求和工作環(huán)境選擇不同的優(yōu)化指標(biāo)。
[0003] 以往的容錯(cuò)規(guī)劃方法大多是在速度層上進(jìn)行的，其得到結(jié)果為速度值，而這并不 能滿足某些采用非速度控制的冗余度機(jī)械臂的要求。更重要的是，速度層上的求解方法未 考慮到加速度的極限，這使得機(jī)械臂在執(zhí)行任務(wù)的過(guò)程中，存在超越加速度極限從而造成 物理?yè)p壞的可能性。
[0004] 見本發(fā)明人團(tuán)隊(duì)之前的中國(guó)發(fā)明專利，名稱：《一種冗余度機(jī)械臂容錯(cuò)規(guī)劃的方 法》，專利號(hào)=201010264141. 6,為一種速度層的容錯(cuò)解析方法，可以克服上述速度層控制的 冗余度機(jī)械臂的容錯(cuò)問(wèn)題。但是，該發(fā)明的技術(shù)方案，仍在以下不足之處：（1)不能消除機(jī) 械臂超越加速度極限，從而造成物理?yè)p壞的可能性，而且無(wú)法實(shí)現(xiàn)障礙物躲避。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明的目的在于克服上述現(xiàn)有技術(shù)存在的不足，而提供一種新型的、計(jì)算量小 的、普遍用于加速度控制的冗余度機(jī)械臂容錯(cuò)規(guī)劃方法，有效地消除機(jī)械臂超越加速度極 限而造成物理?yè)p壞的可能性，其推導(dǎo)過(guò)程從速度到加速度更深一層，且可以實(shí)現(xiàn)障礙物躲 避，因此具有較強(qiáng)的適用性、安全性和實(shí)用性。
[0006] 本發(fā)明的目的是采用以下技術(shù)措施解決。
[0007] -種冗余度機(jī)械臂的新型加速度層的容錯(cuò)規(guī)劃方法包括如下步驟： 步驟一，通過(guò)上位機(jī)采用二次型優(yōu)化方案在加速度層上對(duì)機(jī)械臂的容錯(cuò)進(jìn)行逆運(yùn)動(dòng)學(xué) 解析，設(shè)計(jì)的最小化性能指標(biāo)為容錯(cuò)，受約束于加速度雅可比等式、障礙物躲避不等式、關(guān) 節(jié)角度極限、關(guān)節(jié)速度極限和關(guān)節(jié)加速度極限； 步驟二，將步驟一的二次型優(yōu)化方案轉(zhuǎn)化為QP，即：二次規(guī)劃； 步驟三，將步驟二的QP用QP求解器進(jìn)行求解； 步驟四，將步驟三的求解結(jié)果傳遞給下位機(jī)控制器，以驅(qū)動(dòng)機(jī)械臂運(yùn)動(dòng)。
[0008] 冗余度機(jī)械臂加速度層的容錯(cuò)方案設(shè)計(jì)為：最小化
，受約束于
，其中e表示關(guān)節(jié)角度，々表示機(jī)械 臂的關(guān)節(jié)速度，#表示機(jī)械臂的關(guān)節(jié)加速度，表示關(guān)節(jié)角度上下限，夕表示關(guān)節(jié)速度上 下限，表示關(guān)節(jié)加速度上下限，上標(biāo)T表示矩陣和向量的轉(zhuǎn)置，
，J是用來(lái)控 制收斂率的設(shè)計(jì)參數(shù)，g是容錯(cuò)矩陣（當(dāng)?shù)趈個(gè)關(guān)節(jié)出錯(cuò)鎖定時(shí)，其中je{X2,…，氣丨，^中 對(duì)應(yīng)對(duì)角線上的元素?賦值為1，其它元素賦值為0)，等式約束5對(duì)應(yīng)機(jī)械臂末端運(yùn) 動(dòng)軌跡，/表示機(jī)械臂的雅可比矩陣，j是/的時(shí)間導(dǎo)數(shù)，
，卩表示機(jī)械臂末端執(zhí) 行器加速度向量，d和&是障礙物躲避參數(shù)。
[0009] 上述二次型優(yōu)化方案可以轉(zhuǎn)化為一個(gè)QP，即最小化
，受約束于
分別表不合成雙端 約束的上下限，它們第，個(gè)元素分別定義為
和
，其中i = U《表示關(guān)節(jié)序號(hào)，正常數(shù) < 和心用來(lái) 調(diào)節(jié)關(guān)節(jié)加速度并保證關(guān)節(jié)加速度有足夠大的可行域，巧為裕度。
[0010] 作為更具體的方案，上述QP可以用QP求解器求解。
[0011] 上述QP求解器，求解結(jié)果轉(zhuǎn)化為電機(jī)驅(qū)動(dòng)所需要的控制信號(hào)，從而驅(qū)動(dòng)各關(guān)節(jié)電 機(jī)使機(jī)械臂具有容錯(cuò)性能。
[0012] 本發(fā)明的有益效果如下。
[0013] 以往的冗余度機(jī)械臂不考慮加速度的極限，因而存在超越加速度極限從而造成物 理?yè)p壞的可能性，而且無(wú)法實(shí)現(xiàn)障礙物躲避，本發(fā)明給出了一個(gè)新型的加速度層的容錯(cuò)方 案，有效地消除了機(jī)械臂超越加速度極限從而造成物理?yè)p壞的可能性，同時(shí)其推導(dǎo)過(guò)程更 加深入嚴(yán)密，且能夠?qū)崿F(xiàn)障礙物躲避，因此，在實(shí)際應(yīng)用中有非常重要的作用和廣闊的前 景。
【附圖說(shuō)明】
[0014] 圖1為本發(fā)明的流程圖。
[0015] 圖2為實(shí)現(xiàn)本發(fā)明的機(jī)械臂二維模型圖。
[0016] 圖3為實(shí)現(xiàn)本發(fā)明的機(jī)械臂容錯(cuò)示意圖。
[0017] 圖4為實(shí)現(xiàn)本發(fā)明的機(jī)械臂關(guān)節(jié)角度示意圖。
[0018] 圖5為實(shí)現(xiàn)本發(fā)明的機(jī)械臂關(guān)節(jié)速度示意圖。
[0019] 圖6為實(shí)現(xiàn)本發(fā)明的機(jī)械臂關(guān)節(jié)加速度示意圖。
【具體實(shí)施方式】
[0020] 下面結(jié)合附圖對(duì)本發(fā)明做進(jìn)一步的說(shuō)明。
[0021] 如圖1所示，本發(fā)明冗余度機(jī)械臂容錯(cuò)規(guī)劃方法，首先提出加速度層的容錯(cuò)性能 指標(biāo)與約束；然后將其轉(zhuǎn)化為相應(yīng)的QP ;接著應(yīng)用QP求解器求解；最后下位機(jī)控制器根據(jù) 求解結(jié)果控制機(jī)械臂的運(yùn)動(dòng)。
[0022] 見圖2所示，實(shí)現(xiàn)本發(fā)明的機(jī)械臂為一個(gè)平面六連桿機(jī)械臂；該機(jī)械臂由六個(gè)連 桿組成，通過(guò)關(guān)節(jié)1、關(guān)節(jié)2、關(guān)節(jié)3、關(guān)節(jié)4、關(guān)節(jié)5和關(guān)節(jié)6組成。
[0023] 見圖3所示，為平面六連桿機(jī)械臂的容錯(cuò)示意圖；給定末端任務(wù)為畫出一個(gè)平面 圓形，當(dāng)關(guān)節(jié)1和關(guān)節(jié)2都出錯(cuò)鎖定時(shí)，其它關(guān)節(jié)及時(shí)調(diào)整到最佳運(yùn)行軌跡，以保證機(jī)械臂 末端完成畫圓的任務(wù)。
[0024]見圖4所示，為平面六連桿機(jī)械臂的六個(gè)關(guān)節(jié)的關(guān)節(jié)角度示意圖；其中，關(guān)節(jié)1、關(guān) 節(jié)2、關(guān)節(jié)3、關(guān)節(jié)4、關(guān)節(jié)5和關(guān)節(jié)6的關(guān)節(jié)角度分別是巧、馬、色、巧、咚和烏。
[0025]見圖5所示，為平面六連桿機(jī)械臂的六個(gè)關(guān)節(jié)的關(guān)節(jié)速度示意圖；其中，關(guān)節(jié)1、關(guān) 節(jié)2、關(guān)節(jié)3、關(guān)節(jié)4、關(guān)節(jié)5和關(guān)節(jié)6的關(guān)節(jié)速度分別是戎、4、4、4、4和4。
[0026] 見圖6所示，為平面六連桿機(jī)械臂的六個(gè)關(guān)節(jié)的關(guān)節(jié)加速度示意圖；其中，關(guān)節(jié)1、 關(guān)節(jié)2、關(guān)節(jié)3、關(guān)節(jié)4、關(guān)節(jié)5和關(guān)節(jié)6的關(guān)節(jié)加速度分別是吳、兵、鳥、兵、馬和4。
[0027] 本發(fā)明設(shè)計(jì)的加速度層容錯(cuò)規(guī)劃方案為
其中1^/2+/備為新的加速度層的容錯(cuò)性能指標(biāo)，$表示機(jī)械臂的關(guān)節(jié)角度，表示 機(jī)械臂的關(guān)節(jié)