小天体撞击探测自主导航与制导控制规划调度方法

摘要

小天体撞击探测自主导航与制导控制规划调度方法，涉及一种深空探测器的自主导航与控制规划调度方法。解决了现有技术的自主导航与制导控制规划调度方法方法存在任务众多、可替换性差、可扩展性差的问题，它具体有下列5个模块化任务组成：主任务、GNC规划任务、数据采集任务、GNC任务和轨道确定任务；主任务的优先级最高，GNC规划任务的优先级仅次于主任务，数据采集任务的优先级仅次于GNC规划任务，根据敏感器测量数据类型的不同，将数据采集任务分为4个子任务，GNC任务的优先级仅次于数据采集任务，轨道确定任务的优先级最低，对小天体撞击探测任务进行模块化分解，提出各个任务模块间的同步方式与通信手段，最终完成小天体撞击探测任务。

说明书

技术领域

本发明涉及一种深空探测器的自主导航与控制规划调度方法。

背景技术

自主导航与制导控制系统通过软件实现在不需地面站支持就能自主确定探测器的状态，并导引探测器完成预定撞击任务。小天体撞击探测飞行距离远，运行阶段多，各阶段所处环境差异大，这要求导航与制导控制系统需要完成复杂繁琐的在轨操作，以保证工程任务与科学任务的顺利实现。繁杂的在轨操作使星载软件因任务多而使规模变大，各任务有不同的实时性要求，任务间的联系增多。以往进行导航、制导与控制系统的软件结构设计时主要针对单任务或少量任务，一般采用顺序编程加中断方法。系统的复杂化及对实时性的高要求使传统串行的软件设计方式不再满足实际应用需求，如果仍然用传统星载软件的设计方法、开发手段，则系统设计时头绪多、结构复杂、条理不清，而且很难借鉴其它软件的先进经验不利于探测器技术的延续发展。同时为了扩大探测任务的适应性和探测器的功能，以及便于后续探测任务的继承性，需要自主导航与制导控制系统软件要具有较强的可替换性及可扩展性。嵌入式实时操作系统(RTOS)在此方面则有着其突出的优点。VxWorks操作系统就是一种嵌入式实时操作系统，提供了高效的实时任务调度、中断管理、实时的系统资源以及任务间通信等功能。高性能的操作系统微内核Wind支持实时特性，其设计减少了系统开销，高效的任务管理保证了对外部事件快速、准确的反应；快速灵活的任务间和进程间通信允许独立的任务在实时系统中与其行动相协调。VxWorks嵌入式实时操作系统经过广泛的验证，已成功的应用在航空、航天中的关键领域。

发明内容

本发明为了让解决现有技术的自主导航与制导控制规划调度方法存在任务众多、可替换性差、可扩展性差的问题，提出一种小天体撞击探测自主导航与制导控制规划调度方法。

小天体撞击探测自主导航与制导控制规划调度方法，它由主任务main_task和四个子任务实现，所述四个子任务为GNC规划任务Task_GNC_pro、数据采集任务Task_SAM_X、GNC任务Task_GNC和轨道确定任务；

主任务main_task的优先级最高，用于完成各部分的初始化，设置系统时钟中断频率、系统当前时间及任务调度策略，完成对各二进制信号量、子任务及定时器的创建，并在获取得到相关信号量时，启动与所述信号量相关的定时器，结束相关任务；

GNC规划任务Task_GNC_pro的优先级仅次于主任务main_task，用于在每个执行周期开始时，根据当前撞击器的轨道和姿态状态量，给出探测器轨道机动标识位flag_mot_orb、探测器姿态机动标识位flag_mot_ati和机动点火标识位flag_mot_ign，为确定同周期GNC任务Task_GNC下的功能函数的执行顺序提供判断依据；

数据采集任务Task_SAM_X的优先级仅次于GNC规划任务Task_GNC_pro，用于在不同采样周期读取各敏感器的测量数据；所述数据采集任务Task_SAM_X包括4个数据采集子任务，分别为速率陀螺数据采集任务Task_SAM_omega、加速度计数据采集任务Task_SAM_acel、星敏感器数据采集任务Task_SAM_q和光学导航相机数据采集任务Task_SAM_p；所述4个子任务的优先级相同，所述4个子任务间采用时间片轮转调度算法进行调度执行；

GNC任务Task_GNC的优先级仅次于数据采集任务Task_SAM_X，根据GNC规划任务Task_GNC_pro给出的各个标志位的组合来确定飞行模式，并根据数据采集任务Task_SAM_X得到的当前时刻敏感器更新数据，完成在每个执行周期内对撞击器状态的导航、制导与控制任务；

