CN110986929B - 导航与控制周期不同步的飞行控制方案的软件实现方法 - Google Patents

导航与控制周期不同步的飞行控制方案的软件实现方法 Download PDF

Info

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
Application number
CN201911167150.0A
Other languages
English (en)
Other versions
CN110986929A (zh
Inventor
张奔
姚天问
张波
陈华
邱翊峰
谢黛茜
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Sichuan Aerospace System Engineering Research Institute
Original Assignee
Sichuan Aerospace System Engineering Research Institute
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Sichuan Aerospace System Engineering Research Institute filed Critical Sichuan Aerospace System Engineering Research Institute
Priority to CN201911167150.0A priority Critical patent/CN110986929B/zh
Publication of CN110986929A publication Critical patent/CN110986929A/zh
Application granted granted Critical
Publication of CN110986929B publication Critical patent/CN110986929B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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所述的导航与控制周期不同步的飞行控制方案的软件实现方法，其特征在于，在所述组合导航修正算法过程中对相关变量进行加锁。
CN201911167150.0A 2019-11-25 2019-11-25 导航与控制周期不同步的飞行控制方案的软件实现方法 Active CN110986929B (zh)

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)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113465593B (zh) * 2021-04-20 2022-01-25 星河动力(北京)空间科技有限公司 火箭飞行导航方法、装置、设备、***及存储介质

Citations (2)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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 中国航天空气动力技术研究院 高超声速飞行器导航和控制***半实物仿真***及方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
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