第5章控制理论

视频讲解

实现四旋翼无人机的姿态控制和导航控制离不开控制理论，自动驾驶仪中的控制律可以说是整个四旋翼无人机的“灵魂”。本章主要介绍Paparazzi中所用控制律的相关知识。

5.1节介绍控制理论中的基本概念。5.2节介绍目前应用广泛的PID控制器，5.3节介绍复合控制的知识，5.4节介绍前置滤波器作用。Paparazzi中的控制器大多是结合了前置滤波器、前馈控制和PID反馈控制器的复合控制器。5.5节介绍常用的数字滤波算法，在传感器测量部分(通常为闭环控制回路的反馈环节)时常需要数字滤波算法，这些滤波算法往往是关系到反馈控制回路性能的关键因素之一。5.6节简单介绍Paparazzi实现的四旋翼无人机控制系统。

5.1控制理论的基本概念
如果一个系统由人直接操作则称为人工控制，而自动控制是指在没有人直接参与的情况下，利用外加的设备或装置(称为控制装置或控制器)，使被控对象的某个工作状态或参数(即被控量)自动地按照预定的规律运行。例如，对四旋翼无人机姿态的控制由自动驾驶仪实现，则对四旋翼无人机姿态的控制就是自动控制； 若由飞控手直接操作四旋翼无人机的导航飞行，则四旋翼无人机的导航飞行就是人工控制。

控制理论的范畴很宽泛，此处所说的控制理论是指研究自动控制技术的理论。控制理论研究的内容之一就是如何改进动态系统的性能。何为动态系统呢？系统状态随时间而变化的系统或者按确定性规律随时间演化的系统就称为动态系统。
5.1.1控制的基本方式

一个控制系统包含了控制器(也称为控制装置)和被控对象(也称为受控对象)两个部分，若按照控制方式区分，可以分为开环控制系统、闭环控制系统和复合控制系统。
1. 开环控制系统
系统的控制器不受系统输出影响的控制系统是开环控制系统，开环控制系统中的信息流向是单向的。开环控制系统结构示意图如图51所示。


图51开环系统结构示意图


开环控制系统结构简单、成本低，但是不具备自动修正输出量偏差的能力，更容易受到外界干扰的影响。
如果外界干扰量能够被测量到，可以采用补偿的方式抑制外界干扰的影响，称为按干扰补偿的开环控制方式，也称为前馈方式，其结构示意图如图52所示。


图52按干扰补偿的开环控制结构示意图





2. 闭环控制系统
闭环控制系统也称为反馈控制系统，在闭环控制系统中既有信息的前向通路，也存在从输出到输入反馈通路，两者共同组成了一个闭合的回路。反馈控制是一种检测偏差、利用变差最后消除偏差的控制方式。控制器利用输入信息和输出信息的偏差量以一定控制规律产生控制作用，该控制作用使偏差量减小。闭环控制是最常见的一种控制方式，其结构示意图如图53所示。



图53闭环系统结构示意图

在闭环控制中由于反馈的存在，可以抑制反馈回路中干扰的影响，这些干扰可能是外部环境的外扰，也可能是回路中元器件参数的波动。例如，在四旋翼无人机姿态控制回路的外部环境中阵风的影响和电子调速器参数、电机参数的波动等都能被姿态控制回路所抑制。

相对于开环控制方式，闭环控制能够提供更好的性能，但是闭环系统也较为复杂。另外，反馈也会引入两个新问题： 一个是控制系统稳定性的问题； 另一个是反馈元件(传感器)的噪声问题。
3. 复合控制系统
闭环控制系统中的反馈控制和开环控制系统中的补偿方式可以同时存在，两者之间并不矛盾，其中补偿部分也
称为前馈控制或顺馈控制。若控制系统同时存在反馈控制和补偿部分，则称这种控制系统是复合控制。
为什么要将反馈控制和补偿方式(即前馈控制)相结合呢？因为在闭环控制系统中有些性能参数对反馈控制器参数的要求是矛盾的，而在反馈控制的基础上增加前馈控制可以化解这些矛盾。
复合控制系统有两种形式： 一种是按干扰补偿的复合控制； 另一种是按输入补偿的复合控制。
1) 按干扰补偿的复合控制
反馈控制是一种“滞后”的控制方式，虽然对反馈回路中的干扰有抑制作用，但这个过程是要等干扰作用“体现”出来后。也就是说，要等干扰作用使系统的输出偏离了输入，两者之间产生了偏差，反馈控制才能按照检测偏差、利用偏差、消除偏差的过程起作用。由于反馈的存在，干扰信号造成的偏差最终会被消除，越强的干扰在系统的输出中留下的“痕迹”越大。这个过程其实是比较快的，一般的干扰不必太担心。但是干扰会加重反馈控制器的负担，为了抑制回路中的干扰，通常要求控制器具有一个适当高的开环增益或是一定的型别，开环增益越高、型别越高(确切地说应该是干扰点前的等效型别)对干扰的抑制作用越好，但是开环增益越高、型别越高通常系统的动态特性越差，系统更容易出现超调甚至不稳定，所以控制系统开环增益和型别是受到一定限制的。

如果系统的干扰较强，而这个干扰信号恰好可以被测量到或辨识估计到，就可以采用补偿的方式消除这个干扰的影响，其结构示意图如图54所示。


图54按干扰补偿的复合控制结构示意图


按干扰补偿的部分也称为前馈控制，它的作用原理和反馈控制的“滞后”不同。前馈控制方式是一种“预见性”的控制，一旦出现干扰就会立刻动作，而反馈控制要等干扰作用“体现”到输出中，并采集到反馈量中，控制器才会有反应。理论上对干扰的补偿是可以完全消除干扰对系统的影响。
2) 按输入补偿的复合控制
按干扰补偿的复合控制可以在不影响系统动态性能的前提下减小干扰所造成的误差，而按输入补偿的复合控制同样可以在不影响系统动态性能的前提下减小输入所造成的误差。按输入补偿的复合控制结构示意图如图55所示。


图55按输入补偿的复合控制结构示意图


在Paparazzi四旋翼无人机姿态控制和导航控制中有多处控制器采用了复合控制的方式。例如，姿态控制器、水平线运动控制器和垂直方向运动控制器等。
5.1.2控制系统的性能
衡量一个控制系统是否优秀，称为控制系统的性能，主要包括3个方面，即稳定性、动态特性和稳态特性。另外，还有抗干扰性、鲁棒性等。
1. 稳定性
控制系统能够正常工作的首要条件就是控制系统必须是稳定的。在实际工程应用中，不仅要求控制系统是稳定的，而且还要求控制系统具备一定的稳定裕度，一个接近临界稳定的控制系统一般没有实际应用的价值。
稳定性的定义有多种方式，按照工程中常用的经典提法： 若控制系统在足够小的偏差作用下，其动态响应过程逐渐衰减并趋向于零，即系统具有恢复原平衡状态的能力，则称该系统是稳定的。
非线性系统的稳定性是比较复杂的，而线性定常系统的稳定性则是系统自身的性能。对于线性定常系统而言，稳定的充要条件是系统的特征根全部具有负实部。
稳定性是控制系统的首要条件，只有在系统稳定的前提下，对其动态特性和稳态特性的分析才有意义。一个稳定的系统对外界的激励或内部状态的变化可以分为两个响应过程，即动态过程和稳态过程。描述这两个响应过程性能的是动态性能和稳态性能。

2. 动态性能
系统从一个状态变化到另一个状态的动态过渡过程中体现的特性称为动态性能，系统的动态性能包括快速性和平稳性。

快速性描述了系统状态变化时的速度，不同的系统差异可能会很大。例如，大型轮船转弯的时间数量级是分钟级的，而航模级的飞行器转弯时间数量级是秒级的。因此，对系统快速性的分析要结合具体被控对象进行。

平稳性是指系统在过渡过程中的过调现象，过调越大，平稳性越差。虽然平稳性和稳定性是两个不同的概念，但是两者之间有固定的联系，通常一个系统稳定性越好，其平稳性也越好。在系统分析中可以使用相对稳定的程度(稳定裕度)描述系统的平稳性。

