匿名飞控底层PID代码解析

大半年就快过去了。
大二上学期转瞬即逝，离上一次博客也有一段时间了，没错，庆幸的是我又回来了，其实本来第二篇博客是想着写一下两轮平衡车来着，代码早就写好了，但是硬件不给力，分着用还挺好，一整合起来，不是电驱烧就是板子烧，所以没调试也就没脸写博客了。
那么今天回归，写一篇博客是解析飞控底层PID代码的，仅仅解析最底层的PID函数代码部分，串级PID的解析什么的等有空下一篇再写吧。
偷得浮生半日闲。写完就去复习电路了。
先贴上代码。

float PID_calculate(
          float dT_s,            //周期（单位：秒）
          float in_ff,    //前馈值
          float expect,    //期望值（设定值）
          float feedback,   //反馈值（）
          _PID_arg_st *pid_arg, //PID参数结构体
          _PID_val_st *pid_val, //PID数据结构体
          float inte_d_lim,//积分误差限幅
          float inte_lim   //integration limit，积分限幅         
          )
 {
 float differential,hz;
 hz = safe_div(1.0f,dT_s,0);
 pid_val->exp_d = (expect - pid_val->exp_old) *hz;
 
 if(pid_arg->fb_d_mode == 0)
 {
  pid_val->fb_d = (feedback - pid_val->feedback_old) *hz;
 }
 else
 {
  pid_val->fb_d = pid_val->fb_d_ex;
 } 
 differential = (pid_arg->kd_ex *pid_val->exp_d - pid_arg->kd_fb *pid_val->fb_d);
 
 pid_val->err = (expect - feedback);
  pid_val->err_i += pid_arg->ki *LIMIT((pid_val->err ),-inte_d_lim,inte_d_lim )*dT_s;//)*T;//+ differential/pid_arg->kp
 //pid_val->err_i += pid_arg->ki *(pid_val->err )*T;//)*T;//+ pid_arg->k_pre_d *pid_val->feedback_d
 pid_val->err_i = LIMIT(pid_val->err_i,-inte_lim,inte_lim);
 pid_val->out = pid_arg->k_ff *in_ff 
     + pid_arg->kp *pid_val->err  
   + differential
//     + pid_arg->k_inc_d_norm *pid_val->err_d_lpf + (1.0f-pid_arg->k_inc_d_norm) *differential
     + pid_val->err_i;
  pid_val->feedback_old = feedback;
 pid_val->exp_old = expect;
 
 return (pid_val->out);
}

首先，简化代码，即用自己的话理解别人的代码，因为别人写的代码总是不如自己写的容易理解。
我先贴上我的理解。
1 　　 　hz=1.0f/dT_s
2 　　　exp_d=(期望值-前期望值)＊hz
3 　　　fb_d=(反馈值-前反馈值)＊hz
4 　　　differential=微分系数＊exp_d-微分先行系数＊fb_d
5 　　　err=期望值-反馈值
6 　　　err_i+=积分系数＊err＊dT_s
7 　　　err_i=err_i(限幅)
8 　　　out=前馈+kp＊err+differential+err_i
9 　　　feedback_old=feedback
10 　　　exp_old=expect
11 　　　return(out)

均省略了结构体前标，我也省事。

解释一下这个代码

先贴一张PID公式

1 　　　hz就是dt的倒数也就是PID图中Kd后面的分母
234 　　就是Kd控制，不过采取了一个新颖的点（微分先行）相对于传统的PID，先记下来，稍后讲
5　　　 比例也就是Kp的那一项参数不详细解释了这些最基础的不懂的百度CSDN都可查到
6　　　 积分控制dT_s也就是图中第二项的dt也没啥好说的
7　　　比较重要的限幅这里讲一下
积分抗饱和
积分控制在实际应用的时候要非常小心。举个例子，假设我们给一个姿态角30度的目标值，但是你人为的让飞机还是保持水平，那这时候，控制器里面的积分会不断的累加这个误差，时间越长，这个积分项的数值越大。前面我们讲过，最终控制器的输出是要由执行机构执行的，我们这里指的是电机。假设电机PWM波的行程最大范围接收到1000，那你控制器的输出，由于积分的累加，假设到了2000，自然是没办法执行的。所以，一般对控制器的输出还会有限幅，保护执行机构，具体数值根据实际情况去定。当然，这个还不是最可怕的。假设你这时候松开飞机，让它去执行这个30度的命令，那这个时候积分项还是从2000开始计算的，实际上你到了30度以后，它还会往这个方向继续执行，出现越界，这时候出现反方向的误差，理论上积分开始慢慢的退，但是由于你之前累加的过长，导致这个退到正确方向太慢了，所以可能飞机早就翻掉了。
8 　　　这里使用了前馈加反馈的控制，这里也记一下也稍后讲
910　　就是将此次的值储存为下一次的计算做准备，期望值反馈值赋值前期望值前反馈值
11　　　不用说了

代码部分其实和传统PID相似度还是比较高的，所做的改进也有，这里详细讲一下，一是微分先行

微分先行PID控制是只对输出量进行微分，而对给定指令不起微分作用，因此它适合于给定指令频繁升降的场合，可以避免指令的改变导致超调过大。
想要详细理解请看这篇博客
PID控制器开发笔记之七：微分先行PID控制器的实现
简单点说其实微分先行就是期望值与前期望值的差的Kd系数和反馈值与前反馈值的差的Kd系数不一样，如果两者的系数相等了，那就是传统PID的微分项了。

二是前馈
见此博客
简单点说
前馈控制属于开环控制，反馈控制属于负反馈的闭环控制
一般定值控制系统是按照测量值与给定值比较得到的偏差进行调节，属于闭环负反馈调节。其特点是在被控变量出现偏差后才进行调节；如果干扰已经发生而没有产生偏差，调节器不会进行工作。因此反馈控制方式的调节作用落后于干扰作用。
前馈调节是按照干扰作用来进行调节的。前馈控制将干扰测量出来并直接引入调节装置，对于干扰的克服比反馈控制及时。
前馈控制系统中测量干扰量，反馈控制系统中测量被控变量
在单纯的前馈控制系统中，不测量被控变量，而单纯的反馈控制系统中不测量干扰量。
3、前馈控制需要专用调节器，反馈控制一般采用通用PID调节器
反馈调节符合PID调节规律,常用通用PID调节器、DCS等或PLC控制系统实现。前馈调节使用的调节器是是根据被控对象的特点来确定调节规律的前馈调节器。
4、前馈控制只能克服所测量的干扰，反馈控制则可克服所有干扰
前馈控制系统中若干扰量不可测量，前馈就不可能加以克服。而反馈控制系统中，任何干扰，只要它影响到被控变量，都能在一定程度上加以克服。
5、前馈控制理论上可以无差，反馈控制必定有差
反馈调节使系统达到动态稳定，让被调参数稳定在给定值附近动态变化，却不能使被调参数稳定在给定值上不动。
前馈调节在理论上可以实现无差调节。
6、前馈控制的局限性
A、在生产应用中各种环节的特性是随负荷变化的，对象动态特性形式多样性难以精确测量，容易造成过补偿或欠补偿。为了补偿前馈调节的不准确，通常将前馈和反馈控制系统结合起来组成前馈反馈控制系统。
B、工业对象存在多个扰动，若均设置前馈控制器，那设备投资高，工作量大。
C、很多前馈补偿结果在现有技术条件下没有检测手段。
D、前馈控制受到前馈控制模型精度限制。
E、前馈控制算法，往往做近似处理。
这里搬运过来了，可以去看看原文章更容易理解

然后
完。
期待更新。