PID控制器原理及模块化的源码

摘要

小飞机由于飞手的技术和外部环境因素（刮风等）导致飞机飞行不稳定。

PID就是用来小飞机自动控制的作用（自稳作用）

PID原理概述

• PID控制的原理是利用给定的目标值(期望值)与实际测量值构成的偏差(误差)，对被控对象进行的一种线性控制，控制系统通常由被控对象和PID控制器两部分组成。

比例环节

• 主要用于提高系统的动态响应速度和减少系统稳态误差即提高系统的控制精度。
• 成比例地反映控制系统的偏差信号，一旦产出误差，控制器立即产生控制作用，以减少偏差使实际值接近目标值。
• 控制作用的强弱主要取决于比例系数的大小，比例系数过大，会使系统的动态特性变差，引起输出震荡，还可能导致闭环系统的不稳定;比例系数过小，被控对象会产生较大的静差，达不到预期控制的效果，所以在选择比例系数时要合理适当。

$$P_{out}=K_{p}e(t)$$

积分环节

• 一个简单的比例控制会震荡，会在设定值的附近来回变化，因为系统无法消除多余的纠正，通过加上负的平均误差值，平均系统误差值就会渐渐减少，最终系统会在设定值稳定下来。
• 积分环节会加速系统趋近设定值的过程，并且消除纯比例环节出现的稳态误差。
• 积分环节考虑过去误差，将误差值过去一段时间和乘以一个系数 $$K_i$$，通常积分系数来表示积分作用的强弱，积分作用越强，消除偏差的过程会加快，但取值太大会导致系统趋于不稳定。

$$I_{out}=K_i{\int }_0^{t}e(\tau )d\tau$$

微分环节

• 根据偏差信号的变化趋势对其进行修正，在偏差信号值变得太大之前，引入一个有效的修正信号，从而使系统的动作速度加快，减小调节时间。

$$D_{out}=K_d\frac{d}{dt}e(t)$$

串级双闭环PID控制

外环

• 目标值：姿态角度
• 测量值：实际姿态角度
• PID输入： 目标值 - 测量值 （相减）
• PID输出：提供使测量值趋近目标值的方法

内环

• 标值：外环输出，即角速度目标值
• 测量值：实际角速度
• PID输入：目标值-测量值
• PID输出：提供使测量值趋近目标值的方法（调整电机转速）

模块化的源码

主函数

#include "include.h"
void main()
{
    WDT_A_hold(WDT_A_BASE);
    _DINT();  //禁止所有中断
    //@@@
    Hardware_Init();//硬件初始化  
    _EINT();//使能中断
    while (1)
    {
        PollingKernel();  
    }
}

硬件初始化

void Hardware_Init(void)
{
    System_Clock_Init();
    I2C_INit();
    Motor_Init();
    LEDInit();			//LED灯闪初始化
    MPU6050Init();    	//g_MPUManager初始化
    SPL06_Init();		//SPL06初始化
    NRF_Radio_Init(); 
   	if(HARDWARE_CHECK)  //硬件检测
    {
        g_LedManager.emLEDPower = PowerOn;
    }
    
    gcs_init();   		//地面站通信初始化
    
    //@@@
    PID_Init();    		//PID参数初始化   
    
    USART_Init(USCI_A_UART_CLOCKSOURCE_ACLK,115200);
    Timer_Init();
}

##PID参数初始化

