﻿ 基于角加速度估计的非线性增量动态逆控制及试飞
 文章快速检索 高级检索

1. 北京航空航天大学 能源与动力工程学院, 北京 100083;
2. 北京航空航天大学 交通科学与工程学院, 北京 100083

Incremental nonlinear dynamic inversion control and flight test based on angular acceleration estimation
DANG Xiaowei1, TANG Peng2, SUN Hongqiang2, ZHENG Chen1
1. School of Energy and Power Engineering, Beihang University, Beijing 100083, China;
2. School of Transportations Science and Engineering, Beihang University, Beijing 100083, China
Abstract: The Incremental Nonlinear Dynamic Inversion (INDI) control law requires estimation of state derivative (angular acceleration) for real-time feedback and exists time delay caused by angular acceleration estimation, This paper proposes research and practical engineering solutions to the above problems. The dynamic model of the flight test Unmanned Aerial Vehicle (UAV) platform was established. The incremental nonlinear dynamic inversion control law and the controller layered design method were used to design the UAV attitude control system. Using Kalman filter, the angular acceleration estimator is designed to provide real-time feedback of angular acceleration for the control law. The control law is implemented by a model-based control system design method, and the actual flight test is performed. The test results show that the control method is achievable in engineering and show good robustness and command tracking capability.
Keywords: flight control    incremental nonlinear dynamic inversion    angular acceleration estimation    Kalman filter    model-based design

