本發(fā)明涉及冗余度機械臂的運動規(guī)劃領(lǐng)域，具體涉及一種具有速度優(yōu)化特性的冗余度機械臂突加度層運動規(guī)劃方法。

背景技術(shù)：

冗余度機械臂是一種自身自由度多于執(zhí)行末端任務所需自由度的機械裝置，其應用已擴展至諸多工業(yè)領(lǐng)域，如焊接組裝、宇宙探索和醫(yī)療手術(shù)等。作為加速度的時間導數(shù)，突加度對整個機械臂系統(tǒng)的穩(wěn)定性有非常大的影響，并且其與人體運動存在著直接的關(guān)系。因此，機械臂在突加度層上的研究在近年來受到了廣泛的關(guān)注，且許多運動規(guī)劃方案被相繼提出。然而，這些規(guī)劃方案往往沒有考慮機械臂的關(guān)節(jié)物理極限，且其解析得到的末態(tài)速度通常不為零，因而難以廣泛適用于實際工程當中。

技術(shù)實現(xiàn)要素：

本發(fā)明的目的在于克服現(xiàn)有方法的不足，提供一種具有速度優(yōu)化特性的冗余度機械臂突加度層運動規(guī)劃方法。

為了實現(xiàn)上述發(fā)明目的，采用的技術(shù)方案如下：

一種具有速度優(yōu)化特性的冗余度機械臂突加度層運動規(guī)劃方法，包括如下步驟：

根據(jù)機械臂突加度層性能指標，通過兩個設(shè)計參數(shù)引入速度和加速度，設(shè)計具有速度優(yōu)化特性的新型性能指標；

基于新型性能指標，建立相應的突加度層運動規(guī)劃方案，所述的規(guī)劃方案受約束于速度、加速度、突加度的雅克比矩陣等式、關(guān)節(jié)角度極限、關(guān)節(jié)速度極限、關(guān)節(jié)加速度極限和關(guān)節(jié)突加度極限；

將上述的規(guī)劃方案轉(zhuǎn)化為一個二次型優(yōu)化問題，并通過數(shù)值算法求解器對其進行求解；

下位機控制器根據(jù)二次型優(yōu)化問題的求解結(jié)果，驅(qū)動機械臂使其完成給定的末端任務。

上述技術(shù)方案中，所述具有速度優(yōu)化特性的新型性能指標設(shè)計為：

其中，設(shè)計參數(shù)α>0和β>0，上標^t表示矩陣或向量的轉(zhuǎn)置，表示機械臂關(guān)節(jié)速度，表示機械臂關(guān)節(jié)加速度，表示機械臂關(guān)節(jié)突加度。

基于該性能指標，所述突加度層運動規(guī)劃方案設(shè)計為：

最小化

受約束于θ^-≤θ≤θ⁺，

其中，等式約束對應于機械臂在速度層的末端運動軌跡，j表示機械臂的雅可比矩陣，表示機械臂末端期望運動軌跡rd的時間導數(shù)；等式約束對應于機械臂在加速度層的末端運動軌跡，表示j的時間導數(shù)，表示的時間導數(shù)；等式約束對應于機械臂在突加度層的末端運動軌跡，表示的時間導數(shù)，表示的時間導數(shù)；θ^±、和分別表示關(guān)節(jié)角度θ、關(guān)節(jié)速度關(guān)節(jié)加速度和關(guān)節(jié)突加度的極限。

所述將突加度層運動規(guī)劃方案轉(zhuǎn)化為一個二次型優(yōu)化問題，其性能指標為x^tx/2+p^tx，約束條件為ax＝b，x^-≤x≤x⁺，其中，p＝c，a＝j(luò)，x±表示x的上下限。

通過數(shù)值算法求解器對二次型優(yōu)化問題進行求解，具體為：將所述二次型優(yōu)化問題進一步變換為分段線性投影方程，從而構(gòu)造相應的數(shù)值算法求解器進行求解；

下位機控制器根據(jù)二次型優(yōu)化問題的求解結(jié)果，驅(qū)動機械臂使其完成給定的末端任務。

本發(fā)明與現(xiàn)有方法相比，具有以下優(yōu)點：

本發(fā)明能有效克服現(xiàn)有方法的不足，提供了一種能使得冗余度機械臂完成給定末端任務后各關(guān)節(jié)速度為零的突加度層運動規(guī)劃方法。

以下結(jié)合附圖及實施例對本發(fā)明作進一步詳細說明，但本發(fā)明的一種具有速度優(yōu)化特性的冗余度機械臂突加度層運動規(guī)劃方法不局限于實施例。