在时域中通常使用系统单位阶跃响应描述动态性能： 快速性可以使用调节时间(也称为过渡过程时间)ts描述，调节时间描述了过渡过程时间的长短，定义为响应曲线完全进入稳态值误差带的时间。常用的误差带为5%或2%。平稳性可以使用超调量σ%描述，超调量定义为响应曲线超出稳态值的最大量占稳态值的百分比。

在频域中通常使用闭环频域的带宽ωb和闭环频域的谐振峰Mr以及开环频域的开环截止频率ωc和稳定裕度(相位稳定裕度γ和幅值稳定裕度h)等物理量。

3. 稳态性能
稳态性能描述了系统过渡过程结束后系统输出对系统输入的跟踪情况，通常可以使用稳态误差描述稳态性能。稳态误差包括原理性稳态误差和结构性稳态误差，这里的稳态误差仅指原理性稳态误差。稳态误差反映了系统对输入信号的跟踪能力，影响因素取决于输入信号、系统型别和开环增益。系统的型别是指开环传递函数中积分环节的个数，常见的是Ⅰ型系统，开环传递函数中包含一个积分环节，能够跟踪上速度信号，但是与输入信号存在一个固定的位置偏差，该偏差的大小与开环增益成反比。Ⅱ型以上系统能够跟踪上速度信号且没有位置偏差。系统的型别越高，跟踪信号的能力越强，但是相应的系统的稳定性、动态性能越差。因此，在设定系统的型别和开环增益时，需要折衷考虑系统的动态性能和稳态性能。另外，也可以在闭环系统基础上增加开环补偿的方式，即复合控制方式，在不影响系统稳定性的前提下改善系统稳态性能。
5.2PID控制器
基于反馈思想可以有多种方法实现其反馈控制器，其中比较简单的反馈控制器有通断控制器(继电器控制器)、比例控制器(P控制器)和比例积分微分控制器(PID控制器)。
5.2.1通断控制器

通断控制是一种简单的反馈机制，可以由式(51)描述，即


u(t)=umax若e(t)>0



umin若e(t)<0(51)



式中e(t)——偏差量； 
u(t)——控制量。
偏差量e(t)和控制量u(t)之间的关系如图56所示。
通断控制的优点是简单，很多时候能够使被控量接近参考量。通断控制器很容易引起控制量的振荡，当这个振荡比较慢时，通断控制的方法也是可以接受的，通断控制器适用于比较简单的应用场合。

式(51)中的控制量u(t)在e(t)=0时是没有定义的，当偏差e(t)≈0时极易造成控制量的振荡，为了减弱该振荡可以引
入死区特性（图57）和滞回特性（图58）。


图56通断控制



图57死区特性



图58滞回特性

5.2.2比例控制器(P控制器)
对通断控制器进一步改进，令控制器输出的控制量和系统偏差成线性比例时，就构成了比例控制器，其控制律为式(52)，比例控制器输入输出特性曲线如图59所示。


u(t)=umax若e(t)>emax

Kpe(t)若emin≤e(t)≤emax

umin若e(t)<emin(52)




图59比例控制器输入
输出特性曲线


图59中具有饱和限幅的部分，饱和限幅并非人为有意增加的，而是受实际器件的限制实际器件的饱和限幅是客观存在的，有时也会在控制律中人为增加限幅环节，限制控制量的输出范围。。对于所有现实存在的、物理可实现的系统在大范围内都具有饱和的特性，但是对于很多系统在一定范围内可以忽略这种饱和特性，这样就可以将系统视为线性系统。例如，对于比例控制器在比例区间
(Proportionalband)(emin,emax)内可将其视为线性系统，式(52)可以简化为u(t)=Kpe(t)，表示为传递函数形式为


D(s)=U(s)E(s)=Kp(53)


线性系统满足叠加原理，而且求解比较方便，另外在控制理论中也有成熟的研究线性系统理论。因此，在后面的章节中如非特别声明，均指系统工作在线性区域。

比例控制器是一种即时的、当前的控制器，只要有偏差信号就会立刻动作。比例控制器中的Kp称为比例系数，Kp越大则系统的稳态误差越小，通常系统动态特性的阻尼越小，系统的超调量就越大。

也就是说，从稳态误差的需求看，比例系数Kp越大越好，但从动态特性的需求看，比例系数Kp不能太大。若比例系数Kp太大，则系统动态特性中的平稳性变差，甚至会引起系统的不稳定。另外，比例系数Kp也不能太小，若比例系数Kp太小，会使系统的开环增益过小，而开环增益太小则反馈作用会被削弱，反馈抑制干扰和系统非线性等能力就会相应下降，因此比例系数Kp也不宜过小。
5.2.3比例积分控制器(PI控制器)

在比例控制的基础上再增加一个积分项就构成了比例积分控制器，简称PI控制器，其时域微分方程和传递函数分别为


u(t)=Kpe(t)+Ki∫t0e(τ)dτ(54)

D(s)=U(s)E(s)=Kp+Ki1s(55)


增加了积分控制之后，即使系统的当前偏差为0，PI控制器的输出也可能是非零的。不同于比例控制的“当前”控制，积分控制是一种“过去”的控制。积分控制能够理论上减小甚至消除系统的稳态误差，但是由于积分的“滞后”作用，会恶化系统的动态特性，影响系统的稳定性。
5.2.4比例微分控制器(PD控制器)

在比例控制的基础上再增加一个微分项就构成了比例微分控制器，简称PD控制器，其时域微分方程和传递函数分别为


u(t)=Kpe(t)+Kdde(t)dt(56)

D(s)=U(s)E(s)=Kp+Kds(57)


微分控制是一种预测“未来”的超前控制，能够改善系统的动态特性，但是对系统的稳态误差没有影响。微分作用对噪声信号比较敏感，若系统的偏差信号中包含较强的噪声信号，则微分控制会放大这些噪声信号的影响。然而系统的传感器中通常会耦合一定的噪声信号，因此微分控制通常需要进一步改进才能正常应用。

5.2.5比例积分微分控制器(PID控制器)
1. 连续PID控制器


将比例、积分和微分控制结合在一起就构成了著名的比例积分微分控制器，通常简称为PID控制。PID控制器是目前工程系统中应用最广泛的控制器，前面介绍的PI控制和PD控制可以视为PID控制器的特例。
PID控制器时域微分方程的表达式为


u(t)=Kpe(t)+Ki∫t0e(τ)dτ+Kdde(t)dt(58)


式中Kp——比例系数； 
Ki——积分系数； 
Kd——微分系数。
PID控制器也可以写成时间常数的形式，即



u(t)=Kpe(t)+1Ti∫t0e(τ)dτ+Tdde(t)dt(59)


式中Kp——比例系数； 
Ti——积分时间常数； 
Td——微分时间常数。
PID控制器传递函数为式(510)，其结构如图510所示。


D(s)=U(s)E(s)

=Kp+Ki1s+Kds

=Kp+KpTis+KpTds
(510)




图510PID控制器的结构框图

2. 数字PID控制器
目前，控制器的实现大多采用数字计算机实现，而计算机系统是一种采样控制系统，只能处理数字信号。也就是说，在计算机控制系统中不能直接实现式(58)的连续PID控制器，需要对式(58)离散化，用差分方程代替原来连续系统的微分方程。
用离散求和代替积分，用后向差分代替微分，由式(59)可得离散PID表达式，即


u(tk)=Kpe(tk)+TTi∑tkj=0e(tj)+TdT(e(tk)-e(tk-1))(511)


式中T——采样周期。

在本书中对于离散系统的变量在tk时刻的值采用了两种简写方式。例如，变量u在tk时刻的值为


utk=uk=uk


故式(511)还可以表示为


u(k)=Kpe(k)+TTi∑kj=0e(j)+TdT(e(k)-e(k-1))(512)


或


uk=Kpek+TTi∑kj=0ej+TdT(ek-ek-1)(513)


后面的离散系统在无歧义时将采用类似式(513)的表示方式，若变量有脚标则采用类似式(512)的表示方式。
若令