轨道确定任务Task_ORB_fuse的优先级最低，所述轨道确定任务Task_ORB_fuse每隔15s进行一次，在执行任务时，首先判断轨道和姿态是否有机动的情况，当轨道和姿态均无机动，则小天体处于稳定状态，利用光学导航相机数据对小天体轨道参数进行修正；否则退出任务。

本发明针对小天体撞击探测任务，应用VxWorks嵌入式实时操作系统，提出一种小天体撞击探测自主导航与制导控制规划调度方法。对自主导航与制导控制系统的控制模式进行设计；在此基础上，综合考虑小天体撞击探测任务的实时性要求、不同飞行模式的耦合关系、轨道确定的数学运算量以及对不同敏感器数据采集的周期性控制等多方面因素，对小天体撞击探测任务进行模块化分解，提出各个任务模块间的同步方式与通信手段，最终完成小天体撞击探测任务。

附图说明

图1为是小天体撞击探测自主导航与制导控制规划调度方法单个执行周期的任务调度流程与函数执行示意图。图2为主任务main_task实现流程图；图3为GNC规划任务Task_GNC_pro实现流程图；图4为速率陀螺数据采集任务Task_SAM_omega实现流程图；图5为加速度计数据采集任务Task_SAM_acel实现流程图；图6为星敏感器数据采集任务Task_SAM_q实现流程图；图7为光学导航相机数据采集任务Task_SAM_p实现流程图；图8为GNC任务Task_GNC实现流程图；图9为首次执行GNC任务Task_GNC的流程图；图10为第二次执行GNC任务Task_GNC的流程图；图11为第二次以后执行GNC任务Task_GNC的流程图；图12为轨道确定任务Task_ORB_fuse实现流程图；图13为25ms定时器处理函数流程图；图14为2s定时器处理函数流程图；图15为15s定时器处理函数流程图。

具体实施方式

具体实施方式一、结合图1说明本实施方式，小天体撞击探测自主导航与制导控制规划调度方法，它由主任务main_task和四个子任务实现，所述四个子任务为GNC规划任务Task_GNC_pro、数据采集任务Task_SAM_X、GNC任务Task_GNC和轨道确定任务；

主任务main_task的优先级最高，用于完成各部分的初始化，设置系统时钟中断频率、系统当前时间及任务调度策略，完成对各二进制信号量、子任务及定时器的创建，并在获取得到相关信号量时，启动与所述信号量相关的定时器，结束相关任务；

GNC规划任务Task_GNC_pro的优先级仅次于主任务main_task，用于在每个执行周期开始时，根据当前撞击器的轨道和姿态状态量，给出探测器轨道机动标识位flag_mot_orb、探测器姿态机动标识位flag_mot_ati和机动点火标识位flag_mot_ign，为确定同周期GNC任务Task_GNC下的功能函数的执行顺序提供判断依据；

数据采集任务Task_SAM_X的优先级仅次于GNC规划任务Task_GNC_pro，用于在不同采样周期读取各敏感器的测量数据；所述数据采集任务Task_SAM_X包括4个数据采集子任务，分别为速率陀螺数据采集任务Task_SAM_omega、加速度计数据采集任务Task_SAM_acel、星敏感器数据采集任务Task_SAM_q和光学导航相机数据采集任务Task_SAM_p；所述4个子任务的优先级相同，所述4个子任务间采用时间片轮转调度算法进行调度执行；

GNC任务Task_GNC的优先级仅次于数据采集任务Task_SAM_X，根据GNC规划任务Task_GNC_pro给出的各个标志位的组合来确定飞行模式，并根据数据采集任务Task_SAM_X得到的当前时刻敏感器更新数据，完成在每个执行周期内对撞击器状态的导航、制导与控制任务；

轨道确定任务Task_ORB_fuse的优先级最低，所述轨道确定任务Task_ORB_fuse每隔15s进行一次，在执行任务时，首先判断轨道和姿态是否有机动的情况，当轨道和姿态均无机动，则小天体处于稳定状态，利用光学导航相机数据对小天体轨道参数进行修正；否则退出任务。

轨道确定任务受到相机图像处理耗时长及传输延时的限制，在满足拍照条件的情况下，轨道确定每15s进行一次，以提高导航精度。

