[發(fā)明專利]一種基于機(jī)動能力辨識的抗干擾四元數(shù)姿態(tài)機(jī)動路徑規(guī)劃方法

有效

• 申請?zhí)枺?/td>: 201811504397.2
• 申請日: 2018-12-10
• 公開（公告）號: CN109634294B
• 公開（公告）日: 2022-01-04
• 發(fā)明（設(shè)計(jì)）人: 朱艦;陳海朋;黃飛;盛宏媛;楊孔進(jìn)
• 申請（專利權(quán)）人: 上海航天控制技術(shù)研究所
• 主分類號: G05D1/08
• 分類號: G05D1/08
• 代理公司: 中國航天科技專利中心 11009
• 代理人: 胡健男

1.一種基于機(jī)動能力辨識的抗干擾四元數(shù)姿態(tài)機(jī)動路徑規(guī)劃方法，其特征在于步驟如下：

(1)判斷當(dāng)前飛行器三軸角速度是否滿足姿態(tài)穩(wěn)定狀態(tài)時角速度要求，若滿足進(jìn)行步驟(2)；若不滿足保持當(dāng)前四元數(shù)不變；

(2)設(shè)飛行器三軸角加速度為設(shè)K_i為飛行器執(zhí)行機(jī)構(gòu)的開關(guān)系數(shù)，b_3i為當(dāng)前軸i的力矩系數(shù)，K_i表示開關(guān)特征量用當(dāng)前軸i的力矩系數(shù)表示初始時刻力矩系數(shù)特征量的初值Θ(0)，用三軸角加速度表示觀測量y(k)；根據(jù)開關(guān)特征量力矩系數(shù)特征量Θ(k)的初值Θ(0)和觀測量y(k)，遞推確定力矩系數(shù)特征量

(3)根據(jù)步驟(2)確定的計(jì)算姿態(tài)機(jī)動過程的最大角加速度

(4)判斷任務(wù)要求的姿態(tài)機(jī)動總時間T是否能夠完成姿態(tài)機(jī)動，若能完成，則根據(jù)步驟(3)得到的最大角加速度、任務(wù)要求的姿態(tài)機(jī)動總時間T和姿態(tài)角度變化最大量θ，計(jì)算滿足任務(wù)要求的最大角速度ω_max；否則，結(jié)束路徑規(guī)劃，無法完成任務(wù)；

(5)根據(jù)當(dāng)前四元數(shù)q_gb和目標(biāo)四元數(shù)q_gm，計(jì)算機(jī)動四元數(shù)q_bm并進(jìn)行符號歸一化處理，再根據(jù)符號歸一化處理后的q_bm的矢量部分最大值|q_max|，計(jì)算三軸機(jī)動角加速度

(6)根據(jù)步驟(3)得到的步驟(4)得到的ω_max和步驟(5)得到的三軸機(jī)動角加速度計(jì)算加速時間t₂、加速角θ₂和勻速角Δq₀；

(7)根據(jù)步驟(6)得到的Δq₀的符號，生成相應(yīng)角速度插值矩陣Γ(ω)；

(8)根據(jù)步驟(7)角速度插值矩陣Γ(ω)，生成當(dāng)前程序四元數(shù)q_cx(n)，完成路徑規(guī)劃；

步驟(2)根據(jù)開關(guān)特征量力矩系數(shù)特征量Θ(k)的初值Θ(0)和觀測量y(k)，遞推確定力矩系數(shù)特征量具體如下：

采用如下遞推公式計(jì)算

遞推結(jié)果誤差不大于0.01°/s；

其中，k＝1,2,3,…為遞推的當(dāng)前步數(shù)，k-1表示第k步的前一步；右上標(biāo)T表示轉(zhuǎn)置。

2.根據(jù)權(quán)利要求1所述的一種基于機(jī)動能力辨識的抗干擾四元數(shù)姿態(tài)機(jī)動路徑規(guī)劃方法，其特征在于：步驟(1)判斷當(dāng)前飛行器三軸角速度是否滿足姿態(tài)穩(wěn)定狀態(tài)時角速度要求，具體如下：

設(shè)姿態(tài)穩(wěn)定狀態(tài)時角速度為δ，若|ω_i|≤δ則判定當(dāng)前飛行器三軸角速度滿足姿態(tài)穩(wěn)定狀態(tài)時角速度要求，否則判定不滿足姿態(tài)穩(wěn)定狀態(tài)時角速度要求。

3.根據(jù)權(quán)利要求1所述的一種基于機(jī)動能力辨識的抗干擾四元數(shù)姿態(tài)機(jī)動路徑規(guī)劃方法，其特征在于：步驟(3)根據(jù)步驟(2)確定的計(jì)算姿態(tài)機(jī)動過程的最大角加速度具體為：

計(jì)算公式為：從步驟(2)遞推確定的中取。

4.根據(jù)權(quán)利要求1所述的一種基于機(jī)動能力辨識的抗干擾四元數(shù)姿態(tài)機(jī)動路徑規(guī)劃方法，其特征在于：步驟(4)判斷任務(wù)要求的姿態(tài)機(jī)動總時間T是否能夠完成姿態(tài)機(jī)動，若能完成，則根據(jù)步驟(3)得到的最大角加速度、任務(wù)要求的姿態(tài)機(jī)動總時間T和姿態(tài)角度變化最大量θ，計(jì)算滿足任務(wù)要求的最大角速度ω_max；否則，結(jié)束路徑規(guī)劃，具體如下：

設(shè)飛行任務(wù)要求的三軸姿態(tài)角變化量最大為θ，T為任務(wù)要求的姿態(tài)機(jī)動總時間，根據(jù)步驟(3)得到的計(jì)算

若則無法完成任務(wù)；

否則，

5.根據(jù)權(quán)利要求1所述的一種基于機(jī)動能力辨識的抗干擾四元數(shù)姿態(tài)機(jī)動路徑規(guī)劃方法，其特征在于：x、y、z為飛行器本體坐標(biāo)系的X軸、Y軸、Z軸，本體坐標(biāo)系的原點(diǎn)為飛行器質(zhì)心，X軸正方向?yàn)轱w行器頭部指向的方向，Y軸在飛行面內(nèi)與X軸垂直且指向上方，根據(jù)右手定則確定Z軸正方向。