KI=KpTTi

KD=KpTdT(514)


则可以得到式(58)的离散PID表达式为


uk=Kpek+KI∑kj=0ej+KD(ek-ek-1)
(515)


该式也称为PID的位置式控制算式或位置式PID控制算法。
根据式(515)还能够得到PID位置算式的递推形式，即


uk=uk-1+Kp(ek-ek-1)+KIek+KD(ek-2ek-1+ek-2)
(516)


PID位置算式，即式(514)、式(515)和式(516)都是编程时经常使用的PID算式，PID位置算式适用于执行机构没有记忆的情况，如直流电机、无刷电机、舵机等。
另外，由式(516)还能得到PID控制器的增量形式，即


Δuk=uk-uk-1=Kp(ek-ek-1)+KIek+KD(ek-2ek-1+ek-2)
(517)


若执行结构能够使用增量进行控制时，如步进电机，可以使用PID增量式计算。
5.2.6PID控制器的改进
前面所介绍的基本PID控制器在应用中还有一些不足之处，这些不足主要体现在积分控制器和微分控制器中，在实际
应用中通常需要对其进行改进，这些改进方式主要包括以下几个方面。
1. 积分饱和与其防止方法
在一个实际的控制系统中，控制器的输出会受到执行机构的约束而限制在一个有限的范围内，即


umin≤u≤umax(518)


例如，电机的最高转速是定值，当控制器的输出超过一定值后，电机的转速则运行在饱和临界值上，此时控制器的输出只有数字意义，不能表示实际执行机构的输出，从而发生饱和效应。

当控制系统启动、停止或是大幅度改变输入值时，系统会出现长时间较大的偏差，该偏差值经过积分控制器的累加后，可能会使控制量超出执行机构的临界值，从而影响控制效果。

例如，当控制量u>umax时，那么实际输出只能取其上限umax，而不是控制器计算得到的结果。控制系统的输出虽然在上升，偏差也在减小，但是相对于没有饱和效应的情况要慢，PID中的积分项也会积累较大的值，即便是出现了负偏差积分项也需要较长的时间才能脱离饱和区。

这种主要由于积分项引起的PID控制器的饱和称为“积分饱和”。积分饱和会增加系统的调节时间和超调量，对控制系统的性能是不利的。积分饱和的防止方法有多种，常见的两种方法是积分分离法和遇限削弱积分法。
1) 积分分离法
积分分离PID也称为PIDPD算法，是克服积分饱和的算法之一。PID控制器中的积分项的主要作用是减小稳态误差，对动态特性是不利的，积分分离法正是利用了这个特点，仅在稳态值的一个区域内使用积分，即这个区域内是PID算法，而在这个区域外则不使用积分，即区域外为PD算法。积分分离法的数学描述为



uk=Kpek+KI∑kj=0ej+KD(ek-ek-1)若|ek|>ε

Kpek+KD(ek-ek-1)若|ek|≤ε(519)


式中ε——积分分离的界限。
积分分离法的原理示意图如图511所示。


图511积分分离法的原理示意图

使用该方法是要注意积分分离的界限ε的设置。ε太大则积分分离效果减弱； ε太小则可能导致输出量无法进入该区间，积分项始终不起作用，PIDPD控制器事实上成为了PD控制器。
2) 遇限削弱积分法

遇限削弱积分法的基本思想是： 当控制量u进入饱和区域后，积分项不再累加，仅执行削弱积分的运算。遇限削弱积分法原理示意图如图512所示。


图512遇限削弱积分法原理示意图


在计算控制量uk时，先判断uk-1是否饱和，若umin<uk-1<umax则正常累加积分，若uk-1≤umin或uk-1≥umax则积分项仅累加使积分项减小的值。遇限削弱积分法可以避免控制量长时间停留在饱和区。
另外，还有一些该方法的变化形式，如“遇限”的条件可以设置为积分项的限制，即ui(k-1)≤ui,min或ui(k-1)≥ui,max时积分项仅累加使积分项减小的值。


2. 非线性积分器
PID控制器中积分项的主要作用是减小稳态误差，从频域角度看，由于积分的存在会使系统相位滞后90°，对系统的稳定性和动态性能的影响都是不利的。为此很多学者提出了非线性积分器，在保留提高系统稳态性能的
前提下，尽量减小其相位滞后。常见的非线性积分器有Clegg非线性积分器(CNI)、Karybakas非线性积分器(KNI)和智能非线性积分器(INI)。
1) Clegg非线性积分器(CNI)
Clegg非线性积分器(CNI)的微分方程为


u(t)=1Ti∫e(t)dte(t)≠0


0e(t)=0(520)


CNI积分器的相位滞后只有38.1°。

当系统处于平稳过程时，CNI积分器是一个线性积分器； 当系统处于振荡状态时，CNI积分器表现为一个非线性积分器。CNI积分器具有抑制自振荡的作用。
CNI积分器的输入信号过零时输出会产生跳变，因此CNI积分器会产生较多的高次谐波信号，并且CNI积分器不具备线性积分器的高频滤波特性，因此可能对系统产生不利影响。
CNI积分示意图如图513所示。


图513CNI积分示意图

2) Karybakas非线性积分器(KNI)
Karybakas非线性积分器(KNI)的微分方程为


u(t)=1Ti∫|e(t)|dte·(t)>0

-1Ti∫|e(t)|dt
e·(t)<0(521)


KNI积分器的相位滞后为0°。实际应用中，KNI对输入的高频干扰噪声比较敏感。
KNI积分示意图如图514所示。




图514KNI积分示意图

3) 智能非线性积分器(INI)
智能非线性积分器(INI)的微分方程为


u(t)=0e(t)=0,t=tm

1Ti∫ttme(t)dte(t)e·(t)>0,tm<t<tn

1Ti∫tntme(t)dte·(t)=0,t=tn

u(tn)e(t)e·(t)<0,t>tn(522)



式中tm——e(tm)=0的时刻； 
tn——e·(tn)=0的时刻。
智能非线性积分器(INI)的相位滞后为27.6°，且输出的3次谐波也比CNI小得多。因此，INI积分控制器
具有较好的性能。
INI积分示意图如图515所示。


图515INI积分示意图

3. 微分算法的改进

在PID控制器中微分的作用主要是为了改善动态性能，但是由于微分对噪声信号很敏感，所以在实际应用中往往需要对微分控制器进行改进。

1) 四点中心差分法
微分运算容易引入干扰，特别是计算机控制中用差分代替了微分，更接近理想的微分，对噪声更敏感，因此要设法减小噪声对微分项的影响。常用的对微分的改进方法有四点中心差分法和不完全微分。

四点中心差分法是数字PID算法中应用成功的一种方法。一方面将KD选择得比理想微分稍小些，另一方面组成微分时，不直接使用当前的偏差ek，而是应用过去和当前4个时刻偏差的平均值，即


e-k=ek+ek-1+ek-2+ek-34(523)


然后再通过加权求和形式，将差分近似成


Δek=14ek-e-k1.5+ek-1-e-k0.5-ek-2-e-k0.5-ek-3-e-k1.5

=16(ek+3ek-1-3ek-2-ek-3)
(524)


2) 不完全微分
使用数字计算机实现的数字PID中的微分项比使用运算放大器实现的模拟PID中的微分项更接近理想的微分。数字微分器会将计算的微分在一个采样周期内全部输出，这就会造成在这个采样周期中控制器的输出值会比较高，从而超出控制量极限边界进入饱和区。这样，实际上相当于一部分微分项的输出损失了，微分的作用事实上是被削弱了。

另外，数字控制系统的采样回路中会产生高频干扰，而微分项对这些高频干扰很敏感，因此，几乎所有的数字控制回路都设置了一级低通滤波器，控制器形式和效果都比较接近模拟控制器。

常用的不完全微分的PID控制器有两种形式。一种是在微分控制器前附加一个时间常数为Td/N的惯性环节，模拟形式的PID控制器传递函数为


D(s)=U(s)E(s)

=Kp+Ki1s+Kds1+TdNs

