冗余空間機(jī)械臂最小基座姿態(tài)擾動(dòng)的軌跡規(guī)劃方法
【專利摘要】本發(fā)明涉及一種基于改進(jìn)混合粒子群算法的冗余空間機(jī)械臂最小基座姿態(tài)擾動(dòng)的軌跡規(guī)劃方法，其屬于航空航天技術(shù)領(lǐng)域。本發(fā)明的步驟如下：(1)設(shè)計(jì)冗余空間機(jī)械臂的三維模型；(2)建立冗余空間機(jī)械臂的運(yùn)動(dòng)學(xué)模型；(3)利用廣義雅克比矩陣建立冗余空間機(jī)械臂的運(yùn)動(dòng)學(xué)方程；(4)采用5階正弦多項(xiàng)式函數(shù)進(jìn)行關(guān)節(jié)的參數(shù)化；(5)在基座姿態(tài)擾動(dòng)最小的約束條件下建立代價(jià)函數(shù)方程式；(6)提出改進(jìn)混合粒子群算法；(7)利用混合粒子群算法對(duì)代價(jià)函數(shù)進(jìn)行優(yōu)化求解，得出在約束條件下的規(guī)劃軌跡。本發(fā)明解決了在基座姿態(tài)擾動(dòng)最小約束條件下冗余空間機(jī)械臂的軌跡規(guī)劃問題，利用改進(jìn)的混合粒子群算法求解的規(guī)劃軌跡精度高且平穩(wěn)，效果較好。
【專利說明】
冗余空間機(jī)械臂最小基座姿態(tài)擾動(dòng)的軌跡規(guī)劃方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及一種冗余空間機(jī)械臂最小基座姿態(tài)擾動(dòng)的軌跡規(guī)劃方法，屬于航空航 天技術(shù)領(lǐng)域。
【背景技術(shù)】
[0002] 隨著各個(gè)國(guó)家在軌服務(wù)技術(shù)的不斷發(fā)展，空間機(jī)器人的應(yīng)用越來越廣泛，也成為 了學(xué)者們研究的熱點(diǎn)和難點(diǎn)。當(dāng)空間機(jī)器人執(zhí)行任務(wù)時(shí)，為了節(jié)約燃料的消耗，姿控系統(tǒng)會(huì) 被主動(dòng)關(guān)閉。此時(shí)，一般的空間機(jī)器人就會(huì)處于自由漂浮狀態(tài)。自由漂浮空間機(jī)器人的基座 具有不固定性，機(jī)械臂與基座之間存在動(dòng)力學(xué)耦合。當(dāng)機(jī)械臂執(zhí)行任務(wù)時(shí)會(huì)引起基座姿態(tài) 的擾動(dòng)，反過來，基座姿態(tài)的擾動(dòng)會(huì)影響機(jī)械臂正常的工作狀態(tài)。為了保證空間在軌服務(wù)的 順利完成，需要基座姿態(tài)保持不變或被控制在一定的范圍之內(nèi)。因此，許多學(xué)者對(duì)于空間機(jī) 械臂的軌跡規(guī)劃算法進(jìn)行了研究，以減小基座姿態(tài)擾動(dòng)的影響。
[0003] 對(duì)于機(jī)械臂的軌跡規(guī)劃問題，國(guó)內(nèi)外學(xué)者提出了多種不同的解決方案。主要包括 有基于Lyaponov雙向搜索法、關(guān)節(jié)周期運(yùn)動(dòng)法、增強(qiáng)擾動(dòng)圖法、零反作用法、關(guān)節(jié)軌跡參數(shù) 法和各種智能優(yōu)化算法?；贚yaponov函數(shù)的方法是由Nakamura和Mukher jee在1989年提 出的。此方法可以同時(shí)調(diào)整關(guān)節(jié)角和基座位姿的狀態(tài)，但是優(yōu)化后的關(guān)節(jié)軌跡不夠平滑，而 且系統(tǒng)的穩(wěn)定性在理論上沒有被證明。1991年，Dubowsky和Torres提出了一種增強(qiáng)擾動(dòng)圖 法進(jìn)行空間機(jī)械臂的運(yùn)動(dòng)規(guī)劃。此方法可以優(yōu)化空間機(jī)器人的運(yùn)動(dòng)軌跡進(jìn)而減小對(duì)基座的 擾動(dòng)，但是僅對(duì)兩自由度空間機(jī)械臂進(jìn)行了論證，不能廣泛應(yīng)用于多自由度空間機(jī)械臂。在 1993年，Vafa和Dubowsky利用虛擬機(jī)械臂的思想提出了一種路徑規(guī)劃方法，即關(guān)節(jié)周期運(yùn) 動(dòng)法。此方法可以使末端執(zhí)行器到達(dá)指定的位姿，但是機(jī)械臂的模型與數(shù)學(xué)模型不太相符 合?；趶V義雅克比矩陣，在2001年Yoshida等提出了一種零反作用的思想。在基座姿態(tài)無(wú) 擾動(dòng)的約束下進(jìn)行空間機(jī)械臂軌跡規(guī)劃，但不適用于冗余空間機(jī)械臂。在2005年， Papadopoulos提出了一種利用多項(xiàng)式函數(shù)對(duì)關(guān)節(jié)角進(jìn)行軌跡參數(shù)化的方法。此方法不受奇 異性的影響，優(yōu)化后的路徑平滑，但是收斂時(shí)間過長(zhǎng)。Huang等在2006年提出了一種基于遺 傳算法的最小基座擾動(dòng)路徑規(guī)劃。但遺傳算法的實(shí)現(xiàn)比較復(fù)雜且收斂時(shí)間較長(zhǎng)。2011年，王 明等人提出了基于混沌粒子群算法的基座干擾力矩最小的軌跡規(guī)劃，但并未考慮基座的位 姿擾動(dòng)最小。Liu Xiaodong等人在2013年提出了利用一種新型的混合全局優(yōu)化算法解決冗 余空間機(jī)械臂的軌跡規(guī)劃問題，但此方法的缺陷是在某些情況下計(jì)算時(shí)間比較長(zhǎng)。2014年， 夏紅偉等提出基于混沌粒子群算法的基座姿態(tài)擾動(dòng)最小軌跡規(guī)劃，其應(yīng)用對(duì)象為6自由度 空間機(jī)械臂，而未考慮冗余空間機(jī)械臂的情況。Wang Mingming等人在2015年闡述了利用改 進(jìn)的粒子群算法求解7自由度空間機(jī)械臂的軌跡參數(shù)，而求解的精度不高。

