本發(fā)明涉及航天器在軌服務(wù)技術(shù)，具體涉及一種基于耦合動(dòng)力學(xué)建模的平臺(tái)無(wú)擾動(dòng)空間機(jī)械臂運(yùn)動(dòng)路徑規(guī)劃方法。
背景技術(shù)：
：隨著航天技術(shù)發(fā)展對(duì)航天器的性能、結(jié)構(gòu)、組成提出的日趨復(fù)雜要求，航天器在復(fù)雜的空間環(huán)境中持久、可靠運(yùn)行成為完成復(fù)雜任務(wù)的前提條件。針對(duì)這一需求，目前正在大力發(fā)展的在軌服務(wù)技術(shù)將成為解決上述重要問(wèn)題的重要途徑：通過(guò)針對(duì)復(fù)雜大型航天器開(kāi)展的在軌捕獲、模塊維修與部件更換等操作，可顯著提升航天器在軌運(yùn)行的可靠性，降低重新發(fā)射替代航天器的時(shí)間和資金成本，因而具有廣泛應(yīng)用前景。在軌操作通常需要服務(wù)航天器在經(jīng)歷遠(yuǎn)程變軌、近程導(dǎo)引和超近距離逼近后，利用其上配置的機(jī)械臂對(duì)目標(biāo)上的部件進(jìn)行抓取操作。然而，由于機(jī)械臂和航天器本體的動(dòng)力學(xué)耦合效應(yīng)，地面固定基座機(jī)械臂的運(yùn)動(dòng)規(guī)劃算法不適用于空間機(jī)械臂系統(tǒng)，目標(biāo)捕獲后空間機(jī)械臂的運(yùn)動(dòng)通常都會(huì)對(duì)本體的姿態(tài)穩(wěn)定形成干擾，這對(duì)星上能源消耗、GNC系統(tǒng)的運(yùn)行、對(duì)地通信等任務(wù)都形成安全隱患。因此，有必要在機(jī)械臂運(yùn)動(dòng)與航天器本體姿態(tài)轉(zhuǎn)動(dòng)耦合關(guān)系分析的基礎(chǔ)上，尋找運(yùn)動(dòng)學(xué)約束條件下對(duì)本體姿態(tài)無(wú)擾動(dòng)的機(jī)械臂最優(yōu)運(yùn)動(dòng)路徑規(guī)劃方法，使該方法同樣適于利用機(jī)械臂運(yùn)動(dòng)對(duì)本體進(jìn)行輔助姿態(tài)控制方案的設(shè)計(jì)。技術(shù)實(shí)現(xiàn)要素：為了解決對(duì)航天器本體無(wú)擾動(dòng)的空間機(jī)械臂的非完整路徑規(guī)劃問(wèn)題，本發(fā)明提供一種多自由度空間機(jī)械臂非完整路徑規(guī)劃方法，利用遺傳算法對(duì)機(jī)械臂各關(guān)節(jié)運(yùn)動(dòng)參數(shù)尋優(yōu)，以確定特定運(yùn)動(dòng)學(xué)約束條件下機(jī)械臂最優(yōu)運(yùn)動(dòng)路徑。本發(fā)明所述的多自由度空間機(jī)械臂非完整路徑規(guī)劃方法，包括如下步驟：步驟一，機(jī)械臂各關(guān)節(jié)運(yùn)動(dòng)規(guī)律函數(shù)化：采用正弦函數(shù)對(duì)關(guān)節(jié)角進(jìn)行參數(shù)化設(shè)計(jì)；步驟二，化簡(jiǎn)待定參數(shù)：利用初始、終端、過(guò)程狀態(tài)約束條件將未知參數(shù)用待定參數(shù)表示，以減少待定參數(shù)個(gè)數(shù)；步驟三，設(shè)計(jì)適應(yīng)度目標(biāo)函數(shù)：基于終端時(shí)刻本體控制精度和過(guò)程約束要求設(shè)計(jì)合適的適應(yīng)度目標(biāo)函數(shù)；步驟四，待定參數(shù)的遺傳算法尋優(yōu)：基于本體-臂耦合動(dòng)力學(xué)關(guān)系尋找使適應(yīng)度目標(biāo)函數(shù)最小的待定參數(shù)組合，據(jù)此確定機(jī)械臂完整運(yùn)動(dòng)路徑。與現(xiàn)有技術(shù)相比，本發(fā)明具有以下優(yōu)點(diǎn)：1、采用遺傳算法可以搜索得到機(jī)械臂運(yùn)動(dòng)待定參數(shù)矩陣的全局最優(yōu)解，可保證機(jī)械臂各關(guān)節(jié)運(yùn)動(dòng)期望角度的同時(shí)實(shí)現(xiàn)本體穩(wěn)定或達(dá)到期望姿態(tài)，實(shí)現(xiàn)本體姿態(tài)的機(jī)械臂運(yùn)動(dòng)輔助控制，節(jié)省衛(wèi)星本體姿態(tài)控制的能量消耗與任務(wù)執(zhí)行時(shí)間。2、僅用到了表示機(jī)械臂關(guān)節(jié)角速度-本體姿態(tài)角速度關(guān)系的正運(yùn)動(dòng)學(xué)方程，且根據(jù)實(shí)際終端及過(guò)程運(yùn)動(dòng)約束，采用正弦函數(shù)對(duì)關(guān)節(jié)路徑進(jìn)行參數(shù)化設(shè)計(jì)，得到了平滑的關(guān)節(jié)運(yùn)動(dòng)形式，適合于實(shí)際執(zhí)行機(jī)構(gòu)對(duì)機(jī)械臂控制的工程應(yīng)用。附圖說(shuō)明以下將結(jié)合附圖和具體實(shí)施方式對(duì)本發(fā)明作進(jìn)一步的說(shuō)明。圖1是基于遺傳算法的平臺(tái)無(wú)擾動(dòng)空間機(jī)械臂運(yùn)動(dòng)參數(shù)尋優(yōu)流程圖。具體實(shí)施方式下文中，結(jié)合附圖和實(shí)施例對(duì)本發(fā)明做進(jìn)一步闡述。如圖1所示，本實(shí)施例的多自由度空間機(jī)械臂非完整路徑規(guī)劃方法，包括如下步驟：步驟一、機(jī)械臂各關(guān)節(jié)運(yùn)動(dòng)規(guī)律函數(shù)化；步驟二、利用兩端約束條件化簡(jiǎn)待定參數(shù)；步驟三、基于終端時(shí)刻控制精度和過(guò)程約束要求確定適應(yīng)度目標(biāo)函數(shù)；步驟四、利用遺傳算法對(duì)待定參數(shù)尋優(yōu)。步驟一，機(jī)械臂各關(guān)節(jié)運(yùn)動(dòng)規(guī)律函數(shù)化：采用正弦函數(shù)對(duì)關(guān)節(jié)角進(jìn)行參數(shù)化設(shè)計(jì)；采用了關(guān)于時(shí)間的五次多項(xiàng)式正弦函數(shù)對(duì)關(guān)節(jié)角進(jìn)行參數(shù)化設(shè)計(jì)，直接約束了關(guān)節(jié)角的運(yùn)動(dòng)范圍，同時(shí)保證了機(jī)械臂運(yùn)動(dòng)過(guò)程的平滑，該函數(shù)關(guān)系表示為：θi(t)＝Ai1sin(ai7t7+ai6t6+ai5t5+ai4t4+ai3t3+ai2t2+ai1t+ai0)+Ai2(1)其中，變量下標(biāo)i對(duì)應(yīng)機(jī)械臂系統(tǒng)的第i個(gè)關(guān)節(jié)。對(duì)上式求時(shí)間導(dǎo)數(shù)，可得：θ·i(t)=Ai1cos(ai7t7+ai6t6+ai5t5+ai4t4+ai3t3+ai2t2+ai1t+ai0)·(7ai7t6+6ai6t5+5ai5t4+4ai4t3+3ai3t2+2ai2t+ai1)---(8)]]>θ··i(t)=-Ai1sin(ai7t7+ai6t6+ai5t5+ai4t4+ai3t3+ai2t2+ai1t+ai0)·(7ai7t6+6ai6t5+5ai5t4+4ai4t3+3ai3t2+2ai2t+ai1)2+Ai1cos(ai7t7+ai6t6+ai5t5+ai4t4+ai3t3+ai2t2+ai1t+ai0)·(42ai7t5+30ai6t4+20ai5t3+12ai4t2+6ai3t+2ai2)---(9)]]>步驟二，化簡(jiǎn)待定參數(shù)：利用初始、終端、過(guò)程狀態(tài)約束條件將未知參數(shù)用待定參數(shù)表示，以減少待定參數(shù)個(gè)數(shù)；根據(jù)如下約束條件化簡(jiǎn)待定參數(shù)：Θ(t0)=Θ0,Θ(tf)=ΘdΘ·(t0)=0,Θ·(tf)=0Θ·(t0)=0,Θ··(tf)=0θi_min≤θi(t)≤θi_max---(2)]]>其中，Θ＝[θ1θ2...θn]T，n為機(jī)械臂關(guān)節(jié)總數(shù)，1≤i≤n，且t0≤t≤tf。將(2)式定義的兩端與過(guò)程約束條件代入(1)、(8)、(9)式，可計(jì)算得到：Ai1=θi_max-θi_min2,Ai2=θi_max+θi_min2ai0=sin-1[(θi0-Ai2)/Ai1]ai1=ai2=0ai3=-3ai7tf7+ai6tf6-10(arcsinθid-Ai2Ai1-arcsinθi0-Ai2Ai1)tf3ai4=-8ai7tf7+3ai6tf6-15(arcsinθid-Ai2Ai1-arcsinθi0-Ai2Ai1)tf4ai5=-6ai7tf7+3ai6tf6-6(arcsinθid-Ai2Ai1-arcsinθi0-Ai2Ai1)tf5---(3)]]>由此，ai6、ai7為待定參數(shù)，其余未知參數(shù)均用待定參數(shù)表示，定義：A=a16a26...an6a17a27...an7---(4)]]>由(4)式可見(jiàn)，通過(guò)對(duì)A中待定參數(shù)組合的設(shè)定，可以調(diào)節(jié)機(jī)械臂關(guān)節(jié)空間從初始狀態(tài)Θ0運(yùn)動(dòng)到期望終端狀態(tài)Θd的路徑，進(jìn)而對(duì)本體姿態(tài)運(yùn)動(dòng)加以干預(yù)。步驟三，設(shè)計(jì)適應(yīng)度目標(biāo)函數(shù)：基于終端時(shí)刻本體控制精度和過(guò)程約束要求設(shè)計(jì)合適的適應(yīng)度目標(biāo)函數(shù)；適應(yīng)度目標(biāo)函數(shù)定義為：J=||δq||Kq+JΘ·KΘ·+JΘ··KΘ··+JΔqKΔq---(5)]]>δq為本體姿態(tài)四元數(shù)終值誤差，||·||為求范數(shù)運(yùn)算，Kq為根據(jù)本體姿態(tài)控制精度要求設(shè)定的閾值，只要||δq||＜Kq，即認(rèn)為結(jié)果滿足要求。表示關(guān)節(jié)角速度和角加速度超出其允許值的百分比，為相應(yīng)閾值。為機(jī)械臂運(yùn)動(dòng)過(guò)程中星本體姿態(tài)q(t)相對(duì)于初值q(t0)最大變化量超出其允許值Δqlimit的百分比，KΔq為相應(yīng)閾值。以上敘述中相應(yīng)參數(shù)的表達(dá)式如下：Jθ·i=0;θ·i_max≤θ·i_limitθ·i_max-θ·i_limitθ·i_limit;θ·i_max>θ·i_limitJθ··i=0;θ··i_max≤θ··i_limitθ··i_max-θ··i_limitθ··i_limit;θ··i_max>θ··i_limitJΔq=0;Δqmax≤ΔqlimitΔqmax-ΔqlimitΔqlimit;Δqmax>Δqlimit---(10)]]>步驟四，待定參數(shù)的遺傳算法尋優(yōu)：基于本體-臂耦合動(dòng)力學(xué)關(guān)系尋找使適應(yīng)度目標(biāo)函數(shù)最小的待定參數(shù)組合，據(jù)此確定機(jī)械臂完整運(yùn)動(dòng)路徑。首先，星本體-機(jī)械臂耦合動(dòng)力學(xué)與運(yùn)動(dòng)學(xué)關(guān)系表示為：ω0=Jba_ωΘ·---(6)]]>q·=12q0-q1-q2-q3q1q0-q3q2q2q3q0-q1q3-q2q1q00ω0---(7)]]>其中，Jba_ω為本體角速度-機(jī)械臂關(guān)節(jié)角速度雅可比矩陣，需根據(jù)和機(jī)械臂各關(guān)節(jié)的歷史構(gòu)型進(jìn)行實(shí)時(shí)計(jì)算：Jba_ω=-[Mr~0gr~0g+Σi=1n(Ii+mir~0iTr~0i)+I0]-1[Σi=1n(IiJRi+mir~0iJTi)-r~0gJtw]---(11)]]>其中，M—系統(tǒng)總質(zhì)量，r0g—本體質(zhì)心到系統(tǒng)質(zhì)心矢量，Ii—第i節(jié)臂的慣量矩陣(I0為本體慣量陣)，mi—第i節(jié)臂的質(zhì)量，r0i—本體質(zhì)心到第i節(jié)臂質(zhì)心矢量，JRi、JTi、Jtw是由各關(guān)節(jié)旋轉(zhuǎn)方向單位矢量、各節(jié)臂質(zhì)心位置矢量、各關(guān)節(jié)位置矢量的矩陣運(yùn)算組成。其次，適應(yīng)度目標(biāo)函數(shù)按如下流程進(jìn)行計(jì)算：1)對(duì)于一組確定的A值，代入(8)式計(jì)算各關(guān)節(jié)角速度，進(jìn)而確定以及(5)式中的2)根據(jù)(6)式計(jì)算Jba_ω，進(jìn)而計(jì)算本體的角速度ω0；3)根據(jù)(7)式計(jì)算本體四元數(shù)的時(shí)間導(dǎo)數(shù)4)計(jì)算本體四元數(shù)終值以及(5)式中JΔq；5)根據(jù)(5)式計(jì)算適應(yīng)度目標(biāo)函數(shù)。最后，采用遺傳算法對(duì)待定參數(shù)矩陣A尋優(yōu)的流程如下(算法參數(shù)：種群大小np＝40，復(fù)制概率pc＝0.8，交叉概率pm＝0.08，有效基因數(shù)ne＝4，進(jìn)化總代數(shù)Ng_max＝300)：1)隨機(jī)產(chǎn)生含np個(gè)個(gè)體的初始種群P0；2)計(jì)算每個(gè)個(gè)體的適應(yīng)度目標(biāo)函數(shù)：若存在某個(gè)體，其適應(yīng)度目標(biāo)函數(shù)(5)式每一項(xiàng)均小于1，則終止尋優(yōu)，確定該個(gè)體為待定參數(shù)矩陣A的最優(yōu)值，并根據(jù)此最優(yōu)值確定的(1)式θi(t)和(6)式中計(jì)算機(jī)械臂最優(yōu)運(yùn)動(dòng)路徑；否則轉(zhuǎn)3)；3)確定當(dāng)代種群的進(jìn)化代數(shù)Ng：若Ng＝Ng_max，將當(dāng)代種群中適應(yīng)度目標(biāo)函數(shù)最小的個(gè)體作為待定參數(shù)矩陣A的次優(yōu)估計(jì)值，并根據(jù)此次優(yōu)估計(jì)值確定的(1)式θi(t)和(6)式中計(jì)算機(jī)械臂次優(yōu)運(yùn)動(dòng)路徑；否則轉(zhuǎn)4)；4)進(jìn)行復(fù)制、交叉、變異操作生成后代種群，Ng＝Ng+1，并轉(zhuǎn)2)。綜上所述，本發(fā)明基于帶操作臂的自由漂浮航天器本體-機(jī)械臂耦合動(dòng)力學(xué)建模，運(yùn)用遺傳算法尋找最優(yōu)的機(jī)械臂的運(yùn)動(dòng)路徑，實(shí)現(xiàn)本體姿態(tài)及機(jī)械臂關(guān)節(jié)角同時(shí)達(dá)到期望狀態(tài)，節(jié)省衛(wèi)星本體姿態(tài)控制的能量消耗與任務(wù)執(zhí)行時(shí)間，同時(shí)避免傳統(tǒng)逆運(yùn)動(dòng)學(xué)關(guān)系求解過(guò)程中可能造成的動(dòng)力學(xué)奇異問(wèn)題，可運(yùn)用到以在軌服務(wù)為背景的目標(biāo)捕獲后機(jī)械臂與本體綜合控制任務(wù)。當(dāng)前第1頁(yè)1 2 3