=Kp+KpTis+KpTds1+TdNs(525)


通常N的取值范围为3~10，对式(525)离散化就得到不完全微分的PID算式。
另一种是所有的PID输出信号都通过一个惯性环节，其模拟PID的传递函数为


D(s)=U(s)E(s)

=1Tfs+1Kp+Ki1s+Kds

=KpTfs+11+1Tis+Tds
(526)


对式(526)离散化后同样可以得到不完全微分的PID算式。式(525)和式(526)都是常用的不完全微分的PID算式。
3) 微分先行PID算法
微分先行PID算法是将微分运算放在其他控制项的前面。微分先行PID算法有两种结构： 一种是对输出量的微分先行，如图516所示； 另一种是对偏差量的微分先行，如图517所示。


图516对输出量微分先行



图517对偏差量微分先行

图516所示对输出量的微分先行结构中，只对输出量进行微分运算，相当于在控制系统中增加了测速反馈控制器，只不过其中的速度反馈量不是由传感器测量得到，而是由对位置量的微分得到，其作用与测速反馈控制作用类似。


图517所示的另一种微分先行的结构是对偏差的微分先行，适用于串级控制的副控回路。
4. 带死区的PID控制

在计算机控制系统中，某些时候对控制精度的要求不太高，更希望系统的控制器不要频繁动作，这时就可以采用带死区的PID控制器。带死区的控制器就是在控制系统中人为地设置一个死区环节，不灵敏区的范围为B，当偏差量进入死区范围后(|ek|≤B)，控制器的输出维持之前的输出，即Δuk=0，也就是说，在一定范围内都认为偏差为零，数学描述为


Δuk=Δupid(k)|ek|>B



0|ek|≤B(527)


式中Δuk——带死区的PID控制器的增量输出； 
Δupid(k)——普通PID控制器的增量输出。
死区B是一个可调的参数，该值的设置需要根据实际情况而定。B太小，则控制器调节动作依旧频繁； B太大，则会产生比较大的纯滞后。
5.3复合控制
5.3.1前馈控制

利用开环补偿原理的控制方式也称为前馈控制或顺馈控制。与反馈控制相比，只要输入信号作用到系统上，前馈控制的
作用就会产生，由于不需要等系统产生偏差后再形成控制作用，所以前馈控制比反馈控制更“及时”。但是前馈控制不能形成闭环，没有反馈作用，前馈控制器的误差或作用到前馈控制通路的外部干扰将与前馈控制量一起作用到被控对象上，从而造成输出量的误差。因此前馈控制通常不单独使用，而是和反馈控制结合在一起使用，依靠反馈控制减弱或消除前馈通路的误差。而前馈控制作为一种开环补偿的方式，不会影响闭环反馈系统的稳定性。

这种将前馈控制和反馈控制有机结合在一起的控制方式称为复合控制。复合控制有按干扰补偿的复合控制和按输入补偿的复合控制。
5.3.2按干扰补偿的复合控制
按干扰补偿的复合控制的前提条件是要求被补偿的干扰信号必须能够被测量或估计。常用的按干扰补偿的复合控制的结构
原理如图518所示。
在图518所示系统中，干扰信号的误差传递函数Φn(s)为


Φen(s)=En(s)N(s)=-1+Dff(s)G(s)1+Dfb(s)G(s)(528)




图518按干扰补偿的复合控制的结构原理框图

若前馈控制器Dff(s)为


Dff(s)=-1G(s)(529)


则系统完全不受干扰信号N(s)的影响，即干扰信号N(s)产生的误差Een(s)始终为零。式(529)是对干扰信号N(s)的全补偿条件。但是由于被控对象G(s)是现实系统的传递函数，故G(s)总是真有理分式的形式，所以全补偿条件的前馈控制器D
ff(s)一定不是真有理分式，即前馈控制器Dff(s)不具有物理可实现性。

如果在全补偿条件的基础上，给前馈控制器Dff(s)增加一定数量的小时间常数的惯性环节，使其具备物理可实现性，则称为近似全补偿。
如果只考虑稳态误差，将干扰信号N(s)产生的稳态误差补偿为零，则称为稳态全补偿。稳态全补偿的条件为


lims→0 Φen(s)=0(530)


将前馈控制器Dff(s)设置比例控制器，选择满足式(530)的比例系数，就能够实现对干扰信号N(s)稳态补偿。
5.3.3按输入补偿的复合控制
常用的按输入补偿的复合控制的结构原理图如图519所示。


图519按输入补偿的复合控制的结构原理框图

图519所示系统的闭环传递函数Φ(s)和误差传递函数Φe(s)分别为


Φ(s)=Y(s)R(s)=(Dfb(s)+Dff(s))G(s)1+Dfb(s)G(s)(531)

Φe(s)=E(s)R(s)=1-Dff(s)G(s)1+Dfb(s)G(s)(532)


由式(532)表示的误差传递函数可知，若前馈控制器Dff(s)为



Dff(s)=1G(s)(533)


则系统完全没有误差，即输出Y(s)与输入R(s)完全一致。式(533)是输入误差的全补偿条件。但是由于被控对象G(s)是现实系统的传递函数，即G(s)总是真有理分式的形式，所以全补偿条件的前馈控制器Dff(s)一定不是真有理分式，故前馈控制器Dff(s)不具有物理可实现性。

如果在全补偿条件的基础上，给前馈控制器Dff(s)增加一定数量的小时间常数的惯性环节，使其具备物理可实现性，则称为近似全补偿。如果被控对象的传递函数较复杂，则近似全补偿条件的前馈控制器Dff(s)也会较复杂。如果再考虑时变、不确定因素等，近似全补偿条件的前馈控制器Dff(s)将会更加复杂。

在实际应用中通常采用部分补偿的形式，增加开环补偿形式的前馈控制器，提高系统的型别，使其满足系统稳态精度的需求。

为前馈控制器Dff(s)设置适合的传递函数，使系统的误差传递函数Φe(s)可表示为



Φe(s)=bmsm+bm-1sm-1+…+bisiansn+an-1sn-1+…+a1s+a0(534)


则系统可等效为i型系统，常见系统i≤2，即系统的型别不高于Ⅱ型。此时前馈控制器通常为


Dff(s)=λ0+λ1s+λ2s2(535)


例如，在图520所示的控制系统中，前馈控制器Dff(s)=s，将原控制系统的型别
从Ⅰ型提高到了Ⅱ型。


图520部分补偿的前馈控制

图520所示控制系统的闭环传递函数和误差传递函数分别为


Φ无前馈(s)=0.44s+1(0.3s+1)(0.04s+0.24s+1)(536)

Φ有前馈(s)=0.012s2+0.54s+1(0.3s+1)(0.04s+0.24s+1)(537)

Φe,无前馈(s)=0.1s(s+1)(0.12s+1)(0.3s+1)(0.04s+0.24s+1)(538)

Φe,有前馈(s)=0.1s2(0.12s+1)(0.3s+1)(0.04s+0.24s+1)(539)


图520所示系统的单位阶跃响应和单位速度响应如图521和图522所示。由示例分析可以得到，合适的前馈控制可以改善系统的稳态性能，在不影响系统稳定性和闭环极点的前提下能够提高系统的型别。另外，前馈控制增加了系统的闭环零点，一

图521复合控制单位阶跃响应



图522复合控制单位速度响应

般会使系统的运动加速，超调量增加，某些情况下调节时间会缩短。减小前馈控制的增益可以减小控制系统的超调量，但这是以稳态误差增加为代价的，在实际应用中可以根据需求对其进行调整。

5.4前置滤波器
控制系统中动态性能和稳态性能对开环增益和系统的类型要求是矛盾的，抗干扰能力和伺服跟踪能力对开环增益和系统类型的要求也是矛盾的，为系统设置前置滤波器是解决这些矛盾的方法之一。
前置滤波器也可以称为目标值滤波器或安排过渡过程，其结构原理框图如图523所示。


图523前置滤波器的结构原理框图

