速度，g表示機(jī)械臂的關(guān)節(jié)加速度，鏟表示關(guān)節(jié)角度上下限，妒表示關(guān)節(jié)速 度上下限，占*表示關(guān)節(jié)加速度上下限，上標(biāo)*表示矩陣和向量的轉(zhuǎn)置，，a是用 來(lái)控制收斂率的設(shè)計(jì)參數(shù)，g是容錯(cuò)矩陣（當(dāng)?shù)趈個(gè)關(guān)節(jié)出錯(cuò)鎖定時(shí)，其中， g中對(duì)應(yīng)對(duì)角線上的元素賦值為1，其它元素賦值為0)，等式約束《對(duì)應(yīng)機(jī)械臂末 端運(yùn)動(dòng)軌跡，J表示機(jī)械臂的雅可比矩陣，j是J的時(shí)間導(dǎo)數(shù)，6 P表示機(jī)械臂末 端執(zhí)行器加速度向量，d和&是障礙物躲避參數(shù)。
[0028]考慮到上述優(yōu)化問(wèn)題是在加速度層上求解，因此需將機(jī)械臂的關(guān)節(jié)角度約束（4)、 關(guān)節(jié)速度約束（5)和關(guān)節(jié)加速度約束（6)合并，從而可以得到基于加速度#的雙端不等式 約束： <&<<;* , 其中f和f分別表示合成雙端約束的上下限，它們第f個(gè)元素分別定義為
,其中 /= 1,2,…,rt表示關(guān)節(jié)序號(hào)，正常數(shù)&和&用來(lái)調(diào)節(jié)關(guān)節(jié)加速度并保證關(guān)節(jié)加速度有足夠 大的可行域，爲(wèi)為裕度。用Z表示機(jī)械臂的關(guān)節(jié)加速度邊，帶物理約束的機(jī)械臂的新型二次 型優(yōu)化方案（1)_(6)便可描述為如下的QP方案：
其中x=盧，C= J。上述QP問(wèn)題求解等價(jià)于尋求一個(gè)向量


