三、MPC算法应用与仿真

1、MPC的Simulink模型

（1）Simulink模块库提供了MPC工具箱，其中有自适应MPC控制模块、显式MPC控制模块、基本MPC控制模块、多重显式MPC控制模块、多重MPC控制模块，本节主要介绍基本MPC控制模块的使用。

（2）默认的基本MPC控制模块提供了三个输入端口和一个输出端口：

①mo端口（Measured Outputs）：从被控对象传感器测量到的当前输出信号，或者状态观测器估计出的当前状态信号（如位置、速度等）。

②ref端口（References）：输入系统的设定值或参考轨迹，即希望被控量达到的目标值（如指令位置）。

③md端口（Measured Disturbances）：输入系统中可测量的前馈扰动信号，该端口是可选项，用于抑制干扰，通过在模块对话框中取消勾选“Measured disturbance(md)”可禁用该端口。（需要说明的是，此端口输入的是可计算的扰动信号，与三要素中的“反馈校正”无关）

④mv端口（Manipulated Variable）：此端口输出通过内部求解器计算出的最优操作变量，即实际作用于被控对象的控制信号。

（3）双击基本MPC控制模块，可进入参数设置页面。

①Parameters一栏可对MPC控制器的参数和初始状态进行设置。

②General选项卡主要用于配置附加的输入输出端口，如果勾选最后一条，mo端口会变为x[k|k]端口，用于输入由用户自定义状态观测器（如自己设计的卡尔曼滤波器）估计出的状态。

③Online Features选项卡主要用于配置约束条件、权重参数（即代价函数的Q、R、S和F，如下为代价函数的“完全体”，其中△u为操作变量变化率权重），勾选相应的选项，相应的参数即可从模型端口输入。

[1]输出变量权重（Output Weights）：定义了希望系统以多快的速度、多高的精度跟踪设定值。权重增加，控制器对输出误差的惩罚变大，会更加“积极地”消除误差，但可能会让控制动作更剧烈；反之，跟踪精度会下降。

[2]操作变量权重（MV Weights）：体现了对控制动作本身消耗（比如能耗、机构磨损）的重视程度。权重增加，控制器的动作会更“吝啬”、更平缓，有利于节能，但代价是可能牺牲响应速度，系统会变“迟钝”。

[3]操作变量变化率权重（MV Rate Weights）：用来抑制控制信号的剧烈跳变，也称为“阻尼项”。权重增加，控制信号变化更平滑，能有效避免超调，但如果过大，系统可能对误差反应迟钝。

④Default Conditions选项卡主要用于配置默认条件，如采样时间间隔、预测周期、系统输入信号大小及系统输出信号大小（或者说变量个数）。

（4）示例模型搭建（需要说明的是，以下例程仅仅演示如何配置MPC，在设计上并没有太多实际参考价值）：

①首先按照下图所示进行模型搭建，禁用MPC控制器的md端口（简单起见，不考虑干扰抑制）。

②被控系统就是专题——基于临界阻尼二阶系统的响应调节器中介绍的系统，直接借用即可。

（5）使用MPC Designer设计MPC控制器：

①端口及仿真参数配置：

[1]双击MPC控制器模块，点击“Design”→“MPC Structure”，在弹出的对话框中可设置MPC控制器的结构。

[2]基于上一步，点击“Change I/O Sizes”，可以设置系统输入输出参数的个数。

[3]基于第一步，还可以设置采样时间和仿真信号，这里设置采样时间为0.1s，仿真信号在模型搭建完成后会自动选取，一般无需额外手动配置。

[4]配置完成后，点击“Define and Linearize”，MPC Designer将被控对象的数学模型导入Data Browser中，如下图所示，“mpc1(current)”是系统作为内部模型创建的默认MPC控制器，“scenario1”是系统默认的模拟场景，MPC Designer在默认场景中运行默认MPC控制器，然后在右侧更新输入输出响应曲线图。（需注意，如果要更改MPC控制器的结构，修改结构后需重新打开会话窗口）

