本發(fā)明屬于空間在軌服務(wù)技術(shù)領(lǐng)域，具體涉及一種基于集合優(yōu)化的空間操作規(guī)劃方法。

背景技術(shù)：

基座處于漂浮狀態(tài)的空間機(jī)器人在執(zhí)行任務(wù)過程中可以節(jié)省大量燃料，引起國內(nèi)外學(xué)者的廣泛關(guān)注。此外，若機(jī)器人自由度冗余，則其具有高度靈敏性，且在執(zhí)行任務(wù)中可以最小化對漂浮基座的姿態(tài)擾動或成功避開障礙物等。現(xiàn)有研究主要是自由度冗余的漂浮空間機(jī)器人操作規(guī)劃問題以及自由度冗余空間機(jī)器人的避障路徑規(guī)劃問題，以實(shí)現(xiàn)對漂浮基座姿態(tài)擾動的最小化，但未對末端執(zhí)行器的運(yùn)動做出任何約束限制。

若機(jī)器人自由度冗余，則對于操作空間中的一組位姿向量，存在關(guān)節(jié)空間中的無限組關(guān)節(jié)角向量與之對應(yīng)。因此，機(jī)器人運(yùn)動過程中，在每個時刻均存在關(guān)節(jié)空間中的無限組關(guān)節(jié)角向量與此時的末端執(zhí)行器位姿向量對應(yīng)。如何選擇每個時刻的關(guān)節(jié)角向量，使末端執(zhí)行器在該時刻到達(dá)指定位姿，且運(yùn)動過程中實(shí)現(xiàn)對漂浮基座的姿態(tài)擾動最小，達(dá)到降低空間燃料的目的，是十分有必要的。

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

本發(fā)明所要解決的技術(shù)問題在于針對上述現(xiàn)有技術(shù)中的不足，提供一種基于集合優(yōu)化的空間操作規(guī)劃方法，解決基座處于漂浮狀態(tài)的自由度冗余空間機(jī)器人末端執(zhí)行器運(yùn)動時，如何選擇每個時刻的關(guān)節(jié)角向量，保證末端執(zhí)行器在該時刻到達(dá)指定位姿，且運(yùn)動過程中實(shí)現(xiàn)對漂浮基座的姿態(tài)擾動最小，達(dá)到降低空間燃料的目的。

本發(fā)明采用以下技術(shù)方案：

一種基于集合優(yōu)化的空間操作規(guī)劃方法，包括以下步驟：

s1、建立與操作空間中末端執(zhí)行器位姿向量對應(yīng)的關(guān)節(jié)角向量集合；

s2、建立關(guān)節(jié)角向量集合的偏序關(guān)系；

s3、利用粒子群優(yōu)化算法在無限個包含無限組關(guān)節(jié)角向量的集合中選擇與每個時刻對應(yīng)的最優(yōu)關(guān)節(jié)角向量。

優(yōu)選的，步驟s1中，所述關(guān)節(jié)角向量集合為：

f:xee→2^q

其中，xee表示操作空間中的一點(diǎn)，q表示與操作空間點(diǎn)對應(yīng)的關(guān)節(jié)角向量集合。

優(yōu)選的，末端執(zhí)行器在操作空間運(yùn)動時，對應(yīng)的關(guān)節(jié)運(yùn)動如下：

f(xee(t)):xee(t)→2^q(t)

其中，xee(t)為末端執(zhí)行器在t時刻的位姿向量。

優(yōu)選的，步驟s2中，所述關(guān)節(jié)角向量集合的偏序關(guān)系為：

其中，q^*(t)為t時刻與末端執(zhí)行器位姿對應(yīng)的最優(yōu)關(guān)節(jié)角向量，tf為終止時間。

優(yōu)選的，步驟s3中，根據(jù)所述偏序關(guān)系，利用粒子群優(yōu)化算法對基于集合優(yōu)化的操作規(guī)劃進(jìn)行尋優(yōu)求解，具體為：

其中，vi^m和分別表示第i個粒子在第m次迭代后的速度和位置，和分別表示第i個粒子在第m次迭代中的控制參數(shù)，pi^m表示第i個粒子在第m次迭代后的最優(yōu)位置，g^m表示全體在第m次迭代后的全局最優(yōu)位置。

優(yōu)選的，所述粒子群優(yōu)化算法的步驟如下：

s31、初始化參數(shù)值，設(shè)定群體維數(shù)為7，群體個數(shù)為50，群體最大迭代次數(shù)為200，速度上限為3，控制參數(shù)ω范圍為[0.4,0.94]，控制參數(shù)c1和c2的范圍為[0.75,2.5]；

s32、更新控制參數(shù)；

s33、根據(jù)所述偏序關(guān)系更新每個粒子的速度和位置；

s34、計算適應(yīng)度值；

