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

基于被控对象为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 * Phi
    Matrix PhiT_Q = matrix_init(NC, NP);
    Matrix H = matrix_init(NC, NC);
    
    // 计算 Phi^T * Q
    for (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) * Phi
    for (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 + g
        Matrix 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 * grad
        for (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 * Phi
    Matrix g = matrix_init(NC, 1);
    
    // 计算 F*i_meas - Y_ref
    Matrix 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 * Q
    Matrix 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) * Phi
    for (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;  // 目标电流1A
    float 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;
}

代码说明与嵌入式移植要点

1. 代码结构

• 系统参数：定义了RL电路的参数(L, R)和采样时间(TS)

• MPC参数：预测步长(NP)、控制步长(NC)和权重(Q_WEIGHT, R_WEIGHT)

• 约束条件：定义了电压和电流的上下限

• 矩阵操作：简化实现了基本的矩阵操作函数

• MPC核心函数：mpc_controller()实现了MPC算法

• QP求解器：使用梯度下降法实现了一个简化的QP求解器

2. 嵌入式移植考虑

内存优化

• 使用固定大小的数组而非动态内存分配（在嵌入式系统中更安全）

• 预先计算并存储不变的矩阵（如H矩阵），减少在线计算量

• 使用单精度浮点数而非双精度以节省内存和计算时间

计算效率优化

• 简化矩阵运算，避免不必要的计算

• 使用迭代求解器（如梯度下降）而非直接求逆，更适合嵌入式平台

• 限制迭代次数以确保实时性

实时性保证

• 确保最坏情况下的执行时间小于采样时间TS

• 使用定点数运算替代浮点运算（如果处理器没有FPU）

• 考虑使用更高效的QP求解算法，如Active Set方法

3. 实际部署建议

1. 性能分析：在实际硬件上分析代码执行时间，确保满足实时性要求

2. 定点化：如果处理器性能有限，考虑将算法转换为定点数实现

3. 参数整定：根据实际系统响应调整MPC参数(NP, NC, Q, R)

4. 约束处理：根据实际系统能力调整约束条件(V_MIN, V_MAX, I_MIN, I_MAX)

5. 抗饱和处理：增加积分项或抗饱和机制处理长时间约束激活情况

4. 进一步优化方向

• 使用更高效的矩阵存储方式（如稀疏矩阵）

• 采用显式MPC（离线计算最优控制律，在线查表）

• 使用专用优化库（如qpOASES、OSQP等的嵌入式版本）

• 利用处理器特定指令集优化矩阵运算

这个实现提供了一个完整的MPC控制器框架，可以直接用于嵌入式系统。根据具体的硬件平台和性能要求，可能需要进行进一步的优化和调整。