[5]点击“I/O Attributes”，在弹出的对话框中可以配置输入输出信号的名称、单位、标称值和比例因子，后两者一般保持默认值即可。

[6]双击“scenario1”，在弹出的对话框中可以配置仿真时间，以及参考信号、输出扰动和负载扰动，配置完成后点击“OK”即可。

②控制器参数配置：

[1]切换至TUNING选项卡，点击“Constraints”，可以配置输入输出约束，也即上下限。在实际控制工程中，输入输出约束可能是会动态变化的，可以在配置Online Features选项卡的参数时增加系统限制相关的输入端口。

[2]点击“Weights”，可以配置代价函数中的权重系数，不过MPC Designer的界面只能填写对角线元素（每个变量对应一个权重参数），如果希望配置非对角权重，需要从模块外部输入完整的权重矩阵。

        通常建议先确定输出权重（最主要的目标）与输入/输入变化率权重（能耗、平顺性）之间的相对比例，这个比例比设置的绝对数值更重要

        跟踪精度不足或响应慢：优先增加输出变量权重

        控制动作过猛、振荡或超调：可以增加操作变量变化率权重

        系统稳态误差大：考虑增加输出变量权重，或者检查状态估计是否准确

[3]如果实际调试过程中发现上一步的调试规则不适用，很可能是预测时域过短导致的，需要保证预测时域大于控制时域，且比控制时域长得多，比如控制时域为2步，预测时域可以设置为50步；除此之外，还需要注意采样时间不能过大、输出权重与变化率权重的比例不能失调，如果增长预测时域不管用，就需要考虑这些原因。

[4]MPC Designer提供了一个方便的功能——“Review Design”按钮，点击它，应用会自动检查当前的权重、约束等参数设置是否合理，并会给出清晰的警告或建议，快速定位问题。

③MPC控制器代入Simulink模型：

[1]点击“Store Controller”，即可将MPC控制器保存到系统的工作区内。

②右键保存的控制器，选择“Update Block”，即可将设计的控制器更新到Simulink的MPC模块中。

（6）如下为上面设计的MPC模块应用示例效果展示，当然，这只是简单的展示，实际工程中远比这复杂得多。

2、C语言实现MPC算法

（1）C语言实现MPC算法，可以包含osqp.h文件，当然，二次规划求解器库不止OSQP一个，比如还有HPIPM、qpSWIFT、HiGHS。

（2）以下为示例代码，适用于类似于小车在直线上运动的控制系统，其中mpc_set_constraints函数可以动态调整约束边界，mpc_compute函数用于求解最优控制输入。

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "osqp.h"

/* 系统参数 */
#define NX 2      /* 状态维度: [位置, 速度] */
#define NU 1      /* 控制维度: 加速度 */
#define N 5       /* 预测时域 */

/* MPC工作区 */
typedef struct {
    /* 系统矩阵 */
    double A[NX][NX];   /* 状态转移矩阵 */
    double B[NX][NU];   /* 输入矩阵 */
    double Q[NX][NX];   /* 状态代价矩阵 */
    double R[NU][NU];   /* 输入代价矩阵 */
    double Qf[NX][NX];  /* 终端代价矩阵 */
    
    /* 当前状态 */
    double x0[NX];
    double x_ref[NX];
    
    /* 动态约束边界 */
    double u_min, u_max;
    double x1_min, x1_max;   /* 位置约束 */
    double x2_min, x2_max;   /* 速度约束 */
    
    /* QP矩阵 (紧凑形式: 只优化控制序列, 状态通过动态模型传递) */
    double H[N*NU][N*NU];    /* 目标函数的Hessian矩阵 */
    double f[N*NU];             /* 目标函数的线性项 */
    double A_ineq[2*N*NX + 2*N*NU][N*NU]; /* 不等式约束矩阵 */
    double l_ineq[2*N*NX + 2*N*NU];         /* 下界 */
    double u_ineq[2*N*NX + 2*N*NU];        /* 上界 */
    int n_ineq;               /* 不等式约束数量 */
    
    /* 工作区 */
    OSQPSolver* solver;
    OSQPCscMatrix P, A;
    OSQPData data;
    OSQPSettings settings;
    
    int horizon;
} MPCWorkspace;

