基于角加速度估计的非线性增量动态逆控制及试飞

党小为，唐鹏，孙洪强，郑琛

1. 北京航空航天大学 能源与动力工程学院，北京 100083

2. 北京航空航天大学 交通科学与工程学院，北京 100083

非线性动态逆(Nonlinear Dynamic Inversion， NDI)控制作为一种非线性控制方法，通过对消非线性项来精确线性化系统，进而利用线性控制律设计方法来设计控制律，该方法在飞行控制领域中得到了广泛的应用[1]。但其对系统的精确模型依赖度高导致鲁棒性不足。国内外有许多学者对动态逆方法的鲁棒性问题进行了研究，一部分研究[2-4]采用神经网络等智能控制方法对模型偏差进行补偿，还有部分研究[5-7]将NDI与鲁棒控制方法结合进行设计。这些方法设计的控制器结构较为复杂，阶次较高，不利于工程实现。20世纪末，Bugajski和Enns[8]，Bacon等[9]提出了一种动态逆的控制方法，利用增量形式的控制方程，通过状态量的速率即角加速度反馈来降低控制律对于被控对象模型的依赖性，提高了鲁棒性。

在非线性增量动态逆(Incremental Nonlinear Dynamic Inversion， INDI)方法的基础之上，研究者等进行了相关的研究工作，将其广泛地应用于飞行控制领域[10-12]。INDI方法依赖角加速度反馈，而一般飞控系统无法直接测量角加速度，针对INDI对于角加速度反馈的需求，有各种角加速度估计方法被提出。常规的方法是利用角速度差分结合低通滤波进行角加速度估计，但低通滤波带来的时间延迟会影响系统的稳定性，对此，Koschorke等[13]研究了时间延迟对INDI闭环系统稳定性的影响，Smeur等[14]和周池军等[15]利用低通滤波估计角加速度的同时，对控制量进行滤波补偿。Bacon和Ostroff[16]、陈海兵等[17]利用几何方法估计角加速度，但此方法数值求解较为复杂，不利于工程应用。尹航等[18]利用卡尔曼滤波(Kalman Filter，KF)估计角加速度，通过仿真实验证明了该方法的有效性。

本文针对小型三旋翼无人机(Unmanned Aerial Vehicle，UAV)飞行平台，在增量动态逆控制方法的基础之上，利用飞控惯性测量单元测得的角速度通过卡尔曼滤波角加速度估计器实时提供角加速度反馈，分回路设计了姿态控制器，并利用基于模型的控制系统设计方法将控制器实现。最后，通过试飞试验验证基于角加速度估计器的增量动态逆控制方法的有效性和鲁棒性。

1 三旋翼动力学模型

本文所采用的三旋翼无人机模型如图1所示。其简化模型的受力分析，如图2所示。图中：F1、F2、F3为3个电机产生的拉力，为克服3个电机转动产生的偏航力矩，前2个电机为可倾转差动设计；δ为前2个电机的差动角；l0为1号、2号电机连线到重心的距离；l1和l2为1号、 2号电机到x轴的距离；l3为3号电机到重心的距离。

机体轴系下的三旋翼姿态转动动力学方程为

图1 无人机模型

图2 受力分析图

(1)

定义控制输入量和简化后三旋翼无人机的转动惯量矩阵分别为

u=[F1,F2,F3,δ]T

式中：p、q、r为无人机绕机体轴转动的角速度。

2 三旋翼控制系统设计

2.1 增量动态逆控制律

考察的飞机非线性动力学系统方程为

(2)

式中：x为n维状态量；u为p维控制输入；y为m维输出。

设某一时刻的状态量和输入量分别为(x0,u0)，则在(x0,u0)的某邻域内，非线性状态方程可利用泰勒一阶展开近似为

(3)

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

(4)

定义2个偏导数为

(5)

式(3)可整理为

(6)

输出方程对时间求导可得

(7)

Δu=(hxB(x0,u0))-1·

(8)

当控制律的解算频率足够高，步长足够小时，状态量的增量Δx相对于控制输入的增量Δu来说是高阶小量，故可略去，则增量动态逆控制律为

(9)

一般来说，控制输入的维数p大于输出维数m，故(hxB(x0,u0))-1为广义右逆。控制律中不显含非线性状态项f(x)，降低了控制律对于飞机本体数学模型的依赖，使得控制律对于飞机本体数学模型中的参数摄动具有一定的鲁棒性。同时，采用增量式的控制律算法，使得控制律需要利用到状态量的导数，在三旋翼姿态控制中意味着需要角加速度反馈量，因此需要利用到第3节所提到的角加速度估计器。

2.2 控制律设计

对于本文所研究的被控对象即三旋翼无人机的姿态控制而言，被控对象的动力学方程如式(1)所示。

定义状态量x=Ω=[p,q,r]T，控制输入为u=[F1,F2,F3,δ]T，输出为y=x=Ω。故由式(9)可得相应的增量动态逆控制律为

(10)

根据三旋翼无人机姿态控制的特性，基于以上的增量动态逆控制律，对姿态控制进行双回路控制律设计。

外回路包括姿态角回路(慢回路)和角速度回路(快回路)，控制框图如图3所示。ωφ、ωθ、ωψ和ωp、ωq、ωr分别表示姿态角回路和角速度回路的带宽。快回路带宽一般为慢回路带宽的3～5倍，具体设置为：ωφ=ωθ=ωψ=3 rad/s，ωp=ωq=ωr=10 rad/s；下标c表示指令。

图3 外回路控制框图

图4 内回路控制框图

