用于在軌快速抓捕的姿軌臂一體化運(yùn)動(dòng)規(guī)劃方法
【專利摘要】本發(fā)明屬于航天器運(yùn)動(dòng)規(guī)劃領(lǐng)域，涉及一種用于在軌快速抓捕的姿軌臂一體化運(yùn)動(dòng)規(guī)劃方法，本方法具有抓捕操作用時(shí)少、抓捕操作安全性好、求解趨于全局最優(yōu)等優(yōu)點(diǎn)。該方法首次提出對(duì)空間機(jī)器人的飛越式逼近目標(biāo)過程與機(jī)械臂目標(biāo)抓捕過程進(jìn)行一體化規(guī)劃，實(shí)現(xiàn)了區(qū)別于傳統(tǒng)“停靠?抓捕”方式的飛躍式動(dòng)態(tài)抓捕。該方法首先提出了姿軌臂一體化運(yùn)動(dòng)規(guī)劃的模型，主要是確立了空間機(jī)器人飛越接近和機(jī)械臂抓捕過程的約束條件模型；其次，針對(duì)設(shè)計(jì)變量的離散化問題，提出了基于切比雪夫多項(xiàng)式的離散方法；最后，針對(duì)一般優(yōu)化算法的陷入局部最優(yōu)的問題，提出了適用于該規(guī)劃模型的混沌差分進(jìn)化算法。
【專利說明】
用于在軌快速抓捕的姿軌臂一體化運(yùn)動(dòng)規(guī)劃方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及航天器系統(tǒng)動(dòng)力學(xué)及控制技術(shù)領(lǐng)域，具體說涉及一種用于在軌快速抓 捕的姿軌臂一體化運(yùn)動(dòng)規(guī)劃方法。
【背景技術(shù)】
[0002] 空間機(jī)器人在軌抓捕通常的做法是，先交會(huì)接近到?？奎c(diǎn)然后再實(shí)施抓捕操作， 這種方式將在軌抓捕過程分為交會(huì)和抓捕兩個(gè)獨(dú)立的階段。對(duì)交會(huì)階段，目前比較成熟的 方式為沿V-軸或R-軸的準(zhǔn)直線受迫運(yùn)動(dòng)，經(jīng)實(shí)際應(yīng)用證明對(duì)受控穩(wěn)定目標(biāo)是適合的。然而， 對(duì)于更一般的在軌抓捕任務(wù)，目標(biāo)通常是非合作的，抓捕點(diǎn)可能在任意方向而非V-軸或R-軸方向。對(duì)此類目標(biāo)，直線交會(huì)策略難以滿足任務(wù)要求。對(duì)抓捕階段，目前大量的研究成果 大都基于初始相對(duì)靜止的假設(shè)，將抓捕規(guī)劃問題轉(zhuǎn)化為笛卡爾坐標(biāo)系下定點(diǎn)轉(zhuǎn)移的機(jī)械臂 運(yùn)動(dòng)規(guī)劃，而沒有考慮其軌道動(dòng)力學(xué)背景。況且，這種先停靠再抓捕的方式實(shí)施周期較長(zhǎng)， 不能滿足某些快速在軌服務(wù)任務(wù)的需求。
[0003] 文獻(xiàn)（S.Matsumoto，S.Dubowsky，S.Jacobsen，Y.Ohkami .Fly-By Approach and Guidance for Uncontrolled Rotating Satellite Capture[C]·AIAA Guidance, Navigation，and Control Conference and Exhibit.Austin，USA，2003:1-10.)提出一種 服從軌道動(dòng)力學(xué)的無(wú)碰撞接近策略，利用軌道相對(duì)運(yùn)動(dòng)規(guī)律，使空間機(jī)器人以自由漂浮模 式飛越接近目標(biāo)，若捕獲失敗又能夠沿安全路徑逃離目標(biāo)?；谠摻咏呗匀羰箍臻g機(jī)器 人在相對(duì)運(yùn)動(dòng)的同時(shí)開展機(jī)械臂抓捕操作，不僅能夠適用于非合作目標(biāo)抓捕，而且可以實(shí) 現(xiàn)更快速的在軌抓捕。但文獻(xiàn)中僅研究了空間機(jī)器人與目標(biāo)質(zhì)心交會(huì)接近問題，沒有考慮 機(jī)械臂抓捕運(yùn)動(dòng)規(guī)劃問題。對(duì)于飛越式逼近方式，抓捕時(shí)刻空間機(jī)器人相對(duì)目標(biāo)體存在相 對(duì)運(yùn)動(dòng)，不滿足相對(duì)靜止的條件，并且一旦進(jìn)入飛越段的初始條件確定，空間機(jī)器人的逼近 軌跡即嚴(yán)格確定，而空間機(jī)器人與目標(biāo)的最近距離、交會(huì)速度以及可抓捕時(shí)間對(duì)機(jī)械臂能 否成功抓捕目標(biāo)有決定性影響。

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

