專利名稱：小天體撞擊探測自主導航與制導控制規(guī)劃調(diào)度方法
技術(shù)領域：
本發(fā)明涉及一種深空探測器的自主導航與控制規(guī)劃調(diào)度方法。
背景技術(shù)：
自主導航與制導控制系統(tǒng)通過軟件實現(xiàn)在不需地面站支持就能自主確定探測器 的狀態(tài)，并導引探測器完成預定撞擊任務。小天體撞擊探測飛行距離遠，運行階段多，各階 段所處環(huán)境差異大，這要求導航與制導控制系統(tǒng)需要完成復雜繁瑣的在軌操作，以保證工 程任務與科學任務的順利實現(xiàn)。繁雜的在軌操作使星載軟件因任務多而使規(guī)模變大，各任 務有不同的實時性要求，任務間的聯(lián)系增多。以往進行導航、制導與控制系統(tǒng)的軟件結(jié)構(gòu)設 計時主要針對單任務或少量任務，一般采用順序編程加中斷方法。系統(tǒng)的復雜化及對實時 性的高要求使傳統(tǒng)串行的軟件設計方式不再滿足實際應用需求，如果仍然用傳統(tǒng)星載軟件 的設計方法、開發(fā)手段，則系統(tǒng)設計時頭緒多、結(jié)構(gòu)復雜、條理不清，而且很難借鑒其它軟件 的先進經(jīng)驗不利于探測器技術(shù)的延續(xù)發(fā)展。同時為了擴大探測任務的適應性和探測器的功 能，以及便于后續(xù)探測任務的繼承性，需要自主導航與制導控制系統(tǒng)軟件要具有較強的可 替換性及可擴展性。嵌入式實時操作系統(tǒng)(RTOS)在此方面則有著其突出的優(yōu)點。VxWorks 操作系統(tǒng)就是一種嵌入式實時操作系統(tǒng)，提供了高效的實時任務調(diào)度、中斷管理、實時的系 統(tǒng)資源以及任務間通信等功能。高性能的操作系統(tǒng)微內(nèi)核Wind支持實時特性，其設計減少 了系統(tǒng)開銷，高效的任務管理保證了對外部事件快速、準確的反應；快速靈活的任務間和進 程間通信允許獨立的任務在實時系統(tǒng)中與其行動相協(xié)調(diào)。VxWorks嵌入式實時操作系統(tǒng)經(jīng) 過廣泛的驗證，已成功的應用在航空、航天中的關鍵領域。

發(fā)明內(nèi)容
本發(fā)明為了讓解決現(xiàn)有技術(shù)的自主導航與制導控制規(guī)劃調(diào)度方法存在任務眾多、 可替換性差、可擴展性差的問題，提出一種小天體撞擊探測自主導航與制導控制規(guī)劃調(diào)度 方法。小天體撞擊探測自主導航與制導控制規(guī)劃調(diào)度方法，它由主任務mairutask和四 個子任務實現(xiàn)，所述四個子任務為GNC規(guī)劃任務Task_GNC_pr0、數(shù)據(jù)采集任務Task_SAM_X、 GNC任務Task_GNC和軌道確定任務；主任務mainjask的優(yōu)先級最高，用于完成各部分的初始化，設置系統(tǒng)時鐘中斷 頻率、系統(tǒng)當前時間及任務調(diào)度策略，完成對各二進制信號量、子任務及定時器的創(chuàng)建，并 在獲取得到相關信號量時，啟動與所述信號量相關的定時器，結(jié)束相關任務；GNC規(guī)劃任務Task_GNC_pro的優(yōu)先級僅次于主任務main_task，用于在每個執(zhí)行 周期開始時，根據(jù)當前撞擊器的軌道和姿態(tài)狀態(tài)量，給出探測器軌道機動標識位flag_mot_ orb、探測器姿態(tài)機動標識位flag_mot_ati和機動點火標識位flag_mot_ign，為確定同周 期GNC任務Task_GNC下的功能函數(shù)的執(zhí)行順序提供判斷依據(jù)；數(shù)據(jù)采集任務Task_SAM_X的優(yōu)先級僅次于GNC規(guī)劃任務Task_GNC_pr0，用于在不同采樣周期讀取各敏感器的測量數(shù)據(jù)；所述數(shù)據(jù)采集任務Task_SAM_X包括4個數(shù)據(jù)采集子 任務，分別為速率陀螺數(shù)據(jù)采集任務TaSk_SAM_0mega、加速度計數(shù)據(jù)采集任務Task_SAM_ acel,星敏感器數(shù)據(jù)采集任務Task_SAM_q和光學導航相機數(shù)據(jù)采集任務Task_SAM_p ；所述 4個子任務的優(yōu)先級相同，所述4個子任務間采用時間片輪轉(zhuǎn)調(diào)度算法進行調(diào)度執(zhí)行；GNC任務Task_GNC的優(yōu)先級僅次于數(shù)據(jù)采集任務Task_SAM_X，根據(jù)GNC規(guī)劃任務 Task_GNC_pro給出的各個標志位的組合來確定飛行模式，并根據(jù)數(shù)據(jù)采集任務Task_SAM_ X得到的當前時刻敏感器更新數(shù)據(jù)，完成在每個執(zhí)行周期內(nèi)對撞擊器狀態(tài)的導航、制導與控 制任務；軌道確定任務Task_0RB_fuse的優(yōu)先級最低，所述軌道確定任務Task_0RB_fuse 每隔15s進行一次，在執(zhí)行任務時，首先判斷軌道和姿態(tài)是否有機動的情況，當軌道和姿態(tài) 均無機動，則小天體處于穩(wěn)定狀態(tài)，利用光學導航相機數(shù)據(jù)對小天體軌道參數(shù)進行修正；否 則退出任務。本發(fā)明針對小天體撞擊探測任務，應用VxWorks嵌入式實時操作系統(tǒng)，提出一種 小天體撞擊探測自主導航與制導控制規(guī)劃調(diào)度方法。對自主導航與制導控制系統(tǒng)的控制模 式進行設計；在此基礎上，綜合考慮小天體撞擊探測任務的實時性要求、不同飛行模式的耦 合關系、軌道確定的數(shù)學運算量以及對不同敏感器數(shù)據(jù)采集的周期性控制等多方面因素， 對小天體撞擊探測任務進行模塊化分解，提出各個任務模塊間的同步方式與通信手段，最 終完成小天體撞擊探測任務。