/*************************************************************************************
  *函数名称：PID_Init
  *函数描述：初始化所有PID参数
  *输   入：void
  *输   出：void
  *返   回：void
  *备   注：null
  *
  *
*************************************************************************************/
//@@@
void PID_Init(void)
{
    //Roll滚转 Pitch俯仰 Yaw偏航
    //Kp
    //可以加f表示float这样用？？？
    PIDGroup[emPID_Roll_Spd].kp		= 2.0f;  
    PIDGroup[emPID_Pitch_Spd].kp	= 2.0f;
    PIDGroup[emPID_Yaw_Spd].kp 		= 7.0f;
    
    //Ki
    PIDGroup[emPID_Roll_Spd].ki 	= 0.05f;
    PIDGroup[emPID_Pitch_Spd].ki	= 0.05f;
    PIDGroup[emPID_Yaw_Spd].ki		= 0.0f;
        
   	//Kd
    PIDGroup[emPID_Roll_Spd].kd 	= 0.15f;
    PIDGroup[emPID_Pitch_Spd].kd	= 0.15f;
    PIDGroup[emPID_Yaw_Spd].kd		= 0.2f;
    
    //Pos  //Kp
    PIDGroup[emPID_Roll_Pos].kp 	= 3.5f;
    PIDGroup[emPID_Pitch_Pos].kp	= 3.5f;
    PIDGroup[emPID_Yaw_Pos].kp 		= 6.0f;
    
    //不建议修改以下参数   为啥？？？
    //Height  //Spd
    PIDGroup[emPID_Height_Spd].kp 	= 3.5f;
    PIDGroup[emPID_Height_Spd].ki	= 0.00f;
    PIDGroup[emPID_Height_Spd].kd	= 0.5f;
         
    PIDGroup[emPID_Height_Pos].kp			= 2.0f;
    PIDGroup[emPID_Height_Pos].desired		= 80;
    PIDGroup[emPID_Height_Pos].OutLimitHigh	= 50;
    PIDGroup[emPID_Height_Pos].OutLimitLow	= -50;
        
	//初始化UAV相关信息  ？？？
    //很熟悉？？？
    g_UAVinfo.UAV_Mode = Altitude_Hold;
}

• 说明：
  • PID控制是一维的控制

##结构体数组声明

//extern引用
//在此处声明，在别处定义
extern  PIDInfo_t  PIDGroup[emNum_of_PID_List];

##定义数据结构

//定义PIDInfo_t结构体

typedef struct
{
    float kp;
    float ki;
    float kd;
    float out;
    float Err;
    float desired;
    float measured;
    
    float Err_LimitHigh;
    float Err_LimitLow;
    
    float offset;
    float prevError;
    float integ;
    
    float IntegLimitHigh;
    float IntegLimitLow;
    
    float OutLimitHigh;
    float OutLimitLow;
    
}PIDInfo_t; //结构体名

//定义emPID_List_t枚举类型

typedef enum
{
	emPID_Pitch_Spd	=0;
	emPID_Roll_Spd,
	emPID_Yaw_Spd,
	
	emPID_Pitch_Pos,
	emPID_Roll_Pos,
	emPID_Yaw_Pos,
	
	emPID_Height_Spd,
	emPID_Height_Pos,
	
	emPID_AUX1,    //缺省变量，占位以后使用
	emPID_AUX2,
	emPID_AUX3,
	emPID_AUX4,
	emPID_AUX5,
	
	emNum_of_PID_List
	
}emPID_List_t;

##死循环部分

void PollingKernel()
{
	if(Thread_3MS)
	{
		Thread_3MS = false;
		GetMPU6050Data();
		
		//@@@
		FlightPidControl(0.003f);   //PID控制
		
		switch(g_UAVinfo.UAV_Mode)
		{
			//.....此部分代码省略
		}
       // .....此部分代码省略
	}
   // .....此部分代码省略
}

##PID控制函数

/*************************************************************************************
  *函数名称：FlightPidControl
  *函数描述：PID控制
  *输   入：float dt时间变量
  *输   出：void
  *返   回：void
  *备   注：null
  *
  *
*************************************************************************************/

