專利名稱：：一種冗余度機(jī)械臂運(yùn)動(dòng)規(guī)劃方法
技術(shù)領(lǐng)域：
：本發(fā)明屬于冗余度機(jī)械臂運(yùn)動(dòng)規(guī)劃方法，特別是涉及一種關(guān)節(jié)角速度變極限的冗余度機(jī)械臂運(yùn)動(dòng)規(guī)劃方法。
背景技術(shù)：
：冗余度機(jī)械臂是一種自由度大于任務(wù)空間所需最少自由度的末端能動(dòng)機(jī)械裝置，其運(yùn)動(dòng)任務(wù)包括焊接、油漆、組裝、挖掘和繪圖等，廣泛應(yīng)用于裝備制造、產(chǎn)品加工、機(jī)器作業(yè)等國民經(jīng)濟(jì)生產(chǎn)活動(dòng)中。冗余度機(jī)械臂的逆運(yùn)動(dòng)學(xué)問題是指已知機(jī)械臂末端位姿，確定機(jī)械臂的關(guān)節(jié)角問題。傳統(tǒng)的冗余度解析方法以及工業(yè)機(jī)械臂控制方法主要是基于偽逆的方法即，把問題的解轉(zhuǎn)化成求一個(gè)最小范數(shù)解加上一個(gè)同類解。次目標(biāo)可以被指定到同類解上，去控制機(jī)械臂的自運(yùn)動(dòng)以躲避障礙物、關(guān)節(jié)極限、奇異點(diǎn)和優(yōu)化其它目標(biāo)函數(shù)。其缺點(diǎn)是在處理不等式約束上有困難，計(jì)算量大，實(shí)時(shí)性差，而且它會(huì)遇到奇異情況而生成不可行解，在實(shí)際機(jī)械臂的應(yīng)用中受到很大的制約。
發(fā)明內(nèi)容本發(fā)明的目的在于克服現(xiàn)有技術(shù)的不足，提供一種計(jì)算量小、實(shí)時(shí)性強(qiáng)且能適應(yīng)關(guān)節(jié)角速度極限變化的冗余度機(jī)械臂運(yùn)動(dòng)規(guī)劃方法。為了實(shí)現(xiàn)上述發(fā)明目的，采用的技術(shù)方案如下一種冗余度機(jī)械臂運(yùn)動(dòng)規(guī)劃方法，包括如下步驟1)通過上位機(jī)采用二次型優(yōu)化在速度層上對(duì)機(jī)械臂的逆運(yùn)動(dòng)學(xué)解析，設(shè)計(jì)的最小性能指標(biāo)可為速度范數(shù)、重復(fù)運(yùn)動(dòng)或動(dòng)能，受約束于速度雅可比等式、不等式和關(guān)節(jié)角速度極限，該角速度極限是隨關(guān)節(jié)角度變化的；2)將步驟1)的二次型優(yōu)化轉(zhuǎn)化為二次規(guī)劃問題；3)將步驟2)的二次規(guī)劃問題用線性變分不等式原對(duì)偶神經(jīng)網(wǎng)絡(luò)求解器或數(shù)值方法求解；4)將步驟3)的求解結(jié)果傳遞給下位機(jī)控制器驅(qū)動(dòng)機(jī)械臂運(yùn)動(dòng)。本發(fā)明基于線性變分不等式的原對(duì)偶神經(jīng)網(wǎng)絡(luò)具有全局指數(shù)收斂性，且沒有涉及到矩陣求逆等復(fù)雜運(yùn)算，大大地提高了計(jì)算效率，同時(shí)實(shí)時(shí)性強(qiáng)且能適應(yīng)關(guān)節(jié)角速度極限變化。圖1為本發(fā)明的流程圖；圖2為實(shí)現(xiàn)本發(fā)明的機(jī)械臂結(jié)構(gòu)主視圖；圖3為實(shí)現(xiàn)本發(fā)明的機(jī)械臂結(jié)構(gòu)俯視圖；圖4為機(jī)械臂局部示意圖。具體實(shí)施例方式下面結(jié)合附圖對(duì)本發(fā)明做進(jìn)一步的說明。圖1所示的冗余度機(jī)械臂運(yùn)動(dòng)規(guī)劃方法主要由目標(biāo)問題1、二次規(guī)劃問題2、基于線性變分不等式原對(duì)偶神經(jīng)網(wǎng)絡(luò)求解器或二次規(guī)劃數(shù)值算法3、下位機(jī)控制器4和機(jī)械臂5組成。先將逆運(yùn)動(dòng)學(xué)求解在速度層上設(shè)計(jì)為最小化辦久甸，且受約束于力=纟，▲。，θ彡0+，々_(外<々<爐(的；欲優(yōu)化的性能指標(biāo)興0，句可以是最小速度范數(shù)函數(shù)φ{(diào)θ)=\\θ\γ2Ι2=θτθΙ、重復(fù)運(yùn)動(dòng)指標(biāo)樹句=…++或最小動(dòng)能函數(shù)樹句將上述的各種冗余度解析方案轉(zhuǎn)化為通用的二次型優(yōu)化標(biāo)準(zhǔn)形式2，再使用基于線性變分不等式的原對(duì)偶神經(jīng)網(wǎng)絡(luò)或二次規(guī)劃數(shù)值方法3求解，并將求解結(jié)果傳遞給下位機(jī)控制器4驅(qū)動(dòng)機(jī)械臂5運(yùn)動(dòng)。圖2和圖3所示的機(jī)械臂由機(jī)械臂連桿1、推桿2、關(guān)節(jié)3、關(guān)節(jié)與推桿施力點(diǎn)連結(jié)部4和基座5組成。其中推桿2的存在使其不同于傳統(tǒng)無推桿串聯(lián)機(jī)械臂的角速度恒定極限方式，而是一種變極限機(jī)械臂。在實(shí)時(shí)計(jì)算機(jī)械臂的逆解時(shí)，這個(gè)角速度極限是角度的函數(shù)。因此，通過改變二次規(guī)劃的約束條件t<θ<θ+%θ'{θ)<θ<θ+{θ)，從而實(shí)現(xiàn)變極限的控制。圖4所示機(jī)械臂局部示意圖，在通常的設(shè)計(jì)中，認(rèn)為機(jī)械的動(dòng)力產(chǎn)生于內(nèi)部電機(jī)力矩，即假設(shè)動(dòng)力不是來自于推桿，或者認(rèn)為4非常小，幾乎可以忽略，這樣1和2就會(huì)重合在一起。本發(fā)明涉及的機(jī)械臂設(shè)計(jì)中，推桿是存在的，即4不可能忽略。這樣，由于推桿的存在使得原來的角速度極限在每時(shí)每刻都會(huì)變化，是角度的函數(shù)。假設(shè)1的長度為a，4的長度為b，2的長度為C。具體推導(dǎo)過程如下C2=a2+b2-2a*b*cos(^+ff);(1)將上述公式對(duì)時(shí)間t求導(dǎo)，得2c*—=+(2)dt2其中，c=C(1+v*At*Δc，C(1為初始的c邊長，ν為步進(jìn)電機(jī)的轉(zhuǎn)速，Δc為電機(jī)轉(zhuǎn)一圈對(duì)應(yīng)的電推桿伸長量。進(jìn)一步可得2c*(v*Ac)=2a*b*+θ)*θ=2a*b*cos0*0；(3)艮f]c*Ac*v=o*6*cos6>*々；(4)所以▲Ac*v*.a2+b2-2a*b*cos(—+0)θ==_ι_2__，(5)α*/*cosθα*厶*cosθ因此可得關(guān)節(jié)速度的變極限為+()--^-:=θ~(θ)<θa*b*cosθ<formula>formulaseeoriginaldocumentpage6</formula>其中，V+和ν—分別為相關(guān)關(guān)節(jié)步進(jìn)電機(jī)的轉(zhuǎn)速正極限和負(fù)極限。通過考慮角速度的極限為隨角度變化的函數(shù)，在設(shè)計(jì)控制方法時(shí)，修改相應(yīng)的約束條件，組成帶有變極限參數(shù)的角速度極限條件，從而實(shí)現(xiàn)對(duì)變極限問題的解決。基于前面的分析，機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解在速度層上可設(shè)計(jì)為minφ{(diào)θ,θ)(7)<formula>formulaseeoriginaldocumentpage6</formula>其中，辦化句代表欲優(yōu)化的性能指標(biāo)；等式約束=表述機(jī)械臂末端運(yùn)動(dòng)軌跡；不等式約束可以用于環(huán)境障礙物的躲避或其它性能約束；θ(θ+、θ~{θ)<θ<爐(的分別是關(guān)節(jié)角度極限、關(guān)節(jié)角速度極限。欲優(yōu)化的性能指標(biāo)興句可以設(shè)計(jì)為各種冗余度解析方案的優(yōu)化判據(jù)。其可以是最小速度范數(shù)函數(shù)，即列句=丨I湘/2=爐辦2;也可是重復(fù)運(yùn)動(dòng)指標(biāo)，即辦久々)=(々+z)r0+z)/2，其中ζ=λ(Θ-Θ(0)),λ>0是用來控制關(guān)節(jié)位移幅值的正設(shè)計(jì)參數(shù)；還可以是最小動(dòng)能函數(shù)列句等。如圖1所示的步驟1，將上述問題轉(zhuǎn)化為一個(gè)標(biāo)準(zhǔn)的二次規(guī)劃問題去求解才能應(yīng)用到機(jī)械臂的控制上去。該二次規(guī)劃問題可寫為如下通用形式<table>tableseeoriginaldocumentpage6</column></row><table>其中，決策變量χ可以被定義為^，W，q,J，d，A,b，x_，X+為已知的相對(duì)應(yīng)的系數(shù)矩陣和向量，比如，在最小速度范數(shù)方案中，W為單位矩陣，q=0，J為雅可比矩陣，J=—，而A，b可以是障礙物躲避參數(shù)或者由優(yōu)化指標(biāo)轉(zhuǎn)化得到的不等式約束，χ—，x+由公式(10)、(11)通過變換獲得。下面說明關(guān)節(jié)物理極限的處理和變換過程，即如何由公式(10)、(11)轉(zhuǎn)換得到公式(15)。在速度層上解析的時(shí)候，需要將(10)式轉(zhuǎn)換為速度層(χ=上的表達(dá)形式μ{θ~-θ)<θ<μφ+-θ)f(16)其中系數(shù)μ>0是用來調(diào)節(jié)關(guān)節(jié)角速度的可行域，系數(shù)μ的選取應(yīng)該使(16)式轉(zhuǎn)換后的可行域比原來的關(guān)節(jié)角速度可行域在一般情況下略大。由此，雙端約束公式(10)和(11)可以合并為一個(gè)統(tǒng)一的雙端約束Χ-彡X彡X+，其中X—和X+的第i個(gè)元素分別定義如下x「=ηχ{θ-(θ),μ(θ'-θ,)}，(I7)x；=χη\η{θ:{θ),μ(θ；-θ,)}。Q8)本發(fā)明用雙端不等式來表述關(guān)節(jié)物理極限的躲避，以上相關(guān)參數(shù)μ的選擇可以基于理論分析或基于經(jīng)驗(yàn)。得到上述的二次規(guī)劃問題(12)_(15)式后，本發(fā)明的求解方法是采用基于線性變分不等式的原對(duì)偶神經(jīng)網(wǎng)絡(luò)或二次規(guī)劃數(shù)值算法來實(shí)時(shí)求解此二次規(guī)劃問題。以下就是基于線性變分不等式的原對(duì)偶神經(jīng)網(wǎng)絡(luò)來求解帶約束的二次規(guī)劃問題(12)-(15)的神經(jīng)網(wǎng)絡(luò)求解器的構(gòu)造過程。首先，將二次規(guī)劃問題(12)_(15)式轉(zhuǎn)化為一個(gè)線性變分不等式，即求一個(gè)原對(duì)偶變量/eQ={y\y-<y<y+}c廣⑴⑷，使得eΩ，<formula>formulaseeoriginaldocumentpage7</formula>(19)其中，原對(duì)偶變量y及其上下限定義如下<formula>formulaseeoriginaldocumentpage7</formula>對(duì)偶變量u和ν分別與等式約束(13)和不等式約束(14)相對(duì)應(yīng)；lv:=[1，...，1]T是元素都為1的相應(yīng)維數(shù)向量；^7>>0是足夠大的常數(shù)，用于數(shù)值上替代無窮大+⑴，而擴(kuò)展矩陣Μ、P分別定義如下<formula>formulaseeoriginaldocumentpage7</formula>由此可總結(jié)歸納為至少存在一個(gè)最優(yōu)解χ*時(shí)，二次規(guī)劃問題(12)-(15)可轉(zhuǎn)化為線性變分不等式問題(19)。其次，線性變分不等式問題(19)又等價(jià)于線性投影方程，即ΡΩ(γ-(Μγ+ρ))ι=0，其中Ρω(·)為空間(b)到集合Ω的分段線性投影算子，ΡΩ(y)的第i個(gè)計(jì)算單元定義為<formula>formulaseeoriginaldocumentpage7</formula>接著，用下面的動(dòng)力學(xué)系統(tǒng)(作為基于線性變分不等式的原對(duì)偶神經(jīng)網(wǎng)絡(luò)的動(dòng)力學(xué)描述形式，如圖1的步驟3)來求解上述線性變分不等式問題及二次規(guī)劃問題<formula>formulaseeoriginaldocumentpage7</formula>(20)其中，設(shè)計(jì)參數(shù)Y>0用來調(diào)節(jié)網(wǎng)絡(luò)的收斂性，Y越大該網(wǎng)絡(luò)收斂得越快。此外，當(dāng)(12)-(15)至少存在一個(gè)最優(yōu)解χ*時(shí)，從任何初始狀態(tài)出發(fā)，線性變分不等式原對(duì)偶神經(jīng)網(wǎng)絡(luò)(20)的狀態(tài)向量y(t)都將收斂至某平衡點(diǎn)/，其前dim(x)個(gè)元素組成了二次規(guī)劃問題(12)_(15)的最優(yōu)解χ*。如果存在一個(gè)常數(shù)P>0，使得γ-ΡΩ(γ-(Μγ+ρ))II2彡P(guān)Iy-y*I12成立，則神經(jīng)網(wǎng)絡(luò)(20)全局指數(shù)收斂于平衡點(diǎn)/和問題最優(yōu)解其收斂率正比于YP)。將計(jì)算得到的角速度再傳送給下位機(jī)控制器從而控制機(jī)械臂的運(yùn)動(dòng)，實(shí)現(xiàn)本發(fā)明的方法。權(quán)利要求一種冗余度機(jī)械臂運(yùn)動(dòng)規(guī)劃方法，其特征在于包括如下步驟1)通過上位機(jī)采用二次型優(yōu)化在速度層上對(duì)機(jī)械臂的逆運(yùn)動(dòng)學(xué)解析，設(shè)計(jì)的最小性能指標(biāo)可為速度范數(shù)、重復(fù)運(yùn)動(dòng)或動(dòng)能，受約束于速度雅可比等式、不等式和關(guān)節(jié)角速度極限，該角速度極限是隨關(guān)節(jié)角度變化的；2)將步驟1)的二次型優(yōu)化轉(zhuǎn)化為二次規(guī)劃問題；3)將步驟2)的二次規(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ī)械臂運(yùn)動(dòng)規(guī)劃方法，其特征在于所述步驟1)的二次型優(yōu)化的冗余度解析方案將機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解在速度層上設(shè)計(jì)為最小化<formula>formulaseeoriginaldocumentpage2</formula>其中興代句代表欲優(yōu)化的性能指標(biāo)，等式約束=f表述機(jī)械臂末端運(yùn)動(dòng)軌跡，不等式約束<6表示用于環(huán)境障礙物的躲避性能約束，<formula>formulaseeoriginaldocumentpage2</formula>(的分別表示關(guān)節(jié)角度極限、關(guān)節(jié)角速度極限；所述欲優(yōu)化的性能指標(biāo)興仏句為各種冗余度解析方案的優(yōu)化判據(jù)，在速度層上興^甸采用最小速度范數(shù)函數(shù)，即樹句=丨丨郵/2=少辦2，或重復(fù)運(yùn)動(dòng)指標(biāo)，即樹句=(知沙妨+ζ)/2，其中<formula>formulaseeoriginaldocumentpage2</formula>是用來控制關(guān)節(jié)位移幅值的正設(shè)計(jì)參數(shù)，或最小動(dòng)能函數(shù)餌句=θτΗθ2，其中H為機(jī)械臂慣性矩陣。3.根據(jù)權(quán)利要求2所述的冗余度機(jī)械臂運(yùn)動(dòng)規(guī)劃方法，其特征在于所述步驟2)的二次規(guī)劃問題可以轉(zhuǎn)化為一個(gè)線性變分不等式，即求一個(gè)原對(duì)偶變量量y及其上下限定義如下<formula>formulaseeoriginaldocumentpage2</formula>對(duì)偶變量u和ν分別與等式約束(7)和不等式約束⑶相對(duì)應(yīng)；lv:=[1，...，1是元素都為1的相應(yīng)維數(shù)向量皿》0是足夠大的常數(shù)，用于數(shù)值上替代無窮大+⑴，而擴(kuò)展矩陣Μ、ρ分別定義如下<formula>formulaseeoriginaldocumentpage2</formula>4.根據(jù)權(quán)利要求3所述的線性變分不等式，其特征在于等價(jià)于線性投影方程ΡΩ(y-(My+p))-y=0，其中ΡΩ(·)為空間爐―到集合Ω的分段線性投影算子，ΡΩ(γ)的第i個(gè)計(jì)算單元定義為<formula>formulaseeoriginaldocumentpage2</formula>接著，用動(dòng)力學(xué)系統(tǒng)少=++解上述線性變分不等式問題及二次規(guī)劃問題，其中，設(shè)計(jì)參數(shù)Y>O用來調(diào)節(jié)網(wǎng)絡(luò)的收斂性，Y越大該網(wǎng)絡(luò)收斂得越快.全文摘要本發(fā)明提供了一種冗余度機(jī)械臂運(yùn)動(dòng)規(guī)劃方法，包括如下步驟1)通過上位機(jī)采用二次型優(yōu)化在速度層上對(duì)機(jī)械臂的逆運(yùn)動(dòng)學(xué)解析，設(shè)計(jì)的最小性能指標(biāo)可為速度范數(shù)、重復(fù)運(yùn)動(dòng)或動(dòng)能，受約束于速度雅可比等式、不等式和關(guān)節(jié)角速度極限，該角速度極限是隨關(guān)節(jié)角度變化的；2)將步驟1)的二次型優(yōu)化轉(zhuǎn)化為二次規(guī)劃問題；3)將步驟2)的二次規(guī)劃問題用線性變分不等式原對(duì)偶神經(jīng)網(wǎng)絡(luò)求解器或數(shù)值方法求解；4)將步驟3)的求解結(jié)果傳遞給下位機(jī)控制器驅(qū)動(dòng)機(jī)械臂運(yùn)動(dòng)。本發(fā)明基于線性變分不等式的原對(duì)偶神經(jīng)網(wǎng)絡(luò)具有全局指數(shù)收斂性，且沒有涉及到矩陣求逆等復(fù)雜運(yùn)算，大大地提高了計(jì)算效率，同時(shí)實(shí)時(shí)性強(qiáng)且能適應(yīng)關(guān)節(jié)角速度極限變化。文檔編號(hào)B25J9/08GK101804627SQ20101014451公開日2010年8月18日申請日期2010年4月2日優(yōu)先權(quán)日2010年4月2日發(fā)明者張智軍,張雨濃,李克訥申請人:中山大學(xué)