[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 文件夾下。