/* 辅助函数: 矩阵乘法 */
void mat_mul(int m, int n, int p, double* A, double* B, double* C) {
    for (int i = 0; i < m; i++) {
        for (int j = 0; j < p; j++) {
            C[i*p + j] = 0.0;
            for (int k = 0; k < n; k++) {
                C[i*p + j] += A[i*n + k] * B[k*p + j];
            }
        }
    }
}

/* 计算收缩矩阵的乘积: M * M^T */
void mat_mul_trans(int m, int n, double* A, double* C) {
    double* AT = (double*)malloc(n * m * sizeof(double));
    for (int i = 0; i < m; i++) {
        for (int j = 0; j < n; j++) {
            AT[j*m + i] = A[i*n + j];
        }
    }
    mat_mul(m, n, m, A, AT, C);
    free(AT);
}

/* 构建QP问题的Hessian矩阵和线性项
   H = B'*Q*B + R, 其中B是扩展后的控制传递矩阵
   f = B'*Q*(A*x0 - x_ref) 的修正项 */
void build_qp(MPCWorkspace* mpc) {
    int nu = NU, nx = NX, Np = mpc->horizon;
    int n_vars = nu * Np;
    
    /* 构建扩展控制传递矩阵 Phi = [B, AB, A^2B, ...] 
       用于将初始状态和输入序列映射到状态轨迹 */
    double* Phi = (double*)calloc(nx * (Np+1) * n_vars, sizeof(double));
    double* Apow = (double*)calloc((Np+1) * nx * nx, sizeof(double));
    
    /* A的幂: Apow[0] = I */
    for (int i = 0; i < nx; i++) Apow[i*nx + i] = 1.0;
    
    /* 构建Phi矩阵块 */
    double* temp = (double*)calloc(nx * nx, sizeof(double));
    for (int k = 0; k < Np; k++) {
        /* Phi的第k个块: A^k * B */
        double* block = Phi + k * nx * n_vars;
        if (k == 0) {
            /* k=0: 直接复制B */
            for (int i = 0; i < nx; i++) {
                for (int j = 0; j < nu; j++) {
                    block[i*n_vars + j] = mpc->B[i][j];
                }
            }
        } 
        else {
            /* k>0: Apow[k] * B */
            mat_mul(nx, nx, nu, Apow + k * nx * nx, (double*)mpc->B, block);
        }
        
        /* 更新A的幂: 计算A^(k+1) */
        if (k < Np-1) {
            mat_mul(nx, nx, nx, (double*)mpc->A, Apow + k * nx * nx, temp);
            memcpy(Apow + (k+1) * nx * nx, temp, nx * nx * sizeof(double));
        }
    }
    
    /* 计算 H = Phi' * Q_bar * Phi + R_bar
       其中Q_bar是扩展的状态代价矩阵（块对角，包含终端代价）
       这里简化处理，使用块对角权重 */
    double* Q_bar = (double*)calloc(nx * (Np+1) * nx * (Np+1), sizeof(double));
    for (int k = 0; k < Np; k++) {
        for (int i = 0; i < nx; i++) {
            for (int j = 0; j < nx; j++) {
                Q_bar[k*nx*nx*(Np+1) + i*nx + j] = mpc->Q[i][j];
            }
        }
    }
    for (int i = 0; i < nx; i++) {
        for (int j = 0; j < nx; j++) {
            Q_bar[Np*nx*nx*(Np+1) + i*nx + j] = mpc->Qf[i][j];
        }
    }
    
    /* 计算 Phi' * Q_bar * Phi */
    double* tmp = (double*)calloc(n_vars * nx*(Np+1), sizeof(double));
    mat_mul(nx*(Np+1), nx, n_vars, Q_bar, Phi, tmp);
    
    double H_full[20][20] = {0};
    for (int i = 0; i < n_vars; i++) {
        for (int j = 0; j < n_vars; j++) {
            for (int k = 0; k < nx*(Np+1); k++) {
                H_full[i][j] += tmp[k*n_vars + i] * Phi[k*n_vars + j];
            }
        }
    }
    
    /* 添加R_bar项 */
    double R_bar[N*NU][N*NU] = {0};
    for (int k = 0; k < Np; k++) {
        for (int i = 0; i < NU; i++) {
            R_bar[k*NU + i][k*NU + i] = mpc->R[i][i];
        }
    }
    for (int i = 0; i < n_vars; i++) {
        for (int j = 0; j < n_vars; j++) {
            H_full[i][j] += R_bar[i][j];
        }
    }
    
    /* 复制到mpc->H */
    for (int i = 0; i < n_vars; i++) {
        for (int j = 0; j < n_vars; j++) {
            mpc->H[i][j] = H_full[i][j];
        }
    }
    
    /* 计算f向量: f = (A*x0 - x_ref)' * Q * Phi */
    /* 构建预测状态偏差向量 */
    double* X_pred = (double*)calloc(nx * (Np+1), sizeof(double));
    double* x_k = (double*)calloc(nx, sizeof(double));
    memcpy(x_k, mpc->x0, nx * sizeof(double));
    
    /* 计算状态轨迹（无控制输入时） */
    for (int k = 0; k <= Np; k++) {
        memcpy(X_pred + k*nx, x_k, nx * sizeof(double));
        /* 将x_k减去参考值 */
        for (int i = 0; i < nx; i++) {
            X_pred[k*nx + i] -= mpc->x_ref[i];
        }
        /* 更新x_{k+1} = A * x_k */
        double* x_next = (double*)calloc(nx, sizeof(double));
        for (int i = 0; i < nx; i++) {
            for (int j = 0; j < nx; j++) {
                x_next[i] += mpc->A[i][j] * x_k[j];
            }
        }
        memcpy(x_k, x_next, nx * sizeof(double));
        free(x_next);
    }
    
    /* f = x0偏差 * Q_bar * Phi */
    double tmpf[20] = {0};
    mat_mul(1, nx*(Np+1), n_vars, X_pred, tmp, tmpf);
    for (int i = 0; i < n_vars; i++) {
        mpc->f[i] = tmpf[i];
    }
    
    free(Phi);
    free(Apow);
    free(temp);
    free(Q_bar);
    free(tmp);
    free(X_pred);
    free(x_k);
}