内回路即为角加速度回路，采用增量动态逆控制律，控制框图如图4所示，u0为上一采样时刻的控制输入量，因为无法直接反馈实际电机的拉力和舵机偏转的角度，因此加入执行机构模型模拟电机和舵机的动力学环节，同时加入角加速度估计器实时地估计飞机的角加速度。

3 角加速度估计器设计

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

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

1) 运动学模型

(11)

ΔT为采样周期;z为观测量，观测量为俯仰角速度，来源为惯性测量单元(IMU)的角速率；Q和R分别为过程噪声和观测噪声。

2) 滤波估计过程[19]

角加速度估计的过程即卡尔曼滤波器的递推求解过程，故可以用于控制律中进行实时的角加速度反馈。

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

(12)

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

(13)

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

(14)

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

(15)

3) 角加速度估计结果

根据三旋翼飞行平台实际试飞过程中测量的俯仰角速度q，分别利用直接差分、直接差分加低通滤波、卡尔曼滤波器估计3种方法得到相应的角加速度，并进行比较，如图5所示。

可见，对角速度信号进行直接差分会将噪声放大，采用传统的差分加低通滤波则会带来一定的延迟，而用KF进行角加速度估计可以去除高频噪声并保证较小的延迟。

图5 角加速度估计对比图

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

互相关函数是信号分析里衡量2个信号之间相互关联程度的函数。可以通过求取互相关函数得到2个相似信号之间的时间延迟。

对于连续信号定义为

(16)

即将g(t)对时间轴原点进行翻转后与f(t)作卷积。

对于离散时间序列定义为

(17)

它表示序列x(n)不动，将y(n)在时间轴上向左或右移动m个采样间隔后与序列x(n)相乘后求和，得到平移m之后的对应互相关函数值rxy(m)，该值越大表示此时两信号越相似。因此可以通过互相关函数曲线上的峰值点得出2个相似信号之间的时间延迟。

定义归一化的互相关函数，互相关函数越接近1表示此时两信号越相似。

(18)

考察利用卡尔曼滤波得到的角加速度估计值和传统差分加低通滤波分别带来的时间延迟。选择直接进行数值差分得到的角加速度信号作为基准信号，认为此信号无时间延迟，分别计算2种方法与基准信号之间的互相关函数来比较两者的时间延迟。数据来源同3.1节中数据。

由图6结果可知，KF方法的时间延迟为0.23个 采样间隔，而差分加低通滤波方法的时间延迟为0.46个采样间隔(由于两者延迟都小于1个 采样间隔，故求取互相关函数之后采用插值的方法得到了平移非整数采样间隔点的互相关函数值)。故结果表明，采用KF进行角加速度估计可以去除高频噪声并保证较小的延迟。

图6 时间延迟对比

4 飞行试验验证

4.1 控制律实现及试飞平台

为验证增量动态逆控制律和角加速度估计器的可行性，本文进一步基于小型三旋翼无人机平台开展了飞行试验，试验所用三旋翼无人机平台如图7所示，其基本参数如表1所示。

图7 无人机实物图

表1 无人机基本参数

无人机飞控硬件平台为雷迅pixhack V3，内置3组IMU冗余设计(包含3组加速度计、3组陀螺仪、1组电子罗盘、2组数字气压计)。飞行控制律设计平台为MATLAB/SIMULINK，利用基于模型的设计方法[20]，在SIMILINK中搭建好控制器仿真模型之后，通过SIMULINK代码自动生成技术生成可执行文件到目标硬件中，并进行试飞测试等等。具体流程如图8所示。

图8 控制律设计流程图

4.2 试飞结果

三旋翼无人机姿态控制试验在室内无风条件下进行。测试姿态角的跟踪情况，验证角加速度估计器和增量动态逆控制律的有效性。

1) 三轴姿态响应

图9为三轴姿态响应仿真图。

图9 三轴姿态响应

2) 鲁棒性测试

在三旋翼无人机平台上附加重物，改变飞行器的重心位置，测试姿态角的跟踪情况，同时与常规PID控制律进行对比，考察INDI控制律对于飞机本体模型参数摄动的鲁棒性。

重心位置摄动俯仰角跟踪情况如图10所示。改变三旋翼其中一个动力单元的螺旋桨的尺寸，以达到改变执行机构效能的目的，考察控制律对于执行机构效能摄动的鲁棒性。

动力效能摄动俯仰角跟踪情况如图11所示。可以明显看出，在重心位置和动力效能有较大摄动时，常规PID控制律存在很大的稳态误差，无法很好地跟踪指令，而基于角加速度反馈的增量动态逆控制律由于降低了对飞机本体模型的敏感度，故仍能实现很好的指令跟踪效果，具有较好的鲁棒性。

图10 重心位置摄动测试结果

图11 动力效能摄动测试结果

3) 指令跟踪

图12为俯仰角、俯仰角速度以及俯仰角加速度的指令跟踪情况。图13为利用角速度差分加低通滤波进行角加速度估计得到的俯仰角指令跟踪情况。

试飞数据显示，俯仰角、俯仰角速度和俯仰角加速度均能够很好的跟踪指令，响应动态良好。俯仰角响应与俯仰角指令之间的时间延迟约为0.3 s， 与姿态角回路 3 rad/s 的带宽一致。稳定点附近有小幅的2 °左右的振荡，该振荡是由传感器噪声以及气动-结构耦合振动等因素导致。角加速度响应通过角加速度估计器实时估计得到，可以看出，2种角加速度估计方法均能达到控制效果，利用本文的角加速度估计器可以在滤除高频噪声的同时保证较小的时间延迟，保障了更好的稳定性。

图12 指令跟踪结果

图13 差分加低通滤波指令跟踪结果

5 结 论

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

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

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