基于在軌數(shù)據(jù)進行相對導(dǎo)航敏感器狀態(tài)輔助判斷的方法
【專利摘要】基于在軌數(shù)據(jù)進行相對導(dǎo)航敏感器狀態(tài)輔助判斷的方法，包括步驟：1、根據(jù)在軌注入的飛船和目標的相對位置和相對姿態(tài)信息初始化相對導(dǎo)航敏感器的相對位置和相對姿態(tài)估值；2、根據(jù)飛船陀螺、加速度計測量的脈沖數(shù)計算飛船當前周期的三軸姿態(tài)角速度和三軸線速度；3、獲取當前相對導(dǎo)航敏感器的相對位置和相對姿態(tài)的測量值并對測量值進行濾波；4、對當前周期的相對導(dǎo)航敏感器的狀態(tài)進行判讀得到導(dǎo)航敏感器狀態(tài)的判讀信息；5、遙測下傳相對導(dǎo)航敏感器狀態(tài)的判讀信息，并根據(jù)判讀信息對相對導(dǎo)航敏感器的工作狀態(tài)進行判斷。本發(fā)明提高了航天器交會對接過程的可靠性和安全性，便于飛控人員及時發(fā)現(xiàn)運行異常狀況，及時排查問題。
【專利說明】基于在軌數(shù)據(jù)進行相對導(dǎo)航敏感器狀態(tài)輔助判斷的方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及了一種基于在軌實時數(shù)據(jù)進行相對導(dǎo)航敏感器狀態(tài)輔助判斷的方法， 可用于空間交會對接過程中的交會對接敏感器數(shù)據(jù)正確性判斷，為飛行過程的正確性提供 判斷依據(jù)，屬于空間交會對接【技術(shù)領(lǐng)域】。

【背景技術(shù)】
[0002] 空間交會對接技術(shù)是通過對本體航天器的位置和姿態(tài)進行精確控制，在近地軌道 上實現(xiàn)與目標航天器的物理接觸的技術(shù)。與傳統(tǒng)的衛(wèi)星控制技術(shù)相比，空間交會對接技術(shù) 涉及到與目標航天器的相對位置和姿態(tài)信息的獲取及處理，需要用到一系列相對導(dǎo)航敏感 器，如激光雷達、微波雷達、CRDS相機等。
[0003] 在交會對接過程中，相對導(dǎo)航敏感器的工作狀態(tài)直接影響到交會對接的成敗，因 此無論是型號研制過程中的調(diào)試、測試環(huán)節(jié)，還是型號發(fā)射階段的飛控過程，都需要相對導(dǎo) 航敏感器狀態(tài)的輔助判斷方法。
[0004] 航天器的在軌運行期間，地面飛控人員需要關(guān)注大量的遙測信息，對相對導(dǎo)航敏 感器狀態(tài)的監(jiān)測更需要較高的實時性，因此必須設(shè)計自動化、直觀、可靠的輔助判斷方法， 對相對導(dǎo)航敏感器狀態(tài)進行實時的監(jiān)測。


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

