一種對時變調姿目標的程序四元數在線規(guī)劃方法
【專利摘要】本發(fā)明公開了一種對時變調姿目標的程序四元數在線規(guī)劃方法，屬于飛行器控制系統(tǒng)設計領域。該方法根據調姿初始姿態(tài)的四元數描述和t時刻調姿時變目標姿態(tài)計算轉動四元數P(t)，根據P(t)計算參數α(t)及E1(t)、E2(t)、E3(t)，并根據α(t)的范圍進一步得到及根據調姿總時間T和參數α(t)計算參數從而計算得到t時刻在線規(guī)劃的程序四元數。本發(fā)明針對調姿目標姿態(tài)隨時間變化，且目標姿態(tài)時變過程中調姿范圍可由小于180度變?yōu)榇笥?80度的情形，提供了一種普適性強、易于軟件實現的飛行器調姿程序四元數在線規(guī)劃方法，該方法可保證調姿空間角度為最小，調姿過程平滑連續(xù)，實現原理清楚，且簡單可靠，算法復雜度低、易于軟件實現。
【專利說明】
-種對時變調姿目標的程序四元數在線規(guī)劃方法
技術領域
[0001 ]本發(fā)明設及一種具有連續(xù)時變調姿目標的程序四元數在線規(guī)劃方法，屬于飛行器 控制系統(tǒng)設計領域。
【背景技術】
[0002] 飛行器由于飛行任務需求，往往需要調整自身姿態(tài)到期望的目標姿態(tài)，并且經常 遇到期望的目標姿態(tài)是隨時間不斷變化的，即在姿態(tài)調整過程中目標姿態(tài)仍在時時刻刻發(fā) 生變化。運就需要在當前姿態(tài)和時變目標姿態(tài)之間實時規(guī)劃出光滑的姿態(tài)曲線，實現飛行 器的平穩(wěn)調姿。現在常見的姿態(tài)調姿主要是利用Ξ通道程序角分別進行在線規(guī)劃，所使用 方法包括角速度線性調姿、角速度梯形調姿等等。Ξ通道程序角在線規(guī)劃方法在飛行器飛 行過程中姿態(tài)角變化范圍大時，會出現約定轉序下姿態(tài)角解算奇異問題，為了克服運一問 題，一些飛行器姿態(tài)控制系統(tǒng)設計采用基于四元數直接控制方法，W回避姿態(tài)解算過程。在 此情況下，跟蹤程序姿態(tài)角就變?yōu)楦櫝绦蛩脑獢?，相應程序姿態(tài)角在線規(guī)劃調姿就變?yōu)?程序四元數在線規(guī)劃計算。
[0003] 目前，針對程序四元數在線規(guī)劃調姿已有的方法是在目標姿態(tài)非時變情形下設計 的，具有時變調姿目標姿態(tài)的在線規(guī)劃調姿還沒有相關方法。對于時變調姿目標的調姿規(guī) 劃，所面臨的主要問題：（1)如何設計調姿目標姿態(tài)時變情形下的平滑連續(xù)變化調姿軌跡， W滿足工程應用需求；（2)整個調姿需要保證四元數調姿空間角度為小角度，W節(jié)省調姿控 制所需燃料和減少調姿所需時間；（3)保證大范圍時變調姿情形下的適應性，需要滿足調姿 目標姿態(tài)時變并在180度左右變化的情況。

【發(fā)明內容】

