專利名稱：一種冗余度機械臂運動規劃的初始化方法
技術領域：
本發明屬于一種冗余度機械臂運動規劃的初始化方法。
背景技術：
冗余度機械臂是一種自由度大于任務空間所需最少自由度的末端能動機械裝置， 其運動任務包括焊接、油漆、組裝、挖掘和繪圖等，廣泛應用于裝備制造、產品加工、機器作 業等國民經濟生產活動中。冗余度機械臂在空間不同位置執行任務時，當結束一個任務后， 往往需要從當前位置移動到下一個任務的執行位置。一般而言，冗余度機械臂從當前位置 移動到下一個任務的執行位置，都是通過慢慢移動機械臂并不斷調整來實現。然而，這種做 法是相當繁瑣且存在著不足的每次改變任務操作位置時都有可能需要多次度量各個關節 角以使機械臂準確到達任務執行的位置；并且任務執行位置測量(或調整)的不準確也會 影響到機械臂正常完成任務。
發明內容
本發明的目的在于克服一般技術的不足，提供一種計算量小、操作方便、工作量 少、作業規范的冗余度機械臂運動規劃初始化方法。為了實現上述發明目的，采用的技術方案如下一種冗余度機械臂運動規劃的初始化方法，包括如下步驟1)啟動冗余度機械臂控制器，將冗余度機械臂調節到極限位置；2)將步驟1)的極限位置作為起始值，并設定機械臂任務執行位置作為終止值，從 而把機械臂的控制轉化為二次型規劃問題；3)將步驟2)的二次型規劃問題利用二次型規劃求解器進行求解；4)將步驟3)的求解結果用于驅動各關節電機使機械臂由極限位置轉動到任務執 行位置。上述技術方案中，所述步驟1)將冗余度機械臂調節到極限位置，具體操作是依次 驅動各個關節的電機使其最終達到各自的極限位置。所述步驟2)把機械臂的控制轉化為二次型規劃問題，其性能指標為 々^f々/2 + zM，約束條件為Θ-彡θ ( θ+，欠幺汐幺一，其中…為機械臂關節角速度，W 為合適維數的單位陣，ζ = λ ( θ - θ f)表示關節位移幅值偏差，λ > 0是用來控制關節位 移幅值的設計參數，θ ,為任務執行初始位置，θ ±、^±分別表示機械臂的關節角度極限、關 節角速度極限。所述步驟3)利用二次型規劃求解器來求解二次型規劃問題，具體是將該二次型 規劃問題變換為分段線性投影方程，從而構造二次型規劃求解器進行求解。所述二次型規劃求解器采用基于線性變分不等式的原對偶神經網絡求解器或二 次規劃數值方法。所述步驟4)將二次型規劃求解器的結果轉化為電機驅動所需要的脈沖頻率，從而驅動各關節電機使機械臂由極限位置轉動到任務執行位置。本發明能有效克服現有技術的不足，提供一種計算量小、操作方便、工作量少、作 業規范的冗余度機械臂運動規劃初始化方法。
圖1為本發明的流程圖；圖2為實現本發明的機械臂關節極限狀態俯視圖；圖3為實現本發明的機械臂關節極限狀態主視圖；圖4為實現本發明的機械臂任務初始關節位置俯視圖；圖5為實現本發明的機械臂任務初始關節位置主視具體實施例方式
下面結合附圖對本發明做進一步的說明。圖1所示的一種冗余度機械臂運動規劃的初始化方法主要由機械臂調節到極限 位置1、轉為二次型規劃問題2、二次規劃型求解器3、驅動機械臂轉到任務執行位置4組成。圖2、圖3、圖4和圖5所示的冗余度機械臂為一個平面六連桿冗余度機械臂，主要 由關節5、關節6、關節7、關節8、關節9和關節10組成。首先啟動冗余度機械臂控制器，將冗余度機械臂調節到極限位置；再將該極限位 置作為初始化方法的起始值，并設定機械臂任務執行位置作為初始化方法的終止值，從而 把機械臂的控制問題轉化為相應的二次型規劃問題；然后構造相應的二次型規劃求解器將 該問題求解；最后把求解結果用于驅動機械臂的各關節電機使其由極限位置轉動到任務執 行位置。基于最小化機械臂關節當前位置與任務執行位置之間位移量的思想，機械臂運動 規劃的初始化方法可設計為最小化&爐々/2 + /汐(1)約束條件θ _彡θ彡θ +，(2)0- 10 么 θ+。(3)其中，W為合適維數的單位陣，ζ = λ ( θ - θ f) e Rn, λ > 0是用來控制關節位移 幅值的設計參數，θ 任務執行初始位置，θ i、爐分別表示關節角度極限、關節角速度極 限。考慮到上述優化問題是在速度層上求解，因此，將機械臂的關節角度約束(2)和關節角 速度約束(3)合并，便可得到基于角速度力的雙端不等式約束ξ~ <θ<ξ+,其中，ξ_和ξ+中的第i個元素可分別定義為《,-=Π^χ{^,// -代)}，
6+=min0+,//(^+-《)丨。常數y是用來調節由關節角速度決定的可行域。用χ表示機 械臂的關節角速度力，帶物理約束的機械臂運動規劃初始化方法(1)_(3)便可描述為如下 的二次型規劃方案最小化xTWx/2+zTx，(4)約束條件ξ _彡χ彡ξ +，(5)
其又等價于如下的分段線性投影方程P Ω (χ- (ffx+z)) -χ = 0。(6)其中，集合0 = {太|廣<太<€+}〔滅"，投影算子？￡2(·)是從鏟到Ω上的，它的 每一個元素函數可以定義如下
《,-, if Xi < ξ~-x,, if ξ； < Χ, <ξ；, = 1,2,.. °
ifX對于上述分段線性投影方程(6)和二次規劃問題(4)_ (5)，可采用如下算法求解。首先，定義計算誤差e(x) = χ-ΡΩ (x-(Wx+z))。(7)顯然，當誤差為零時，對應的χ值便為二次規劃問題(4)_(5)的解。然后，給定初 始值x° e Rn，通過如下的迭代方法來得到χ更新值使得誤差(7)達到預設的精度xk+1 = xk-p (xk) d (Xk)，(8)其中，k = 0，1，2，…，d(xk)和P (xk)分別定義為d(xk) = (ffT+I) e (xk) ,(9)廠(？)。(10)
W) Il2、…通過算法不斷迭代，便可得到分段線性投影方程(6)的解，從而得到二次型優化 問題(4)-(5)的最優解。通過上位機的二次型規劃求解器得到該二次型規劃問題的解后，再將結果轉化為 電機驅動所需要的脈沖頻率，并通過該冗余度機械臂的控制器來驅動機械臂各個關節電機 使其由極限位置轉動到任務執行位置，從而完成冗余度機械臂運動規劃的初始化工作。
權利要求
一種冗余度機械臂運動規劃的初始化方法，其特征在于包括如下步驟1)啟動冗余度機械臂控制器，將冗余度機械臂調節到極限位置；2)將步驟1)的極限位置作為起始值，并設定機械臂任務執行位置作為終止值，從而把機械臂的控制轉化為二次型規劃問題；3)將步驟2)的二次型規劃問題利用二次型規劃求解器進行求解；4)將步驟3)的求解結果用于驅動各關節電機使機械臂由極限位置轉動到任務執行位置。
2.根據權利要求1所述的冗余度機械臂運動規劃方法，其特征在于所述步驟1)將冗余 度機械臂調節到極限位置，具體操作是依次驅動各個關節的電機使其最終達到各自的極限 位置。
3.根據權利要求1或2所述的冗余度機械臂運動規劃方法，其特征在于所述步驟 2)把機械臂的控制轉化為二次型規劃問題，其性能指標為儼妒6/2+ 76，約束條件為 Θ —≤θ≤θ+，力-<々<爾，其中j為機械臂關節角速度，W為合適維數的單位陣，ζ = λ ( θ - θ f)表示關節位移幅值偏差，λ > 0是用來控制關節位移幅值的設計參數，θ f為任 務執行初始位置，θ +、力i分別表示機械臂的關節角度極限、關節角速度極限。
4.根據權利要求3所述的冗余度機械臂運動規劃方法，其特征在于所述步驟3)利用二 次型規劃求解器來求解二次型規劃問題，具體是將該二次型規劃問題變換為分段線性投影 方程，從而構造二次型規劃求解器進行求解。
5.根據權利要求4所述的冗余度機械臂運動規劃方法，其特征在于所述二次型規劃求 解器采用基于線性變分不等式的原對偶神經網絡求解器或二次規劃數值方法。
6.根據權利要求5所述的冗余度機械臂運動規劃方法，其特征在于所述步驟4)將二次 型規劃求解器的結果轉化為電機驅動所需要的脈沖頻率，從而驅動各關節電機使機械臂由 極限位置轉動到任務執行位置。
全文摘要
本發明提供了一種冗余度機械臂運動規劃的初始化方法，包括如下步驟1)啟動冗余度機械臂控制器，將冗余度機械臂調節到極限位置；2)將步驟1)的極限位置作為本初始化方法的起始值，并設定機械臂任務執行位置作為初始化方法的終止值，從而把機械臂的控制問題轉化為相應的二次型規劃問題；3)將步驟2)的二次型規劃問題利用二次型規劃求解器進行求解；4)將步驟3)的求解結果用于驅動各關節電機使機械臂由極限位置轉動到任務執行位置。本發明基于二次型規劃的初始化方法，避免了每次任務操作時都需要度量各個關節角度的繁瑣過程，同時根據任務要求規范了每次作業時的起始位置。
文檔編號B25J13/00GK101890718SQ201010193200
公開日2010年11月24日 申請日期2010年6月1日 優先權日2010年6月1日
發明者張智軍, 張雨濃, 郭東生 申請人:中山大學