[0005] 本發(fā)明的技術(shù)解決問題是：針對現(xiàn)有技術(shù)的不足，提供了基于在軌實時數(shù)據(jù)進行 相對導(dǎo)航敏感器狀態(tài)輔助判斷的方法，實現(xiàn)了對各個相對導(dǎo)航敏感器的運行狀態(tài)進行實時 監(jiān)視及預(yù)警，提高了航天器交會對接過程的可靠性和安全性，便于飛控人員及時發(fā)現(xiàn)運行 異常狀況，及時排查問題。
[0006] 本發(fā)明的技術(shù)解決方案是：
[0007] 基于在軌數(shù)據(jù)進行相對導(dǎo)航敏感器狀態(tài)輔助判斷的方法，包括步驟如下：
[0008] 步驟A1 :根據(jù)在軌注入的飛船和目標的相對位置和相對姿態(tài)信息初始化相對導(dǎo) 航敏感器的相對位置（相對敏感器的相對位置由相對導(dǎo)航敏感器獲得的飛船和目標的相 對位置）和相對姿態(tài)估值；
[0009] 步驟A2 :根據(jù)飛船陀螺、加速度計測量的脈沖數(shù)計算飛船當前周期的三軸姿態(tài)角 速度和三軸線速度；
[0010] 步驟A3 :獲取當前相對導(dǎo)航敏感器的相對位置和相對姿態(tài)的測量值并對測量值 進行濾波；
[0011] 步驟A4:對當前周期的相對導(dǎo)航敏感器的狀態(tài)進行判讀得到導(dǎo)航敏感器狀態(tài)的 判讀信息，所述的判讀信息包括相對導(dǎo)航敏感器數(shù)據(jù)可用標志、相對導(dǎo)航敏感器導(dǎo)航允許 標志和相對導(dǎo)航敏感器濾波收斂標志三個狀態(tài)標志和相對導(dǎo)航敏感器濾波計數(shù)器值；所述 相對導(dǎo)航敏感器數(shù)據(jù)可用標志表示相對導(dǎo)航敏感器的相對位置和相對姿態(tài)的測量值在通 信信道中傳輸正確的標志；所述相對導(dǎo)航敏感器導(dǎo)航允許標志表示相對導(dǎo)航敏感器在一定 時間內(nèi)測量數(shù)據(jù)比較穩(wěn)定時，相對導(dǎo)航敏感器的濾波過程可以進行的標志；所述相對導(dǎo)航 敏感器濾波收斂標志表示當前周期相對導(dǎo)航敏感器測量的相對位置和相對姿態(tài)有效的標 志；所述相對導(dǎo)航敏感器濾波計數(shù)器值表示相對導(dǎo)航敏感器狀態(tài)判讀過程的計數(shù)器；
[0012] 步驟A5 :遙測下傳步驟A4得到的當前周期的相對導(dǎo)航敏感器狀態(tài)的判讀信息，并 根據(jù)判讀信息對相對導(dǎo)航敏感器的工作狀態(tài)進行判斷。
[0013] 所述步驟A3中對當前相對導(dǎo)航敏感器的相對位置和相對姿態(tài)的測量值進行濾波 的具體方法如下：
[0014] 步驟B1 :判斷相對導(dǎo)航敏感器導(dǎo)航允許標志是否有效，若有效則轉(zhuǎn)步驟B2,否則 退出濾波過程進入步驟B8;
[0015] 步驟B2 :利用空間動力學(xué)算法，根據(jù)步驟A2計算出的飛船當前周期的三軸姿態(tài)角 速度和三軸線速度以及上一周期的相對導(dǎo)航敏感器的相對位置和姿態(tài)估值，計算當前周期 的相對導(dǎo)航敏感器的相對位置和姿態(tài)的估值；若為首次濾波，所述的上一周期相對導(dǎo)航敏 感器的相對位置和姿態(tài)估值為步驟A1中的初始化的相對位置和姿態(tài)估值，否則所述的上 一周期相對導(dǎo)航敏感器的相對位置和姿態(tài)估值為上一周期濾波過程中計算出的相對導(dǎo)航 敏感器的相對位置和姿態(tài)估值；
[0016] 步驟B3 :判斷相對導(dǎo)航敏感器數(shù)據(jù)可用標志是否有效，若有效則轉(zhuǎn)步驟B4,否則 退出濾波過程進入步驟B8;
[0017] 步驟Μ :計算當前周期的相對導(dǎo)航敏感器的相對位置和相對姿態(tài)的測量值與步 驟Β2中計算出的估值的差值，并將差值的絕對值與剃野閾值比較，若相對位置或相對姿態(tài) 的測量值與估值的差值絕對值大于剃野閾值，則判斷當前周期的相對位置或相對姿態(tài)測量 值無效，轉(zhuǎn)步驟Β5,否則轉(zhuǎn)步驟Β6 ;所述剃野閾值表示相對位置和相對姿態(tài)的測量值的浮 動范圍；
[0018] 步驟Β5 :置相對導(dǎo)航敏感器數(shù)據(jù)可用標志無效，并退出濾波過程，進入步驟Β8 ;
[0019] 步驟Β6 :將步驟Μ中計算出的相對位置和相對姿態(tài)的測量值與估值的差值的絕 對值與限幅閾值比較，若相對位置或相對姿態(tài)的測量值與估值的差值的絕對值大于限幅閾 值，則將限幅閾值賦值給該差值，否則不做任何處理直接進入步驟Β7 ;所述限幅閾值表示 測量值的浮動范圍；
[0020] 步驟Β7 :利用當前周期的相對導(dǎo)航敏感器的相對位置和相對姿態(tài)的估值與步驟 Β6計算出的差值求和，并利用該求和結(jié)果更新當前周期相對導(dǎo)航敏感器位置姿態(tài)估值； (當進行下一周期濾波循環(huán)時，當前周期的相對敏感器的相對位置和相對姿態(tài)的估值作為 步驟Β2);
[0021] 步驟Β8:結(jié)束。
[0022] 所述步驟Μ中的剃野閾值和所述步驟Β6中的限幅閾值的具體數(shù)值根據(jù)具體應(yīng)用 而定。
[0023] 所述步驟Α4中相對導(dǎo)航敏感器狀態(tài)判讀過程的具體步驟如下：
[0024] 步驟C1 :判斷相對導(dǎo)航敏感器導(dǎo)航允許標志是否有效，若無效則轉(zhuǎn)步驟C2,否則 轉(zhuǎn)步驟C7 ;
[0025] 步驟C2 :判斷相對導(dǎo)航敏感器數(shù)據(jù)可用標志是否有效，若有效則轉(zhuǎn)步驟C3,否則 轉(zhuǎn)步驟C4 ;
[0026] 步驟C3 :將相對導(dǎo)航敏感器濾波計數(shù)器加 1并進入步驟C5 ;
[0027] 步驟C4 :將相對導(dǎo)航敏感器濾波計數(shù)器清零并進入步驟C5 ;
[0028] 步驟C5 :判斷相對導(dǎo)航敏感器濾波計數(shù)器的值是否大于閾值a，若大于則轉(zhuǎn)步驟 C6,否則轉(zhuǎn)步驟C7 ;
[0029] 步驟C6 :置相對導(dǎo)航敏感器導(dǎo)航允許標志有效以及置相對導(dǎo)航敏感器濾波收斂 標志無效；
[0030] 步驟C7 :若相對導(dǎo)航敏感器導(dǎo)航允許標志有效，且相對導(dǎo)航敏感器濾波收斂標志 無效，則轉(zhuǎn)步驟C8,否則轉(zhuǎn)步驟C15 ;
[0031] 步驟C8 :若相對導(dǎo)航敏感器數(shù)據(jù)可用標志有效，則轉(zhuǎn)步驟C9,否則轉(zhuǎn)步驟C10 ;
[0032] 步驟C9 :將相對導(dǎo)航敏感器濾波計數(shù)器加 1 ;
[0033] 步驟C10 :將相對導(dǎo)航敏感器濾波計數(shù)器減1 ;
[0034] 步驟C11 :判斷相對導(dǎo)航敏感器濾波計數(shù)器的值是否大于閾值b，若大于則轉(zhuǎn)步驟 C12,否則轉(zhuǎn)步驟C13 ;
[0035] 步驟C12 :置相對導(dǎo)航敏感器濾波收斂標志有效；
[0036] 步驟C13 :判斷相對導(dǎo)航敏感器濾波計數(shù)器的值是否小于閾值c，若小于則轉(zhuǎn)步驟 C14,否則退出相對導(dǎo)航敏感器狀態(tài)判讀過程并進入步驟C21 ;
[0037] 步驟C14 :置相對導(dǎo)航敏感器導(dǎo)航允許標志無效，相對導(dǎo)航敏感器濾波收斂標志 無效，并將相對導(dǎo)航敏感器濾波計數(shù)器清零并進入步驟C21 ;
[0038] 步驟C15 :判斷相對導(dǎo)航敏感器濾波收斂標志是否有效，若有效則轉(zhuǎn)步驟C16,否 則退出相對導(dǎo)航敏感器狀態(tài)判讀過程并進入步驟C21 ;
[0039] 步驟C16 :判斷相對導(dǎo)航敏感器數(shù)據(jù)可用標志是否，若有效則轉(zhuǎn)步驟C17,否則，轉(zhuǎn) 步驟C18 ;
[0040] 步驟C17 :將相對導(dǎo)航敏感器濾波計數(shù)器加 1 ;
[0041] 步驟C18 :將相對導(dǎo)航敏感器濾波計數(shù)器減1 ;
[0042] 步驟C19 :判斷相對導(dǎo)航敏感器濾波計數(shù)器的值是否小于閾值d，若小于則轉(zhuǎn)步驟 C20,否則退出相對導(dǎo)航敏感器狀態(tài)判讀過程并進入步驟C21 ;
[0043] 步驟C20 :置相對導(dǎo)航敏感器導(dǎo)航允許標志無效，相對導(dǎo)航敏感器濾波收斂標志 無效，并將相對導(dǎo)航敏感器濾波計數(shù)器清零；
[0044] 步驟C21:結(jié)束。
[0045] 所述步驟C5中的閾值a、所述步驟C11中的閾值b、所述步驟C13中的閾值c和所 述步驟C19中的閾值d均根據(jù)具體應(yīng)用情況而定（例如飛船的控制周期為160ms，則設(shè)置閾 值a為32,則表示相對導(dǎo)航敏感器的相對位置和相對姿態(tài)的測量值連續(xù)有效5. 12s后開始 進行相對導(dǎo)航敏感期狀態(tài)的判讀；設(shè)置閾值b為64,則表示進行相對導(dǎo)航敏感器狀態(tài)判讀 后相對位置和相對姿態(tài)的測量值有效累計時間達到10.24s以上，則認為相對導(dǎo)航敏感器 狀態(tài)有效）。
[0046] 所述步驟A5中根據(jù)判斷信息對相對導(dǎo)航敏感期的工作狀態(tài)進行判斷的具體方法 如下：
[0047] (1)若相對導(dǎo)航敏感器數(shù)據(jù)可用標志有效，則表示相對導(dǎo)航敏感器的相對位置和 相對姿態(tài)的測量值在通信信道中傳輸正確，否則表示測量值在通信信道中傳輸錯誤；
[0048] (2)若相對導(dǎo)航敏感器導(dǎo)航允許標志有效，貝U表不相對導(dǎo)航敏感器在一定時間內(nèi) (時間長度由步驟C5中閾值a控制）相對位置和相對姿態(tài)的測量值比較穩(wěn)定，相對導(dǎo)航敏 感器的濾波過程正在進行；否則表示一定時間內(nèi)相對導(dǎo)航敏感器測量值不穩(wěn)，飛船需等待 相對導(dǎo)航敏感器測量值穩(wěn)定后才能開始進行測量值濾波；
[0049] (3)若相對導(dǎo)航敏感器濾波收斂標志有效，則表示當前相對導(dǎo)航敏感器測量狀態(tài) 有效；若相對導(dǎo)航敏感器濾波收斂標志無效，則表示當前相對導(dǎo)航敏感器測量狀態(tài)無效。
[0050] 本發(fā)明的實現(xiàn)方法與現(xiàn)有技術(shù)相比的優(yōu)點在于：
[0051] (1)本發(fā)明實現(xiàn)了對各個相對導(dǎo)航敏感器的運行狀態(tài)進行實時監(jiān)視及預(yù)警，提高 了航天器交會對接過程的可靠性和安全性，便于飛控人員及時發(fā)現(xiàn)運行異常狀況，及時排 查問題
[0052] (2)本發(fā)明將相對導(dǎo)航敏感器狀態(tài)判讀過程融入到濾波過程中，通過數(shù)據(jù)剃野、限 幅操作消除了由于敏感器測量異常等原因引起的測量錯誤對判讀結(jié)果的影響，所以本發(fā)明 相對導(dǎo)航敏感器狀態(tài)判讀的魯棒性高，準確性較強。
[0053] (3)本發(fā)明遙測下傳的相對導(dǎo)航敏感器狀態(tài)判讀結(jié)果通過相對導(dǎo)航敏感器數(shù)據(jù)可 用標志、相對導(dǎo)航敏感器導(dǎo)航允許標志和相對導(dǎo)航敏感器濾波收斂標志三個標志和相對導(dǎo) 航敏感器濾波計數(shù)器描述，不僅能夠?qū)崟r掌握相對導(dǎo)航敏感器的工作狀態(tài)，而且能夠掌握 判讀階段，方便相對導(dǎo)航敏感器狀態(tài)異常時的錯誤定位，因此本發(fā)明相對導(dǎo)航敏感器狀態(tài) 判讀的可讀性好。

