用于在軌快速抓捕的姿軌臂一體化運動規(guī)劃方法
【專利摘要】本發(fā)明屬于航天器運動規(guī)劃領域，涉及一種用于在軌快速抓捕的姿軌臂一體化運動規(guī)劃方法，本方法具有抓捕操作用時少、抓捕操作安全性好、求解趨于全局最優(yōu)等優(yōu)點。該方法首次提出對空間機器人的飛越式逼近目標過程與機械臂目標抓捕過程進行一體化規(guī)劃，實現(xiàn)了區(qū)別于傳統(tǒng)“?？?抓捕”方式的飛躍式動態(tài)抓捕。該方法首先提出了姿軌臂一體化運動規(guī)劃的模型，主要是確立了空間機器人飛越接近和機械臂抓捕過程的約束條件模型；其次，針對設計變量的離散化問題，提出了基于切比雪夫多項式的離散方法；最后，針對一般優(yōu)化算法的陷入局部最優(yōu)的問題，提出了適用于該規(guī)劃模型的混沌差分進化算法。
【專利說明】
用于在軌快速抓捕的姿軌臂一體化運動規(guī)劃方法
技術領域
[0001] 本發(fā)明涉及航天器系統(tǒng)動力學及控制技術領域，具體說涉及一種用于在軌快速抓 捕的姿軌臂一體化運動規(guī)劃方法。
【背景技術】
[0002] 空間機器人在軌抓捕通常的做法是，先交會接近到停靠點然后再實施抓捕操作， 這種方式將在軌抓捕過程分為交會和抓捕兩個獨立的階段。對交會階段，目前比較成熟的 方式為沿V-軸或R-軸的準直線受迫運動，經實際應用證明對受控穩(wěn)定目標是適合的。然而， 對于更一般的在軌抓捕任務，目標通常是非合作的，抓捕點可能在任意方向而非V-軸或R-軸方向。對此類目標，直線交會策略難以滿足任務要求。對抓捕階段，目前大量的研究成果 大都基于初始相對靜止的假設，將抓捕規(guī)劃問題轉化為笛卡爾坐標系下定點轉移的機械臂 運動規(guī)劃，而沒有考慮其軌道動力學背景。況且，這種先?？吭僮ゲ兜姆绞綄嵤┲芷谳^長， 不能滿足某些快速在軌服務任務的需求。
[0003] 文獻（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.)提出一種 服從軌道動力學的無碰撞接近策略，利用軌道相對運動規(guī)律，使空間機器人以自由漂浮模 式飛越接近目標，若捕獲失敗又能夠沿安全路徑逃離目標。基于該接近策略若使空間機器 人在相對運動的同時開展機械臂抓捕操作，不僅能夠適用于非合作目標抓捕，而且可以實 現(xiàn)更快速的在軌抓捕。但文獻中僅研究了空間機器人與目標質心交會接近問題，沒有考慮 機械臂抓捕運動規(guī)劃問題。對于飛越式逼近方式，抓捕時刻空間機器人相對目標體存在相 對運動，不滿足相對靜止的條件，并且一旦進入飛越段的初始條件確定，空間機器人的逼近 軌跡即嚴格確定，而空間機器人與目標的最近距離、交會速度以及可抓捕時間對機械臂能 否成功抓捕目標有決定性影響。

【發(fā)明內容】

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