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

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


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