/* 构建动态约束（状态和控制边界） */
void build_constraints(MPCWorkspace* mpc) {
    int nu = NU, nx = NX, Np = mpc->horizon;
    int n_vars = nu * Np;
    int n_ineq = 2 * Np * nx + 2 * Np * nu;  /* 每个控制有上下界，每个状态有上下界 */
    mpc->n_ineq = n_ineq;
    
    /* 构建扩展控制传递矩阵Phi（用于状态预测） */
    double* Phi = (double*)calloc(nx * (Np+1) * n_vars, sizeof(double));
    double* Apow = (double*)calloc((Np+1) * nx * nx, sizeof(double));
    
    for (int i = 0; i < nx; i++) Apow[i*nx + i] = 1.0;
    
    double* temp = (double*)calloc(nx * nx, sizeof(double));
    for (int k = 0; k < Np; k++) {
        double* block = Phi + k * nx * n_vars;
        if (k == 0) {
            for (int i = 0; i < nx; i++) {
                for (int j = 0; j < nu; j++) {
                    block[i*n_vars + j] = mpc->B[i][j];
                }
            }
        } 
        else {
            mat_mul(nx, nx, nu, Apow + k * nx * nx, (double*)mpc->B, block);
        }
        if (k < Np-1) {
            mat_mul(nx, nx, nx, (double*)mpc->A, Apow + k * nx * nx, temp);
            memcpy(Apow + (k+1) * nx * nx, temp, nx * nx * sizeof(double));
        }
    }
    
    /* 构建状态自由响应轨迹 */
    double* X_free = (double*)calloc(nx * (Np+1), sizeof(double));
    double* x_k = (double*)calloc(nx, sizeof(double));
    memcpy(x_k, mpc->x0, nx * sizeof(double));
    
    for (int k = 0; k <= Np; k++) {
        memcpy(X_free + k*nx, x_k, nx * sizeof(double));
        double* x_next = (double*)calloc(nx, sizeof(double));
        for (int i = 0; i < nx; i++) {
            for (int j = 0; j < nx; j++) {
                x_next[i] += mpc->A[i][j] * x_k[j];
            }
        }
        memcpy(x_k, x_next, nx * sizeof(double));
        free(x_next);
    }
    
    /* 构建不等式约束: X_free + Phi * u 必须在边界内 */
    for (int k = 0; k < Np; k++) {
        /* 状态约束: x_min ≤ x_k ≤ x_max */
        double* row_low = mpc->l_ineq + 2*k*nx;
        double* row_up = mpc->u_ineq + 2*k*nx;
        for (int i = 0; i < nx; i++) {
            row_low[i] = -(mpc->x1_max);  /* 简化：只约束第一维（位置） */
            row_low[nx + i] = -1e20; 
            row_up[i] = -mpc->x1_min;
            row_up[nx + i] = 1e20;
        }
        
        /* 不等式约束的系数矩阵：C_ineq * u = -X_free + bound */
        for (int i = 0; i < nx; i++) {
            for (int j = 0; j < n_vars; j++) {
                mpc->A_ineq[2*k*nx + i][j] = Phi[k*nx*n_vars + i*n_vars + j];
                mpc->A_ineq[2*k*nx + nx + i][j] = -Phi[k*nx*n_vars + i*n_vars + j];
            }
            mpc->l_ineq[2*k*nx + i] += (mpc->x1_max - X_free[k*nx + i]);
            mpc->u_ineq[2*k*nx + i] += (mpc->x1_min - X_free[k*nx + i]);
            mpc->l_ineq[2*k*nx + nx + i] += -(mpc->x1_max - X_free[k*nx + i]);
            mpc->u_ineq[2*k*nx + nx + i] += -(mpc->x1_min - X_free[k*nx + i]);
        }
        
        /* 控制输入约束: u_min ≤ u_k ≤ u_max */
        for (int i = 0; i < nu; i++) {
            mpc->l_ineq[2*Np*nx + 2*k*nu + i] = mpc->u_min;
            mpc->u_ineq[2*Np*nx + 2*k*nu + i] = mpc->u_max;
            mpc->l_ineq[2*Np*nx + 2*k*nu + nu + i] = -mpc->u_max;
            mpc->u_ineq[2*Np*nx + 2*k*nu + nu + i] = -mpc->u_min;
            
            for (int j = 0; j < n_vars; j++) {
                mpc->A_ineq[2*Np*nx + 2*k*nu + i][j] = (j == k*nu + i) ? 1.0 : 0.0;
                mpc->A_ineq[2*Np*nx + 2*k*nu + nu + i][j] = (j == k*nu + i) ? -1.0 : 0.0;
            }
        }
    }
    
    free(Phi);
    free(Apow);
    free(temp);
    free(X_free);
    free(x_k);
}