时间片轮转调度算法的基本思想是，在更高优先级任务调度依然优先运行的前提下，同优先级任务之间调度时追求一定意义下的公平。一组任务中的每个任务执行一个预先确定的时间段，成为一个时间片；然后另一个任务执行相等的一个时间片，一次进行。这种分配是公正的，它保证一个优先级组中，在所有任务都得到一个时间片之前，不会有任务得到第二个时间片。更准确的说，如果使用轮转调度算法，系统中的每个任务都有一个运行时间计数器。这个计数器随系统始终增加而增加。当达到规定的时间片的值时，也就是说一个规定的时间片已经完成，这个计数器清零，时间片轮转调度算法将这个任务放到相应任务优先级队列的尾部，将CPU交给对头的任务执行。新加入一个优先级组队列的任务将放到队列的尾部，计数器初始化为0。如果任务在它的时间片中被高优先级的任务抢占，时间片轮转调度算法保存它的运行时间计数器，当它再一次符合执行条件的时候，时间片轮转调度算法恢复运行时间计数器。

具体实施方式二、结合图2说明本实施方式，本实施方式是对具体实施方式一中的主任务main_task的进一步说明：主任务main_task的工作过程如下：

步骤1A、初始化时间规格、定时器信号事件、定时器ID标识、定时器信号处理和任务ID标识；

步骤1B、设置系统时钟终端频率和任务调度策略，所述系统时钟终端频率为200Hz，所述任务调度策略为采用基于任务优先级的时间片轮转换的调度方法；

步骤1C、设置当前时间；

步骤1D、创建二进制信号量，所述二进制信号量包括状态指标信号量semaphore、主任务更新同步数据信号量semb1_sam、GNC规划任务更新同步数据信号量semb2_sam、GNC任务更新同步数据信号量semb3_sam、更新星敏感器数据信号semb_q、更新速率陀螺数据信号量semb_omega、更新光学导航相机数据信号量semb_p、更新加速度计数据信号semb_acel、速率陀螺控制采样周期信号量semb_sam_omega、加速度计控制采样周期信号量semb_sam_acel、星敏感器控制采样周期信号量semb_sam_q、星敏感器控制采样周期临时信号量semb_sam_q_temp、光学导航相机控制采样周期信号量semb_sam_p、光学导航相机控制采样周期临时信号量semb_sam_p_temp和控制GNC规划任务Task_GNC_pro每25ms最先执行信号量，所述控制GNC规划任务Task_GNC_pro每25ms最先执行信号量包括25ms执行周期执行完毕信号量semb和25ms定时器到期信号量semb_tm；

步骤1E、创建四个子任务：GNC规划任务Task_GNC_pro、数据采集任务Task_SAM_X、GNC任务Task_GNC和轨道确定任务Task_ORB_fuse；

步骤1F、设置定时器信号处理函数，分别为25ms定时器处理任务time_handler、2s定时器处理任务time_handler_one和15s定时器处理任务time_handler_two；

步骤1G、创建定时器；

步骤1H、获取主任务更新同步数据信号量semb1_sam的内容；

步骤1I、启动定时器，设定三个采样周期分别为25ms、2s和15s；

步骤1J、获取状态指标信号量semaphore的内容；

步骤1K、当撞击器状态满足撞击条件时，进行信号量的删除、定时器的删除、任务的删除操作。

主任务main_task的优先级(101)最高，用于完成各部分的初始化，设置系统时钟中断频率(200Hz)及系统当前时间，设置任务调度策略(基于任务优先级的时间片轮转调度)，完成对各信号量、子任务及定时器的创建，并在分别获取得到相关信号量后，启动定时器及进行任务结束处理。

步骤1D中二进制信号量的初始值为empty，表示不可用，阻塞在信号量上，按照先入先出顺序等待信号量。

更新敏感器数据信号量主任务更新同步数据信号量semb1_sam用于保证目标机与外部接口启动同步，即在外部系统启动后，目标机第一次接收到外部接口的传输数据后，再启动目标机定时器开始计时，控制数据采集周期及任务执行周期；获取状态指标信号量semaphore用于保证撞击探测器的状态量达到要求指标后，再进行任务结束相关处理。

具体实施方式三、结合图3说明本实施方式，本实施方式是对具体实施方式一中的GNC规划任务Task_GNC_pro的进一步说明：所述GNC规划任务Task_GNC_pro的工作过程如下：

步骤2A、判断是否为首次执行，用i表示执行次数，如果是首次执行，用i＝0表示，执行步骤2B；如果不是首次执行，则执行步骤2C；

步骤2B、获取系统当前时间；然后执行步骤2G；

步骤2C、判断是否为第一个执行周期，即判断i是否为1；如果是，则执行步骤2D；如果否，则执行步骤2E；

步骤2D、获取更新敏感器数据信号量GNC规划任务更新同步数据信号量semb2_sam的内容；然后执行步骤2F；

