[TOC]
# 1. 位置式PID控制器
PX4 的控制一共分为两部分，姿态控制和位置控制。姿态控制是让飞 机由现在的姿态到达期望的姿态，姿态方面源码中更多以四元数 q 和旋转矩阵 DCM 表示。位置控制即如何由现在的位置达到期望的位置，每个控制都是串 级 PID，而位置控制又相当于姿态控制的外环。
PID算法发展至今已经衍生出了许多种不同的PID算法，而PX4所使用的的PID算法为**位置式PID算法**。
位置式PID控制器由最基本的PID算法进行**离散化**得到，进行离散化的目的是：方便将数学表达式编写成代码，运行在处理器上。下面是离散化的过程：
**（1）由式（3-1）得到：**
```[tex]
u(t)=K_{p}(e(t)+K_{i}\int_{0}^{t}e(t)dt+K_{d}\frac{d}{dt}e(t))
```
**（2）将积分环节和微分环节离散化：**
* t 时刻的偏差为：`$ e(t)= rin(t)-rout(t) $`，`$ rin(t) $`为输入量，`$ rout(t) $`为输出量。
* 将积分环节离散化得到：`$ e(t)+e(t+1)+...... $`。
* 将微分环节离散化得到：`$ e(t)-e(t-1) $`。
**（3）离散化后得到的位置式PID控制器如下：**
```[tex]
u(t)=K_{p}(e(t)+[K_{i}\sum_{j=0}e(t+j)]+K_{d}(e(t)-e(t-1)))
```
# 2. PID控制器在多旋翼上的应用
PX4使用的位置式PID控制器分为内环控制器与外环控制器，外环控制器只用到PI，而内环控制器用到PID。当控制姿态时：内环控制器主要负责调整角速度；外环控制器主要负责调整角度；当控制定高时：高度则作为外环，z轴上的加速度作为内环。最终PID系统会输出油门值，油门给定电子调速度器值，电子调速度控制电机使空间三轴的欧拉角和高度变化。如下图：
# 3. 位置式PID控制器的C代码
在PX4 1.10.1版本中，位置式PID控制器使用抗积分饱法来防止**积分饱和**，所谓的积分饱和是指：如果系统存在一个方向的偏差，PID 控制器的输出由于积分作用的不断累加而加大，从而导致执行机构达到极限位置，若控制器输出 `$ u(t) $`继续增大，执行器开度不可能再增大，此时计算机输出控制量超出了正常运行范围而进入饱和区。一旦系统出现反向偏差，`$ u(t) $`逐渐从饱和区退出。进入饱和区越深则退出饱和区时间越长。在这段时间里，执行机构仍然停留在极限位置不是立即做出相应的改变，这时系统就像失控一样，造成控制性能恶化，这种现象称为积分饱和现象或积分失控现象。如下图所示：
<br/>
防止积分饱和的方法之一就是抗积分饱和法，该方法的思路是在计算`$ u(t) $`时，首先判断上一时刻的控制量`$ u(t-1) $`是否已超出极限范围。如果`$ u(t-1)>umax $`，则累加**负偏差**；如果`$ u(t-1)<umin $`则累加**正偏差**从而避免控制量长时间停留在饱和区，用C语言表示下（这不是PX4官方的代码）。
```
#include <stdio.h>
#include <math.h>
// 第一步：将PID用到的参数定义在一个结构体中，方便调用
struct _pid {
float SetSpeed; // 目标值
float ActualSpeed; // 实际值，即u(t)
float umax; // u(t)的上限
float umin; // u(t)的下限
float err; // 偏差值
float err_last; // 上一步的偏差值
float kp, ki, kd; // 比例系数、积分系数、微分系数
float voltage; // 电压值（控制执行器的变量）
float integral; // 积分值
}pid;
// 第二步：初始化变量
void pid_init() {
printf("pid_init begin \n");
pid.SetSpeed = 0.0;
pid.ActualSpeed = 0.0;
pid.umax = 500;
pid.umin = 10;
pid.err = 0.0;
pid.err_last = 0.0;
pid.integral = 0.0;
pid.kp = 0.2;
pid.ki = 0.015;
pid.kd = 0.2;
printf("pid_init end\n");
}
// 第三步：编写控制算法，抗积分饱和
float pid_realize(float speed) {
int index;
pid.SetSpeed = speed;
pid.err = pid.SetSpeed - pid.ActualSpeed; // 计算当前偏差
if (pid.ActualSpeed > pid.umax) {
if (abs(pid.err) > 200) {
index = 0;
}else {
index = 1;
if (pid.err < 0) {
// u(t)超出上限，累加负偏差或什么都不做，避免进入饱和区
pid.integral += pid.err;
}
}
}else if(pid.ActualSpeed < pid.umin) {
if (abs(pid.err) > 200) {
index = 0;
}else {
index = 1;
if (pid.err > 0) {
// 如果u(t)超出下限，累加正偏差或什么都不做，避免进入饱和区
pid.integral += pid.err;
}
}
}else {
if (abs(pid.err) > 200) {
index = 0;
}else {
index = 1;
pid.integral += pid.err;
}
}
pid.voltage = pid.kp * pid.err + index * pid.ki * pid.integral + pid.kd*(pid.err-pid.err_last);
pid.err_last = pid.err;
pid.ActualSpeed = pid.voltage * 1.0;
return pid.ActualSpeed;
}
// 测试代码
int main(){
printf("System begin \n");
pid_init();
int count = 0;
while(count<1000)
{
float speed = pid_realize(200.0);
printf("%f\n",speed);
count++;
}
return 0;
}
```
对于PX4 1.10.1 版本PID控制器代码位于 Fimware > src > lib > pid 文件夹下。