/* 初始化MPC工作区 */
MPCWorkspace* mpc_init(double dt, double Q_diag[2], double R_diag[1], double Qf_diag[2]) {
    MPCWorkspace* mpc = (MPCWorkspace*)malloc(sizeof(MPCWorkspace));
    memset(mpc, 0, sizeof(MPCWorkspace));
    
    mpc->horizon = N;
    
    /* 初始化系统矩阵 (双积分器模型) */
    mpc->A[0][0] = 1.0; mpc->A[0][1] = dt;
    mpc->A[1][0] = 0.0; mpc->A[1][1] = 1.0;
    
    mpc->B[0][0] = 0.5 * dt * dt;
    mpc->B[1][0] = dt;
    
    /* 初始化代价矩阵 */
    mpc->Q[0][0] = Q_diag[0]; mpc->Q[0][1] = 0.0;
    mpc->Q[1][0] = 0.0; mpc->Q[1][1] = Q_diag[1];
    
    mpc->R[0][0] = R_diag[0];
    
    mpc->Qf[0][0] = Qf_diag[0]; mpc->Qf[0][1] = 0.0;
    mpc->Qf[1][0] = 0.0; mpc->Qf[1][1] = Qf_diag[1];
    
    /* 默认动态约束边界 */
    mpc->u_min = -1.0; mpc->u_max = 1.0;
    mpc->x1_min = -10.0; mpc->x1_max = 10.0;
    mpc->x2_min = -5.0; mpc->x2_max = 5.0;
    
    /* OSQP求解器设置 */
    osqp_set_default_settings(&mpc->settings);
    mpc->settings.verbose = 0;
    mpc->settings.polish = 1;
    mpc->settings.scaling = 10;
    mpc->settings.max_iter = 200;
    mpc->settings.eps_abs = 1e-4;
    mpc->settings.eps_rel = 1e-4;
    
    return mpc;
}

