无人机线性控制系统PID控制算法
PID线性控制
比例微分积分线性控制，即PID线性控制理论，是经典控制理论中线性控制系统甚至一些非线性控制系统中最常见的控制方法。
PID控制系统具有简单，实用，响应快，适应性强等特点，对于被控制对象，往往不需要了解控制模型和响应模型等具体的被控规律，仅使用系统输出的反馈来实现精确控制的规律，因为它的控制与人工手动控制的规律相同。 什么是PID？
它分成三个部分，P代表比例控制，I代表积分控制，D代表微分控制。PID控制器的出现甚至早于经典控制理论的发展。现代控制理论是建立在状态空间基础上的模型控制理论，在现代控制理论中，对控制系统的分析和设计主要通过对系统的状态建模和状态的描述来进行，因此现代控制理论所采用的控制方法都是应用于各不相同的具体的控制对象，能够更为精确精准的对不同的系统进行定制化设计。 PID控制器的问题：
与现代控制理论相比，它们是两种完全不同的控制方法。PID是控制领域的三种对时间域的三维元素（当前，历史和未来），将三者优化组合在一起完成控制目标，而现代控制则是基于针对某种优化指标，通过设计算法来实现控制的目标PID控制器的最大问题是没有一个明确的系统函数及优化准则，因此PID控制器的调参只能通过经验测试调整，并且它只能将系统调整到某种可接受的近似状态，且并不确定是否能调整到有更优的控制器。与现代控制理论相比，它们是两种完全不同的控制方法。PID是控制领域的三种对时间域的三维元素（当前，历史和未来），将三者优化组合在一起完成控制目标，而现代控制则是基于针对某种优化指标，通过设计算法来实现控制的目标。 比例控制
当操作人员控制一个系统要求系统的输出达到某个值时给予一定的输入值，输出值逐渐接近直到达到指定的输出值，这个过程中输出值与指定输出值之间的差可以作为输入值的参考控制量，并进行一定的比例调整，这种调整成为比例控制。
例如操作人员手动控制加热器的温度，加热器带有温度传感器作为输出值的表真，在控制过程中，操作人员可以通过视觉直接读取温度传感器的反馈量，并与给定值的比较计算误差，然后通过旋钮操控控制输入量调节加热器的电源输入，使得温度接近并保持给定值的附近。
这种闭环系统中各个环节都有可能存在各种各样的延迟等作用。例如调节输入后，温度上升到新的稳态值时需要一定的时间。由于延迟因素存在，调节输入控制量并不能马上看到调节的效果，因此闭环控制系统调节比较困难的一个原因是系统延迟的效应。比例控制的系数太小，则相同的误差得到的输
入控制量较小，系统输出量变化较慢，逼近指定的输出量的速度也比较慢，调节力度偏弱，调节所需的时间较长。
积分控制
往往有些系统存在长时间偏差，而比例控制项所计算得到的输入控制量无法修正该偏差，这时候就需要通过对历史误差量的累积分析来辨别并控制长时间的累积误差。这就是积分控制。
由于一般在数字处理系统中PID控制程序是周期性运算执行的，每次控制算法执行一个周期时，积分控制项累加当前的误差值，因此当输出量与指定输出量完全相等时，积分项为0。积分项有消除稳态误差的作用，可以提高控制精度。
由于积分项是将过去的历史误差信息累积起来，具有比较大的滞后效果，对于系统本身的稳定性会有影响，因此如果积分项的系数设置不合理，它的影响很难被迅速修正，导致系统响应的延迟，因此通常情况下都是将响应实时的比例控制项与积分控制项结合起来使用。 微分控制
微分控制项对应的是实际输出值与指定输出值之间误差的微分项与微分系数的乘积，也就是对应了误差项的变化率，误差变化率越快，微分绝对值越大，误差增大时，微分的符号为正，误差减小时，微分的符号为负。
微分控制项相当于对输出量的二阶预测，并针对预测量提前进行调整，例如有经验的操作人员在调节加热器时能够感知温度上升或下降太快，但还未达到设定值时，预感到按照当前的输入量有可能出现输出超调或者过冲的现象，于是提前减小控制输入量。因此微分控制项具有对输出控制的阻尼作用，或者具有提前预测的作用。 PID线性控制
当飞行器正常飞行或者悬停时，突遇外力(风等)或磁场干扰，使加速度传感器或磁力传感器采集数据失真，造成姿态解算出来的欧拉角错误，只用角度单环情况下，使系统很难稳定运行。
为了实现无人飞行器的稳定控制飞行，“光标”飞控采用了两级PID闭环串联控制的方式，即外环PID和内环PID。 内环PID
其中角速度控制环作为内环，角速度由陀螺仪采集数据输出，采集值一般不存在受外界影响情况，抗干扰能力强，并且角速度变化灵敏，当受外界干扰时，回复迅速；同理，高度环中气压传感器同样也会受到外界干扰，引入z轴加速度环可有效避免外界干扰造成的影响，增强了系统的鲁棒性。 外环PID
外环PID采用遥控输入信号作为目标变化角度，与姿态解算任务解算出的当前姿态角得到误差角，外环误差角速度为第二级角速度控制器的输入进行内环PID控制。
遥控输入 + - 机体姿态 角度误差 外环角度PI pi_outloop 内环速率PID pid_rate ESC 控制输出 三轴速率 IMU 姿态解算 传感器 内环 外环
PID控制算法位置式数字PID控制
u(t)?kpe(t)?ki?0e(t)dt?ktde(t)ddt
在将积分量，微分量离散化得到PID计算公式： 、
e(t?1)u(t)?kpe(t)?ki?e(j)T?kde(t)?Tj?0t
基于以上公式，姿态PID控制算法如式 AnglePIDOut(t)
e(t?1)?kpe(t)?ki?e(j)T?kde(t)?Tj?0tte?(t?1)AngleRatePIDOut(t)?kp?e?(t)?ki??e?(j)T?kd?e?(t)?Tj?0AngelPIDOut(t)为角度环PID输出 AngelRatePIDOut(t)为角速度环PID输出
e(t)=期望角度一实际角度，e＇(t)=AngelPIDOut(t) - 实际角速度 外环PID实现
在“光标”飞控中，由于采用了实时操作系统来编排算法时序，操作系统可以确保内外环运行的准确时序，并且内环与外环由于执行的特性，它的运行频率不同，因此采用了不同的实时任务来分别实现内环与外环控制算法。 这样可以确保内外环的PID迭代循环不被其他低优先级的任务和循环所打断，且能够达到既定的控制频率。
在双环级联PID系统中，外环PID作为控制的输入稳定环，主要负责对输入环节进行稳定控制，并提供内环的输入信号。 以下为外环PID详细结构图
内环PID实现
在双环级联PID系统中，内环作为控制的速率稳定环，它主要负责稳定控制角速率的输出。
以下为内环PID的详细结构图
移动平滑滤波
由于飞行器电机马达和桨叶挥舞产生的机体振动较大，频率较高，MEMS传感器的原始数据会受到该振动的影响，从而输出带有高频噪声的原始数据。这种高频噪声对于后续的姿态解算以及飞控算法都是有一定的损害的，因此必须将其滤除。
一个比较常用的简单滤波方法是平滑滤波。带有噪声的传感器原始信号在时域内所带有的短时间跳变的起伏不平的属性在频域范围内代表了高频成分，上升和下降的速度越快，则表示频率越高，那种不随时间变化或者随时间缓慢变化的部分则是频率较低的信号，所谓平滑滤波就是指使这些不平滑的高频成分变得平滑，使得变化没有原来那么剧烈，整个信号更能反映原信号低频分量的成分。因此平滑滤波实际上是一种低通滤波器，和其他滤波器不同的是，它是一种从时域方面进行设计的滤波算法，属于低频增强的时域滤波技术，有的图像处理技术也通过空间域平滑滤波的算法进行降噪处理的应用。
平滑滤波一般采用时域或空间域的简单平均法的方式实现，也就是求时域或空间域向邻近的采样点的数值求平均来去除突变的数据。因此邻近采样点的数值大小偏差直接影响平滑滤波的效果，邻域的数值相差越大，则平滑效果越好，但高频信息的损失也越大。
无人机线性控制系统PID控制算法