void FlightPidControl(float dt)
{
	volatile static uint8_t status = WAITING_1;  //状态变量
	
	switch(status)
	{
		case WAITING_1:
			if(g_FMUf(g.unlock)  //等待解锁
			{
				status = READY_11;
			}
			break;
		case READY_11:
			ResetPID(); 	//批量复位PID
			g_Attitude.yaw = 0;
			PIDGroup[emPID_Yaw_Pos].desired = 0;
			PIDGroup[emPID_Yaw_Pos].measured = 0;
			status = PROCESS_31;
			break;
		case PROCESS_31:
			//内环测量值（角速度）  单位：角度/秒
			PIDGroup[emPID_Roll_Spd].measured 	= g.MPUManager.gyroX * Gyro_G;
			PIDGroup[emPID_Pitch_Spd].measured 	= g.MPUManager.gyroY * Gyro_G;
			PIDGroup[emPID_Yaw_Spd].measured 	= g.MPUManager.gyroZ * Gyro_G;
		
			//外环测量值（角度）    单位：角度
			PIDGroup[emPID_Pitch_Pos].measured 	= g_Attitude.roll;
			PIDGroup[emPID_Yaw_Pos].measured 	= g_Attitude.pitch;
			PIDGroup[emPID_Roll_Pos].measured 	= g_Attitude.yaw;
		
			//调节电机参数
            //X轴
			ClacCassadePID(&PIDGroup[emPID_Roll_Spd],&[PIDGroup[emPID_Roll_Pos],dt);
            //Y轴                                           
			ClacCassadePID(&PIDGroup[emPID_Pitch_Spd],&[PIDGroup[emPID_Pitch_Pos],dt);
            //Z轴                                            
			ClacCassadePID(&PIDGroup[emPID_Yaw_Spd],&[PIDGroup[emPID_Yaw_Pos],dt);  
			break;
		case EXIT_255; //退出控制
			ResetPID();
			status = WAITING_1;  //返回等待解锁
			break;
		default:
			status = EXIT_255;
			break;	
	}
}

PID复位

/*************************************************************************************
  *函数名称：ResetPID
  *函数描述：复位PID参数中经过计算得来的结果
  *输   入：void
  *输   出：void
  *返   回：void
  *备   注：null
  *
  *
*************************************************************************************/

void ResetPID(void)
{
    //在里面定义i也可以吗？？？
    for(int i=0;i<emNum_of_PID_List;i++)
    {
        PIDGroup[i].integ 		= 0;
        PIDGroup[i].prevError 	= 0;
        PIDGroup[i].out			= 0;
        PIDGroup[i].offset		= 0;
	}
    
    PIDGroup[emPID_Height_Pos].desired = 80;
}

定义私有变量

//私有变量区

const float  M_PI	= 3.1415926535; 
const float  RtA	= 57.2957795f;    //（360/2π） 弧度化为角度
const float  AtR	= 0.0174532925f;  //（2π/360） 角度化弧度

//@@@
const float  Gyro_G	= 0.03051756f * 2;  //（4000/65536）MPU6050精度

const float  Gyro_Gr= 0.0005326f * 2；  //Gyro_G换算成弧度
const float  PI_2	= 1.570796f;

计算串级PID

/*************************************************************************************
  *函数名称：ClacCascadePID
  *函数描述：计算串级PID
  *输   入：PIDInfo_t * pidRate ,PID速度环
  *		   PIDInfo_t * pidAngE ,PID角度环
  *		   const float dt ,单位运行时间
  *输   出：void
  *返   回：void
  *备   注：null
  *
  *
*************************************************************************************/

void ClacCascadePID(PIDInfo_t * pidRate, PIDInfo_t * pidAngE, const float dt)
{
    UpdatePID(pidAngE,dt);   //先计算外环
    
    pidRate->desired = pidAngE->out;  //将外环结果传递给内环
    
    UpdatePID(pidRate,dt);   //计算内环
}

PID计算公式源码

/*************************************************************************************
  *函数名称：UpdatePID
  *函数描述：计算PID相关值
  *输   入：PIDInfo_t * pid ,要计算的PID结构体指针
  *		   float dt	,单位运行时间
  *输   出：void
  *返   回：void
  *备   注：null
  *
  *
*************************************************************************************/

void UpdatePID(PIDInfo_t * pid, const float dt)
{
    float deriv;
    //(误差=目标值-测量值)
    pid->Err = pid->desired - pid->measured + pid->offset;
    
    //控制误差范围在[Err_LimitLow , Err_LimitHigh]之间
    if(pid->Err_LimitHigh != 0 && pid->Err_LimitLow != 0)
    {
        pid->Err = LIMIT(pid->Err, pid->Err_LimitLow, pid->Err_LimitHigh);
	}
    
    pid->Integ += pid->Err * dt;  //积分运算
    
     if(pid->IntegLimitHigh != 0 && pid->IntegLimitLow != 0)
    {
        pid->Integ = LIMIT(pid->Integ, pid->IntegLimitLow, pid->IntegLimitHigh);
	}
    
    deriv = (pid->Err - pid->prevError) / dt;  //微分运算
    
    //PID输出
    pid->out = pid->kp * pid->Err + pid->ki * pid->Integ + pid->kd * deriv;   
    
    if(pid->OutLimitHigh != 0 && pid->OutLimitLow != 0)
    {
        pid->out = LIMIT(pid->out, pid->OutLimitLow, pid->OutLimitHigh);
	}
    
    pid->prevError = pid->err; //供下次调用本次PID
}

LIMIT范围控制宏定义

//宏定义区

#define LIMIT(x,min,max)  ((x)<(min)?(min):((x)>(max)?(max):(x)))

思维导图