s35、檢查適應(yīng)度值是否滿足終止條件，若滿足，則循環(huán)結(jié)束，否則轉(zhuǎn)s32。

優(yōu)選的，所述步驟s32中，根據(jù)如下方程式對三個控制參數(shù)進(jìn)行更新：

其中，mmax最大迭代次數(shù)，c1和c2表示學(xué)習(xí)因子，i表示第i個粒子。

優(yōu)選的，步驟s34中，計算末端執(zhí)行器實(shí)際路徑與期望路徑的位姿誤差及基座姿態(tài)模值，并與上一次迭代適應(yīng)度值進(jìn)行比較，選擇滿足所述偏序關(guān)系的關(guān)節(jié)運(yùn)動曲線。

優(yōu)選的，步驟s3中，保證末端執(zhí)行器按照指定路徑運(yùn)動，且運(yùn)動過程中對漂浮基座的姿態(tài)擾動最小。

與現(xiàn)有技術(shù)相比，本發(fā)明至少具有以下有益效果：

本發(fā)明將基于集合的偏序關(guān)系應(yīng)用于基座處于漂浮狀態(tài)的自由度冗余空間機(jī)器人末端執(zhí)行器位姿向量對應(yīng)的的關(guān)節(jié)角向量優(yōu)化中，形式簡單、直觀，并利用基于偏序關(guān)系和機(jī)器人正運(yùn)動學(xué)方程的自適應(yīng)粒子群優(yōu)化算法，對關(guān)節(jié)角向量進(jìn)行優(yōu)化，首先避免了機(jī)器人運(yùn)動過程中的奇異性，其次優(yōu)化過程中可實(shí)現(xiàn)對算法參數(shù)的實(shí)時調(diào)整，可避免算法在尋優(yōu)過程中陷入局部最優(yōu)。

綜上所述，本發(fā)明適用于末端執(zhí)行器在操作空間沿指定路徑運(yùn)動時，基于集合優(yōu)化得到的關(guān)節(jié)空間的最優(yōu)關(guān)節(jié)運(yùn)動。

下面通過附圖和實(shí)施例，對本發(fā)明的技術(shù)方案做進(jìn)一步的詳細(xì)描述。

附圖說明

圖1為本發(fā)明粒子群優(yōu)化算法的流程圖；

圖2為本發(fā)明末端執(zhí)行器位置誤差變化關(guān)節(jié)角變化；

圖3為本發(fā)明末端執(zhí)行器姿態(tài)誤差變化關(guān)節(jié)角變化；

圖4為本發(fā)明漂浮基座姿態(tài)變化關(guān)節(jié)角變化；

圖5為本發(fā)明關(guān)節(jié)角變化示意圖。

具體實(shí)施方式

本發(fā)明提供了一種基于集合優(yōu)化的空間操作規(guī)劃方法，針對操作空間中末端執(zhí)行器沿指定路徑運(yùn)動的任務(wù)情形，在關(guān)節(jié)空間中提出了一種基于集合優(yōu)化的操作規(guī)劃方法。首先，建立與操作空間中末端執(zhí)行器位姿向量對應(yīng)的關(guān)節(jié)角向量集合；其次，建立關(guān)節(jié)角向量集合的偏序關(guān)系；最后，利用改進(jìn)的粒子群優(yōu)化算法在無限個包含無限組關(guān)節(jié)角向量的集合中選擇與每個時刻對應(yīng)的最優(yōu)關(guān)節(jié)角向量，保證末端執(zhí)行器按照指定路徑運(yùn)動，且運(yùn)動過程中對漂浮基座的姿態(tài)擾動最小。

本發(fā)明一種基于集合優(yōu)化的空間操作規(guī)劃方法，包括以下步驟：

s1、建立與末端執(zhí)行器位姿向量對應(yīng)的關(guān)節(jié)角向量集合；

若空間機(jī)器人自由度冗余，則操作空間與關(guān)節(jié)空間的對應(yīng)關(guān)系為一集值映射：

f:xee→2^q(1)

其中，xee表示操作空間中的一點(diǎn)(即末端執(zhí)行器的位姿向量)，q表示與操作空間點(diǎn)對應(yīng)的關(guān)節(jié)空間中的點(diǎn)(即關(guān)節(jié)角向量)集合。

由于機(jī)器人自由度冗余，則對于操作空間中的一點(diǎn)，有無限個關(guān)節(jié)空間中的點(diǎn)與之對應(yīng)，即與之對應(yīng)的關(guān)節(jié)角向量集合包含無限個元素(其中每一個元素為一個向量)。

記末端執(zhí)行器在t時刻的位姿向量為xee(t)，則與之對應(yīng)的關(guān)節(jié)運(yùn)動描述如下：

f(xee(t)):xee(t)→2^q(t)(2)