步骤2E、获取25ms执行周期执行完毕信号量semb和25ms定时器到期信号量semb_tm的内容；当25ms执行周期执行完毕信号量semb或25ms定时器到期信号量semb_tm有效时；

步骤2F、计算首次执行中等待外部仿真机dSpace启动运算及数据传输时间，并利用其对系统时间进行修正；

步骤2G、根据探测器估计位置、探测器估计速度、探测器估计姿态、估计角速度、探测器期望姿态、探测器期望角速度、期望增量速度、加速度测量值和探测器上一时刻所处飞行状态，计算出探测器轨道机动标识位flag_mot_orb、探测器姿态机动标识位flag_mot_ati、机动点火标识位flag_mot_ign，从而判断当前探测器所处的飞行状态；

步骤2H、令执行次数i＝i+1，本次任务执行完毕，将25ms执行周期执行完毕信号量semb置位有效。

GNC规划任务Task_GNC_pro的优先级(101)仅次于主任务main_task，旨在每个25ms执行周期的首部，根据当前撞击探测器的轨道/姿态状态量，给出轨道/姿态/点火机动标志位，为同周期GNC任务Task_GNC下的功能函数执行顺序提供判断依据。任务流程图如图7所示。

判断条件i＝0的设置是为解决目标机与外部接口启动同步问题所必需的，在目标机启动后首次执行各任务时并不与外部接口发生数据传输关系，而是利用自带数据初值进行一步运算，为与外部接口同步运行做准备；出于相同目的设置判断条件“是否为第一执行周期”用于保证目标机与外部接口的启动同步，即在外部系统启动后，目标机第一次接收到外部接口的传输数据后(此处实现表现为获取得到信号量GNC规划任务更新同步数据信号量semb2_sam)，再进入第一个25ms执行周期的任务规划；而信号量semb_tm则是为了保证GNC规划任务Task_GNC_pro在每一个25ms执行周期的首部占用CPU运行，信号量semb用于保证上一个25ms执行周期完成所有任务的运行，再进入下一个执行周期；为了使目标机中的计算所用时间与外部系统的运算时间一致，在每一个执行周期中，GNC任务规划要把目标机系统时间中用于保证两机同步启动的首次执行GNC任务Task_GNC后等待外部接口传输数据的时间修正掉。

外部仿真机dSpace主要模拟探测器动力学部分。

具体实施方式四、本实施方式是对具体实施方式一中的数据采集任务Task_SAM_X的进一步说明：数据采集任务Task_SAM_X中的4个数据采集子任务为定时器触发的工作方式，其中速率陀螺数据采集任务Task_SAM_omega和加速度计数据采集任务Task_SAM_acel的数据采样周期为25ms，；

星敏感器数据采集任务Task_SAM_q的采样周期为2s，在执行星敏感器数据采集任务Task_SAM_q的时候，首先判断轨道和姿态是否有机动的情况，如果轨道或姿态有机动，则退出任务本任务，否则采集星敏感器数据；

光学导航相机数据采集任务Task_SAM_p的采样周期为15s，在执行光学导航相机数据采集任务Task_SAM_p的时候，首先判断轨道和姿态是否有机动情况，当轨道和姿态均无机动情况下执行数据采集任务，并记录所述相机拍照时刻的轨道/姿态状态信息，否则退出任务。

数据采集任务Task_SAM_X的优先级(102)仅次于GNC规划任务Task_GNC_pro，用于在不同采样周期读取各敏感器的测量数据。

具体实施方式五、结合图4说明本实施方式，本实施方式是对具体实施方式四中的速率陀螺数据采集任务Task_SAM_omega的进一步说明，速率陀螺数据采集任务Task_SAM_omega的工作过程如下：

步骤3A、获取速率陀螺控制采样周期信号量semb_sam_omega；

步骤3B、将采集到的陀螺数据写入数据缓冲区；

步骤3C、释放更新陀螺数据信号量semb_omega。

具体实施方式六、结合图5说明本实施方式，本实施方式是对具体实施方式四中的加速度计数据采集任务Task_SAM_acel的进一步说明，加速度计数据采集任务Task_SAM_acel的工作过程如下：

步骤3D、获取加速度计控制采样周期信号量semb_sam_acel；

步骤3E、将采集到的加速度计数据写入数据缓冲区；

步骤3F、释放更新加速度计数据信号semb_acel。

具体实施方式七、结合图6说明本实施方式，本实施方式是对具体实施方式四中的星敏感器数据采集任务Task_SAM_q的进一步说明，星敏感器数据采集任务Task_SAM_q的工作过程如下：

