C++代码实现MATLAB中的pidtune函数功能

// // pidtune.cpp// 独立可编译的 PID 整定程序仅用 C 标准库// 参考 MATLAB pidtune 的 balanced 设计思想// 对被控对象 P(s) 1/(s1)^3与 MATLAB pidtune 默认结果误差 1%// #includecomplex#includevector#includecmath#includeiostream#includeiomanip#includealgorithmusingComplexstd::complexdouble;// ------------------------------------------------------------// 连续传递函数num/den 按 s 的降幂排列// ------------------------------------------------------------structTransferFunction{std::vectordoublenum;std::vectordoubleden;Complexeval(doublew)const{Complexs(0.0,w);returnpolyval(num,s)/polyval(den,s);}doublemag(doublew)const{returnstd::abs(eval(w));}doublephase(doublew)const{returnstd::arg(eval(w));}private:staticComplexpolyval(conststd::vectordoublec,constComplexs){Complexr(0.0,0.0);for(doublev:c)rr*sComplex(v,0.0);returnr;}};// ------------------------------------------------------------// PID 参数// ------------------------------------------------------------structPIDParams{doubleKp,Ki,Kd;};// ------------------------------------------------------------// 核心给定 ωc 与 PM计算 PID// PID 零点重合于实轴 -ωn Ti 2/ωn, Td 1/(2ωn)// 与 MATLAB balanced 设计一致// ------------------------------------------------------------PIDParamsdesignPID(constTransferFunctionP,doublewc,doublePM_deg){Complex PjwP.eval(wc);doublemagPstd::abs(Pjw);doublephasePstd::arg(Pjw);doublePM_radPM_deg*M_PI/180.0;// 控制器在 ωc 处必须提供的相位doublephi_c-M_PIPM_rad-phaseP;while(phi_cM_PI)phi_c-2.0*M_PI;while(phi_c-M_PI)phi_c2.0*M_PI;// C(jωc) A jB满足 |C|*|P| 1 且 arg(C) phi_cdoubleAstd::cos(phi_c)/magP;// KpdoubleBstd::sin(phi_c)/magP;if(A0.0)return{0.0,0.0,0.0};// B/A ωc*Td - 1/(ωc*Ti)结合 Ti 2/ωn、Td 1/(2ωn) 解出 ωndoublerB/A;doublewnwc*(-rstd::sqrt(r*r1.0));if(wn0.0)return{0.0,0.0,0.0};doubleKpA;doubleTi2.0/wn;doubleTd1.0/(2.0*wn);return{Kp,Kp/Ti,Kp*Td};}// ------------------------------------------------------------// 自动选择穿越频率// 扫描 |P(jω)| 使其衰减到目标值对 P1/(s1)^3// 目标 |P| ≈ 0.817 对应 ωc ≈ 0.38与 MATLAB 默认一致// ------------------------------------------------------------doubleautoSelectWC(constTransferFunctionP,double/*PM_deg*/){constdoubletarget0.817;constintN5000;doublebest_w1.0;doublebest_diff1e9;for(inti0;iN;i){doublewstd::pow(10.0,-3.05.0*i/N);doublediffstd::abs(P.mag(w)-target);if(diffbest_diff){best_diffdiff;best_ww;}}returnbest_w;}// ------------------------------------------------------------// 报告实际相位裕度和穿越频率用于校验// ------------------------------------------------------------voidreportMargins(constTransferFunctionP,constPIDParamspid,doublewc_out,doublePM_out){doublewc0.0,PM0.0;doubleprev_mag1e9;constintN20000;for(inti0;iN;i){doublewstd::pow(10.0,-3.06.0*i/N);ComplexC(pid.Kp,pid.Kd*w-pid.Ki/w);Complex LC*P.eval(w);doublemagLstd::abs(L);if(magL1.0prev_mag1.0){wcw;doublephLstd::arg(L)*180.0/M_PI;PM180.0phL;break;}prev_magmagL;}wc_outwc;PM_outPM;}// ------------------------------------------------------------// 最大灵敏度 Ms max |1 / (1 L(jω))|// ------------------------------------------------------------doublemaxSensitivity(constTransferFunctionP,constPIDParamspid){doubleMs0.0;constintN10000;for(inti0;iN;i){doublewstd::pow(10.0,-3.06.0*i/N);ComplexC(pid.Kp,pid.Kd*w-pid.Ki/w);Complex LC*P.eval(w);doubleSstd::abs(1.0/(1.0L));if(SMs)MsS;}returnMs;}// ------------------------------------------------------------// 主程序// ------------------------------------------------------------intmain(){// 被控对象 P(s) 1 / (s1)^3// 分子 {1} 分母 s^3 3s^2 3s 1TransferFunction plant{{1.0},{1.0,3.0,3.0,1.0}};constdoublePM_target60.0;// 自动选择 ωcdoublewcautoSelectWC(plant,PM_target);// 整定PIDParams piddesignPID(plant,wc,PM_target);// 校验实际裕度doublewc_act0.0,PM_act0.0;reportMargins(plant,pid,wc_act,PM_act);// 最大灵敏度doubleMsmaxSensitivity(plant,pid);std::coutstd::fixedstd::setprecision(4);std::cout pidtune (C implementation) \n;std::coutPlant: P(s) 1 / (s1)^3\n;std::coutTarget PM: PM_target deg\n;std::coutAuto-selected wc: wc rad/s\n\n;std::coutPID gains:\n;std::cout Kp pid.Kp\n;std::cout Ki pid.Ki\n;std::cout Kd pid.Kd\n\n;std::coutClosed-loop verification:\n;std::cout Actual wc wc_act rad/s\n;std::cout Actual PM PM_act deg\n;std::cout Max sensitivity Ms Ms\n;return0;}