專利名稱：一種冗余度機(jī)械臂重復(fù)運(yùn)動(dòng)規(guī)劃方法
技術(shù)領(lǐng)域：
本發(fā)明涉及冗余度機(jī)械臂控制領(lǐng)域，具體涉及一種冗余度機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解 方法。
背景技術(shù)：
冗余度機(jī)械臂是一種自由度大于任務(wù)空間所需最少自由度的末端能動(dòng)機(jī)械裝置， 其運(yùn)動(dòng)任務(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)問題；而重復(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)生物理損壞。本發(fā)明能彌補(bǔ)這 些不足。

發(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ī)劃方法包括如下步驟1)通過上位機(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)將步驟1)的二次型優(yōu)化轉(zhuǎn)化為標(biāo)準(zhǔn)二次規(guī)劃；3)將步驟2)的標(biāo)準(zhǔn)二次規(guī)劃用基于線性投影方程的原對(duì)偶神經(jīng)網(wǎng)絡(luò)求解器或二 次規(guī)劃數(shù)值方法進(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 + ；/鄉(xiāng)，受約束于 J0 = ra, θ-彡θ ( Θ\θ-<θ<θ+,滬，其中盧表示關(guān)節(jié)加速度，W為合適維數(shù)的 單位陣，ξ =廠-括,ρ = {μ + ν)θ + μ·ν[θ-θ(0)]，ρτ表示ρ的轉(zhuǎn)置，μ和ν是用來控制關(guān)節(jié)
位移幅值和收斂率的正設(shè)計(jì)參數(shù)，等式約束·/々=^1表示機(jī)械臂末端運(yùn)動(dòng)軌跡，J表示機(jī)械臂 的雅可比矩陣，j是其時(shí)間導(dǎo)數(shù)，θ表示關(guān)節(jié)角，力表示關(guān)節(jié)速度，卩·表示機(jī)械臂末端執(zhí)行器加 速度矢量，θ彡0+、大^々<伊、力-<力、滬分別表示關(guān)節(jié)角極限、關(guān)節(jié)速度極限和關(guān) 節(jié)加速度極限，θ 土表示關(guān)節(jié)角上下限，爐表示關(guān)節(jié)速度上下限，爐表示關(guān)節(jié)加速度上下限。
3、上述的二次型優(yōu)化可以轉(zhuǎn)化為一個(gè)標(biāo)準(zhǔn)二次規(guī)劃，即最小化xTWX/2+zTX，受約束 于Cx = d，ζ ^ x^ ‘+，其中，；^鄉(xiāng)，^為單位矩陣』=…+ ”)々+ //+!^-^))]，。=， d = ra=r-J9 , ； = max {λθ； -- φ, θ；} , =min{、(;i《-《)人(牙-φ，殘+}，正常數(shù)
1^、1^、λ用來調(diào)節(jié)關(guān)節(jié)加速度可行域。4、此標(biāo)準(zhǔn)二次規(guī)劃可以轉(zhuǎn)化為一個(gè)線性投影方程ΡΩ [u-(Mu+q)]-u = 0的求解，其 中ΡΩ[ ·]為空間Rn+m到集合Ω = { 丨《_““+} c/r+"的分段線性投影算子，m為笛卡爾空 間的維數(shù)，η為關(guān)節(jié)空間的維數(shù)，u表示原對(duì)偶變量，U"表示原對(duì)偶變量下極限，U+表示原對(duì) 偶變量上極限，M、q為相關(guān)擴(kuò)展矩陣。5、該線性投影方程用原對(duì)偶神經(jīng)網(wǎng)絡(luò)求解器或二次規(guī)劃數(shù)值方法求解。


圖1為本發(fā)明的流程圖；圖2為實(shí)現(xiàn)本發(fā)明的機(jī)械臂三維模型圖；圖3為機(jī)械臂非重復(fù)運(yùn)動(dòng)示意圖；圖4為實(shí)現(xiàn)本發(fā)明的機(jī)械臂重復(fù)運(yùn)動(dòng)示意圖。
具體實(shí)施例方式下面結(jié)合附圖對(duì)本發(fā)明做進(jìn)一步的說明。圖1所示的冗余度機(jī)械臂重復(fù)運(yùn)動(dòng)規(guī)劃方法主要由加速度層重復(fù)運(yùn)動(dòng)性能指標(biāo) 與約束1、標(biāo)準(zhǔn)二次規(guī)劃2、基于線性投影方程的原對(duì)偶神經(jīng)網(wǎng)絡(luò)求解器或二次規(guī)劃數(shù)值方 法3、下位機(jī)控制器4和機(jī)械臂5組成。圖2所示實(shí)現(xiàn)本發(fā)明的機(jī)械臂為一個(gè)平面六自由度的機(jī)械臂。該機(jī)械臂由六個(gè)連 桿所組成，通過關(guān)節(jié)9、關(guān)節(jié)10、關(guān)節(jié)11、關(guān)節(jié)12、關(guān)節(jié)13和關(guān)節(jié)14組成。圖3所示機(jī)械臂的非重復(fù)運(yùn)動(dòng)示意圖。給定末端任務(wù)8為一個(gè)閉合曲線，機(jī)械臂 從初始位置6開始執(zhí)行，完成任務(wù)后，機(jī)械臂停在位置7，而并沒有回復(fù)到初始位置1。它沒 有做到重復(fù)運(yùn)動(dòng)；即，完成任務(wù)時(shí)，機(jī)械臂未回復(fù)到初始位置，各個(gè)關(guān)節(jié)末態(tài)位置角不等于 初始位置角。圖4為實(shí)現(xiàn)本發(fā)明的機(jī)械臂重復(fù)運(yùn)動(dòng)示意圖。本發(fā)明設(shè)計(jì)的加速度層重復(fù)運(yùn)動(dòng)規(guī)
劃方案為最小化擴(kuò)階鄉(xiāng)/2 + //鄉(xiāng)(1)約束條件《/盧=Ya=Y-'3Θ(2)θ < θ+ (3)θ~<θ<θ*(4)θ~ < θ <θ+ (5) 其中，^^/2 + ；/鄉(xiāng)為加速度層重復(fù)運(yùn)動(dòng)性能指標(biāo)，鄉(xiāng)表示關(guān)節(jié)加速度，W為合適維 數(shù)的單位陣，/^(// +…力+ //卞|>-~0)]，ρτ表示ρ的轉(zhuǎn)置，μ和V是用來控制關(guān)節(jié)位移幅
值和收斂率的正設(shè)計(jì)參數(shù)，等式約束·/々=^l表示機(jī)械臂末端運(yùn)動(dòng)軌跡，J表示機(jī)械
臂的雅可比 陣，j是其時(shí)間導(dǎo)數(shù)，θ表示關(guān)節(jié)角，々表示關(guān)節(jié)速度，f··表示機(jī)械臂末端執(zhí)行器加速度矢量
爐、&<鄉(xiāng)<滬分別表示關(guān)節(jié)角極限、關(guān)節(jié)速度極限和 關(guān)節(jié)加速度極限，θ ±表示關(guān)節(jié)角上下限，爐表示關(guān)節(jié)速度上下限，爐表示關(guān)節(jié)加速度上下 限?？紤]到上述優(yōu)化問題是在加速度層上求解，因此需將機(jī)械臂的關(guān)節(jié)角度約束(3)、 關(guān)節(jié)速度約束(4)，關(guān)節(jié)加速度約束(5)合并，從而可以得到基于加速度鄉(xiāng)的雙端不等式約 束 其中，ζ-和ζ +中的第i個(gè)元素分別定義為
民(圳--《)人樹--φ，殘_}，
<=111111氏(叫+-代)人(牙-幻，礦}。正常數(shù)1^、1^、λ用來調(diào)節(jié)關(guān)節(jié)加速度的可行域。用
χ表示機(jī)械臂的關(guān)節(jié)加速度鄉(xiāng)，帶物理約束的機(jī)械臂二次型優(yōu)化方案(1)_(5)便可描述為如 下的標(biāo)準(zhǔn)二次規(guī)劃方案最小化 約束條件
(6) 其中，義=鄉(xiāng)，1為單位矩陣，2=(// + ”)々 + //卞[沒-沒(0)],。= ],d = ra=r-j0, ；-OiUM~Α、Α、> =min丨、(圳+ -《)人(矛-Oi),θ]。上述標(biāo)準(zhǔn)二次規(guī)劃問
題可以轉(zhuǎn)化為一個(gè)線性投影方程ΡΩ [u-(Mu+q)]-u = 0的求解，其中ΡΩ [ ·]為空間Rn+m到 集合Ω = {Μ|Μ_&《Μ+}^2Τ+η的分段線性投影算子，m為笛卡爾空間的維數(shù)，η為關(guān)節(jié)空間 的維數(shù)，u表示原對(duì)偶變量，if表示原對(duì)偶變量下極限，U+表示原對(duì)偶變量上極限，原對(duì)偶變 量u及其上下限定義如下 y e Rm是對(duì)應(yīng)于等式約束々的對(duì)偶決策向量，Iv:= [1，...，1]工是元 素都為1的相應(yīng)維數(shù)向量；CT^O是足夠大的常數(shù)，用于數(shù)值上替代無窮大+⑴，而擴(kuò)展矩 陣M、q分別定義如下 接著，線性投影方程用原對(duì)偶神經(jīng)網(wǎng)絡(luò)求解器或二次規(guī)劃數(shù)值方法求 解。原對(duì)偶神經(jīng)網(wǎng)絡(luò)求解器的動(dòng)力學(xué)系統(tǒng)為 = χ{ + Μτ){Ρη [u-(Mu + q)]-u}或
"=/(仏|>-(她+力]- )。當(dāng)使用二次規(guī)劃數(shù)值方法時(shí)，設(shè)計(jì)其計(jì)算誤差為e(u)= ιι-ΡΩ[ιι-(Μιι+( )]，當(dāng)誤差為零時(shí)，對(duì)應(yīng)的u值便為分段線性方程的解。給定初始值u° e Rn, 通過如下的迭代來得到u使得誤差e (u)達(dá)到預(yù)設(shè)的精度uk+1 = uk-p (Uk) d (Uk)，其中k = 0,1,2,…，d(uk)和 P (uk)定義為 d(uk) = (MT+I)e(uk), P (uk) = | e (uk) | |2/ d(uk) | |2， 通過算法不斷迭代，便可得到分段線性投影方程Pi2 (u-(MU+P))-U = 0的解，從而得到加速 度層重復(fù)運(yùn)動(dòng)二次規(guī)劃的最優(yōu)解?，F(xiàn)結(jié)合一個(gè)具體的實(shí)例操作對(duì)本系統(tǒng)的工作流程進(jìn)行如下說明。在加速度層重復(fù)運(yùn)動(dòng)方案實(shí)施過程中，機(jī)械臂初始位置關(guān)節(jié)9-14角度設(shè)置為[π/3，π/θ, π/θ, 31/3, π/θ, π/6]τ 弧度，角度上極限設(shè)置為 Θ + = [2. 775，0. 750，3. 142， 2. 967,0. 035,3. 142]τ 弧度，角度下極限設(shè)置為 Θ — = [-2. 775,-3. 142,-0. 906,-1. 920,-1 .745,-3. ^？？弧度，速度上極限設(shè)置為擴(kuò)^丨么！及^，^，！及^^瓜度/秒，速度下極限設(shè) 置為^^-^，-^，-！么-！及-^，-^廣弧度/秒，加速度上極限設(shè)置為爐^^尖+令斗]7·
弧度/秒2，加速度下極限設(shè)置為=[_4，"4,4，4，-4，弧度/秒2。所使用的基于線性 投影方程的原對(duì)偶神經(jīng)網(wǎng)絡(luò)器動(dòng)力學(xué)系統(tǒng)為 ^^Ζ + Λ^^/^Μ—ΜΜ + )]- 丨。將計(jì)算得 到的加速度再傳送給機(jī)械臂控制器從而控制機(jī)械臂的運(yùn)動(dòng)。 機(jī)械臂從初始位置6開始執(zhí)行，完成任務(wù)后，機(jī)械臂回到了位置7，而位置7與位置 6重合；也即，機(jī)械臂的各個(gè)末態(tài)關(guān)節(jié)角等于初始關(guān)節(jié)角，機(jī)械臂實(shí)現(xiàn)了重復(fù)運(yùn)動(dòng)。
權(quán)利要求
一種冗余度機(jī)械臂重復(fù)運(yùn)動(dòng)的規(guī)劃方法，其特征在于包括如下步驟1)通過上位機(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)將步驟1)的二次型優(yōu)化轉(zhuǎn)化為標(biāo)準(zhǔn)二次規(guī)劃；3)將步驟2)的標(biāo)準(zhǔn)二次規(guī)劃用基于線性投影方程的原對(duì)偶神經(jīng)網(wǎng)絡(luò)求解器或二次規(guī)劃數(shù)值方法進(jìn)行求解；4)將步驟3)的求解結(jié)果傳遞給下位機(jī)控制器驅(qū)動(dòng)機(jī)械臂運(yùn)動(dòng)。
2.根據(jù)權(quán)利要求1所述的冗余度機(jī)械臂重復(fù)運(yùn)動(dòng)規(guī)劃方法，其特征在于所述步驟1)的 加速度層重復(fù)運(yùn)動(dòng)方案設(shè)計(jì)為最小化^^_/2 +廠力，受約束于加速度雅可比等式# = &、 關(guān)節(jié)角極限θ < θ+、關(guān)節(jié)速度極限爐，關(guān)節(jié)加速度極限化^鄉(xiāng)^滬，其中鄉(xiāng)表 示關(guān)節(jié)加速度，W為合適維數(shù)的單位陣，ξ—·-抽，Ρ = + + 卞|>-0(Ο)]，ρτ表示ρ的轉(zhuǎn)置，μ和ν是用來控制關(guān)節(jié)位移幅值和收斂率的正設(shè)計(jì)參數(shù)，等式約束·/々=^1表示機(jī)械臂 末端運(yùn)動(dòng)軌跡，J表示機(jī)械臂的雅可比矩陣，j是其時(shí)間導(dǎo)數(shù)，θ表示關(guān)節(jié)角，々表示關(guān)節(jié)速 度7表示機(jī)械臂末端執(zhí)行器加速度矢量，θ < ^\θτ<θ<θ^纊 <々、妒分別表示 關(guān)節(jié)角極限、關(guān)節(jié)速度極限和關(guān)節(jié)加速度極限，θ 土表示關(guān)節(jié)角上下限，爐表示關(guān)節(jié)速度上 下限，滬表示關(guān)節(jié)加速度上下限。
3.根據(jù)權(quán)利要求2所述的冗余度機(jī)械臂重復(fù)運(yùn)動(dòng)規(guī)劃方法，其特征在于所述步驟2) 的二次型優(yōu)化轉(zhuǎn)化為一個(gè)標(biāo)準(zhǔn)二次規(guī)劃，設(shè)計(jì)其性能指標(biāo)為最小化xTWX/2+zTX，受約束 于 Cx = d，ζ ^ χ ^ ζ+，其中，Λ =盧，W 為單位陣，2 = (// + ^ + //卞[沒-沒(0)]，C = J， d = ra=r-j0 , ； =Tmxi^UOr-Oi) Jcv0' -Oi),θ；}，{、(圳+ -Oi),kv0，正常數(shù) k,、kv 和 λ用來調(diào)節(jié)關(guān)節(jié)加速度的可行域。
4.根據(jù)權(quán)利要求3所述的冗余度機(jī)械臂重復(fù)運(yùn)動(dòng)規(guī)劃方法，其特征在于所述步驟3)的 標(biāo)準(zhǔn)二次規(guī)劃轉(zhuǎn)化為一個(gè)線性投影方程ΡΩ [u-(Mu+q)]-u = 0的求解，其中ΡΩ []為空間Rn+m 到集合Ω = { 丨M_ “ < +} c TT+"的分段線性投影算子，m為笛卡爾空間的維數(shù)，η為關(guān)節(jié)空 間的維數(shù)，u表示原對(duì)偶變量，if表示原對(duì)偶變量下極限，U+表示原對(duì)偶變量上極限，M、q為 相關(guān)擴(kuò)展矩陣。
5.根據(jù)權(quán)利要求4所述的冗余度機(jī)械臂重復(fù)運(yùn)動(dòng)規(guī)劃方法，其特征在于所述步驟3)的 線性投影方程，用原對(duì)偶神經(jīng)網(wǎng)絡(luò)求解器或二次規(guī)劃數(shù)值方法求解。
全文摘要
本發(fā)明提供了一種冗余度機(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)將步驟1)的二次型優(yōu)化轉(zhuǎn)化為標(biāo)準(zhǔn)二次規(guī)劃；3)將步驟2)的標(biāo)準(zhǔn)二次規(guī)劃用基于線性投影方程的原對(duì)偶神經(jīng)網(wǎng)絡(luò)求解器或二次規(guī)劃數(shù)值方法進(jìn)行求解；4)將步驟3)的求解結(jié)果傳遞給下位機(jī)控制器驅(qū)動(dòng)機(jī)械臂運(yùn)動(dòng)。本發(fā)明在加速度層上實(shí)現(xiàn)機(jī)械臂的重復(fù)運(yùn)動(dòng)，對(duì)于不便于速度控制的冗余度機(jī)械臂具有較好的控制效果，且本發(fā)明考慮到多種關(guān)節(jié)極限，能有效防止機(jī)械臂超越極限產(chǎn)生物理損壞。
文檔編號(hào)B25J13/00GK101927495SQ20101026414
公開日2010年12月29日 申請(qǐng)日期2010年8月25日 優(yōu)先權(quán)日2010年8月25日
發(fā)明者張智軍, 張雨濃, 朱虹 申請(qǐng)人:中山大學(xué)