为了使控制系统具有较好的稳态性能、抗干扰能力以及抑制回路中非线性和参数波动的能力，系统的开环增益不宜过小，但是较大的开环增益可能会造成系统的超调，即影响其平稳性。分析其原因不难发现，主要由于输入的阶跃信号使系统在较长的时间内维持了一个较大的偏差，在比例控制和积分控制共同作用下极易产生较大的超调。然而由于被控对象自身固有的惯性以及执行机构输出固有的饱和特性的存在，被控量不可能完美地跟随输入的阶跃信号，也就是说输入所造成的长时间的大偏差只会增加系统的超调量，而对系统快速性没有太大影响。

前置滤波器的实现可以使用线性滤波器、PID控制器或跟踪微分器等，在此以二阶低通滤波器为例。

假设一个使用PID控制器的反馈控制系统，被控对象的传递函数为G(s)=14s2+0.8s+1，系统存在一个常值干扰n(t)=1，能够满足其抗干扰性能的3个PID控制器参数分别为Kp=40、Ki=10、Kd=8，控制器输出值饱和范围为-5≤u≤+5，控制系统结构框图如图524所示。


图524PID控制系统

当前整定的PID控制器参数能够满足控制系统抑制外部干扰的需求，但是其输入响应具有较大的超调，可以采用增加前置滤波器的方法改善输入响应，前置滤波器的时间常数应与被控对象的时间常数相当。
在常值干扰n(t)=1作用下，图524和图525所示的两个控制系统产生的输出是相同的，其干扰的响应如图526所示。也就是说，前置滤波器对反馈抑制干扰的能力没有影响。




图525具有前置滤波器的PID控制系统



图526干扰作用下系统响应

前置滤波器能够在不改变其抗扰动能力的前提下，改善系统的动态特性。图524和图525所示系统的单位阶跃响应如图527所示。


图527单位阶跃输入作用下系统响应

5.5常用数字滤波算法
在反馈控制系统中，被控量的测量是构成反馈控制的基础。被控量的测量需要使用传感器实现，在传感器测量过程中往往会有噪声干扰耦合到测量信息中，需要增加滤波器保证被控量的测量。另外，在控制系统中有时不能直接测量到被控量，但是可以测量到和被控量相关的某些物理量。例如，假设被控量为速度，使用的传感器却只能测量到加速度，那么就可以对加速度测量值积分而得到速度量。
5.5.1算术平均值滤波

算术平均值滤波就是在k时刻连续采样N次，把N次采样的算术平均值作为k时刻滤波器的输出值，其算法公式为


x-k=1N∑Ni=1xi
(540)


算术平均值滤波要求传感器的采样周期要比控制系统的采样周期至少快N倍，这样才能保证在控制系统的一个采样周期内完成传感器数据的N次采样。
算术平均值滤波对信号的平滑程度取决于平均次数N，平均次数越大，平滑度越好，但灵敏度越低。算术平均值滤波对周期性的干扰噪声具有良好的抑制作用，但是对脉冲干扰抑制效果不佳。
5.5.2加权平均值滤波

算术平均值滤波N次采样得到的采样值在滤波过程中所占的比例是相同的，均为1/N。有时为了突出某几次采样值在平均值中的比例，可以对不同时刻的采样值赋予不同的加权因子，这种方法称为加权平均值滤波，其算法公式为


x-k=1N∑Ni=1aixi(541)


加权平均值滤波适用于控制系统存在较大延迟而传感器采样周期较短的情况。
5.5.3递推平均滤波(滑动平均滤波)

算术平均值滤波和加权平均值滤波均要求有较快的测量速度，要在控制系统一个采样周期内至少完成N次传感器的测量，因此不适用于测量速度较慢或要求数据计算较快的控制系统。递推平均滤波(滑动平均滤波)传感器的采样周期和控制系统的采样周期是相同的，能够在传感器测量速度不快的系统中使用，其算法公式为


x-k=1N∑ki=k-Nxi(542)


同算术平均值滤波适合的干扰噪声类似，递推平均滤波同样对周期性干扰具有较好的抑制作用，对脉冲干扰抑制效果也较差。

当利用式(542)计算均值时，为每个采样时刻的采样值设置不同的权重系数，也可以构成加权递推平均滤波。
递推平均滤波可以视为加权递推平均滤波的一种特殊情况，递推平均滤波和加权递推平均滤波其实是
FIR滤波的一种形式，用脉冲传递函数表示加权递推平均滤波公式为


yk=∑N-1k=0z-kakxk(543)


当系数ak为1时，则式(543)就成为了递推平均滤波。
5.5.4中值滤波
中值滤波是对传感器连续测量N次(N通常为奇数)，然后对N次测量值按大小排序，取中间值作为本次测量的结果。
中值滤波对脉冲干扰具有较好的抑制作用，但是对变化速度较快的物理量不适用。
5.5.5程序判断滤波
程序判断滤波是根据两次采样数据之间关系进行判断的一种滤波方法，这是一种简单而又有效的滤波方法，常用的程序判断滤波可以分为限幅滤波和限速滤波两种方式。
1.  限幅滤波
限幅滤波认为在传感器两次采样时间间隔内，被测量的真实变化量不会超过某个范围，如果测量到的数据超出了该范围，则认为是干扰造成的，继续使用上一次采样数据代替本次采样。限幅滤波的算法计算式为


yk=xk若|xk-xk-1|≤Δx



xk-1若|xk-xk-1|>Δx(544)


式中yk——在第k次采样时刻滤波器的输出； 
xk——传感器第k次采样数据； 
Δx——两次采样值的最大变化范围，Δx的选择取决于传感器采样的间隔时间和被测量x的动态响应速度。
2. 限速滤波
限速滤波也是先设定相邻两次采样的最大允许增量Δx，若本次采样值和上次采样值的变化量在允许范围
内，则认为该次采样数据有效； 若本次采样值和上次采样值的变化量超出允许范围，则需要根据下一次传感器的采样数据做进一步判断，限速滤波的算法计算式为


yk=xk若|xk-xk-1|≤Δx

xk+1若|xk-xk-1|>Δx,且|xk+1-xk|≤Δx

xk+1+xk2若|xk-xk-1|>Δx,且|xk+1-xk|>Δx(545)


限幅滤波和限速滤波都能有效地克服因偶然因素引起的脉冲干扰，但是无法抑制周期性的干扰，而且平滑度差。

5.5.6惯性滤波
惯性滤波也称为一阶滞后滤波，是将连续系统中的一阶惯性环节离散化后得到的数字滤波算法。连续系统的惯性环节传递函数为


G(s)=Y(s)X(s)=1τs+1(546)


式中τ——采样周期。
按照后向差分离散化后的脉冲传递函数为


G(z)=Y(z)X(z)=TT+τ1-τT+τz-1(547)


式中T——采样周期。
令α=τT+τ，并将式(547)转换为便于编程的差分方程形式，得到


yk=(1-α)xk+αyk-1(548)


惯性滤波是由连续的一阶低通滤波经过离散化后得到的，其滤波特性与连续一阶低通滤波相似，对高于截止频率1/τ的噪声信号有抑制作用。
惯性滤波是IIR滤波器的一种，可以使用IIR滤波器的方法对其进行分析。在实际应用中，如果简单的一阶低通滤波器不能满足滤波要求，还可以使用高阶的模拟滤波器离散化后得到的数字滤波器。
5.5.7互补滤波

当多个传感器对同一个物理量各自有独立的测量数据且各自干扰噪声不同时，可以使用互补滤波。互补滤波也可以看作是传感器数据融合的一种算法。

例如，对于位置量x，位置传感器可以直接测量，测量值为xm,p=x+ξp，其中ξp为高频干扰噪声


。而速度传感器测量到的速度值x·m,v=x·+ξv经过积分之后也能得到位置量x的测量值xm,v=x+
∫ξvdt，由于积分作用的影响，干扰噪声∫ξvdt为低频干扰，如果噪声信号ξv中包含常值偏差，则∫ξvdt信号是单调发散的在此处脚标m是测量（measure）的缩写，p指位置（position）传感器，v指速度（velocity）传感器。 。
可以选择一对互补的滤波器传递函数F低(s)和F高(s)，满足互补条件是指F低(s)+F高(s)=1，其中F低(s)是低通滤波器，F高(s)是高通滤波器，则位置量x的估计值在频域中可以表示为



