姿态解算（一）飞控控制方法学习

飞控控制方法学习

---

“飞控”是指无人机的飞行控制器，用于自动化保持飞行器处于一个特定的状态（悬停、飞行等）。由于无人机经常处于“超视距”的环境飞行，所以自动化控制的飞控对于无人机来说是不能缺少的。好的飞控，还会搭配不少有用的功能，方便控制者进行复杂的运动。多轴飞行器的飞控主要由主控器、姿态感应器、GPS天线以及电子罗盘组成，周边还会有电源控制模块、LED提示灯、蓝牙连接模块等等。

一个旋翼类无人机系统的算法主要有两类：姿态检测算法、姿态控制算法。姿态控制、被控对象（即四旋翼无人机）、姿态检测三个部分构成一个闭环控制系统。被控对象的模型是由其物理系统决定，我们设计无人机的算法就是设计姿态控制算法、姿态检测算法。

---

姿态检测算法

---

姿态的三个自由度可以用欧拉角表示，也可以用四元数表示。姿态检测算法的作用就是将加速度计、陀螺仪等传感器的测量值解算成姿态，进而作为系统的反馈量。常用的姿态检测算法有卡尔曼滤波、互补滤波等。

---

姿态控制算法

---

控制飞行器姿态的三个自由度，以给定姿态与姿态检测算法得出的姿态偏差作为输入，被控对象模型的输入量作为输出（例如姿态增量），从而达到控制飞行器姿态的作用。最常用的就是PID控制及其各种PID扩展（分段、模糊等）、高端点的有自适应控制。当然，姿态控制算法里面又常用角速度、角度双闭环控制，所以常常有PID外环+PID内环等等，这些细节就不说了。

---

PID算法

---

对于过程控制的典型对象-“一阶滞后+纯滞后”与“二阶滞后+纯滞后”的控制对象，PID控制器是一种最佳控制。

PID调节规律是连续系统动态品质校正的一种有效方法。

PID调节器类型：1。比例调节器 2.比例积分调节器 3.比例微分调节器 4.比例积分微分调节器

a.比例调节器微分方程：Y=KpE(t) Y为调节器输出；Kp为比例系数；E（t)为调节器输入偏差。
调节器的输出与输出偏差成正比，偏差出现，就能即使产生与之成比例的调节作用，具有调节及时的特点。

b.比例积分调节器：
积分作用是指调节器的输出与输入偏差的积分比例的作用。方程为：
Y=(1/T1)∫e(t)dt T1为积分时间常数，表示积分速度的大小，T1越大，积分速度越慢，积分作用越弱

将比例和积分两种作用结合起来，就构成PI调节器，规律为：Y=Kp【E(t)+(1/T1)∫e(t)dt]

c.比例微分调节器：
方程为:Y=Td(de(t)/dt)

d.比例积分微分调节器：
PID微分方程为：Y=Kp[e(t)+(1/T1)∫e(t)dt+Td(de(t)/dt)]

模拟PID控制系统原理图链接：http://pan.baidu.com/s/1kUvjMGB

模拟PID控制器的控制规律为：u(t)=Kp[e(t)+(1/T1)∫e(t)dt+Td(de(t)/dt)]+u0

u(t)–调节器的输出信号；e(t)–调节控制器的偏差信号，等于给定值r(t)与测量值y(t)之差；KP–比例积分；Td–微分时间；u0–控制变量；KP/T1–积分系数；Kp/Td–微分时间；

比例环节：对偏差瞬间做出快速反应。偏差一旦产生，控制器立即产生控制作用，是控制量向减少偏差的反向变化。控制作用的强弱取决于比例系数Kp,Kp越大，控制越强，但过大的Kp会导致系统震荡，破坏系统的稳定性。

积分环节：把偏差的积累作为输出。在控制过程中，只要有偏差存在，积分环节的输出就会不断增大。直到偏差e(t)=0,在输出的u(t)才可能为此在某一常量，使系统在给定值r（t)不变的条件下趋于稳态，但也会降低系统的响应速度，增加系统的超调量。积分常数T1越大，积分的积累作用越弱。增大积分常数T1会减慢静态误差的消除过程，但可以减少超调量，提高系统的稳定性。

微分环节：阻止偏差的变化。根据偏差的变化趋势（变化速度)进行控制。偏差变化控制的越快，微分控制器的输出越大，并能在偏差值变大之前进行修正。微分作用的引入，将有助于减小超调量，克服震荡，使系统趋于稳定。但微分的作用对于输入信号的噪声很敏感，对噪音大的系统一般不用微分，活在微分起作用之前现对输入信号进行滤波。

