system/lib/control/custom/pid_zh1.c

620 lines
19 KiB
C
Raw Permalink Blame History

This file contains invisible Unicode characters

This file contains invisible Unicode characters that are indistinguishable to humans but may be processed differently by a computer. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

#include <math.h>
#include "pid_zh1.h"
// 定义输出量比例因子
#define KUP 1.0f
#define KUI 1.0f
#define KUD 1.0f
// 模糊集合
#define NL -3
#define NM -2
#define NS -1
#define ZE 0
#define PS 1
#define PM 2
#define PL 3
// 定义偏差E的范围因为设置了非线性区间误差在10时才开始进行PID调节这里E的范围为10
#define MAXE (10)
#define MINE (-MAXE)
// 定义EC的范围因为变化非常缓慢每次的EC都非常小这里可以根据实际需求来调整
#define MAXEC (10)
#define MINEC (-MAXEC)
// 定义e,ec的量化因子
#define KE 3 / MAXE
#define KEC 3 / MAXEC
/*
static const float fuzzyRuleKp[7][7] = {
PL, PL, PM, PL, PS, PM, PL,
PL, PM, PM, PM, PS, PM, PL,
PM, PS, PS, PS, PS, PS, PM,
PM, PS, ZE, ZE, ZE, PS, PM,
PS, PS, PS, PS, PS, PM, PM,
PM, PM, PM, PM, PL, PL, PL,
PM, PL, PL, PL, PL, PL, PL};
static const float fuzzyRuleKi[7][7] = {
PL, PL, PL, PL, PM, PL, PL,
PL, PL, PM, PM, PM, PL, PL,
PM, PM, PS, PS, PS, PM, PM,
PM, PS, ZE, ZE, ZE, PS, PM,
PM, PS, PS, PS, PS, PM, PM,
PM, PM, PS, PM, PM, PL, PL,
PM, PL, PM, PL, PL, PL, PL};
static const float fuzzyRuleKd[7][7] = {
PS, PS, ZE, ZE, ZE, PL, PL,
PS, PS, PS, PS, ZE, PS, PM,
PL, PL, PM, PS, ZE, PS, PM,
PL, PM, PM, PS, ZE, PS, PM,
PL, PM, PS, PS, ZE, PS, PS,
PM, PS, PS, PS, ZE, PS, PS,
PS, ZE, ZE, ZE, ZE, PL, PL};
*/
static const float fuzzyRuleKp[7][7] = {
PL, PL, PM, PL, PS, PM, PL,
PL, PM, PM, PM, PS, PM, PL,
PM, PS, PS, PS, PS, PS, PM,
PM, PS, ZE, ZE, ZE, PS, PM,
PS, PS, PS, PS, PS, PM, PM,
PM, PM, PM, PM, PL, PL, PL,
PM, PL, PL, PL, PL, PL, PL};
static const float fuzzyRuleKi[7][7] = {
PL, PL, PL, PL, PM, PL, PL,
PL, PL, PM, PM, PM, PL, PL,
PM, PM, PS, PS, PS, PM, PM,
PM, PS, ZE, ZE, ZE, PS, PM,
PM, PS, PS, PS, PS, PM, PM,
PM, PM, PS, PM, PM, PL, PL,
PM, PL, PM, PL, PL, PL, PL};
static const float fuzzyRuleKd[7][7] = {
PS, PS, ZE, ZE, ZE, PL, PL,
PS, PS, PS, PS, ZE, PS, PM,
PL, PL, PM, PS, ZE, PS, PM,
PL, PM, PM, PS, ZE, PS, PM,
PL, PM, PS, PS, ZE, PS, PS,
PM, PS, PS, PS, ZE, PS, PS,
PS, ZE, ZE, ZE, ZE, PL, PL};
static void fuzzy(float e, float ec, fuzzy_pid_t *fuzzy_pid)
{
float etemp, ectemp;
float eLefttemp, ecLefttemp; // 左隶属度
float eRighttemp, ecRighttemp; // 右隶属度
int eLeftIndex, ecLeftIndex; // 模糊位置标号
int eRightIndex, ecRightIndex;
e = RANGE(e, MINE, MAXE);
ec = RANGE(ec, MINEC, MAXEC);
e = e * KE;
ec = ec * KEC;
etemp = e > 3.0f ? 0.0f : (e < -3.0f ? 0.0f : (e >= 0.0f ? (e >= 2.0f ? 2.5f : (e >= 1.0f ? 1.5f : 0.5f)) : (e >= -1.0f ? -0.5f : (e >= -2.0f ? -1.5f : (e >= -3.0f ? -2.5f : 0.0f)))));
eLeftIndex = (int)((etemp - 0.5f) + 3);
eRightIndex = (int)((etemp + 0.5f) + 3);
eLefttemp = etemp == 0.0f ? 0.0f : ((etemp + 0.5f) - e);
eRighttemp = etemp == 0.0f ? 0.0f : (e - (etemp - 0.5f));
ectemp = ec > 3.0f ? 0.0f : (ec < -3.0f ? 0.0f : (ec >= 0.0f ? (ec >= 2.0f ? 2.5f : (ec >= 1.0f ? 1.5f : 0.5f)) : (ec >= -1.0f ? -0.5f : (ec >= -2.0f ? -1.5f : (ec >= -3.0f ? -2.5f : 0.0f)))));
ecLeftIndex = (int)((ectemp - 0.5f) + 3);
ecRightIndex = (int)((ectemp + 0.5f) + 3);
ecLefttemp = ectemp == 0.0f ? 0.0f : ((ectemp + 0.5f) - ec);
ecRighttemp = ectemp == 0.0f ? 0.0f : (ec - (ectemp - 0.5f));
/*************************************反模糊*************************************/
fuzzy_pid->kp = (eLefttemp * ecLefttemp * fuzzyRuleKp[eLeftIndex][ecLeftIndex] + eLefttemp * ecRighttemp * fuzzyRuleKp[eLeftIndex][ecRightIndex] + eRighttemp * ecLefttemp * fuzzyRuleKp[eRightIndex][ecLeftIndex] + eRighttemp * ecRighttemp * fuzzyRuleKp[eRightIndex][ecRightIndex]);
fuzzy_pid->ki = (eLefttemp * ecLefttemp * fuzzyRuleKi[eLeftIndex][ecLeftIndex] + eLefttemp * ecRighttemp * fuzzyRuleKi[eLeftIndex][ecRightIndex] + eRighttemp * ecLefttemp * fuzzyRuleKi[eRightIndex][ecLeftIndex] + eRighttemp * ecRighttemp * fuzzyRuleKi[eRightIndex][ecRightIndex]);
fuzzy_pid->kd = (eLefttemp * ecLefttemp * fuzzyRuleKd[eLeftIndex][ecLeftIndex] + eLefttemp * ecRighttemp * fuzzyRuleKd[eLeftIndex][ecRightIndex] + eRighttemp * ecLefttemp * fuzzyRuleKd[eRightIndex][ecLeftIndex] + eRighttemp * ecRighttemp * fuzzyRuleKd[eRightIndex][ecRightIndex]);
// 对解算出的KP,KI,KD进行量化映射
fuzzy_pid->kp = fuzzy_pid->kp * fuzzy_pid->kup;
fuzzy_pid->ki = fuzzy_pid->ki * fuzzy_pid->kui;
fuzzy_pid->kd = fuzzy_pid->kd * fuzzy_pid->kud;
}
static void smooth_init(smart_pid_t *pid, BOOL sm_open, float maxTarget)
{
if (!pid)
return;
pid->sm_open = sm_open;
pid->maxTarget = maxTarget;
}
static void smooth_target(smart_pid_t *pid, float *target)
{
if ((!pid) || (!target))
return;
if (!pid->maxTarget)
return;
float sm_step = (pid->maxTarget) * 0.1f;
float k = 0.0f;
if (fabs(pid->target - *target) <= sm_step)
{
pid->target = *target;
}
else
{
if (pid->target - *target > 0)
{
k = -1.0f;
}
else if (pid->target - *target < 0)
{
k = 1.0f;
}
else
{
k = 0.0f;
}
pid->target += k * sm_step;
}
}
static void smart_pid_init(smart_pid_t *pid, float *duty, float *kp, float *ki, float *kd, float *errorDead, float *iDetachCondation, float *maxOut)
{
if ((!pid) || (!duty) || (!kp) || (!ki) || (!kd) || (!iDetachCondation) || (!maxOut))
return;
pid->duty = *duty;
pid->kp = *kp;
pid->ki = *ki;
pid->kd = *kd;
pid->errorDead = *errorDead;
pid->iDetachCondation = *iDetachCondation;
pid->maxOutput = *maxOut;
}
void cascade_pid_init(cascade_pid_t *pid, smart_pid_t *pid_outer, smart_pid_t *pid_inner)
{
smooth_init(&pid->outer, pid_outer->sm_open, pid_outer->maxTarget);
smooth_init(&pid->inner, pid_inner->sm_open, pid_inner->maxTarget);
smart_pid_init(&pid->outer, &pid_outer->duty, &pid_outer->kp, &pid_outer->ki, &pid_outer->kd, &pid_outer->errorDead, &pid_outer->iDetachCondation, &pid_outer->maxOutput);
smart_pid_init(&pid->inner, &pid_inner->duty, &pid_inner->kp, &pid_inner->ki, &pid_inner->kd, &pid_inner->errorDead, &pid_inner->iDetachCondation, &pid_inner->maxOutput);
}
void smart_pid_calc(smart_pid_t *pid, float *target, float *feedback)
{
// 将旧error存起来
pid->lastError = pid->error;
// 平滑处理目标值
if (pid->sm_open == TRUE)
smooth_target(pid, target);
else
pid->target = *target;
// 计算新error
pid->error = pid->target - *feedback;
if (fabs(pid->error) <= pid->errorDead)
pid->error = 0.0f;
// 计算误差变化
pid->dError = pid->error - pid->lastError;
// 选用模糊规则
if (pid->fuzzy_open == TRUE)
{
fuzzy(pid->error, pid->dError, &pid->fuzzy_pid);
}
// 计算微分
float dout = pid->dError * (pid->kd + pid->fuzzy_pid.kd);
// 计算比例
float pout = pid->error * (pid->kp + pid->fuzzy_pid.kp);
// 积分分离
if (fabs(pid->error) <= pid->iDetachCondation)
{
pid->integral += pid->error * (pid->ki + pid->fuzzy_pid.ki);
pid->iDetach = FALSE;
}
else
{
pid->iDetach = TRUE;
pid->integral = 0;
}
// 积分限幅
if (pid->integral > pid->maxOutput)
pid->integral = pid->maxOutput;
else if (pid->integral < -pid->maxOutput)
pid->integral = -pid->maxOutput;
// 计算输出
pid->output = pout + dout + pid->integral * (!pid->iDetach);
// 输出限幅
if (pid->output > pid->maxOutput)
pid->output = pid->maxOutput;
else if (pid->output < -pid->maxOutput)
pid->output = -pid->maxOutput;
}
void cascade_pid_calc(struct CascadePID *pid, float *outerRef, float *outerFdb, float *innerFdb)
{
// 外环位置控制
smart_pid_calc(&pid->cascade_pid.outer, outerRef, outerFdb);
// 内环速度控制
smart_pid_calc(&pid->cascade_pid.inner, &pid->cascade_pid.outer.output, innerFdb);
// 内环输出就是串级PID的输出
// pid->cascade_pid.output = pid->cascade_pid.inner.output;
pid->cascade_pid.output = pid->cascade_pid.outer.output;
}
void pid_zh_constructor1(struct CascadePID *pid)
{
pid->smart_pid_init = smart_pid_init;
pid->smart_pid_calc = smart_pid_calc;
pid->cascade_pid_init = cascade_pid_init;
pid->cascade_pid_calc = cascade_pid_calc;
}
/*
典型输入信号
L[1] -> 1/s
L[t] -> 1/s^2
L[1/2t^2] -> 1/s^3
系统传递函数
G(s) = k/(TS+1)
G(s) = k/S^i(TS+1)
i = 0 0型系统 i = 1 I 型系统 I = 2 II 型系统
系统开环传递函数: G(s)
系统闭环传递函数: G闭(s) = G(s) / 1 + G(s)
系统静差: R(s) = 1/1+G(s)
终值定理limf(t) = lim s*G(s)
t->无穷 s->0
o型系统单位输入r/s
e = lim f(t) = lim s*G(s) = G(s) = k/1+G(s) = k/ 1 + (Ts+1+k)..
t->无穷 s->0 s->0 s->0 = s->0
*系统误差有给定和扰动引起的
*线性系统符合叠加原理
*动态性能指标
超调 、过度过程时间、上升时间 tr 延迟时间 td 峰值指标tp
误差积分指标大 好小 好?
典型二阶系统
G(s) = 1/ s^2 *(2ξwn)s + wn^2
ξ:阻尼系统
wn = 1/T:震荡频率
如果有一个极点和零点很接近,可以忽略该极点。
主导极点用二阶系统定义的
开环传递函数 K T
复变函数
频率特性
阻容RC电路 一阶低通滤波器 一阶惰性系统
Gs = 1/Ts+1
正弦稳态
控制系统设计依据:
考虑到被控对象包含 弹簧储能、填料阻尼、EPM气压控制等主导环节忽略其它非主导惯性、阻尼环节将被控系统模型归纳为典型二阶阻尼震荡系统
被控对象开环传递函数G(s) = wn^2/ s*( s + 2ξwn)
未加控制器被控对象闭环传递函数G(s) = wn^2/ (s^2 + (2ξwn)s + wn^2)
ξ :为系统阻尼比。跟阀座填料材、压力正相关(忽略其他非主导阻尼环节)
Wn :无阻尼自然震荡角频率。跟弹簧的固有振荡频率正相关(忽略其他非主导惯性环节)
阀杆填料阻尼比: 0 < ξ < 1
系统特征方程s^2 + (2ξwn)s + wn^2 = 0 时,有两个共轭复根,要想系统稳定,根的实部要远离虚轴,且在允许情况下,越远越稳定。
标准二阶系统是零型系统,存在静差。而且,考虑气体压缩比大,导致系统的惯性环节增大。
因此需要控制器将系统变为I型系统来消除静差增加积分环节而且需要在控制器传递函数的零点增加最小项来克服系统的大惯性增加微分环节。因此设计控制器类型为PID控制器。
※ 难点在于系统的无阻尼自然震荡角频率和阀座阻尼系数无法准确获得,整定算法计算的特性参数不一定合理。
※ 另一个难点在于EPM本身的控制特性线性度差会导致控制器的矫正环节达不到理想效果或在部分区间达不到理想效果因此
系统会不稳定或在部分区间不稳定。
※ EMP本身的控制线性度差通过控制器弥补EPM性能的不足。
因为阀杆速度变化快慢直接由EPM IP开度大小决定因此考虑增加阀杆速度环控制来弥补EPM本身的不足
算法核心原理说明 :
核心算法: PID
    u(t) = k*(e(t) +∫e(t)dt + de(t)dt)
u(n) = k*(e(n) +  ∑e(n)/Ti + Td*(e(n) - e(n-1)))
位置控制:PID + PD控制
    误差在[-5%,5%] 内 PID 控制,控制静差 < 0.3%
    误差在[-100%,5%] 或 [5%,100%] 时PD控制防止积分引起的系统不稳定
   
    积分分离:当误差<= 5% 时,加入积分
              当误差> 5%时,积分分离、积分项清零
    积分限幅:
              当积分项 >= 100%  积分项 =  100%
              当积分项 <= -100% 积分项 = -100%
整定算法: 继电反馈整定
    设定目标位置:额定行程 60%
    控制初始位置: < 60%
    施加最大控制量 ->检测实时位置->过零,计t0、上冲峰值 -> 施加最小控制量 -> 检测实时位置 -> 过零,计t1、下冲峰值— ↓
      ↑ <—————————————————————————————————————————————————————————————————————————————————————————————————|
    循环三次记录系统稳态下波峰、波谷和震荡周期Tu
    计算: Ku = (控制量最大值 - 控制量最小值)/ (位置波峰 - 位置波谷)
    查表Kp = 0.8 * Ku
          Ti = 0.6 * Tu
          Td = 0.125 * Tu
          Ki = Kp / Ti = Kp / (0.6 * Tu)
          Kd = Kp * Td = Kp * 0.125 * Td
速度控制算法:
    s1:初始位置AD值
    s2:单位时间位置移动后AD值
    v1:当前速度
    v2:  前速度
    v: 实时速度
a实时加速度
t: 速度计算周期
v = (s2 - s1) / t
a = (v2 - v1) / t
    整定过程,分别整定上升过程平均速度和下降过程平均速度。
    分别以上升和下降速度为PID 内环参考速度,计算速度控制量。
    速度偏差 = 当前速度 - 参考速度
    加速度 = 速度偏差变化(即位置的二阶导数)
速度控制量 = Kp*速度偏差  +  Ki * ∑速度偏差  + Kd * 加速度
串级控制
    外环位置检测,进行速度控制
    内环速度检测,进行位置控制
     位置偏差 = 位置给定 - 位置反馈
     位置偏差变化 = 位置偏差 - 上次位置偏差
     速度控制量 = Kp1*位置偏差  +  Ki1 * ∑位置偏差  + Kd1 * 位置偏差变化
     速度偏差 = 速度控制量 - 速度反馈
     速度偏差变化 = 速度偏差 - 上次速度偏差
     位置控制量 = Kp2*速度偏差  +  Ki2 * ∑速度偏差  + Kd2 * 速度偏差变化
串级速度整定:???
automatic control theory
control system
linear and nonlinear system
线性连续系统
离散系统
modelling
analysis
矫正
分析 时域分析方法 微分方程 传递函数 一阶系统 二阶系统
频域分析方法 闭环特性 低频特性 高频特性
根轨迹
建模 分析 矫正
离散系统
非线性控制理论
L变换 Z变换
采样控制系统 数字控制系统
条件稳定系统
problems
time consuming
行列式计算
相对稳定性
劳斯准则只能判断系统是否稳定,不能判断稳定程度
伯德图
nyquist 图
相位裕度
闭环特征方程的根闭开环增益0到无穷 特征方程根的变化规律
root locus
单位负反馈
G(s) = k/s(s+1)
复角条件复值条件
渐近线与实轴交点
出射角p-n/n-m
lambda
大纯滞后
积分对象
lambda
纯滞后 tao/t
斯密斯预估 闭环传递函数 整定方法
阶跃响应 鲁棒性 积分过程 输入干扰
执行机构偏离,不确定不准确,调整速度
PI 为主 D
两自由度PID 比例积分先行
鲁棒性 性能不完全微分
负载扰
makefile 工作原理:
1、可执行文件生成过程预处理->编译->链接
#source object target
SRC := *.c
OBJ := *.o
TARGET := mqtt_test
#compile library include
CC := cc
LIB:= -lpthread
LDFLAG := -L. libmosquitto.so
CFLAG :=
CXXFLAG :=
#link
$(TARGET):$(OBJ)
$(CC) -o $(TARGET) $(BOJ) $(LIB) $(LDFLAG)
$(CC) -o $@ $^ $(LIB) $(LDFLAG)
$(OBJ):$(SRC)
$(CC) -c $(SRC)
#all
all:$(TARGET)
$(CC) -o $(TARGET) $(SRC) $(LIB) $(LDFLAG)
#clean
clean:
rm -rf *.o $(TARGET)
*/
// ————————————————————————————————————
/*
typedef struct
{
int argc;
char **argv;
int sock;
}st_app_param;
const char *g_version = "V1.0.0 24.08.16";
int child_mian_ini()
{
int i_re;
i_re = gw_support_init();
if(i_re < 0)
{
printf("解析支持文件错误.\n");
return -1;
}
i_re = gw_config_init();
if(i_re < 0)
{
printf("解析配置文件错误.\n");
return -1;
}
i_re = gw_data_init();
if(i_re < 0)
{
printf("数据初始化错误.\n");
return -1;
}
i_re = gw_modol_init();
if(i_re < 0)
{
printf("解析模型文件错误.\n");
return -1;
}
i_re = gw_stat_init();
if(i_re < 0)
{
printf("数据统计初始化错误.\n");
return -1;
}
i_re = gw_process_init();
if(i_re < 0)
{
printf("规约初始化错误.\n");
return -1;
}
i_re = gw_manage_init();
if(i_re < 0)
{
printf("界面通讯初始化错误.\n");
return -1;
}
pthread_create(tid,thread_task1,p_param);
while(1)
{
get_sys_time();
sleep(1);
}
}
int child_main(int argc,char **argv)
{
if((argc == 2)&&(strcasecmp(argv[1],"-version") == 0))
{
printf("version [%s]",g_version);
}
child_mian_ini();
}
static int run_child_process_thread((void *)param)
{
st_app_param *p_param;
p_param = param;
chlid_main(p_parm->argc,p_param->argv);
}
static int run_child_process(int argc,char **argv,int sock)
{
int i_re;
pthread_t *tid;
st_app_param p_param;
p_param.argc = argc;
p_param.argv = argv;
p_param.sock = sock;
pthread_creat(tid,run_child_process_thread,(void *)p_param);
i_re = recv(sock,..,0);//阻塞接收
send(sock,'q',0);
}
int main(int argc,char**argv)
{
int i_re,fd,pid;
char lock_file[16];
socketpair sockpaire[2];
sprintf(lock_file,"%s.lock",argv[0]);
fd = open(lock_file,_O_WRONLY|_EXC,0777);
if(fd < 0)
{
printf("应用:[%s]已运行.\n",argv[0]);
return 0;
}
sockpaire[0] = -1;
while(1)
{
i_re = socketpaire(sockpaire,AN_UNIX,..);
if(i_re < 0)
{
printf("[%d]创建sockpaire失败.\n",getpid());
return 0;
}
pid = fork();
if(pid < 0)
{
printf("[%d]创建进程失败.\n",getpid());
close(sockpaire[0]);
sockpaire[0] = -1
close(sockpaire[1]);
return 0;
}
if(pid == 0)//child process
{
close(sockpair[0]);
return run_child_process(argc,argv,sockpair[1]);
}
printf("[%d]创建子进程成功.\n",getpid());
close(sockpair[1]);
while(1)//father process
{
i_re = recv(sockpaire[0],....,0);//阻塞接收
if(i_re <= 0)
{
//连接中断,重连
sockpaire[0] = -1;
break;
}
else
{
//正常退出
goto EXIT:
}
}
EXIT:
//退出操作
if(sockpaire[0] == -1)
{
//有子进程,关闭子进程
}
close(fd);
unlink(file_lock);
}
}
*/