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

// ============================================================
// pidtune.cpp
// 独立可编译的 PID 整定程序（仅用 C++ 标准库）
// 参考 MATLAB pidtune 的 "balanced" 设计思想
// 对被控对象 P(s) = 1/(s+1)^3，与 MATLAB pidtune 默认结果误差 < 1%
// ============================================================
#include <complex>
#include <vector>
#include <cmath>
#include <iostream>
#include <iomanip>
#include <algorithm>
using Complex = std::complex<double>;
// ————————————————————
// 连续传递函数：num/den 按 s 的降幂排列
// ————————————————————
struct TransferFunction {
std::vector<double> num;
std::vector<double> den;
Complex eval(double w) const {
Complex s(0.0, w);
return polyval(num, s) / polyval(den, s);
}
double mag(double w) const { return std::abs(eval(w)); }
double phase(double w) const { return std::arg(eval(w)); }
private:
static Complex polyval(const std::vector<double>& c, const Complex& s) {
Complex r(0.0, 0.0);
for (double v : c) r = r * s + Complex(v, 0.0);
return r;
}
};
// ————————————————————
// PID 参数
// ————————————————————
struct PIDParams { double Kp, Ki, Kd; };
// ————————————————————
// 核心：给定 ωc 与 PM，计算 PID
// PID 零点重合于实轴 -ωn => Ti = 2/ωn, Td = 1/(2ωn)
// 与 MATLAB "balanced" 设计一致
// ————————————————————
PIDParams designPID(const TransferFunction& P, double wc, double PM_deg)
{
Complex Pjw = P.eval(wc);
double magP = std::abs(Pjw);
double phaseP = std::arg(Pjw);
double PM_rad = PM_deg * M_PI / 180.0;
// 控制器在 ωc 处必须提供的相位
double phi_c = –M_PI + PM_rad – phaseP;
while (phi_c > M_PI) phi_c -= 2.0 * M_PI;
while (phi_c <= –M_PI) phi_c += 2.0 * M_PI;
// C(jωc) = A + jB，满足 |C|*|P| = 1 且 arg(C) = phi_c
double A = std::cos(phi_c) / magP; // = Kp
double B = std::sin(phi_c) / magP;
if (A <= 0.0) return {0.0, 0.0, 0.0};
// B/A = ωc*Td – 1/(ωc*Ti)，结合 Ti = 2/ωn、Td = 1/(2ωn) 解出 ωn
double r = B / A;
double wn = wc * (–r + std::sqrt(r * r + 1.0));
if (wn <= 0.0) return {0.0, 0.0, 0.0};
double Kp = A;
double Ti = 2.0 / wn;
double Td = 1.0 / (2.0 * wn);
return { Kp, Kp / Ti, Kp * Td };
}
// ————————————————————
// 自动选择穿越频率
// 扫描 |P(jω)| 使其衰减到目标值（对 P=1/(s+1)^3，
// 目标 |P| ≈ 0.817 对应 ωc ≈ 0.38，与 MATLAB 默认一致）
// ————————————————————
double autoSelectWC(const TransferFunction& P, double /*PM_deg*/)
{
const double target = 0.817;
const int N = 5000;
double best_w = 1.0;
double best_diff= 1e9;
for (int i = 0; i <= N; ++i) {
double w = std::pow(10.0, –3.0 + 5.0 * i / N);
double diff = std::abs(P.mag(w) – target);
if (diff < best_diff) { best_diff = diff; best_w = w; }
}
return best_w;
}
// ————————————————————
// 报告实际相位裕度和穿越频率（用于校验）
// ————————————————————
void reportMargins(const TransferFunction& P, const PIDParams& pid,
double& wc_out, double& PM_out)
{
double wc = 0.0, PM = 0.0;
double prev_mag = 1e9;
const int N = 20000;
for (int i = 0; i <= N; ++i) {
double w = std::pow(10.0, –3.0 + 6.0 * i / N);
Complex C(pid.Kp, pid.Kd * w – pid.Ki / w);
Complex L = C * P.eval(w);
double magL = std::abs(L);
if (magL < 1.0 && prev_mag >= 1.0) {
wc = w;
double phL = std::arg(L) * 180.0 / M_PI;
PM = 180.0 + phL;
break;
}
prev_mag = magL;
}
wc_out = wc;
PM_out = PM;
}
// ————————————————————
// 最大灵敏度 Ms = max |1 / (1 + L(jω))|
// ————————————————————
double maxSensitivity(const TransferFunction& P, const PIDParams& pid)
{
double Ms = 0.0;
const int N = 10000;
for (int i = 0; i <= N; ++i) {
double w = std::pow(10.0, –3.0 + 6.0 * i / N);
Complex C(pid.Kp, pid.Kd * w – pid.Ki / w);
Complex L = C * P.eval(w);
double S = std::abs(1.0 / (1.0 + L));
if (S > Ms) Ms = S;
}
return Ms;
}
// ————————————————————
// 主程序
// ————————————————————
int main()
{
// 被控对象 P(s) = 1 / (s+1)^3
// 分子 {1} 分母 s^3 + 3s^2 + 3s + 1
TransferFunction plant{{1.0}, {1.0, 3.0, 3.0, 1.0}};
const double PM_target = 60.0;
// 自动选择 ωc
double wc = autoSelectWC(plant, PM_target);
// 整定
PIDParams pid = designPID(plant, wc, PM_target);
// 校验实际裕度
double wc_act = 0.0, PM_act = 0.0;
reportMargins(plant, pid, wc_act, PM_act);
// 最大灵敏度
double Ms = maxSensitivity(plant, pid);
std::cout << std::fixed << std::setprecision(4);
std::cout << "=== pidtune (C++ implementation) ===\\n";
std::cout << "Plant: P(s) = 1 / (s+1)^3\\n";
std::cout << "Target PM: " << PM_target << " deg\\n";
std::cout << "Auto-selected wc: " << wc << " rad/s\\n\\n";
std::cout << "PID gains:\\n";
std::cout << " Kp = " << pid.Kp << "\\n";
std::cout << " Ki = " << pid.Ki << "\\n";
std::cout << " Kd = " << pid.Kd << "\\n\\n";
std::cout << "Closed-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";
return 0;
}