圖1為是小天體撞擊探測自主導航與制導控制規(guī)劃調(diào)度方法單個執(zhí)行周期的任 務調(diào)度流程與函數(shù)執(zhí)行示意圖。圖2為主任務mairutask實現(xiàn)流程圖；圖3為GNC規(guī)劃任 務Task_GNC_pr0實現(xiàn)流程圖；圖4為速率陀螺數(shù)據(jù)采集任務Task_SAM_0mega實現(xiàn)流程圖； 圖5為加速度計數(shù)據(jù)采集任務Task_SAM_acel實現(xiàn)流程圖；圖6為星敏感器數(shù)據(jù)采集任務 Task_SAM_q實現(xiàn)流程圖；圖7為光學導航相機數(shù)據(jù)采集任務Task_SAM_p實現(xiàn)流程圖；圖8 為GNC任務Task_GNC實現(xiàn)流程圖；圖9為首次執(zhí)行GNC任務Task_GNC的流程圖；圖10為 第二次執(zhí)行GNC任務Task_GNC的流程圖；圖11為第二次以后執(zhí)行GNC任務Task_GNC的流 程圖；圖12為軌道確定任務Task_0RB_fuSe實現(xiàn)流程圖；圖13為25ms定時器處理函數(shù)流 程圖；圖14為2s定時器處理函數(shù)流程圖；圖15為15s定時器處理函數(shù)流程圖。
具體實施例方式具體實施方式
一、結(jié)合圖1說明本實施方式，小天體撞擊探測自主導航與制導控 制規(guī)劃調(diào)度方法，它由主任務mairutask和四個子任務實現(xiàn)，所述四個子任務為GNC規(guī)劃任 務Task_GNC_pro、數(shù)據(jù)采集任務Task_SAM_X、GNC任務Task_GNC和軌道確定任務；主任務mainjask的優(yōu)先級最高，用于完成各部分的初始化，設置系統(tǒng)時鐘中斷 頻率、系統(tǒng)當前時間及任務調(diào)度策略，完成對各二進制信號量、子任務及定時器的創(chuàng)建，并 在獲取得到相關信號量時，啟動與所述信號量相關的定時器，結(jié)束相關任務；GNC規(guī)劃任務Task_GNC_pro的優(yōu)先級僅次于主任務main_task，用于在每個執(zhí)行 周期開始時，根據(jù)當前撞擊器的軌道和姿態(tài)狀態(tài)量，給出探測器軌道機動標識位flag_mot_orb、探測器姿態(tài)機動標識位flag_mot_ati和機動點火標識位flag_mot_ign，為確定同周 期GNC任務Task_GNC下的功能函數(shù)的執(zhí)行順序提供判斷依據(jù)；數(shù)據(jù)采集任務Task_SAM_X的優(yōu)先級僅次于GNC規(guī)劃任務Task_GNC_pr0，用于在不 同采樣周期讀取各敏感器的測量數(shù)據(jù)；所述數(shù)據(jù)采集任務Task_SAM_X包括4個數(shù)據(jù)采集子 任務，分別為速率陀螺數(shù)據(jù)采集任務TaSk_SAM_0mega、加速度計數(shù)據(jù)采集任務Task_SAM_ acel,星敏感器數(shù)據(jù)采集任務Task_SAM_q和光學導航相機數(shù)據(jù)采集任務Task_SAM_p ；所述 4個子任務的優(yōu)先級相同，所述4個子任務間采用時間片輪轉(zhuǎn)調(diào)度算法進行調(diào)度執(zhí)行；GNC任務Task_GNC的優(yōu)先級僅次于數(shù)據(jù)采集任務Task_SAM_X，根據(jù)GNC規(guī)劃任務 Task_GNC_pro給出的各個標志位的組合來確定飛行模式，并根據(jù)數(shù)據(jù)采集任務Task_SAM_ X得到的當前時刻敏感器更新數(shù)據(jù)，完成在每個執(zhí)行周期內(nèi)對撞擊器狀態(tài)的導航、制導與控 制任務；軌道確定任務Task_0RB_fuse的優(yōu)先級最低，所述軌道確定任務Task_0RB_fuse 每隔15s進行一次，在執(zhí)行任務時，首先判斷軌道和姿態(tài)是否有機動的情況，當軌道和姿態(tài) 均無機動，則小天體處于穩(wěn)定狀態(tài)，利用光學導航相機數(shù)據(jù)對小天體軌道參數(shù)進行修正；否 則退出任務。軌道確定任務受到相機圖像處理耗時長及傳輸延時的限制，在滿足拍照條件的情 況下，軌道確定每15s進行一次，以提高導航精度。時間片輪轉(zhuǎn)調(diào)度算法的基本思想是，在更高優(yōu)先級任務調(diào)度依然優(yōu)先運行的前提 下，同優(yōu)先級任務之間調(diào)度時追求一定意義下的公平。一組任務中的每個任務執(zhí)行一個預 先確定的時間段，成為一個時間片；然后另一個任務執(zhí)行相等的一個時間片，一次進行。這 種分配是公正的，它保證一個優(yōu)先級組中，在所有任務都得到一個時間片之前，不會有任務 得到第二個時間片。更準確的說，如果使用輪轉(zhuǎn)調(diào)度算法，系統(tǒng)中的每個任務都有一個運行 時間計數(shù)器。這個計數(shù)器隨系統(tǒng)始終增加而增加。當達到規(guī)定的時間片的值時，也就是說 一個規(guī)定的時間片已經(jīng)完成，這個計數(shù)器清零，時間片輪轉(zhuǎn)調(diào)度算法將這個任務放到相應 任務優(yōu)先級隊列的尾部，將CPU交給對頭的任務執(zhí)行。新加入一個優(yōu)先級組隊列的任務將 放到隊列的尾部，計數(shù)器初始化為0。如果任務在它的時間片中被高優(yōu)先級的任務搶占，時 間片輪轉(zhuǎn)調(diào)度算法保存它的運行時間計數(shù)器，當它再一次符合執(zhí)行條件的時候，時間片輪 轉(zhuǎn)調(diào)度算法恢復運行時間計數(shù)器。
具體實施方式
二、結(jié)合圖2說明本實施方式，本實施方式是對具體實施方式
一中 的主任務main_task的進一步說明主任務main_task的工作過程如下步驟1A、初始化時間規(guī)格、定時器信號事件、定時器ID標識、定時器信號處理和任 務ID標識；步驟1B、設置系統(tǒng)時鐘終端頻率和任務調(diào)度策略，所述系統(tǒng)時鐘終端頻率為 200Hz，所述任務調(diào)度策略為采用基于任務優(yōu)先級的時間片輪轉(zhuǎn)換的調(diào)度方法；步驟1C、設置當前時間；步驟1D、創(chuàng)建二進制信號量，所述二進制信號量包括狀態(tài)指標信號量semaphore、 主任務更新同步數(shù)據(jù)信號量semblsam、GNC規(guī)劃任務更新同步數(shù)據(jù)信號量Semb2_Sam、 GNC任務更新同步數(shù)據(jù)信號量Semb3_Sam、更新星敏感器數(shù)據(jù)信號semb_q、更新速率陀螺 數(shù)據(jù)信號量semb^mega、更新光學導航相機數(shù)據(jù)信號量semb_p、更新加速度計數(shù)據(jù)信號Semb_acel、速率陀螺控制采樣周期信號量semb_sam_0mega、加速度計控制采樣周期信號量 Semb_Sam_acel、星敏感器控制采樣周期信號量Semb_Sam_q、星敏感器控制采樣周期臨時信 號量Semb_Sam_q_temp、光學導航相機控制采樣周期信號量Semb_Sam_p、光學導航相機控 制采樣周期臨時信號量semb_sam_p_temp和控制GNC規(guī)劃任務Task_GNC_pro每25ms最先 執(zhí)行信號量，所述控制GNC規(guī)劃任務Task_GNC_pro每25ms最先執(zhí)行信號量包括25ms執(zhí)行 周期執(zhí)行完畢信號量semb和25ms定時器到期信號量semb_tm ；步驟1E、創(chuàng)建四個子任務GNC規(guī)劃任務Task_GNC_pro、數(shù)據(jù)采集任務Task_SAM_ X、GNC任務Task_GNC和軌道確定任務Task_0RB_fuse ；步驟1F、設置定時器信號處理函數(shù)，分別為25ms定時器處理任務timejiandler、 2s定時器處理任務time_handler_one和15s定時器處理任務time_handler_two ；步驟1G、創(chuàng)建定時器；步驟1H、獲取主任務更新同步數(shù)據(jù)信號量semblsam的內(nèi)容；步驟II、啟動定時器，設定三個采樣周期分別為25ms、2s和15s ；步驟1J、獲取狀態(tài)指標信號量semaphore的內(nèi)容；步驟1K、當撞擊器狀態(tài)滿足撞擊條件時，進行信號量的刪除、定時器的刪除、任務 的刪除操作。主任務mairutask的優(yōu)先級(101)最高，用于完成各部分的初始化，設置系統(tǒng)時鐘 中斷頻率(200Hz)及系統(tǒng)當前時間，設置任務調(diào)度策略(基于任務優(yōu)先級的時間片輪轉(zhuǎn)調(diào) 度)，完成對各信號量、子任務及定時器的創(chuàng)建，并在分別獲取得到相關信號量后，啟動定時 器及進行任務結(jié)束處理。步驟ID中二進制信號量的初始值為empty，表示不可用，阻塞在信號量上，按照先 入先出順序等待信號量。更新敏感器數(shù)據(jù)信號量主任務更新同步數(shù)據(jù)信號量semblsam用于保證目標機 與外部接口啟動同步，即在外部系統(tǒng)啟動后，目標機第一次接收到外部接口的傳輸數(shù)據(jù)后， 再啟動目標機定時器開始計時，控制數(shù)據(jù)采集周期及任務執(zhí)行周期；獲取狀態(tài)指標信號量 semaphore用于保證撞擊探測器的狀態(tài)量達到要求指標后，再進行任務結(jié)束相關處理。
具體實施方式
三、結(jié)合圖3說明本實施方式，本實施方式是對具體實施方式
一中 的GNC規(guī)劃任務Task_GNC_pro的進一步說明所述GNC規(guī)劃任務Task_GNC_pro的工作過 程如下步驟2A、判斷是否為首次執(zhí)行，用i表示執(zhí)行次數(shù)，如果是首次執(zhí)行，用i = 0表 示，執(zhí)行步驟2B ；如果不是首次執(zhí)行，則執(zhí)行步驟2C ；步驟2B、獲取系統(tǒng)當前時間；然后執(zhí)行步驟2G ；步驟2C、判斷是否為第一個執(zhí)行周期，即判斷i是否為1 ；如果是，則執(zhí)行步驟2D ； 如果否，則執(zhí)行步驟2E;步驟2D、獲取更新敏感器數(shù)據(jù)信號量GNC規(guī)劃任務更新同步數(shù)據(jù)信號量semb2_ sam的內(nèi)容；然后執(zhí)行步驟2F;步驟2E、獲取25ms執(zhí)行周期執(zhí)行完畢信號量semb和25ms定時器到期信號量 semb_tm的內(nèi)容；當25ms執(zhí)行周期執(zhí)行完畢信號量semb或25ms定時器到期信號量semb_ tm有效時；
步驟2F、計算首次執(zhí)行中等待外部仿真機dSpace啟動運算及數(shù)據(jù)傳輸時間，并利 用其對系統(tǒng)時間進行修正；步驟2G、根據(jù)探測器估計位置、探測器估計速度、探測器估計姿態(tài)、估計角速度、探 測器期望姿態(tài)、探測器期望角速度、期望增量速度、加速度測量值和探測器上一時刻所處飛 行狀態(tài)，計算出探測器軌道機動標識位flag_mot_orb、探測器姿態(tài)機動標識位flag_m0t_ ati、機動點火標識位flag_m0t_ign，從而判斷當前探測器所處的飛行狀態(tài)；步驟2H、令執(zhí)行次數(shù)i = i+Ι，本次任務執(zhí)行完畢，將25ms執(zhí)行周期執(zhí)行完畢信號 量semb置位有效。GNC規(guī)劃任務Task_GNC_pro的優(yōu)先級(101)僅次于主任務main_task，旨在每個 25ms執(zhí)行周期的首部，根據(jù)當前撞擊探測器的軌道/姿態(tài)狀態(tài)量，給出軌道/姿態(tài)/點火機 動標志位，為同周期GNC任務Task_GNC下的功能函數(shù)執(zhí)行順序提供判斷依據(jù)。任務流程圖 如圖7所示。判斷條件i = 0的設置是為解決目標機與外部接口啟動同步問題所必需的，在目 標機啟動后首次執(zhí)行各任務時并不與外部接口發(fā)生數(shù)據(jù)傳輸關系，而是利用自帶數(shù)據(jù)初值 進行一步運算，為與外部接口同步運行做準備；出于相同目的設置判斷條件“是否為第一執(zhí) 行周期”用于保證目標機與外部接口的啟動同步，即在外部系統(tǒng)啟動后，目標機第一次接收 到外部接口的傳輸數(shù)據(jù)后( 此處實現(xiàn)表現(xiàn)為獲取得到信號量GNC規(guī)劃任務更新同步數(shù)據(jù)信 號量semb2_sam)，再進入第一個25ms執(zhí)行周期的任務規(guī)劃；而信號量semb_tm則是為了保 證GNC規(guī)劃任務Task_GNC_pr0在每一個25ms執(zhí)行周期的首部占用CPU運行，信號量semb 用于保證上一個25ms執(zhí)行周期完成所有任務的運行，再進入下一個執(zhí)行周期；為了使目標 機中的計算所用時間與外部系統(tǒng)的運算時間一致，在每一個執(zhí)行周期中，GNC任務規(guī)劃要把 目標機系統(tǒng)時間中用于保證兩機同步啟動的首次執(zhí)行GNC任務Task_GNC后等待外部接口 傳輸數(shù)據(jù)的時間修正掉。外部仿真機dSpace主要模擬探測器動力學部分。
具體實施方式
四、本實施方式是對具體實施方式
一中的數(shù)據(jù)采集任務Task_SAM_X 的進一步說明數(shù)據(jù)采集任務Task_SAM_X中的4個數(shù)據(jù)采集子任務為定時器觸發(fā)的工作方 式，其中速率陀螺數(shù)據(jù)采集任務TaSk_SAM_0mega和加速度計數(shù)據(jù)采集任務Task_SAM_acel 的數(shù)據(jù)采樣周期為25ms，；星敏感器數(shù)據(jù)采集任務Task_SAM_q的采樣周期為2s，在執(zhí)行星敏感器數(shù)據(jù)采集 任務Task_SAM_q的時候，首先判斷軌道和姿態(tài)是否有機動的情況，如果軌道或姿態(tài)有機 動，則退出任務本任務，否則采集星敏感器數(shù)據(jù)；光學導航相機數(shù)據(jù)采集任務Task_SAM_p的采樣周期為15s，在執(zhí)行光學導航相機 數(shù)據(jù)采集任務Task_SAM_p的時候，首先判斷軌道和姿態(tài)是否有機動情況，當軌道和姿態(tài)均 無機動情況下執(zhí)行數(shù)據(jù)采集任務，并記錄所述相機拍照時刻的軌道/姿態(tài)狀態(tài)信息，否則 退出任務。數(shù)據(jù)采集任務Task_SAM_X的優(yōu)先級(102)僅次于GNC規(guī)劃任務Task_GNC_pr0，用 于在不同采樣周期讀取各敏感器的測量數(shù)據(jù)。
具體實施方式
五、結(jié)合圖4說明本實施方式，本實施方式是對具體實施方式
四中 的速率陀螺數(shù)據(jù)采集任務TaSk_SAM_0mega的進一步說明，速率陀螺數(shù)據(jù)采集任務Task_CN
SAM_omega的工作過程如下步驟3A、獲取速率陀螺控制采樣周期信號量Semb_Sam_0mega ；步驟3B、將采集到的陀螺數(shù)據(jù)寫入數(shù)據(jù)緩沖區(qū)；步驟3C、釋放更新陀螺數(shù)據(jù)信號量Semb_0mega。
具體實施方式
六、結(jié)合圖5說明本實施方式，本實施方式是對具體實施方式
四中 的加速度計數(shù)據(jù)采集任務Task_SAM_acel的進一步說明，加速度計數(shù)據(jù)采集任務Task_ SAM_acel的工作過程如下步驟3D、獲取加速度計控制采樣周期信號量Semb_Sam_acel ；步驟3E、將采集到的加速度計數(shù)據(jù)寫入數(shù)據(jù)緩沖區(qū)；步驟3F、釋放更新加速度計數(shù)據(jù)信號Semb_acel。
具體實施方式
七、結(jié)合圖6說明本實施方式，本實施方式是對具體實施方式
四中 的星敏感器數(shù)據(jù)采集任務Task_SAM_q的進一步說明，星敏感器數(shù)據(jù)采集任務Task_SAM_q 的工作過程如下步驟3G、獲取星敏感器控制采樣周期信號量Semb_Sam_q和星敏感器控制采樣周 期臨時信號量semb_sam_q_temp ；步驟3H、判斷探測器軌道機動標識位flag_m0t_0rb和探測器姿態(tài)機動標識位 flag_mot_ati的內(nèi)容，如果探測器軌道機動標識位flag_m0t_0rb = 0并且探測器姿態(tài)機動 標識位flag_mot_ati = 0，則執(zhí)行步驟31 ；否則，不做任何處理，本次任務結(jié)束；步驟31、將調(diào)用姿態(tài)確定功能函數(shù)的標志位flag_tm的內(nèi)容置1，即令flag_tm = 1 ；步驟3J、將采集到的陀螺數(shù)據(jù)寫入數(shù)據(jù)緩沖區(qū)；步驟3K、釋放更新星敏感器數(shù)據(jù)信號semb_q。
具體實施方式
八、結(jié)合圖7說明本實施方式，本實施方式是對具體實施方式
四中 的光學導航相機數(shù)據(jù)采集任務Task_SAM_p的進一步說明，光學導航相機數(shù)據(jù)采集任務 Task_SAM_p的工作過程如下步驟3L、獲取光學導航相機控制采樣周期信號量Semb_Sam_p和光學導航相機控 制采樣周期臨時信號量semb_sam_p_temp ；步驟3M、判斷探測器軌道機動標識位flag_m0t_OTb和探測器姿態(tài)機動標識位 flag_mot_ati的內(nèi)容，如果探測器軌道機動標識位flag_m0t_0rb = 0并且探測器姿態(tài)機動 標識位flag_mot_ati = 0，則執(zhí)行步驟3N ；否則，不做任何處理，本次任務結(jié)束；步驟3N、獲取拍照時刻系統(tǒng)時間，并記錄拍照時刻對應的撞擊軌道/姿態(tài)狀態(tài)量， 所述撞擊軌道/姿態(tài)狀態(tài)量包括撞擊器位置和速度；步驟30、將采集到的光學導航相機數(shù)據(jù)寫入數(shù)據(jù)緩沖區(qū)；步驟3P、釋放更新光學導航相機數(shù)據(jù)信號量semb_p。
具體實施方式
九、結(jié)合圖8、圖9、圖10和圖11說明本實施方式，本實施方式是對具體實施方式
一中的GNC任務Task_GNC的進一步說明，GNC任務Task_GNC —個周期內(nèi)的 具體步驟如下步驟4A、判斷是否為首次執(zhí)行，如果是，則執(zhí)行步驟4B，如果否，則執(zhí)行步驟4L ；首次執(zhí)行GNC任務Task_GNC情況如下
14
步驟4B、根據(jù)軌道面法線方向矢量、探測器估計姿態(tài)和探測器估計角速度，計算 出探測器期望的姿態(tài)余弦陣和探測器三軸期望角速度，從而獲得探測器定向姿態(tài)的期望姿 態(tài)；步驟4C、判斷探測器姿態(tài)機動標識位flag_m0t_ati的內(nèi)容， 如果flag_mot_ati = 0，則執(zhí)行步驟4D,否則，即flag_mot_ati = 1，則執(zhí)行步驟4E ；步驟4D、根據(jù)探測器估計姿態(tài)、探測器估計角速度、探測器期望姿態(tài)和探測器期望 角速度，計算出反作用飛輪控制信號，從而獲得反作用飛輪控制量；然后執(zhí)行步驟4G ；步驟4E、根據(jù)探測器估計姿態(tài)、探測器估計角速度、探測器期望姿態(tài)和探測器期望 角速度，計算出三軸噴嘴開關序列，從而獲得噴嘴開關的邏輯；步驟4F、將系統(tǒng)當前時間結(jié)構(gòu)體數(shù)據(jù)中的秒級分量t_Current. tv_sec加100，并 賦值給所需的穩(wěn)定時間t_ati ；即令如果t_ati = t_current. tv_sec+100 ；步驟4G、根據(jù)陀螺輸出的姿態(tài)角速度、陀螺漂移更新值和上一時刻探測器估計姿 態(tài)，計算出探測器估計姿態(tài)和探測器估計角速度，從而遞推出探測器相對慣性空間的姿態(tài) 矩陣；步驟4H、根據(jù)探測器估計姿態(tài)、探測器預測位置、探測器預測速度、加速度測量值 和機動點火標識位，計算出探測器估計位置和探測器估計速度，從而遞推出探測器位置和 速度；步驟41、將計算得到的指令寫入相應的指令緩沖區(qū)；步驟4J、接收外部仿真機dSpace動力學模塊傳入的敏感器數(shù)據(jù)，并將采集到的數(shù) 據(jù)寫入相應的數(shù)據(jù)緩沖區(qū)；步驟4K、釋放主任務更新同步數(shù)據(jù)信號量Sembl_Sam，GNC規(guī)劃任務更新同步數(shù)據(jù) 信號量semb2_Sam和GNC任務更新同步數(shù)據(jù)信號量semb3_sam ；步驟4L、判斷是否為第二次執(zhí)行；如果是，則執(zhí)行步驟4M，如果否，則執(zhí)行步驟 4Z ；第二次執(zhí)行GNC任務Task_GNC情況如下步驟4M、獲取GNC任務更新同步數(shù)據(jù)信號量semb3_sam ；步驟4N、將首次任務執(zhí)行的指令發(fā)出，并將相應指令緩沖區(qū)置0 ；步驟40、根據(jù)軌道面法線方向矢量、探測器估計姿態(tài)和探測器估計角速度，計算 出探測器期望的姿態(tài)余弦陣和探測器三軸期望角速度，從而獲得探測器定向姿態(tài)的期望姿 態(tài)；步驟4P、判斷探測器姿態(tài)機動標識位flag_m0t_ati的內(nèi)容，如果flag_mot_ati = 0，則執(zhí)行步驟4Q,否則，即flag_mot_ati = 1，則執(zhí)行步驟4R ；步驟4Q、根據(jù)探測器估計姿態(tài)、探測器估計角速度、探測器期望姿態(tài)和探測器期望 角速度，計算出反作用飛輪控制信號，從而獲得反作用飛輪控制量；然后執(zhí)行步驟4T ；步驟4R、根據(jù)探測器估計姿態(tài)、探測器估計角速度、探測器期望姿態(tài)和探測器期望 角速度，計算出三軸噴嘴開關序列，從而獲得噴嘴開關的邏輯；步驟4S、將系統(tǒng)當前時間結(jié)構(gòu)體數(shù)據(jù)中的秒級分量t_Current. tv_sec加100，并 賦值給所需的穩(wěn)定時間t_ati ；即令如果t_ati = t_current. tv_sec+100 ；步驟4T、根據(jù)陀螺輸出的姿態(tài)角速度、陀螺漂移更新值和上一時刻探測器估計姿態(tài)，計算出探測器估計姿態(tài)和探測器估計角速度，從而遞推出探測器相對慣性空間的姿態(tài) 矩陣；步驟4U、根據(jù)探測器估計姿態(tài)、探測器預測位置、探測器預測速度、加速度測量值 和機動點火標識位，計算出探測器估計位置和探測器估計速度，從而遞推出探測器位置和 速度；步驟4V、將串口通信丟數(shù)標志位flag_sam置1，即令flag_sam = 1 ；步驟4W、記錄本次執(zhí)行周期軌道機動標志位flag_m0t_0rb_te mp，令執(zhí)行周期軌 道機動標志位flag_m0t_0rb_temp的內(nèi)容為探測器軌道機動標識位flag_mot_orb的內(nèi)容， 艮口 f1ag_mot_orb_temp = flag_mot_orb ；步驟4X、將計算得到指令寫入相應的指令緩沖區(qū)，并在指令發(fā)出后，將相應的指令 緩沖區(qū)置0 ；步驟4Y、釋放25ms執(zhí)行周期執(zhí)行完畢信號量semb ；第二次之后執(zhí)行GNC任務Task_GNC情況如下步驟4Z、獲取更新陀螺數(shù)據(jù)信號量Semb_0mega和更新加速度計數(shù)據(jù)信號semb_ acel ；然后執(zhí)行步驟41 ；步驟41、判斷調(diào)用姿態(tài)確定功能函數(shù)的標志位flag_tm的內(nèi)容，如果flag_tm = 1，則執(zhí)行步驟42 ；否則，執(zhí)行步驟45 ；步驟42、將調(diào)用姿態(tài)確定功能函數(shù)的標志位flag_tm置0，即令flag_tm = 0 ；步驟43、獲取更新星敏感器數(shù)據(jù)信號semb_q ；步驟44、根據(jù)星敏感器輸出的姿態(tài)四元數(shù)、陀螺輸出的姿態(tài)角速度，計算出探測器 估計姿態(tài)、探測器估計角速度、陀螺漂移更新值，從而實現(xiàn)通過卡爾曼濾波校正撞擊探測器 姿態(tài)和陀螺漂移；步驟45、判斷探測器軌道機動標識位flag_m0t_0rb的內(nèi)容，如果flag_m0t_0rb = 0，則執(zhí)行步驟46 ；否則，即flag_mot_orb = 1，執(zhí)行步驟411 ；步驟46、根據(jù)軌道面法線方向矢量、探測器估計姿態(tài)和探測器估計角速度，計算 出探測器期望的姿態(tài)余弦陣和探測器三軸期望角速度，從而計算探測器定向姿態(tài)的期望姿 態(tài)；步驟47、判斷探測器姿態(tài)機動標識位flag_m0t_ati的內(nèi)容，如果flag_mot_ati = 0，則執(zhí)行步驟48，否則，即flag_m0t_ati = 1，則執(zhí)行步驟49 ；步驟48、根據(jù)探測器估計姿態(tài)、探測器估計角速度、探測器期望姿態(tài)和探測器期望 角速度，計算出反作用飛輪控制信號，從而獲得反作用飛輪控制量；然后執(zhí)行步驟416 ；步驟49、根據(jù)探測器估計姿態(tài)、探測器估計角速度、探測器期望姿態(tài)和探測器期望 角速度，計算出三軸噴嘴開關序列，從而獲得噴嘴開關的邏輯；然后執(zhí)行步驟410 ；步驟410、將系統(tǒng)當前時間結(jié)構(gòu)體數(shù)據(jù)中的秒級分量t_Current. tv_sec加100，并 賦值給所需的穩(wěn)定時間t_ati ；即令如果t_ati = t_current. tv_sec+100 ；然后執(zhí)行步驟 416 ；步驟411、判斷執(zhí)行周期軌道機動標志位flag_m0t_0rb_temp的內(nèi)容，如果flag_ mot_orb_temp = 0，則執(zhí)行步驟 412，否則，即 flag_mot_orb_temp = 1，則執(zhí)行步驟 413 ；步驟412、根據(jù)探測器估計位置、探測器估計速度和期望撞擊目標點，計算出期望增量速度，從而獲得接近軌道需要的增量速度；步驟413、根據(jù)探測器估計位置和制導增量速度，計算出探測器期望的姿態(tài)余弦陣 和探測器三軸期望角速度，從而獲得探測器定向姿態(tài)的期望姿態(tài)；步驟414、根據(jù)探測器估計姿態(tài)、探測器估計角速度、探測器期望姿態(tài)和探測器期 望角速度，計算出三軸噴嘴開關序列，從而獲得噴嘴開關的邏輯；步驟415、將系統(tǒng)當前時間結(jié)構(gòu)體數(shù)據(jù)中的秒級分量t_Current. tv_sec加100，并 賦值給所需的穩(wěn)定時間t_ati ；即令如果t_ati = t_current. tv_sec+100 ；然后執(zhí)行步驟 416 ；步驟416、根據(jù)陀螺輸出的姿態(tài)角速度、陀螺漂移更新值和上一時刻探測器估計姿 態(tài)，計算出探測器估計姿態(tài)和探測器估計角速度，從而獲得探測器相對慣性空間的姿態(tài)矩 陣；步驟417、根據(jù)探測器估計姿態(tài)、探測器預測位置、探測器預測速度、加速度測量值 和機動點火標識位，計算出探測器估計位置和探測器估計速度，從而獲得探測器位置和速 度；步驟418、記錄本次執(zhí)行周期軌道機動標志位f lag_m0t_0rb_temp，令執(zhí)行周期軌 道機動標志位flag_m0t_0rb_temp的內(nèi)容為探測器軌道機動標識位flag_mot_orb的內(nèi)容， 艮口 f1ag_mo t_orb_t emp = flag_mot_orb ；步驟419、將計算得到指令寫入相應的指令緩沖區(qū)，并在指令發(fā)出后，將相應的指 令緩沖區(qū)置0 ；步驟420、釋放25ms執(zhí)行周期執(zhí)行完畢信號量semb。GNC任務的優(yōu)先級(103)僅次于數(shù)據(jù)采集任務，在該任務中應根據(jù)GNC規(guī)劃任務 給出的各個標志位的不同組合來確定不同的飛行模式，并根據(jù)數(shù)據(jù)采集任務得到的當前時 刻敏感器更新數(shù)據(jù)，完成在每個25ms執(zhí)行周期內(nèi)對撞擊探測器狀態(tài)的導航、制導與控制任 務。具體飛行模式劃分如下1)飛行模式I探測器軌道機動標識位為0且探測器姿態(tài)機動標識位為0(flag_mOt_Orb == 0&&flag_mot_ati = = 0)。存在星敏感器更新數(shù)據(jù)在此模式中，由于星敏感器數(shù)據(jù)有更新值，因此，此時執(zhí)行星敏感器+速率陀螺 +EKF聯(lián)合濾波，對撞擊探測器姿態(tài)進行確定。不存在星敏感器更新數(shù)據(jù)在此模式中，單純地利用陀螺信息結(jié)合姿態(tài)運動學方程對探測器姿態(tài)進行估計。2)飛行模式II探測器軌道機動標識位為0且探測器姿態(tài)機動標識位為1 (flag_m0t_0rb == 0&&f lag_mot_ati ==1)。該模式主要應用在撞擊探測器將姿態(tài)轉(zhuǎn)向期望點火姿態(tài)，與完成點火后將姿態(tài)轉(zhuǎn) 回對目標天體定向的過程中。在該模式下，采用噴嘴對姿態(tài)進行控制，利用常規(guī)飛行模式姿 態(tài)制導算法給出期望姿態(tài)。3)飛行模式III
探測器軌道機動標識位為1 (f lag_m0t_0rb = = 1)。該模式為軌道機動模式，首先由撞擊制導律給出期望的點火方向，姿態(tài)制導函數(shù) 產(chǎn)生期望的姿態(tài)使發(fā)動機指向該點火方向，利用姿控噴嘴完成姿態(tài)機動。由于在本模式下， 姿態(tài)處于機動與粗穩(wěn)定狀態(tài)，星敏感器數(shù)據(jù)不能得到應用，因此在本模式下，只能利用速率 陀螺進行姿態(tài)估計。判斷是否為首次執(zhí)行，在目標機啟動后首次執(zhí)行各任務時并不與外部接口發(fā)生數(shù) 據(jù)傳輸關系，而是利用自帶數(shù)據(jù)初值進行一步運算，為與外部接口同步運行做準備；而信號 量GNC任務更新同步數(shù)據(jù)信號量semb3_sam用于保證目標機與外部系統(tǒng)的啟動同步，即在 外部系統(tǒng)啟動后，目標機第一次接收到外部接口傳輸?shù)母聰?shù)據(jù)后，再進入第一個25ms執(zhí) 行周期的GNC任務執(zhí)行；變量t_ati是用于解決姿態(tài)機動后需要穩(wěn)定100s左右的時間，以 確保本次軌道確定是在姿態(tài)相對穩(wěn)定的情況下進行，t_ati的設置即為在軌道確定任務中 判斷是否適合進行軌道確定提供依據(jù)；標志位flag_Sam是為解決串口通信丟數(shù)問題而設 置的，其初值為0(flag_Sam = 0)，則數(shù)據(jù)采集功能函數(shù)revbyte在程序首次執(zhí)行時進入死 循環(huán)等待外部系統(tǒng)啟動，直到接收到外部接口傳來的第一組數(shù)據(jù)(由于是首次執(zhí)行定時器 并未啟動，故無定時限值)，而當進入第一個執(zhí)行周期的同時，定時器啟動，則將該標志位置 1 (f lag_sam = 1)，此時函數(shù)revbyte若多于三次循環(huán)還未成功接收到數(shù)據(jù)，則沿用上一步 數(shù)據(jù)。
具體實施方式
十、結(jié)合圖12說明本實施方式，本實施方式是對具體實施方式
一中 的軌道確定任務Task_0RB_fuSe的進一步說明，軌道確定任務Task_0RB_fuSe —個周期內(nèi) 的具體步驟如下步驟5A、獲取更新光學導航相機數(shù)據(jù)信號量semb_p ；步驟5B、判斷執(zhí)行姿態(tài)大角度控制后所需的穩(wěn)定時間t_ati與系統(tǒng)當前時間結(jié)構(gòu) 體數(shù)據(jù)中的秒級分量t_current. tv_sec的大小關系；如果t_current. tv_sec > t_ati，則 執(zhí)行步驟5C ；否則，不做任何處理，本次任務結(jié)束；步驟5C、根據(jù)目標天體光心像素坐標、探測器估計姿態(tài)、探測器預測位置、探測 器預測速度和預測誤差方差陣，計算出探測器估計位置、探測器估計速度和估計誤差方差 陣；步驟5D、記錄本次軌道機動標志位fag_orb_fuse = 1。軌道確定任務的優(yōu)先級(104)最低，該任務用于在穩(wěn)定狀態(tài)下(即軌道/姿態(tài)均 無機動)，利用光學導航相機數(shù)據(jù)對軌道進行修正。受到相機圖像處理及傳輸時間的限制， 在滿足拍照條件的情況下，軌道確定每15s進行一次，以提高導航精度。更新光學導航相機數(shù)據(jù)信號量semb_p用于保證15s定時器到期，并且滿足光 學導航相機拍照條件，同時采集到相應圖像信息的情況下，執(zhí)行軌道確定；判斷條件t_ current. tv_sec > t_ati為判斷當前時間是否與最近一次姿態(tài)機動時間間隔足夠長，以確 保本次軌道確定是在姿態(tài)相對穩(wěn)定的情況下進行。
具體實施方式
十一、結(jié)合13說明本實施方式，本實施方式是對具體實施方式
二、 三、五、六或九中25ms定時器處理任務timejiandler的進一步說明25ms定時器處理任務 time_handler 一個周期內(nèi)的具體步驟如下步驟6A、判斷是否滿足撞擊條件，即判斷撞擊器軌道狀態(tài)量r_esti
是否大于
180，如果r_esti
> 0，則執(zhí)行步驟6B ；否則，則執(zhí)行步驟6D ；步驟6B、停止定時器；步驟6C、釋放狀態(tài)指標信號量semaphore ；本次任務結(jié)束；步驟6D、判斷探測器軌道機動標識位flag_m0t_OTb和探測器姿態(tài)機動標識位 flag_mot_ati的內(nèi)容，如果探測器軌道機動標識位flag_m0t_0rb = 1或者探測器姿態(tài)機動 標識位flag_mot_ati = 1，則執(zhí)行步驟6E ；否則，本次任務結(jié)束；步驟6E、將機動標志位flag_mot置1，即令flag_mot = 1 ；步驟6F、釋放25ms定時器到期信號量semb_tm ；步驟6G、采集外部仿真機dSpace動力學部分傳入的敏感器數(shù)據(jù)；步驟6H、釋放速率陀螺控制采樣周期信號量Semb_Sam_0mega、加速度計控制采樣 周期信號量semb_Sam_acel、星敏感器控制采樣周期臨時信號量Semb_Sam_q_temp和光學 導航相機控制采樣周期臨時信號量semb_sam_p_temp。判斷條件為軌道機動標志位和姿態(tài)機動標志位至少其中一位為1，進行此判斷為 是否進行軌道修正提供判斷依據(jù)(機動標志位flag_mot)；信號量semb_tm用于保證每一 個25ms執(zhí)行周期的控制，而信號量Semb_Sam_X則是用于控制敏感器數(shù)據(jù)的采樣周期。
具體實施方式
十二、結(jié)合14說明本實施方式，本實施方式是對具體實施方式
七中 2s定時器處理任務time_handler_one的進一步說明2s定時器處理任務time_handler_ one 一個周期內(nèi)的具體步驟如下步驟61、判斷是否滿足撞擊條件，即判斷撞擊器軌道狀態(tài)量r_esti
是否大于 0，如果是，則執(zhí)行步驟6J ；如果否，則執(zhí)行步驟6L ；步驟6J、停止定時器；步驟6K、釋放狀態(tài)指標信號量semaphore ；本次任務結(jié)束；步驟6L、釋放星敏感器控制采樣周期信號量Semb_Sam_q。信號量星敏感器控制采樣周期信號量Semb_Sam_q用于控制星敏感器數(shù)據(jù)的采樣 周期為2s。
具體實施方式
十三、結(jié)合15說明本實施方式，本實施方式是對具體實施方式
八或 十中15s定時器處理任務time_handler_two的進一步說明15s定時器處理任務time_ handler_two 一個周期內(nèi)的具體步驟如下步驟6M、判斷是否滿足撞擊條件，即判斷撞擊器軌道狀態(tài)量r_esti
是否大于 0，如果是，則執(zhí)行步驟6N ；如果否，則執(zhí)行步驟6P ；步驟6N、停止定時器；步驟60、釋放狀態(tài)指標信號量semaphore ；本次任務結(jié)束；步驟6P、判斷機動標志位flag_mot和軌道確定標志位flag_orb_fuse的內(nèi)容，如 果機動標志位flag_mot = 0并且軌道確定標志位flag_orb_fuSe = 1，則執(zhí)行步驟6Q ；否 則，執(zhí)行步驟6S;步驟6Q、將軌道確定標志位flag_orb_fuse重新置0，即令f lag_orb_fuse = 0 ；步驟6R、利用軌道確定結(jié)果對撞擊器的位置和速度進行修正；然后執(zhí)行步驟6T ；步驟6S、將機動標志位flag_mot置0，即令flag_mot = 0 ；并且將軌道確定標志 位 flag_orb_fuse 置 0,艮口令 flag_orb_fuse = 0 ；
19
步驟6T、釋放光學導航相機控制采樣周期信號量Semb_Sam_p。判斷條件flag_mot = 0&&flag_orb_fuse = 1為上一個15s內(nèi)無機動且執(zhí)行完成 一次軌道確定，在此情況下可利用軌道確定的結(jié)果對撞擊探測器的當前狀態(tài)量進行一次修 正；信號量光學導航相機控制采樣周期信號量Semb_sam_p用于控制光學導航相機數(shù)據(jù)的 采樣周期為15s。
權(quán)利要求
小天體撞擊探測自主導航與制導控制規(guī)劃調(diào)度方法，其特征在于它由主任務main_task和四個子任務實現(xiàn)，所述四個子任務為GNC規(guī)劃任務Task_GNC_pro、數(shù)據(jù)采集任務Task_SAM_X、GNC任務Task_GNC和軌道確定任務；主任務main_task的優(yōu)先級最高，用于完成各部分的初始化，設置系統(tǒng)時鐘中斷頻率、系統(tǒng)當前時間及任務調(diào)度策略，完成對各二進制信號量、子任務及定時器的創(chuàng)建，并在獲取得到相關信號量時，啟動與所述信號量相關的定時器，結(jié)束相關任務；GNC規(guī)劃任務Task_GNC_pro的優(yōu)先級僅次于主任務main_task，用于在每個執(zhí)行周期開始時，根據(jù)當前撞擊器的軌道和姿態(tài)狀態(tài)量，給出探測器軌道機動標識位flag_mot_orb、探測器姿態(tài)機動標識位flag_mot_ati和機動點火標識位flag_mot_ign，為確定同周期GNC任務Task_GNC下的功能函數(shù)的執(zhí)行順序提供判斷依據(jù)；數(shù)據(jù)采集任務Task_SAM_X的優(yōu)先級僅次于GNC規(guī)劃任務Task_GNC_pro，用于在不同采樣周期讀取各敏感器的測量數(shù)據(jù)；所述數(shù)據(jù)采集任務Task_SAM_X包括4個數(shù)據(jù)采集子任務，分別為速率陀螺數(shù)據(jù)采集任務Task_SAM_omega、加速度計數(shù)據(jù)采集任務Task_SAM_acel、星敏感器數(shù)據(jù)采集任務Task_SAM_q和光學導航相機數(shù)據(jù)采集任務Task_SAM_p；所述4個子任務的優(yōu)先級相同，所述4個子任務間采用時間片輪轉(zhuǎn)調(diào)度算法進行調(diào)度執(zhí)行；GNC任務Task_GNC的優(yōu)先級僅次于數(shù)據(jù)采集任務Task_SAM_X，根據(jù)GNC規(guī)劃任務Task_GNC_pro給出的各個標志位的組合來確定飛行模式，并根據(jù)數(shù)據(jù)采集任務Task_SAM_X得到的當前時刻敏感器更新數(shù)據(jù)，完成在每個執(zhí)行周期內(nèi)對撞擊器狀態(tài)的導航、制導與控制任務；軌道確定任務Task_ORB_fuse的優(yōu)先級最低，所述軌道確定任務Task_ORB_fuse每隔15s進行一次，在執(zhí)行任務時，首先判斷軌道和姿態(tài)是否有機動的情況，當軌道和姿態(tài)均無機動，則小天體處于穩(wěn)定狀態(tài)，利用光學導航相機數(shù)據(jù)對小天體軌道參數(shù)進行修正；否則退出任務。
2.根據(jù)權(quán)利要求1所述的小天體撞擊探測自主導航與制導控制規(guī)劃調(diào)度方法，其特征 在于主任務mair^task的工作過程如下步驟1A、初始化時間規(guī)格、定時器信號事件、定時器ID標識、定時器信號處理和任務ID 標識；步驟1B、設置系統(tǒng)時鐘終端頻率和任務調(diào)度策略，所述系統(tǒng)時鐘終端頻率為200Hz，所 述任務調(diào)度策略為采用基于任務優(yōu)先級的時間片輪轉(zhuǎn)換的調(diào)度方法；步驟1C、設置當前時間；步驟1D、創(chuàng)建二進制信號量，所述二進制信號量包括狀態(tài)指標信號量semaphore、主任 務更新同步數(shù)據(jù)信號量sembl_Sam、GNC規(guī)劃任務更新同步數(shù)據(jù)信號量semb2_sam、GNC任務 更新同步數(shù)據(jù)信號量semb3_Sam、更新星敏感器數(shù)據(jù)信號semb_q、更新速率陀螺數(shù)據(jù)信號 量Semb_0mega、更新光學導航相機數(shù)據(jù)信號量semb_p、更新加速度計數(shù)據(jù)信號Semb_acel、 速率陀螺控制采樣周期信號量Semb_sam_0mega、加速度計控制采樣周期信號量semb_ Sam_aCel、星敏感器控制采樣周期信號量Semb_Sam_q、星敏感器控制采樣周期臨時信號量 Semb_Sam_q_temp、光學導航相機控制采樣周期信號量Semb_Sam_p、光學導航相機控制采樣 周期臨時信號量semb_sam_p_temp和控制GNC規(guī)劃任務Task_GNC_pro每25ms最先執(zhí)行信 號量，所述控制GNC規(guī)劃任務Task_GNC_pro每25ms最先執(zhí)行信號量包括25ms執(zhí)行周期執(zhí)行完畢信號量semb和25ms定時器到期信號量semb_tm ；步驟1E、創(chuàng)建四個子任務GNC規(guī)劃任務Task_GNC_pro、數(shù)據(jù)采集任務Task_SAM_X、GNC 任務Task_GNC和軌道確定任務Task_0RB_fuse ；步驟1F、設置定時器信號處理函數(shù)，分別為25ms定時器處理任務time_handler、2S定 時器處理任務time_handler_one和15s定時器處理任務time_handler_two ； 步驟1G、創(chuàng)建定時器；步驟1H、獲取主任務更新同步數(shù)據(jù)信號量semblsam的內(nèi)容； 步驟II、啟動定時器，設定三個采樣周期分別為25ms、2s和15s ； 步驟1J、獲取狀態(tài)指標信號量semaphore的內(nèi)容；步驟1K、當撞擊器狀態(tài)滿足撞擊條件時，進行信號量的刪除、定時器的刪除、任務的刪 除操作。
3.根據(jù)權(quán)利要求1所述的小天體撞擊探測自主導航與制導控制規(guī)劃調(diào)度方法，其特征 在于所述GNC規(guī)劃任務Task_GNC_pr0的工作過程如下步驟2A、判斷是否為首次執(zhí)行，用i表示執(zhí)行次數(shù)，如果是首次執(zhí)行，用i = 0表示，執(zhí) 行步驟2B ；如果不是首次執(zhí)行，則執(zhí)行步驟2C ； 步驟2B、獲取系統(tǒng)當前時間；然后執(zhí)行步驟2G ；步驟2C、判斷是否為第一個執(zhí)行周期，即判斷i是否為1 ；如果是，則執(zhí)行步驟2D ；如果 否，則執(zhí)行步驟2E;步驟2D、獲取更新敏感器數(shù)據(jù)信號量GNC規(guī)劃任務更新同步數(shù)據(jù)信號量Semb2_Sam的 內(nèi)容；然后執(zhí)行步驟2F;步驟2E、獲取25ms執(zhí)行周期執(zhí)行完畢信號量semb和25ms定時器到期信號量semb_tm 的內(nèi)容；當25ms執(zhí)行周期執(zhí)行完畢信號量semb或25ms定時器到期信號量semb_tm有效 時；步驟2F、計算首次執(zhí)行中等待外部仿真機dSpace啟動運算及數(shù)據(jù)傳輸時間，并利用其 對系統(tǒng)時間進行修正；步驟2G、根據(jù)探測器估計位置、探測器估計速度、探測器估計姿態(tài)、估計角速度、探測器 期望姿態(tài)、探測器期望角速度、期望增量速度、加速度測量值和探測器上一時刻所處飛行狀 態(tài)，計算出探測器軌道機動標識位flag_m0t_OTb、探測器姿態(tài)機動標識位flag_m0t_ati、 機動點火標識位flag_m0t_ign，從而判斷當前探測器所處的飛行狀態(tài)；步驟2H、令執(zhí)行次數(shù)i = i+1，本次任務執(zhí)行完畢，將25ms執(zhí)行周期執(zhí)行完畢信號量 semb置位有效。
4.根據(jù)權(quán)利要求1所述的小天體撞擊探測自主導航與制導控制規(guī)劃調(diào)度方法，其特征 在于數(shù)據(jù)采集任務Task_SAM_X中的4個數(shù)據(jù)采集子任務為定時器觸發(fā)的工作方式，其中速 率陀螺數(shù)據(jù)采集任務TaSk_SAM_0mega和加速度計數(shù)據(jù)采集任務Task_SAM_acel的數(shù)據(jù)采 樣周期為25ms ；星敏感器數(shù)據(jù)采集任務Task_SAM_q的采樣周期為2s，在執(zhí)行星敏感器數(shù)據(jù)采集任務 Task_SAM_q的時候，首先判斷軌道和姿態(tài)是否有機動的情況，如果軌道或姿態(tài)有機動，則退 出任務，否則采集星敏感器數(shù)據(jù)；光學導航相機數(shù)據(jù)采集任務Task_SAM_p的采樣周期為15s，在執(zhí)行光學導航相機數(shù)據(jù)采集任務Task_SAM_p的時候，首先判斷軌道和姿態(tài)是否有機動情況，當軌道和姿態(tài)均無機 動情況下執(zhí)行數(shù)據(jù)采集任務，并記錄所述相機拍照時刻的軌道/姿態(tài)狀態(tài)信息，否則退出 任務；速率陀螺數(shù)據(jù)采集任務TaSk_SAM_0mega的工作過程如下 步驟3A、獲取速率陀螺控制采樣周期信號量semb_sam_0mega ； 步驟3B、將采集到的陀螺數(shù)據(jù)寫入數(shù)據(jù)緩沖區(qū)； 步驟3C、釋放更新陀螺數(shù)據(jù)信號量semb_0mega ； 加速度計數(shù)據(jù)采集任務Task_SAM_acel的工作過程如下 步驟3D、獲取加速度計控制采樣周期信號量semb_sam_aCel ； 步驟3E、將采集到的加速度計數(shù)據(jù)寫入數(shù)據(jù)緩沖區(qū)； 步驟3F、釋放更新加速度計數(shù)據(jù)信號Semb_acel ； 星敏感器數(shù)據(jù)采集任務Task_SAM_q的工作過程如下步驟3G、獲取星敏感器控制采樣周期信號量Semb_Sam_q和星敏感器控制采樣周期臨 時信號量 semb—sam—q—temp ；步驟3H、判斷探測器軌道機動標識位flag_mot_orb和探測器姿態(tài)機動標識位flag_ mot_ati的內(nèi)容，如果探測器軌道機動標識位flag_mot_orb = O并且探測器姿態(tài)機動標識 位flag_m0t_ati = 0，則執(zhí)行步驟31 ；否則，不做任何處理，本次任務結(jié)束；步驟31、將調(diào)用姿態(tài)確定功能函數(shù)的標志位flag_tm的內(nèi)容置1，即令flag_tm = 1 ； 步驟3J、將采集到的陀螺數(shù)據(jù)寫入數(shù)據(jù)緩沖區(qū)； 步驟3K、釋放更新星敏感器數(shù)據(jù)信號semb_q ； 光學導航相機數(shù)據(jù)采集任務Task_SAM_p的工作過程如下步驟3L、獲取光學導航相機控制采樣周期信號量semb_sam_p和光學導航相機控制采 樣周期臨時信號量semb_sam_p_temp ；步驟3M、判斷探測器軌道機動標識位flag_mot_orb和探測器姿態(tài)機動標識位flag_ mot_ati的內(nèi)容，如果探測器軌道機動標識位flag_mot_orb = 0并且探測器姿態(tài)機動標識 位flag_m0t_ati = 0，則執(zhí)行步驟3N ；否則，不做任何處理，本次任務結(jié)束；步驟3N、獲取拍照時刻系統(tǒng)時間，并記錄拍照時刻對應的撞擊軌道/姿態(tài)狀態(tài)量，所述 撞擊軌道/姿態(tài)狀態(tài)量包括撞擊器位置和速度；步驟30、將采集到的光學導航相機數(shù)據(jù)寫入數(shù)據(jù)緩沖區(qū)； 步驟3P、釋放更新光學導航相機數(shù)據(jù)信號量semb_p。
5.根據(jù)權(quán)利要求1所述的小天體撞擊探測自主導航與制導控制規(guī)劃調(diào)度方法，其特征 在于GNC任務Task_GNC —個周期內(nèi)的具體步驟如下步驟4A、判斷是否為首次執(zhí)行，如果是，則執(zhí)行步驟4B，如果否，則執(zhí)行步驟4L ； 首次執(zhí)行GNC任務Task_GNC情況如下步驟4B、根據(jù)軌道面法線方向矢量、探測器估計姿態(tài)和探測器估計角速度，計算出探測 器期望的姿態(tài)余弦陣和探測器三軸期望角速度，從而獲得探測器定向姿態(tài)的期望姿態(tài)；步驟4C、判斷探測器姿態(tài)機動標識位flag_mot_ati的內(nèi)容，如果flag_mot_ati = 0，則執(zhí)行步驟4D，否則，即flag_mot_ati = 1，則執(zhí)行步驟4E ；步驟4D、根據(jù)探測器估計姿態(tài)、探測器估計角速度、探測器期望姿態(tài)和探測器期望角速度，計算出反作用飛輪控制信號，從而獲得反作用飛輪控制量；然后執(zhí)行步驟4G ；步驟4E、根據(jù)探測器估計姿態(tài)、探測器估計角速度、探測器期望姿態(tài)和探測器期望角速 度，計算出三軸噴嘴開關序列，從而獲得噴嘴開關的邏輯；步驟4F、將系統(tǒng)當前時間結(jié)構(gòu)體數(shù)據(jù)中的秒級分量t_current. tv_sec加100，并賦值 給所需的穩(wěn)定時間t_ati ；即令如果t_ati = t_current. tv_sec+100 ；步驟4G、根據(jù)陀螺輸出的姿態(tài)角速度、陀螺漂移更新值和上一時刻探測器估計姿態(tài)，計 算出探測器估計姿態(tài)和探測器估計角速度，從而遞推出探測器相對慣性空間的姿態(tài)矩陣；步驟4H、根據(jù)探測器估計姿態(tài)、探測器預測位置、探測器預測速度、加速度測量值和 機動點火標識位，計算出探測器估計位置和探測器估計速度，從而遞推出探測器位置和速 度；步驟41、將計算得到的指令寫入相應的指令緩沖區(qū)；步驟4J、接收外部仿真機dSpace動力學模塊傳入的敏感器數(shù)據(jù)，并將采集到的數(shù)據(jù)寫 入相應的數(shù)據(jù)緩沖區(qū)；步驟4K、釋放主任務更新同步數(shù)據(jù)信號量sembl_sam，GNC規(guī)劃任務更新同步數(shù)據(jù)信號 量semb2_sam和GNC任務更新同步數(shù)據(jù)信號量semb3_sam ；步驟4L、判斷是否為第二次執(zhí)行；如果是，則執(zhí)行步驟4M，如果否，則執(zhí)行步驟4Z ； 第二次執(zhí)行GNC任務Task_GNC情況如下 步驟4M、獲取GNC任務更新同步數(shù)據(jù)信號量Semb3_Sam ； 步驟4N、將首次任務執(zhí)行的指令發(fā)出，并將相應指令緩沖區(qū)置0 ； 步驟40、根據(jù)軌道面法線方向矢量、探測器估計姿態(tài)和探測器估計角速度，計算出探測 器期望的姿態(tài)余弦陣和探測器三軸期望角速度，從而獲得探測器定向姿態(tài)的期望姿態(tài)；步驟4P、判斷探測器姿態(tài)機動標識位flag_mot_ati的內(nèi)容，如果flag_mot_ati = 0， 則執(zhí)行步驟4Q，否則，即flag_m0t_ati = 1，則執(zhí)行步驟步驟4Q、根據(jù)探測器估計姿態(tài)、探測器估計角速度、探測器期望姿態(tài)和探測器期望角速 度，計算出反作用飛輪控制信號，從而獲得反作用飛輪控制量；然后執(zhí)行步驟4T ；步驟4R、根據(jù)探測器估計姿態(tài)、探測器估計角速度、探測器期望姿態(tài)和探測器期望角速 度，計算出三軸噴嘴開關序列，從而獲得噴嘴開關的邏輯；步驟4S、將系統(tǒng)當前時間結(jié)構(gòu)體數(shù)據(jù)中的秒級分量t_current. tv_sec加100，并賦值 給所需的穩(wěn)定時間t_ati ；即令如果t_ati = t_current. tv_sec+100 ；步驟4T、根據(jù)陀螺輸出的姿態(tài)角速度、陀螺漂移更新值和上一時刻探測器估計姿態(tài)，計 算出探測器估計姿態(tài)和探測器估計角速度，從而遞推出探測器相對慣性空間的姿態(tài)矩陣；步驟4U、根據(jù)探測器估計姿態(tài)、探測器預測位置、探測器預測速度、加速度測量值和機動點火標識位，計算出探測器估計位置和探測器估計速度，從而遞推出探測器位置和速 度；步驟4V、將串口通信丟數(shù)標志位flag_sam置1，即令flag_sam = 1 ； 步驟4W、記錄本次執(zhí)行周期軌道機動標志位flagjiiot_orb_temp，令執(zhí)行周期軌道機 動標志位flag_mot_orb_temp的內(nèi)容為探測器軌道機動標識位flag_mot_orb的內(nèi)容，即 f1ag_mo t_orb_t emp = flag_mot_orb ；步驟4X、將計算得到指令寫入相應的指令緩沖區(qū)，并在指令發(fā)出后，將相應的指令緩沖區(qū)置0;步驟4Y、釋放25ms執(zhí)行周期執(zhí)行完畢信號量semb ； 第二次之后執(zhí)行GNC任務Task_GNC情況如下步驟4Z、獲取更新陀螺數(shù)據(jù)信號量Semb_0mega和更新加速度計數(shù)據(jù)信號Semb_acel ； 然后執(zhí)行步驟41 ；步驟41、判斷調(diào)用姿態(tài)確定功能函數(shù)的標志位flag_tm的內(nèi)容，如果flag_tm = 1，則 執(zhí)行步驟42 ；否則，執(zhí)行步驟45 ；步驟42、將調(diào)用姿態(tài)確定功能函數(shù)的標志位flag_tm置0，即令flag_tm = 0 ； 步驟43、獲取更新星敏感器數(shù)據(jù)信號semb_q ；步驟44、根據(jù)星敏感器輸出的姿態(tài)四元數(shù)、陀螺輸出的姿態(tài)角速度，計算出探測器估計 姿態(tài)、探測器估計角速度、陀螺漂移更新值，從而實現(xiàn)通過卡爾曼濾波校正撞擊探測器姿態(tài) 和陀螺漂移；步驟45、判斷探測器軌道機動標識位flag_mot_0rb的內(nèi)容，如果flag_mot_0rb = 0， 則執(zhí)行步驟46 ；否則，即flag_mot_orb = 1，執(zhí)行步驟411 ；步驟46、根據(jù)軌道面法線方向矢量、探測器估計姿態(tài)和探測器估計角速度，計算出探測 器期望的姿態(tài)余弦陣和探測器三軸期望角速度，從而計算探測器定向姿態(tài)的期望姿態(tài)；步驟47、判斷探測器姿態(tài)機動標識位flag_m0t_ati的內(nèi)容，如果flag_mot_ati = 0， 則執(zhí)行步驟48，否則，即flag_m0t_ati = 1，則執(zhí)行步驟49 ；步驟48、根據(jù)探測器估計姿態(tài)、探測器估計角速度、探測器期望姿態(tài)和探測器期望角速 度，計算出反作用飛輪控制信號，從而獲得反作用飛輪控制量；然后執(zhí)行步驟416 ；步驟49、根據(jù)探測器估計姿態(tài)、探測器估計角速度、探測器期望姿態(tài)和探測器期望角速 度，計算出三軸噴嘴開關序列，從而獲得噴嘴開關的邏輯；然后執(zhí)行步驟410 ；步驟410、將系統(tǒng)當前時間結(jié)構(gòu)體數(shù)據(jù)中的秒級分量t_current. tv_sec加100，并賦值 給所需的穩(wěn)定時間t_ati ；即令如果t_ati = t_current. tv_sec+100 ；然后執(zhí)行步驟416 ； 步驟411、判斷執(zhí)行周期軌道機動標志位flag_m0t_0rb_temp的內(nèi)容，如果flag_mot_ orb_temp = 0，則執(zhí)行步驟 412，否則，即 f lag_mot_orb_temp = 1，則執(zhí)行步驟 413 ；步驟412、根據(jù)探測器估計位置、探測器估計速度和期望撞擊目標點，計算出期望增量 速度，從而獲得接近軌道需要的增量速度；步驟413、根據(jù)探測器估計位置和制導增量速度，計算出探測器期望的姿態(tài)余弦陣和探 測器三軸期望角速度，從而獲得探測器定向姿態(tài)的期望姿態(tài)；步驟414、根據(jù)探測器估計姿態(tài)、探測器估計角速度、探測器期望姿態(tài)和探測器期望角 速度，計算出三軸噴嘴開關序列，從而獲得噴嘴開關的邏輯；步驟415、將系統(tǒng)當前時間結(jié)構(gòu)體數(shù)據(jù)中的秒級分量t_current. tv_sec加100，并賦值 給所需的穩(wěn)定時間t_ati ；即令如果t_ati = t_current. tv_sec+100 ；然后執(zhí)行步驟416 ； 步驟416、根據(jù)陀螺輸出的姿態(tài)角速度、陀螺漂移更新值和上一時刻探測器估計姿態(tài)， 計算出探測器估計姿態(tài)和探測器估計角速度，從而獲得探測器相對慣性空間的姿態(tài)矩陣；步驟417、根據(jù)探測器估計姿態(tài)、探測器預測位置、探測器預測速度、加速度測量值和機 動點火標識位，計算出探測器估計位置和探測器估計速度，從而獲得探測器位置和速度； 步驟418、記錄本次執(zhí)行周期軌道機動標志位flag_m0t_0rb_temp，令執(zhí)行周期軌道機動標志位flag_mot_orb_temp的內(nèi)容為探測器軌道機動標識位flag_mot_orb的內(nèi)容，即 f1ag_mo t_orb_t emp = flag_mot_orb ；步驟419、將計算得到指令寫入相應的指令緩沖區(qū)，并在指令發(fā)出后，將相應的指令緩 沖區(qū)置0 ；步驟420、釋放25ms執(zhí)行周期執(zhí)行完畢信號量semb。
6.根據(jù)權(quán)利要求1所述的小天體撞擊探測自主導航與制導控制規(guī)劃調(diào)度方法，其特征 在于軌道確定任務Task_0RB_fuSe —個周期內(nèi)的具體步驟如下步驟5A、獲取更新光學導航相機數(shù)據(jù)信號量semb_p ；步驟5B、判斷執(zhí)行姿態(tài)大角度控制后所需的穩(wěn)定時間t_ati與系統(tǒng)當前時間結(jié)構(gòu)體數(shù) 據(jù)中的秒級分量t_current. tv_sec的大小關系；如果t_current. tv_sec > t_ati，則執(zhí)行 步驟5C ；否則，不做任何處理，本次任務結(jié)束；步驟5C、根據(jù)目標天體光心像素坐標、探測器估計姿態(tài)、探測器預測位置、探測器預測 速度和預測誤差方差陣，計算出探測器估計位置、探測器估計速度和估計誤差方差陣； 步驟5D、記錄本次軌道確定標志位flag_orb_fuse = 1。
7.根據(jù)權(quán)利要求2所述的小天體撞擊探測自主導航與制導控制規(guī)劃調(diào)度方法，其特征 在于25ms定時器處理任務timejiandler —個周期內(nèi)的具體步驟如下步驟6A、判斷是否滿足撞擊條件，即判斷撞擊器軌道狀態(tài)量r_esti
是否大于0，如 果r_esti
> 0，則執(zhí)行步驟6B ；否則，則執(zhí)行步驟6D ； 步驟6B、停止定時器；步驟6C、釋放狀態(tài)指標信號量semaphore ；本次任務結(jié)束；步驟6D、判斷探測器軌道機動標識位flag_mot_orb和探測器姿態(tài)機動標識位flag_ mot_ati的內(nèi)容，如果探測器軌道機動標識位flag_m0t_0rb = 1或者探測器姿態(tài)機動標識 位flag_m0t_ati = 1，則執(zhí)行步驟6E ；否則，本次任務結(jié)束； 步驟6E、將機動標志位flag_mot置1，即令flag_mot = 1 ； 步驟6F、釋放25ms定時器到期信號量semb_tm ； 步驟6G、采集外部仿真機dSpace動力學部分傳入的敏感器數(shù)據(jù)； 步驟6H、釋放速率陀螺控制采樣周期信號量semb_sam_0mega、加速度計控制采樣周期 信號量semb_Sam_acel、星敏感器控制采樣周期臨時信號量Semb_Sam_q_temp和光學導航 相機控制采樣周期臨時信號量semb_sam_p_temp。 根據(jù)權(quán)利要求2所述的小天體撞擊探測自主導航與制導控制規(guī)劃調(diào)度方法，其特征 在于2s定時器處理任務time_handler_one —個周期內(nèi)的具體步驟如下步驟61、判斷是否滿足撞擊條件，即判斷撞擊器軌道狀態(tài)量r_esti
是否大于0，如 果是，則執(zhí)行步驟6J ；如果否，則執(zhí)行步驟6L ； 步驟6J、停止定時器；步驟6K、釋放狀態(tài)指標信號量semaphore ；本次任務結(jié)束； 步驟6L、釋放星敏感器控制采樣周期信號量Semb_Sam_q。 根據(jù)權(quán)利要求2所述的小天體撞擊探測自主導航與制導控制規(guī)劃調(diào)度方法，其特征 在于15s定時器處理任務time_handler_two —個周期內(nèi)的具體步驟如下步驟6M、判斷是否滿足撞擊條件，即判斷撞擊器軌道狀態(tài)量r_esti
是否大于0，如果是，則執(zhí)行步驟6N ；如果否，則執(zhí)行步驟6P ； 步驟6N、停止定時器；步驟60、釋放狀態(tài)指標信號量semaphore ；本次任務結(jié)束；步驟6P、判斷機動標志位flagjiiot和軌道確定標志位flag_0rb_fuse的內(nèi)容，如果機 動標志位flag_mot = 0并且軌道確定標志位flag_orb_fuse = 1，則執(zhí)行步驟6Q ；否則，執(zhí) 行步驟6S ；步驟6Q、將軌道確定標志位flag_orb_fuse重新置0，即令flag_orb_fuse = 0 ； 步驟6R、利用軌道確定結(jié)果對撞擊器的位置和速度進行修正；然后執(zhí)行步驟6T ； 步驟6S、將機動標志位flag_mot置0，即令flagjiiot = 0 ；并且將軌道確定標志位 flag_orb_fuse 置 0,艮口令 flag_orb_fuse = 0 ；步驟6T、釋放光學導航相機控制采樣周期信號量semb_sam_p。
全文摘要
小天體撞擊探測自主導航與制導控制規(guī)劃調(diào)度方法，涉及一種深空探測器的自主導航與控制規(guī)劃調(diào)度方法。解決了現(xiàn)有技術(shù)的自主導航與制導控制規(guī)劃調(diào)度方法方法存在任務眾多、可替換性差、可擴展性差的問題，它具體有下列5個模塊化任務組成主任務、GNC規(guī)劃任務、數(shù)據(jù)采集任務、GNC任務和軌道確定任務；主任務的優(yōu)先級最高，GNC規(guī)劃任務的優(yōu)先級僅次于主任務，數(shù)據(jù)采集任務的優(yōu)先級僅次于GNC規(guī)劃任務，根據(jù)敏感器測量數(shù)據(jù)類型的不同，將數(shù)據(jù)采集任務分為4個子任務，GNC任務的優(yōu)先級僅次于數(shù)據(jù)采集任務，軌道確定任務的優(yōu)先級最低，對小天體撞擊探測任務進行模塊化分解，提出各個任務模塊間的同步方式與通信手段，最終完成小天體撞擊探測任務。
文檔編號B64G1/24GK101830290SQ20101010946
公開日2010年9月15日 申請日期2010年2月12日 優(yōu)先權(quán)日2010年2月12日
發(fā)明者崔平遠, 崔祜濤, 朱圣英, 高艾 申請人:哈爾濱工業(yè)大學