/* 更新动态约束边界 */
void mpc_set_constraints(MPCWorkspace* mpc, double u_min, double u_max, 
                         double x1_min, double x1_max) {
    mpc->u_min = u_min;
    mpc->u_max = u_max;
    mpc->x1_min = x1_min;
    mpc->x1_max = x1_max;
}

/* 执行MPC计算，返回当前控制量 */
double mpc_compute(MPCWorkspace* mpc, double* state, double* ref) {
    /* 更新当前状态和参考值 */
    memcpy(mpc->x0, state, NX * sizeof(double));
    memcpy(mpc->x_ref, ref, NX * sizeof(double));
    
    /* 构建QP问题 */
    build_qp(mpc);
    build_constraints(mpc);
    
    int n_vars = NU * mpc->horizon;
    
    /* 将Hessian矩阵转换为稀疏格式 */
    int nnz_P = 0;
    for (int i = 0; i < n_vars; i++) {
        for (int j = 0; j < n_vars; j++) {
            if (fabs(mpc->H[i][j]) > 1e-10) nnz_P++;
        }
    }
    double* P_x = (double*)malloc(nnz_P * sizeof(double));
    OSQPInt* P_i = (OSQPInt*)malloc(nnz_P * sizeof(OSQPInt));
    OSQPInt* P_p = (OSQPInt*)malloc((n_vars+1) * sizeof(OSQPInt));
    int idx = 0;
    P_p[0] = 0;
    for (int j = 0; j < n_vars; j++) {
        for (int i = 0; i < n_vars; i++) {
            if (fabs(mpc->H[i][j]) > 1e-10) {
                P_x[idx] = mpc->H[i][j];
                P_i[idx] = i;
                idx++;
            }
        }
        P_p[j+1] = idx;
    }
    
    /* 将约束矩阵转换为稀疏格式 */
    int n_ineq = mpc->n_ineq;
    double* A_x = (double*)malloc(n_vars * n_ineq * sizeof(double));
    OSQPInt* A_i = (OSQPInt*)malloc(n_vars * n_ineq * sizeof(OSQPInt));
    OSQPInt* A_p = (OSQPInt*)malloc((n_vars+1) * sizeof(OSQPInt));
    idx = 0;
    A_p[0] = 0;
    for (int j = 0; j < n_vars; j++) {
        for (int i = 0; i < n_ineq; i++) {
            if (fabs(mpc->A_ineq[i][j]) > 1e-10) {
                A_x[idx] = mpc->A_ineq[i][j];
                A_i[idx] = i;
                idx++;
            }
        }
        A_p[j+1] = idx;
    }
    
    /* 设置OSQP数据 */
    mpc->data.n = n_vars;
    mpc->data.m = n_ineq;
    mpc->data.P = osqp_csc_matrix(n_vars, n_vars, nnz_P, P_x, P_i, P_p);
    mpc->data.q = mpc->f;
    mpc->data.A = osqp_csc_matrix(n_ineq, n_vars, idx, A_x, A_i, A_p);
    mpc->data.l = mpc->l_ineq;
    mpc->data.u = mpc->u_ineq;
    
    /* 求解QP问题 */
    if (mpc->solver) osqp_cleanup(mpc->solver);
    osqp_setup(&mpc->solver, &mpc->data, &mpc->settings);
    osqp_solve(mpc->solver);
    
    /* 获取最优控制量 */
    double u_opt = mpc->solver->solution->x[0];
    
    /* 清理动态分配的数组 */
    free(P_x); free(P_i); free(P_p);
    free(A_x); free(A_i); free(A_p);
    
    return u_opt;
}

