匿名飞控TI版_PID部分，串级PID，微分先行，前馈控制
文章目录
- PID介绍
-
- 有趣的故事
- 控制模型
- 位置式PID和增量式PID
-
- 位置式PID
- 增量式PID
- 串级PID
- 前馈控制
- 微分先行
- 匿名代码分析
PID介绍
PID介绍
有趣的故事
PID的故事
\space\space\space\space\space\space 小明接到这样一个任务：有一个水缸点漏水(而且漏水的速度还不一定固定不变)，要求水面高度维持在某个位置，一旦发现水面高度低于要求位置，就要往水缸里加水。
\space\space\space\space\space\space 小明接到任务后就一直守在水缸旁边，时间长就觉得无聊，就跑到房里看小说了，每30分钟来检查一次水面高度。水漏得太快，每次小明来检查时，水都快漏完了，离要求的高度相差很远，小明改为每3分钟来检查一次，结果每次来水都没怎么漏，不需要加水，来得太频繁做的是无用功。几次试验后，确定每10分钟来检查一次。这个检查时间就称为采样周期。
\space\space\space\space\space\space 开始小明用瓢加水，水龙头离水缸有十几米的距离，经常要跑好几趟才加够水，于是小明又改为用桶加，一加就是一桶，跑的次数少了，加水的速度也快了，但好几次将缸给加溢出了，不小心弄湿了几次鞋，小明又动脑筋，我不用瓢也不用桶，老子用盆，几次下来，发现刚刚好，不用跑太多次，也不会让水溢出。这个加水工具的大小就称为比例系数。
\space\space\space\space\space\space 小明又发现水虽然不会加过量溢出了，有时会高过要求位置比较多，还是有打湿鞋的危险。他又想了个办法，在水缸上装一个漏斗，每次加水不直接倒进水缸，而是倒进漏斗让它慢慢加。这样溢出的问题解决了，但加水的速度又慢了，有时还赶不上漏水的速度。于是他试着变换不同大小口径的漏斗来控制加水的速度，最后终于找到了满意的漏斗。漏斗的时间就称为积分时间。
\space\space\space\space\space\space 小明终于喘了一口，但任务的要求突然严了，水位控制的及时性要求大大提高，一旦水位过低，必须立即将水加到要求位置，而且不能高出太多，否则不给工钱。小明又为难了！于是他又开努脑筋，终于让它想到一个办法，常放一盆备用水在旁边，一发现水位低了，不经过漏斗就是一盆水下去，这样及时性是保证了，但水位有时会高多了。他又在要求水面位置上面一点将水凿一孔，再接一根管子到下面的备用桶里这样多出的水会从上面的孔里漏出来。这个水漏出的快慢就称为微分时间。
PID的一些介绍
控制模型
人以PID控制的方式用水壶往水杯里倒印有刻度的半杯水后停下；
\space\space\space\space\space\space 设定值：水杯的半杯刻度；
\space\space\space\space\space\space 实际值：水杯的实际水量；
\space\space\space\space\space\space 输出值：水壶的倒出数量和水杯舀出水量；
\space\space\space\space\space\space 测量传感器：人的眼睛
\space\space\space\space\space\space 执行对象：人
\space\space\space\space\space\space 正执行：倒水
\space\space\space\space\space\space 反执行：舀水
1.P 比例控制，就是人看到水杯里水量没有达到水杯的半杯刻度，就按照一定水量从水壶里往水杯里倒水或者水杯的水量多过刻度，就以一定水量从水杯里舀水出来，这个一个动作可能会造成不到半杯或者多了半杯就停下来。
\space\space\space\space\space\space 说明：P比例控制是一种最简单的控制方式。其控制器的输出与输入误差信号成比例关系。当仅有比例控制时系统输出存在稳态误差（Steady-state error）。
2.PI 积分控制，就是按照一定水量往水杯里倒，如果发现杯里的水量没有刻度就一直倒，后来发现水量超过了半杯，就从杯里往外面舀水，然后反复不够就倒水，多了就舀水，直到水量达到刻度。
\space\space\space\space\space\space 说明：在积分I控制中，控制器的输出与输入误差信号的积分成正比关系。对一个自动控制系统，如果在进入稳态后存在稳态误差，则称这个控制系统是有稳态误差的或简称有差系统（System with Steady-state Error）。为了消除稳态误差，在控制器中必须引入“积分项”。积分项的误差取决于时间的积分，随着时间的增加，积分项会增大。这样，即便误差很小，积分项也会随着时间的增加而加大，它推动控制器的输出增大使稳态误差进一步减小，直到等于零。因此，比例+积分（PI）控制器，可以使系统在进入稳态后无稳态误差。
3.PID 微分控制，就是人的眼睛看着杯里水量和刻度的距离，当差距很大的时候，就用水壶进行大水量的倒水，当人看到水量快要接近刻度的时候，就减少水壶的出水量，慢慢的逼近刻度，直到停留在杯中的刻度。如果最后能精确停在刻度的位置，就是无静差控制；如果停在刻度附近，就是有静差控制。
位置式PID和增量式PID
位置式PID
e(k): 用户设定的值（目标值） - 控制对象的当前的状态值
比例P : e(k)
积分I : ∑e(i) 误差的累加
微分D : e(k) - e(k-1) 这次误差-上次误差
\space\space\space\space\space\space 因为有误差积分 ∑e(i)，一直累加，也就是当前的输出u(k)与过去的所有状态都有关系，用到了误差的累加值；（误差e会有误差累加），输出的u(k)对应的是执行机构的实际位置，，一旦控制输出出错(控制对象的当前的状态值出现问题） ，u(k)的大幅变化会引起系统的大幅变化。
\space\space\space\space\space\space 并且位置式PID在积分项达到饱和时,误差仍然会在积分作用下继续累积，一旦误差开始反向变化，系统需要一定时间从饱和区退出，所以在u(k)达到最大和最小时，要停止积分作用，并且要有积分限幅和输出限幅。
\space\space\space\space\space\space 所以在使用位置式PID时，一般我们直接使用PD控制, 而位置式 PID 适用于执行机构不带积分部件的对象，如舵机和平衡小车的直立和温控系统的控制。
增量式PID
比例P : e(k)-e(k-1) 这次误差-上次误差
积分I : e(i) 误差
微分D : e(k) - 2e(k-1)+e(k-2) 这次误差-2*上次误差+上上次误差
而得出的控制量▲u(k)对应的是近几次位置误差的增量，而不是对应与实际位置的偏差 没有误差累加
\space\space\space\space\space\space 也就是说，增量式PID中不需要累加。控制增量Δu(k)的确定仅与最近3次的采样值有关，容易通过加权处理获得比较好的控制效果，并且在系统发生问题时，增量式不会严重影响系统的工作
区别：
\space\space\space\space\space\space 1 增量式算法不需要做累加，控制量增量的确定仅与最近几次偏差采样值有关，计算误差对控制 量计算的影响较小。而位置式算法要用到过去偏差的累加值，容易产生较大的累加误差。
\space\space\space\space\space\space 2 增量式算法得出的是控制量的增量，例如在阀门控制中，只输出阀门开度的变化部分，误动作 影响小，必要时还可通过逻辑判断限制或禁止本次输出，不会严重影响系统的工作。 而位置式的输出直接对应对象的输出，因此对系统影响较大。
\space\space\space\space\space\space 3 增量式PID控制输出的是控制量增量，并无积分作用，因此该方法适用于执行机构带积分部件的对象，如步进电机等，而位置式PID适用于执行机构不带积分部件的对象，如电液伺服阀。
\space\space\space\space\space\space 4 在进行PID控制时，位置式PID需要有积分限幅和输出限幅，而增量式PID只需输出限幅
串级PID
理解参考链接
代码参考链接
\space\space\space\space\space\space 串级PID是PID的一种扩展，等于是给内环一个动态的期望。这个动态的期望由外环确定，让系统更加稳定。
\space\space\space\space\space\space 因为这个控制系统所包含的信息更多更全面，控制的变量更多，系统更加稳定。就如位置和速度加速度。我们控制的量越多，越稳定，只对一个量控制的话只是一个不算太稳定的系统。
\space\space\space\space\space\space 实际应用过程中内环的更新频率应该比外环快。因为内环才是最直接的控制运动的核心外环只是起一个引导的作用让曲线更加丝滑。匿名的代码中高度速度环 (内环)的期望=前馈*前馈系数+外环的输出，它们同时调节，让起飞有了保证的前馈，角速度环。
前馈控制
\space\space\space\space\space\space 就是按扰动量进行补偿，但用的话就是开环的，一般要和反馈控制结合使用。在匿名的代码里一个特别单独加入了前馈的地方，就是高度速度环加入一个（起飞速度+设定速度）的前馈量，保证飞行过程中速度在一定程度的恒定，到达指定高度应该要变成零，代码中自己写（这里是自己的理解。）
//=====3、通用程控速度功能接口函数=====
/**********************************************************************************************************
*函 数 名: Program_Ctrl_User_Set_WHZcmps
*功能说明: 程控功能，上升下降速度设定（实时控制）
*参 数: 速度（厘米每秒，正为上升，负为下降）
*返 回 值: 无
**********************************************************************************************************/
void Program_Ctrl_User_Set_Zcmps(float z_vel_cmps)
{
//
pc_user.vel_cmps_set_z = z_vel_cmps;
//限幅
pc_user.vel_cmps_set_z = LIMIT(pc_user.vel_cmps_set_z,-MAX_PC_ZVEL_CMPS,MAX_PC_ZVEL_CMPS);
}
case (0x03): //上升
{
//目标速度赋值
program_ctrl.vel_cmps_ref[Z] = spd;
//目标时间赋值
if(spd != 0)
{
program_ctrl.exp_process_t_ms[Z] = val*1000/LIMIT(spd,0,fc_stv.vel_limit_z_p);
}
else
{
program_ctrl.exp_process_t_ms[Z] = 0;
}
//判断开始和完成
if(program_ctrl.fb_process_t_ms[Z] == 0)
{
//
ANO_DT_SendString("Go up!");
}
else if(program_ctrl.exp_process_t_ms[Z] < program_ctrl.fb_process_t_ms[Z])
{
//
ANO_DT_SendString("Go up OK!");
//
program_ctrl.cmd_state[0] = 0;
}
//计时反馈
program_ctrl.fb_process_t_ms[Z] += dT_ms;
}
break;
void Alt_1level_Ctrl(float dT_s)
{
u8 out_en;
out_en = (flag.taking_off != 0) ? 1 : 0;
flag.thr_mode = THR_AUTO;//THR_MANUAL;
loc_ctrl_1.exp[Z] = 0.6f *fs.alt_ctrl_speed_set + alt_val_2.out;//速度前馈0.6f，直接给速度
w_acc_z_lpf += 0.2f *(imu_data.w_acc[Z] - w_acc_z_lpf); //低通滤波
loc_ctrl_1.fb[Z] = wcz_spe_fus.out + Ano_Parame.set.pid_alt_1level[KD] *w_acc_z_lpf;//微分先行，下边PID函数微分系数为0
PID_calculate( dT_s, //周期（单位：秒）
0, //前馈值
loc_ctrl_1.exp[Z], //期望值（设定值）
loc_ctrl_1.fb[Z] , //反馈值（）
&alt_arg_1, //PID参数结构体
&alt_val_1, //PID数据结构体
100,//积分误差限幅
(THR_INTE_LIM *10 - err_i_comp )*out_en //integration limit，积分限幅
);
if(flag.taking_off == 1)
{
LPF_1_(1.0f,dT_s,THR_START *10,err_i_comp);//err_i_comp = THR_START *10;
}
else
{
err_i_comp = 0;
}
loc_ctrl_1.out[Z] = out_en *(alt_val_1.out + err_i_comp);
loc_ctrl_1.out[Z] = LIMIT(loc_ctrl_1.out[Z],0,MAX_THR_SET *10);
mc.ct_val_thr = loc_ctrl_1.out[Z];
}
微分先行
\space\space\space\space\space\space 发生突变，而被调量的变化，通常总是比较缓慢的。这种输出量先行微分控制适合于给定值频繁变化的场合，可以避免给定值变化时可能引起的系统振荡，明显地改善了系统的动态特性。其实就是d参数对于期望值和反馈值的比重不同。先分别对期望值和反馈值微分，然后乘以不同的微分系数后做差，微分系数分为d_exp和d_fb，如果这两个值相等，和普通的pid就一样了。
\space\space\space\space\space\space 飞控的控制都较为频繁且变化快，适用于这种场合。
微分先行详细描述
匿名代码分析
高度环分析 在这里高度环的输出作为高度速度环的部分输入
void Alt_2level_Ctrl(float dT_s)
{
Auto_Take_Off_Land_Task(1000*dT_s);
fs.alt_ctrl_speed_set = fs.speed_set_h[Z] + auto_taking_off_speed;//高度速度设置 起飞速度（到达起飞高度时之前的速度,到达起飞高度后为0）+设置高度速度(遥控器速度+程序控制速度+上位机设置速度)
//
loc_ctrl_2.exp[Z] += fs.alt_ctrl_speed_set *dT_s;//速度积分就是位移
loc_ctrl_2.exp[Z] = LIMIT(loc_ctrl_2.exp[Z],loc_ctrl_2.fb[Z]-200,loc_ctrl_2.fb[Z]+200);//位移限幅，高度限幅
//
loc_ctrl_2.fb[Z] = (s32)wcz_hei_fus.out;/反馈值由其他函数计算得到
if(fs.alt_ctrl_speed_set != 0)
{
flag.ct_alt_hold = 0;
}
else
{
if(ABS(loc_ctrl_1.exp[Z] - loc_ctrl_1.fb[Z])<20)//速度为零，高度差不多在20mm范围内//如果测量值和微分值都对的话）
{
flag.ct_alt_hold = 1;//悬停
}
}
if(flag.taking_off == 1)
{
PID_calculate( dT_s, //周期（单位：秒）
0, //前馈值
loc_ctrl_2.exp[Z], //期望值（设定值）
loc_ctrl_2.fb[Z], //反馈值（）
&alt_arg_2, //PID参数结构体
&alt_val_2, //PID数据结构体
100,//积分误差限幅
0 //integration limit，积分限幅
);
}
else//没起飞，期望直接等于反馈输出为0 双重保障
{
loc_ctrl_2.exp[Z] = loc_ctrl_2.fb[Z];
alt_val_2.out = 0;
}
alt_val_2.out = LIMIT(alt_val_2.out,-150,150);//输出限幅
}
PID 函数解析
位置式PID要注意积分限幅，和输出限幅
/******************** (C) COPYRIGHT 2016 ANO Tech ***************************
* 作者 ：匿名科创
* 文件名 ：ANO_PID.c
* 描述 ：PID函数
* 官网 ：www.anotc.com
* 淘宝 ：anotc.taobao.com
* 技术Q群 ：190169595
*****************************************************************************/
#include "Ano_Pid.h"
#include "Ano_Math.h"
#include "Ano_Filter.h"
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_arg->k_inc_d_norm = LIMIT(pid_arg->k_inc_d_norm,0,1);
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
//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);
}
/******************* (C) COPYRIGHT 2016 ANO TECH *****END OF FILE************/
姿态角速度环更新要快于姿态角度环，角速度环直接控制到电机更加丝滑，对于角度环的控制只是一个引导作用的控制甚至可以直接用KP控制。
static void Loop_Task_1(u32 dT_us) //2ms执行一次
{
//
float t1_dT_s;
t1_dT_s = (float)dT_us *1e-6f;
//========================
/*姿态角速度环控制*/
Att_1level_Ctrl(2*1e-3f);
/*电机输出控制*/
Motor_Ctrl_Task(2);
//
}
static void Loop_Task_2(u32 dT_us) //6ms执行一次
{
//
float t2_dT_s;
t2_dT_s = (float)dT_us *1e-6f;
//========================
/*获取姿态角（ROLL PITCH YAW）*/
calculate_RPY();
/*姿态角度环控制*/
Att_2level_Ctrl(6e-3f,CH_N);
//
//
}