X^(s)=F低(s)X
m,p+F高(s)Xm,v


=X(s)+F低(s)Ξp+F高(s)Ξvs
(549)




图528互补滤波器的结构原理框图

由于信号X(s)是全通的、无损的，而噪声信号Ξp和Ξv/s则分别只有低频和高频通过，所以这个滤波器也是无损滤波器。
互补滤波器的构造比较简单，其结构框图如图528所示。

其中，互补滤波器的输出为


X^(s)=C(s)Xm,p(s)+
X·m,v(s)C(s)+s=C(s)Xm,p(s)+sXm,v(s)C(s)+s(550)


在图528中，式(549)中的低通滤波器和高通滤波器可分别表示为



F低(s)=C(s)C(s)+s

F高(s)=sC(s)+s(551)


将xm,p=x+ξp和xm,v=x+∫ξvdt代入式(550)，有


X^(s)=X(s)+C(s)Ξp(s)+Ξv(s)C(s)+s(552)


1. 一阶互补滤波

若图528中的C(s)为比例控制器，即C(s)=kp，则图528所示的互补滤波器为一阶互补滤波器。位置传感器噪声和速度传感器噪声的滤波器传递函数可分别表示为



Fp(s)=kpkp+s

Fv(s)=1kp+s
(553)


若令kp=10，则两者的对数频率特性曲线如图529所示。


图529一阶互补滤波Fp(s)和Fv(s)在kp=10时的Bode图


由A(Fp(j0))=1和A(Fv(j0))=1/kp可知，Fp(s)和Fv(s)在低频段的增益分别为0dB和-20log(kp)dBlog是以10为底的对数函数。。而Fp(s)和Fv(s)的转折频率均为1/kprad/s，且该转折频率是Fp(s)的截止频率。
由图529和以上分析可知，Fp(s)对小于截止频率1/kprad/s的噪声没有衰减作用，而Fv(s)不仅对大于1/kprad/s的噪声有-20dB/dec斜率的滚降衰减作用，而且对小于1/kprad/s的噪声有-20log(kp)dB的衰减作用。若kp增大，则Fp(s)的截止频率增大，Fp(s)的滤波效果变差，而kv(s)的低频段增益减小，Fv(s)的滤波效果变好。所以，kp的选择应该全面考虑位置传感器和速度传感器的噪声特点，折中选择该kp值的大小。

另外，Fp(s)滤波器不能减小位置传感器噪声中包含的非零常值偏差，而Fv(s)滤波器能够将速度传感器噪声中包含的非零常值偏差衰减-20log(kp)dB倍，但是由于Fv(s)低频段斜率为0dB/dec，所以Fv(s)无法完全消除速度传感器噪声中包含的非零常值偏差的影响。若要进一步提高互补滤波器的效果，可以采用二阶互补滤波。
2. 二阶互补滤波
若图528中的C(s)为比例积分控制器，即C(s)=kp+ki/s，则图528所示的互补滤波器为二阶互补滤波器，如图530所示。


图530二阶互补滤波器结构框图（一）


在图530中，位置传感器和速度传感器的滤波器传递函数可分别表示为



Fp(s)=kps+kis2+kps+ki

Fv(s)=ss2+kps+ki
(554)


从二阶滤波器的角度来看，式(554)也可以表示为



Fp(s)=2ζωns+ω2ns2+2ζωns+ω2n

Fv(s)=ss2+2ζωns+ω2n
(555)


式中ζ——阻尼比； 
ωn——自然振荡频率，rad/s。
式(554)和式(555)存在的等式关系为



kp=2ζωn


ki=ω2n
(556)


由式(555)还可以得到二阶互补滤波器动态结构图的另一种形式，即二阶互补滤波器的动态结构图也可以表示为
图531所示的形式。


图531二阶互补滤波器结构框图（二）

若令kp=10、ki=0.1(即ζ≈15.811,ωn=
0.1≈0.316)，则Fp(s)和Fv(s)的对数频率特性曲线如图532所示。


图532二阶互补滤波器Fp(s)和Fv(s)在kp=10、ki=0.1时的Bode图

位置滤波器Fp(s)中由于包含一阶微分环节2ζωns+ω2n，因此当ζ≥1/2时，Fp(s)的滤波特性基本和一阶低通滤波器相似，低频段增益约为0dB，高频段衰减的滚降速度为-20dB/dec，转折频率位于1/kprad/s附近。Fp(s)的对数幅频特性曲线的形状与阻尼比ζ相关。ζ越大，Fp(s)的对数幅频特性曲线的形状与一阶低通滤波的对数幅频特性曲线越相近； ζ越小，Fp(s)的对数幅频特性曲线中的谐振峰越大，当ζ=1/2时，Fp(s)的对数幅频特性曲线中的谐振峰约为3.3dB。

速度滤波器Fv(s)可以视为带通滤波器，通带频段的增益为-20log(kp)，通带频段的中心频率点约为ωn，当ki≤kp时，Fv(s)的两个转折频率分别为ki/kprad/s和kprad/s。

当ζ≥1/2时，二阶互补滤波Bode图（图532）和一阶互补滤波Bode图（图529）相比，二阶互补滤波器的速度滤波器Fv(s)在低频段增益的斜率为20dB，而一阶互补滤波器的速度滤波器Fv(s)在低频段增益的斜率为0dB。这就意味着二阶互补滤波器比一阶互补滤波器对速度传感器噪声在低频段具有更好的滤波特性，特别是一阶互补滤波只能削弱但不能完全消除速度传感器噪声中的非零常值偏差，而二阶互补滤波在理论上能够完全消除速度传感器噪声中的非零常值偏差。
5.5.8卡尔曼滤波
卡尔曼滤波是1960年由卡尔曼首先提出的一种滤波算法根据文献可知，实际上Peter Swerling在更早之前（1958年）就提出了一种类似的算法。。卡尔曼滤波器是一种纯粹的时域滤波器，不需在频域设计再转换到时域实现。

1. 最小方差估计
卡尔曼滤波是线性最小方差估计的递推实现形式。最小方差估计的最优估计准则是要求估计量
x^与被估计量x的误差x～=x-x^的均方误差
(Mean Squared Error，MSE)最小。误差x～的均方误差是x～协方差矩阵的迹，即



MSE=tr(E(x～x～T))

=tr(E((x-x^)
(x-x^)T))

=E(x-x^)T(x-x^)
(557)


因此，最小方差估计的最优估计准则可表示为


x^MMSE=
argmin
x^
(E((x-x^)T(x-x^)))
(558)


设x为随机向量，z为x的量测向量，即
z=Z(x)+�瘙
經，其中�瘙
經是随机误差，则x的最小方差估计x^就是x的条件均值，即


x^=E(x|z)
(559)


最小方差估计是x的无偏估计，即


E(x～)=E(x-x^)=0(560)


若x和z均服从正态分布，假设
x和z是联合高斯随机矢量，其均值分别为
mx和mz，其协方差矩阵为


P=PxxPxz

PzxPzz(561)


则最小方差估计和估计误差方差矩阵分别为


x^=E(x|z)= mx+PxzP-1zz(z-mx)(562)

E（x～
x～T
）=E（（x-x^）
（x-x^）T
）=
Pxx-Pxz
P-1zzPzx(563)


2. 卡尔曼滤波算法

应用最小方差估计式(562)和估计误差方差矩阵式(563)的计算公式直接进行计算是很困难的。首先，矩阵Pzz的求逆计算量很大； 其次，在获得k次测量后计算x^j|k时，需要用到k次全部的测量值z(1),z(2),…,z(k)，数据的存储量会随着时间的推移越来越大，而且也满足不了实时性的要求。卡尔曼滤波算法使用递推计算的方式解决了这两个困难，是便于计算机实现的最小方差估计算法。
卡尔曼滤波建立在线性代数和隐马尔可夫模型(Hidden Markov Model)基础上，其基本动态系统可以用一个马尔可夫链表示，该马尔可夫链建立在一个被高斯噪声(即正态分布的噪声)干扰的线性算子上，如图533所示。


