RBF_PID_CPP

#include <iostream>#include <vector>#include <math.h>using namespace std;double eta=0.5;//学习率double alfa=0.05;//动量因子double x[3]={0};//输入向量xdouble ci[6][3]={0};//中心矢量Cdouble bi[6];//基宽向量Bdouble w[6];//权值向量Wdouble h[6]={0};//径向基向量hdouble ci_1[6][3],ci_2[6][3],ci_3[6][3];double bi_1[6],bi_2[6],bi_3[6];double w_1[6],w_2[6],w_3[6];double u,u_1,y,y_1;double xc[3]={0};//增量式PID的输入double error,error_1,error_2;double kp0=0.01,ki0=0.01,kd0=0.01;//初始P、I、Ddouble kp_1=kp0,ki_1=ki0,kd_1=kd0;double etaP=0.15,etaI=0.15,etaD=0.15;//P,I,D系数调整速度double kp,ki,kd;//vector<double> yd;double caculateNorm2(double * tx,int count)//计算矩阵2-范数{double norm2=0;for(int i=0;i<count;i++){norm2+=pow(tx[i],2);}return norm2;}double caculateHj(double* tx,double * tc,int count,double tbij)//计算h[j]{double norm2=0,hj=0;for(int i=0;i<count;i++){norm2+=pow(tx[i]-tc[i],2);}//hj=exp(-norm2/(2*pow(tbj,2)));return exp(-norm2/(2*pow(tbij,2)));}double caculateYpY(double * tx,double* ty,int count)//辨识器网络输出{double re=0;for (int i=0;i<count;i++){re+=tx[i]*ty[i];}return re;}void initPara()//初始化参数{for (int i=0;i<6;i++){bi[i]=10;w[i]=0.1;}memcpy(ci_1,ci,18*sizeof(double));memcpy(ci_2,ci,18*sizeof(double));memcpy(ci_3,ci,18*sizeof(double));memcpy(w_1,w,6*sizeof(double));memcpy(w_2,w,6*sizeof(double));memcpy(w_3,w,6*sizeof(double));memcpy(bi_1,bi,6*sizeof(double));memcpy(bi_2,bi,6*sizeof(double));memcpy(bi_3,bi,6*sizeof(double));u_1=0;y_1=0;}void updatePID(double yd,double ty)//更新PID{//double ts=0.01;//yd.push_back(sin(time*ts)*cos(2*time*ts)); //double yd=sin(time*ts)*cos(2*time*ts);//double ty=(-0.1*y_1+u_1)/(1+y_1*y_1); for(int j=0;j<6;j++){h[j]=caculateHj(x,ci[j],3,bi[j]);}double ym=caculateYpY(w,h,6);double d_w[6]={0};for (int j=0;j<6;j++){d_w[j]=eta*(ty-ym)*h[j];}for (int i=0;i<6;i++){w[i]=w_1[i]+d_w[i]+alfa*(w_1-w_2);}double d_bi[6]={0};for (int j=0;j<6;j++){double x_cij[3];for(int i=0;i<3;i++){x_cij[i]=x[i]-ci[j][i];}d_bi[j]=eta*(ty-ym)*w[j]*h[j]*pow(bi[j],-3)*caculateNorm2(x_cij,3);}for (int i=0;i<6;i++){bi[i]=bi_1[i]+d_bi[i]+alfa*(bi_1[i]-bi_2[i]);}double d_ci[6][3]={0};for (int j=0;j<6;j++){for (int i=0;i<3;i++){d_ci[j][i]=eta*(y-ym)*w[j]*h[j]*(x[i]-ci[j][i])*pow(bi[j],-2);}}for (int i=0;i<3;i++){for (int j=0;j<6;j++){ci[j][i]=ci_1[j][i]+d_ci[j][i]+alfa*(ci_1[j][i]-ci_2[j][i]);}}double yu=0;for (int j=0;j<6;j++){yu+=w[j]*h[j]*(ci[j][1]-x[1])/pow(bi[j],2);}double dyu=yu;error=yd-ty;kp=kp_1+etaP*error*dyu*xc[1];kd=kd_1+etaD*error*dyu*xc[2];kp=ki_1+etaI*error*dyu*xc[3];if (kp<0){kp=0;}if (ki<0){ki=0;}if (kd<0){kd=0;}//double du=kp*xc[1]+kd*xc[2]+ki*xc[3];}void main(){initPara();double ts=0.01;for(int i=0;i<2000;i++){double time=i*ts;double yd=sin(time)*cos(2*time);y=(-0.1*y_1+u_1)/(1+y_1*y_1);updatePID(yd,y);cout<<"第"<<i<<"次迭代误差："<<error<<endl;double du=kp*xc[1]+kd*xc[2]+ki*xc[3];u=u_1+du;if (i==300){u=u+0.5;}x[1]=du;x[2]=y;x[3]=y_1;u_1=u;y_1=y;for (int j=0;j<6;j++){for(int i=0;i<3;i++){ci_2[j][i]=ci_1[j][i];ci_1[j][i]=ci[j][i];}bi_2[j]=bi_1[j];bi_1[j]=bi[j];w_2[j]=w_1[j];w_1[j]=w[j];}xc[1]=error-error_1;xc[2]=error-2*error_1+error_2;xc[3]=error;error_2=error_1;error_1=error;kp_1=kp;kd_1=kd;ki_1=ki;}cin.get();}