关于无人机上层控制的PID算法的思考

一、前言

背景介绍：PID虽然出现了很多年，但是目前工业界还是把PID作为主流的控制算法（尽管学术界有很多非常时尚的控制算法，包括鲁邦控制，神经网络控制等等），PID的算法在于其不需要对系统进行复杂的建模，就可以完成比较好的控制效果。
PID算法的优势在于其非常简单，很多时候算法好不好取决于对参数的设置，所以很多时候，PID算法其实是一个体力活，主要精力都在于寻找最优参数去了。当然，对于开环系统就不用说这种PID算法了
PID算法只适合于闭合系统。对于非线性系统，时滞比较严重的系统个人也不建议使用PID，这种情况控制效果不怎么好。

二、算法介绍

• 开环控制系统
  
  在开环控制系统中，系统输出只受输入的控制，控制精度和抑制干扰的特性都比较差。就没有什么误差控制输入的说法，就不谈什么PID了

• 闭环控制系统
  

PID简介：PID控制应该算是应用非常广泛的控制算法了。小到控制一个元件的温度，大到控制无人机的飞行姿态和飞行速度等等，都可以使用PID控制。这里我们从原理上来理解PID控制。
PID(proportion integration differentiation)其实就是指比例，积分，微分控制。

公式描述如下：

其中U(t)是PID控制器的输出，PID控制器的输出是执行器的输入，如果我们知道执行器的模型，就可以实行精确控制。
这里我们引用知乎上的作者例子知乎网址为了简单说明情况和好理解，知乎这边文章里面把控制器和执行器进行了等价，就是控制器输出等于执行器输出。

2.1、P控制算法

  我们先说PID中最简单的比例控制，抛开其他两个不谈。还是用一个经典的例子吧。假设我有一个水缸，最终的控制目的是要保证水缸里的水位永远的维持在1米的高度。假设初始时刻，水缸里的水位是0.2米，那么当前时刻的水位和目标水位之间是存在一个误差的error，且error为0.8.这个时候，假设旁边站着一个人，这个人通过往缸里加水的方式来控制水位。如果单纯的用比例控制算法，就是指加入的水量u和误差error是成正比的。即
  u=kp*error  设kp=0.5，
  那么t=1时（表示第1次加水，也就是第一次对系统施加控制），那么u=0.50.8=0.4，所以这一次加入的水量会使水位在0.2的基础上上升0.4，达到0.6.
  接着，t=2时刻（第2次施加控制），当前水位是0.6，所以error是0.4。u=0.50.4=0.2，会使水位再次上升0.2，达到0.8.
如此这么循环下去，就是比例控制算法的运行方法。
可以看到，最终水位会达到我们需要的1米。（无限下去数学上面就是一个逼近1m的求和级数，但是是永远达不到准确的1m的）
  但是，单单的比例控制存在着一些不足，其中一点就是 –稳态误差。
