使用CVXPY求解MPC二次规划时成本矩阵影响可行性的问题

问题现象 #

在实现基于CVXPY+OSQP的MPC控制器时，出现违背理论的异常：问题可行性居然受成本函数权重矩阵影响。当成本矩阵Qo设为np.diag([40, 40])时求解正常，但增大到np.diag([40000, 40000])时，求解器返回optimal inaccurate；继续增大Qo权重，直接返回infeasible。但理论上QP问题的可行性仅由约束条件决定，与成本函数无关。

代码复现 #

import numpy as np
import cvxpy as cp

ns = 2
ni = 1


def main():
    params = Parameters_Fast()
    t_end = 20.0
    t_hor = 10.0
    num_steps = int(t_end/params.dt)
 
    # Initialize Controller 
    MPC = MPCController(parameters=params)
    MPC.build_optcon_problem(t_hor=t_hor, dt=params.dt)

    # Collect vectors for plots
    xk = np.zeros((ns, num_steps))
    uk = np.zeros((ni, num_steps-1))

    Fmax = np.zeros(num_steps-1)
    Fmin = np.zeros(num_steps-1)

    # Initial conditions
    xk[0,0] = -5.0
    xk[1,0] = 1.0

    # Target point
    xk_tar = np.zeros(ns)
    xk_tar[0] = 0.0
    xk_tar[1] = 0.0

    for t in range(0, num_steps-1):

        # Find input constraints
        Fmax[t] = params.Cm1 - params.Cd * (xk[1,t]**2) - params.Croll
        Fmin[t] = -params.Cd * (xk[1,t]**2) - params.Croll

        xk_opt, uk_opt = MPC.solve_optcon_problem(xk_meas=xk[:,t], xk_tar=xk_tar, Fmax=Fmax[t], Fmin=Fmin[t])

        uk[:,t] = uk_opt[:,0]

        xk[:,t+1] = params.A @ xk[:,t] + params.B @ uk[:,t]

# Params class
class Parameters_Fast():
    dt = 0.1
        
    # Vehicle Parameters 
    m = 300   
    Cf = 10

    Cd = 2.15            
    Croll = 80              
    Cm1 = 920  


    A = np.array([[1, dt],
                  [0, 1-dt*(Cf/m)]])
    
    B = np.array([[0],
                  [dt/m]])

    Qo = np.diag([40, 40])  # 调整此矩阵会引发问题
    Ro = np.array([[1e-1]])    

# Controller class
class MPCController():
    """ MPC for steering to a set point"""

    def __init__(self, parameters) -> None:
        self.params = parameters
        self.A = self.params.A
        self.B = self.params.B
        self.Qo = self.params.Qo
        self.Ro = self.params.Ro

    def build_optcon_problem(self, t_hor, dt):
        ns = 2
        ni = 1
        num_steps = int(t_hor/dt)

        self.xk = cp.Variable((ns, num_steps))
        self.uk = cp.Variable((ni, num_steps-1))

        self.x0 = cp.Parameter(ns)
        self.xk_tar = cp.Parameter(ns)
        self.Fmax = cp.Parameter()
        self.Fmin = cp.Parameter()

        constraints = []
        cost = 0.0

        constraints += [self.xk[:,0] == self.x0]

        for t in range(0, num_steps-1):
            constraints += [self.xk[:,t+1] == self.A @ self.xk[:,t] + self.B @ self.uk[:,t]]

            # Input constraints
            constraints += [self.uk[0,t] <= self.Fmax,
                            self.uk[0,t] >= self.Fmin]
            
            # Cost function
            cost += cp.quad_form(self.xk[:,t] - self.xk_tar, self.Qo) + cp.quad_form(self.uk[:,t], self.Ro)

        # Zero terminal constraint
        constraints += [self.xk[:,-1] == self.xk_tar]

        self.prob = cp.Problem(cp.Minimize(cost), constraints)


    def solve_optcon_problem(self, xk_meas, xk_tar, Fmax, Fmin):
        """ Solves optimal control problem for given initial condition """

        self.x0.value = xk_meas
        self.xk_tar.value = xk_tar
        self.Fmax.value = Fmax
        self.Fmin.value = Fmin

        self.prob.solve(solver = cp.OSQP, warm_start=True) 
        if self.prob.status != cp.OPTIMAL:
            print(f"Error at step {t}: {self.prob.status}")

        xk_opt = self.xk.value
        uk_opt = self.uk.value

        return xk_opt, uk_opt 

if __name__ == "__main__":
    main()

问题根源分析 #

1. 硬终端约束的矛盾
   代码中添加了self.xk[:,-1] == self.xk_tar的硬约束，要求预测时域最后一步状态必须完全等于目标点。当Qo权重极大时，求解器会强制状态快速收敛，但输入约束（Fmax/Fmin）限制了最大控制力，导致给定预测时域内根本无法满足终端硬约束。小Qo时，求解器允许状态缓慢趋近，终端约束可满足；大Qo时，求解器试图强行满足快速收敛+终端约束，输入能力不足引发数值求解异常。

2. 数值条件恶化
   当Qo权重远大于Ro时，QP问题的Hessian矩阵条件数急剧增大（最大/最小特征值之比）。OSQP作为数值求解器，处理高条件数问题时易出现数值不稳定，导致求解精度下降（optimal inaccurate），甚至误判约束不可行（infeasible）。

解决方案 #

1. 替换硬终端约束为软终端成本 #

移除硬终端约束，改为在成本函数中添加大权重的终端状态惩罚项，既保证终端状态接近目标，又避免硬约束导致的不可行：

# 移除原硬终端约束：constraints += [self.xk[:,-1] == self.xk_tar]
# 改为添加终端成本
cost += cp.quad_form(self.xk[:,-1] - self.xk_tar, 100 * self.Qo)  # 权重可按需调整

2. 优化成本矩阵的数值缩放 #

确保Qo和Ro权重处于同一数量级，或对状态、输入进行归一化处理，降低Hessian矩阵条件数：

• 例如将位置（单位m）除以最大预期位置10m、速度（单位m/s）除以最大速度20m/s、输入力除以最大力Cm1，将所有变量归一化到[-1,1]区间，提升数值稳定性。

3. 调整OSQP求解参数 #

修改求解器参数，提升数值稳定性：

self.prob.solve(
    solver=cp.OSQP,
    warm_start=False,  # 关闭暖启动避免误差累积
    max_iter=10000,    # 增大最大迭代次数
    eps_abs=1e-6,      # 调整精度阈值
    eps_rel=1e-6
)

也可尝试换用cp.ECOS等其他QP求解器，验证是否为OSQP数值特性导致的问题。

4. 验证输入约束的有效性 #

确保所有时刻Fmax >= Fmin，避免输入约束上下界交叉导致真不可行：

# 计算Fmax和Fmin后添加检查
if Fmax[t] < Fmin[t]:
    print(f"Input constraint invalid at step {t}: Fmax={Fmax[t]}, Fmin={Fmin[t]}")
    # 设置默认值或调整约束
    Fmax[t] = max(Fmax[t], Fmin[t] + 1e-3)

内容的提问来源于stack exchange，提问作者Giulia Cutini