文章目录
- 摘要
- PID原理概述
-
- 串级双闭环PID控制
-
- 模块化的源码
- 主函数
- 硬件初始化
- PID复位
- 定义私有变量
- 计算串级PID
- PID计算公式源码
- LIMIT范围控制宏定义
- 思维导图
摘要
小飞机由于飞手的技术和外部环境因素(刮风等)导致飞机飞行不稳定。
PID就是用来小飞机自动控制的作用(自稳作用)
PID原理概述
- PID控制的原理是利用给定的目标值(期望值)与实际测量值构成的偏差(误差),对被控对象进行的一种线性控制,控制系统通常由被控对象和PID控制器两部分组成。
比例环节
- 主要用于提高系统的动态响应速度和减少系统稳态误差即提高系统的控制精度。
- 成比例地反映控制系统的偏差信号,一旦产出误差,控制器立即产生控制作用,以减少偏差使实际值接近目标值。
- 控制作用的强弱主要取决于比例系数的大小,比例系数过大,会使系统的动态特性变差,引起输出震荡,还可能导致闭环系统的不稳定;比例系数过小,被控对象会产生较大的静差,达不到预期控制的效果,所以在选择比例系数时要合理适当。
P
o
u
t
=
K
p
e
(
t
)
P_{out} = K_pe(t)
Pout=Kpe(t)
积分环节
- 一个简单的比例控制会震荡,会在设定值的附近来回变化,因为系统无法消除多余的纠正,通过加上负的平均误差值,平均系统误差值就会渐渐减少,最终系统会在设定值稳定下来。
- 积分环节会加速系统趋近设定值的过程,并且消除纯比例环节出现的稳态误差。
- 积分环节考虑过去误差,将误差值过去一段时间和乘以一个系数
K
i
K_i
Ki,通常积分系数来表示积分作用的强弱,积分作用越强,消除偏差的过程会加快,但取值太大会导致系统趋于不稳定。
I
o
u
t
=
K
i
∫
0
t
e
(
τ
)
d
τ
I_{out} = K_i\int_0^te(τ)dτ
Iout=Ki∫0te(τ)dτ
微分环节
- 根据偏差信号的变化趋势对其进行修正,在偏差信号值变得太大之前,引入一个有效的修正信号,从而使系统的动作速度加快,减小调节时间。
D
o
u
t
=
K
d
d
d
t
e
(
t
)
D_{out} = K_d\frac{d}{dt}e(t)
Dout=Kddtde(t)
串级双闭环PID控制
外环
- 目标值:姿态角度
- 测量值:实际姿态角度
- PID输入: 目标值 - 测量值 (相减)
- PID输出:提供使测量值趋近目标值的方法
内环
- 标值:外环输出,即角速度目标值
- 测量值:实际角速度
- PID输入:目标值-测量值
- PID输出:提供使测量值趋近目标值的方法(调整电机转速)
文章目录
- 摘要
- PID原理概述
-
- 串级双闭环PID控制
-
- 模块化的源码
- 主函数
- 硬件初始化
- PID复位
- 定义私有变量
- 计算串级PID
- PID计算公式源码
- LIMIT范围控制宏定义
- 思维导图
模块化的源码
主函数
#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();
MPU6050Init();
SPL06_Init();
NRF_Radio_Init();
if(HARDWARE_CHECK)
{
g_LedManager.emLEDPower = PowerOn;
}
gcs_init();
PID_Init();
USART_Init(USCI_A_UART_CLOCKSOURCE_ACLK,115200);
Timer_Init();
}
##PID参数初始化
void PID_Init(void)
{
PIDGroup[emPID_Roll_Spd].kp = 2.0f;
PIDGroup[emPID_Pitch_Spd].kp = 2.0f;
PIDGroup[emPID_Yaw_Spd].kp = 7.0f;
PIDGroup[emPID_Roll_Spd].ki = 0.05f;
PIDGroup[emPID_Pitch_Spd].ki = 0.05f;
PIDGroup[emPID_Yaw_Spd].ki = 0.0f;
PIDGroup[emPID_Roll_Spd].kd = 0.15f;
PIDGroup[emPID_Pitch_Spd].kd = 0.15f;
PIDGroup[emPID_Yaw_Spd].kd = 0.2f;
PIDGroup[emPID_Roll_Pos].kp = 3.5f;
PIDGroup[emPID_Pitch_Pos].kp = 3.5f;
PIDGroup[emPID_Yaw_Pos].kp = 6.0f;
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;
g_UAVinfo.UAV_Mode = Altitude_Hold;
}
##结构体数组声明
extern PIDInfo_t PIDGroup[emNum_of_PID_List];
##定义数据结构
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;
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);
switch(g_UAVinfo.UAV_Mode)
{
}
}
}
##PID控制函数
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();
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;
ClacCassadePID(&PIDGroup[emPID_Roll_Spd],&[PIDGroup[emPID_Roll_Pos],dt);
ClacCassadePID(&PIDGroup[emPID_Pitch_Spd],&[PIDGroup[emPID_Pitch_Pos],dt);
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复位
void ResetPID(void)
{
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;
const float AtR = 0.0174532925f;
const float Gyro_G = 0.03051756f * 2;
const float Gyro_Gr= 0.0005326f * 2;
const float PI_2 = 1.570796f;
计算串级PID
void ClacCascadePID(PIDInfo_t * pidRate, PIDInfo_t * pidAngE, const float dt)
{
UpdatePID(pidAngE,dt);
pidRate->desired = pidAngE->out;
UpdatePID(pidRate,dt);
}
PID计算公式源码
void UpdatePID(PIDInfo_t * pid, const float dt)
{
float deriv;
pid->Err = pid->desired - pid->measured + pid->offset;
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->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;
}
LIMIT范围控制宏定义
#define LIMIT(x,min,max) ((x)<(min)?(min):((x)>(max)?(max):(x)))
思维导图
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)