[0004] 為解決空間機(jī)器人服從軌道動(dòng)力學(xué)飛越式接近空間目標(biāo)過程中的姿臂規(guī)劃問題， 本發(fā)明綜合考慮接近運(yùn)動(dòng)與機(jī)械臂抓捕操作，提供一種用于在軌快速抓捕的姿軌臂一體化 運(yùn)動(dòng)規(guī)劃方法。
[0005] 本發(fā)明的技術(shù)方案是：
[0006] 本發(fā)明中空間目標(biāo)主要適用于重力梯度穩(wěn)定衛(wèi)星和慢速自旋周期約為20s~360s 的自旋衛(wèi)星。
[0007] 空間機(jī)器人的快速逼近策略如下：空間機(jī)器人相對(duì)目標(biāo)在最終逼近階段的運(yùn)動(dòng)過 程，采取飛越式逼近的快速逼近方式，即空間機(jī)器人從相距目標(biāo)幾百米的位置開始轉(zhuǎn)移機(jī) 動(dòng)，沿服從軌道動(dòng)力學(xué)的無(wú)碰撞路徑不施加軌道控制而以合適的速度接近目標(biāo)位置，至捕 獲區(qū)域內(nèi)實(shí)施機(jī)械臂抓捕操作。若接近過程中出現(xiàn)意外故障，則可以沿著一條安全軌跡逃 離目標(biāo)而不施加有源控制。飛越式逼近過程包括軌跡調(diào)整段、飛越段、捕獲段和逃離段。其 中軌跡調(diào)整段是指空間機(jī)器人由初始位置開始經(jīng)機(jī)動(dòng)到達(dá)設(shè)定的位置和速度條件，至軌道 控制系統(tǒng)關(guān)機(jī)為止的飛行段;飛越段指空間機(jī)器人軌道控制系統(tǒng)關(guān)機(jī)至目標(biāo)進(jìn)入機(jī)械臂工 作范圍之前的飛行段;捕獲段指目標(biāo)進(jìn)入機(jī)械臂的工作范圍后空間機(jī)器人的飛行段;逃離 段是指空間機(jī)器人在捕獲段的抓捕操作未成功或飛行中出現(xiàn)意外故障而轉(zhuǎn)入的飛行段。
[0008] 空間機(jī)器人的快速抓捕策略如下：空間機(jī)器人完成軌跡調(diào)整后，相距目標(biāo)的位置 已相對(duì)較近，此時(shí)需避免使用噴氣裝置，以免對(duì)目標(biāo)造成羽流污染。同時(shí)，為避免反作用飛 輪等姿態(tài)控制系統(tǒng)的啟動(dòng)對(duì)機(jī)械臂操作造成擾動(dòng)及帶來碰撞風(fēng)險(xiǎn)，空間機(jī)器人將關(guān)閉星載 姿態(tài)控制系統(tǒng)，飛越式逼近軌跡進(jìn)入飛躍段與捕獲段。成功抓捕前自由漂浮空間機(jī)器人相 對(duì)目標(biāo)處于相對(duì)運(yùn)動(dòng)中，機(jī)械臂的操作也在相對(duì)運(yùn)動(dòng)過程中完成。抓捕過程主要包括四個(gè) 步驟，分別為:初始化、抓捕、穩(wěn)定(鎖定）、收攏撤離。
[0009] 按照上述設(shè)計(jì)的策略，本發(fā)明一種用于在軌快速抓捕的姿軌臂一體化運(yùn)動(dòng)規(guī)劃方 法，包括以下步驟：
[0010] SI，確定規(guī)劃設(shè)計(jì)變量：
[0011]
(1)
[0012 ]其中表示飛越段初始時(shí)刻，空間機(jī)器人在把11坐標(biāo)系中相對(duì)軌道運(yùn)動(dòng)的位 置、速度、姿態(tài)以及角速度矢量，、ΗΩ〖分別表示機(jī)械臂末端的位置和姿態(tài)；
[0013] S2,確定運(yùn)動(dòng)規(guī)劃約束條件
[0014] 運(yùn)動(dòng)規(guī)劃約束條件包括飛越式逼近軌跡約束條件和快速抓捕約束條件，其中飛越 式逼近軌跡約束條件包括安全操作距離約束、凸曲線約束和相對(duì)速度約束；
[0015] (1)安今操作距離約東
[0016]
(2)
[0017] 其中瓦為空間機(jī)器人質(zhì)心相對(duì)于目標(biāo)抓捕點(diǎn)的最小安全距離，Rw,max表示空間 機(jī)器人操作空間的最大范圍，d cp(t)為空間機(jī)器人質(zhì)心相對(duì)目標(biāo)抓捕點(diǎn)的距離;tr為捕獲段 的起始時(shí)刻;Uf為接觸點(diǎn)時(shí)刻。
[0018] 定義罰函數(shù)
[0019]
[0020]
[0021] (3)
[0022] 其中，Ηωτ為目標(biāo)抓捕點(diǎn)自旋角速度向量，Hxcp是指相對(duì)運(yùn)動(dòng)位置向量； Hkq,為相對(duì) 運(yùn)動(dòng)加速度向量；
[0023] (3)相對(duì)速度約束
[0024]
(4)
[0025] 其中Hie，S九分別為目標(biāo)抓捕點(diǎn)的速度分量/% 1分別機(jī)械臂末端的速度分量 νε為安全操作最大相對(duì)速度；
[0026] 所述的快速抓捕軌跡約束條件包括抓捕操作時(shí)間約束、機(jī)械臂末端位置約束、機(jī) 械臂末端姿態(tài)約束、機(jī)械臂物理約束、工作空間約束和接觸擾動(dòng)最小約束；
[0027] (1)抓捕操作時(shí)間約束
[0028]
[0029]其中tmf為接觸點(diǎn)時(shí)刻，txmin為軌道相對(duì)運(yùn)動(dòng)速度最小或距離最近時(shí)刻，t P表示逃 離段初始時(shí)刻；
[0030] (2)機(jī)械臂末端位置約束
[0031]
(6)
[0032]其中％^〇表示初始時(shí)刻機(jī)械臂末端與目標(biāo)點(diǎn)的距離，％(^)、％(以)分別表示 抓捕操作起始與終止時(shí)刻機(jī)械臂末端相對(duì)空間機(jī)器人質(zhì)心的位置，Uo表示抓捕操作起始時(shí) 刻，tmf表示抓捕操作終止時(shí)刻;HPr, Q和HPr, f分別表示抓捕操作起始與終止時(shí)刻的空間機(jī)器 人質(zhì)心與目標(biāo)衛(wèi)星質(zhì)心的相對(duì)位置;hRg表示抓捕點(diǎn)相對(duì)目標(biāo)衛(wèi)星質(zhì)心的位置矢量。
[0033] (3)機(jī)械臂末端姿態(tài)約束
[0034]
(7)
[0035] 其中η Ω E,Q和η Ω ^分別為抓捕操作起始和終止時(shí)刻的機(jī)械臂末端姿態(tài)；
[0036] (4)機(jī)械臂物理約束
[0037]
(8)
[0038] 其中qm,max和分別表示關(guān)節(jié)角和角速率允許的最大值，qmi表示機(jī)械臂第i個(gè)關(guān) 節(jié)的關(guān)節(jié)角;進(jìn)一步地，定義罰函數(shù)如下：
[0041 ] (5)工作空間約束，用罰函數(shù)的形式表示為：
[0039]
[0040]
[0042]
(9)
[0043]其中ε為小于1的正數(shù)，；ε。為大于100的正數(shù)，；QJg(qm)表示空間機(jī)器人的廣義雅克 比矩陣，I · I表示某矩陣的行列式，qm表示機(jī)械臂關(guān)節(jié)角向量;Re表示機(jī)械臂末端位置;PIW 和row分別表示為自由漂浮空間機(jī)器人工作空間中的路徑無(wú)關(guān)工作空間和路徑相關(guān)工作空 間，可參見文南犬（Papadopoulos E1Dubowsky S.Dynamic Singularity in Free-Floating Space Robots[J].JDyn Syst Meas Contr,1993,115(1):44-52.)
[0044] (6)接觸擾動(dòng)最小約束
[0045]
(10)
[0046] 其中，表示接觸前空間機(jī)器人的速度，為使得接觸引起的基座角速度變化 為零的機(jī)械臂末端相對(duì)抓捕點(diǎn)的速度，為接觸前抓捕點(diǎn)的速度；
[0047] S3，確定運(yùn)動(dòng)規(guī)劃目標(biāo)函數(shù)
[0048] 結(jié)合約束條件中罰函數(shù)指標(biāo)，令一體化運(yùn)動(dòng)規(guī)劃的目標(biāo)函數(shù)設(shè)定為
[0049] min J = Jc^Jctrs (11)
[0050] 其中Jctrs = Jt^Jq+Jv+Js為相應(yīng)的罰函數(shù)指標(biāo);如表示基座擾動(dòng)指標(biāo)(基座擾動(dòng)指標(biāo) 是做空間機(jī)器人運(yùn)動(dòng)規(guī)劃的基本指標(biāo)，目的在于使空間機(jī)器人機(jī)械臂運(yùn)動(dòng)時(shí)對(duì)基座擾動(dòng)最 小，從而保持基座對(duì)地定向等，從而使機(jī)械臂運(yùn)動(dòng)不影響對(duì)地通信等任務(wù)的進(jìn)行。），表示如 下：
[0051 ] (12)
[0052]這里％是指空間機(jī)器人基座的角速度矢量。
[0053] S4,機(jī)械臂末端軌跡參數(shù)化
[0054] 對(duì)待規(guī)劃時(shí)間段
A ;機(jī)械臂末端位置變量可以
表示為：
[0055]
[0056] 機(jī)械臂末端姿態(tài)由規(guī)劃初始時(shí)刻Uo的機(jī)械臂末端姿態(tài)η ΩΕ,〇繞單位向量κ旋轉(zhuǎn)Φ? 角后調(diào)整到HQE,f，0彡Φ彡180°，因此機(jī)械臂末端姿態(tài)變量可以表示為
[0057] ηΩε=[κ Φ(ξ)],ξε[0,1] (14)
[0058] 其中由ΗΩΕ,〇和ηΩε,?可以計(jì)算出1〇與(1^，使?jié)M足Φ(〇)=〇，Φ(1) = Φ?;
[0059] 假設(shè)機(jī)械臂的期望運(yùn)動(dòng)軌跡連續(xù)可微，則其末端的速度軌跡可以表示為
[0060]
(15)
數(shù)量選取，N>等式約束條件的數(shù)量;Pk(t)表示關(guān)于時(shí)間變量t的切比雪夫多項(xiàng)式;bik表示Pk (t)的權(quán)系數(shù)。
[0066] 對(duì)式(17)式求導(dǎo)，得到參數(shù)化形式的速度與加速度變量：
[0067]
(18)
[0068] 對(duì)應(yīng)時(shí)間條件t = tmQ和t = tmf分別取ξ = 0和ξ = 1，根據(jù)等式約束式（6)、（7)和 (10)，對(duì)每一個(gè)i，可以得到5個(gè)關(guān)于參數(shù)bik的方程Wj(bik) =0，j = l，2,…5,由此可知，參數(shù) bi0~biN中只有N-5是獨(dú)立變量。不妨假設(shè)b i6~biN為獨(dú)立變量，機(jī)械臂運(yùn)動(dòng)規(guī)劃問題轉(zhuǎn)變?yōu)?優(yōu)化配置bl6~b lN使相應(yīng)的目標(biāo)函數(shù)最小；
[0069] S5，混沌差分進(jìn)化求解 [0070] S5.1:初始化參數(shù)
[0071]設(shè)定種群規(guī)模Np，個(gè)體維數(shù)dim，最大進(jìn)化代數(shù)Itermax，終止條誤差件ε，混沌搜索 次數(shù)kc，調(diào)節(jié)因子Fs，交叉因子Cr;
[0072] S5.2:種群初始化
[0073]每個(gè)種群個(gè)體包含以下變量：即飛越段初始時(shí)刻，空間機(jī)器人在Hill坐標(biāo)系中相 對(duì)軌道運(yùn)動(dòng)的位置、速度、姿態(tài)以及角速度矢量Η3??，以及機(jī)械臂末端的位置11:!^的軌跡和 姿態(tài)的軌跡。其中的維數(shù)為4Χ3 = 12，ηΚ〗>Ρη?【的各個(gè)維度可以由式（17)離散為 nr個(gè)變量，又因?yàn)榇嬖?個(gè)等式約束條件，所以共η= 12+3 X (nr-5)個(gè)獨(dú)立參數(shù)，因此隨機(jī)產(chǎn) 生Np個(gè)參數(shù)向量
，分別表示該設(shè)計(jì)變量的各個(gè)獨(dú)立參數(shù)， 構(gòu)成的初始種群P〇P(0);
[0074] S5.3:計(jì)算個(gè)體適應(yīng)度
[0075] 首先將種群參數(shù)代入相對(duì)軌道運(yùn)動(dòng)方程(相對(duì)軌道運(yùn)動(dòng)方程是本領(lǐng)域的一個(gè)通用 方程式），其次在相對(duì)運(yùn)動(dòng)基礎(chǔ)上由式（17)、（18)得到機(jī)械臂末端標(biāo)準(zhǔn)軌跡，利用廣義雅克 比矩陣計(jì)算空間機(jī)器人的基座速度和位姿變化(空間機(jī)器人由基座和機(jī)械臂組成），代入目 標(biāo)函數(shù)式(11)得到個(gè)體適應(yīng)度指標(biāo)；
[0076] S5.4:精英選擇
[0077]對(duì)種群適應(yīng)度進(jìn)行評(píng)價(jià)，找出適應(yīng)度最佳個(gè)體Bbest(即式11計(jì)算得到的值最小的 個(gè)體），記錄最佳適應(yīng)度值及個(gè)體在種群中的索引位置;Bbest作為當(dāng)代種群的精英；
[0078] S5.5 :混沌局部搜索
[0079]以Bbest作為混沌搜索的初值，在Bbest附近的區(qū)域進(jìn)行kc次混沌搜索：
[0080] Bk = Bbest+Fs · Pk (19)
[00811 其中&為L(zhǎng)ogistic混沌方程的迭代變量，F(xiàn)s為調(diào)節(jié)因子，可使Bk向著正反兩個(gè)方向 變化;從得到的個(gè)體Bk中找出適應(yīng)度最佳的B ' be3St，若B ' bsd尤于Bbe3st，則將B ' be3St隨機(jī)取代種 群中個(gè)體后進(jìn)入下一步驟;若Bbe3st優(yōu)于B ' be3St，則直接進(jìn)入下一步驟；
[0082] S5.6:判斷終止條件
[0083] 判斷是否滿足終止準(zhǔn)則（如最大進(jìn)化代數(shù)或最佳適應(yīng)度均值的更新誤差），若滿足 則終止計(jì)算，輸出最佳個(gè)體仏^*;若不滿足則進(jìn)入S5.7，且Iter = Iter+Ι;
[0084] S5.7:進(jìn)化操作
[0085]按照DE/local-to-best/Ι進(jìn)化模式的規(guī)則進(jìn)行相應(yīng)的選擇和進(jìn)化操作，生成新一 代種群pop(Iter)，返回S5.3;
[0086]重復(fù)S5.3至S5.7的過程直至滿足終止條件，輸出最佳個(gè)體Bgbest及其適應(yīng)度的值。 [0087]本發(fā)明的有益技術(shù)效果是：
[0088] 本方法具有抓捕操作用時(shí)少、抓捕操作安全性好、求解趨于全局最優(yōu)等優(yōu)點(diǎn)。該方 法首次提出對(duì)空間機(jī)器人的飛越式逼近目標(biāo)過程與機(jī)械臂目標(biāo)抓捕過程進(jìn)行一體化規(guī)劃， 實(shí)現(xiàn)了區(qū)別于傳統(tǒng)"?？?抓捕"方式的飛躍式動(dòng)態(tài)抓捕。該方法首先提出了姿軌臂一體化 運(yùn)動(dòng)規(guī)劃的模型，主要是確立了空間機(jī)器人飛越接近和機(jī)械臂抓捕過程的約束條件模型； 其次，針對(duì)設(shè)計(jì)變量的離散化問題，提出了基于切比雪夫多項(xiàng)式的離散方法;最后，針對(duì)一 般優(yōu)化算法的陷入局部最優(yōu)的問題，提出了適用于該規(guī)劃模型的混沌差分進(jìn)化算法。
【附圖說明】
[0089] 圖1是空間機(jī)器人飛越式逼近目標(biāo)過程示意圖。
[0090] 圖2是抓捕操作流程示意圖。
[0091 ]圖3是混沌差分算法流程圖。
[0092] 圖4是空間機(jī)器人軌道平面內(nèi)的逼近路徑及機(jī)械臂末端抓捕軌跡。
[0093] 圖5是參考慣性系內(nèi)空間機(jī)器人的運(yùn)動(dòng)過程。
[0094]圖6是抓捕過程中基座姿態(tài)軌跡。
[0095] 圖7是抓捕過程中機(jī)械臂末端速度軌跡。
[0096] 圖8是抓捕過程中機(jī)械臂關(guān)節(jié)軌跡。
[0097]圖9是機(jī)械臂末端姿態(tài)變化軌跡。
【具體實(shí)施方式】
[0098] 為了使本發(fā)明所解決的技術(shù)問題、技術(shù)方案及有益效果更加清楚明白，以下結(jié)合 附圖及實(shí)施例，對(duì)本發(fā)明進(jìn)行進(jìn)一步詳細(xì)說明。應(yīng)當(dāng)理解，此處所描述的具體實(shí)施例僅用以 解釋本發(fā)明，并不用于限定本發(fā)明。
[0099] 圖1描述了空間機(jī)器人的快速逼近策略。空間機(jī)器人相對(duì)目標(biāo)在最終逼近階段的 運(yùn)動(dòng)過程，采取飛越式逼近的快速逼近方式，即空間機(jī)器人從相距目標(biāo)幾百米的位置開始 轉(zhuǎn)移機(jī)動(dòng)，沿服從軌道動(dòng)力學(xué)的無(wú)碰撞路徑不施加軌道控制而以合適的速度接近目標(biāo)位 置，至捕獲區(qū)域內(nèi)實(shí)施機(jī)械臂抓捕操作。若接近過程中出現(xiàn)意外故障，則可以沿著一條安全 軌跡逃離目標(biāo)而不施加有源控制。飛越式逼近過程包括軌跡調(diào)整段So-S 1、飛越段S:\S2、 捕獲段S2~S3和逃離段S3~S4。
[0100]圖2描述了空間機(jī)器人的快速抓捕策略?？臻g機(jī)器人完成軌跡調(diào)整后，相距目標(biāo)的 位置已相對(duì)較近，此時(shí)需避免使用噴氣裝置，以免對(duì)目標(biāo)造成羽流污染。同時(shí)，為避免反作 用飛輪等姿態(tài)控制系統(tǒng)的啟動(dòng)對(duì)機(jī)械臂操作造成擾動(dòng)及帶來碰撞風(fēng)險(xiǎn)，空間機(jī)器人將關(guān)閉 星載姿態(tài)控制系統(tǒng)，飛越式逼近軌跡進(jìn)入飛躍段與捕獲段。成功抓捕前自由漂浮空間機(jī)器 人相對(duì)目標(biāo)處于相對(duì)運(yùn)動(dòng)中，機(jī)械臂的操作也在相對(duì)運(yùn)動(dòng)過程中完成。抓捕過程主要包括 四個(gè)步驟，分別為:初始化、抓捕、穩(wěn)定(鎖定）、收攏撤離。
[0101] 按照上述設(shè)計(jì)的策略，本發(fā)明提供一種用于在軌快速抓捕的姿軌臂一體化運(yùn)動(dòng)規(guī) 劃方法，包括以下步驟：
[0102] 第一步，確定設(shè)計(jì)變量。選擇規(guī)劃設(shè)計(jì)變量：
[0103]
(1)
[0104] 其中表示表示飛越段初始時(shí)刻，空間機(jī)器人在Hi 11坐標(biāo)系中相對(duì)軌道運(yùn) 動(dòng)的位置和速度矢量、ΗΩ〗分別表示機(jī)械臂末端的位置和姿態(tài)?？臻g機(jī)器人逼近與 抓捕過程的運(yùn)動(dòng)規(guī)劃問題可以表述為:求解te[tf，tP]時(shí)間內(nèi)的逼近運(yùn)動(dòng)軌跡HXf(t)和te [tm0，W]時(shí)間內(nèi)的機(jī)械臂抓捕運(yùn)動(dòng)軌跡[HRE(t)T， HQE(t)T]T，使?jié)M足逼近與抓捕過程的位形 約束、物理約束和安全約束等約束條件。
[0105] 第二步，確定運(yùn)動(dòng)規(guī)劃約束條件。運(yùn)動(dòng)規(guī)劃的約束條件包括飛越式逼近軌跡約束 條件和快速抓捕約束條件。其中所述的飛越式逼近軌跡約束條件包括安全操作距離約束、 凸曲線約束、相對(duì)速度約束等。
[0106] 所述的安全操作距離約束表述如下：機(jī)械臂的工作空間為Ω R ={ R I Rw, min彡R彡 Rw,_}，設(shè)空間機(jī)器人質(zhì)心相對(duì)目標(biāo)抓捕點(diǎn)的距離為心(〇。考慮捕獲段的定義，當(dāng)te[tr， tP]時(shí)，存在約束條件：
[0107] dcP(t)<Rw,max (2)
[0108] 同時(shí)，考慮機(jī)械臂操作安全性，應(yīng)在空間機(jī)器人與目標(biāo)抓捕點(diǎn)之間留有一定的安 全距離以調(diào)整操作偏差，設(shè)空間機(jī)器人質(zhì)心相對(duì)于目標(biāo)抓捕點(diǎn)的最小安全距離為
(3)
[0112] 凸曲線約束可表示為:為保證空間機(jī)器人逼近軌跡為凸曲線，有如下不等式成立：
[0113]
(4)
[0114] 其中，Η ω T為目標(biāo)抓捕點(diǎn)自旋角速度向量，hXcp是指相對(duì)運(yùn)動(dòng)位置向量，aX ljp,為相對(duì) 運(yùn)動(dòng)加速度向量；
[0115] 相對(duì)速度約束可表示為：定義ZH方向?yàn)槟繕?biāo)星軌道平面的法線方向，令空間機(jī)器 人的質(zhì)心在ZH方向無(wú)相對(duì)運(yùn)動(dòng)，因此僅考慮空間機(jī)器人質(zhì)心相對(duì)目標(biāo)星的速度矢量在目標(biāo) 衛(wèi)星軌道平面內(nèi)的速度分量η九?？臻g機(jī)器人軌道相對(duì)運(yùn)動(dòng)速度表示為：
[0116]
(.5:)
[0117] 其中，Xcf表示飛躍段初始時(shí)刻空間機(jī)器人質(zhì)心相對(duì)目標(biāo)衛(wèi)星的位置矢量在目標(biāo)衛(wèi) 星軌道平面上的某個(gè)分量，該分量的方向由地心指向目標(biāo)衛(wèi)星。.~、_ iV.分別表示飛躍段初 始時(shí)刻空間機(jī)器人質(zhì)心相對(duì)于目標(biāo)衛(wèi)星的速度矢量在目標(biāo)衛(wèi)星軌道平面內(nèi)的速度分量。
[0118] 為保證抓捕操作安全性，在抓捕時(shí)間內(nèi)空間機(jī)器人相對(duì)抓捕點(diǎn)的速度應(yīng)不超過安 全抓捕所允許的最大速度V6:
[0119]
(6)
[0120] 其中％，11I分別為目標(biāo)抓捕點(diǎn)的速度分量/W分別空間機(jī)器人質(zhì)心相對(duì)于 目標(biāo)衛(wèi)星的速度在軌道平面上分量，ν ε為安全操作最大相對(duì)速度。
[0121] 所述的快速抓捕軌跡約束條件包括抓捕操作時(shí)間約束、機(jī)械臂末端位置約束、機(jī) 械臂末端姿態(tài)約束、機(jī)械臂物理約束、工作空間約束、接觸擾動(dòng)最小約束等。
[0122] 抓捕操作時(shí)間約束可表示為：在軌抓捕的操作時(shí)間受軌道相對(duì)運(yùn)動(dòng)動(dòng)力學(xué)的約 束，與逼近軌跡緊密相關(guān)。受機(jī)械臂工作范圍的限制，抓捕操作必須在捕獲段內(nèi)完成。4表 示逃離段初始時(shí)刻，則成功抓捕必須滿足：
[0123] W^tp (7)
[0124] 接觸時(shí)刻的選擇還需要考慮目標(biāo)自旋周期等約束，若選擇軌道相對(duì)運(yùn)動(dòng)速度最小 或距離最近時(shí)刻txmin，則須滿足
[0125] tmf = txmin (8)
[0126] 機(jī)械臂末端位置約束可表示為：假設(shè)機(jī)械臂末端相對(duì)目標(biāo)抓捕點(diǎn)的初始距離為 hRce,〇,機(jī)械臂末端的位置約束條件：
[0127]
(9)
[0128] 其中HRGE,o表示初始時(shí)刻機(jī)械臂末端與目標(biāo)點(diǎn)的距離，HRE(t mQ)、HRE(tmf)分別表示 抓捕操作起始與終止時(shí)刻機(jī)械臂末端相對(duì)空間機(jī)器人質(zhì)心的位置，Uo表示抓捕操作起始時(shí) 刻，tmf表示抓捕操作終止時(shí)刻; HPr, Q和HPr, f分別表示抓捕操作起始與終止時(shí)刻的空間機(jī)器 人質(zhì)心與目標(biāo)衛(wèi)星質(zhì)心的相對(duì)位置;hRg表示抓捕點(diǎn)相對(duì)目標(biāo)衛(wèi)星質(zhì)心的位置矢量。
[0129] 所述的機(jī)械臂末端姿態(tài)約束可表示為：
[0130](10.)
[0131] 其中ηΩΕ,0和
ηΩΕ,?分別為抓捕操作起始和終止時(shí)刻的機(jī)械臂末端姿態(tài)。
[0132] 所述的機(jī)械臂物理約束可表示為：
[0133]
(11)
[0134] 其中qm,max和分別表示關(guān)節(jié)角和角速率允許的最大值。qmi表示機(jī)械臂第i個(gè)
[0135] 關(guān)節(jié)的關(guān)節(jié)角；同時(shí)定義罰函數(shù)如下：
[0136]
[0137] 工作空間約束可表示為：規(guī)劃設(shè)計(jì)變量為機(jī)械臂末端在笛卡爾空間中的位置，因 此設(shè)計(jì)變量須滿足空間機(jī)器人工作空間的限制。機(jī)械臂末端在相對(duì)參考慣性系的可達(dá)空間 范圍內(nèi)運(yùn)動(dòng)，則得到：
[0138] Rw,min<|RE(t)|$R w,max (12)
[0139] 進(jìn)一步，若機(jī)械臂末端進(jìn)入PDW空間，則運(yùn)動(dòng)中會(huì)出現(xiàn)動(dòng)力學(xué)奇異點(diǎn)，將給控制系 統(tǒng)設(shè)計(jì)增加困難。應(yīng)盡量使機(jī)械臂末端在PIW范圍內(nèi)運(yùn)動(dòng)。但這一約束縮小了機(jī)械臂的工作 范圍，對(duì)某些工況可能無(wú)法滿足任務(wù)要求。因此，考慮在PIW內(nèi)無(wú)最優(yōu)解時(shí)允許機(jī)械臂末端 進(jìn)入PDW內(nèi)。定義懲罰因子J s:
[0140]
(13)
[0141] 其中ε為很小的正數(shù)，ε。為比較大的正數(shù);qjqm)表示空間機(jī)器人的廣義雅克比矩 陣，I · I表示某矩陣的行列式，qm表示機(jī)械臂關(guān)節(jié)角向量;Re表示機(jī)械臂末端位置;PIW和PDW 分別表示為自由漂浮空間機(jī)器人工作空間中的路徑無(wú)關(guān)工作空間和路徑相關(guān)工作空間。
[0142] 接觸擾動(dòng)最小約束可表示為：設(shè)'=(^了表示目標(biāo)抓捕點(diǎn)的廣義速度向 量，VC表示抓捕點(diǎn)的線速度，Wc表示抓捕點(diǎn)的角速度，有：
[0143]
(14)
[0144] 其中，七為目標(biāo)衛(wèi)星的廣義速度矢量為抓捕點(diǎn)的廣義速度矢量，As表示抓捕點(diǎn) 到目標(biāo)質(zhì)心的定常轉(zhuǎn)移矩陣。同理，忽略目標(biāo)衛(wèi)星姿態(tài)運(yùn)動(dòng)的非線性項(xiàng)，將目標(biāo)衛(wèi)星的動(dòng)力 學(xué)方程在A t~>0時(shí)間內(nèi)積分得到：
[0145]
( 15)
[0146] 其中Hs表示目標(biāo)衛(wèi)星的廣義質(zhì)量矩陣，瓦表示機(jī)械臂末端與抓捕點(diǎn)接觸時(shí)的相互 作用力；
[0147] 機(jī)械臂末端與目標(biāo)抓捕點(diǎn)接觸后目標(biāo)抓捕點(diǎn)的速度增量：
[0148]
(16)
[0149] 假設(shè)下標(biāo)"α"表示接觸前的物理量，下標(biāo)"β"表示接觸后的物理量，將％丨= 代 入(16)式，整理得到：
[0150]
(17.)
[0151] 由空間機(jī)器人運(yùn)動(dòng)學(xué)關(guān)系，得：
[0152]
( 18 )
[0153] 將（18)代入(17)中，整理得到：
[0154]
[0155] 良示機(jī)械臂末端與目 標(biāo)抓捕點(diǎn)的相對(duì)速度。
[0156] 由（19)可得，
[0157]
(20)
[0158] 式(20)表明，接觸引起的基座角速度變化與接觸前機(jī)械臂末端相對(duì)目標(biāo)抓捕點(diǎn)的 速度有關(guān)。因此，若使基座角度擾動(dòng)盡量小，可以通過優(yōu)化使得相對(duì)速度|* UE I最小來實(shí)現(xiàn)。
[0159] 假設(shè)接觸碰撞對(duì)基座角速度沒有擾動(dòng)，即Δ ω〇 = 〇,則有兩種可行的實(shí)現(xiàn)方式，而 "直臂抓捕"方式較難實(shí)現(xiàn)，因此采取第⑵種方式，即#0。將Δ Coq = O代入式(20)中可 以解得：
[0160] (21)
[0161 ] ·)+表示矩陣的Moore-Penrose偽逆，\e叱為任意速度向 量。顯然，為式(20)的零解，由式(21)確定的相對(duì)速度可以使接觸對(duì)基座角速度的擾 動(dòng)嚴(yán)格為零。因接觸前速度心^·是定值，可以得到接觸擾動(dòng)最小的速度約束條件：
[0162]
(22)
[0163] 其中，表示接觸前空間機(jī)器人的速度，為使得接觸引起的基座角速度變化 為零的機(jī)械臂末端相對(duì)目標(biāo)抓捕點(diǎn)的速度，為接觸前抓捕點(diǎn)的速度。
[0164] 第三步，確定運(yùn)動(dòng)規(guī)劃目標(biāo)函數(shù)。
[0165] 飛越式抓捕過程對(duì)基座姿態(tài)產(chǎn)生擾動(dòng)最小是規(guī)劃期望的最終目標(biāo)，基座受到的擾 動(dòng)因素主要有兩項(xiàng)。一項(xiàng)是由機(jī)械臂運(yùn)動(dòng)產(chǎn)生的耦合擾動(dòng)，由于空間機(jī)器人系統(tǒng)的非完整 性，基座姿態(tài)與關(guān)節(jié)變量沒有位置級(jí)約束關(guān)系，而僅存在速度級(jí)約束，因此，基座擾動(dòng)指標(biāo) 表示為
[0166] (23)
[0167] 其中仏是指空間機(jī)器人基座的角速度矢量。
[0168] 另一項(xiàng)則為機(jī)械臂末端接觸目標(biāo)產(chǎn)生的擾動(dòng)影響，接觸擾動(dòng)最小問題可轉(zhuǎn)化為對(duì) 終端時(shí)刻的機(jī)械臂速度約束。因此結(jié)合約束條件中罰函數(shù)指標(biāo)，令一體化運(yùn)動(dòng)規(guī)劃的目標(biāo) 函數(shù)設(shè)定力
[0169]
[0170]其中Jctrs = Jd+Jq+Jv+Js為相應(yīng)的罰函數(shù)指標(biāo)。
[0171] 第四步，機(jī)械臂末端軌跡參數(shù)化。
[0172] 對(duì)待規(guī)劃時(shí)間厓?chǔ)桅臶0，1]。機(jī)械臂末端位置變量可 以表示為：
[0173] hRe = R(C),ξε[0,1] (25)
[0174] 機(jī)械臂末端姿態(tài)由規(guī)劃初始時(shí)刻Uo的機(jī)械臂末端姿態(tài)η ΩΕ,Q繞單位向量κ旋轉(zhuǎn)Φ? 角后調(diào)整到Η Ω E,f，因此機(jī)械臂末端姿態(tài)變量可以表示為
[0175] _ _
_ _
[0176] 其中由ΗΩΕ,。和HQE, f可以計(jì)算出κ與Φ?，使?jié)M足Φ(〇)=〇，Φ(1) = Φ?。
[0177] 假設(shè)機(jī)械臂的期望運(yùn)動(dòng)軌跡連續(xù)可微，則其末端的速度軌跡可以表示為
[0178]
(27)
[0179] 由式(25)-(27)，機(jī)械臂末端的位置與姿態(tài)軌跡最終由RU)和Φ(ξ)描述，因此對(duì) 機(jī)械臂運(yùn)動(dòng)規(guī)劃問題，設(shè)計(jì)變量可以統(tǒng)一表示為
[0180] Χ?(ξ) = [Rt(C) Φτ(ξ)]τ,? = 1,2,···,ξε[0,1] (28)
[0181 ] 在Hill坐標(biāo)系中對(duì)Xi進(jìn)行參數(shù)化表示：
[0182]
:(2:9:)
[0183] 其中幻表示N階多項(xiàng)式形式的Xi(I)，N根據(jù)等式約束條件的數(shù)量選取，N>等式 約束條件的數(shù)量;Pk(t)表示關(guān)于時(shí)間變量t的切比雪夫多項(xiàng)式;b lk表示Pk(t)的權(quán)系數(shù)。
[0184] 對(duì)式(29)式求導(dǎo)，得到參數(shù)化形式的速度與加速度變量：
[0185]
、30)
[0186] 對(duì)應(yīng)時(shí)間條件t = tm〇和t = tmf分別取ξ = 0和ξ = 1，根據(jù)等式約束式（9)、（ 10)和 (22)，對(duì)每一個(gè)i，可以得到5個(gè)關(guān)于參數(shù)bik的方程wj(bik) =O，j = 1，2，…5，由此可知，參數(shù) bi0~biN中只有N-5是獨(dú)立變量。不妨假設(shè)bi6~biN為獨(dú)立變量，機(jī)械臂運(yùn)動(dòng)規(guī)劃問題轉(zhuǎn)變?yōu)?優(yōu)化配置b l6~blN使相應(yīng)的目標(biāo)函數(shù)最小。
[0187] 第五步，混沌差分進(jìn)化求解，具體步驟如下：
[0188] 步驟1:初始化參數(shù)
[0189] 設(shè)定種群規(guī)模Np，個(gè)體維數(shù)dim，最大進(jìn)化代數(shù)Itermax，終止條誤差件ε，混沌搜索 次數(shù)k c，調(diào)節(jié)因子Fs，交叉因子Cr。
[0190] 步驟2:種群初始化
[0191] 每個(gè)種群個(gè)體包含以下變量:飛越段初始時(shí)刻，空間機(jī)器人在Hill坐標(biāo)系中相對(duì) 軌道運(yùn)動(dòng)的位置、速度、姿態(tài)以及角速度矢量 Ηχ?，機(jī)械臂末端的位置HR|的軌跡和姿態(tài) ΗΩ:【的軌跡;其中HX；：的維數(shù)為4\3=12，：》〗和11〇〖的各個(gè)維度可以由式(29)離散為1^個(gè) 變量，因還存在5個(gè)等式約束條件，所以共η= 12+3 X (nr-5)個(gè)獨(dú)立參數(shù)，因此隨機(jī)產(chǎn)生^個(gè) 參數(shù)向量萬(wàn)f =1?為a,.·:··為B]，i = l，2,…，NP，分別表示該設(shè)計(jì)變量的各個(gè)獨(dú)立參數(shù)，構(gòu)成的 初始種群P〇P(0)。
[0192] 步驟3:計(jì)算個(gè)體適應(yīng)度。首先將種群參數(shù)代入相對(duì)軌道運(yùn)動(dòng)方程，其次在相對(duì)運(yùn) 動(dòng)基礎(chǔ)上由式(29)、（30)得到機(jī)械臂末端標(biāo)準(zhǔn)軌跡，利用廣義雅克比矩陣計(jì)算基座速度和 位姿變化，代入目標(biāo)函數(shù)式(24)得到個(gè)體適應(yīng)度指標(biāo)。
[0193] 步驟4:精英選擇
[0194] 對(duì)種群適應(yīng)度進(jìn)行評(píng)價(jià)，找出適應(yīng)度最佳個(gè)體Bbest，記錄最佳適應(yīng)度值及個(gè)體在 種群中的索引位置。B be3st作為當(dāng)代種群的精英。
[0195] 步驟5:混沌局部搜索
[0196] 以Bbest作為混沌搜索的初值，在Bbest附近的區(qū)域進(jìn)行kc次混沌搜索：
[0197] Bk = Bbest+Fs · Pk (31)
[0198] 其中&為L(zhǎng)ogistic混沌方程的迭代變量，F(xiàn)s為調(diào)節(jié)因子，可使B k向著正反兩個(gè)方向 變化。從得到的個(gè)體Bk中找出適應(yīng)度最佳的B ' be3St，若B ' bsd尤于Bbe3st，則將B ' be3St隨機(jī)取代種 群中個(gè)體后進(jìn)入步驟6;若Bbe3st優(yōu)于B ' be3St，則直接進(jìn)入步驟6。
[0199] 步驟6:判斷終止條件
[0200]判斷是否滿足終止準(zhǔn)則(本章中為最大進(jìn)化代數(shù)或最佳適應(yīng)度均值的更新誤差）， 若滿足則終止計(jì)算，輸出最佳個(gè)體仏^*;若不滿足則進(jìn)入步驟7，且Iter = Iter+Ι。
[0201] 步驟7:進(jìn)化操作
[0202] 按照DE/local-to-best/Ι進(jìn)化模式的規(guī)則進(jìn)行相應(yīng)的選擇和進(jìn)化操作，生成新一 代種群pop(Iter)，返回步驟3。
[0203] 重復(fù)步驟3至步驟7的過程直至滿足終止條件，輸出最佳個(gè)體^^*及其適應(yīng)度的 值。
[0204]下面以某一仿真實(shí)例來說明本發(fā)明的具體實(shí)施效果。
[0205]以自由漂浮三關(guān)節(jié)空間機(jī)器人模型為例進(jìn)行仿真計(jì)算，系統(tǒng)物理參數(shù)如下表所 示。設(shè)定種群規(guī)模Np = 200，個(gè)體維數(shù)dim= 13，進(jìn)化次數(shù)上限Itermax= 1000，混純搜索次數(shù)kc =30 〇
[0207]實(shí)例中的仿真條件分別為：待抓捕目標(biāo)軌道半徑rT = 7000km，軌道角速率ωτ = 1.078 X 10-3rad/s，自旋角速率ω s = 〇. 15rad/s，抓捕點(diǎn)位置坐標(biāo)sRg= [-1，I，0]τ;空間機(jī)器 人相距目標(biāo)初始位置坐標(biāo)Pr = [ 0，-100，0 ]Τ，相對(duì)參考慣性系的初始姿態(tài)qb = [0,0,0]T，qrn= [0，-3t/6，-jt/6]t?？紤]飛越式逼近時(shí)間tc^300s，逼近最小安全距離4m，抓捕操作完成時(shí)間 tm^lOOSo
[0208] 經(jīng)計(jì)算得到飛越段初始點(diǎn)位置坐標(biāo)Hxcf = [-8.9120,29.0342,0]τ，速度f(wàn)lVcf =
[0.0576 ,-0.1374，0]τ，飛越式逼近過程時(shí)間為200.068s，機(jī)械臂抓捕操作時(shí)間100s。最終 生成的空間機(jī)器人軌道平面內(nèi)逼近路徑及抓捕軌跡如圖4所示。圖中虛線表示空間機(jī)器人 飛越式逼近軌跡，實(shí)線為機(jī)械臂末端抓捕運(yùn)動(dòng)軌跡，帶叉的點(diǎn)劃線為空間機(jī)器人撤離軌跡。 可以看出，空間機(jī)器人實(shí)現(xiàn)了凸路徑逼近運(yùn)動(dòng)，逼近中機(jī)械臂沿光滑連續(xù)軌跡執(zhí)行抓捕操 作，若此過程中出現(xiàn)意外故障，空間機(jī)器人可繼續(xù)沿?zé)o碰撞軌跡撤離。因此，規(guī)劃得到的軌 跡滿足模型要求，是合理有效的。
[0209] 圖5為參考慣性系內(nèi)空間機(jī)器人末端規(guī)劃軌跡及運(yùn)動(dòng)過程，圖6為抓捕過程中基座 姿態(tài)變化軌跡。由圖5和圖6可以看出機(jī)械臂末端抓捕運(yùn)動(dòng)軌跡光滑連續(xù)，且抓捕過程中基 座姿態(tài)受到的擾動(dòng)很小。圖7為抓捕過程中機(jī)械臂末端的速度軌跡，可以看出速度變化是連 續(xù)的，且恰在接觸時(shí)刻減小到零。圖8為抓捕過程中機(jī)械臂關(guān)節(jié)運(yùn)動(dòng)軌跡，圖9為機(jī)械臂末端 姿態(tài)規(guī)劃軌跡。可以看出關(guān)節(jié)及末端姿態(tài)變化軌跡符合規(guī)劃模型中的約束條件，是有效的 可行解。
[0210] 以上所述僅是本發(fā)明的優(yōu)選實(shí)施方式，本發(fā)明的保護(hù)范圍并不僅局限于上述實(shí)施 例，凡屬于本發(fā)明思路下的技術(shù)方案均屬于本發(fā)明的保護(hù)范圍。應(yīng)該提出，對(duì)于本技術(shù)領(lǐng)域 的普通技術(shù)人員來說，在不脫離本發(fā)明原理前提下的改進(jìn)和潤(rùn)飾，這些改進(jìn)和潤(rùn)飾也應(yīng)視 為本發(fā)明的保護(hù)范圍。
【主權(quán)項(xiàng)】
1. 一種用于在軌快速抓捕的姿軌臂一體化運(yùn)動(dòng)規(guī)劃方法，其特征在于:包括以下步驟： S1，確定規(guī)劃設(shè)計(jì)變量： x(〇 = [HxTfhrte 珥]T ⑴ 其中表示飛越段初始時(shí)刻，空間機(jī)器人在Hi 11坐標(biāo)系中相對(duì)軌道運(yùn)動(dòng)的位置和速 度矢量，HR〗、分別表示機(jī)械臂末端的位置和姿態(tài)； S2，確定運(yùn)動(dòng)規(guī)劃約束條件 運(yùn)動(dòng)規(guī)劃約束條件包括飛越式逼近軌跡約束條件和快速抓捕約束條件，其中飛越式逼 近軌跡約束條件包括安全操作距離約束、凸曲線約束和相對(duì)速度約束； (1)安全操作距離約束 ,iam <<p(0<^w,max5 ) 其中疋為空間機(jī)器人質(zhì)心相對(duì)于目標(biāo)抓捕點(diǎn)的最小安全距離，Rw,max表示空間機(jī)器人 操作空間的最大范圍，dcp(t)為空間機(jī)器人質(zhì)心相對(duì)目標(biāo)抓捕點(diǎn)的距離，tr為捕獲段的起始 時(shí)刻;Uf為接觸點(diǎn)時(shí)刻； 同時(shí)定義罰函數(shù)其中，H?T為目標(biāo)抓捕點(diǎn)自旋角速度向量，Hxcp是指相對(duì)運(yùn)動(dòng)位置向量， 1為相對(duì)運(yùn)動(dòng) 加速度向量； (3)相對(duì)速度約束其中:H4，分別為目標(biāo)抓捕點(diǎn)的速度分量，HA分別機(jī)械臂末端的速度分量，^為 安全操作最大相對(duì)速度； 所述的快速抓捕軌跡約束條件包括抓捕操作時(shí)間約束、機(jī)械臂末端位置約束、機(jī)械臂 末端姿態(tài)約束、機(jī)械臂物理約束、工作空間約束和接觸擾動(dòng)最小約束； (1) 抓捕操作時(shí)間約束 tmf-txmin<tp ( 5 ) 其中tmf為接觸點(diǎn)時(shí)刻，txmin為軌道相對(duì)運(yùn)動(dòng)速度最小或距離最近時(shí)刻，tp表示逃離段初 始時(shí)刻； (2) 機(jī)械臂末端位置約束其中HRCE,o表示初始時(shí)刻機(jī)械臂末端與目標(biāo)點(diǎn)的距離，HRE(t mQ)、HRE(tmf)分別表示抓捕操 作起始與終止時(shí)刻機(jī)械臂末端相對(duì)空間機(jī)器人質(zhì)心的位置，表示抓捕操作起始時(shí)刻，tmf 表示抓捕操作終止時(shí)刻;Hpr,0和HPr,f分別表示抓捕操作起始與終止時(shí)刻的空間機(jī)器人質(zhì)心 與目標(biāo)衛(wèi)星質(zhì)心的相對(duì)位置; hRg表示抓捕點(diǎn)相對(duì)目標(biāo)衛(wèi)星質(zhì)心的位置矢量； (3) 機(jī)械臂末端姿態(tài)約束其中H Q E,Q和H Q E,f分別為抓捕操作起始和終止時(shí)刻的機(jī)械臂末端姿態(tài)； (4) 機(jī)械臂物理約束其中qm,mM和分別表示關(guān)節(jié)角和角速率允許的最大值，qmi表示機(jī)械臂第i個(gè)關(guān)節(jié)的 關(guān)節(jié)角；同時(shí)定義罰函數(shù)如下：(5) 工作空間約束，用罰函數(shù)的形式表示為：其中e為小于1的正數(shù)，；￡。為大于1〇〇的正數(shù);hjqj表示空間機(jī)器人的廣義雅克比矩 陣，| ? |表示某矩陣的行列式，qm表示機(jī)械臂關(guān)節(jié)角向量;Re表示機(jī)械臂末端位置;PIW和PDW 分別表示為自由漂浮空間機(jī)器人工作空間中的路徑無(wú)關(guān)工作空間和路徑相關(guān)工作空間； (6) 接觸擾動(dòng)最小約束其中，iEu表示接觸前空間機(jī)器人的速度，為使得接觸引起的基座角速度變化為零 的機(jī)械臂末端相對(duì)抓捕點(diǎn)的速度，t?為接觸前抓捕點(diǎn)的速度； S3,確定運(yùn)動(dòng)規(guī)劃目標(biāo)函數(shù) 結(jié)合約束條件中罰函數(shù)指標(biāo)，令一體化運(yùn)動(dòng)規(guī)劃的目標(biāo)函數(shù)設(shè)定為 min J = Ju+Jctrs (11) 其中?1_ = 1+1+1+1為相應(yīng)的罰函數(shù)指標(biāo)山表示基座擾動(dòng)指標(biāo)，表示如下：其中4b是指空間機(jī)器人基座的角速度矢量；S4，機(jī)械臂末端軌跡參數(shù)化 對(duì)待規(guī)劃時(shí)間段t G [ tmQ，tmf ]， I G [ 〇，1 ];機(jī)械臂末端位置變量可以表示為： HRE=RU)，|e[〇，l] (13) 機(jī)械臂末端姿態(tài)由規(guī)劃初始時(shí)刻的機(jī)械臂末端姿態(tài)hQe,q繞單位向量k旋轉(zhuǎn)Of角后 調(diào)整到HQE,f，〇彡〇彡180°，因此機(jī)械臂末端姿態(tài)變量可以表示為 hQe=[k 〇a)],|G[〇,l] (14) 其中由^"和^以可以計(jì)算出^與^^使?jié)M足①⑶^乂⑴二％; 假設(shè)機(jī)械臂的期望運(yùn)動(dòng)軌跡連續(xù)可微，則其末端的速度軌跡可以表示為由式（13)-(15)，機(jī)械臂末端的位置與姿態(tài)軌跡最終由描述，因此對(duì)機(jī)械 臂運(yùn)動(dòng)規(guī)劃問題，設(shè)計(jì)變量可以統(tǒng)一表示為 xi") = [RTU)①T")]T，i = l，V"，|G[〇，l] (16) 在Hi 11坐標(biāo)系中對(duì)xi進(jìn)行參數(shù)化表示：其中xf 表示N階多項(xiàng)式形式的Xi(|)，N根據(jù)等式約束條件的數(shù)量選取，N>等式約束 條件的數(shù)量;Pk⑴表示關(guān)于時(shí)間變量t的切比雪夫多項(xiàng)式;blk表示Pk(t)的權(quán)系數(shù)； 對(duì)式(17)式求導(dǎo)，得到參數(shù)化形式的速度與加速度變量：S5,混沌差分進(jìn)化求解 S5.1:初始化參數(shù) 設(shè)定種群規(guī)模NP，個(gè)體維數(shù)dim，最大進(jìn)化代數(shù)Itermax，終止條誤差件e，混沌搜索次數(shù) kc，調(diào)節(jié)因子Fs，交叉因子Cr; S5.2:種群初始化 每個(gè)種群個(gè)體包含以下變量：飛越段初始時(shí)刻，空間機(jī)器人在Hi 11坐標(biāo)系中相對(duì)軌道 運(yùn)動(dòng)的位置、速度、姿態(tài)以及角速度矢量Hxt機(jī)械臂末端的位置的軌跡和姿態(tài)的 軌跡;其中的維數(shù)為4X3 = 12，HI^和的各個(gè)維度可以由式（17)離散為nr個(gè)變量， 因還存在5個(gè)等式約束條件，所以共n=12+3X(nr-5)個(gè)獨(dú)立參數(shù)，因此隨機(jī)產(chǎn)生NPf參數(shù)向 量 = [A i々:…A., ]，i = 1，2，…，NP，分別表示該設(shè)計(jì)變量的各個(gè)獨(dú)立參數(shù)，構(gòu)成的初始種 群pop(O); S5.3:計(jì)算個(gè)體適應(yīng)度 首先將種群參數(shù)代入相對(duì)軌道運(yùn)動(dòng)方程，其次在相對(duì)運(yùn)動(dòng)基礎(chǔ)上由式（17)、（18)得到 機(jī)械臂末端標(biāo)準(zhǔn)軌跡，利用廣義雅克比矩陣計(jì)算基座速度和位姿變化，代入目標(biāo)函數(shù)式 (11)得到個(gè)體適應(yīng)度指標(biāo)； S5.4:精英選擇 對(duì)種群適應(yīng)度進(jìn)行評(píng)價(jià)，找出適應(yīng)度最佳個(gè)體Bbest，記錄最佳適應(yīng)度值及個(gè)體在種群中 的索引位置;Bbest作為當(dāng)代種群的精英； S5.5:混沌局部搜索 以Bbest作為混沌搜索的初值，在Bbest附近的區(qū)域進(jìn)行kc次混沌搜索： Bk = Bbest+Fs ? 0k (19) 其中&為L(zhǎng)ogistic混沌方程的迭代變量，F(xiàn)s為調(diào)節(jié)因子，可使Bk向著正反兩個(gè)方向變 化;從得到的個(gè)體Bk中找出適應(yīng)度最佳的B ' be5St，若B ' bse5t優(yōu)于Bb(5St，則將B ' be5St隨機(jī)取代種群 中個(gè)體后轉(zhuǎn)S5.6 ;若Bbest優(yōu)于B ' best，則直接轉(zhuǎn)S5.6; S5.6:判斷終止條件 判斷是否滿足終止準(zhǔn)則，若滿足則終止計(jì)算，輸出最佳個(gè)體^^*;若不滿足則進(jìn)入 S5 ? 7，且Iter = Iter+1; S5.7:進(jìn)化操作 按照DE/local-to-best/1進(jìn)化模式的規(guī)則進(jìn)行相應(yīng)的選擇和進(jìn)化操作，生成新一代種 群pop(Iter)，返回S5.3; 重復(fù)S5.3至S5.7的過程直至滿足終止條件，輸出最佳個(gè)體Bgbest及其適應(yīng)度的值。
【文檔編號(hào)】B25J9/16GK106055810SQ201610396508
【公開日】2016年10月26日
【申請(qǐng)日】2016年6月7日
【發(fā)明人】李東旭, 范才智, 郭勝鵬, 劉望, 李思侃, 郝瑞
【申請(qǐng)人】中國(guó)人民解放軍國(guó)防科學(xué)技術(shù)大學(xué)