步骤3G、获取星敏感器控制采样周期信号量semb_sam_q和星敏感器控制采样周期临时信号量semb_sam_q_temp；

步骤3H、判断探测器轨道机动标识位flag_mot_orb和探测器姿态机动标识位flag_mot_ati的内容，如果探测器轨道机动标识位flag_mot_orb＝0并且探测器姿态机动标识位flag_mot_ati＝0，则执行步骤3I；否则，不做任何处理，本次任务结束；

步骤3I、将调用姿态确定功能函数的标志位flag_tm的内容置1，即令flag_tm＝1；

步骤3J、将采集到的陀螺数据写入数据缓冲区；

步骤3K、释放更新星敏感器数据信号semb_q。

具体实施方式八、结合图7说明本实施方式，本实施方式是对具体实施方式四中的光学导航相机数据采集任务Task_SAM_p的进一步说明，光学导航相机数据采集任务Task_SAM_p的工作过程如下：

步骤3L、获取光学导航相机控制采样周期信号量semb_sam_p和光学导航相机控制采样周期临时信号量semb_sam_p_temp；

步骤3M、判断探测器轨道机动标识位flag_mot_orb和探测器姿态机动标识位flag_mot_ati的内容，如果探测器轨道机动标识位flag_mot_orb＝0并且探测器姿态机动标识位flag_mot_ati＝0，则执行步骤3N；否则，不做任何处理，本次任务结束；

步骤3N、获取拍照时刻系统时间，并记录拍照时刻对应的撞击轨道/姿态状态量，所述撞击轨道/姿态状态量包括撞击器位置和速度；

步骤3O、将采集到的光学导航相机数据写入数据缓冲区；

步骤3P、释放更新光学导航相机数据信号量semb_p。

具体实施方式九、结合图8、图9、图10和图11说明本实施方式，本实施方式是对具体实施方式一中的GNC任务Task_GNC的进一步说明，GNC任务Task_GNC一个周期内的具体步骤如下：

步骤4A、判断是否为首次执行，如果是，则执行步骤4B，如果否，则执行步骤4L；

首次执行GNC任务Task_GNC情况如下：

步骤4B、根据轨道面法线方向矢量、探测器估计姿态和探测器估计角速度，计算出探测器期望的姿态余弦阵和探测器三轴期望角速度，从而获得探测器定向姿态的期望姿态；

步骤4C、判断探测器姿态机动标识位flag_mot_ati的内容，如果flag_mot_ati＝0，则执行步骤4D，否则，即flag_mot_ati＝1，则执行步骤4E；

步骤4D、根据探测器估计姿态、探测器估计角速度、探测器期望姿态和探测器期望角速度，计算出反作用飞轮控制信号，从而获得反作用飞轮控制量；然后执行步骤4G；

步骤4E、根据探测器估计姿态、探测器估计角速度、探测器期望姿态和探测器期望角速度，计算出三轴喷嘴开关序列，从而获得喷嘴开关的逻辑；

步骤4F、将系统当前时间结构体数据中的秒级分量t_current.tv_sec加100，并赋值给所需的稳定时间t_ati；即令如果t_ati＝t_current.tv_sec+100；

步骤4G、根据陀螺输出的姿态角速度、陀螺漂移更新值和上一时刻探测器估计姿态，计算出探测器估计姿态和探测器估计角速度，从而递推出探测器相对惯性空间的姿态矩阵；

步骤4H、根据探测器估计姿态、探测器预测位置、探测器预测速度、加速度测量值和机动点火标识位，计算出探测器估计位置和探测器估计速度，从而递推出探测器位置和速度；

步骤4I、将计算得到的指令写入相应的指令缓冲区；

步骤4J、接收外部仿真机dSpace动力学模块传入的敏感器数据，并将采集到的数据写入相应的数据缓冲区；

步骤4K、释放主任务更新同步数据信号量semb1_sam，GNC规划任务更新同步数据信号量semb2_sam和GNC任务更新同步数据信号量semb3_sam；

步骤4L、判断是否为第二次执行；如果是，则执行步骤4M，如果否，则执行步骤4Z；

第二次执行GNC任务Task_GNC情况如下：

步骤4M、获取GNC任务更新同步数据信号量semb3_sam；

步骤4N、将首次任务执行的指令发出，并将相应指令缓冲区置0；

步骤4O、根据轨道面法线方向矢量、探测器估计姿态和探测器估计角速度，计算出探测器期望的姿态余弦阵和探测器三轴期望角速度，从而获得探测器定向姿态的期望姿态；

步骤4P、判断探测器姿态机动标识位flag_mot_ati的内容，如果flag_mot_ati＝0，则执行步骤4Q，否则，即flag_mot_ati＝1，则执行步骤4R；

