CN110986929B - 导航与控制周期不同步的飞行控制方案的软件实现方法 - Google Patents
导航与控制周期不同步的飞行控制方案的软件实现方法 Download PDFInfo
- Publication number
- CN110986929B CN110986929B CN201911167150.0A CN201911167150A CN110986929B CN 110986929 B CN110986929 B CN 110986929B CN 201911167150 A CN201911167150 A CN 201911167150A CN 110986929 B CN110986929 B CN 110986929B
- Authority
- CN
- China
- Prior art keywords
- algorithm
- period
- control
- inertial navigation
- navigation
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明公开了导航与控制周期不同步的飞行控制方案的软件实现方法,包括通信连接的惯性导航模块和控制周期模块,所述的方法包括:将惯性导航算法运行在定时器中断服务函数中;通过在定时器中断服务函数中置控制周期标志、Main函数中while循环读取控制周期标志的方式给控制算法提供在Main函数中的运算周期,将组合导航修正算法运行在Main函数的控制算法周期中;本发明将惯性导航算法运行在定时器中断服务函数中,控制算法周期运行在Main函数中,简化了导航与控制算法的数据交互时的读写锁,避免了潜在的死锁现象。
Description
技术领域
本发明涉及飞行控制软件设计技术领域,特别是一种导航周期与控制周期不同步控制方案的软件实现方法。
背景技术
飞行控制软件是各种飞行器***中的核心软件,其控制算法依赖于导航精度。缩短惯性导航运算周期是提升导航精度的一个有效方法。当惯性导航运算周期缩小到一定程度后,由于在惯性导航算法之后运行的组合导航修正算法和控制算法无法在一个惯性导航周期内运算完成,故而出现了导航周期与控制周期不同步的控制方案。通常的设计中,导航周期与控制周期成倍数关系,例如控制周期是惯性导航周期的两倍。
由于运行在控制周期中的组合导航修正算法和控制算法需要使用惯性导航算法的输出作为输入,而组合导航算法要修正当前的惯性导航输出并作为下个惯性导航周期的输入,因此在导航周期与控制周期不同步的控制方案中,如何处理导航与控制算法的输入输出数据传递过程是非常关键的问题。
发明内容
为解决现有技术中存在的问题,本发明的目的是提供一种导航与控制周期不同步的飞行控制方案的软件实现方法,本发明简化了程序结构,同时避免了死锁的出现。
为实现上述目的,本发明采用的技术方案是:一种导航与控制周期不同步的飞行控制方案的软件实现方法,包括通信连接的惯性导航模块和控制周期模块,所述控制周期模块用于接收卫星信号,并对卫星信号的有效性进行判断,以及接收所述惯性导航模块输出的导航数据,所述惯性导航模块用于接收所述控制周期模块输出的组合导航修正数据,并判断是否需要用所述组合导航修正数据代替上一个周期的惯性导航算法的结果作为本周期惯性导航模块中惯性导航算法的输入;所述的方法还包括:
将预置于所述惯性导航模块中的惯性导航算法运行在定时器中断服务函数中,通过在定时器中断服务函数中置控制周期标志、Main函数中while循环读取控制周期标志的方式给预置于所述控制周期模块中的控制算法提供在Main函数中的运算周期,并且将预置于控制周期模块中的组合导航修正算法运行在Main函数的控制算法周期中,若所述控制周期模块收到卫星信号且对卫星的定位结果与当前控制周期模块中的惯性导航结果在预先设定的阈值范围内,则认为卫星信号有效,此时利用预置于控制周期模块中的组合导航修正算法对卫星进行导航修正,若惯性导航模块判断控制周期模块中Main函数上个控制算法周期进行了组合导航算法修正,则将修正结果代替上一个周期的惯性导航算法结果,然后再进行本周期的惯性导航解算。
作为一种优选的实施方式,所述控制周期标志根据控制算法周期与惯性导航算法周期的倍数关系进行设置,具体的倍数关系决定通过多少次惯性导航算法的中断置一次控制周期标志。
作为另一种优选的实施方式,还包括:
当惯性导航算法运行完定时器中断服务函数,进入控制算法周期时,将最新的惯性导航算法的解算结果复制一份用于进入的控制算法周期的解算。
作为另一种优选的实施方式,所述惯性导航算法的解算结果包括姿态角、速度、位置以及关键的中间变量。
作为另一种优选的实施方式,还包括:当组合导航修正算法完成后,将完成标志置位,提供给定时器中断服务函数中的惯性导航算法。
作为另一种优选的实施方式,在所述组合导航修正算法过程中对相关变量进行加锁。
本发明的有益效果是:本发明将惯性导航算法运行在定时器中断服务函数中,控制算法周期运行在Main函数中,简化了导航与控制算法的数据交互时的读写锁,避免了潜在的死锁现象。
附图说明
图1为本发明实施例中信号的流向示意图;
图2为本发明实施例中算法运行时序图;
图3为本发明实施例中定时器中断服务函数逻辑流程图;
图4为本发明实施例Main函数中控制算法周期的逻辑流程图。
具体实施方式
下面结合附图对本发明的实施例进行详细说明。
实施例
如图1所示,一种导航与控制周期不同步的飞行控制方案的软件实现方法,包括通信连接的惯性导航模块和控制周期模块,所述控制周期模块用于接收卫星信号,并对卫星信号的有效性进行判断,以及接收所述惯性导航模块输出的导航数据,所述惯性导航模块用于接收所述控制周期模块输出的组合导航修正数据,并判断是否需要用所述组合导航修正数据代替上一个周期的惯性导航算法的结果作为本周期惯性导航模块中惯性导航算法的输入;所述的方法还包括:
将预置于所述惯性导航模块中的惯性导航算法运行在定时器中断服务函数中,通过在定时器中断服务函数中置控制周期标志、Main函数中while循环读取控制周期标志的方式给预置于所述控制周期模块中的控制算法提供在Main函数中的运算周期,并且将预置于控制周期模块中的组合导航修正算法运行在Main函数的控制算法周期中,若所述控制周期模块收到卫星信号且对卫星的定位结果与当前控制周期模块中的惯性导航结果在预先设定的阈值范围内,则认为卫星信号有效,此时利用预置于控制周期模块中的组合导航修正算法对卫星进行导航修正,若惯性导航模块判断控制周期模块中Main函数上个控制算法周期进行了组合导航算法修正,则将修正结果代替上一个周期的惯性导航算法结果,然后再进行本周期的惯性导航解算。
进入控制算法周期时,必然是刚运行完中断服务函数,此时将最新的惯性导航算法的解算结果(如姿态角、速度、位置以及一些关键的中间变量)复制一份用于本控制算法周期的解算。复制一份的目的是为了保证本控制算法周期自始至终使用的导航输出数据是一致的,防止在控制算法周期的解算过程中触发了下一次中断并更新了导航输出数据。由于组合导航修正算法修正了惯性导航算法输出的速度和位置,因此在组合导航算法的修正过程中需要对相关变量加锁,以保证修正完成后数据才能给定时器中断服务函数中的惯性导航算法使用。
定时器中断触发后,进入中断服务函数中的惯性导航算法周期,在进行导航解算前,需判断是否需要用组合导航修正算法修正后的数据代替上周期的惯性导航算法的结果作为本周期惯性导航算法的输入。在一般情况下,控制算法周期短于卫星信号周期,而本实施例所述情况下,惯性导航算法周期比控制算法周期还要短。也就是说,经过多个惯性导航算法周期后才会利用卫星信号进行一次组合导航修正算法进行修正。因此,每次进入中断服务函数的惯性导航算法周期进行解算前,首先需要判断Main函数中上个控制算法周期是否进行了组合导航算法修正,如果确实修正了上周期的惯性导航结果,则将修正结果(即Main函数中解算出的结果)代替上周期的惯性导航算法结果(前一导航周期在中断服务函数中解算的结果),然后再进行本周期的惯性导航解算;总的来说,本周期惯导解算需要用到上周期的解算结果作为输入,由于上周期的Main函数中对惯导结果进行了组合导航修正,因此这个修正结果要体现在本周期中断服务函数的惯性导航解算里。
由于惯性导航算法的周期运行在定时器中断服务函数中,不必担心运行过程中被Main函数中的控制算法周期打断,因此本发明可以省去在惯性导航算法过程中给变量加锁的操作,简化了程序结构,同时也有效避免了死锁的出现。
下面对本实施例作进一步地说明:
其中的一个实施例采用5ms惯性导航算法周期和10ms控制算法周期的方案,该实施例的算法运行时序如图2所示。由于组合导航修正算法和控制算法总运行时间超过了5ms,故存在惯性导航算法打断控制算法周期的情况。
定时器中断服务函数中的惯性导航算法周期的逻辑流程图如图3所示,由于在控制算法周期中使用的是惯性导航算法相关变量的一个副本,因此进入惯性导航算法周期时,如果检测到组合导航修正算法完成标志已置位,则将标志位清除,并将上周期的惯性导航算法的输出结果更新为经组合导航算法修正后的结果并作为本周期惯性导航算法的输入。
Main函数中的控制算法周期的逻辑流程图如图4所示。进入控制算法周期后,首先需要复制一份惯性导航算法输出的相关变量,包括姿态角、速度、位置等。然后根据是否收到卫星信号以及信号有效性等因素判断本控制算法周期是否需要进行组合导航算法修正,由于卫星信号周期一般长于控制算法周期(比如控制算法周期为10ms,而卫星信号周期为100ms),因此必须在每个控制算法周期中判断本周期是否收到了卫星信号,若收到卫星信号且卫星定位结果与当前控制周期的惯性导航结果在事先设定的阈值范围内,则认为卫星信号有效,在这种情况下,本控制周期才进行组合导航修正。组合导航算法修正完成后,将完成标志置位,提供给中断服务函数中的惯性导航算法。最后运行控制算法,完成一个完整控制周期的解算。
以上所述实施例仅表达了本发明的具体实施方式,其描述较为具体和详细,但并不能因此而理解为对本发明专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本发明构思的前提下,还可以做出若干变形和改进,这些都属于本发明的保护范围。
Claims (5)
1.一种导航与控制周期不同步的飞行控制方案的软件实现方法,其特征在于,包括通信连接的惯性导航模块和控制周期模块,所述控制周期模块用于接收卫星信号,并对卫星信号的有效性进行判断,以及接收所述惯性导航模块输出的导航数据,所述惯性导航模块用于接收所述控制周期模块输出的组合导航修正数据,并判断是否需要用所述组合导航修正数据代替上一个周期的惯性导航算法的结果作为本周期惯性导航模块中惯性导航算法的输入;所述的方法还包括:
将预置于所述惯性导航模块中的惯性导航算法运行在定时器中断服务函数中,通过在定时器中断服务函数中置控制周期标志、Main函数中while循环读取控制周期标志的方式给预置于所述控制周期模块中的控制算法提供在Main函数中的运算周期,并且将预置于控制周期模块中的组合导航修正算法运行在Main函数的控制算法周期中,若所述控制周期模块收到卫星信号且对卫星的定位结果与当前控制周期模块中的惯性导航结果在预先设定的阈值范围内,则认为卫星信号有效,此时利用预置于控制周期模块中的组合导航修正算法对卫星进行导航修正,若惯性导航模块判断控制周期模块中Main函数上个控制算法周期进行了组合导航算法修正,则将修正结果代替上一个周期的惯性导航算法结果,然后再进行本周期的惯性导航解算;
所述控制周期标志根据控制算法周期与惯性导航算法周期的倍数关系进行设置,具体的倍数关系决定通过多少次惯性导航算法的中断置一次控制周期标志。
2.根据权利要求1所述的导航与控制周期不同步的飞行控制方案的软件实现方法,其特征在于,还包括:
当惯性导航算法运行完定时器中断服务函数,进入控制算法周期时,将最新的惯性导航算法的解算结果复制一份用于进入的控制算法周期的解算。
3.根据权利要求2所述的导航与控制周期不同步的飞行控制方案的软件实现方法,其特征在于,所述惯性导航算法的解算结果包括姿态角、速度、位置以及关键的中间变量。
4.根据权利要求1所述的导航与控制周期不同步的飞行控制方案的软件实现方法,其特征在于,还包括:当组合导航修正算法完成后,将完成标志置位,提供给定时器中断服务函数中的惯性导航算法。
5.根据权利要求1所述的导航与控制周期不同步的飞行控制方案的软件实现方法,其特征在于,在所述组合导航修正算法过程中对相关变量进行加锁。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911167150.0A CN110986929B (zh) | 2019-11-25 | 2019-11-25 | 导航与控制周期不同步的飞行控制方案的软件实现方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911167150.0A CN110986929B (zh) | 2019-11-25 | 2019-11-25 | 导航与控制周期不同步的飞行控制方案的软件实现方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110986929A CN110986929A (zh) | 2020-04-10 |
CN110986929B true CN110986929B (zh) | 2023-05-02 |
Family
ID=70086612
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911167150.0A Active CN110986929B (zh) | 2019-11-25 | 2019-11-25 | 导航与控制周期不同步的飞行控制方案的软件实现方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110986929B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113465593B (zh) * | 2021-04-20 | 2022-01-25 | 星河动力(北京)空间科技有限公司 | 火箭飞行导航方法、装置、设备、***及存储介质 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6317688B1 (en) * | 2000-01-31 | 2001-11-13 | Rockwell Collins | Method and apparatus for achieving sole means navigation from global navigation satelite systems |
CN109781100A (zh) * | 2019-03-08 | 2019-05-21 | 哈尔滨工程大学 | 一种基于卡尔曼滤波估计的极区组合导航双通道误差校正方法 |
Family Cites Families (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7274504B2 (en) * | 2005-01-14 | 2007-09-25 | L-3 Communications Corporation | System and method for advanced tight coupling of GPS and inertial navigation sensors |
CN100498232C (zh) * | 2007-08-06 | 2009-06-10 | 北京航空航天大学 | 一种sdins/gps组合导航***时间同步及同步数据提取方法 |
CN102426373A (zh) * | 2011-09-01 | 2012-04-25 | 中国航空工业第六一八研究所 | 一种惯性/卫星组合导航***开闭环混合修正方法 |
CN102636798B (zh) * | 2012-04-12 | 2013-10-02 | 南京航空航天大学 | 基于环路状态自检测的sins/gps深组合导航方法 |
CN104913790B (zh) * | 2015-05-28 | 2017-11-28 | 北京航天控制仪器研究所 | 一种应用于动中通的惯导***航向漂移误差闭环补偿方法 |
CN106855633B (zh) * | 2015-12-08 | 2019-06-11 | 中国航空工业第六一八研究所 | 一种同步提取惯性卫星组合量测数据的方法 |
CN106775659B (zh) * | 2016-11-28 | 2020-01-31 | 四川航天***工程研究所 | 基于高速Linkport接口的嵌入式双核飞行控制软件架构方法 |
CN107643534B (zh) * | 2017-09-11 | 2019-07-12 | 东南大学 | 一种基于gnss/ins深组合导航的双速率卡尔曼滤波方法 |
CN108279430A (zh) * | 2017-12-25 | 2018-07-13 | 广州市中海达测绘仪器有限公司 | 数据同步定位的方法、装置、计算机设备及存储介质 |
CN109634139B (zh) * | 2018-12-10 | 2021-12-07 | 中国航天空气动力技术研究院 | 高超声速飞行器导航和控制***半实物仿真***及方法 |
-
2019
- 2019-11-25 CN CN201911167150.0A patent/CN110986929B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US6317688B1 (en) * | 2000-01-31 | 2001-11-13 | Rockwell Collins | Method and apparatus for achieving sole means navigation from global navigation satelite systems |
CN109781100A (zh) * | 2019-03-08 | 2019-05-21 | 哈尔滨工程大学 | 一种基于卡尔曼滤波估计的极区组合导航双通道误差校正方法 |
Also Published As
Publication number | Publication date |
---|---|
CN110986929A (zh) | 2020-04-10 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN101243407B (zh) | 用于控制计算器***的方法和装置 | |
US6212660B1 (en) | Methods and apparatuses for identification of the position of data packets which are located in a serial received data stream | |
US20070260939A1 (en) | Error filtering in fault tolerant computing systems | |
US7987385B2 (en) | Method for high integrity and high availability computer processing | |
US20070220367A1 (en) | Fault tolerant computing system | |
US5572620A (en) | Fault-tolerant voter system for output data from a plurality of non-synchronized redundant processors | |
CN102656568A (zh) | 微计算机及其动作方法 | |
JPH05216700A (ja) | リカバリ制御レジスタ及びリカバリ制御システム | |
CN110986929B (zh) | 导航与控制周期不同步的飞行控制方案的软件实现方法 | |
US6212134B1 (en) | Watch dog timer system | |
US5394407A (en) | Method of transferring error correcting code and circuit therefor | |
EP2247992B1 (en) | Clock switching circuits and methods | |
EP0482495B1 (en) | Finite-state machine for reliable computing and adjustment systems | |
WO2009015276A2 (en) | High integrity and high availability computer processing module | |
EP0239054B1 (en) | Process control system and method | |
US20100299557A1 (en) | Providing tuning limits for operational parameters in data processing apparatus | |
CN109707517A (zh) | 一种控制双通道同步的方法及*** | |
US20040003207A1 (en) | Program counter control method and processor | |
US6839834B2 (en) | Microprocessor protected against parasitic interrupt signals | |
US9977720B2 (en) | Method, information processing apparatus, and computer readable medium | |
US6122694A (en) | Serial bus controller | |
EP0265366B1 (en) | An independent backup mode transfer method and mechanism for digital control computers | |
US6209019B1 (en) | Data processing system, computer network, and data processing method | |
US7210051B2 (en) | System and method for handling state change conditions by a program status register | |
US20240007486A1 (en) | Signal detection apparatus, vehicle, and method |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |