我正在参加中秋创意投稿大赛，详情请看：中秋创意投稿大赛。

写在前面

探月工程已取得阶段性成功，今天就来谈谈飞天探月离不开的算法之PID。PID算法是飞控系统中的的重要算法，它可以自动对控制系统进行准确且迅速的校正，因此被广泛地应用于工业控制系统。

PID算法基本原理

PID算法主要分为三个部分：P：比例环节；I：积分环节；D：微分环节；PID算法的执行流程是非常简单的，即利用反馈来检测偏差信号，并通过偏差信号来控制被控量。而控制器本身就是比例、积分、微分三个环节的加和。

比例参数：控制器的输出与输入偏差值成比例关系。系统一旦出现偏差，比例调节立即产生调节作用以减少偏差。特点：过程简单快速、比例作用大，可以加快调节，减小误差；但是使系统稳定性下降，造成不稳定，有余差。

积分参数：积分环节主要是用来消除静差，所谓静差，就是系统稳定后输出值和设定值之间的差值，积分环节实际上就是偏差累计的过程，把累计的误差加到原有系统上以抵消系统造成的静差。

微分参数：微分信号则反应了偏差信号的变化规律，或者说是变化趋势，根据偏差信号的变化趋势来进行超前调节，从而增加了系统的快速性。流程图如下：

执行图.PNG

根据上图在某一时刻t，此时输入量为rin(t)，输出量为rout(t)，于是偏差就可计算为err(t)=rin(t)-rout(t)。于是PID的基本控制规律就可以表示为如下公式：

数学1.png

在实际应用中，一定要理解控制量U(t)的物理意义。由上述时域表示，不难得知其S域的表达式为：

知乎1.PNG

再用后项差分变换：

知乎2.PNG

对上述PID连续域表达式进行后项差分离散化，则式中各项可近似表示为：

知乎3.PNG

把采样序列kT用 k简化表示，则上面的公式离散为：

知乎4.PNG

进行变换后，则PID控制算法的通用数学表达式如下：

数学2.png 也可以表示为：

数学3.png

PID算法实现

void AC_PID::set_input_filter_all(float input)
{
    if (!isfinite(input)) {
        return;
    }

    if (_flags._reset_filter) {
        _flags._reset_filter = false;
        _input = input;
        _derivative = 0.0f;
    }

    float input_filt_change = get_filt_alpha() * (input - _input);
    _input = _input + input_filt_change;
    if (_dt > 0.0f) {
        _derivative = input_filt_change / _dt;
    }
}

void AC_PID::set_input_filter_d(float input)
{
    if (!isfinite(input)) {
        return;
    }
    if (_flags._reset_filter) {
        _flags._reset_filter = false;
        _derivative = 0.0f;
    }

    if (_dt > 0.0f) {
        float derivative = (input - _input) / _dt;
        _derivative = _derivative + get_filt_alpha() * (derivative-_derivative);
    }

    _input = input;
}

float AC_PID::get_p()
{
    _pid_info.P = (_input * _kp);
    return _pid_info.P;
}

float AC_PID::get_i()
{
    if(!is_zero(_ki) && !is_zero(_dt)) {
        _integrator += ((float)_input * _ki) * _dt;
        if (_integrator < -_imax) {
            _integrator = -_imax;
        } else if (_integrator > _imax) {
            _integrator = _imax;
        }
        _pid_info.I = _integrator;
        return _integrator;
    }
    return 0;
}

float AC_PID::get_d()
{
    _pid_info.D = (_kd * _derivative);
    return _pid_info.D;
}

float AC_PID::get_ff(float requested_rate)
{
    _pid_info.FF = (float)requested_rate * _ff;
    return _pid_info.FF;
}


float AC_PID::get_pi()
{
    return get_p() + get_i();
}

float AC_PID::get_pid()
{
    return get_p() + get_i() + get_d();
}

float AC_PID::get_filt_alpha() const
{
    if (is_zero(_filt_hz)) {
        return 1.0f;
    }

    float rc = 1/(M_2PI*_filt_hz);
    return _dt / (_dt + rc);
}

这就实现了一个最简单的PID控制器，没有考虑任何的干扰条件，仅仅只是对公式的代码实现。完整的控制器：所有的函数值就会先赋值一次，并且整个程序也会先运转一次。设定PID三个参数分别是KP KI KD，这三个参数会来决定他们在这个控制器中的表现。然后就是完成整个控制器的实现，误差的计算，以及新一轮的赋值。再把已经经历过第一轮处理的值重新赋值给将要被PID控制处理的变量的值，这就形成了一个完整的闭环控制系统。

总结

这里对pid算法的原理及推导过程做了简要说明以及简单的实现。pid算法被广泛应用在火箭姿态调整，探测器轨道校正等场景，感兴趣的同学可以继续深入研究一下。

探月工程飞控算法之PID

写在前面

PID算法基本原理

PID算法实现

总结