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