图533卡尔曼滤波的系统模型

在图533中，“输入”部分和“测量”部分是可见部分，其他是隐藏部分。图533所示的线性离散系统可以表示为



xk=Φk,k-1xk-1+Bk-1uk-1+wk-1


zk=Hkxk+�瘙
經k
(564)


式中wk和vk——零均值、不相关的白噪声，两者的协方差矩阵是已知
的，分别为Qk和Rk，即



wk~N(0,Qk)



�瘙
經k~N(0,Rk)
(565)


由wk和�瘙
經k的定义可知，wk和�瘙
經k满足下列等式，即



E(wk)=0

E(�瘙
經k)=0

cov(wk,wj)=E(wkwTj)=Qkδk,j

cov(�瘙
經k,�瘙
經j)=E(�瘙
經kvTj)=
Rkδk,j

cov(wk,�瘙
經j)=E(wkvTj)=0


式中δk,j——Kroneckerδ函数，表示为



δk,j=1若k=j


0若k≠j(566)


状态向量xk的最小方差估计x^k是xk的条件期望值。如果是利用k时刻和k时刻之前的测量值{z1,z2,…,zk}估计xk，那么就得到一个后验估计，表示为x^k|k，即



x^k|k=E(
xk|{z1,z2,…,zk})(567)


如果是利用k时刻之前的测量值{z1,z2,…,zk-1}估计xk，那么就得到一个先验估计，表示为x^k|k-1，即


x^k|k-1=E(xk|{z1,z2,…,zk-1})
(568)


1) 状态预测方程
状态预测方程是计算先验估计x^k|k-1，若已知x^k-1|k-1，可利用式(564)估计xk，即


xk=Φk,k-1x^k-1|k-1+Bk-1uk-1+
wk-1(569)


对式(569)两端取均值，有


x^k|k-1=Φk,k-1x^k-1|k-1+Bk-1uk-1(570)


式(570)称为状态预测方程，x^k|k-1是xk的先验估计。
2) 误差协方差矩阵预测方程

误差协方差矩阵预测方程就是计算先验估计误差的协方差矩阵Pk|k-1，即cov(xk-x^k|k-1)。由式(564)和式(570)中的xk=Φk,k-1xk-1+Bk-1uk-1+wk-1和x^k|k-1=Φk,k-1x^k-1|k-1+Bk-1uk-1代入
协方差矩阵Pk|k-1中，并且x～和w相互独立，可以得到先验估计的误差协方差矩阵预测方程为


Pk|k-1=cov(xk-x^k|k-1)


=cov(Φk,k-1(xk-1-x^k-1|k-1)+wk-1)

=cov(Φk,k-1(x～k-1|k-1)+wk-1)

=Φk,k-1Pk-1|k-1Φk,k-1+ Qk-1(571)


状态预测方程和误差协方差矩阵预测方程也被称为时间更新方程或预测方程。
3) 状态估计方程
由式(562)可知最小方差估计可以表示为


x^k|k= x^k|k-1+Kk(zk-Hkx^k|k-1)
(572)


式中Kk——卡尔曼增益。
4) 误差协方差矩阵估计方程

误差协方差矩阵估计方程就是计算后验估计误差的协方差矩阵Pk|k，即cov(xk-x^k|k)。
将式(571)代入误差的协方差矩阵
Pk|k中，并且由于k-1时刻和之前时刻的测量值{z1,z2,…,zk-1}估计的x～k|k-1与k时刻的随机噪声�瘙
經k是无关的，可得


Pk|k=cov(xk-x^k|k)

=cov(xk-x^k|k-1-Kk(zk-Hkx^k|k-1))

=cov(x～k|k-1-Kk(Hkx～k|k-1+�瘙
經k))

=cov((I-KkHk)x～k|k-1-Kk
�瘙
經k)

=(I-KkHk)cov(x～k|k-1)(
I-KkHk)T+Kkcov(�瘙
經k)KTk

=(I-KkHk)Pk|k-1(
I-KkHk)T+ KkRkKTk
(573)


5) 卡尔曼增益方程
卡尔曼滤波满足最小方差估计的最优估计准则，即x～k|k的均方误差最小。



Kk=argminMSE(x～k|k)

=argmintr(Pk|k)
(574)


求式(574)的极值，可转换为


dtr(Pk|k)dKk=0
(575)


方程式(575)等号的左侧为


dtr(Pk|k)dKk=ddKktr((I-KkHk)Pk|k-1(I-KkHk)T+KkRkKTk)

=ddKktr(Pk|k-1-KkHkPk|k-1-Pk|k-1HTkKTk

+KkHkPk|k-1HTkKTk+KkRkKTk)

=ddKktr(Pk|k-1-KkHkPk|k-1-Pk|k-1HTkKTk

+Kk(HkPk|k-1HTk+Rk)KTk)

=-2(HkPk|k-1)T+2Kk(HkPk|k-1HTk+Rk)
(576)


因此，卡尔曼增益Kk为


Kk=(HkPk|k-1)T(
HkPk|k-1
HTk
+Rk)-1

= Pk|k-1HTk(HkPk|k-1HTk+Rk)-1
(577)


卡尔曼增益为式(577)的最优值时，误差协方差矩阵估计方程式(573)可以简化。式(573)展开后为



Pk|k= Pk|k-1-KkHkPk|k-1-Pk|k-1HTkKTk+Kk(HkPk|k-1HTk+Rk)KTk(578)


式(577)等式左右均右乘(HkPk|k-1HTk+Rk)KTk，可得


Kk(HkPk|k-1HTk+Rk)KTk= Pk|k-1HTkKTk(579)


故式(578)可化简为后验协方差矩阵的另一种形式，即


Pk|k=(I-KkHk)Pk|k-1(580)


式(580)和式(573)相比计算量更少，但是由于计算过程中计算误差的积累，式(580)可能会引起Pk|k的非负定性甚至对称性，所以实际使用中经常使用式(573)计算后验协方差。
5.5.9扩展卡尔曼滤波
5.5.8节介绍的卡尔曼滤波器适用于线性离散系统，在实际应用中还会经常遇到非线性系统的情况，包括连续时间的非线性系统、离散时间的非线性系统和混合非线性系统。混合非线性系统是指状态方程是非线性连续方程，而观测方程是离散型的，这是实际应用中最普遍的情况。

将卡尔曼滤波推广到非线性系统的方法之一就是扩展卡尔曼滤波(EKF)，扩展卡尔曼滤波有连续扩展卡尔曼滤波、混合扩展卡尔曼滤波和离散扩展卡尔曼滤波等3种形式。尽管实际应用中的系统大多是混合系统，但是扩展卡尔曼滤波需要在数字计算机中运行，而计算机就有可能没有足够的时间完成连续扩展卡尔曼滤波或混合扩展卡尔曼滤波中的积分运算，因此通常将连续的状态方程离散化后，再使用离散扩展卡尔曼滤波。
1. 混合非线性系统的离散化
混合非线性系统的状态方程为


x·(t)= fc(x(t),u(t),t)+w(t)(581)


混合非线性系统的观测方程为


zk=h(x(k),k)+�瘙
經k(582)


式中w(t)，�瘙
經k——零均值、不相关的白噪声，两者的协方差矩阵是已知的，分别为Q(t)和Rk，即



w(t)~N(0,Q(t))



�瘙
經k~N(0,Rk)
(583)


由w(t)和�瘙
經k的定义可知，w(t)和�瘙
經k满足下列等式，即



E(w(t))=0

E(�瘙
經k)=0

cov(w(t),w(τ))=E(w(t)w
(τ)T)=Q(t)δ(t-τ)

cov(�瘙
經k,�瘙
經j)=E(�瘙
經kvTj)= Rkδk,j

cov(w(t),�瘙
經j)=E(w(t)vTj)=
0



将式(581)所示的连续状态方程以采样周期T，使用前向差分的方法离散化，即