像上述的例子，根据kp取值不同，系统最后都会达到1米，只不过kp大了到达的快，kp小了到达的慢一些。不会有稳态误差。但是，考虑另外一种情况，假设这个水缸在加水的过程中，存在漏水的情况，假设每次加水的过程，都会漏掉0.1米高度的水。仍然假设kp取0.5，那么会存在着某种情况，假设经过几次加水，水缸中的水位到0.8时，水位将不会再变换！！！因为，水位为0.8，则误差error=0.2. 所以每次往水缸中加水的量为u=0.5*0.2=0.1.同时，每次加水，缸里又会流出去0.1米的水！！！加入的水和流出的水相抵消，水位将不再变化！！
也就是说，我的目标是1米，但是最后系统达到0.8米的水位就不再变化了，且系统已经达到稳定。由此产生的误差就是稳态误差了。
(上诉例子是一个增量PID控制算法的解释，具体什么是增量控制算法，后续会说，先有个基本概念）

2.2 I控制算法

  还是用上面的例子，如果仅仅用比例，可以发现存在暂态误差，最后的水位就卡在0.8了。于是，在控制中，我们再引入一个分量，该分量和误差的积分是正比关系。所以，比例+积分控制算法为：
  u=kp*error+ ki∗∫ error
还是用上面的例子来说明，第一次的误差error是0.8，第二次的误差是0.4，至此，误差的积分（离散情况下积分其实就是做累加），∫error=0.8+0.4=1.2. 这个时候的控制量，除了比例的那一部分，还有一部分就是一个系数ki乘以这个积分项。由于这个积分项会将前面若干次的误差进行累计，所以可以很好的消除稳态误差（假设在仅有比例项的情况下，系统卡在稳态误差了，即上例中的0.8，由于加入了积分项的存在，会让输入增大，从而使得水缸的水位可以大于0.8，渐渐到达目标的1.0.）这就是积分项的作用。

2.3 D控制算法

  换一个另外的例子，考虑刹车情况。平稳的驾驶车辆，当发现前面有红灯时，为了使得行车平稳，基本上提前几十米就放松油门并踩刹车了。当车辆离停车线非常近的时候，则使劲踩刹车，使车辆停下来。整个过程可以看做一个加入微分的控制策略。
微分，说白了在离散情况下，就是error的差值，就是t时刻和t-1时刻error的差，即u=kd*（error（t）-error（t-1）），其中的kd是一个系数项。可以看到，在刹车过程中，因为error是越来越小的，所以这个微分控制项一定是负数，在控制中加入一个负数项，他存在的作用就是为了防止汽车由于刹车不及时而闯过了线。从常识上可以理解，越是靠近停车线，越是应该注意踩刹车，不能让车过线，所以这个微分项的作用，就可以理解为刹车，当车离停车线很近并且车速还很快时，这个微分项的绝对值（实际上是一个负数）就会很大，从而表示应该用力踩刹车才能让车停下来。
切换到上面给水缸加水的例子，就是当发现水缸里的水快要接近1的时候，加入微分项，可以防止给水缸里的水加到超过1米的高度，说白了就是减少控制过程中的震荡。

3、PID控制算法的变形

3.1连续PID

虽然在教程和文献中有各种时间常数，比如TI TD这种，但是由于其本质也是一个参数，所以很多人为了方便，最后统一成了Kp,KI,Kd。

3.2（位置型）数字PID

数字也是一样，可以简化为，


C++代码

#include 

class PositionPID {
private:
    double kp;  // 比例增益
    double ki;  // 积分增益
    double kd;  // 微分增益
    double prevError;  // 上一次误差
    double integral;   // 积分项累加值

public:
    PositionPID(double kp, double ki, double kd) : kp(kp), ki(ki), kd(kd), prevError(0), integral(0) {}

    double calculate(double setpoint, double current) {
        double error = setpoint - current;
        integral += error;
        
        double output = kp * error + ki * integral + kd * (error - prevError);
        
        prevError = error;
        return output;
    }
};

int main() {
    // 示例使用
    double setpoint = 50.0;  // 目标值
    double current = 0.0;   // 当前值

    PositionPID positionPID(0.1, 0.01, 0.05);  // 用具体的参数值初始化PID控制器

    for (int i = 0; i < 100; ++i) {
        double output = positionPID.calculate(setpoint, current);
        // 在实际系统中应用输出值，更新当前值 current
        // 这里仅简单输出控制器的输出值
        std::cout << "Iteration " << i << ": Output = " << output << std::endl;
    }

    return 0;
}

3.2增量型数字PID

C++代码

#include 

class IncrementalPID {
private:
    double kp;  // 比例增益
    double ki;  // 积分增益
    double kd;  // 微分增益
    double prevOutput;  // 上一次的输出
    double prevError;   // 上一次误差

public:
    IncrementalPID(double kp, double ki, double kd) : kp(kp), ki(ki), kd(kd), prevOutput(0), prevError(0) {}

    double calculate(double setpoint, double current) {
        double error = setpoint - current;
        
        double pTerm = kp * error;
        double iTerm = ki * (error + prevError);
        double dTerm = kd * (error - prevError);
        
        double output = prevOutput + pTerm + iTerm + dTerm;

        prevOutput = output;
        prevError = error;

        return output;
    }
};

int main() {
    // 示例使用
    double setpoint = 50.0;  // 目标值
    double current = 0.0;   // 当前值

    IncrementalPID incrementalPID(0.1, 0.01, 0.05);  // 用具体的参数值初始化PID控制器

    for (int i = 0; i < 100; ++i) {
        double output = incrementalPID.calculate(setpoint, current);
        // 在实际系统中应用输出值，更新当前值 current
        // 这里仅简单输出控制器的输出值
        std::cout << "Iteration " << i << ": Output = " << output << std::endl;
    }

    return 0;
}

个人总结

在无人机项目中，我为了迫使无人机姿态一直处于目标中央，通过调整无人机yaw角度来使用水平方向使得识别的目标一直处于无人机中央，但是给出的代码确实直接假设了控制输出等于执行器输入这里绝对是错误的，PID控制器的输出是图像目标中心位置640，但是不能简单的将这个输出等价于偏航角。

---

Command_Now.Reference_State.yaw_ref =incrementalPID.calculate(set_target_x , now_target_x); ;//PID角度控制

---

实际情况是对控制器的输出的目标位置量与角度调整量进行建模，而不是将二者简单相等。