附圖說明

圖1為本發(fā)明的流程圖。

具體實施方式

下面結(jié)合附圖對本發(fā)明做進一步的說明。

參見圖1所示，一種具有速度優(yōu)化特性的冗余度機械臂突加度層運動規(guī)劃方法主要由：設(shè)計具有速度優(yōu)化特性的新型性能指標1、建立突加度層運動規(guī)劃方案2、轉(zhuǎn)為二次型優(yōu)化問題3、數(shù)值算法求解器4、下位機控制器5和冗余度機械臂6這六個部分組成。

首先根據(jù)機械臂突加度層性能指標，通過兩個設(shè)計參數(shù)引入速度和加速度，設(shè)計具有速度優(yōu)化特性的新型性能指標；然后結(jié)合所需要優(yōu)化的性能指標，建立相應的突加度層運動規(guī)劃方案，并將其轉(zhuǎn)化為一個二次型優(yōu)化問題，從而構(gòu)造相應的數(shù)值算法求解器來求解該優(yōu)化問題；最后將求解結(jié)果用于驅(qū)動機械臂的各個關(guān)節(jié)以使機械臂完成給定的末端任務。

根據(jù)機械臂突加度層性能指標，通過兩個設(shè)計參數(shù)引入速度和加速度，具有速度優(yōu)化特性的新型性能指標設(shè)計為：

其中，設(shè)計參數(shù)α>0和β>0，上標^t表示矩陣或向量的轉(zhuǎn)置，表示機械臂關(guān)節(jié)速度，表示機械臂關(guān)節(jié)加速度，表示機械臂關(guān)節(jié)突加度。

基于上述性能指標(1)，考慮機械臂的關(guān)節(jié)物理極限，便可建立如下的突加度層運動規(guī)劃方案：

最小化

約束條件：

θ^-≤θ≤θ⁺，(6)

其中，等式約束對應于機械臂在速度層的末端運動軌跡，j表示機械臂的雅可比矩陣，表示機械臂末端期望運動軌跡rd的時間導數(shù)；等式約束對應于機械臂在加速度層的末端運動軌跡，表示j的時間導數(shù)，表示的時間導數(shù)；等式約束對應于機械臂在突加度層的末端運動軌跡，表示的時間導數(shù)，表示的時間導數(shù)；θ^±、和分別表示關(guān)節(jié)角度θ、關(guān)節(jié)速度關(guān)節(jié)加速度和關(guān)節(jié)突加度的極限。

對于上述帶物理約束的突加度層運動規(guī)劃方案(2)-(9)，其可轉(zhuǎn)化為如下的二次型優(yōu)化問題：

最小化x^tx/2+p^tx，(10)

約束條件：ax＝b，(11)

x^-≤x≤x⁺，(12)

其中，p＝c，a＝j(luò)，x^±表示x的上下限。

上述二次型優(yōu)化問題(10)-(12)的求解可等價于求解如下的分段線性投影方程：

pω(y-(my+q))-y＝0，(13)

其中，pω(·)表示分段線性投影算子。分段線性投影方程(13)中的原對偶決策變向量y，系數(shù)矩陣m和系數(shù)向量q分別定義如下：

其中，i表示單位矩陣，對偶決策變量u對應于等式約束(11)。對于分段線性投影方程(13)和二次型優(yōu)化問題(10)-(12)，可采用如下的數(shù)值算法進行求解：

e(y^k)＝y(tǒng)^k-pω(y^k-(my^k+q))，

y^k+1＝pω(y^k-ρ(y^k)φ(y^k))，

φ(y^k)＝m^te(y^k)+my^k+q，

其中，||·||2表示向量的二范數(shù)，迭代次數(shù)k＝0,1,2,…。給定一個初始值y⁰，通過該數(shù)值算法的不斷迭代計算，便可得到分段線性投影方程(13)的理論解，從而得到二次型優(yōu)化問題(10)-(12)的最優(yōu)解，也即前文所述的突加度層運動規(guī)劃方案(2)-(9)的最優(yōu)解。

通過數(shù)值算法計算得到二次型優(yōu)化問題的最優(yōu)解之后，再將求解結(jié)果傳遞給下位機控制器驅(qū)動機械臂的運動，從而使機械臂完成給定的末端規(guī)劃任務。

以上所述僅為本發(fā)明的較佳實施例，并不用以限制本發(fā)明，凡在本發(fā)明的精神和原則之內(nèi)，所作的任何修改、等同替換、改進等，均應包含在本發(fā)明的保護范圍之內(nèi)。