【專利附圖】

【附圖說明】
[0054] 圖1為本發(fā)明方法流程圖；
[0055] 圖2為本發(fā)明相對位置與速度濾波過程流程圖；
[0056] 圖3為本發(fā)明相對導(dǎo)航敏感器狀態(tài)判讀過程流程圖。

【具體實施方式】
[0057] 下面結(jié)合附圖和具體實施例對本發(fā)明作進一步詳細的描述：
[0058] 如圖1所示，本發(fā)明一種基于在軌實時數(shù)據(jù)進行相對導(dǎo)航敏感器狀態(tài)輔助判斷的 方法，步驟如下：
[0059] 基于在軌數(shù)據(jù)進行相對導(dǎo)航敏感器狀態(tài)輔助判斷的方法，包括步驟如下：
[0060] 步驟A1 :根據(jù)在軌注入的飛船和目標的相對位置和相對姿態(tài)信息初始化相對導(dǎo) 航敏感器的相對位置（相對敏感器的相對位置由相對導(dǎo)航敏感器獲得的飛船和目標的相 對位置）和相對姿態(tài)估值；
[0061] 步驟A2 :根據(jù)飛船陀螺、加速度計測量的脈沖數(shù)計算飛船當前周期的三軸姿態(tài)角 速度和三軸線速度；
[0062] 步驟A3 :獲取當前相對導(dǎo)航敏感器的相對位置和相對姿態(tài)的測量值并對測量值 進行濾波；
[0063] 對當前相對導(dǎo)航敏感器的相對位置和相對姿態(tài)的測量值進行濾波的具體方法如 下：
[0064] 步驟B1 :判斷相對導(dǎo)航敏感器導(dǎo)航允許標志是否有效，若有效則轉(zhuǎn)步驟B2,否則 退出濾波過程進入步驟B8 ;
[0065] 步驟B2 :利用空間動力學(xué)算法，根據(jù)步驟A2計算出的飛船當前周期的三軸姿態(tài)角 速度和三軸線速度以及上一周期的相對導(dǎo)航敏感器的相對位置和姿態(tài)估值，計算當前周期 的相對導(dǎo)航敏感器的相對位置和姿態(tài)的估值；若為首次濾波，所述的上一周期相對導(dǎo)航敏 感器的相對位置和姿態(tài)估值為步驟A1中的初始化的相對位置和姿態(tài)估值，否則所述的上 一周期相對導(dǎo)航敏感器的相對位置和姿態(tài)估值為上一周期濾波過程中計算出的相對導(dǎo)航 敏感器的相對位置和姿態(tài)估值；
[0066] 步驟B3 :判斷相對導(dǎo)航敏感器數(shù)據(jù)可用標志是否有效，若有效則轉(zhuǎn)步驟B4,否則 退出濾波過程進入步驟B8;
[0067] 步驟Μ :計算當前周期的相對導(dǎo)航敏感器的相對位置和相對姿態(tài)的測量值與步 驟Β2中計算出的估值的差值，并將差值的絕對值與剃野閾值比較，若相對位置或相對姿態(tài) 的測量值與估值的差值絕對值大于剃野閾值，則判斷當前周期的相對位置或相對姿態(tài)測量 值無效，轉(zhuǎn)步驟Β5,否則轉(zhuǎn)步驟Β6 ;所述剃野閾值表示相對位置和相對姿態(tài)的測量值的浮 動范圍；剃野閾值具體數(shù)值根據(jù)具體應(yīng)用而定。
[0068] 步驟Β5 :置相對導(dǎo)航敏感器數(shù)據(jù)可用標志無效，并退出濾波過程，進入步驟Β8 ;
[0069] 步驟Β6 :將步驟Μ中計算出的相對位置和相對姿態(tài)的測量值與估值的差值的絕 對值與限幅閾值比較，若相對位置或相對姿態(tài)的測量值與估值的差值的絕對值大于限幅閾 值，則保留差值的符號不變，將限幅閾值賦值給該差值的數(shù)值部分，否則不做任何處理直接 進入步驟Β7 ;所述限幅閾值表示測量值的浮動范圍；步驟Β6中的限幅閾值的具體數(shù)值根據(jù) 具體應(yīng)用而定。
[0070] 步驟Β7 :利用當前周期的相對導(dǎo)航敏感器的相對位置和相對姿態(tài)的估值與步驟 Β6計算出的差值求和，并利用該求和結(jié)果更新當前周期相對導(dǎo)航敏感器位置姿態(tài)估值； (當進行下一周期濾波循環(huán)時，當前周期的相對敏感器的相對位置和相對姿態(tài)的估值作為 步驟Β2);
[0071] 步驟Β8:結(jié)束。
[0072] 步驟Α4:對當前周期的相對導(dǎo)航敏感器的狀態(tài)進行判讀得到導(dǎo)航敏感器狀態(tài)的 判讀信息，所述的判讀信息包括相對導(dǎo)航敏感器數(shù)據(jù)可用標志、相對導(dǎo)航敏感器導(dǎo)航允許 標志和相對導(dǎo)航敏感器濾波收斂標志三個狀態(tài)標志和相對導(dǎo)航敏感器濾波計數(shù)器值；所述 相對導(dǎo)航敏感器數(shù)據(jù)可用標志表示相對導(dǎo)航敏感器的相對位置和相對姿態(tài)的測量值在通 信信道中傳輸正確的標志；所述相對導(dǎo)航敏感器導(dǎo)航允許標志表示相對導(dǎo)航敏感器在一定 時間內(nèi)測量數(shù)據(jù)比較穩(wěn)定時，相對導(dǎo)航敏感器的濾波過程可以進行的標志；所述相對導(dǎo)航 敏感器濾波收斂標志表示當前周期相對導(dǎo)航敏感器測量的相對位置和相對姿態(tài)有效的標 志；所述相對導(dǎo)航敏感器濾波計數(shù)器值表示相對導(dǎo)航敏感器狀態(tài)判讀過程的計數(shù)器；
[0073] 相對導(dǎo)航敏感器狀態(tài)判讀過程的具體步驟如下：
[0074] 步驟C1 :判斷相對導(dǎo)航敏感器導(dǎo)航允許標志是否有效，若無效則轉(zhuǎn)步驟C2,否則 轉(zhuǎn)步驟C7 ;
[0075] 步驟C2 :判斷相對導(dǎo)航敏感器數(shù)據(jù)可用標志是否有效，若有效則轉(zhuǎn)步驟C3,否則 轉(zhuǎn)步驟C4 ;
[0076] 步驟C3 :將相對導(dǎo)航敏感器濾波計數(shù)器加 1并進入步驟C5 ;
[0077] 步驟C4 :將相對導(dǎo)航敏感器濾波計數(shù)器清零并進入步驟C5 ;
[0078] 步驟C5 :判斷相對導(dǎo)航敏感器濾波計數(shù)器的值是否大于閾值a，若大于則轉(zhuǎn)步驟 C6,否則轉(zhuǎn)步驟C7 ;
[0079] 步驟C6 :置相對導(dǎo)航敏感器導(dǎo)航允許標志有效以及置相對導(dǎo)航敏感器濾波收斂 標志無效；
[0080] 步驟C7 :若相對導(dǎo)航敏感器導(dǎo)航允許標志有效，且相對導(dǎo)航敏感器濾波收斂標志 無效，則轉(zhuǎn)步驟C8,否則轉(zhuǎn)步驟C15 ;
[0081] 步驟C8 :若相對導(dǎo)航敏感器數(shù)據(jù)可用標志有效，則轉(zhuǎn)步驟C9,否則轉(zhuǎn)步驟C10 ;
[0082] 步驟C9 :將相對導(dǎo)航敏感器濾波計數(shù)器加 1 ;
[0083] 步驟C10 :將相對導(dǎo)航敏感器濾波計數(shù)器減1 ;
[0084] 步驟C11 :判斷相對導(dǎo)航敏感器濾波計數(shù)器的值是否大于閾值b，若大于則轉(zhuǎn)步驟 C12,否則轉(zhuǎn)步驟C13 ;
[0085] 步驟C12 :置相對導(dǎo)航敏感器濾波收斂標志有效；
[0086] 步驟C13 :判斷相對導(dǎo)航敏感器濾波計數(shù)器的值是否小于閾值c，若小于則轉(zhuǎn)步驟 C14,否則退出相對導(dǎo)航敏感器狀態(tài)判讀過程并進入步驟C21 ;
[0087] 步驟C14 :置相對導(dǎo)航敏感器導(dǎo)航允許標志無效，相對導(dǎo)航敏感器濾波收斂標志 無效，并將相對導(dǎo)航敏感器濾波計數(shù)器清零并進入步驟C21 ;
[0088] 步驟C15 :判斷相對導(dǎo)航敏感器濾波收斂標志是否有效，若有效則轉(zhuǎn)步驟C16,否 則退出相對導(dǎo)航敏感器狀態(tài)判讀過程并進入步驟C21 ;
[0089] 步驟C16 :判斷相對導(dǎo)航敏感器數(shù)據(jù)可用標志是否，若有效則轉(zhuǎn)步驟C17,否則，轉(zhuǎn) 步驟C18 ;
[0090] 步驟C17 :將相對導(dǎo)航敏感器濾波計數(shù)器加1 ;
[0091] 步驟C18 :將相對導(dǎo)航敏感器濾波計數(shù)器減1 ;
[0092] 步驟C19 :判斷相對導(dǎo)航敏感器濾波計數(shù)器的值是否小于閾值d，若小于則轉(zhuǎn)步驟 C20,否則退出相對導(dǎo)航敏感器狀態(tài)判讀過程并進入步驟C21 ;
[0093] 步驟C20 :置相對導(dǎo)航敏感器導(dǎo)航允許標志無效，相對導(dǎo)航敏感器濾波收斂標志 無效，并將相對導(dǎo)航敏感器濾波計數(shù)器清零；
[0094] 步驟C21:結(jié)束。
[0095] 步驟C5中的閾值a、步驟C11中的閾值b、步驟C13中的閾值c和步驟C19中的閾 值d均根據(jù)具體應(yīng)用情況而定（例如飛船的控制周期為160ms，則設(shè)置閾值a為32,則表示 相對導(dǎo)航敏感器的相對位置和相對姿態(tài)的測量值連續(xù)有效5. 12s后開始進行相對導(dǎo)航敏 感期狀態(tài)的判讀；設(shè)置閾值b為64,則表示進行相對導(dǎo)航敏感器狀態(tài)判讀后相對位置和相 對姿態(tài)的測量值有效累計時間達到10. 24s以上，則認為相對導(dǎo)航敏感器狀態(tài)有效）。
[0096] 步驟A5 :遙測下傳步驟A4得到的當前周期的相對導(dǎo)航敏感器狀態(tài)的判讀信息，并 根據(jù)判讀信息對相對導(dǎo)航敏感器的工作狀態(tài)進行判斷。
[0097] 根據(jù)判斷信息對相對導(dǎo)航敏感期的工作狀態(tài)進行判斷的具體方法如下：
[0098] (1)若相對導(dǎo)航敏感器數(shù)據(jù)可用標志有效，則表示相對導(dǎo)航敏感器的相對位置和 相對姿態(tài)的測量值在通信信道中傳輸正確，否則表示測量值在通信信道中傳輸錯誤；
[0099] (2)若相對導(dǎo)航敏感器導(dǎo)航允許標志有效，貝U表不相對導(dǎo)航敏感器在一定時間內(nèi) (時間長度由步驟C5中閾值a控制）相對位置和相對姿態(tài)的測量值比較穩(wěn)定，相對導(dǎo)航敏 感器的濾波過程正在進行；否則表示一定時間內(nèi)相對導(dǎo)航敏感器測量值不穩(wěn)，飛船需等待 相對導(dǎo)航敏感器測量值穩(wěn)定后才能開始進行測量值濾波；
[0100] (3)若相對導(dǎo)航敏感器濾波收斂標志有效，則表示當前相對導(dǎo)航敏感器測量狀態(tài) 有效；若相對導(dǎo)航敏感器濾波收斂標志無效，則表示當前相對導(dǎo)航敏感器測量狀態(tài)無效。
[0101] 下面以一個具體實施例進一步說明本發(fā)明的工作過程和工作原理：
[0102] 以飛船和目標的相對位置X為例進一步說明本發(fā)明的濾波過程和判讀過程（飛船 和目標的相對位置X是指飛船與目標在X軸方向上的距離，其中在以目標為坐標系原點，目 標前向為X軸，目標底部垂直方向為Z軸，按照右手螺旋定則確定Y軸），具體實施例如下：
[0103] 1、飛船加電后，假設(shè)相對導(dǎo)航敏感器導(dǎo)航允許標志初始值為無效，因此不進入步 驟A3的濾波過程，在步驟A4的相對導(dǎo)航敏感器判讀過程中，設(shè)相對導(dǎo)航敏感器當前通訊狀 態(tài)正常，因此相對導(dǎo)航敏感器數(shù)據(jù)可用標志有效，設(shè)閾值a為32,則經(jīng)過32次循環(huán)后，相對 導(dǎo)航敏感器導(dǎo)航允許標志有效，相對導(dǎo)航敏感器濾波收斂標志無效；
[0104] 2、此時進入到步驟C7,假設(shè)相對導(dǎo)航敏感器濾波計數(shù)器為32,設(shè)閾值b為64,由于 濾波計數(shù)器不大于閾值b，因此退出相對導(dǎo)航敏感器判讀過程，退出時相對導(dǎo)航敏感器濾波 計數(shù)器為33 ;
[0105] 3、下一個循環(huán)中，進入步驟A3的濾波過程，由于上一循環(huán)沒有進行濾波過程，當 前相對位置的值為初始值，假設(shè)在軌注入的相對位置X為140m，當前周期陀螺測量得到的 飛船X方向線速度為lm/s，則在進入步驟A3時，計算當前周期的相對導(dǎo)航敏感器的相對位 置X的估值為140. 16m(計算過程：線速度lm/s，假設(shè)飛船控制周期為0. 16s，控制周期內(nèi)前 進距離0. 16m，所以當前周期的相對位置X為140. 16m);
[0106] 假設(shè)相對導(dǎo)航敏感器測量得到的相對位置X為141m，相對位置的測量值與估值的 差值為0. 84m，因此差值小于限幅閾值2 (假設(shè)剃野閾值為5,限幅閾值為2)，因此利用當 前周期的相對導(dǎo)航敏感器的相對位置140. 16m和相對姿態(tài)的估值與步驟B6計算出的差值 0. 84求和得到當前的相對位置X的估值為141m ;進入步驟A4的判讀過程；
[0107] 4、再下一個循環(huán)中，進入步驟A3的濾波過程，當前相對導(dǎo)航敏感器的位置X估值 為141m(步驟3中的更新值），設(shè)當前周期陀螺測量得到的飛船X方向線速度為lm/s，則在 進入步驟A3時，相對導(dǎo)航敏感器的相對位置X的估值為141. 16m，設(shè)相對導(dǎo)航敏感器測量 得到的相對位置X為144m，測量值和估值的差值2. 84大于限幅閾值2,則經(jīng)過步驟B6限幅 后，退出濾波過程時，此時差值重新賦值為2,因此得到相對導(dǎo)航敏感器的相對位置X的估 值為 143. 16m ;
[0108] 6、繼續(xù)循環(huán)，當相對導(dǎo)航敏感器濾波計數(shù)器為65時，會大于閾值b(b為64)，進入 步驟C12,并置相對導(dǎo)航敏感器濾波收斂標志有效，地面觀察到相應(yīng)的遙測結(jié)果后，能夠判 斷處當前相對導(dǎo)航敏感器狀態(tài)正常；
[0109] 7、假設(shè)接下來相對導(dǎo)航敏感器狀態(tài)異常，表現(xiàn)為測量值不正確，假設(shè)相對導(dǎo)航敏 感器的相對位置X測量值為40m，假設(shè)此時的相對位置X的估值為169m，則估值和測量值的 差值大于剃野閾值5,則在步驟Μ中，判斷為剃野超差，進入步驟B5,置相對導(dǎo)航敏感器數(shù) 據(jù)可用標志無效；
[0110] 8、則在步驟Α4的判讀過程中，進入到步驟C18,相對導(dǎo)航敏感器濾波計數(shù)器遞減， 設(shè)閾值d為-10,繼續(xù)循環(huán)，當相對導(dǎo)航敏感器濾波計數(shù)器為-11時，置相對導(dǎo)航敏感器濾波 收斂標志無效，地面觀察到相應(yīng)的遙測結(jié)果后，能夠判斷處當前相對導(dǎo)航敏感器狀態(tài)異常。 [0111] 本發(fā)明已經(jīng)經(jīng)過專家評定，且實際應(yīng)用于的相應(yīng)的移動衛(wèi)星型號，取得了良好的 技術(shù)效果。
[0112] 本發(fā)明未詳細闡述部分屬于本領(lǐng)域公知技術(shù)。
【權(quán)利要求】
1. 基于在軌數(shù)據(jù)進行相對導(dǎo)航敏感器狀態(tài)輔助判斷的方法，其特征在于包括步驟如 下： 步驟A1 :根據(jù)在軌注入的飛船和目標的相對位置和相對姿態(tài)信息初始化相對導(dǎo)航敏 感器的相對位置和相對姿態(tài)估值； 步驟A2 :根據(jù)飛船陀螺、加速度計測量的脈沖數(shù)計算飛船當前周期的三軸姿態(tài)角速度 和三軸線速度； 步驟A3 :獲取當前相對導(dǎo)航敏感器的相對位置和相對姿態(tài)的測量值并對測量值進行 濾波； 步驟A4 :對當前周期的相對導(dǎo)航敏感器的狀態(tài)進行判讀得到導(dǎo)航敏感器狀態(tài)的判讀 信息，所述的判讀信息包括相對導(dǎo)航敏感器數(shù)據(jù)可用標志、相對導(dǎo)航敏感器導(dǎo)航允許標志 和相對導(dǎo)航敏感器濾波收斂標志三個狀態(tài)標志和相對導(dǎo)航敏感器濾波計數(shù)器值；所述相對 導(dǎo)航敏感器數(shù)據(jù)可用標志表示相對導(dǎo)航敏感器的相對位置和相對姿態(tài)的測量值在通信信 道中傳輸正確的標志；所述相對導(dǎo)航敏感器導(dǎo)航允許標志表不相對導(dǎo)航敏感器在一定時間 內(nèi)測量數(shù)據(jù)比較穩(wěn)定時，相對導(dǎo)航敏感器的濾波過程可以進行的標志；所述相對導(dǎo)航敏感 器濾波收斂標志表示當前周期相對導(dǎo)航敏感器測量的相對位置和相對姿態(tài)有效的標志；所 述相對導(dǎo)航敏感器濾波計數(shù)器值表示相對導(dǎo)航敏感器狀態(tài)判讀過程的計數(shù)器； 步驟A5 :遙測下傳步驟A4得到的當前周期的相對導(dǎo)航敏感器狀態(tài)的判讀信息，并根據(jù) 判讀信息對相對導(dǎo)航敏感器的工作狀態(tài)進行判斷。
2. 根據(jù)權(quán)利要求1所述的基于在軌數(shù)據(jù)進行相對導(dǎo)航敏感器狀態(tài)輔助判斷的方法，其 特征在于：所述步驟A3中對當前相對導(dǎo)航敏感器的相對位置和相對姿態(tài)的測量值進行濾 波的具體方法如下： 步驟B1 :判斷相對導(dǎo)航敏感器導(dǎo)航允許標志是否有效，若有效則轉(zhuǎn)步驟B2,否則退出 濾波過程進入步驟B8 ; 步驟B2 :利用空間動力學(xué)算法，根據(jù)步驟A2計算出的飛船當前周期的三軸姿態(tài)角速度 和三軸線速度以及上一周期的相對導(dǎo)航敏感器的相對位置和姿態(tài)估值，計算當前周期的相 對導(dǎo)航敏感器的相對位置和姿態(tài)的估值；若為首次濾波，所述的上一周期相對導(dǎo)航敏感器 的相對位置和姿態(tài)估值為步驟A1中的初始化的相對位置和姿態(tài)估值，否則所述的上一周 期相對導(dǎo)航敏感器的相對位置和姿態(tài)估值為上一周期濾波過程中計算出的相對導(dǎo)航敏感 器的相對位置和姿態(tài)估值； 步驟B3 :判斷相對導(dǎo)航敏感器數(shù)據(jù)可用標志是否有效，若有效則轉(zhuǎn)步驟B4,否則退出 濾波過程進入步驟B8 ; 步驟Μ :計算當前周期的相對導(dǎo)航敏感器的相對位置和相對姿態(tài)的測量值與步驟B2 中計算出的估值的差值，并將差值的絕對值與剃野閾值比較，若相對位置或相對姿態(tài)的測 量值與估值的差值絕對值大于剃野閾值，則判斷當前周期的相對位置或相對姿態(tài)測量值無 效，轉(zhuǎn)步驟Β5,否則轉(zhuǎn)步驟Β6 ;所述剃野閾值表示相對位置和相對姿態(tài)的測量值的浮動范 圍； 步驟Β5 :置相對導(dǎo)航敏感器數(shù)據(jù)可用標志無效，并退出濾波過程，進入步驟Β8 ; 步驟Β6 :將步驟Μ中計算出的相對位置和相對姿態(tài)的測量值與估值的差值的絕對值 與限幅閾值比較，若相對位置或相對姿態(tài)的測量值與估值的差值的絕對值大于限幅閾值， 則將限幅閾值賦值給該差值，否則不做任何處理直接進入步驟B7 ;所述限幅閾值表示測量 值的浮動范圍； 步驟B7 :利用當前周期的相對導(dǎo)航敏感器的相對位置和相對姿態(tài)的估值與步驟B6計 算出的差值求和，并利用該求和結(jié)果更新當前周期相對導(dǎo)航敏感器位置姿態(tài)估值； 步驟B8 :結(jié)束。
3. 根據(jù)權(quán)利要求2所述的基于在軌數(shù)據(jù)進行相對導(dǎo)航敏感器狀態(tài)輔助判斷的方法，其 特征在于：所述步驟Μ中的剃野閾值和所述步驟B6中的限幅閾值的具體數(shù)值根據(jù)具體應(yīng) 用而定。
4. 根據(jù)權(quán)利要求1所述的基于在軌數(shù)據(jù)進行相對導(dǎo)航敏感器狀態(tài)輔助判斷的方法，其 特征在于：所述步驟Α4中相對導(dǎo)航敏感器狀態(tài)判讀過程的具體步驟如下： 步驟C1 :判斷相對導(dǎo)航敏感器導(dǎo)航允許標志是否有效，若無效則轉(zhuǎn)步驟C2,否則轉(zhuǎn)步 驟C7 ; 步驟C2 :判斷相對導(dǎo)航敏感器數(shù)據(jù)可用標志是否有效，若有效則轉(zhuǎn)步驟C3,否則轉(zhuǎn)步 驟C4 ; 步驟C3 :將相對導(dǎo)航敏感器濾波計數(shù)器加1并進入步驟C5 ; 步驟C4 :將相對導(dǎo)航敏感器濾波計數(shù)器清零并進入步驟C5 ; 步驟C5 :判斷相對導(dǎo)航敏感器濾波計數(shù)器的值是否大于閾值a，若大于則轉(zhuǎn)步驟C6,否 則轉(zhuǎn)步驟C7 ; 步驟C6 :置相對導(dǎo)航敏感器導(dǎo)航允許標志有效以及置相對導(dǎo)航敏感器濾波收斂標志 無效； 步驟C7 :若相對導(dǎo)航敏感器導(dǎo)航允許標志有效，且相對導(dǎo)航敏感器濾波收斂標志無 效，則轉(zhuǎn)步驟C8,否則轉(zhuǎn)步驟C15 ; 步驟C8 :若相對導(dǎo)航敏感器數(shù)據(jù)可用標志有效，則轉(zhuǎn)步驟C9,否則轉(zhuǎn)步驟CIO ; 步驟C9 :將相對導(dǎo)航敏感器濾波計數(shù)器加1 ; 步驟C10 :將相對導(dǎo)航敏感器濾波計數(shù)器減1 ; 步驟Cl 1 :判斷相對導(dǎo)航敏感器濾波計數(shù)器的值是否大于閾值b，若大于則轉(zhuǎn)步驟C12， 否則轉(zhuǎn)步驟C13 ; 步驟C12 :置相對導(dǎo)航敏感器濾波收斂標志有效； 步驟C13 :判斷相對導(dǎo)航敏感器濾波計數(shù)器的值是否小于閾值c，若小于則轉(zhuǎn)步驟C14， 否則退出相對導(dǎo)航敏感器狀態(tài)判讀過程并進入步驟C21 ; 步驟C14 :置相對導(dǎo)航敏感器導(dǎo)航允許標志無效，相對導(dǎo)航敏感器濾波收斂標志無效， 并將相對導(dǎo)航敏感器濾波計數(shù)器清零并進入步驟C21 ; 步驟C15 :判斷相對導(dǎo)航敏感器濾波收斂標志是否有效，若有效則轉(zhuǎn)步驟C16,否則退 出相對導(dǎo)航敏感器狀態(tài)判讀過程并進入步驟C21 ; 步驟C16 :判斷相對導(dǎo)航敏感器數(shù)據(jù)可用標志是否，若有效則轉(zhuǎn)步驟C17,否則，轉(zhuǎn)步驟 C18 ； 步驟C17 :將相對導(dǎo)航敏感器濾波計數(shù)器加1 ; 步驟C18 :將相對導(dǎo)航敏感器濾波計數(shù)器減1 ; 步驟C19 :判斷相對導(dǎo)航敏感器濾波計數(shù)器的值是否小于閾值d，若小于則轉(zhuǎn)步驟C20， 否則退出相對導(dǎo)航敏感器狀態(tài)判讀過程并進入步驟C21 ; 步驟C20 :置相對導(dǎo)航敏感器導(dǎo)航允許標志無效，相對導(dǎo)航敏感器濾波收斂標志無效， 并將相對導(dǎo)航敏感器濾波計數(shù)器清零； 步驟C21 :結(jié)束。
5. 根據(jù)權(quán)利要求4所述的基于在軌數(shù)據(jù)進行相對導(dǎo)航敏感器狀態(tài)輔助判斷的方法，其 特征在于：所述步驟C5中的閾值a、所述步驟C11中的閾值b、所述步驟C13中的閾值c和 所述步驟C19中的閾值d均根據(jù)具體應(yīng)用情況而定。
6. 根據(jù)權(quán)利要求1所述的基于在軌數(shù)據(jù)進行相對導(dǎo)航敏感器狀態(tài)輔助判斷的方法，其 特征在于：所述步驟A5中根據(jù)判斷信息對相對導(dǎo)航敏感期的工作狀態(tài)進行判斷的具體方 法如下： (1) 若相對導(dǎo)航敏感器數(shù)據(jù)可用標志有效，則表示相對導(dǎo)航敏感器的相對位置和相對 姿態(tài)的測量值在通信信道中傳輸正確，否則表示測量值在通信信道中傳輸錯誤； (2) 若相對導(dǎo)航敏感器導(dǎo)航允許標志有效，貝U表不相對導(dǎo)航敏感器在一定時間內(nèi)相對 位置和相對姿態(tài)的測量值比較穩(wěn)定，相對導(dǎo)航敏感器的濾波過程正在進行；否則表示一定 時間內(nèi)相對導(dǎo)航敏感器測量值不穩(wěn)，飛船需等待相對導(dǎo)航敏感器測量值穩(wěn)定后才能開始進 行測量值濾波； (3) 若相對導(dǎo)航敏感器濾波收斂標志有效，則表示當前相對導(dǎo)航敏感器測量狀態(tài)有效； 若相對導(dǎo)航敏感器濾波收斂標志無效，則表示當前相對導(dǎo)航敏感器測量狀態(tài)無效。
【文檔編號】G01C21/00GK104061926SQ201410286240
【公開日】2014年9月24日 申請日期:2014年6月24日 優(yōu)先權(quán)日:2014年6月24日
【發(fā)明者】張國峰, 蔡雨辰, 王振華, 陳朝暉, 奚坤, 黨紀紅, 張維瑾, 張韓笑, 楊俊春, 黎藜, 魏高樂 申請人:北京控制工程研究所