如何在GCS中构造四阶代价函数？含二次项L2范数场景

你的代价函数$f(p) = ||p^T p + b^T p + c - R||_2$是凸函数，完全兼容Drake的GCS框架。由于该代价是二次标量表达式与常数的差的L2范数（对标量而言等价于绝对值），不能直接用QuadraticCost（仅适用于二次型代价），但可以通过以下两种方式构造：

方法1：直接添加符号表达式代价 #

这是最直观的方式，Drake支持将任意凸符号表达式作为代价加入规划：

import numpy as np
from pydrake.solvers import MathematicalProgram
from pydrake.symbolic import L2Norm

# 定义维度与参数
p_dim = 2
b = np.array([1.0, 2.0])
c = 3.0
R = 5.0

# 初始化规划与决策变量
prog = MathematicalProgram()
p = prog.NewContinuousVariables(p_dim, "p")

# 构造内部二次表达式
quad_expr = p.dot(p) + b.dot(p) + c
# 构造L2范数代价表达式
cost_expr = L2Norm([quad_expr - R])

# 将代价加入规划
prog.AddCost(cost_expr)

方法2：使用L2NormCost类构造 #

通过封装二次表达式向量来创建标准的L2NormCost实例：

import numpy as np
from pydrake.solvers import MathematicalProgram, L2NormCost

# 定义维度与参数
p_dim = 2
b = np.array([1.0, 2.0])
c = 3.0
R = 5.0

# 初始化规划与决策变量
prog = MathematicalProgram()
p = prog.NewContinuousVariables(p_dim, "p")

# 构造二次表达式与R的差
expr = p.dot(p) + b.dot(p) + (c - R)
# 创建L2范数代价（传入标量表达式的列表转为向量形式）
cost = L2NormCost([expr])

# 将代价加入规划
prog.AddCost(cost)

关键注意点 #

1. 凸性验证：内部二次函数$p^T p + b^T p + c$的二次项矩阵是正定单位矩阵，属于凸函数；凸函数的仿射变换（减R）仍为凸函数，L2范数是凸非递减函数，因此复合后的代价函数保持凸性，满足GCS的要求。
2. 加权调整：如果需要对代价加权，只需在表达式前乘权重即可，例如cost_expr = 0.5 * L2Norm([quad_expr - R])。
3. QuadraticCost的局限性：该类仅用于二次型代价（形如$\frac{1}{2}p^T Q p + b^T p + c$），不适用于你的L2范数包裹二次函数的场景。

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