步骤4Q、根据探测器估计姿态、探测器估计角速度、探测器期望姿态和探测器期望角速度，计算出反作用飞轮控制信号，从而获得反作用飞轮控制量；然后执行步骤4T；

步骤4R、根据探测器估计姿态、探测器估计角速度、探测器期望姿态和探测器期望角速度，计算出三轴喷嘴开关序列，从而获得喷嘴开关的逻辑；

步骤4S、将系统当前时间结构体数据中的秒级分量t_current.tv_sec加100，并赋值给所需的稳定时间t_ati；即令如果t_ati＝t_current.tv_sec+100；

步骤4T、根据陀螺输出的姿态角速度、陀螺漂移更新值和上一时刻探测器估计姿态，计算出探测器估计姿态和探测器估计角速度，从而递推出探测器相对惯性空间的姿态矩阵；

步骤4U、根据探测器估计姿态、探测器预测位置、探测器预测速度、加速度测量值和机动点火标识位，计算出探测器估计位置和探测器估计速度，从而递推出探测器位置和速度；

步骤4V、将串口通信丢数标志位flag_sam置1，即令flag_sam＝1；

步骤4W、记录本次执行周期轨道机动标志位flag_mot_orb_temp，令执行周期轨道机动标志位flag_mot_orb_temp的内容为探测器轨道机动标识位flag_mot_orb的内容，即flag_mot_orb_temp＝flag_mot_orb；

步骤4X、将计算得到指令写入相应的指令缓冲区，并在指令发出后，将相应的指令缓冲区置0；

步骤4Y、释放25ms执行周期执行完毕信号量semb；

第二次之后执行GNC任务Task_GNC情况如下：

步骤4Z、获取更新陀螺数据信号量semb_omega和更新加速度计数据信号semb_acel；然后执行步骤41；

步骤41、判断调用姿态确定功能函数的标志位flag_tm的内容，如果flag_tm＝1，则执行步骤42；否则，执行步骤45；

步骤42、将调用姿态确定功能函数的标志位flag_tm置0，即令flag_tm＝0；

步骤43、获取更新星敏感器数据信号semb_q；

步骤44、根据星敏感器输出的姿态四元数、陀螺输出的姿态角速度，计算出探测器估计姿态、探测器估计角速度、陀螺漂移更新值，从而实现通过卡尔曼滤波校正撞击探测器姿态和陀螺漂移；

步骤45、判断探测器轨道机动标识位flag_mot_orb的内容，如果flag_mot_orb＝0，则执行步骤46；否则，即flag_mot_orb＝1，执行步骤411；

步骤46、根据轨道面法线方向矢量、探测器估计姿态和探测器估计角速度，计算出探测器期望的姿态余弦阵和探测器三轴期望角速度，从而计算探测器定向姿态的期望姿态；

步骤47、判断探测器姿态机动标识位flag_mot_ati的内容，如果flag_mot_ati＝0，则执行步骤48，否则，即flag_mot_ati＝1，则执行步骤49；

步骤48、根据探测器估计姿态、探测器估计角速度、探测器期望姿态和探测器期望角速度，计算出反作用飞轮控制信号，从而获得反作用飞轮控制量；然后执行步骤416；

步骤49、根据探测器估计姿态、探测器估计角速度、探测器期望姿态和探测器期望角速度，计算出三轴喷嘴开关序列，从而获得喷嘴开关的逻辑；然后执行步骤410；

步骤410、将系统当前时间结构体数据中的秒级分量t_current.tv_sec加100，并赋值给所需的稳定时间t_ati；即令如果t_ati＝t_current.tv_sec+100；然后执行步骤416；

步骤411、判断执行周期轨道机动标志位flag_mot_orb_temp的内容，如果flag_mot_orb_temp＝0，则执行步骤412，否则，即flag_mot_orb_temp＝1，则执行步骤413；

步骤412、根据探测器估计位置、探测器估计速度和期望撞击目标点，计算出期望增量速度，从而获得接近轨道需要的增量速度；

步骤413、根据探测器估计位置和制导增量速度，计算出探测器期望的姿态余弦阵和探测器三轴期望角速度，从而获得探测器定向姿态的期望姿态；

步骤414、根据探测器估计姿态、探测器估计角速度、探测器期望姿态和探测器期望角速度，计算出三轴喷嘴开关序列，从而获得喷嘴开关的逻辑；

步骤415、将系统当前时间结构体数据中的秒级分量t_current.tv_sec加100，并赋值给所需的稳定时间t_ati；即令如果t_ati＝t_current.tv_sec+100；然后执行步骤416；