s2、建立關(guān)節(jié)角集合的偏序關(guān)系；

記t0時刻的末端執(zhí)行器位姿向量為xee(t0)，與之對應(yīng)的關(guān)節(jié)角向量集合為：

q(t0)＝{q(t0):(q1(t0),...,qn(t0))}

其中，n表示機(jī)器人的關(guān)節(jié)數(shù)目，則有：

q(t0)＝{q(t0):(q1(t0),...,qn(t0))|f(xee(t0))＝q(t0)}(3)

設(shè)q^*(t0),q^**(t0)∈q(t0)，稱q^*(t0)≤^rsq^**(t0)，當(dāng)且僅當(dāng)：

f(q^*(t0))＜f(q^**(t0))(4)

其中，f(q(t0)):q(t0)→ω(t0)表示t0時刻關(guān)節(jié)角至漂浮基座姿態(tài)模值的映射，ω(t0)表示t0時刻的基座姿態(tài)模值。

機(jī)器人運(yùn)動過程中，稱q^*(t)≤^rsq^**(t)(t∈[t0,tf])，當(dāng)且僅當(dāng)：

s3、優(yōu)化關(guān)節(jié)運(yùn)動；

根據(jù)建立的偏序關(guān)系，利用自適應(yīng)粒子群優(yōu)化算法對基于集合優(yōu)化的操作規(guī)劃問題進(jìn)行尋優(yōu)求解，公式見式(6)。

其中，vi^m和分別表示第i個粒子在第m次迭代后的速度和位置，和分別表示第i個粒子在第m次迭代中的控制參數(shù)，pi^m表示第i個粒子在第m次迭代后的最優(yōu)位置，g^m表示全體在第m次迭代后的全局最優(yōu)位置。

請參閱圖1，自適應(yīng)粒子群優(yōu)化算法的具體步驟如下：

s31、初始化參數(shù)值。

其中，群體維數(shù)(所優(yōu)化參數(shù))為7，群體個數(shù)為50，群體最大迭代次數(shù)為200，速度上限為3，控制參數(shù)ω范圍為[0.4,0.94]，控制參數(shù)c1和c2的范圍為[0.75,2.5]。

s32、更新控制參數(shù)。

根據(jù)如下方程式對三個控制參數(shù)進(jìn)行更新：

其中，mmax表示最大迭代次數(shù)。

s33、根據(jù)公式(5)更新每個粒子的速度和位置；

s34、計算適應(yīng)度值；

包括末端執(zhí)行器實(shí)際路徑與期望路徑的位姿誤差及基座姿態(tài)模值，并與上一次迭代適應(yīng)度值進(jìn)行比較，選擇滿足公式(5)的關(guān)節(jié)運(yùn)動曲線。

s35、檢查適應(yīng)度值是否滿足終止條件，若滿足，則循環(huán)結(jié)束，否則轉(zhuǎn)s32。

空間機(jī)器人的初始關(guān)節(jié)角為[120°30°-45°60°120°-45°45°]，對應(yīng)的末端執(zhí)行器(ee)初始位置和姿態(tài)分別為[-4.772m-4.682m8.075m]和[11.87°-28.21°24.67°]，要求末端執(zhí)行器的終點(diǎn)位置和姿態(tài)分別為[-4.00m5.02m8.25m]和[15.30°-25.40°20.10°]，且要求末端執(zhí)行器初始點(diǎn)至終點(diǎn)的運(yùn)動路徑為直線。圖2至5描述了基于集合優(yōu)化的自適應(yīng)粒子群優(yōu)化算法的尋優(yōu)結(jié)果。

圖2至4為自適應(yīng)粒子群優(yōu)化算法運(yùn)行過程中適應(yīng)度值的變化，即末端執(zhí)行器的規(guī)劃路徑與期望路徑位姿誤差及漂浮基座的姿態(tài)的變化。圖2和3描述了末端執(zhí)行器實(shí)際位姿和期望位姿模值誤差變化，可知末端執(zhí)行器按照指定位姿路徑移動；圖4描述了自由度冗余空間機(jī)器人操作系統(tǒng)對漂浮基座姿態(tài)擾動的模值的變化，在約束范圍內(nèi)(<0.2°)；

圖5為利用基于集合偏序關(guān)系的自適應(yīng)粒子群算法優(yōu)化得到的各關(guān)節(jié)角變化曲線。由圖可知，各關(guān)節(jié)角度曲線光滑。

以上內(nèi)容僅為說明本發(fā)明的技術(shù)思想，不能以此限定本發(fā)明的保護(hù)范圍，凡是按照本發(fā)明提出的技術(shù)思想，在技術(shù)方案基礎(chǔ)上所做的任何改動，均落入本發(fā)明權(quán)利要求書的保護(hù)范圍之內(nèi)。