基于Gekko库的倒立摆NMPC控制器终端集与终端成本添加问询

一、终端成本优化 #

你当前代码里的final*(状态-目标)^2已经是终端成本的基础形式，但可以根据系统特性优化，让惩罚更贴合稳定需求：

1. 统一加权的基础终端成本
   给不同状态变量设置差异化权重，调整惩罚力度，比如：

   # 定义各状态的终端惩罚权重（可根据调试结果调整）
   wx, wxdot, wq1, wq1dot = 10, 5, 20, 10
   # 合并终端成本项
   m.Obj(final*(wx*(x-xf)**2 + wxdot*(xdot-xdotf)**2 + wq1*(q1-q1f)**2 + wq1dot*(q1dot-q1dotf)**2))
   m.Obj(TF)  # 保留原有的时域长度惩罚
   

2. 基于LQR的专业终端成本
   如果要严格保证系统稳定性，可先对倒立摆线性化模型求解LQR，得到Riccati方程的正定解P矩阵，用它构建终端成本：

   # 假设P是已求解的4x4正定矩阵（对应x, xdot, q1, q1dot四个状态）
   x_vec = [x, xdot, q1, q1dot]
   xf_vec = [xf, xdotf, q1f, q1dotf]
   # 构建二次型终端成本：(x-xf)^T * P * (x-xf)
   terminal_cost = sum([sum([P[i][j]*(x_vec[i]-xf_vec[i])*(x_vec[j]-xf_vec[j]) for j in range(4)]) for i in range(4)])
   m.Obj(final*terminal_cost)
   m.Obj(TF)
   

二、添加终端集约束 #

终端集是预测时域最后一步状态必须满足的约束，强制状态落在稳定区域内。在Gekko中通过final变量激活仅在最后一步生效的约束：

1. 区间型硬约束
   限制每个状态变量与目标值的偏差不超过允许阈值：

   delta = 0.1  # 稳态误差允许范围，可根据系统调整
   # 给每个状态添加终端区间约束
   m.Equation(final*(x >= xf - delta))
   m.Equation(final*(x <= xf + delta))
   m.Equation(final*(xdot >= xdotf - delta))
   m.Equation(final*(xdot <= xdotf + delta))
   m.Equation(final*(q1 >= q1f - delta))
   m.Equation(final*(q1 <= q1f + delta))
   m.Equation(final*(q1dot >= q1dotf - delta))
   m.Equation(final*(q1dot <= q1dotf + delta))
   

2. 不变集约束
   如果已推导倒立摆的正定不变集（比如基于LQR的Lyapunov函数V(x)），可直接添加约束：

   # 假设P是LQR得到的正定矩阵，V_max是不变集的阈值
   x_vec = [x, xdot, q1, q1dot]
   xf_vec = [xf, xdotf, q1f, q1dotf]
   V = sum([sum([P[i][j]*(x_vec[i]-xf_vec[i])*(x_vec[j]-xf_vec[j]) for j in range(4)]) for i in range(4)])
   m.Equation(final*(V <= V_max))
   

三、整合后的代码示例 #

把终端成本和终端集加入现有代码，完整片段如下：

# 终端成本定义
wx, wxdot, wq1, wq1dot = 10, 5, 20, 10
m.Obj(final*(wx*(x-xf)**2 + wxdot*(xdot-xdotf)**2 + wq1*(q1-q1f)**2 + wq1dot*(q1dot-q1dotf)**2))
m.Obj(TF)

# 终端集约束定义
delta = 0.1
m.Equation(final*(x >= xf - delta))
m.Equation(final*(x <= xf + delta))
m.Equation(final*(xdot >= xdotf - delta))
m.Equation(final*(xdot <= xdotf + delta))
m.Equation(final*(q1 >= q1f - delta))
m.Equation(final*(q1 <= q1f + delta))
m.Equation(final*(q1dot >= q1dotf - delta))
m.Equation(final*(q1dot <= q1dotf + delta))

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