本文旨在深入探讨acados中非线性成本函数的实现方法，重点介绍`nonlinear_ls`和`external`两种成本类型。我们将通过一个移动机器人模型的案例，详细阐述如何利用casadi表达式定义轨迹跟踪和避障等复杂非线性成本，并结合acados优化求解器进行配置，为实时控制器开发提供专业指导。
1. ACADOS中的成本函数概述
在模型预测控制（MPC）问题中，成本函数是核心组成部分，它量化了系统状态和控制输入与期望目标之间的偏差。ACADOS作为一款高性能的MPC求解器，提供了灵活的机制来定义各种类型的成本函数，包括线性和非线性形式。对于需要处理复杂系统行为（如轨迹跟踪误差、避障、能量消耗等）的MPC问题，非线性成本函数的正确实现至关重要。
ACADOS主要通过两种方式支持非线性成本：非线性最小二乘 (NONLINEAR_LS) 和 外部成本 (EXTERNAL)。选择哪种类型取决于成本函数的具体数学结构以及对求解器性能（如Hessian矩阵计算）的要求。
2. 移动机器人模型定义
首先，我们定义一个简单的移动机器人模型，它将作为我们配置成本函数的基础。该模型描述了机器人在二维平面上的位置 (x, y)、线速度 v 和航向角 theta。控制输入包括线加速度 a 和角速度 w。
import casadi as ca
import numpy as np
from acados_template import AcadosModel, AcadosOcp, AcadosOcpOptions
def mobile_robot_model():
"""
定义一个简单的移动机器人模型。
返回:
model (AcadosModel): 包含机器人动力学定义的ACADOS模型对象。
"""
model_name = 'mobile_robot'
# 定义符号变量 (状态)
x = ca.MX.sym('x')
y = ca.MX.sym('y')
v = ca.MX.sym('v')
theta = ca.MX.sym('theta')
# 控制输入
a = ca.MX.sym('a') # 加速度
w = ca.MX.sym('w') # 角速度
# 定义状态和控制向量
states = ca.vertcat(x, y, v, theta)
controls = ca.vertcat(a, w)
# 定义连续时间动力学
rhs = [v * ca.cos(theta), v * ca.sin(theta), a, w]
x_dot = ca.MX.sym('x_dot', len(rhs)) # 状态导数符号变量
# 创建CasADi函数表示连续时间动力学
continuous_dynamics = ca.Function(
'continuous_dynamics',
[states, controls],
[ca.vcat(rhs)],
["state", "control_input"],
["rhs"]
)
# ACADOS模型设置
model = AcadosModel()
model.f_expl_expr = continuous_dynamics(states, controls) # 显式动力学
model.x = states
model.xdot = x_dot
model.u = controls
model.p = [] # 无外部参数
model.name = model_name
# 对于隐式动力学，需要定义 f_impl_expr
# f_impl = x_dot - continuous_dynamics(states, controls)
# model.f_impl_expr = f_impl
# 如果使用显式形式，通常不需要 f_impl_expr，或者可以简单设置为 x_dot - f_expl_expr
return model3. 配置ACADOS OCP求解器基础
在定义成本函数之前，我们需要初始化ACADOS OCP (Optimal Control Problem) 对象并设置基本的维度、时间步长和初始条件。
def create_ocp_solver():
# 创建AcadosOcp对象
ocp = AcadosOcp()
# 设置优化问题
model = mobile_robot_model()
ocp.model = model
# --------------------参数设置--------------
nx = model.x.size()[0] # 状态维度
nu = model.u.size()[0] # 控制输入维度
N = 100 # 预测步长
T = 30 # 预测时间
ocp.dims.N = N
ocp.dims.nx = nx
ocp.dims.nu = nu
ocp.solver_options.tf = T
# 初始状态
x0 = np.array([0.0, 0.0, 0.0, 0.0]) # 初始位置 (0,0), 速度0, 角度0
ocp.constraints.x0 = x0
# 初始化参数 (如果模型有参数)
ocp.dims.np = len(model.p)
ocp.parameter_values = np.zeros(ocp.dims.np)
# ---------------------约束设置------------------
ocp.constraints.lbu = np.array([-0.1, -0.3]) # 控制输入下界 [a_min, w_min]
ocp.constraints.ubu = np.array([0.1, 0.3]) # 控制输入上界 [a_max, w_max]
ocp.constraints.idxbu = np.array([0, 1]) # 对应控制输入的索引
# 状态约束 (示例，如果需要)
# ocp.constraints.lx = np.array([-100, -100, 0, -np.pi]) # 状态下界
# ocp.constraints.ux = np.array([100, 100, 1, np.pi]) # 状态上界
# ocp.constraints.idxbx = np.array([0, 1, 2, 3]) # 对应状态的索引
return ocp, model4. 实现非线性成本函数
现在我们来详细讨论如何实现两种主要的非线性成本函数。
4.1 非线性最小二乘成本 (NONLINEAR_LS)
当成本函数可以表示为残差向量的L2范数平方（即 $\sum ||y(x, u) - y_{ref}||_W^2$）时，应使用 NONLINEAR_LS 类型。ACADOS能够利用这种结构来计算Gauss-Newton Hessian，从而提高求解效率。
结构:
- ocp.cost.cost_type / ocp.cost.cost_type_e: 设置为 'NONLINEAR_LS'。
- model.cost_y_expr / model.cost_y_expr_e: 定义残差向量 $y(x, u)$ 的CasADi表达式。对于中间阶段，cost_y_expr 通常是状态和控制输入的函数；对于终端阶段，cost_y_expr_e 通常只依赖于状态。
- ocp.cost.yref / ocp.cost.yref_e: 定义参考向量 $y_{ref}$。这些值可以在求解器创建后动态更新。
- ocp.cost.W / ocp.cost.W_e: 定义权重矩阵，用于加权残差。
示例：轨迹跟踪成本
假设我们要最小化机器人轨迹与期望参考轨迹之间的误差，以及控制输入的偏差。成本函数形式为： $J = \sum_{k=0}^{N-1} (||qk - q{ref,k}||_{W_q}^2 + ||uk - u{ref,k}||_{W_u}^2) + ||qN - q{ref,N}||{W{q,e}}^2$ 其中 $q_k$ 是状态向量， $uk$ 是控制向量。 $q{ref,k}$ 和 $u_{ref,k}$ 是参考轨迹和控制输入。
def add_nonlinear_ls_cost(ocp, model):
nx = model.x.size()[0]
nu = model.u.size()[0]
# 定义NONLINEAR_LS的维度
# 这里我们将状态和控制输入连接起来作为残差向量
ny = nx + nu # 中间阶段的残差向量维度
ny_e = nx # 终端阶段的残差向量维度 (只考虑状态)
ocp.dims.ny = ny
ocp.dims.ny_e = ny_e
# 设置中间阶段成本类型为NONLINEAR_LS
ocp.cost.cost_type = 'NONLINEAR_LS'
ocp.cost.W = np.diag([100, 100, 10, 10, 1, 1]) # 权重矩阵，对应 [x, y, v, theta, a, w]
# 设置终端阶段成本类型为NONLINEAR_LS
ocp.cost.cost_type_e = 'NONLINEAR_LS'
ocp.cost.W_e = np.diag([1000, 1000, 100, 100]) # 终端权重矩阵，对应 [x, y, v, theta]
# 定义CasADi表达式 y(x,u)
# 对于中间阶段，残差是 (状态 - 参考状态) 和 (控制 - 参考控制)
# 假设参考控制输入为零，参考状态为 x_ref, y_ref, v_ref, theta_ref
x_ref_sym = ca.MX.sym('x_ref_sym', nx)
u_ref_sym = ca.MX.sym('u_ref_sym', nu)
# model.cost_y_expr 定义了残差向量 [x-x_ref, y-y_ref, v-v_ref, theta-theta_ref, a-a_ref, w-w_ref]
model.cost_y_expr = ca.vertcat(model.x - x_ref_sym, model.u - u_ref_sym)
# 对于终端阶段，残差只考虑状态
model.cost_y_expr_e = model.x - x_ref_sym
# 设置参考值 yref
# 这些值可以在求解器创建后通过 solver.set(stage, 'yref', new_yref) 动态更新
ocp.cost.yref = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # 默认参考值，例如跟踪原点，控制输入为0
ocp.cost.yref_e = np.array([0.0, 0.0, 0.0, 0.0]) # 终端默认参考值
# 也可以将参考值定义为参数，以便在每次求解前更新
# ocp.model.p 列表中可以添加参考值作为参数
# 如果将参考值作为参数，则 cost_y_expr 应该直接使用这些参数
# 例如：model.cost_y_expr = ca.vertcat(model.x - model.p[:nx], model.u - model.p[nx:])
# 此时 ocp.cost.yref 将不再直接使用，而是通过更新 ocp.parameter_values 来传递参考值
print("NONLINEAR_LS 成本函数已配置。")
return ocp, model4.2 外部成本 (EXTERNAL)
当成本函数的形式非常通用，无法直接表示为最小二乘形式，或者您希望完全自定义Hessian矩阵的计算方式时，可以使用 EXTERNAL 成本类型。这种方式提供了最大的灵活性。
结构:
- ocp.cost.cost_type / ocp.cost.cost_type_e: 设置为 'EXTERNAL'。
- model.cost_expr_ext_cost / model.cost_expr_ext_cost_e: 定义一个标量CasADi表达式，表示中间阶段或终端阶段的外部成本。
示例：避障成本
假设我们要添加一个避障成本，当机器人靠近某个圆形障碍物时，成本急剧增加。例如，障碍物中心在 (ox, oy)，半径为 r_obs。机器人与障碍物中心的距离为 $d = \sqrt{(x-ox)^2 + (y-oy)^2}$。我们可以定义一个成本，当 $d < r_{safe}$ 时惩罚。
def add_external_cost(ocp, model):
# 设置中间阶段成本类型为EXTERNAL
ocp.cost.cost_type = 'EXTERNAL'
# 定义避障成本
# 假设障碍物中心 (ox, oy) 和安全距离 r_safe 为参数
ox = ca.MX.sym('ox')
oy = ca.MX.sym('oy')
r_safe = ca.MX.sym('r_safe')
# 将障碍物参数添加到模型参数中
model.p = ca.vertcat(model.p, ox, oy, r_safe)
ocp.dims.np = len(model.p)
# 机器人当前位置
x_robot = model.x[0]
y_robot = model.x[1]
# 机器人与障碍物中心的距离平方
dist_sq = (x_robot - ox)**2 + (y_robot - oy)**2
# 定义一个平滑的惩罚函数，当距离小于安全距离时惩罚
# 这里使用一个简单的Sigmoid或指数形式来模拟，避免不连续性
# 例如：cost = C * exp(-k * (dist_sq - r_safe^2))，当 dist_sq < r_safe^2 时
# 更常见的是使用 max(0, r_safe - dist) 的平方，并进行平滑处理
# 简化示例：当距离小于r_safe时，成本增加
# 使用 CasADi 的 if_else 或 smooth_max
# penalty = ca.if_else(dist_sq < r_safe**2, 1000 * (r_safe**2 - dist_sq), 0)
# 更平滑的避障成本，例如 inverse distance squared
# 避免分母为零，添加一个小的epsilon
epsilon = 1e-6
penalty = 1000 * ca.exp(-10 * (dist_sq - r_safe**2)) # 示例形式，可根据需要调整
# 如果需要，也可以将控制输入的惩罚加到外部成本中
# penalty += 0.1 * ca.sumsqr(model.u)
model.cost_expr_ext_cost = penalty
# 终端阶段成本 (如果需要，也可以定义外部终端成本)
ocp.cost.cost_type_e = 'EXTERNAL'
model.cost_expr_ext_cost_e = 0.0 # 终端没有避障成本，或者可以定义一个状态相关的成本
# 设置参数的初始值
ocp.parameter_values = np.array([0.0, 0.0, 0.0, 2.0, 0.5]) # 假设 [ox, oy, r_safe] 初始值
# 注意：这里假设 model.p 只有 ox, oy, r_safe
# 如果之前已经有其他参数，需要合并
print("EXTERNAL 成本函数已配置。")
return ocp, model5. 综合配置示例
现在我们将上述成本函数配置集成到 create_ocp_solver 函数中。您可以根据需要选择使用 NONLINEAR_LS 或 EXTERNAL，或者两者结合。
def create_ocp_solver_with_costs():
ocp, model = create_ocp_solver()
# --- 选择并配置成本函数 ---
# 方案1: 使用 NONLINEAR_LS 进行轨迹跟踪
# ocp, model = add_nonlinear_ls_cost(ocp, model)
# 方案2: 使用 EXTERNAL 进行避障 (并可包含其他自定义成本)
# ocp, model = add_external_cost(ocp, model)
# 方案3: 混合使用 (例如，中间阶段用 NONLINEAR_LS 跟踪，终端阶段用 EXTERNAL 复杂惩罚)
# 或者如果只需要一种类型，则只配置一种。
# 这里以同时配置两种成本的示例，但实际上，一个阶段只能有一种成本类型。
# 假设我们主要用 NONLINEAR_LS 跟踪，但将避障成本合并到 NONLINEAR_LS 的 y_expr 中，
# 或者使用 EXTERNAL 作为主成本。
# 这里我们演示一个更实际的场景：使用 NONLINEAR_LS 进行轨迹跟踪，
# 并将避障逻辑整合到 NONLINEAR_LS 的残差表达式中（如果避障可以表示为最小二乘形式）
# 或者，如果避障更复杂，就使用 EXTERNAL。
# 示例：使用 NONLINEAR_LS 进行轨迹跟踪，并将避障作为 EXTERNAL 成本添加
# 注意：ACADOS 一个阶段只能有一种 cost_type。如果同时需要跟踪和避障，
# 通常是把它们合并到 EXTERNAL_COST 中，或者把避障也转换为 NONLINEAR_LS 的形式。
# 为了演示，我们先配置 NONLINEAR_LS 为主，然后说明 EXTERNAL 的使用方式。
# 配置 NONLINEAR_LS 成本 (轨迹跟踪)
nx = model.x.size()[0]
nu = model.u.size()[0]
ny = nx + nu
ny_e = nx
ocp.dims.ny = ny
ocp.dims.ny_e = ny_e
ocp.cost.cost_type = 'NONLINEAR_LS'
ocp.cost.W = np.diag([100, 100, 10, 10, 1, 1])
ocp.cost.cost_type_e = 'NONLINEAR_LS'
ocp.cost.W_e = np.diag([1000, 1000, 100, 100])
# 定义参考状态和控制输入符号变量
x_ref_sym = ca.MX.sym('x_ref_sym', nx)
u_ref_sym = ca.MX.sym('u_ref_sym', nu)
# 将参考值作为模型参数，这样可以在运行时更新
# 注意：如果模型本身没有p，需要初始化 model.p
if not hasattr(model, 'p') or model.p is None:
model.p = ca.MX([])
model.p = ca.vertcat(model.p, x_ref_sym, u_ref_sym)
ocp.dims.np = len(model.p)
# NONLINEAR_LS 的 y_expr 使用参数
model.cost_y_expr = ca.vertcat(model.x - model.p[:nx], model.u - model.p[nx:nx+nu])
model.cost_y_expr_e = model.x - model.p[:nx] # 终端只跟踪状态
# 设置初始参数值 (即初始的参考值)
initial_x_ref = np.array([5.0, 5.0, 0.5, np.pi/4]) # 示例：目标位置(5,5), 速度0.5, 角度pi/4
initial_u_ref = np.array([0.0, 0.0]) # 示例：目标控制输入为0
ocp.parameter_values = np.hstack((initial_x_ref, initial_u_ref))
# 如果需要，可以在此基础上添加 EXTERNAL 成本 (作为额外的惩罚项，此时需要将 cost_type 