步骤416、根据陀螺输出的姿态角速度、陀螺漂移更新值和上一时刻探测器估计姿态，计算出探测器估计姿态和探测器估计角速度，从而获得探测器相对惯性空间的姿态矩阵；

步骤417、根据探测器估计姿态、探测器预测位置、探测器预测速度、加速度测量值和机动点火标识位，计算出探测器估计位置和探测器估计速度，从而获得探测器位置和速度；

步骤418、记录本次执行周期轨道机动标志位flag_mot_orb_temp，令执行周期轨道机动标志位flag_mot_orb_temp的内容为探测器轨道机动标识位flag_mot_orb的内容，即flag_mot_orb_temp＝flag_mot_orb；

步骤419、将计算得到指令写入相应的指令缓冲区，并在指令发出后，将相应的指令缓冲区置0；

步骤420、释放25ms执行周期执行完毕信号量semb。

GNC任务的优先级(103)仅次于数据采集任务，在该任务中应根据GNC规划任务给出的各个标志位的不同组合来确定不同的飞行模式，并根据数据采集任务得到的当前时刻敏感器更新数据，完成在每个25ms执行周期内对撞击探测器状态的导航、制导与控制任务。具体飞行模式划分如下：

1)飞行模式I

探测器轨道机动标识位为0且探测器姿态机动标识位为0(flag_mot_orb＝＝0&&flag_mot_ati＝＝0)。

存在星敏感器更新数据：

在此模式中，由于星敏感器数据有更新值，因此，此时执行星敏感器+速率陀螺+EKF联合滤波，对撞击探测器姿态进行确定。

不存在星敏感器更新数据：

在此模式中，单纯地利用陀螺信息结合姿态运动学方程对探测器姿态进行估计。

2)飞行模式II

探测器轨道机动标识位为0且探测器姿态机动标识位为1(flag_mot_orb＝＝0&&flag_mot_ati＝＝1)。

该模式主要应用在撞击探测器将姿态转向期望点火姿态，与完成点火后将姿态转回对目标天体定向的过程中。在该模式下，采用喷嘴对姿态进行控制，利用常规飞行模式姿态制导算法给出期望姿态。

3)飞行模式III

探测器轨道机动标识位为1(flag_mot_orb＝＝1)。

该模式为轨道机动模式，首先由撞击制导律给出期望的点火方向，姿态制导函数产生期望的姿态使发动机指向该点火方向，利用姿控喷嘴完成姿态机动。由于在本模式下，姿态处于机动与粗稳定状态，星敏感器数据不能得到应用，因此在本模式下，只能利用速率陀螺进行姿态估计。

判断是否为首次执行，在目标机启动后首次执行各任务时并不与外部接口发生数据传输关系，而是利用自带数据初值进行一步运算，为与外部接口同步运行做准备；而信号量GNC任务更新同步数据信号量semb3_sam用于保证目标机与外部系统的启动同步，即在外部系统启动后，目标机第一次接收到外部接口传输的更新数据后，再进入第一个25ms执行周期的GNC任务执行；变量t_ati是用于解决姿态机动后需要稳定100s左右的时间，以确保本次轨道确定是在姿态相对稳定的情况下进行，t_ati的设置即为在轨道确定任务中判断是否适合进行轨道确定提供依据；标志位flag_sam是为解决串口通信丢数问题而设置的，其初值为0(flag_sam＝0)，则数据采集功能函数revbyte在程序首次执行时进入死循环等待外部系统启动，直到接收到外部接口传来的第一组数据(由于是首次执行定时器并未启动，故无定时限值)，而当进入第一个执行周期的同时，定时器启动，则将该标志位置1(flag_sam＝1)，此时函数revbyte若多于三次循环还未成功接收到数据，则沿用上一步数据。

具体实施方式十、结合图12说明本实施方式，本实施方式是对具体实施方式一中的轨道确定任务Task_ORB_fuse的进一步说明，轨道确定任务Task_ORB_fuse一个周期内的具体步骤如下：

步骤5A、获取更新光学导航相机数据信号量semb_p；

步骤5B、判断执行姿态大角度控制后所需的稳定时间t_ati与系统当前时间结构体数据中的秒级分量t_current.tv_sec的大小关系；如果t_current.tv_sec＞t_ati，则执行步骤5C；否则，不做任何处理，本次任务结束；

步骤5C、根据目标天体光心像素坐标、探测器估计姿态、探测器预测位置、探测器预测速度和预测误差方差阵，计算出探测器估计位置、探测器估计速度和估计误差方差阵；

