匿名飞控TI版_PID部分，串级PID，微分先行，前馈控制

星*眼

已于 2022-02-04 00:14:27 修改

阅读量4.6k

点赞数 18

分类专栏：无人机文章标签：算法

于 2022-02-03 22:49:21 首次发布

本文链接：https://blog.csdn.net/qq_52396872/article/details/122759983

版权

无人机专栏收录该内容

3 篇文章 2 订阅

订阅专栏

文章目录

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);
}

或者设置速度和距离修改FlyCtrl.c里面的val和spd 这里自带一个时间记录比较方便，但是会和上位机控制冲突（其实一般用不到随便改）

		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);

	//
	
//	
}

星*眼

关注

18
点赞
踩
120

收藏

觉得还不错? 一键收藏
8
评论
匿名飞控TI版_PID部分，串级PID，微分先行，前馈控制

文章目录PID介绍有趣的故事控制模型位置式PID和增量式PID位置式PID增量式PID串级PID前馈控制微分先行匿名代码分析PID介绍PID介绍有趣的故事PID的故事      \space\space\space\space\space\space      小明接到这样一个任务：有一个水缸点漏水(而且漏水的速度还不一定固定不变)，要求水面高度维持在某个位置，一旦发现水面高
复制链接

扫一扫