【發(fā)明內(nèi)容】

[0004] 鑒于現(xiàn)有技術(shù)中存在的問題，本發(fā)明提供一種冗余空間機(jī)械臂最小基座姿態(tài)擾動(dòng) 的軌跡規(guī)劃方法，從冗余空間機(jī)械臂的設(shè)計(jì)開始，并成功地將實(shí)際應(yīng)用問題轉(zhuǎn)化為數(shù)學(xué)求 解問題，直到利用改進(jìn)的混合粒子群算法，采用自然選擇機(jī)理與粒子群算法相結(jié)合，快速有 效的解決了最優(yōu)參數(shù)的求解，得到了良好的規(guī)劃軌跡，提高了精度。
[0005] 為達(dá)到上述目的，本發(fā)明研究了冗余空間機(jī)械臂最小基座姿態(tài)擾動(dòng)的軌跡規(guī)劃問 題，在基座姿態(tài)擾動(dòng)最小的約束條件下，提出一種解決此問題的方法，并且采用改進(jìn)的混合 粒子群算法進(jìn)行參數(shù)優(yōu)化。本發(fā)明的技術(shù)方案如下：
[0006] (1)基于SolidWorks 2013設(shè)計(jì)了7自由度冗余空間機(jī)械臂的三維模型，7個(gè)自由度 均采用旋轉(zhuǎn)關(guān)節(jié)，采用S-R-S結(jié)構(gòu)，大臂與小臂的比例為1:0.618;
[0007] (2)采用經(jīng)典的D-H參數(shù)法建立冗余空間機(jī)械臂的運(yùn)動(dòng)學(xué)模型；
[0008] (3)為了避免計(jì)算過程中奇異性的出現(xiàn)，利用正運(yùn)動(dòng)學(xué)方程與廣義雅克比矩陣推 導(dǎo)出冗余空間機(jī)械臂的運(yùn)動(dòng)學(xué)方程；
[0009] (4)采用5階正弦多項(xiàng)式函數(shù)法對(duì)關(guān)節(jié)角進(jìn)行參數(shù)化；
[0010] (5)在基座姿態(tài)擾動(dòng)最小的約束條件下建立代價(jià)函數(shù)方程；
[0011] (6)根據(jù)自然界中"適者生存，不適者被淘汰"的原則，并且結(jié)合多群體協(xié)同進(jìn)化， 提出一種改進(jìn)混合粒子群算法；
[0012] (7)利用改進(jìn)的混合粒子群算法求解最優(yōu)參數(shù)，得到最優(yōu)規(guī)劃軌跡。
[0013] 所述步驟（1)中，冗余空間機(jī)械臂的7個(gè)自由度均采用旋轉(zhuǎn)關(guān)節(jié)，能夠滿足復(fù)雜空 間環(huán)境下的在軌服務(wù)。其常見的結(jié)構(gòu)有四種，本文采用的S-R-S結(jié)構(gòu)被證明在運(yùn)動(dòng)學(xué)上是最 優(yōu)的。根據(jù)冗余空間機(jī)械臂的設(shè)計(jì)模型，基于SolidWorks 2013進(jìn)行三維建模。在建模過程 中采用模塊化和自頂向下的設(shè)計(jì)原則，即從草圖設(shè)計(jì)-工作特征設(shè)計(jì)-零件設(shè)計(jì)-裝配體。
[0014] 所述步驟(5)中，在基座姿態(tài)擾動(dòng)最小的約束下建立代價(jià)函數(shù)方程為：
[0015] 0i(t)= A (t)
[0017]其中，范數(shù)| |5qb| | =5qbT5qb，為姿態(tài)誤差的權(quán)重系數(shù)。
[0018] 所述步驟(7)中，改進(jìn)的混合粒子群算法策略受由自然界中的生物原則啟發(fā)，包括 自然選擇機(jī)理與多群體協(xié)同進(jìn)化。算法的具體實(shí)驗(yàn)方案包括：（1)設(shè)置參數(shù)；（2)隨機(jī)初始化 三個(gè)種群中粒子的速度和位置；（3)每個(gè)種群計(jì)算每個(gè)粒子的代價(jià)函數(shù)值，將各個(gè)粒子自身 最優(yōu)的位置和代價(jià)函數(shù)值存儲(chǔ)在PU中，將種群的最優(yōu)位置和代價(jià)函數(shù)值存儲(chǔ)在p gj中；（4) 更新粒子的速度和位置；（5)對(duì)每個(gè)粒子自身當(dāng)前的位置與之前最優(yōu)位置進(jìn)行比較，如果比 較好，則將其更新為最好的位置，反之，則保留之前最優(yōu)位置；（6)比較當(dāng)前所有的 Plj和pgJ， 并且更新口^; (7)將粒子按照代價(jià)函數(shù)值進(jìn)行排序，用群體中最好一半粒子的速度和位置替 換最差一半的速度和位置，同時(shí)保留每個(gè)個(gè)體的最優(yōu)值；（8)選出每個(gè)種群中的最優(yōu)粒子和 最差粒子，3個(gè)最優(yōu)粒子進(jìn)行以上步驟的迭代選出最佳粒子，然后替換每個(gè)種群中的最差粒 子;（9)滿足迭代次數(shù)，輸出最優(yōu)參數(shù)，確定軌跡。
[0019] 本發(fā)明與現(xiàn)有技術(shù)相比具有以下有益效果:本發(fā)明提出了一種合適的解決方案對(duì) 于冗余空間機(jī)械臂基座擾動(dòng)最小軌跡規(guī)劃問題，從冗余空間機(jī)械臂的設(shè)計(jì)開始，并成功地 將實(shí)際應(yīng)用問題轉(zhuǎn)化為數(shù)學(xué)求解問題，直到利用改進(jìn)的混合粒子群算法快速有效的解決了 最優(yōu)參數(shù)的求解，得到了良好的規(guī)劃軌跡，提高了精確度和穩(wěn)定性。
【附圖說明】
[0020] 以下通過附圖及具體實(shí)施例對(duì)本發(fā)明進(jìn)行詳細(xì)說明。
[0021] 圖1本發(fā)明技術(shù)方案流程圖；
[0022] 圖2簡(jiǎn)化設(shè)計(jì)模型圖；
[0023] 圖3三維模型圖；
[0024]圖4改進(jìn)混合粒子群算法流程圖；
[0025]圖5基座姿態(tài)曲線圖；
[0026] 圖6關(guān)節(jié)曲線圖；
[0027]圖7關(guān)節(jié)角速度曲線圖；
[0028]圖8關(guān)節(jié)角加速度曲線圖。
【具體實(shí)施方式】
[0029]下面結(jié)合附圖對(duì)本發(fā)明作進(jìn)一步說明，如圖1所示。
[0030]本發(fā)明的步驟如下：
[0031] (1)基于SolidWorks 2013設(shè)計(jì)了7自由度冗余空間機(jī)械臂的三維模型；
[0032] (2)采用經(jīng)典的D-H參數(shù)法建立冗余空間機(jī)械臂的運(yùn)動(dòng)學(xué)模型；
[0033] (3)利用正運(yùn)動(dòng)學(xué)方程與廣義雅克比矩陣推導(dǎo)出冗余空間機(jī)械臂的運(yùn)動(dòng)學(xué)方程；
[0034] (4)采用5階正弦多項(xiàng)式函數(shù)法對(duì)關(guān)節(jié)角進(jìn)行參數(shù)化；
[0035] (5)在基座姿態(tài)擾動(dòng)最小的約束條件下建立代價(jià)函數(shù)方程；
[0036] (6)提出一種改進(jìn)的混合粒子群算法；
[0037] (7)利用改進(jìn)的混合粒子群算法求解最優(yōu)參數(shù)，得到最優(yōu)規(guī)劃軌跡。
[0038] 實(shí)施例1
[0039]本發(fā)明的實(shí)施例是在以本發(fā)明技術(shù)方案為前提下進(jìn)行實(shí)施的，給出了詳細(xì)的實(shí)施 方式和具體的操作過程，但本發(fā)明的保護(hù)范圍不限于下述實(shí)施例。
[0040] 具體實(shí)施步驟如下：
[0041] 步驟1:本發(fā)明的研究對(duì)象是冗余空間機(jī)械臂，它的7個(gè)自由度均采用旋轉(zhuǎn)關(guān)節(jié)，能 夠滿足復(fù)雜空間環(huán)境下的在軌服務(wù)。其常見的結(jié)構(gòu)有四種，本文采用的S-R-S結(jié)構(gòu)被證明在 運(yùn)動(dòng)學(xué)上是最優(yōu)的。其簡(jiǎn)化的設(shè)計(jì)模型如圖2所示。根據(jù)冗余空間機(jī)械臂的設(shè)計(jì)模型，基于 SolidWorks 2013進(jìn)行三維建模。在建模過程中采用模塊化和自頂向下的設(shè)計(jì)原則，即從草 圖設(shè)計(jì)_工作特征設(shè)計(jì)-零件設(shè)計(jì)-裝配體。冗余空間機(jī)械臂的三維模型如圖3所示。冗余空 間機(jī)械臂的三維可視化建模不僅形象生動(dòng)，更能讓人們有直觀的了解和認(rèn)知。
[0042] 步驟2:本發(fā)明采用D-H參數(shù)法建立機(jī)械臂的運(yùn)動(dòng)學(xué)模型。冗余空間機(jī)械臂的D-H參 數(shù)如表1所示。
[0043] 表1冗余空間機(jī)械臂的D-H參數(shù)表
[0045] 其中，ai-hdi和ai-邊三個(gè)固定不變的連桿參數(shù)，0i是關(guān)節(jié)變量。 ai-:表示沿Xi-i軸， 從Zi-i移動(dòng)到Zi的距離;di表示沿Zi軸，從Xi-i移動(dòng)到Xi的距離;表示繞Xi-4S，從Zi-i旋轉(zhuǎn) 到冗:的角度，逆時(shí)針為正;0:表示繞Z4S，從Xh旋轉(zhuǎn)到&的角度，逆時(shí)針為正。
[0046] 空間機(jī)器人系統(tǒng)由空間機(jī)械臂、基座和衛(wèi)星組成，本文所采用的空間機(jī)器人質(zhì)量 特性參數(shù)如表2所示。
[0047]表2空間機(jī)器人質(zhì)量特性參數(shù)
[0048]
[0049] 步驟3:冗余空間機(jī)械臂的運(yùn)動(dòng)學(xué)分析是其他空間機(jī)械臂技術(shù)研究的基礎(chǔ)和前提。 正運(yùn)動(dòng)學(xué)分析是在已知空間機(jī)械臂連桿參數(shù)和關(guān)節(jié)變量的情況下，求末端執(zhí)行器相對(duì)于參 考坐標(biāo)系的位姿解。冗余空間機(jī)械臂相鄰坐標(biāo)系之間轉(zhuǎn)換矩陣的一般表達(dá)式為：
[0051]將7自由度空間機(jī)械臂和基座看作一個(gè)系統(tǒng)，在慣性坐標(biāo)系內(nèi)機(jī)械臂末端執(zhí)行器 的位置矢量可以表不為如下式子：
[0053]其中，Pe G R3X3表示末端執(zhí)行器相對(duì)于慣性坐標(biāo)系的位置矢量，rQ G R3X3表示基座 質(zhì)心相對(duì)于慣性坐標(biāo)系的位置矢量，bQeR3x3表示從基座質(zhì)心指向第一個(gè)關(guān)節(jié)的位置矢量， PleR3x3表示第i個(gè)關(guān)節(jié)相對(duì)于慣性坐標(biāo)系的位置矢量，po表示基座相對(duì)于慣性坐標(biāo)系的位 置矢量。
[0054]對(duì)上式進(jìn)行微分可以得到末端執(zhí)行器的線速度，如下式所示：
[0056]其中，V(5 G R3表示某一時(shí)刻末端執(zhí)行器的線速度，VQ G R3表示某一時(shí)刻基座的線速 度，《QGR3表示某一時(shí)刻基座的角速度，kiGR3表示在慣性坐標(biāo)系里第i個(gè)坐標(biāo)系Z軸的方 向向量。
[0057]對(duì)上式進(jìn)行微分可以得到末端執(zhí)行器的角速度，如下式所示：
[0059] 其中，c〇eGR3表示某一時(shí)刻末端執(zhí)行器的角速度。
[0060] 利用廣義雅克比矩陣對(duì)7自由度空間機(jī)械臂進(jìn)行運(yùn)動(dòng)學(xué)建模。機(jī)械臂的雅克比矩 陣如下式所示：
[0062]其中，Jm表示機(jī)械臂的雅克比矩陣。
[0063]基座的雅克比矩陣下式所示：
[0065] 其中，Jb表示基座的雅克比矩陣，E表示單位矩陣，pQe = pe-r0。
[0066]結(jié)合以上式子可推導(dǎo)出7自由度空間機(jī)械臂的運(yùn)動(dòng)學(xué)方程，如下式所示：
[0068]其中，0表示關(guān)節(jié)角矩陣。
[0069]由于自由漂浮空間機(jī)械臂的整個(gè)系統(tǒng)不受外力，假設(shè)系統(tǒng)初始的線動(dòng)量和角動(dòng)量 都為〇，則根據(jù)動(dòng)量守恒定律可以推導(dǎo)出如下式所示：
[0071 ]其中，Ib表示基座的慣性矩陣，Im表示機(jī)械臂的慣性矩陣。
[0072]則自由漂浮空間機(jī)械臂系統(tǒng)的廣義雅克比矩陣可表示為如下式所示：
[0074] 其中，Ibm表示耦合慣性矩陣，J(K 9，mi，Ii)表示系統(tǒng)的廣義雅克比矩陣池表示基 座的姿態(tài)，mi表示第i個(gè)連桿的質(zhì)量，Ii表示第i個(gè)連桿的慣量矩陣。
[0075] 步驟4:在本發(fā)明中5階正弦函數(shù)多項(xiàng)式法被用來進(jìn)行關(guān)節(jié)角ejt)的參數(shù)化，如下 式所述：
[0076] 9i(t) = A iisin(ai5t5+ai4t4+ai3t3+ai2t2+aiit+ai〇) +A i2
[0077] 其中，i = l，2,…，7，aiQ~&15是多項(xiàng)式系數(shù)，A u與A i2的表達(dá)如下式所示：
[0079] 其中，0^_為第i個(gè)關(guān)節(jié)角的最小值，為第i個(gè)關(guān)節(jié)角的最大值。
[0080] 則關(guān)節(jié)角速度、角加速度可分別表示為如下式子：
[0083] 在運(yùn)動(dòng)過程中，空間機(jī)械臂的約束條件如下式所示：
[0084] 0i(t〇) = 0i〇，0i(tf) = 9if
[0085] 0： (/") = 6^ (/; ) = 0,0, (/,) ：= 0, (/, ) = 0
[0086]其中，01()為初始關(guān)節(jié)角，0^為期望關(guān)節(jié)角。
[0087]將以上約束條件式代入式關(guān)節(jié)角、角速度和角加速度中得：
[0089 ] ai3 = 5/3ai5tf2, ai4 = -5/2aistf
[0090] 由以上式子可知，只有ai5為未知變量，其可以表達(dá)為如下式子：
[0091] a=[ai5，...，a75]
[0092] 因此當(dāng)未知變量a確定時(shí)，冗余空間機(jī)械臂的軌跡就可以被規(guī)劃了。這樣，一個(gè)實(shí) 際的應(yīng)用問題就被轉(zhuǎn)化成了數(shù)學(xué)求解問題。
[0093] 步驟5:本發(fā)明采用四元數(shù)法對(duì)基座的姿態(tài)進(jìn)行描述與表達(dá)。四元數(shù)法的方程如下 式所示：
[0094] Qb= [n,qi]
[0095] 其中，n為四元數(shù)的標(biāo)量部分，qi(i = l，2,3)為四元數(shù)的矢量部分。它們可以表達(dá) 為如下式所示：
[0096] /； = cos(^>/2),q = ksin(^/2)
[0097] 其中，夢(mèng)為坐標(biāo)系繞著歐拉軸所轉(zhuǎn)過的角度，k為歐拉軸單位方向的向量。
[0098] 四元數(shù)法的約束條件如下式所示：
[0099] q2+qi2+q22+q3 2= 1
[0100] 通過四元數(shù)與角速度之間的關(guān)系推導(dǎo)出用四元數(shù)表示的基座姿態(tài)的誤差，如下式 所示：
[0102] 本發(fā)明是求解在基座姿態(tài)擾動(dòng)最小的約束條件下冗余空間機(jī)械臂的軌跡規(guī)劃，則 代價(jià)函數(shù)如下式所示。
[0103] 9i(t) = A (t)
[0105]其中，范數(shù)I |5qb| I =5qbT5qb，Oq為姿態(tài)誤差的權(quán)重系數(shù)。
[0106] 步驟6:在實(shí)際應(yīng)用中，粒子群算法的缺陷是易陷入局部最優(yōu)或求解精度不高。為 了克服易陷入局部最優(yōu)的缺點(diǎn)，提出一種多群體協(xié)同進(jìn)化的方法。此方法受自然界生物系 統(tǒng)中"適者生存"的原則，具體策略包括3步，分別是（1)選擇三個(gè)種群進(jìn)行初始化，分別選出 每個(gè)種群中最優(yōu)與最差的粒子；（2)選出的三個(gè)最優(yōu)粒子組成一個(gè)新的種群并選出最佳粒 子；（3)用最佳粒子替換三個(gè)種群的最差粒子。這樣群體之間的信息共享與交流在一定程度 上就避免了陷入局部最優(yōu)的危險(xiǎn)。為了提高求解精度，采用自然選擇機(jī)理與粒子群算法相 結(jié)合。自然選擇就是生物在生存斗爭(zhēng)中適者生存，不適者被淘汰現(xiàn)象的體現(xiàn)。具體應(yīng)用時(shí)的 基本思想是每次迭代時(shí)將粒子按代價(jià)函數(shù)值進(jìn)行排序，用群體中最好一半粒子的速度和位 置替換最差一半的速度和位置，同時(shí)保留每個(gè)個(gè)體的最優(yōu)值。
[0107] 步驟7:利用改進(jìn)混合粒子群算法求解冗余空間機(jī)械臂最小基座姿態(tài)擾動(dòng)軌跡規(guī) 劃問題的具體實(shí)驗(yàn)步驟如下：
[0108] (1)設(shè)置參數(shù):粒子數(shù)目為N，慣性權(quán)重為《，學(xué)習(xí)因子分別為cdPc2，最大迭代次數(shù) 為M，自變量的個(gè)數(shù)為D;
[0109] (2)隨機(jī)初始化三個(gè)種群中粒子的速度和位置；
[0110] (3)每個(gè)種群計(jì)算每個(gè)粒子的代價(jià)函數(shù)值，將各個(gè)粒子自身最優(yōu)的位置和代價(jià)函 數(shù)值存儲(chǔ)在PU中，將種群的最優(yōu)位置和代價(jià)函數(shù)值存儲(chǔ)在Pg中； (4)更新粒子的速度和位置；
[0112] (5)對(duì)每個(gè)粒子自身當(dāng)前的位置與之前最優(yōu)位置進(jìn)行比較，如果比較好，則將其更 新為最好的位置，反之，則保留之前最優(yōu)位置；
[0113] (6)比較當(dāng)前所有的pij和pgj，并且更新pgj;
[0114] (7)將粒子按照代價(jià)函數(shù)值進(jìn)行排序，用用群體中最好一半粒子的速度和位置替 換最差一半的速度和位置，同時(shí)保留每個(gè)個(gè)體的最優(yōu)值；
[0115] (8)選出每個(gè)種群中的最優(yōu)粒子和最差粒子，3個(gè)最優(yōu)粒子進(jìn)行以上步驟的迭代選 出最佳粒子，然后替換每個(gè)種群中的最差粒子；
[0116] (9)滿足迭代次數(shù)，輸出最優(yōu)位置a = [a15，…，a75 ]，確定軌跡。
[0117] 基于改進(jìn)混合粒子群算法的冗余空間機(jī)械臂最小基座姿態(tài)擾動(dòng)軌跡規(guī)劃的算法 流程如圖4所示。
[0118] 為了驗(yàn)證本發(fā)明所提出方法的有效性，以步驟1所描述的冗余空間機(jī)械臂為研究 對(duì)象，利用平臺(tái)MATLAB R2013b對(duì)其進(jìn)行實(shí)驗(yàn)與仿真。參數(shù)設(shè)置如下：
[0119] 基座的初始與期望姿態(tài)相等如下式所示：
[0120] Qb〇 = Qbf=[l 0 0 0]T
[0121] 冗余空間機(jī)械臂關(guān)節(jié)角的范圍如下式所示：
[0124]冗余空間機(jī)械臂關(guān)節(jié)角速度與關(guān)節(jié)角加速度的范圍如下式所示：
[0126] 代價(jià)函數(shù)中的權(quán)重系數(shù)如下式所示：
[0127] coq = sin(3i/360)
[0128] 本發(fā)明提出改進(jìn)混合粒子群算法的參數(shù)設(shè)置下式所示：
[0129] N=30,ci = C2 = 2, ? = 0.5,D = 7,M=100
[0130] 為了公平的比較本文改進(jìn)混合粒子群算法、粒子群算法和線性遞減權(quán)重粒子群算 法的有效性，所有實(shí)驗(yàn)獨(dú)立運(yùn)行50次。通過實(shí)驗(yàn)仿真，求得的最優(yōu)A、最差A(yù)、A的平均值和 方差如表3所示。
[0131] 表3仿真結(jié)果
[0132]
[0133] 在表3中，加粗表示最好的結(jié)果，最優(yōu)結(jié)果表示50次運(yùn)行中得到的最小值，最差結(jié) 果表示得到的最大值，平均值表示了整體水平值，方差表示了結(jié)果的穩(wěn)定性。通過表3中的 數(shù)據(jù)可以表明，采用本發(fā)明改進(jìn)混合粒子群算法得到的結(jié)果比其他兩種算法均好。因此，本 發(fā)明所提出的改進(jìn)混合粒子群算法在求解冗余空間機(jī)械臂最小基座姿態(tài)擾動(dòng)軌跡規(guī)劃問 題上更加有優(yōu)勢(shì)。
[0134] 通過本發(fā)明所得到的優(yōu)化參數(shù)a和最優(yōu)值A(chǔ)如下式所示：
[0136] 在整個(gè)軌跡規(guī)劃過程中，基座的姿態(tài)變化如圖2所示，冗余空間機(jī)械臂的關(guān)節(jié)角、 角速度和角加速度分別如圖3、圖4和圖5所示?；淖藨B(tài)值如下式所示：
[0137] Qba=[l 1.07e-13 7.7e-14 1.18e-13]T
[0138] 仿真實(shí)驗(yàn)結(jié)果表明本發(fā)明所提方法的有效性。該方法有效的解決了冗余空間機(jī)械 臂最小基座姿態(tài)擾動(dòng)軌跡規(guī)劃問題，并且約束了關(guān)節(jié)角、角速度和角加速度。該方法有較強(qiáng) 的實(shí)用價(jià)值，對(duì)于其他類型的軌跡規(guī)劃問題具有一定的指導(dǎo)意義。同時(shí)，與粒子群算法及線 性遞減權(quán)重粒子群算法相比較，本發(fā)明提出的改進(jìn)混合粒子群算法在優(yōu)化求解時(shí)有著更高 精度和更加穩(wěn)定的優(yōu)勢(shì)。
[0139] 以上所述，僅為本發(fā)明較佳的【具體實(shí)施方式】，但本發(fā)明的保護(hù)范圍并不局限于此， 任何熟悉本技術(shù)領(lǐng)域的技術(shù)人員在本發(fā)明披露的技術(shù)范圍內(nèi)，根據(jù)本發(fā)明的技術(shù)方案及其 發(fā)明構(gòu)思加以等同替換或改變，都應(yīng)涵蓋在本發(fā)明的保護(hù)范圍內(nèi)。
【主權(quán)項(xiàng)】
1. 冗余空間機(jī)械臂最小基座姿態(tài)擾動(dòng)的軌跡規(guī)劃方法，其特征在于:此方法包括如下 步驟： 步驟一:基于Sol idWorks進(jìn)行冗余空間機(jī)械臂的三維模型設(shè)計(jì)； 步驟二:采用D-H參數(shù)法建立冗余空間機(jī)械臂的運(yùn)動(dòng)學(xué)模型，利用廣義雅克比矩陣進(jìn)行 運(yùn)動(dòng)學(xué)方程的建立，采用5階正弦多項(xiàng)式函數(shù)進(jìn)行關(guān)節(jié)的參數(shù)化； 步驟三:在基座姿態(tài)擾動(dòng)最小的約束下建立代價(jià)函數(shù)方程； 步驟四：提出一種改進(jìn)的混合粒子群算法，其具體策略為：首先，選擇三個(gè)種群進(jìn)行初 始化，選出每個(gè)種群中最優(yōu)與最差的粒子;其次，選出的三個(gè)最優(yōu)粒子組成一個(gè)新的種群并 選出最佳粒子;最后，利用最佳粒子替換三個(gè)種群的最差粒子； 步驟五:采用改進(jìn)的混合粒子群算法求解最優(yōu)參數(shù)，得到利用本方法求得在基座姿態(tài) 擾動(dòng)最小下的規(guī)劃軌跡。2. 根據(jù)權(quán)利要求1所述的冗余空間機(jī)械臂最小基座姿態(tài)擾動(dòng)的軌跡規(guī)劃方法，其特征 在于:所述步驟一為基于SolidWorks進(jìn)行了7自由度冗余空間機(jī)械臂的設(shè)計(jì)并且建立了三 維模型，其構(gòu)型采用S-R-S結(jié)構(gòu)，7個(gè)自由度均為旋轉(zhuǎn)關(guān)節(jié)，大臂與小臂采用黃金分割比例即 為1:0.618。3. 根據(jù)權(quán)利要求1所述的冗余空間機(jī)械臂最小基座姿態(tài)擾動(dòng)的軌跡規(guī)劃方法，其特征 在于:所述步驟三中在基座姿態(tài)擾動(dòng)最小的約束下建立代價(jià)函數(shù)方程為： 0i(t)= A (t)其中，范數(shù)| |Sqb| | =5qbTSqb，為姿態(tài)誤差的權(quán)重系數(shù)。4. 根據(jù)權(quán)利要求1所述的冗余空間機(jī)械臂最小基座姿態(tài)擾動(dòng)的軌跡規(guī)劃方法，其特征 在于:所述步驟五中改進(jìn)的混合粒子群算法具體應(yīng)用時(shí)的基本思想是每次迭代時(shí)將粒子按 代價(jià)函數(shù)值進(jìn)行排序，用群體中最好一半粒子的速度和位置替換最差一半的速度和位置， 同時(shí)保留每個(gè)個(gè)體的最優(yōu)值。5. 根據(jù)權(quán)利要求1所述的冗余空間機(jī)械臂最小基座姿態(tài)擾動(dòng)的軌跡規(guī)劃方法，其特征 在于:所述步驟五中改進(jìn)的混合粒子群算法具體實(shí)驗(yàn)方案包括：（1)設(shè)置參數(shù)；（2)隨機(jī)初始 化三個(gè)種群中粒子的速度和位置；（3)每個(gè)種群計(jì)算每個(gè)粒子的代價(jià)函數(shù)值，將各個(gè)粒子自 身最優(yōu)的位置和代價(jià)函數(shù)值存儲(chǔ)在PU中，將種群的最優(yōu)位置和代價(jià)函數(shù)值存儲(chǔ)在Pm中； (4)更新粒子的速度和位置；（5)對(duì)每個(gè)粒子自身當(dāng)前的位置與之前最優(yōu)位置進(jìn)行比較，如 果比較好，則將其更新為最好的位置，反之，則保留之前最優(yōu)位置；（6)比較當(dāng)前所有的 PlJ和 pgJ，并且更新Pfu; (7)將粒子按照代價(jià)函數(shù)值進(jìn)行排序，用群體中最好一半粒子的速度和位 置替換最差一半的速度和位置，同時(shí)保留每個(gè)個(gè)體的最優(yōu)值；（8)選出每個(gè)種群中的最優(yōu)粒 子和最差粒子，3個(gè)最優(yōu)粒子進(jìn)行以上步驟的迭代選出最佳粒子，然后替換每個(gè)種群中的最 差粒子;（9)滿足迭代次數(shù)，輸出最優(yōu)參數(shù)，確定軌跡。
【文檔編號(hào)】G06F17/50GK106055522SQ201610503870
【公開日】2016年10月26日
【申請(qǐng)日】2016年6月30日
【發(fā)明人】張強(qiáng), 張建霞, 周東生, 魏小鵬
【申請(qǐng)人】大連大學(xué)