PID控制算式的数字化

公式：u（t)=Kp[e(t)+(1/T1）∫(上限t，下限0)e(t)d(t)+Td(de(t)/dt)]+u0

中的积分和微分项不能直接使用，必须进行离散化处理，离散化的方法：以T作为采样周期，K作为采样序号，则离散采样时间KT对应着连续时间t，用求和的形式代替积分，用增量的形式代替微分。

凑试法确定PID的参数：

增大比例系数P将加快系统的响应，在有静差的情况下有利于减小静差，过大的比例系数使系统超调，产生振荡，稳定性变坏。

增大积分时间I有利于减小超调，减小振荡，使系统的稳定性增加，但系统静差消除时间变长。

增大微分时间D有利于加快系统的响应速度，使系统超调量减小，稳定性增加，但对扰动的抑制能力减弱。

微分是误差的变化率，具有预见性，因此能产生超前的控制作用，在偏差为形成之前，已被微分调节作用消除。

##飞控PID取值方法（姿态控制算法） ##

P-I-D：比利－积分－微分
PID参数选定规则：

整定参数最佳，从小到大逐渐查；

先调比例后积分，微分作用最后加；

曲线震荡很频繁，比例刻度要放大；

曲线漂浮浮动大，比例额课都要拉小；

曲线偏离回复差，积分时间往小降；

曲线波动周期长，积分时间要加长；

曲线震荡动作繁，微分时间要加长。

首次起飞前PID的基本调整

请先把PID设置成当前程序版本的初始值。
用手小心抓着飞行器，增大油门直到接近悬停的位置，试着把它向前后左右倾斜，你会感觉到飞行器会产生一个反作用力，压制人为造成的倾斜。
改变P值（ROLL/PITCH）的大小，直到飞行器变得难以随意倾斜。如果没有打开自稳功能，飞控会允许你在一段时间内倾斜飞行器，这是正常情况。
现在尝试前后左右摇动飞行器，增大P值（ROLL/PITCH）直到它自己开始出现抖动，再往回减小一点。
重复操作把YAW的P也调整好。
现在已经适合在飞行中调整了。
高级调整，了解P、I和D的实际作用

P – 纠正飞行器回到初始位置的力度大小。
这个修正力度是一个比例值，反映初始位置偏差值减去飞控输入控制方向变化的组合。 较高的P值会产生较强的修正力去抵抗飞行器位置的偏移。
如果P值太高，在飞行器返回初始位置的过程中会过冲然后再次往反方向修正，接着再次进行回到初始位置的补偿。这会导致飞行器出现持续振荡，严重的话会完全破坏平衡。

增大P值：
飞行器会逐渐变得稳定，但P值太高会造成振荡和失控。
18

需要注意的是，提高P值会产生一个更强的修正力度，阻止飞行器的偏移。
减小P值：
飞行器会逐渐开始偏移，但P值太低会让飞行器变得非常不稳定。 它会让阻止飞行器偏移的修正力度变弱。
特技飞行：需要的P值稍高。 普通飞行：需要的P值稍低。

I – 对角度变化进行采样与取平均值计算的时间周期
存在偏差时，返回到初始位置有个修正的过程，在修正过程中力度会越来越大，直到达到最大值。 较高的I值会增强稳定的效果。
增大I值：
减小漂移和提升稳定效果，但过大的I值会延长稳定过程的周期，同时也会降低P的效果。
减小I值：
增强反应速度，但会增加漂移和降低稳定效果，同时会提升P的效果。
特技飞行：需要的I值稍低。 普通飞行：需要的I值稍高。

D – 飞行器回到初始位置的速度
较高的D值（数值与效果相反，较高的D值参数上会更接近0）意味着飞行器会快速返到回初始位置。
增大D值：（请记住，增大效果等于减小设定的参数值）
提升修正的速度，也会提升产生修正过量与振荡的几率，同时也会提升P的效果。
减小D值：（请记住，减小效果等于增大设定的参数值）
降低由偏差状态返回初始位置的振荡，恢复到稳定效果的时间变长，同时也会降低P的效果。
特技飞行：增大D值（请注意是减小参数值）。 普通飞行：减小的D值（请注意是增大参数值）。
实际应用中的高级调整

（下面这些只是建议）
特技飞行：
增大P值直到出现振荡，然后稍微减小一点。
改变I值直到悬停漂移达到无法接受的程度，然后稍微增大一点。
增大D值（请注意是减小参数值）直到出现大动作控制时无法接受的振荡。 P值现在可以稍微减小一点。
稳定飞行（常规/FPV）：
19 增大P值直到出现振荡，然后稍微减小一点。

改变I值直到偏差程度达到无法接受的程度，然后稍微增大一点。
减小D值（请注意是增大参数值）直到出现大动作控制时回中速度变慢，然后稍微增大一点D值（请注意是减小参数值）。
P值现在可以稍微减小一点。

在一般的飞行中，你会找到一组折中的PID值，这是最优化的效果。

---

互补滤波方法（姿态检测算法）

---

加速度计对小车的加速度比较敏感，取瞬时值计算倾角误差比较大；而陀螺仪积分得到的角度不受小车加速度的影响，但是随着时间的增加积分漂移和温度漂移带来的误差比较大。所以这两个传感器正好可以弥补相互的缺点。

互补滤波就是在短时间内采用陀螺仪得到的角度做为最优，定时对加速度采样来的角度进行取平均值来校正陀螺仪的得到的角度。简言之，短时间内用陀螺仪比较准确，以它为主；长时间用加速度计比较准确，这时候加大它的比重，这就是互补了。

加速度计要滤掉高频信号，陀螺仪要滤掉低频信号。

得到陀螺仪的角速度就可以使用四元数或欧拉法求解姿态角，然后通过加速度计的三轴加速度值就可以对姿态角进行融合，这时的姿态角就基本可用了，然后就可以做你想做的事了。

互补滤波器就是根据传感器特性不同，通过不同的滤波器（高通或低通，互补的），然后再相加得到整个频带的信号。例如，加速度计测倾角，其动态响应较慢，在高频时信号不可用，所以可通过低通抑制高频；陀螺响应快，积分后可测倾角，不过由于零漂等，在低频段信号不好。通过高通滤波可抑制低频噪声。将两者结合，就将陀螺和加表的优点融合起来，得到在高频和低频都较好的信号。互补滤波需要选择切换的频率点，即高通和低通的频率

---

卡尔曼滤波取值方法（姿态检测算法）

---

卡尔曼滤波不要求保存过去的测量数据，当新的数据到来时，根据新的数据和前
一时刻的诸量的估值，借助于系统本身的状态转移方程，按照一套递推公式，即可算
出新的诸量的估值。这一点说明卡尔曼滤波器属于IIR滤波器范畴。这就是说，与维
纳滤波器不同，卡尔曼滤波器能够利用先前的运算结果，再从当前数据提供的最新信
息，即可得到当前的估值。卡尔曼递推算法大大减少了滤波装置的存储量和计算量，
并且突破了平稳随机过程的限制，使卡尔曼滤波器适用于对时变信号的实时处理。
卡尔曼滤波的一些参数取值方法:
(1) 初始的状态变量X0影响最小，可以直接取值为第一个测量值，在滤波可以收敛的情况下会很快收敛;
(2) 只要不为零，初始协方差矩阵P0的取值对滤波效果影响很小，都能很快收敛，可以任意取一个不为零的矩阵;
(3) 当状态转换过程为已确定时，Q的取值越小越好，可以使用一个非常小但不为零的矩阵;
(4) 测量噪声协方差R取值取值越小收敛越快，但滤波效果不一定好，可以在滤波前先测定噪声协方差，然后用于后续的滤波。

---

欧拉角

---

一个坐标系到另一个坐标系的变换，可以通过绕不同坐标轴的3次连续转动来实现。从参考系到新的坐标系的变换可以表示：

绕参考坐标系的z轴转动 ψ 角

绕新坐标系的y轴转动 θ 角

绕新坐标系的x轴转动 φ 角

ψ、θ、φ称为欧拉转动角

---

四元数

---

四元数姿态表达式是一个四参数的表达式。它的基本思路是：一个坐标系到另一个坐标系的变换可以通过绕一个定义在参考系的矢量 μ 的单次转动来实现。

四元数用符号q表示，它是一个具有4个元素的矢量，这些元素是该矢量方向和转动大小的函数。

q=[a,b,c,d]=[cos(μ/2),(μx/μ)sin(μ/2),(μy/μ)sin(μ/2),(μz/μ)sin(μ/2)]

μx、μy、μz是角矢量μ的分量，μ是其大小。

---