步骤5D、记录本次轨道机动标志位fag_orb_fuse＝1。

轨道确定任务的优先级(104)最低，该任务用于在稳定状态下(即轨道/姿态均无机动)，利用光学导航相机数据对轨道进行修正。受到相机图像处理及传输时间的限制，在满足拍照条件的情况下，轨道确定每15s进行一次，以提高导航精度。

更新光学导航相机数据信号量semb_p用于保证15s定时器到期，并且满足光学导航相机拍照条件，同时采集到相应图像信息的情况下，执行轨道确定；判断条件t_current.tv_sec＞t_ati为判断当前时间是否与最近一次姿态机动时间间隔足够长，以确保本次轨道确定是在姿态相对稳定的情况下进行。

具体实施方式十一、结合13说明本实施方式，本实施方式是对具体实施方式二、三、五、六或九中25ms定时器处理任务time_handler的进一步说明：25ms定时器处理任务time_handler一个周期内的具体步骤如下：

步骤6A、判断是否满足撞击条件，即判断撞击器轨道状态量r_esti[0]是否大于0，如果r_esti[0]＞0，则执行步骤6B；否则，则执行步骤6D；

步骤6B、停止定时器；

步骤6C、释放状态指标信号量semaphore；本次任务结束；

步骤6D、判断探测器轨道机动标识位flag_mot_orb和探测器姿态机动标识位flag_mot_ati的内容，如果探测器轨道机动标识位flag_mot_orb＝1或者探测器姿态机动标识位flag_mot_ati＝1，则执行步骤6E；否则，本次任务结束；

步骤6E、将机动标志位flag_mot置1，即令flag_mot＝1；

步骤6F、释放25ms定时器到期信号量semb_tm；

步骤6G、采集外部仿真机dSpace动力学部分传入的敏感器数据；

步骤6H、释放速率陀螺控制采样周期信号量semb_sam_omega、加速度计控制采样周期信号量semb_sam_acel、星敏感器控制采样周期临时信号量semb_sam_q_temp和光学导航相机控制采样周期临时信号量semb_sam_p_temp。

判断条件为轨道机动标志位和姿态机动标志位至少其中一位为1，进行此判断为是否进行轨道修正提供判断依据(机动标志位flag_mot)；信号量semb_tm用于保证每一个25ms执行周期的控制，而信号量semb_sam_X则是用于控制敏感器数据的采样周期。

具体实施方式十二、结合14说明本实施方式，本实施方式是对具体实施方式七中2s定时器处理任务time_handler_one的进一步说明：2s定时器处理任务time_handler_one一个周期内的具体步骤如下：

步骤6I、判断是否满足撞击条件，即判断撞击器轨道状态量r_esti[0]是否大于0，如果是，则执行步骤6J；如果否，则执行步骤6L；

步骤6J、停止定时器；

步骤6K、释放状态指标信号量semaphore；本次任务结束；

步骤6L、释放星敏感器控制采样周期信号量semb_sam_q。

信号量星敏感器控制采样周期信号量semb_sam_q用于控制星敏感器数据的采样周期为2s。

具体实施方式十三、结合15说明本实施方式，本实施方式是对具体实施方式八或十中15s定时器处理任务time_handler_two的进一步说明：15s定时器处理任务time_handler_two一个周期内的具体步骤如下：

步骤6M、判断是否满足撞击条件，即判断撞击器轨道状态量r_esti[0]是否大于0，如果是，则执行步骤6N；如果否，则执行步骤6P；

步骤6N、停止定时器；

步骤6O、释放状态指标信号量semaphore；本次任务结束；

步骤6P、判断机动标志位flag_mot和轨道确定标志位flag_orb_fuse的内容，如果机动标志位flag_mot＝0并且轨道确定标志位flag_orb_fuse＝1，则执行步骤6Q；否则，执行步骤6S；

步骤6Q、将轨道确定标志位flag_orb_fuse重新置0，即令flag_orb_fuse＝0；

步骤6R、利用轨道确定结果对撞击器的位置和速度进行修正；然后执行步骤6T；

步骤6S、将机动标志位flag_mot置0，即令flag_mot＝0；并且将轨道确定标志位flag_orb_fuse置0，即令flag_orb_fuse＝0；

步骤6T、释放光学导航相机控制采样周期信号量semb_sam_p。

判断条件flag_mot＝0&&flag_orb_fuse＝1为上一个15s内无机动且执行完成一次轨道确定，在此情况下可利用轨道确定的结果对撞击探测器的当前状态量进行一次修正；信号量光学导航相机控制采样周期信号量semb_sam_p用于控制光学导航相机数据的采样周期为15s。