/* 清理MPC工作区 */
void mpc_cleanup(MPCWorkspace* mpc) {
    if (mpc->solver) osqp_cleanup(mpc->solver);
    free(mpc);
}

/* 主函数示例：仿真闭环控制过程 */
int main() {
    /* 初始化系统状态和参考值 */
    double state[NX] = {0.0, 0.0};   /* 初始位置和速度 */
    double ref[NX] = {5.0, 0.0};     /* 目标位置和速度 */
    double dt = 0.05;                 /* 采样时间 */
    
    /* 代价矩阵权重 */
    double Q_diag[2] = {10.0, 1.0};    /* 位置误差权重10，速度误差权重1 */
    double R_diag[1] = {0.1};          /* 控制量权重0.1 */
    double Qf_diag[2] = {100.0, 10.0}; /* 终端代价权重更高，保证收敛 */
    
    /* 初始化MPC控制器 */
    MPCWorkspace* mpc = mpc_init(dt, Q_diag, R_diag, Qf_diag);
    
    /* 初始约束边界 */
    mpc_set_constraints(mpc, -1.0, 1.0, -8.0, 8.0);
    
    /* 仿真循环 */
    for (int step = 0; step < 200; step++) {
        /* 根据仿真进程动态调整约束边界 */
        if (step == 50) {
            /* 第50步后收紧控制约束 */
            mpc_set_constraints(mpc, -0.8, 0.8, -6.0, 6.0);
            printf("--- 约束收紧: u in [-0.8, 0.8], pos in [-6.0, 6.0] ---\n");
        } else if (step == 120) {
            /* 第120步后放松约束以更快收敛 */
            mpc_set_constraints(mpc, -1.5, 1.5, -10.0, 10.0);
            printf("--- 约束放松: u in [-1.5, 1.5], pos in [-10.0, 10.0] ---\n");
        }
        
        /* 计算最优控制量 */
        double u = mpc_compute(mpc, state, ref);
        
        /* 应用控制量，更新系统状态（离散化双积分器模型） */
        double x1_next = state[0] + dt * state[1] + 0.5 * dt * dt * u;
        double x2_next = state[1] + dt * u;
        state[0] = x1_next;
        state[1] = x2_next;
        
        /* 每10步打印一次信息 */
        if (step % 10 == 0) {
            printf("step %3d: pos=%.3f, vel=%.3f, u=%.3f\n", 
                   step, state[0], state[1], u);
        }
        
        /* 收敛检查 */
        if (fabs(state[0] - ref[0]) < 0.01 && fabs(state[1] - ref[1]) < 0.01) {
            printf("Converged at step %d\n", step);
            break;
        }
    }
    
    /* 清理资源 */
    mpc_cleanup(mpc);
    return 0;
}