[0004] 本發(fā)明所要解決的技術問題是:克服現有技術的不足，提供一種對時變調姿目標 的程序四元數在線規(guī)劃方法，能夠保證調姿空間角度為最小，且調姿過程平滑連續(xù)。
[0005] 本發(fā)明的技術解決方案是：一種對時變調姿目標的程序四元數在線規(guī)劃方法，包 括如下步驟：
[0006] 設調姿起始時刻為to,調姿總時間為T，在調姿過程的任意時刻te [to,to巧]，按如 下步驟計算在線規(guī)劃的程序四元數：
[0007] (1)根據飛行器調姿時的初始姿態(tài)四元數描述Qex(to)和t時刻的時變目標姿態(tài) 錢:巧，利用公式
計算t時刻的轉動四元 數P(t)，其中，
0表示四元數 乘
[0008] (2)根據轉動四元數P(t)計算t時刻的轉角參數a(t)及轉軸參數Ei(t)、E2(t)、E3 (t)，計算公式為:a(t)=2cos-i(p〇(t))
[0009] (3)對轉角參數a(t)進行轉化，使其位于[-31,+31]之間，得到巧叫，同時對Ei(t)、E2 (t)、E3 ( t)進行更新，得到焉議、焉識、與W :
[0010] (4)對嘶}進行連續(xù)化處理，同時更新耳(?)馬(0玉如;：
[0011] (5)利用如下公式計算t時刻的參數^0:
[001^ 當切《*《（切+172))時，有：
[0016] (6)利用公式化(0 = 知(?。)。瓦ω得到t時刻在線規(guī)劃的程序四元數Q。χ(t)，其 中
[0017] 所述步驟(3)的實現方式為：
[001引如果步驟(2)得到的a(t)〉3i，則對轉角參數a(t)進行如下轉化:巧0 = 2冗-挪），同 時對El (t)、E2 (t)、E3 (t)進行更新，得到奇0 = -￡|(0，是(0 = (0，皂(0 = -￡;(/);否 貝iJ，S(i) = a(", E、(j)二 Ε、(?)，F';(i)二 Ε;(!)，￡;(/)=與(0。
[0019] 所述步驟(4)的實現方式為：
[0020] 如果步驟（3)得到的抓)>/r,'l8〇 :，主
則利用公式巧0 = 2兄-巧。 對否的進行連續(xù)化處理，同時利用竊0 =-耳(0，島。:=-荀!)，毎的=-皂灼更新荀?)玉如、島句； 否則巧0及耳(0、與(0、可/)不變；
[0021] 其中，t_表示t時刻的上一時刻，且在to時刻有to = t-。
[0022] 本發(fā)明與現有技術相比具有如下優(yōu)點：
[0023] 本發(fā)明采用四元數描述飛行器姿態(tài)變化，提出了一種調姿目標姿態(tài)隨時間連續(xù)變 化情形下的姿態(tài)軌跡在線規(guī)劃方法，通過對轉角參數a(t)進行轉化，保證在實時規(guī)劃過程 中調姿空間角度連續(xù)且最小，W節(jié)省調姿所需燃料;通過對揉貨進行連續(xù)化處理，可適應目 標姿態(tài)時變過程中調姿范圍由小于180度變?yōu)榇笥?80度的情形。該方法實現原理清楚，且 簡單可靠，算法復雜度低、易于軟件實現。
【附圖說明】
[0024] 圖1為本發(fā)明流程圖；
[0025] 圖2為利用本發(fā)明方法在線規(guī)劃的程序四元數示意圖。
【具體實施方式】
[00%]下面就結合附圖對本發(fā)明做進一步介紹。
[0027] 針對調姿目標姿態(tài)隨時間變化，且目標姿態(tài)時變過程中調姿范圍可由小于180度 變?yōu)榇笥?80度的情形，本發(fā)明提供了一種普適性強、易于軟件實現的飛行器調姿程序四元 數在線規(guī)劃方法，能夠保證調姿空間角度為最小，且調姿過程平滑連續(xù)。
[0028] 如圖1所示，在調姿過程的任意時刻t，本發(fā)明的程序四元數在線規(guī)劃方法包括如 下步驟：
[0029] (1)根據飛行器調姿初始姿態(tài)的四元數描述Qcx(to)和t時刻調姿時變目標姿態(tài) 谷",'ω計算轉動四元數p(t)，如下
[0030] 尸")=[坑的鳳識代(1)化的]?'=化i(t。)。貨(t)
[0031] 其中，te [to，toW]，to為調姿起始時刻，T為調姿總時間

[00創(chuàng)在上式中，V'表示四元數乘，在下面給出妃(t。)。公';(0的具體公式
[0033]
[0034] (2)根據轉動四元數P(t)計算t時刻的參數a(t)及Ei(t)，E2(t)，E3(t)，有a(t) = 2cos_i(p〇(t)):
[0035] (3)如果計算的〇4)〉31，則作如下處理：
[0036]
[0037]如果a(t)《3i，則巧?) = α(1)，耳(/) =與(f)，耳(/) = [](〇，寫(7) = ￡；(〇
[003引 （3)在調姿t時亥IJ，如果切0>兄"80,且扣(r巧"><')(即調姿范圍由小于180度變?yōu)榇笥?!=J. 180度），則按照W下公式更新參數兩0及奇?)，石(0,島η:抓)=記-巧/),奇n=-哥0,耳/)=-島(1，卽)=-島?)， 否則，巧灼及耳(0王>)，有(0不變。其中，t-表示前一個計算周期時間，即t時刻的上一時刻， 且在to時刻有to = t-。
[0039] (4)根據調姿總時間Τ和參數巧0計算參數苗/)，計算公式如下
[0040] 當t〇《t《（t〇+T/2))時，有：
[0041]
[0042] 當（t〇+T/2)《t《t〇 巧）時，有：
[0043]
[0044] (5)按照如下公式計算t時刻在線規(guī)劃的程序四元數Qtx(t):
[0047]經過上述計算后，可W在線實時生成目標時變的調姿程序四元數，為控制系統(tǒng)設 計提供平滑的調姿軌跡。
[004引本方法給出的基于時變調姿目標的程序四元數在線規(guī)劃方法，也可應用于固定調 姿目標的調姿軌跡規(guī)劃，并可適用于飛行器空間任意姿態(tài)調姿軌跡設計需求，具有一定的 普適性。
[0049] 利用本發(fā)明方法對某飛行器進行程序四元數在線規(guī)劃仿真，其中初值數據
末端時變的零攻角目標數據
，得到的程序四元數如圖2所 示。從圖2中可W看出，本發(fā)明能夠實現調姿目標時變的程序四元數在線規(guī)劃，規(guī)劃的調姿 數據平滑連續(xù)，在調姿的終點時刻，調姿給出的程序四元數與零攻角目標對應的四元數重 合，說明本發(fā)明方法有效，能夠可靠規(guī)劃調姿路徑，跟蹤目標。
[0050] 本發(fā)明未詳細說明部分屬本領域技術人員公知常識。
【主權項】
1. 一種對時變調姿目標的程序四元數在線規(guī)劃方法，其特征在于包括如下步驟： 設調姿起始時刻為to,調姿總時間為T，在調姿過程的任意時刻〖^["，如+^安如下步 驟計算在線規(guī)劃的程序四元數： (1) 根據飛行器調姿時的初始姿態(tài)四元數描述'(to)和t時刻的時變目標姿態(tài)利 用公式八，⑴/，，⑴j32(t)尸,⑴y ⑴計算t時刻的轉動四元數p(t)， % (4>) %('θ.) 其中，仏i(u= 為Qcx(t〇)的共輒，α?(υ= ，。表示四元數乘， f2(%) -他) 咖=? q2 (f) (2) 根據轉動四元數P(t)計算t時刻的轉角參數α(t)及轉軸參數El(t)、E2(t)、E3(t)，計 算公式為:(61(1:)=2(3084^()(1:)):(3) 對轉角參數a(t)進行轉化，使其位于[-π,+π]之間，得到可同時對EKthEsU)、^ (t)進行更新，得到〇)、￡: 〇)、￡.; 0): (4) 對泛⑴進行連續(xù)化處理，同時更新瓦(〇、％>)、1(〇 ; (5) 利用如下公式計算t時刻的參數: 當to彡t彡（to+T/2))時，有：當（to+T/2)彡t彡to+T)時，有：(6) 利用公式⑴得到t時刻在線規(guī)劃的程序四元數Ut)，其中，2. 根據權利要求1所述的一種對時變調姿目標的程序四元數在線規(guī)劃方法，其特征在 于:所述步驟(3)的實現方式為： 如果步驟(2)得到的則對轉角參數a(t)進行如下轉化:可〇 = 2/r-?⑴·同時對 Ei (t)、E2 (t)、E3 (t)進行更新，得到瓦(〇 =-盡仿，瓦的=-馬(?)，瓦(/)=-馬(?);否則， ait) = a(i) ￡,(/) = Ε{{?) , Ε1(?) - ￡*.(/), ￡*,(/) = ￡"；(/) ^3.根據權利要求1所述的一種對時變調姿目標的程序四元數在線規(guī)劃方法，其特征在 于:所述步驟(4)的實現方式為： 如果步驟⑶得到的坷〇>冗/1那，且t互(U*郎〗<〇，則利用公式邱) = 2;r-邱)對邱） i--=l 進行連續(xù)化處理，同時利用瓦的=-瓦(0，g(i) = -瓦(〇 =-更新瓦(?)、瓦(0、瓦(r); 否則邱)及瓦(/)、萬(/)瓦(，)不變； 其中，t_表示t時刻的上一時刻，且在to時刻有t〇=t_。
【文檔編號】G05D1/08GK106066648SQ201610695140
【公開日】2016年11月2日
【申請日】2016年8月19日
【發(fā)明人】潘豪, 李學鋒, 王曉東, 王輝, 胡煜榮, 馮昊, 李新明, 李超兵
【申請人】北京航天自動控制研究所, 中國運載火箭技術研究院