1 三旋翼动力学模型

 图 1 无人机模型 Fig. 1 UAV model
 图 2 受力分析图 Fig. 2 Diagram of force analysis

 $\left\{ \begin{array}{l} \dot p = \frac{{qr}}{{{I_x}}}\left( {{I_y} - {I_z}} \right) + \left( {{F_1}{l_1}\cos\delta - {F_2}{l_2}\cos\delta } \right)\\ \dot q = \frac{{rp}}{{{I_y}}}\left( {{I_z} - {I_x}} \right) + \left( {{F_1}\cos\delta + {F_2}\cos\delta } \right){l_0} - {F_3}{l_3}\\ \dot r = \frac{{pq}}{{{I_z}}}\left( {{I_x} - {I_y}} \right) + c{F_3} - \left( {{F_1}{l_1}\sin\delta + {F_2}{l_2}\sin\delta } \right) \end{array} \right.$ （1）

 $\mathit{\boldsymbol{u}} = {\left[ {{F_1},{F_2},{F_3},\delta } \right]^{\rm{T}}}$
 $\mathit{\boldsymbol{I}} = \left[ {\begin{array}{*{20}{c}} {{I_x}}&{}&{}\\ {}&{{I_y}}&{}\\ {}&{}&{{I_z}} \end{array}} \right]$

2 三旋翼控制系统设计 2.1 增量动态逆控制律

 $\left\{ {\begin{array}{*{20}{l}} {\mathit{\boldsymbol{\dot x}} = \mathit{\boldsymbol{f}}\left( \mathit{\boldsymbol{x}} \right) + \mathit{\boldsymbol{g}}\left( {\mathit{\boldsymbol{x}},\mathit{\boldsymbol{u}}} \right)}\\ {\mathit{\boldsymbol{y}} = \mathit{\boldsymbol{h}}\left( \mathit{\boldsymbol{x}} \right)} \end{array}} \right.$ （2）

 $\begin{array}{l} \mathit{\boldsymbol{\dot x}} \approx \mathit{\boldsymbol{f}}\left( {{\mathit{\boldsymbol{x}}_0}} \right) + \mathit{\boldsymbol{g}}\left( {{\mathit{\boldsymbol{x}}_0},{\mathit{\boldsymbol{u}}_0}} \right) + \\ \;\;\;\;\;\frac{\partial }{{\partial \mathit{\boldsymbol{x}}}}{\left( {\mathit{\boldsymbol{f}}\left( \mathit{\boldsymbol{x}} \right) + \mathit{\boldsymbol{g}}\left( {\mathit{\boldsymbol{x}},\mathit{\boldsymbol{u}}} \right)} \right)_{{\mathit{\boldsymbol{x}}_0},{\mathit{\boldsymbol{u}}_0}}}\left( {\mathit{\boldsymbol{x}} - {\mathit{\boldsymbol{x}}_0}} \right) + \\ \;\;\;\;\;\frac{\partial }{{\partial \mathit{\boldsymbol{u}}}}\mathit{\boldsymbol{g}}{\left( {\mathit{\boldsymbol{x}},\mathit{\boldsymbol{u}}} \right)_{{\mathit{\boldsymbol{x}}_0},{\mathit{\boldsymbol{u}}_0}}}\left( {\mathit{\boldsymbol{u}} - {\mathit{\boldsymbol{u}}_0}} \right) \end{array}$ （3）

(x0, u0)点的状态方程为

 ${{\mathit{\boldsymbol{\dot x}}}_0} = \mathit{\boldsymbol{f}}\left( {{\mathit{\boldsymbol{x}}_0}} \right) + \mathit{\boldsymbol{g}}\left( {{\mathit{\boldsymbol{x}}_0},{\mathit{\boldsymbol{u}}_0}} \right)$ （4）

 $\left\{ \begin{array}{l} \mathit{\boldsymbol{A}} = \frac{\partial }{{\partial \mathit{\boldsymbol{x}}}}\left( {\mathit{\boldsymbol{f}}\left( \mathit{\boldsymbol{x}} \right) + \mathit{\boldsymbol{g}}\left( {\mathit{\boldsymbol{x}},\mathit{\boldsymbol{u}}} \right)} \right)\\ \mathit{\boldsymbol{B}} = \frac{\partial }{{\partial \mathit{\boldsymbol{u}}}}\mathit{\boldsymbol{g}}\left( {\mathit{\boldsymbol{x}},\mathit{\boldsymbol{u}}} \right) \end{array} \right.$ （5）

 $\mathit{\boldsymbol{\dot x}} \approx {{\mathit{\boldsymbol{\dot x}}}_0} + \mathit{\boldsymbol{A}}\left( {{\mathit{\boldsymbol{x}}_0},{\mathit{\boldsymbol{u}}_0}} \right)\Delta \mathit{\boldsymbol{x}} + \mathit{\boldsymbol{B}}\left( {{\mathit{\boldsymbol{x}}_0},{\mathit{\boldsymbol{u}}_0}} \right)\Delta \mathit{\boldsymbol{u}}$ （6）

 $\begin{array}{l} \mathit{\boldsymbol{\dot y}} = \frac{{\partial \mathit{\boldsymbol{h}}\left( \mathit{\boldsymbol{x}} \right)}}{{\partial \mathit{\boldsymbol{x}}}}\mathit{\boldsymbol{\dot x}} \approx \\ \;\;\;\;\;{\mathit{\boldsymbol{h}}_\mathit{\boldsymbol{x}}}\left( {{{\mathit{\boldsymbol{\dot x}}}_0} + \mathit{\boldsymbol{A}}\left( {{\mathit{\boldsymbol{x}}_0},{\mathit{\boldsymbol{u}}_0}} \right)\Delta \mathit{\boldsymbol{x}} + \mathit{\boldsymbol{B}}\left( {{\mathit{\boldsymbol{x}}_0},{\mathit{\boldsymbol{u}}_0}} \right)\Delta \mathit{\boldsymbol{u}}} \right) \end{array}$ （7）

 $\begin{array}{l} \Delta \mathit{\boldsymbol{u}} = {\left( {{\mathit{\boldsymbol{h}}_\mathit{\boldsymbol{x}}}\mathit{\boldsymbol{B}}\left( {{\mathit{\boldsymbol{x}}_0},{\mathit{\boldsymbol{u}}_0}} \right)} \right)^{ - 1}} \cdot \\ \;\;\;\;\;\;\left( {{{\mathit{\boldsymbol{\dot y}}}_{\rm{d}}} - {\mathit{\boldsymbol{h}}_\mathit{\boldsymbol{x}}}{{\mathit{\boldsymbol{\dot x}}}_0} - {\mathit{\boldsymbol{h}}_\mathit{\boldsymbol{x}}}\mathit{\boldsymbol{A}}\left( {{\mathit{\boldsymbol{x}}_0},{\mathit{\boldsymbol{u}}_0}} \right)\Delta \mathit{\boldsymbol{x}}} \right) \end{array}$ （8）

 $\Delta \mathit{\boldsymbol{u}} = {\left( {{\mathit{\boldsymbol{h}}_\mathit{\boldsymbol{x}}}\mathit{\boldsymbol{B}}\left( {{\mathit{\boldsymbol{x}}_0},{\mathit{\boldsymbol{u}}_0}} \right)} \right)^{ - 1}}\left( {{{\mathit{\boldsymbol{\dot y}}}_{\rm{d}}} - {\mathit{\boldsymbol{h}}_\mathit{\boldsymbol{x}}}{{\mathit{\boldsymbol{\dot x}}}_0}} \right)$ （9）

2.2 控制律设计

 $\Delta \mathit{\boldsymbol{u}} = {\mathit{\boldsymbol{B}}^{ - 1}}\left( {{\mathit{\boldsymbol{x}}_0},{\mathit{\boldsymbol{u}}_0}} \right)\left( {{{\mathit{\boldsymbol{ \boldsymbol{\dot \varOmega} }}}_{\rm{d}}} - {{\mathit{\boldsymbol{ \boldsymbol{\dot \varOmega} }}}_{\rm{0}}}} \right)$ （10）

 图 3 外回路控制框图 Fig. 3 Block diagram of outer loop control
 图 4 内回路控制框图 Fig. 4 Block diagram of inner loop control

3 角加速度估计器设计 3.1 基于卡尔曼滤波器的角加速度估计

INDI是一种对于模型参数不敏感的鲁棒非线性控制方法，该方法利用角加速度反馈进行解算，而一般飞控硬件采用惯性测量单元测量角速度，没有角加速度传感器直接测量角加速度。直接利用角速度差分得到的角加速度噪声过大，无法用于控制律的解算，因此需要利用传感器的信号估计出角加速度信号。传统的方式是直接对传感器的角速度信号进行差分，然后再进行低通滤波。在此基础之上，本文利用KF进行角加速度的实时估计。

1) 运动学模型

 $\left\{ {\begin{array}{*{20}{l}} {{\mathit{\boldsymbol{x}}_{k + 1}} = \mathit{\boldsymbol{F}}{\mathit{\boldsymbol{x}}_k} + \mathit{\boldsymbol{Q}}}\\ {{\mathit{\boldsymbol{z}}_{k + 1}} = \mathit{\boldsymbol{H}}{\mathit{\boldsymbol{x}}_{k + 1}} + \mathit{\boldsymbol{R}}} \end{array}} \right.$ （11）

2) 滤波估计过程[19]

① 一步状态更新，根据上一采样时刻估计出的状态量经过状态转移矩阵得出一步状态量估计值：

 ${{\mathit{\boldsymbol{\hat x}}}_{k + 1/k}} = \mathit{\boldsymbol{F}}{\mathit{\boldsymbol{x}}_k}$ （12）

② 状态误差协方差一步更新，求解出状态误差协方差阵经过一步状态更新并引入过程噪声之后的估计值：

 ${{\mathit{\boldsymbol{\hat P}}}_{k + 1/k}} = \mathit{\boldsymbol{F}}{\mathit{\boldsymbol{P}}_k}{\mathit{\boldsymbol{F}}^{\rm{T}}} + \mathit{\boldsymbol{Q}}$ （13）

③ 求解使得状态量估计为最小方差无偏估计的卡尔曼增益K

 ${\mathit{\boldsymbol{K}}_{k + 1}} = {{\mathit{\boldsymbol{\hat P}}}_{k + 1/k}}{\mathit{\boldsymbol{H}}^{\rm{T}}}{\left( {\mathit{\boldsymbol{H}}{{\mathit{\boldsymbol{\hat P}}}_{k + 1/k}}{\mathit{\boldsymbol{H}}^{\rm{T}}} + \mathit{\boldsymbol{R}}} \right)^{ - 1}}$ （14）

④ 根据卡尔曼增益进行状态量和协方差的校正：

 $\left\{ \begin{array}{l} {{\mathit{\boldsymbol{\hat x}}}_{k + 1/k + 1}} = {{\mathit{\boldsymbol{\hat x}}}_{k + 1/k}} + {\mathit{\boldsymbol{K}}_{k + 1}}\left( {{\mathit{\boldsymbol{z}}_{k + 1}} - \mathit{\boldsymbol{H}}{{\mathit{\boldsymbol{\hat x}}}_{k + 1/k}}} \right)\\ {{\mathit{\boldsymbol{\hat P}}}_{k + 1/k + 1}} = \left( {\mathit{\boldsymbol{I}} - {\mathit{\boldsymbol{K}}_{k + 1}}\mathit{\boldsymbol{H}}} \right){{\mathit{\boldsymbol{\hat P}}}_{k + 1/k}} \end{array} \right.$ （15）

3) 角加速度估计结果

 图 5 角加速度估计对比图 Fig. 5 Diagram of angular acceleration estimate

3.2 角加速度估计的时间延迟

 ${r_{{\rm{fg}}}}\left( \tau \right) = \int_0^T {f\left( t \right)g\left( {t + \tau } \right){\rm{d}}t}$ （16）

 ${r_{xy}}\left( m \right) = \sum\limits_{n = 0}^N {x\left( n \right)y\left( {n + m} \right)} \;\;\;\;m \in {\bf{Z}}$ （17）

 ${R_{xy}}\left( m \right) = \frac{{\sum\limits_{n = 0}^N {x\left( n \right)y\left( {n + m} \right)} }}{{\sum\limits_{n = 0}^N {x\left( n \right)x\left( n \right)} }}\;\;\;m \in {\bf{Z}}$ （18）

 图 6 时间延迟对比 Fig. 6 Comparison of time delay
4 飞行试验验证 4.1 控制律实现及试飞平台

 图 7 无人机实物图 Fig. 7 Picture of UAV

 参数 数值 飞机总重m/kg 0.82 翼展b/m 0.73 机身总长/m 0.62 螺旋桨型号 6535 单个电机最大拉力/N 6.5 Ixx/(kg·m2) 0.018 8 Iyy/(kg·m2) 0.03 Izz/(kg·m2) 0.013

 图 8 控制律设计流程图 Fig. 8 Diagram of control law design
4.2 试飞结果

1) 三轴姿态响应

 图 9 三轴姿态响应 Fig. 9 Three-axis attitude response

2) 鲁棒性测试

 图 10 重心位置摄动测试结果 Fig. 10 Robust test results at center of gravity

 图 11 动力效能摄动测试结果 Fig. 11 Robust test results of power efficiency

