CN114967716B - 一种基于四元数球面线性插值的固定翼飞机姿态控制方法 - Google Patents
一种基于四元数球面线性插值的固定翼飞机姿态控制方法 Download PDFInfo
- Publication number
- CN114967716B CN114967716B CN202210444221.2A CN202210444221A CN114967716B CN 114967716 B CN114967716 B CN 114967716B CN 202210444221 A CN202210444221 A CN 202210444221A CN 114967716 B CN114967716 B CN 114967716B
- Authority
- CN
- China
- Prior art keywords
- quaternion
- angle
- attitude
- control
- aircraft
- 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
- 238000000034 method Methods 0.000 title claims abstract description 39
- 230000003416 augmentation Effects 0.000 claims abstract description 21
- 238000004364 calculation method Methods 0.000 claims abstract description 13
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 238000010276 construction Methods 0.000 claims description 3
- 238000012937 correction Methods 0.000 claims description 3
- 230000008878 coupling Effects 0.000 claims description 3
- 238000010168 coupling process Methods 0.000 claims description 3
- 238000005859 coupling reaction Methods 0.000 claims description 3
- 230000002787 reinforcement Effects 0.000 abstract description 11
- 238000004422 calculation algorithm Methods 0.000 abstract description 10
- 238000012549 training Methods 0.000 abstract description 7
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 abstract description 6
- 230000006641 stabilisation Effects 0.000 abstract description 3
- 238000011105 stabilization Methods 0.000 abstract description 3
- 238000012546 transfer Methods 0.000 description 9
- 230000009466 transformation Effects 0.000 description 6
- 238000010586 diagram Methods 0.000 description 4
- 230000000694 effects Effects 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 230000004044 response Effects 0.000 description 3
- 230000009471 action Effects 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 238000005096 rolling process Methods 0.000 description 2
- 230000000087 stabilizing effect Effects 0.000 description 2
- 230000007704 transition Effects 0.000 description 2
- 230000006399 behavior Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000005538 encapsulation Methods 0.000 description 1
- 230000033001 locomotion Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000004806 packaging method and process Methods 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 230000000630 rising effect Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 238000011426 transformation method Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
- G05D1/0816—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft to ensure stability
- G05D1/0825—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft to ensure stability using mathematical models
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
- G05D1/106—Change initiated in response to external conditions, e.g. avoidance of elevated terrain or of no-fly zones
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Aviation & Aerospace Engineering (AREA)
- Mathematical Analysis (AREA)
- Pure & Applied Mathematics (AREA)
- Mathematical Physics (AREA)
- Mathematical Optimization (AREA)
- Algebra (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于四元数球面线性插值的固定翼飞机姿态控制方法，该方法包括：基于动力学模型搭建角速度、过载的增稳系统；解算得到当前姿态角，分别将当前姿态角和预设的目标姿态角转化为当前姿态四元数和目标姿态四元数；以及将目标姿态四元数和当前姿态四元数通过球面线性插值法计算得到插值姿态四元数；计算插值姿态四元数与当前姿态四元数的误差四元数并计算得到增稳系统指令；将增稳系统指令输入增稳系统，输出飞机舵面偏转角控制指令以控制飞机飞行姿态。本发明能够作为强化学习环境中飞机模型的控制系统，降低算法训练难度和研究人员建立飞控模型的工作量。
Description
技术领域
本发明涉及飞行控制领域，尤其涉及一种基于四元数球面线性插值的固定翼飞机姿态控制方法和装置。
背景技术
基于强化学习的飞行机动决策系统在空战对抗仿真中取得了一定进展，但飞机模型的逼真程度和控制难度始终影响算法收敛速度和训练效果。
有方案为实现机动控制，建立了飞机模型的基本指令集，并将其与机动动作映射，实现机动控制，但该方案的模型精度受限于基本指令集中的指令数量，同时随着基本指令数量上升，控制器输出的维度将随之上升。
综合来看，现有技术存在以下问题：
(1)为保证算法收敛，普遍采用简易(如三自由度)飞行器模型，与真实六自由度运动学模型存在较大差异；
(2)对飞控模型进行指令封装，导致机动灵活性下降；
(3)采用端到端的方案，利用强化学习算法直接学习机动动作，可解释性差，算法收敛难度大。
发明内容
本发明旨在至少在一定程度上解决相关技术中的技术问题之一。
为此，本发明的目的在于提出了一种基于四元数球面线性插值的固定翼飞机姿态控制方法，利用四元数控制器实现飞机姿态控制，有效避免欧拉角控制器在控制姿态变换时存在奇异点的情况，同时利用球面线性插值，实现模型任意姿态平滑转移。
本发明的另一个目的在于提出基于四元数球面线性插值的固定翼飞机姿态控制装置。
为达上述目的，本发明一方面提出了基于四元数球面线性插值的固定翼飞机姿态控制方法，包括以下步骤：
构建六自由度飞机空气动力学模型，并基于所述动力学模型搭建角速度、过载的增稳系统；通过所述动力学模型解算得到当前姿态角，分别将所述当前姿态角和预设的目标姿态角转化为当前姿态四元数和目标姿态四元数；以及，将所述目标姿态四元数和所述当前姿态四元数通过球面线性插值法计算得到插值姿态四元数；计算所述插值姿态四元数与所述当前姿态四元数的误差四元数，并基于所述误差四元数计算得到所述增稳系统的输入指令；将所述输入指令输入所述增稳系统，输出飞机舵面偏转角控制指令以控制飞机飞行姿态。
根据本发明实施例的基于四元数球面线性插值的固定翼飞机姿态控制方法，能够利用四元数控制器实现飞机姿态控制，有效避免欧拉角控制器在控制姿态变换时存在奇异点的情况，同时利用球面线性插值方法，并且实现模型任意姿态平滑转移能够降低强化学习算法训练难度和研究人员建立飞控模型的工作量。
另外，根据本发明上述实施例的基于四元数球面线性插值的固定翼飞机姿态控制方法还包括：
进一步地，所述目标姿态角包括：第一俯仰角、第一滚转角和第一偏航角；所述当前姿态角包括：第二俯仰角、第二滚转角和第二偏航角；所述输入指令包括：第一俯仰角速度、第一滚转角速度和第一偏航角速度。
进一步地，所述将所述输入指令输入所述增稳系统，输出飞机舵面偏转角控制指令以控制飞机飞行姿态，包括：
将任一的目标姿态角和所述第一俯仰角速度、第一滚转角速度和第一偏航角速度的增稳系统输入指令输入所述增稳系统；其中，所述增稳系统包括：俯仰角速度控制器、滚转角速度控制器和偏航角速度控制器；通过所述增稳系统输出升降舵偏转角度、副翼偏转角度和方向舵偏转角度的飞机舵面偏转角控制指令，以控制所述飞机飞行姿态。
进一步地，所述升降舵偏转角度δelevator：
其中，表示归一化过载误差，/>表示PID控制器比例系数和积分系数，Kn1为角速度前馈增益，/>为俯仰角速度Kq为俯仰角速度反馈增益；
所述副翼偏转角度δaileron：
其中，表示归一化滚转角速度误差，/>表示PID控制器比例系数和积分系数，Kp1为滚转角速度前馈增益，/>表示当前滚转角；
所述方向舵偏转角度δrudder：
其中，表示偏航角速度，加入径向过载ny修正，/>表示PID控制器比例系数和积分系数。
进一步地，将第一俯仰角θcmd、第一滚转角和第一偏航角ψcmd以及第二俯仰角θ、第二滚转角/>和第二偏航角ψ转换为四元数，则转换关系为：
进一步地，将所述目标姿态四元数、所述当前姿态四元数以及所述插值姿态四元数分别表示为：
进一步地，所述将所述目标姿态四元数和所述当前姿态四元数通过球面线性插值法计算得到插值姿态四元数，由下面公式计算得到：
其中，Δα＝arccos(qTqcmd)表示当前姿态四元数与目标姿态四元数的夹角，τ为插值系数，表示插值的密集程度。
进一步地，所述误差四元数，由下面公式表示：
进一步地，所述第一俯仰角速度所述第一滚转角速度/>和所述第一偏航角速度/>分别由下面公式计算得到：
其中，sign(·)函数判断Δw的符号，耦合系数表示为
为达到上述目的，本发明另一方面提出了一种基于四元数球面线性插值的固定翼飞机姿态控制装置，包括：
系统构建模块，用于构建六自由度飞机空气动力学模型，并基于所述动力学模型搭建角速度、过载的增稳系统；四元数计算模块，用于通过所述动力学模型解算得到当前姿态角，分别将所述当前姿态角和预设的目标姿态角转化为当前姿态四元数和目标姿态四元数；以及，将所述目标姿态四元数和所述当前姿态四元数通过球面线性插值法计算得到插值姿态四元数；指令获取模块，用于计算所述插值姿态四元数与所述当前姿态四元数的误差四元数，并基于所述误差四元数计算得到所述增稳系统的输入指令；姿态控制模块，用于将述输入指令输入所述增稳系统，输出飞机舵面偏转角控制指令以控制飞机飞行姿态。
本发明实施例的基于四元数球面线性插值的固定翼飞机姿态控制装置，能够利用四元数控制器实现飞机姿态控制，有效避免欧拉角控制器在控制姿态变换时存在奇异点的情况，同时利用球面线性插值方法，并且实现模型任意姿态平滑转移能够降低强化学习算法训练难度和研究人员建立飞控模型的工作量。
本发明的有益效果：
1.本发明可快速建立强化学习环境中的固定翼飞机控制模型，降低研究人员建立飞控模型的工作量；
2.本发明通过对控制器和飞机气动模型的封装，仅需输入目标欧拉角和速度就能够实现飞机姿态平滑转移，能够有效降低强化学习算法训练难度。
本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
本发明上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：
图1为根据本发明实施例的基于四元数球面线性插值的固定翼飞机姿态控制方法的流程图；
图2为根据本发明实施例的姿态控制器示意图；
图3(a)和图3(b)分别为根据本发明实施例的基于四元数球面线性插值的固定翼飞机姿态控制方法的航向偏转姿态和速度的响应示意图；
图4为根据本发明实施例的基于四元数球面线性插值的固定翼飞机姿态控制方法的半筋斗机动姿态和速度的响应示意图；
图5为根据本发明实施例的基于四元数球面线性插值的固定翼飞机姿态控制装置的结构示意图。
具体实施方式
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。
为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。
下面参照附图描述根据本发明实施例提出的基于四元数球面线性插值的固定翼飞机姿态控制方法及装置，首先将参照附图描述根据本发明实施例提出的基于四元数球面线性插值的固定翼飞机姿态控制方法。
图1是本发明一个实施例的基于四元数球面线性插值的固定翼飞机姿态控制方法的流程图。
本发明实施例基于JSBSim平台，模型平台移植性良好，易于扩展。飞机的姿态主要由发动机推力、升降舵、副翼、方向舵等变量控制。当推力和各操纵面偏转角变化时，模型选择对应的气动系数，改变飞机合力和合力矩，从而改变飞行姿态。
如图1所示，该基于四元数球面线性插值的固定翼飞机姿态控制方法包括以下步骤：
步骤S1，构建六自由度飞机空气动力学模型，并基于动力学模型搭建角速度、过载的增稳系统。
步骤S2，通过动力学模型解算得到当前姿态角，分别将当前姿态角和预设的目标姿态角转化为当前姿态四元数和目标姿态四元数；以及，将目标姿态四元数和当前姿态四元数通过球面线性插值法计算得到插值姿态四元数；
步骤S3，计算插值姿态四元数与当前姿态四元数的误差四元数，并基于误差四元数计算得到增稳系统的输入指令；
步骤S4，将输入指令输入增稳系统，输出飞机舵面偏转角控制指令以控制飞机飞行姿态。
近一步地，本发明基于JSBSim平台，模型平台移植性良好，易于扩展。飞机的姿态主要由发动机推力、升降舵、副翼、方向舵等变量控制。当推力和各操纵面偏转角变化时，模型选择对应的气动系数，改变飞机合力和合力矩，从而改变飞行姿态。
通过本发明的基于四元数球面线性插值的固定翼飞机姿态控制方法，利用四元数控制器实现飞机姿态控制，有效避免欧拉角控制器在控制姿态变换时存在奇异点的情况，同时利用球面线性插值方法，实现模型任意姿态平滑转移。
下面为本发明实施例的具体步骤流程：
步骤S11，构建六自由度飞机空气动力学模型；
步骤S21，利用传统串级PID控制器搭建角速度、过载控制环路，实现飞行稳定增强系统；
步骤S31，将当前姿态角和目标姿态角转化为四元数，由球面线性插值方法产生插值姿态四元数，
步骤S41，计算插值姿态四元数与当前姿态四元数误差并产生PID控制器指令；
步骤S51，搭建基于四元数的PID控制器；
步骤S61，输入控制器指令，控制器自动生成舵面控制指令实现平滑姿态转移。
作为一种示例，本发明的姿态控制器如图2所示，控制器包含基于四元数插值的角度控制器、俯仰角速度控制器、滚转角速度控制器和偏航角速度控制器以及六自由度固定翼飞机空气动力学模型构成。
其中，四元数角度控制器输入为目标姿态角指令和当前姿态角，目标姿态角分别表示为俯仰角θcmd∈[-90°,90°]，滚转角偏航角ψcmd∈[0°,360°]，当前姿态角分别表示为俯仰角θ∈[-90°,90°]，滚转角/>偏航角ψ∈[0°,360°]；四元数角度控制器输出为角速度或过载控制指令，分别表示为俯仰角速度/>滚转角速度偏航角速度/>
进一步地，俯仰角速度控制器、滚转角速度控制器和偏航角速度控制器分别根据控制指令输出升降舵偏转角δelevator，副翼偏转角δaileron，方向舵偏转角δrudder，从而控制飞行姿态。
作为一种实施例，基于控制环路的内环飞行稳定增强系统采用PID控制器实现，基本控制率包括：
1.俯仰角速度与法向过载控制：
控制器输出升降舵偏转角度：
其中，表示归一化过载误差，/>表示俯仰角速度指令，/>表示PID控制器比例系数和积分系数，Kn1为角速度前馈增益，/>为俯仰角速度，Kq为俯仰角速度反馈增益。
2.滚转角速度控制：
控制器输出副翼偏转角度：
其中，表示归一化滚转角速度误差，/>表示滚转角速度指令， 表示PID控制器比例系数和积分系数，Kp1为滚转角速度前馈增益，/>表示当前滚转角。
3.偏航角速度控制：
控制器输出方向舵偏转角度：
其中，表示偏航角速度，/>表示偏航角速度指令，其中加入径向过载ny修正，/>表示PID控制器比例系数和积分系数。
进一步地，将目标姿态角θcmd、ψcmd和当前姿态角θ、/>ψ转换为四元数，姿态角与四元数的转换关系表示如下：
进一步地，将目标姿态四元数、当前姿态四元数以及插值姿态四元数分别表示为：
插值姿态四元数由目标姿态四元数和当前姿态四元数通过球面线性插值方法计算得到：
其中，Δα＝arccos(qTqcmd)表示当前姿态四元数与目标姿态四元数的夹角，τ为插值系数，表示插值的密集程度。
插值姿态四元数和当前姿态四元数的误差四元数表示为：
则俯仰角速度滚转角速度/>偏航角速度/>分别计算为：
其中sign(·)函数判断Δw的符号，航向控制同时由偏转副翼和方向舵实现，耦合系数表示为
进一步地，输入目标控制指令，控制器自动完成对飞机模型的姿态平滑转移的控制。
作为一种实施例，为了验证本发明控制器姿态转移效果，搭建基于JSBSim的飞机空气动力学模型的测试环境，其特征在于包括以下步骤：
1.利用OpenAI gym开源平台，定义状态空间和动作空间等接口信息，注册环境；
2.设置初始位置、速度、姿态等参数；
3.设置姿态、速度指令，观察响应曲线。
根据本发明实施例的两种典型姿态变换实验效果如下：
如图3(a)所示，初始速度800ft/s，初始角速度为0，初始姿态角θ＝0°、ψ＝180°，目标马赫数0.8，目标姿态角θcmd＝0°、/>ψcmd＝90°，即航向偏转90°，控制器通过偏转副翼和方向舵使飞机航向快速偏转并到达目标姿态、速度。
如图3(b)所示，初始速度800ft/s，初始角速度为0，初始姿态角θ＝0°、ψ＝30°，目标马赫数0.8，目标姿态角θcmd＝0°、/>ψcmd＝330°，即航向偏转60°，控制器同样通过偏转副翼和方向舵使飞机航向快速偏转并到达目标姿态、速度，并且通过四元数和球面线性插值方法，找到快速规划姿态路径，避免直接采用欧拉角PID控制器无法处理姿态角跳变的问题。
如图4所示，初始速度800ft/s，初始角速度为0，初始姿态角θ＝0°、ψ＝180°，目标马赫数0.8，控制器目标实现半筋斗机动，该机动由倒扣和滚转两段控制指令构成，第一阶段目标姿态角θcmd＝0°、/>ψcmd＝360°，即完成倒扣，而后设置第二阶段目标姿态角θcmd＝0°、/>ψcmd＝360°，即完成滚转。本发明实施例有效避免欧拉角控制器存在奇点(万向节死锁现象)的问题以及四元数与欧拉角非一对一映射关系问题，实现姿态平滑转移。
可以理解的是，对于复杂机动，本实施例同样可以利用状态机设置关键节点控制指令实现。
作为另一种实现方式，本发明还可以采用旋转矩阵或罗德里格斯变换的方法代替四元数，避免欧拉角奇异点。以及可以状态机或行为树设计姿态连续变换过程中的四元数转移函数，从而代替球面线性插值实现姿态平滑转移。
通过上述步骤，本发明可应用于强化学习环境中飞机模型控制器设计，能够提高飞机模型控制稳定性，从而降低强化学习算法训练难度和研究人员建立飞控模型的工作量。
为了实现上述实施例，如图5所示，本实施例中还提供了一种基于四元数球面线性插值的固定翼飞机姿态控制装置10，该装置10包括：第一姿态控制模块100、四元数计算模块200、指令获取模块300和第二姿态控制模块400。
系统构建模块100，用于构建六自由度飞机空气动力学模型，并基于动力学模型搭建角速度、过载的增稳系统；
四元数计算模块200，用于通过动力学模型解算得到当前姿态角，分别将当前姿态角和预设的目标姿态角转化为当前姿态四元数和目标姿态四元数；以及，将目标姿态四元数和当前姿态四元数通过球面线性插值法计算得到插值姿态四元数；
指令获取模块300，用于计算插值姿态四元数与当前姿态四元数的误差四元数，并基于误差四元数计算得到增稳系统的输入指令；
姿态控制模块400，用于将输入指令输入所述增稳系统，输出飞机舵面偏转角控制指令以控制飞机飞行姿态。
根据本发明实施例的基于四元数球面线性插值的固定翼飞机姿态控制装置，可应用于强化学习环境中飞机模型控制器设计，能够提高飞机模型控制稳定性，从而降低强化学习算法训练难度和研究人员建立飞控模型的工作量。
需要说明的是，前述对基于四元数球面线性插值的固定翼飞机姿态控制方法实施例的解释说明也适用于该实施例的基于四元数球面线性插值的固定翼飞机姿态控制装置，此处不再赘述。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。
Claims (6)
1.一种基于四元数球面线性插值的固定翼飞机姿态控制方法，其特征在于，包括以下步骤：
构建六自由度飞机空气动力学模型，并基于所述动力学模型搭建角速度、过载的增稳系统；
通过所述动力学模型解算得到当前姿态角，分别将所述当前姿态角和预设的目标姿态角转化为当前姿态四元数和目标姿态四元数；以及，将所述目标姿态四元数和所述当前姿态四元数通过球面线性插值法计算得到插值姿态四元数；
计算所述插值姿态四元数与所述当前姿态四元数的误差四元数，并基于所述误差四元数计算得到所述增稳系统的输入指令；
将所述输入指令输入所述增稳系统，输出飞机舵面偏转角控制指令以控制飞机飞行姿态；
将所述目标姿态四元数、所述当前姿态四元数以及所述插值姿态四元数分别表示为：
， />，/>
将所述目标姿态四元数和所述当前姿态四元数通过球面线性插值法计算得到插值姿态四元数，由下面公式计算得到：
其中，表示当前姿态四元数与目标姿态四元数的夹角，/>为插值系数，表示插值的密集程度；
所述误差四元数，由下面公式表示：
第一俯仰角速度、第一滚转角速度/>和第一偏航角速度/>，分别由下面公式计算得到：
其中，函数判断/>的符号，耦合系数表示为/>。
2.根据权利要求1所述的方法，其特征在于，所述目标姿态角包括：第一俯仰角、第一滚转角和第一偏航角；所述当前姿态角包括：第二俯仰角、第二滚转角和第二偏航角；所述输入指令包括：第一俯仰角速度、第一滚转角速度和第一偏航角速度。
3.根据权利要求2所述的方法，其特征在于，所述将所述输入指令输入所述增稳系统，输出飞机舵面偏转角控制指令以控制飞机飞行姿态，包括：
将所述第一俯仰角速度、第一滚转角速度和第一偏航角速度的增稳系统输入指令输入所述增稳系统；其中，所述增稳系统包括：俯仰角速度控制器、滚转角速度控制器和偏航角速度控制器；
通过所述增稳系统输出升降舵偏转角度、副翼偏转角度和方向舵偏转角度的飞机舵面偏转角控制指令，以控制所述飞机飞行姿态。
4.根据权利要求3所述的方法，其特征在于，
所述升降舵偏转角度：
其中，表示归一化过载误差，/>、/>表示PID控制器比例系数和积分系数，/>为角速度前馈增益，/>为俯仰角速度，/>为俯仰角速度反馈增益；
所述副翼偏转角度：
其中，表示归一化滚转角速度误差，/>、/>表示PID控制器比例系数和积分系数，/>为滚转角速度前馈增益，/>表示当前滚转角；
所述方向舵偏转角度：
其中，表示偏航角速度，加入径向过载/>修正，/>、/>表示PID控制器比例系数和积分系数。
5.根据权利要求2所述的方法，其特征在于，将第一俯仰角、第一滚转角/>和第一偏航角/>以及第二俯仰角/>、第二滚转角/>和第二偏航角/>转换为四元数，则转换关系为：
。
6.一种使用权利要求1所述方法的基于四元数球面线性插值的固定翼飞机姿态控制装置，其特征在于，包括：
系统构建模块，用于构建六自由度飞机空气动力学模型，并基于所述动力学模型搭建角速度、过载的增稳系统；
四元数计算模块，用于通过所述动力学模型解算得到当前姿态角，分别将所述当前姿态角和预设的目标姿态角转化为当前姿态四元数和目标姿态四元数；以及，将所述目标姿态四元数和所述当前姿态四元数通过球面线性插值法计算得到插值姿态四元数；
指令获取模块，用于计算所述插值姿态四元数与所述当前姿态四元数的误差四元数，并基于所述误差四元数计算得到所述增稳系统的输入指令；
姿态控制模块，用于将所述输入指令输入所述增稳系统，输出飞机舵面偏转角控制指令以控制飞机飞行姿态。
Applications Claiming Priority (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210350150 | 2022-04-02 | ||
| CN202210350150X | 2022-04-02 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114967716A CN114967716A (zh) | 2022-08-30 |
| CN114967716B true CN114967716B (zh) | 2024-05-03 |
Family
ID=82980455
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210444221.2A Active CN114967716B (zh) | 2022-04-02 | 2022-04-25 | 一种基于四元数球面线性插值的固定翼飞机姿态控制方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114967716B (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116382335A (zh) * | 2023-03-27 | 2023-07-04 | 成都飞机工业（集团）有限责任公司 | 一种无人机半滚倒转机动控制方法 |
| CN116594414B (zh) * | 2023-03-28 | 2024-03-15 | 西北工业大学 | 一种高超声速飞行器纵向控制方法 |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5875993A (en) * | 1996-07-17 | 1999-03-02 | State Of Israel/Ministry Of Defense Armament Development Authority Rafael | Flight control of an airborne vehicle at low velocity |
| WO2014179449A1 (en) * | 2013-05-01 | 2014-11-06 | Hillcrest Laboratories, Inc. | Mapped variable smoothing evolution method and device |
| CN110806754A (zh) * | 2019-11-28 | 2020-02-18 | 北京工业大学 | 四旋翼飞行器姿态修正控制系统及方法 |
| CN111459188A (zh) * | 2020-04-29 | 2020-07-28 | 南京理工大学 | 一种基于四元数的多旋翼非线性飞行控制方法 |
| CN111879325A (zh) * | 2020-08-05 | 2020-11-03 | 东北大学 | 一种具备高精度和抗干扰能力的imu姿态角估算方法 |
| RU2749152C1 (ru) * | 2020-06-19 | 2021-06-07 | Акционерное общество Московский научно-производственный комплекс "Авионика" имени О.В. Успенского (АО МНПК "Авионика") | Адаптивный корректор углов ориентации для БИНС |
| WO2021134644A1 (zh) * | 2019-12-31 | 2021-07-08 | 深圳市大疆创新科技有限公司 | 云台的控制方法和云台 |
| CN113096448A (zh) * | 2021-02-03 | 2021-07-09 | 中国人民解放军海军航空大学 | 一种教练机起落航线总体设计方法 |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CA2569227C (en) * | 2004-06-02 | 2014-09-30 | Athena Technologies, Inc. | Systems and methods for controlling dynamic systems |
| CN113015677A (zh) * | 2018-10-29 | 2021-06-22 | 大疆科技股份有限公司 | 用于执行实时地图构建的可移动物体 |
| JP6934116B1 (ja) * | 2019-11-05 | 2021-09-08 | 楽天グループ株式会社 | 航空機の飛行制御を行う制御装置、及び制御方法 |
-
2022
- 2022-04-25 CN CN202210444221.2A patent/CN114967716B/zh active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5875993A (en) * | 1996-07-17 | 1999-03-02 | State Of Israel/Ministry Of Defense Armament Development Authority Rafael | Flight control of an airborne vehicle at low velocity |
| WO2014179449A1 (en) * | 2013-05-01 | 2014-11-06 | Hillcrest Laboratories, Inc. | Mapped variable smoothing evolution method and device |
| CN110806754A (zh) * | 2019-11-28 | 2020-02-18 | 北京工业大学 | 四旋翼飞行器姿态修正控制系统及方法 |
| WO2021134644A1 (zh) * | 2019-12-31 | 2021-07-08 | 深圳市大疆创新科技有限公司 | 云台的控制方法和云台 |
| CN111459188A (zh) * | 2020-04-29 | 2020-07-28 | 南京理工大学 | 一种基于四元数的多旋翼非线性飞行控制方法 |
| RU2749152C1 (ru) * | 2020-06-19 | 2021-06-07 | Акционерное общество Московский научно-производственный комплекс "Авионика" имени О.В. Успенского (АО МНПК "Авионика") | Адаптивный корректор углов ориентации для БИНС |
| CN111879325A (zh) * | 2020-08-05 | 2020-11-03 | 东北大学 | 一种具备高精度和抗干扰能力的imu姿态角估算方法 |
| CN113096448A (zh) * | 2021-02-03 | 2021-07-09 | 中国人民解放军海军航空大学 | 一种教练机起落航线总体设计方法 |
Non-Patent Citations (5)
| Title |
|---|
| Nonlinear robust output feedback tracking control of a quadrotor UAV using quaternion representation;Xian, B等;Nonlinear Dyn;20141213;第79卷;第2735–2752 页 * |
| 一种基于四元数和向量法的CGF机动模型实现;李勃;彭勇;王辉;;兵器装备工程学报;20160925(第09期);第24-27页 * |
| 四旋翼飞行器智能控制方法研究;于松周;《中国优秀硕士学位论文全文数据库工程科技Ⅱ辑》;20180615(第06（2018）期);第C031-130页 * |
| 固定翼无人机自适应滑模控制;宗群;张睿隆;董琦;张超凡;;哈尔滨工业大学学报;20180930(第09期);第147-155页 * |
| 基于四元数样条曲线的姿态轨迹规划研究;王效杰;《中国优秀硕士学位论文全文数据库信息科技辑》;20160315(第03（2016）期);第I140-820页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114967716A (zh) | 2022-08-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114967716B (zh) | 一种基于四元数球面线性插值的固定翼飞机姿态控制方法 | |
| CN109062237B (zh) | 一种无人倾转旋翼机自抗扰姿态控制方法 | |
| Huang et al. | Fixed-time autonomous shipboard landing control of a helicopter with external disturbances | |
| CN110908278B (zh) | 一种折叠翼飞行器的动力学建模与稳定控制方法 | |
| CN108363305B (zh) | 基于主动干扰补偿的战术导弹鲁棒过载自驾仪设计方法 | |
| Colorado et al. | Mini-quadrotor attitude control based on Hybrid Backstepping & Frenet-Serret theory | |
| Cardoso et al. | A new robust adaptive mixing control for trajectory tracking with improved forward flight of a tilt-rotor UAV | |
| Ding et al. | Robust fixed-time sliding mode controller for flexible air-breathing hypersonic vehicle | |
| CN110347170A (zh) | 可重复使用运载器再入段鲁棒容错制导系统及工作方法 | |
| CN111198570B (zh) | 一种基于固定时间微分器预测的抗时延高精度自抗扰姿态控制方法 | |
| CN114237267B (zh) | 基于强化学习的飞行机动决策的辅助方法 | |
| CN111045440B (zh) | 一种高超声速飞行器俯冲段快速滚转控制方法 | |
| CN111007877A (zh) | 一种四旋翼飞行器的全局鲁棒自适应轨迹跟踪控制方法 | |
| CN112130457A (zh) | 一种变体无人机栖落机动的模糊飞行控制方法 | |
| Liu et al. | Observer-based linear parameter varying control design with unmeasurable varying parameters under sensor faults for quad-tilt rotor unmanned aerial vehicle | |
| CN114721266A (zh) | 飞机舵面结构性缺失故障情况下的自适应重构控制方法 | |
| CN114637203A (zh) | 一种针对中高速、大机动无人机的飞行控制系统 | |
| Zou et al. | Singularity-free backstepping controller for model helicopters | |
| Cheng et al. | Hover-to-cruise transition control for high-speed level flight of ducted fan UAV | |
| CN116088549A (zh) | 一种尾座式垂直起降无人机姿态控制方法 | |
| CN115344056A (zh) | 一种复杂操纵面飞机的智能飞行控制方法及应用 | |
| CN115328185A (zh) | 一种飞行器非线性非定常气动载荷修正系统 | |
| CN114167720A (zh) | 基于观测器的倾转式三旋翼无人机轨迹跟踪控制方法 | |
| Gu et al. | Geometry-Based Adaptive Tracking Control for an Underactuated Small-Size Unmanned Helicopter | |
| Liu et al. | Nonlinear asymptotic attitude tracking control for an unmanned helicopter with input constraints |
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 |