一種空間機(jī)器人最小基座擾動軌跡規(guī)劃方法
【專利摘要】本發(fā)明涉及一種空間機(jī)器人最小基座擾動軌跡規(guī)劃方法,具體講是涉及一種基于改進(jìn)混沌粒子群算法的空間機(jī)器人最小基座擾動軌跡規(guī)劃方法,其屬于航空航天技術(shù)領(lǐng)域;本發(fā)明包括的步驟為:(1)建立6自由度空間機(jī)器人的運(yùn)動學(xué)方程;(2)用7階正弦多項(xiàng)式函數(shù)對關(guān)節(jié)進(jìn)行參數(shù)化;(3)建立適應(yīng)度函數(shù)的方程,包括的約束條件有基座的位置擾動、姿態(tài)擾動、關(guān)節(jié)速度和角速度的約束限制;(3)為了不陷局部最優(yōu)和能快速找到最優(yōu)解,對混沌粒子群算法進(jìn)行了改進(jìn);(4)利用改進(jìn)混沌粒子群算法求解未知參數(shù),在基座位姿擾動最小的情況下得出空間機(jī)器人的最優(yōu)軌跡。本發(fā)明著重解決了針對6自由度自由漂浮空間機(jī)器人的軌跡規(guī)劃問題,在基座位姿擾動最小的情況利用改進(jìn)混沌粒子群算法找出空間機(jī)器人運(yùn)動的最優(yōu)軌跡,效果良好。
【專利說明】
-種空間機(jī)器人最小基座擾動軌跡規(guī)劃方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明設(shè)及一種基于改進(jìn)混濁粒子群算法的空間機(jī)器人最小基座擾動軌跡規(guī)劃 方法,屬于計(jì)算航空航天技術(shù)領(lǐng)域。
【背景技術(shù)】
[0002] 空間機(jī)器人在空間在軌服務(wù)中扮演著一個不可替代的角色,自由漂浮狀態(tài)下的空 間機(jī)器人系統(tǒng)是不穩(wěn)定的,當(dāng)空間機(jī)器人在太空中執(zhí)行任務(wù)時,其運(yùn)動會對基座的位置和 姿態(tài)產(chǎn)生擾動,同時,基座位置和姿態(tài)的改變又會對空間機(jī)器人的運(yùn)動產(chǎn)生一定影響,有可 能導(dǎo)致任務(wù)的失敗,進(jìn)而帶來不可估量的損失。為了維持基座的位置和姿態(tài)保持不變,減少 它們之間的相互影響,則需要對空間機(jī)器人的運(yùn)動軌跡進(jìn)行規(guī)劃,使其在運(yùn)動過程中對基 座的擾動達(dá)到最小。
[0003] 對于空間機(jī)器人最小基座擾動軌跡規(guī)劃的問題一直都是研究的熱點(diǎn)和難點(diǎn)。1991 年,Dubowsky和Torres提出了一種增強(qiáng)干擾圖的技術(shù)方法,此方法可W優(yōu)化空間機(jī)器人 的運(yùn)動軌跡進(jìn)而減小對基座的擾動,但是僅對兩自由度空間機(jī)械臂進(jìn)行了論證,不能廣泛 應(yīng)用于多自由度空間機(jī)械臂;2001年,Yoshida等提出了一種零反作用的思想,基于廣義雅 克比矩陣在基座姿態(tài)無擾動的約束下進(jìn)行空間機(jī)械臂軌跡規(guī)劃,但不適用于冗余空間機(jī)械 臂;2006年,Panfeng化ang等提出了一種基于遺傳算法的最小基座擾動路徑規(guī)劃,但遺傳 算法的實(shí)現(xiàn)比較復(fù)雜且收斂時間較長;2011年,王明等人提出了基于混濁粒子群算法的基 座干擾力矩最小的軌跡規(guī)劃,但并未考慮基座的位姿擾動最??;2014年,夏紅偉等提出基 于混濁粒子群算法的基座姿態(tài)擾動最小軌跡規(guī)劃,但并未考慮基座的位置擾動最小。
[0004] 到目前為止,在空間機(jī)器人軌跡規(guī)劃時并未考慮到空間機(jī)器人的基座位置和姿態(tài) 都應(yīng)該是擾動最小。
【發(fā)明內(nèi)容】
陽〇化]為了克服上述的不足,本發(fā)明的目的在于提出一個基于改進(jìn)混濁粒子群算法的空 間機(jī)器人最小基座擾動軌跡規(guī)劃方法,該方法通過改進(jìn)混濁粒子群算法在基座位姿擾動最 小的情況下求解未知參數(shù),進(jìn)而得到空間機(jī)器人的最優(yōu)運(yùn)動軌跡。
[0006] 為了實(shí)現(xiàn)上述目的,本發(fā)明所采用的技術(shù)方案為:一種空間機(jī)器人最小基座擾動 軌跡規(guī)劃方法,其包括如下步驟:
[0007] (1)利用廣義雅克比矩陣建立6自由度空間機(jī)器人的運(yùn)動學(xué)方程,結(jié)合D-H參數(shù)法 對空間機(jī)器人進(jìn)行建模;
[0008] (2)定義空間機(jī)器人的幾何參數(shù),其包括:包括關(guān)節(jié)的數(shù)量、關(guān)節(jié)之間連桿的長 度、連桿和基座的質(zhì)量及關(guān)節(jié)的力矩;
[0009] (3)利用7階正弦多項(xiàng)式函數(shù)對空間機(jī)器人的關(guān)節(jié)進(jìn)行參數(shù)化;
[0010] (4) W基座位置、姿態(tài)的精度要求,關(guān)節(jié)角速度限制和角加速度的限制為約束條 件,建立空間機(jī)器人基座位置和姿態(tài)擾動最小的關(guān)節(jié)軌跡規(guī)劃目標(biāo)優(yōu)化模型;
[0011] (5)采用改進(jìn)的混濁粒子群算法求解,輸出最優(yōu)值,進(jìn)而得到空間機(jī)器人在基座位 姿擾動最小情況下的最優(yōu)軌跡。
[0012] 所述步驟(4)中,所述目標(biāo)優(yōu)化模型中的目標(biāo)函數(shù)為:
[0013]
[0014] 其中,定義
δ pb為基座末端位置與初始位置之差,δ Qb為基座末 端姿態(tài)與初始姿態(tài)之差,ωρ為位置誤差的權(quán)重系數(shù),ω。為姿態(tài)誤差的權(quán)重系數(shù),嗎為關(guān) 節(jié)角速度約束的權(quán)重系數(shù),嘴為關(guān)節(jié)角加速度約束的權(quán)重系數(shù),《郝巧分別定義為如下 的式子:
[0017] 其中,臺,m 是第i個關(guān)節(jié)的速度最大值,馬,,gK是第i個關(guān)節(jié)的加速度最大值
[0018] 所述步驟巧)中,改進(jìn)的混濁粒子群算法,其采用非線性的動態(tài)慣性權(quán)重,即慣性 權(quán)重隨著粒子的目標(biāo)函數(shù)值而自動改變;加入黃金分割比例的思想,即保留群體中適應(yīng)函 數(shù)值為38. 2%的粒子;選擇Sinusoidal函數(shù)替換Logistic函數(shù)作為混濁序列發(fā)生器。算 法的具體實(shí)現(xiàn)過程為:(1)設(shè)置各個粒子的參數(shù);(2)隨機(jī)初始化種群中各個粒子的位置和 速度;(3)評價每個粒子的目標(biāo)函數(shù)值,將當(dāng)前各個粒子的位置和目標(biāo)函數(shù)值存儲在各個 粒子的pbest中,將所有的目標(biāo)函數(shù)值最有個體的位置和其值存儲在gbest中;(4)更新粒 子的速度和位置;(5)計(jì)算每個粒子的目標(biāo)函數(shù)值,保留種群中性能最好的38. 2%的粒子; (6) 對種群中保存的最佳粒子進(jìn)行混濁局部捜索,并且更新粒子的pbest和種群的gbest ; (7) 如果滿足停止條件(通常為預(yù)設(shè)的最大迭代次數(shù)或是運(yùn)算精度),停止捜索,輸出最優(yōu) 值,如果不滿足,則轉(zhuǎn)為(8) ;(8)進(jìn)行動態(tài)區(qū)域收縮;(9)在收縮的區(qū)域內(nèi)隨機(jī)產(chǎn)生群體中 剩余的61. 8%的粒子,轉(zhuǎn)步驟(3)。
[0019] 所述非線性動態(tài)慣性權(quán)重的公式如下:
[0020]
陽02U 其中,為ω的最小值,ω m。為ω的最大值,f為當(dāng)前粒子的適應(yīng)度函數(shù)值, fmi。為所有粒子的最小適應(yīng)度函數(shù)值,fwg為所有粒子的平均適應(yīng)度函數(shù)值,i為當(dāng)前的迭代 次數(shù),max_gen為最大的迭代次數(shù)。
[0022] 本發(fā)明與現(xiàn)有技術(shù)相比具有W下有益效果:本發(fā)明提出的改進(jìn)混濁粒子群算法被 用于6自由度自由漂浮空間機(jī)器人的路徑規(guī)劃中,考慮到了基座位置、姿態(tài)擾動最小的限 審IJ、關(guān)節(jié)角速度、角加速度限制范圍四個約束條件,具有實(shí)用性;利用改進(jìn)的混濁粒子群算 法可W快速有效的找到最優(yōu)解,提高了求解精度;本發(fā)明提出的改進(jìn)混濁粒子群算法亦可 推廣用于7自由度空間機(jī)械臂的路徑規(guī)劃。
【附圖說明】
[0023] W下通過附圖及具體實(shí)施例對本發(fā)明進(jìn)行詳細(xì)說明。
[0024] 圖1本發(fā)明流程圖;
[00巧]圖2改進(jìn)混濁粒子群算法流程圖; 陽0%] 圖3基座位置變化曲線圖;
[0027] 圖4基座姿態(tài)變化曲線圖;
[0028] 圖5關(guān)節(jié)變化曲線圖;
[0029] 圖6關(guān)節(jié)角速度變化曲線圖;
[0030] 圖7關(guān)節(jié)角加速度變化曲線圖。
【具體實(shí)施方式】
[0031] 下面結(jié)合附圖對本發(fā)明作進(jìn)一步說明,如圖1所示。
[0032] 本發(fā)明包括如下步驟:
[0033] (1)利用廣義雅克比矩陣建立6自由度空間機(jī)器人的運(yùn)動學(xué)方程,結(jié)合D-H參數(shù)法 對空間機(jī)器人進(jìn)行建模,定義了空間機(jī)器人的幾何參數(shù);
[0034] 似利用7階正弦多項(xiàng)式函數(shù)對空間機(jī)器人關(guān)節(jié)進(jìn)行參數(shù)化;
[0035] (3)根據(jù)約束條件建立適應(yīng)度函數(shù)方程,約束條件包括有基座位置、姿態(tài)的精度要 求,對關(guān)節(jié)角速度和角加速度的限制;
[0036] (4)對混濁粒子群算法進(jìn)行改進(jìn);
[0037] (5)利用改進(jìn)的混濁粒子群算法求解未知參數(shù),進(jìn)而得到空間機(jī)器人在基座位姿 擾動最小情況下的最優(yōu)軌跡。 陽ο測實(shí)施例1
[0039] 本發(fā)明的實(shí)施例是在W本發(fā)明技術(shù)方案為前提下進(jìn)行實(shí)施的,給出了詳細(xì)的實(shí)施 方式和具體的操作過程,但本發(fā)明的保護(hù)范圍不限于下述實(shí)施例。
[0040] 具體實(shí)施步驟為:
[0041] 步驟1 :對于自由漂浮的6自由度空間機(jī)器人,由于不受外力,則系統(tǒng)保持動量守 恒,運(yùn)動學(xué)方程可表示為:
[0042]
陽0創(chuàng)其中:ν。^ R3,末端執(zhí)行器的線速度;R3,末端執(zhí)行器的角速度;ν。^ R3, 基座的線速度;ω。^ R3,基座的角速度;Θ e R6,機(jī)器人6個關(guān)節(jié)角組成的關(guān)節(jié)角矩陣。 JbG RSX6,基座的雅克比矩陣;JrG RSX6,機(jī)器人的雅克比矩陣。
[0044] 步驟2 :在本發(fā)明中采用7階多項(xiàng)式函數(shù)對關(guān)節(jié)角進(jìn)行參數(shù)化,如下式所描述:
[0047] 其中,i = 1,…,7, α 1。~α。為多項(xiàng)式系數(shù)
[0048]
W例 Θ i max為第i個關(guān)節(jié)角的最大值,Θ i_mi。為第i個關(guān)節(jié)角的最小值。
[0050] 在運(yùn)動過程中,空間機(jī)器人的關(guān)節(jié)角速度和角加速度均受到限制,且要保障空間 機(jī)器人的軌跡平滑。則對關(guān)節(jié)角的約束如下: 陽 05U Θ i(t〇) = Θ 10,Θ i(tf)=白 if
[0052]
W53] θι "η《目 i(t)《目 i max 陽054] 其中,i = l,'",6, θ 1。為初始關(guān)節(jié)角,θ if為期望關(guān)節(jié)角。 陽化5] 另外,關(guān)節(jié)角速度及角加速度的約束如下:
[0056]
[0057] 其中,W為關(guān)節(jié)角速度的限制值,g為關(guān)節(jié)角加速度的限制值。
[0058] 關(guān)節(jié)角速度,如下式所描述:
[0059]
[0060] 關(guān)節(jié)角加速度,如下式所描述:
[0061]
[0062] 綜合W上式子可得:
W側(cè) 由W上推導(dǎo)可知,關(guān)節(jié)角、角速度及角加速度函數(shù)中的變量只有α W和α。不能確 定,將其記為如下的式子: W例 曰=(日16,曰17,…,曰16,曰17)
[0070] 當(dāng)其確定W后,則對6自由度空間機(jī)器人的關(guān)節(jié)進(jìn)行軌跡規(guī)劃。
[0071] 步驟3 :本發(fā)明是基于空間機(jī)器人基座位置和姿態(tài)擾動最小的關(guān)節(jié)軌跡規(guī)劃,貝U 目標(biāo)函數(shù)定義為如下式子:
[0072]
[007引其中:
δ pb為基座末端位置與初始位置之差,δ Qb為基座末端 姿態(tài)與初始姿態(tài)之差,ωρ為位置誤差的權(quán)重系數(shù),ω q為姿態(tài)誤差的權(quán)重系數(shù),6?為關(guān)節(jié) 角速度約束的權(quán)重系數(shù),保自.為關(guān)節(jié)角加速度約束的權(quán)重系數(shù),與和巧別定義為如下的式 子:
[0076] 其中,3X是第i個關(guān)節(jié)的速度最大值,?是第i個關(guān)節(jié)的加速度最大 值。
[0077] 步驟4 :本發(fā)明提出了一種改進(jìn)混濁粒子群算法,即采用非線性的動態(tài)慣性權(quán)重, 即慣性權(quán)重隨著粒子的目標(biāo)函數(shù)值而自動改變;加入黃金分割比例的思想,即保留群體中 適應(yīng)函數(shù)值最好的38. 2%的粒子;選擇Sinusoidal函數(shù)替換Logistic函數(shù)作為混濁序列 發(fā)生器,整個算法如流程圖2所示。
[0078] 慣性權(quán)重決定了粒子受其本身當(dāng)前速度影響的大小,合適慣性權(quán)重的選擇可W讓 粒子具有均衡的探索能力和開發(fā)能力。本發(fā)明采用改進(jìn)的自適應(yīng)權(quán)重法,非線性動態(tài)慣性 權(quán)重的公式如下:
[0079]
[0080] 其中,為ω的最小值,ω m。為ω的最大值,f為當(dāng)前粒子的適應(yīng)度函數(shù)值, fmi。為所有粒子的最小適應(yīng)度函數(shù)值,fwg為所有粒子的平均適應(yīng)度函數(shù)值,i為當(dāng)前的迭代 次數(shù),max_gen為最大的迭代次數(shù)。
[0081] 在混濁粒子群算法中可W采用不同的混濁序列發(fā)生器,2013年, AmirilosseinGandomi等在文章中對不同的混濁映射函數(shù)利用標(biāo)準(zhǔn)測試函數(shù)進(jìn)行了實(shí)驗(yàn),表 明在大多數(shù)優(yōu)化問題中sinusoidal函數(shù)和singer函數(shù)效果更為明顯,所W在本發(fā)明中采 用了 sinusoidal函數(shù),其簡化公式如下:
[0082] Xk+i= sin ( π X k)
[0083] 混濁粒子群算法是混濁優(yōu)化和粒子群優(yōu)化兩者之間的結(jié)合,而且在算法過程中設(shè) 及到混濁局部捜索算法。在本發(fā)明改進(jìn)的混濁粒子群算法過程中,首先利用自適應(yīng)權(quán)重的 粒子群算法進(jìn)行全局捜索,非線性動態(tài)慣性權(quán)重的公式如方程所示;在捜索完成之后引入 黃金分割比例的思想,保留粒子群體中38. 2%的優(yōu)秀粒子;然后采用sinusoidal函數(shù)作為 混濁序列的發(fā)生器,對38. 2%的粒子進(jìn)行混濁局部捜索;最后,為了保持粒子的多樣性,在 動態(tài)收縮區(qū)域內(nèi)隨機(jī)產(chǎn)生剩余的61. 8%的粒子。動態(tài)收縮區(qū)域公式如下:
[0086] 其中,Xg,.,表示當(dāng)前最優(yōu)值的第j維變量值。
[0087] 步驟5 :利用本發(fā)明提出的改進(jìn)混濁粒子群算法對于空間機(jī)器人最小基座擾動進(jìn) 行軌跡規(guī)劃,建立了一個由自由漂浮基座和6自由度空間機(jī)器人組成系統(tǒng)的運(yùn)動學(xué)模型, 利用Matl油2013b平臺對所提出的方法進(jìn)行仿真與驗(yàn)證,假設(shè)軌跡規(guī)劃時間t = 10s,空間 機(jī)器人的質(zhì)量特性參數(shù)如表1所示,D-H參數(shù)如表2所示。 陽08引表1空間機(jī)器人質(zhì)量特性參數(shù)表
[0089]
[0090] 其中,1為連桿的長度,m為基座和連桿的質(zhì)量,I為機(jī)械臂的慣量。 陽0川表2空間機(jī)器人D-H參數(shù)表
[0092]
[0093] 其中,θ 1為關(guān)節(jié)角,α 1 1為機(jī)械臂的連桿轉(zhuǎn)角,a 1 1為關(guān)節(jié)軸i-1和關(guān)節(jié)軸i之間 公垂線的長度,di為連桿偏距。
[0094] 空間機(jī)器人初始的關(guān)節(jié)角為Θ 1。= [0。,0° ,0° ,0° ,0° ,0° ],期望的關(guān)節(jié) 角為 Θη=[12°,3Γ,-17°,-50°,26°,-83° ]。關(guān)節(jié)角的范圍為: 陽0巧]-160?!栋?+160。,-160。《Θ +160。
[0096] -160?!栋?+160。,-179。《Θ 4《巧0。
[0097] -179?!栋?5《巧0。'-160?!鼎?+160。 陽〇9引關(guān)節(jié)角速度的范圍為:I今(d| <械。/馬扭=1:,...J W"]關(guān)節(jié)角加速度的范圍為:|馬妨I <技〇°7或知1,.·..,6
[0100] 定義適應(yīng)度函數(shù)的系數(shù)分別為:ωρ=2·10 3,Wq = sin ( η /360),巧4 .掛=.化O'l 陽101] 優(yōu)化算法參數(shù)設(shè)置: 陽 10引 N = 30, Ci= C 2= 2, ω max= 0. 9, ω m…=0. 2, X max= 0. 1, 陽 10:3] Xmin二1,Μ = 100, MaxC = 0. 1, D = 12
[0104] 其中,N為粒子數(shù)目,Cl、C2為學(xué)習(xí)因子,X m。潔自變量捜索域的最大值,X mi。是自變 量捜索域的最小值,Μ是最大迭代次數(shù),Maxc是混濁捜索的最大步數(shù),D是自變量的個數(shù)。
[0105] 采用本發(fā)明的改進(jìn)混濁粒子群優(yōu)化算法進(jìn)行軌跡規(guī)劃的最優(yōu)參數(shù)及目標(biāo)函數(shù)值 四舍五入后為: 陽 106]
[0107] 在t時亥Ij,基座的位置坐標(biāo)為[-0.0015,-0. 3177,-3. 0883],誤差為 [-0. 0015, 0. 0156, -0. 0050],基座的姿態(tài)坐標(biāo)為[0. 0169, -0. 0100, 0. 0497, 0. 9986],誤差 為[0. 0169, -0. 0100, 0. 0497, -0. 0014]。
[0108] 本發(fā)明僅僅通過規(guī)劃6自由度自由漂浮空間機(jī)器人的關(guān)節(jié)角就規(guī)劃了其運(yùn)動軌 跡,并且考慮到了基座位置、姿態(tài)擾動最小的限制、關(guān)節(jié)角速度、角加速度限制范圍四個約 束條件,具有實(shí)用性;利用改進(jìn)的混濁粒子群算法可W快速有效的找到最優(yōu)解,提高了求解 精度;本發(fā)明提出的改進(jìn)混濁粒子群算法亦可推廣用于7自由度空間機(jī)器人的路徑規(guī)劃。
[0109] W上所述,僅為本發(fā)明較佳的【具體實(shí)施方式】,但本發(fā)明的保護(hù)范圍并不局限于此, 任何熟悉本技術(shù)領(lǐng)域的技術(shù)人員在本發(fā)明披露的技術(shù)范圍內(nèi),根據(jù)本發(fā)明的技術(shù)方案及其 發(fā)明構(gòu)思加 W等同替換或改變,都應(yīng)涵蓋在本發(fā)明的保護(hù)范圍內(nèi)。
【主權(quán)項(xiàng)】
1. 一種空間機(jī)器人最小基座擾動軌跡規(guī)劃方法,其特征在于:其包括如下步驟: (1) 利用廣義雅克比矩陣建立6自由度空間機(jī)器人的運(yùn)動學(xué)方程,結(jié)合D-H參數(shù)法對空 間機(jī)器人進(jìn)行建模; (2) 定義空間機(jī)器人的幾何參數(shù),其包括:包括關(guān)節(jié)的數(shù)量、關(guān)節(jié)之間連桿的長度、連 桿和基座的質(zhì)量及關(guān)節(jié)的力矩; (3) 利用7階正弦多項(xiàng)式函數(shù)對空間機(jī)器人的關(guān)節(jié)進(jìn)行參數(shù)化; (4) W基座位置、姿態(tài)的精度要求,關(guān)節(jié)角速度限制和角加速度的限制為約束條件,建 立空間機(jī)器人基座位置和姿態(tài)擾動最小的關(guān)節(jié)軌跡規(guī)劃目標(biāo)優(yōu)化模型; (5) 采用改進(jìn)的混濁粒子群算法求解,輸出最優(yōu)值,進(jìn)而得到空間機(jī)器人在基座位姿擾 動最小情況下的最優(yōu)軌跡。2. 根據(jù)權(quán)利要求1所述的空間機(jī)器人最小基座擾動軌跡規(guī)劃方法,其特征在于:所述 步驟(4)中,所述目標(biāo)優(yōu)化模型中的目標(biāo)函數(shù)為:其中,定義,5 Pb為基座末端位置與初始位置之差,5 Qb為基座末端姿 態(tài)與初始姿態(tài)之差,為位置誤差的權(quán)重系數(shù),《。為姿態(tài)誤差的權(quán)重系數(shù),嗎為關(guān)節(jié)角 速度約束的權(quán)重系數(shù),6%;為關(guān)節(jié)角加速度約束的權(quán)重系數(shù),盡和盡分別定義為如下的式 子:其中,接是第i個關(guān)節(jié)的速度最大值,^ m gx是第i個關(guān)節(jié)的加速度最大值。3. 根據(jù)權(quán)利要求1所述的空間機(jī)器人最小基座擾動軌跡規(guī)劃方法,其特征在于:其特 征在于:所述步驟巧)中,改進(jìn)的混濁粒子群算法,其采用非線性的動態(tài)慣性權(quán)重,即慣性 權(quán)重隨著粒子的目標(biāo)函數(shù)值而自動改變;加入黃金分割比例的思想,即保留群體中適應(yīng)函 數(shù)值為38. 2%的粒子;選擇Sinusoidal函數(shù)替換Logistic函數(shù)作為混濁序列發(fā)生器。
【文檔編號】G05B13/04GK105988366SQ201510079167
【公開日】2016年10月5日
【申請日】2015年2月13日
【發(fā)明人】張強(qiáng), 張建霞, 周東生, 魏小鵬
【申請人】大連大學(xué)