1表示由等式約束（8)和不 等式約束（9)得到的向量，1。和1,分別表示由1組成的?維和dm(句維向量，〇于數(shù) 值上替代無(wú)窮大，
其中J是單位矩陣。
[0029] 上述問(wèn)題又可以等價(jià)為分段線性投影方程
，其中
設(shè)計(jì)其計(jì)算誤差為
：當(dāng)誤差為零時(shí)，其對(duì)應(yīng)的直便為分段 線性方程的解，其前《個(gè)元素組成QP的解x。給定初始值
，通過(guò)如下的迭代 來(lái)得到.F使得誤差達(dá)到預(yù)設(shè)的精度，運(yùn)行以下迭代：
其中
，迭代次數(shù) i = 。通過(guò)算法不斷迭代，得到分段線性投影方程
的解，從而 得到冗余度機(jī)械臂運(yùn)動(dòng)規(guī)劃的最優(yōu)解。
[0030] 通過(guò)上位機(jī)的QP求解器得到該QP的解后，再將結(jié)果轉(zhuǎn)化為電機(jī)驅(qū)動(dòng)所需要的控 制信號(hào)，并通過(guò)該冗余度機(jī)械臂的控制器來(lái)驅(qū)動(dòng)機(jī)械臂各個(gè)關(guān)節(jié)電機(jī)運(yùn)動(dòng)，從而驅(qū)動(dòng)各關(guān) 節(jié)電機(jī)使機(jī)械臂具有容錯(cuò)性能。
[0031] 現(xiàn)結(jié)合一個(gè)具體的實(shí)例操作對(duì)本發(fā)明的工作流程進(jìn)行如下說(shuō)明。
[0032] 在加速度層的容錯(cuò)方案實(shí)施過(guò)程中實(shí)現(xiàn)障礙物躲避，
機(jī)械臂初始位置關(guān)節(jié)1至6的角度設(shè)置為
弧度，角 度上限設(shè)置為
弧度，角度下限設(shè)置 為
弧度，速度上限設(shè)置為少
弧度/秒，速度下限設(shè)置為
弧度/秒，加速度上限設(shè)置為
弧度/秒2，加速度下限設(shè)置為孑
弧度/秒2。通過(guò)QP求解器求解，將計(jì)算得到的結(jié)果再傳送給機(jī)械臂 控制器從而控制機(jī)械臂的運(yùn)動(dòng)。
[0033]平面六連桿機(jī)械臂的關(guān)節(jié)1和關(guān)節(jié)2出錯(cuò)鎖定時(shí)，機(jī)械臂未出錯(cuò)的四個(gè)關(guān)節(jié)，即關(guān) 節(jié)3、關(guān)節(jié)4、關(guān)節(jié)5和關(guān)節(jié)6,開(kāi)始執(zhí)行任務(wù)，平面六連桿機(jī)械臂的末端最終成功畫(huà)出一個(gè) 圓，也即，平面六連桿機(jī)械臂根據(jù)任務(wù)要求和工作環(huán)境，很好地展示出其容錯(cuò)性能。
【主權(quán)項(xiàng)】
1. 一種冗余度機(jī)械臂的加速度層的容錯(cuò)規(guī)劃方法，其特征在于包括如下步驟： 步驟一，通過(guò)上位機(jī)采用新型二次型優(yōu)化方案在加速度層上對(duì)機(jī)械臂的容錯(cuò) 進(jìn)行冗余度解析，設(shè)計(jì)的新型最小化性能指標(biāo)為容錯(cuò)，受約束于加速度雅可比等式、 障礙物躲避不等式、關(guān)節(jié)角度極限、關(guān)節(jié)速度極限和關(guān)節(jié)加速度極限，即為最小化 穸占/2.+ /吾，受約束于加速度雅可比等式g、障礙物躲避不等式約束，關(guān)節(jié)角 度極限r(nóng)^沒(méi)sr、關(guān)節(jié)速度極限r(nóng) ，關(guān)節(jié)加速度極限r(nóng)^，其中s表示機(jī) 械臂的關(guān)節(jié)角度，^表示機(jī)械臂的關(guān)節(jié)速度，&表示機(jī)械臂的關(guān)節(jié)加速度，f表示關(guān)節(jié)角 度上下限，妒表示關(guān)節(jié)速度上下限，占1表示關(guān)節(jié)加速度上下限，上標(biāo)T表示矩陣和向量的 轉(zhuǎn)置，##，；!是用來(lái)控制收斂率的設(shè)計(jì)參數(shù)，g是容錯(cuò)矩陣（當(dāng)?shù)趈個(gè)關(guān)節(jié)出錯(cuò)鎖 定時(shí)，其中，g中對(duì)應(yīng)對(duì)角線上的元素仏賦值為1，其它元素賦值為〇)，等 式約束?/#=《對(duì)應(yīng)機(jī)械臂末端運(yùn)動(dòng)軌跡，j表示機(jī)械臂的雅可比矩陣，j是j的時(shí)間導(dǎo)數(shù)， & p表示機(jī)械臂末端執(zhí)行器加速度向量，j和&是障礙物躲避參數(shù)； 步驟二，將步驟一的面向加速度層的容錯(cuò)的二次型優(yōu)化方案轉(zhuǎn)化為QP(二次規(guī)劃）； 步驟三，將步驟二的QP用數(shù)值算法(也即QP求解器）進(jìn)行求解； 步驟四，將步驟三的求解結(jié)果傳遞給下位機(jī)控制器驅(qū)動(dòng)機(jī)械臂運(yùn)動(dòng)。2. 根據(jù)權(quán)利要求1所述的冗余度機(jī)械臂加速度層的容錯(cuò)規(guī)劃方法，其特征在于所述 步驟二的新型二次型優(yōu)化方案轉(zhuǎn)化為一個(gè)QP，設(shè)計(jì)其性能指標(biāo)為最小化rTx/2 + /x，受 約束于，其中x=#，和分別表示合成雙 端約束的上下限，它們的第|個(gè)元素分別定義為和，其中1 = 1,2,《表示關(guān)節(jié)序號(hào)，正常數(shù)L和&用 來(lái)調(diào)節(jié)關(guān)節(jié)加速度并保證關(guān)節(jié)加速度有足夠大的可行域，A為裕度。3. 根據(jù)權(quán)利要求2所述的冗余度機(jī)械臂加速度層的容錯(cuò)規(guī)劃方法，其特征在于所述步 驟三的QP求解器求解。4. 根據(jù)權(quán)利要求3所述的冗余度機(jī)械臂加速度層的容錯(cuò)規(guī)劃方法，其特征在于所述步 驟三的QP求解器求解結(jié)果轉(zhuǎn)化為電機(jī)驅(qū)動(dòng)所需要的控制信號(hào)，從而驅(qū)動(dòng)各關(guān)節(jié)電機(jī)使機(jī) 械臂具備容錯(cuò)性能。
【專利摘要】本發(fā)明提供了一種冗余度機(jī)械臂的加速度層的容錯(cuò)規(guī)劃方法，包括如下步驟：步驟一，先設(shè)計(jì)二次型形式的加速度層的容錯(cuò)性能指標(biāo)，生成二次型優(yōu)化冗余度解析方案；步驟二，將步驟一的二次型優(yōu)化冗余度解析方案轉(zhuǎn)化為QP，即：二次規(guī)劃；步驟三，將步驟二的QP運(yùn)用QP求解器進(jìn)行求解；步驟四，將步驟三的求解結(jié)果傳遞給下位機(jī)控制器驅(qū)動(dòng)機(jī)械臂運(yùn)動(dòng)。本發(fā)明通過(guò)設(shè)計(jì)一種冗余度機(jī)械臂的新型加速度層的容錯(cuò)性能指標(biāo)，保證了末端執(zhí)行器在關(guān)節(jié)出錯(cuò)鎖定的情況下仍能完成要求的任務(wù)，提高了冗余度機(jī)械臂的安全性和有效性，這在工程應(yīng)用領(lǐng)域具有良好前景。
【IPC分類】B25J9/16
【公開(kāi)號(hào)】CN104908040
【申請(qǐng)?zhí)枴緾N201510347924
【發(fā)明人】張雨濃, 王金金, 金龍, 晏小剛, 譚洪舟
【申請(qǐng)人】廣東順德中山大學(xué)卡內(nèi)基梅隆大學(xué)國(guó)際聯(lián)合研究院, 中山大學(xué)
【公開(kāi)日】2015年9月16日
【申請(qǐng)日】2015年6月23日