本發明涉及冗余度機械臂的規劃及控制領域，具體涉及一種具有抗噪特性的冗余度機械臂重復運動規劃方法。
背景技術：
冗余度機械臂是一種末端能動的機械裝置，其擁有的自由度要多于執行末端任務所需的最少自由度；已廣泛應用于裝備制造和產品加工等國民經濟生產活動中。冗余度機械臂的規劃和控制中容易存在的一個問題是關節角偏差問題，即，當機械臂末端的運動軌跡是閉合的，在完成給定的規劃任務后，機械臂各關節角變量在運動空間中的軌跡不是閉合的。為了解決關節角偏差問題(或稱，非重復運動問題)，目前已有多種有效的重復運動規劃方案被提出。然而，這些方案都是在不考慮噪聲的情況下進行研究的；一旦存在噪聲，那么它們將失效，從而無法使機械臂能夠正常/成功地完成所給定的規劃任務。
技術實現要素：
本發明的目的在于克服現有方法的不足，提供一種具有抗噪特性的冗余度機械臂重復運動規劃方法。
為了實現上述發明目的，采用的技術方案如下。
一種具有抗噪特性的冗余度機械臂重復運動規劃方法，包括以下步驟：
根據機械臂的雅可比矩陣等式，通過引入位置誤差反饋，設計具有抗噪特性的新型雅可比矩陣等式；
結合所需要優化的最小化性能指標，建立具有抗噪特性的重復運動規劃方案，所述的規劃方案受約束于新型雅可比矩陣等式、關節角度極限和關節速度極限；
將具有抗噪特性的重復運動規劃方案轉化為一個統一的二次型優化問題，并通過數值算法求解器對該優化問題進行求解；
下位機控制器根據二次型優化問題的求解結果，驅動機械臂使其完成給定的規劃任務。
具有抗噪特性的新型雅可比矩陣等式設計為：
其中，設計參數α>0，β>0，且兩者滿足α2>β；θ表示機械臂關節角度，表示機械臂關節速度，J(θ)表示機械臂的雅可比矩陣；e(t)表示機械臂的位置誤差且
e(t)＝f(θ)-rd，t表示時間，f表示一個非線性映射函數，rd表示機械臂末端期望的運動軌跡；表示rd的時間導數，τ表示積分變量。
具有抗噪特性的重復運動規劃方案設計為：
最小化
受約束于θ-≤θ≤θ+，
其中，設計參數λ>0，θ(0)表示機械臂關節的初始狀態，上標T表示矩陣或向量的轉置；等式約束對應于機械臂在速度層的運動軌跡；θ±和分別表示關節角度極限和關節速度極限。
將具有抗噪特性的重復運動規劃方案轉化為一個統一的二次型優化問題，其性能指標為xTx/2+pTx，約束條件為Ax＝b，x-≤x≤x+，其中，p＝λ(θ-θ(0))，A＝J(θ)，x±表示x的上下限。
通過數值算法求解器對二次型優化問題進行求解，具體為：將所述二次型優化問題進一步變換為分段線性投影方程，從而構造相應的數值算法求解器進行求解。
本發明與先有方法相比，具有以下優點：
本發明能有效克服現有方法的不足，提供了一種能使得冗余度機械臂在具有噪聲的情況下仍可完成給定任務的重復運動規劃方法。
附圖說明
圖1為本發明的流程圖。
具體實施方式
下面結合附圖對本發明做進一步的說明。
圖1所示的一種具有抗噪特性的冗余度機械臂重復運動規劃方法主要由設計具有抗噪特性的新型雅克比矩陣等式1、建立重復運動規劃方案2、轉為二次型優化問題3、數值算法求解器4、下位機控制器5、冗余度機械臂6這六個部分組成。
首先根據機械臂的雅可比矩陣等式，通過引入位置誤差反饋，設計具有抗噪特性的新型雅可比矩陣等式；然后結合所需要優化的最小化性能指標，建立相應的重復運動規劃方案，并將其轉化為一個統一的二次型優化問題，從而構造相應的數值算法求解器來求解該優化問題；最后將求解結果用于驅動機械臂的各個關節以使機械臂完成給定的規劃任務。
根據機械臂的雅可比矩陣等式，通過引入位置誤差反饋，具有抗噪特性的新型雅可比矩陣等式可設計為：
其中設計參數α>0，β>0，且兩者滿足α2>β；θ表示機械臂關節角度，表示機械臂關節速度，J(θ)表示機械臂的雅可比矩陣；e(t)表示機械臂的位置誤差且e(t)＝f(θ)-rd，t表示時間，f表示一個非線性映射函數，rd表示機械臂末端期望的運動軌跡；表示rd的時間導數，τ表示積分變量。
基于上述等式(1)，結合所需要優化的性能指標，便可建立如下的重復運動規劃方案：
最小化
約束條件：
θ-≤θ≤θ+， (4)
其中，設計參數λ>0，θ(0)表示機械臂關節的初始狀態，上標T表示矩陣或向量的轉置；等式約束對應于機械臂在速度層的運動軌跡；θ±和分別表示關節角度極限和關節速度極限。
對于上述帶物理約束的重復運動規劃方案(2)-(5)，其可轉化為如下的二次型優化問題：
最小化xTx/2+pTx， (6)
約束條件：Ax＝b， (7)
x-≤x≤x+， (8)
其中，p＝λ(θ-θ(0))，A＝J(θ)，x±表示x的上下限。
并且，上述的二次型優化問題(6)-(8)等價于如下的分段線性投影方程：
PΩ(y-(My+q))-y＝0， (9)
其中，PΩ(·)表示分段線性投影算子。分段線性投影方程(9)中的原對偶決策變向量y，增廣系數矩陣M和向量q分別定義如下：
其中，I表示單位矩陣，對偶決策變量u對應于等式約束(7)。對于上述的分段線性投影方程(9)和二次型優化問題(6)-(8)，可采用如下的數值算法來進行求解：
e(yk)＝yk-PΩ(yk-(Myk+q))，
yk+1＝yk-ρ(yk)φ(yk)，
φ(yk)＝(MT+I)e(yk)，
其中，||·||2表示向量的二范數，迭代次數k＝0,1,2,…。給定一個初始值y0，通過該算法的不斷迭代，便可得到分段線性投影方程(9)的解，從而得到二次型優化問題(6)-(8)的最優解，也即前文所述的具有抗噪特性的重復運動規劃方案(2)-(5)的最優解。
通過數值算法求解器得到該二次型優化問題的最優解之后，再將求解結果傳遞給下位機控制器驅動機械臂的運動，從而使得機械臂完成所給定的規劃任務。
以上所述僅為本發明的較佳實施例，并不用以限制本發明，凡在本發明的精神和原則之內，所作的任何修改、等同替換、改進等，均應包含在本發明的保護范圍之內。