xk-xk-1T= fc(xk-1,uk-1,k-1)+w(k-1)(584)

xk= xk-1+Tfc(xk-1,uk-1,k-1)+Tw(k-1)(585)


令


fd(xk-1,uk-1,k-1)= xk-1+Tfc(xk-1,uk-1,k-1)(586)


则得到离散化状态方程为


xk=fd(xk-1,uk-1,k-1)+wk-1(587)


式中，



wk-1~N(0,Qk-1)


Qk-1=TQ(t)



2. 围绕最优状态的线性化
将离散非线性状态方程式(587)在xk-1=x^k-1|k-1点进行泰勒级数展开，有



xk= fd(x^k-1|k-1,uk-1,k-1)+fdx|x^k-1|k-1(xk-1-x^k-1|k-1)+wk-1
(588)


对式(588)两端取均值，有


x^k|k-1= fd(x^k-1|k-1,uk-1,k-1)(589)


令δxk=x～k|k-1=xk-x^k|k-1，可得


δxk=Φk,k-1δxk-1+wk-1(590)


式中，


Φk,k-1=fdx
x^k-1|k-1= f
d1x1fd1x2…fd1xn

fd2x1fd2x2…fd2xn

︙︙︙

fdnx1fdnx2…fdnxnx^k-1|k-1(591)


将离散非线性观测方程式(582)在xk=x^k|k-1点进行泰勒级数展开，有


zk=h(x^k|k-1,k)+hx|x^k|k-1(xk-x^k|k-1)+�瘙
經k(592)


令δzk=zk-h(x^k|k-1,k)，可得离散线性观测方程为


δzk= Hkδxk+�瘙
經k(593)


式中，


Hk=hxx^k|k-1= h1x1h1x2…h1xn

h2x1h2x2…h2xn

︙︙︙

hmx1hmx2…hmxnx^k|k-1(594)


这种线性化方法称为小偏差法线性化，所得的线性化方程式(590)和式(593)称为“线性干扰方程”或“小偏差方程”
或“摄动方程”。
3. 线性干扰方程的卡尔曼滤波
在式(590)和式(593)的线性干扰方程基础上，仿照线性卡尔曼滤波方程，可以得到关于偏差δxk的卡尔曼滤波方程
，即


δx^k|k-1=Φk,k-1δx^k-1|k-1
(595)

Pk|k-1=Φk,k-1Pk-1|k-1Φk,k-1+Qk-1(596)

δx^k|k=δx^k|k-1+Kk(δzk-Hkδx^k|k-1)(597)

Pk|k=(I-KkHk)Pk|k-1(I-KkHk)T+KkRkKTk(598)

Kk= Pk|k-1HTk(HkPk|k-1HTk+Rk)-1(599)


4. 离散扩展卡尔曼滤波

离散非线性状态方程式(587)是以xk-1=x^k-1|k-1为工作点进行最小偏差线性化的，因此


δx^k-1|k-1=δxk-1|k-1= xk-1-x^k-1|k-1=0(5100)


故式(595)和式(597)可表示为


δx^k|k-1=
0(5101)

δx^k|k= Kkδzk= Kk(zk-h(x^k|k-1,k))(5102)


由δxk的定义可知


x^k|k= x^k|k-1+δx^k|k(5103)


故


x^k|k= x^k|k-1+Kkδzk= Kk(zk-h(x^k|k-1,k))(5104)



由线性干扰方程的卡尔曼滤波方程组、式(589)和式(5104)等，可以得到离散扩展卡尔曼滤波为



x^k|k-1= fd(x^k-1|k-1,uk-1,k-1)

x^k|k= x^k|k-1+Kk(zk-h(x^k|k-1,k))

Pk|k-1=Φk,k-1Pk-1|k-1Φk,k-1+Qk-1

Pk|k=(I-KkHk)Pk|k-1(I-KkHk)T+ KkRkKTk

Kk= Pk|k-1HTk
(HkPk|k-1
HTk+Rk)-1(5105)


5.6Paparazzi的四旋翼无人机控制系统
由4.9节可知，四旋翼无人机是一种欠驱动MIMO系统，其水平方向的线运动不能由控制量直接控制，需要由四旋翼无人机的转动间接实现对水平方向的线运动的控制。
因此对四旋翼无人机的控制通常由两个回路实现： 内回路实现角度控制； 外回路实现水平线运动的控制。其控制系统示意图如图534所示。


图534Paparazzi四旋翼无人机控制系统示意图


图534中，水平方向控制器实现了对四旋翼水平方向位置、速度和加速度的控制，控制器的输出是四旋翼的姿态设定值； 姿态控制器实现了对四旋翼姿态角和角速率的控制，控制器的输出是执行元件(电子调速器、电机和螺旋桨等)的输入； 垂直方向控制器实现了对四旋翼垂直方向高度、速度和加速度的控制，控制器的输出是执行元件的输入。
图534中，航姿参考系统根据IMU模块的原始数据解算得到四旋翼的姿态角和角速率(角速率在图534中未画出)，属于控制系统的反馈元件。
图534中，组合导航系统根据IMU模块、GPS、气压高度计、超声波高度计、光流传感器和激光高度计等传感器的原始数据解算得到四旋翼的位置、速度和加速度(速度和加速度在图534中未画出)，也属于控制系统的反馈元件。
另外，四旋翼中线运动的控制量需要转换为机体坐标系中的控制量，进而经过混控的转换后生成对电机的控制量。
小结
本章主要介绍了PID控制器、复合控制、前置滤波器和数字滤波算法等在Paparazzi中应用较多的控制理论知识。
了解完整的控制理论知识需要参考其他控制理论的书籍。
本章的知识要点包括： 
 控制系统按照控制方式区分，可以分为开环控制系统、闭环控制系统和复合控制系统。
 控制系统的性能，主要是包括稳定性、动态特性和稳态特性，另外，还有抗干扰性、鲁棒性等。
 PID控制器时域中微分方程的表达式由式(58)或式(59)描述，即



u(t)=Kpe(t)+Ki∫t0e(τ)dτ+Kdde(t)dt

u(t)=Kpe(t)+1Ti∫t0e(τ)dτ+Tdde(t)dt



 PID控制器的改进主要是对其中积分控制器和微分控制器的改进。
 复合控制是将前馈控制和反馈控制有机结合在一起的控制方式。
 前置滤波器可以平衡稳态性能、抗干扰能力等性能指标对反馈控制器的要求。前置滤波器可以视为二自由度控制器的一种形式，称为目标值滤波器，在自抗扰中称为安排过渡过程。
 互补滤波可以应用在航姿参考系统解算算法中。
 互补滤波的原理由式(551)描述，即



F低(s)=C(s)C(s)+s

F高(s)=sC(s)+s



 卡尔曼滤波由5个方程描述。
状态预测方程式(570)，即


x^k|k-1=Φk,k-1x^k-1|k-1+Bk-1uk-1


误差协方差矩阵预测方程式(571)，即


Pk|k-1=Φk,k-1Pk-1|k-1Φk,k-1+Qk-1


状态估计方程式(572)，即


x^k|k= x^k|k-1+Kk(zk-Hkx^k|k-1)


误差协方差矩阵估计方程式(573)或式(580)，即


Pk|k=(I-KkHk)Pk|k-1(I-KkHk)T+KkRkKTk

Pk|k=(I-KkHk)Pk|k-1


卡尔曼增益方程式(577)，即


Kk= Pk|k-1HTk(HkPk|k-1HTk+Rk)-1


 离散扩展卡尔曼滤波由式(5105)描述，即



x^k|k-1= fd(x^k-1|k-1,uk-1,k-1)

x^k|k= x^k|k-1+Kk(zk-h(x^k|k-1,k))

Pk|k-1=Φk,k-1Pk-1|k-1Φk,k-1+ Qk-1

Pk|k=(I-KkHk)Pk|k-1(I-KkHk)T+ KkRkKTk

Kk= Pk|k-1HTk
(HkPk|k-1+ Rk)-1



 离散扩展卡尔曼滤波可以应用在航姿参考系统和组合导航系统中。