3) 指令跟踪

 图 12 指令跟踪结果 Fig. 12 Command tracking results
 图 13 差分加低通滤波指令跟踪结果 Fig. 13 Command tracking results using low-pass filter

5 结论

1) INDI控制律可以运用于实际飞行器的控制中，并且对飞机本体模型参数摄动具有很好的鲁棒性。

2) 角加速度估计器为INDI控制律提供必需的角加速度信息，为控制律的实现提供了一种工程上的解决途径。

3) 基于卡尔曼滤波的角加速度估计方式在滤除高频噪声的基础上带来的时间延迟更小。

 [1] REINER J, BALAS G J. Robust dynamic inversion for control of highly maneuverable aircraft[J]. Journal of Guidance, Control, and Dynamics, 1995, 18(1): 18-24. Click to display the text [2] KIM B, CALISE A. Nonlinear flight control using neural networks[J]. Journal of Guidance, Control, and Dynamics, 1997, 20(1): 97-105. Click to display the text [3] 朱家强, 朱纪洪, 郭锁凤, 等. 基于神经网络的鲁棒自适应逆飞行控制[J]. 控制理论与应用, 2005, 22(2): 182-188. ZHU J Q, ZHU J H, GUO S F, et al. Neural network based robust dynamic inversion flight control[J]. Control Theory & Applications, 2005, 22(2): 182-188. (in Chinese) Cited By in Cnki (63) | Click to display the text [4] XU B, YANG C, SHI Z. Reinforcement learning output feedback NN control using deterministic learning technique[J]. IEEE Transactions on Neural Networks and Learning Systems, 2014, 25(3): 635-641. Click to display the text [5] SMIT Z M, CRAIG I K. Robust flight controller design using H∞ loop-shaping and dynamic inversion techniques: AIAA-1998-4132[R]. Reston, VA: AIAA, 1998. [6] ITO D, WARD D T, VALASEK J. Robust dynamic inversion controller design and analysis for the X-38[C]//AIAA Guidance, Navigation, and Control Conference and Exhibit. Restion, VA: AIAA, 2001: 4380. Click to display the text [7] 范子强, 方振平. 过失速机动飞机的鲁棒非线性飞行控制律设计[J]. 航空学报, 2002, 23(3): 193-196. FAN Z Q, FANG Z P. Robust, nonlinear control design for a poststall maneuver aircraft[J]. Acta Aeronautica et Astronautica Sinica, 2002, 23(3): 193-196. (in Chinese) Cited By in Cnki (17) | Click to display the text [8] BUGAJSKI D, ENNS D. A dynamic inversion based control law with application to high angle of attack research vehicle[C]//Proceeding of AIAA Guidance, Navigation, and Control Conference. Reston, VA: AIAA, 1990: 826-839. Click to display the text [9] BACON B J, OSTROFF A J, JOSHI S M. Reconfigurable NDI controller using inertial sensor failure detection & isolation[J]. IEEE Transactions on Aerospace and Electronic Systems, 2001, 37(4): 1373-1383. Click to display the text [10] SIEBERLING S, CHU Q P, MULDER J A. Robust flight control using incremental nonlinear dynamic inversion and angular acceleration prediction[J]. Journal of Guidance, Control, and Dynamics, 2010, 33(6): 1732-1742. Click to display the text [11] SIMPLICIO P, PAVEL M D, KAMPEN E J V, et al. An acceleration measurements-based approach for helicopter nonlinear flight control using incremental nonlinear dynamic inversion[J]. Control Engineering Practice, 2013, 21(8): 1065-1077. Click to display the text [12] 郑积仕, 蒋新华, 陈兴武. 增量非线性动态逆小型无人机速度控制[J]. 系统工程与电子技术, 2013, 35(9): 1923-1927. ZHENG J S, JIANG X H, CHEN X W. Velocity control design for the small UAV based on the incremental non-linear dynamic inversion[J]. Systems Engineering and Electronics, 2013, 35(9): 1923-1927. (in Chinese) Cited By in Cnki (2) | Click to display the text [13] KOSCHORKE J, FALKENA W, KAMPEN E J V, et al. Time delayed incremental nonlinear control[C]//AIAA Guidance, Navigation, and Control Conference. Reston, VA: AIAA, 2013. Click to display the text [14] SMEUR E J, CHU Q P, CROON G C. Adaptive incremental nonlinear dynamic inversion for attitude control of micro air vehicles[J]. Journal of Guidance, Control, and Dynamics, 2016, 39(3): 450-461. Click to display the text [15] 周池军, 朱纪洪, 袁夏明, 等. 考虑作动器动态补偿的飞机增量滤波非线性控制[J]. 控制理论与应用, 2017, 34(5): 594-600. ZHU C J, ZHU J H, YUAN X M, et al. Incremental filtered nonlinear control for aircraft with actuator dynamics compensation[J]. Control Theory & Applications, 2017, 34(5): 594-600. (in Chinese) Cited By in Cnki | Click to display the text [16] BACON B J, OSTROFF A J. Reconfigurable flight control Using nonlinear dynamic inversion with a special accelerometer implementation[C]//AIAA Guidance, Navigation, and Control Conference and Exhibit. Reston, VA: AIAA, 2000. Click to display the text [17] 陈海兵, 张曙光, 方振平. 加速度反馈的隐式动态逆鲁棒非线性控制律设计[J]. 航空学报, 2009, 30(4): 597-603. CHEN H B, ZHANG S G, FANG Z P. Implicit NDI robust nonlinear control design with acceleration feedback[J]. Acta Aeronautica et Astronautica, 2009, 30(4): 597-603. (in Chinese) Cited By in Cnki (20) | Click to display the text [18] 尹航, 朱纪洪, 周池军, 等. 基于Kalman预报观测器的增量动态逆控制[J]. 清华大学学报:自然科学版, 2014, 54(12): 1534-1538. YIN H, ZHU J H, ZHOU C J, et al. Incremental dynamic inversion control with Kalman prediction observers[J]. Journal of Tsinghua University (Science and Technology), 2014, 54(12): 1534-1538. (in Chinese) Cited By in Cnki (4) | Click to display the text [19] RUDOLF E K. A new approach to linear filtering and prediction problems[J]. Journal of Basic Engineering, 1960, 82: 35-45. Click to display the text [20] 孙忠潇. Simulink仿真及代码生成技术入门到精通[M]. 北京: 北京航空航天大学出版社, 2015: 350-357. SUN Z X. Simulink simulation and code generation technique[M]. Beijing: Beihang University Press, 2015: 350-357. (in Chinese)
http://dx.doi.org/10.7527/S1000-6893.2019.23534

0

#### 文章信息

DANG Xiaowei, TANG Peng, SUN Hongqiang, ZHENG Chen

Incremental nonlinear dynamic inversion control and flight test based on angular acceleration estimation

Acta Aeronautica et Astronautica Sinica, 2020, 41(4): 323534.
http://dx.doi.org/10.7527/S1000-6893.2019.23534