当前位置：首页 > news >正文

MPC控制器C语言实现：基于一阶RL系统

news 2025/9/12 6:03:32

基于被控对象为1/(Ls+R)系统，下面是将的MPC控制器移植到嵌入式系统的C语言实现。这个实现包含了核心的MPC算法，并考虑了嵌入式系统的资源限制。

#include <stdio.h>
#include <stdlib.h>
#include <math.h>// 系统参数
#define L 0.01f    // 电感 (H)
#define R 1.0f     // 电阻 (Ω)
#define TS 0.001f  // 采样时间 (s)// MPC参数
#define NP 10      // 预测步长
#define NC 3       // 控制步长
#define Q_WEIGHT 100.0f  // 输出误差权重
#define R_WEIGHT 1.0f    // 控制量权重// 约束条件
#define V_MIN -5.0f  // 最小电压
#define V_MAX 5.0f   // 最大电压
#define I_MIN -2.0f  // 最小电流
#define I_MAX 2.0f   // 最大电流// 系统离散化参数
float a, b;// 矩阵结构体（简化实现，不使用完整的矩阵运算库）
typedef struct {int rows;int cols;float* data;
} Matrix;// 初始化矩阵
Matrix matrix_init(int rows, int cols) {Matrix mat;mat.rows = rows;mat.cols = cols;mat.data = (float*)malloc(rows * cols * sizeof(float));return mat;
}// 释放矩阵内存
void matrix_free(Matrix mat) {free(mat.data);
}// 获取矩阵元素
float matrix_get(Matrix mat, int row, int col) {return mat.data[row * mat.cols + col];
}// 设置矩阵元素
void matrix_set(Matrix mat, int row, int col, float value) {mat.data[row * mat.cols + col] = value;
}// 打印矩阵（用于调试）
void matrix_print(Matrix mat, const char* name) {printf("%s (%dx%d):\n", name, mat.rows, mat.cols);for (int i = 0; i < mat.rows; i++) {for (int j = 0; j < mat.cols; j++) {printf("%8.4f ", matrix_get(mat, i, j));}printf("\n");}printf("\n");
}// 初始化系统参数
void init_system_params() {a = 1.0f - (R * TS) / L;b = TS / L;printf("System parameters: a=%.4f, b=%.4f\n", a, b);
}// 构建预测矩阵F
Matrix build_F_matrix() {Matrix F = matrix_init(NP, 1);for (int i = 0; i < NP; i++) {matrix_set(F, i, 0, powf(a, i+1));}return F;
}// 构建预测矩阵Phi
Matrix build_Phi_matrix() {Matrix Phi = matrix_init(NP, NC);for (int i = 0; i < NP; i++) {for (int j = 0; j < NC; j++) {if (i >= j) {matrix_set(Phi, i, j, powf(a, i - j) * b);} else {matrix_set(Phi, i, j, 0.0f);}}}return Phi;
}// 构建权重矩阵Q
Matrix build_Q_matrix() {Matrix Q = matrix_init(NP, NP);for (int i = 0; i < NP; i++) {for (int j = 0; j < NP; j++) {if (i == j) {matrix_set(Q, i, j, Q_WEIGHT);} else {matrix_set(Q, i, j, 0.0f);}}}return Q;
}// 构建权重矩阵R
Matrix build_R_matrix() {Matrix R_mat = matrix_init(NC, NC);for (int i = 0; i < NC; i++) {for (int j = 0; j < NC; j++) {if (i == j) {matrix_set(R_mat, i, j, R_WEIGHT);} else {matrix_set(R_mat, i, j, 0.0f);}}}return R_mat;
}// 构建Hessian矩阵 H = 2*(Phi^T * Q * Phi + R)
Matrix build_H_matrix(Matrix Phi, Matrix Q, Matrix R_mat) {// 计算 Phi^T * Q * PhiMatrix PhiT_Q = matrix_init(NC, NP);Matrix H = matrix_init(NC, NC);// 计算 Phi^T * Qfor (int i = 0; i < NC; i++) {for (int j = 0; j < NP; j++) {float sum = 0.0f;for (int k = 0; k < NP; k++) {sum += matrix_get(Phi, k, i) * matrix_get(Q, k, j);}matrix_set(PhiT_Q, i, j, sum);}}// 计算 (Phi^T * Q) * Phifor (int i = 0; i < NC; i++) {for (int j = 0; j < NC; j++) {float sum = 0.0f;for (int k = 0; k < NP; k++) {sum += matrix_get(PhiT_Q, i, k) * matrix_get(Phi, k, j);}matrix_set(H, i, j, 2.0f * (sum + matrix_get(R_mat, i, j)));}}matrix_free(PhiT_Q);return H;
}// 简化的QP求解器（使用梯度下降法，适用于嵌入式系统）
Matrix solve_qp(Matrix H, Matrix g, Matrix U_prev) {Matrix U = matrix_init(NC, 1);// 初始化控制序列（使用上一时刻的优化结果，去掉第一个元素，最后补0）for (int i = 0; i < NC; i++) {if (i < NC - 1) {matrix_set(U, i, 0, matrix_get(U_prev, i+1, 0));} else {matrix_set(U, i, 0, 0.0f);}}// 梯度下降参数float alpha = 0.1f;  // 学习率int max_iter = 50;   // 最大迭代次数float tolerance = 1e-4f; // 收敛容差// 梯度下降优化for (int iter = 0; iter < max_iter; iter++) {// 计算梯度: grad = H * U + gMatrix grad = matrix_init(NC, 1);for (int i = 0; i < NC; i++) {float sum = 0.0f;for (int j = 0; j < NC; j++) {sum += matrix_get(H, i, j) * matrix_get(U, j, 0);}matrix_set(grad, i, 0, sum + matrix_get(g, i, 0));}// 更新控制序列: U = U - alpha * gradfor (int i = 0; i < NC; i++) {float new_val = matrix_get(U, i, 0) - alpha * matrix_get(grad, i, 0);// 应用控制量约束if (new_val < V_MIN) new_val = V_MIN;if (new_val > V_MAX) new_val = V_MAX;matrix_set(U, i, 0, new_val);}// 检查收敛float grad_norm = 0.0f;for (int i = 0; i < NC; i++) {grad_norm += fabsf(matrix_get(grad, i, 0));}matrix_free(grad);if (grad_norm < tolerance) {break;}}return U;
}// MPC控制器主函数
float mpc_controller(float i_ref, float i_meas, Matrix* U_prev_ptr) {// 构建预测矩阵Matrix F = build_F_matrix();Matrix Phi = build_Phi_matrix();Matrix Q = build_Q_matrix();Matrix R_mat = build_R_matrix();// 构建Hessian矩阵Matrix H = build_H_matrix(Phi, Q, R_mat);// 构建g向量: g = 2 * (F*i_meas - Y_ref)^T * Q * PhiMatrix g = matrix_init(NC, 1);// 计算 F*i_meas - Y_refMatrix Y_error = matrix_init(NP, 1);for (int i = 0; i < NP; i++) {matrix_set(Y_error, i, 0, matrix_get(F, i, 0) * i_meas - i_ref);}// 计算 Y_error^T * QMatrix YT_Q = matrix_init(1, NP);for (int j = 0; j < NP; j++) {float sum = 0.0f;for (int i = 0; i < NP; i++) {sum += matrix_get(Y_error, i, 0) * matrix_get(Q, i, j);}matrix_set(YT_Q, 0, j, sum);}// 计算 (Y_error^T * Q) * Phifor (int j = 0; j < NC; j++) {float sum = 0.0f;for (int i = 0; i < NP; i++) {sum += matrix_get(YT_Q, 0, i) * matrix_get(Phi, i, j);}matrix_set(g, j, 0, 2.0f * sum);}// 求解QP问题Matrix U_opt = solve_qp(H, g, *U_prev_ptr);// 保存当前优化结果供下一时刻使用for (int i = 0; i < NC; i++) {matrix_set(*U_prev_ptr, i, 0, matrix_get(U_opt, i, 0));}// 提取第一个控制量float u = matrix_get(U_opt, 0, 0);// 释放内存matrix_free(F);matrix_free(Phi);matrix_free(Q);matrix_free(R_mat);matrix_free(H);matrix_free(g);matrix_free(Y_error);matrix_free(YT_Q);matrix_free(U_opt);return u;
}// 系统模型（用于仿真）
float system_model(float i_prev, float u) {return a * i_prev + b * u;
}int main() {// 初始化系统参数init_system_params();// 初始化控制序列Matrix U_prev = matrix_init(NC, 1);for (int i = 0; i < NC; i++) {matrix_set(U_prev, i, 0, 0.0f);}// 参考电流和初始电流float i_ref = 1.0f;  // 目标电流1Afloat i_meas = 0.0f; // 初始电流0A// 仿真参数int sim_steps = 100;printf("Time(s)\tReference(A)\tMeasured(A)\tControl(V)\n");printf("-------------------------------------------------\n");// 主仿真循环for (int step = 0; step < sim_steps; step++) {// 调用MPC控制器float u = mpc_controller(i_ref, i_meas, &U_prev);// 应用控制量到系统模型i_meas = system_model(i_meas, u);// 打印结果printf("%.3f\t%.3f\t\t%.3f\t\t%.3f\n", step * TS, i_ref, i_meas, u);}// 释放内存matrix_free(U_prev);return 0;
}