ACADOS非线性成本高效配置方法

本文详细阐述了在ACADOS中为移动机器人MPC问题定义动力学模型并配置非线性成本函数的方法。重点介绍了两种主要成本类型：`NONLINEAR_LS`（非线性最小二乘）和`EXTERNAL`（外部成本），并通过具体代码示例指导读者如何设置状态、控制、权重矩阵及参考轨迹，以实现轨迹跟踪和避障等复杂优化目标。

在实时模型预测控制（MPC）中，准确地定义系统动力学和优化目标至关重要。ACADOS作为一款高性能的MPC求解器，提供了灵活的接口来处理复杂的非线性系统和成本函数。本教程将以一个移动机器人为例，详细介绍如何利用CasADi定义机器人模型，并配置ACADOS求解器中的非线性成本函数。

1. 移动机器人动力学模型定义

首先，我们需要使用CasADi定义移动机器人的连续时间动力学模型。该模型将描述机器人的状态如何随控制输入变化。

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')        # x坐标
    y = ca.MX.sym('y')        # 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)

    # 定义连续时间动力学 (x_dot = f(x, u))
    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"]
    )

    # 显式和隐式动力学表达式
    f_expl_expr = continuous_dynamics(states, controls)
    f_impl_expr = x_dot - f_expl_expr

    # 创建AcadosModel对象
    model = AcadosModel()
    model.f_expl_expr = f_expl_expr
    model.f_impl_expr = f_impl_expr
    model.x = states
    model.xdot = x_dot
    model.u = controls
    model.p = [] # 无额外参数
    model.name = model_name

    return model

上述代码定义了机器人的状态（位置x, y，速度v，朝向theta）和控制输入（加速度a，角速度w），并基于CasADi构建了其连续时间动力学方程。

2. ACADOS OCP 求解器基础设置

在定义了机器人模型之后，下一步是设置ACADOS的最优控制问题（OCP）求解器。这包括定义问题维度、时间步长、初始条件和约束。

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.0                # 总时间

    ocp.dims.N = N
    ocp.dims.nx = nx
    ocp.dims.nu = nu
    ocp.solver_options.tf = T

    # 初始状态
    x_ref = np.array([0.0, 0.0, 0.0, 0.0]) # 示例初始状态 [x, y, v, theta]
    ocp.constraints.x0 = x_ref

    # 参数初始化 (如果模型有参数，此处设置)
    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.lbx = np.array([-100, -100, 0, -np.pi]) # 状态下界
    # ocp.constraints.ubx = np.array([100, 100, 10, np.pi])   # 状态上界
    # ocp.constraints.idxbx = np.array([0, 1, 2, 3]) # 对应状态向量的索引

    return ocp

此函数初始化了AcadosOcp对象，加载了机器人模型，并设置了离散化步数、总时间、初始状态以及控制输入的上下界。

3. 非线性成本函数实现

ACADOS提供了多种成本函数类型，其中NONLINEAR_LS（非线性最小二乘）和EXTERNAL（外部成本）是处理复杂非线性目标函数的两种主要方式。

3.1 NONLINEAR_LS (非线性最小二乘) 成本函数

NONLINEAR_LS成本类型适用于可以表示为 ||y(x, u) - y_ref||_W^2 形式的成本函数，其中 y(x, u) 是状态和控制输入的非线性表达式，y_ref 是参考值，W 是权重矩阵。这种形式的成本函数能够利用高斯-牛顿法来近似Hessian矩阵，从而提高求解效率。

应用场景: 轨迹跟踪、速度匹配等，例如： J = (q - p)^T W_1 (q - p) + (u - r)^T W_2 (u - r) 其中 q 是机器人实际输出（通常是状态的子集或变换），p 是期望输出轨迹；u 是实际控制输入，r 是期望控制输入轨迹。

为了在ACADOS中实现这种成本，你需要设置以下关键属性：

• ocp.cost.cost_type 和 ocp.cost.cost_type_e: 分别用于中间阶段和终端阶段的成本类型。设置为 NONLINEAR_LS。
• ocp.model.cost_y_expr 和 ocp.model.cost_y_expr_e: 定义CasADi表达式 y(x, u)，它表示你希望跟踪的输出。
• ocp.cost.W 和 ocp.cost.W_e: 权重矩阵，定义了误差的相对重要性。
• ocp.cost.yref 和 ocp.cost.yref_e: 参考值 y_ref。这些值可以在求解器创建后动态更新。

示例代码：轨迹跟踪成本

假设 q 是状态 x, y, theta 的组合，p 是对应的参考轨迹。u 是控制输入 a, w，r 是期望的零控制输入。

def setup_nonlinear_ls_cost(ocp):
    nx = ocp.dims.nx
    nu = ocp.dims.nu

    # 设置成本类型为非线性最小二乘
    ocp.cost.cost_type = 'NONLINEAR_LS'
    ocp.cost.cost_type_e = 'NONLINEAR_LS' # 终端成本也使用非线性最小二乘

    # 1. 定义输出表达式 y(x, u)
    # 假设我们想跟踪 x, y, theta 和控制输入 a, w
    # 那么 y_expr 应该包含这些项
    q_expr = ca.vertcat(ocp.model.x[0], ocp.model.x[1], ocp.model.x[3]) # x, y, theta
    u_expr = ocp.model.u # a, w
    ocp.model.cost_y_expr = ca.vertcat(q_expr, u_expr) # 中间阶段输出
    ocp.model.cost_y_expr_e = q_expr # 终端阶段输出 (通常只关心状态)

    # 2. 定义权重矩阵 W
    # 假设 q 的维度是 3 (x,y,theta)，u 的维度是 2 (a,w)
    ny = ocp.model.cost_y_expr.size()[0] # 中间阶段输出维度 (3+2=5)
    ny_e = ocp.model.cost_y_expr_e.size()[0] # 终端阶段输出维度 (3)

    # 权重矩阵 W (对角矩阵)
    # 对应 [x_error, y_error, theta_error, a_error, w_error]
    W_diag = np.array([100.0, 100.0, 50.0, 1.0, 1.0])
    ocp.cost.W = np.diag(W_diag)

    # 终端权重矩阵 W_e (对角矩阵)
    # 对应 [x_error, y_error, theta_error]
    W_e_diag = np.array([200.0, 200.0, 100.0])
    ocp.cost.W_e = np.diag(W_e_diag)

    # 3. 定义参考值 y_ref
    # 这些是期望的输出值，可以在运行时更新
    # 示例：假设期望轨迹是原点，期望控制输入是零
    ocp.cost.yref = np.zeros(ny) # 中间阶段参考值
    ocp.cost.yref_e = np.zeros(ny_e) # 终端阶段参考值

    # 可以通过 ocp.cost.set(stage, "yref", new_yref_value) 在求解前更新 yref
    # 例如：ocp.cost.set(0, "yref", np.array([1.0, 1.0, 0.0, 0.0, 0.0]))

    return ocp

3.2 EXTERNAL (外部) 成本函数

EXTERNAL成本类型提供了最大的灵活性，允许用户定义任何形式的CasADi表达式作为成本函数。ACADOS将直接使用这个表达式来计算梯度和Hessian（通过自动微分）。

应用场景: 避障、复杂非线性约束软化、自定义非凸成本等。例如： J = 1/2 * (1 / (d(q, o) - r_robot - r_obstacle))^2 其中 d(q, o) 是机器人位置 q 与障碍物 o 之间的距离，r_robot 和 r_obstacle 分别是机器人和障碍物的半径。

为了在ACADOS中实现这种成本，你需要设置以下关键属性：

• ocp.cost.cost_type 和 ocp.cost.cost_type_e: 分别用于中间阶段和终端阶段的成本类型。设置为 EXTERNAL。
• ocp.model.cost_expr_ext_cost 和 ocp.model.cost_expr_ext_cost_e: 定义CasADi表达式，直接表示成本函数本身。

示例代码：避障成本

def setup_external_cost(ocp):
    # 设置成本类型为外部成本
    ocp.cost.cost_type = 'EXTERNAL'
    ocp.cost.cost_type_e = 'EXTERNAL' # 终端成本也使用外部成本

    # 假设障碍物位置为 (ox, oy)，半径为 r_obstacle
    # 机器人半径为 r_robot
    ox = 5.0
    oy = 5.0
    r_obstacle = 0.5
    r_robot = 0.2

    # 机器人当前位置 (x, y)
    robot_x = ocp.model.x[0]
    robot_y = ocp.model.x[1]

    # 计算机器人与障碍物中心的距离
    distance_sq = (robot_x - ox)**2 + (robot_y - oy)**2
    distance = ca.sqrt(distance_sq)

    # 安全距离裕度
    safety_margin = r_robot + r_obstacle

    # 定义避障成本表达式
    # 当距离小于安全距离时，成本急剧增加
    # 使用 max(0, safety_margin - distance) 避免负距离倒数
    # 或者更直接地，使用一个平滑的惩罚函数

    # 方式一：直接倒数惩罚 (可能导致数值问题，需要截断或平滑)
    # cost_avoidance = 0.5 * (1 / (distance - safety_margin + 1e-6))**2
    # cost_avoidance = ca.if_else(distance < safety_margin, 1e6, 0.0) # 简单粗暴的惩罚

    # 方式二：更平滑的惩罚函数，避免数值问题
    # 例如，当距离小于某个阈值时，成本开始增加
    threshold_distance = safety_margin + 0.5 # 在安全距离外0.5米开始惩罚

    # 定义一个惩罚项，当距离小于阈值时，惩罚值非零
    # 惩罚函数 f(d) = max(0, threshold_distance - d)^2
    # 权重系数 beta 调整惩罚强度
    beta = 1000.0 # 避障权重

    # 计算距离差值，如果为负，则说明在安全距离内
    dist_diff = threshold_distance - distance

    # 只有当 dist_diff > 0 (即距离小于阈值) 时才产生惩罚
    # 使用 CasADi 的 if_else 来实现条件逻辑
    cost_avoidance = beta * ca.if_else(dist_diff > 0, dist_diff**2, 0.0)

    ocp.model.cost_expr_ext_cost = cost_avoidance # 中间阶段避障成本
    ocp.model.cost_expr_ext_cost_e = 0.0 # 终端阶段通常不考虑避障，或设置为0

    return ocp

注意事项：

• 数值稳定性: EXTERNAL成本函数由于其通用性，可能引入复杂的非线性，需要特别注意表达式的数值稳定性，尤其是在接近奇点（如分母为零）时。平滑的惩罚函数通常比硬性倒数惩罚更稳健。

• 性能: NONLINEAR_LS由于其结构特点，通常能提供更好的求解性能，因为它允许ACADOS利用更高效的高斯-牛顿Hessian近似。如果你的成本函数能转化为NONLINEAR_LS形式，优先考虑使用它。

• 多目标集成: 如果需要同时实现轨迹跟踪和避障，可以将两种成本函数组合起来。例如，可以在EXTERNAL成本中包含轨迹跟踪项和避障项，或者使用NONLINEAR_LS进行轨迹跟踪，并额外添加一个EXTERNAL成本来处理避障。但请注意，ACADOS的cost_type只能设置一次，如果需要混合，通常会将所有项合并到EXTERNAL中，或者将NONLINEAR_LS作为主要成本，并通过约束或其他方式处理次要目标。

  • 一个更通用的做法是，如果一个问题同时包含 NONLINEAR_LS 形式的成本和 EXTERNAL 形式的成本，通常会选择将 cost_type 设置为 EXTERNAL，并将所有成本项（包括原本可以表示为 NONLINEAR_LS 的项）合并到 cost_expr_ext_cost 中。 例如：

    # 合并成本示例
    ocp.cost.cost_type = 'EXTERNAL'
    ocp.cost.cost_type_e = 'EXTERNAL'
    
    # 轨迹跟踪项 (转换为EXTERNAL形式)
    q_expr = ca.vertcat(ocp.model.x[0], ocp.model.x[1], ocp.model.x[3])
    u_expr = ocp.model.u
    y_combined = ca.vertcat(q_expr, u_expr)
    yref_combined = np.zeros(y_combined.size()[0]) # 假设参考为零
    W_combined = np.diag(np.array([100.0, 100.0, 50.0, 1.0, 1.0]))
    
    cost_tracking = (y_combined - yref_combined).T @ W_combined @ (y_combined - yref_combined)
    
    # 避障项
    # ... (同上 setup_external_cost 中的 cost_avoidance)
    cost_avoidance = beta * ca.if_else(dist_diff > 0, dist_diff**2, 0.0)
    
    # 总成本
    ocp.model.cost_expr_ext_cost = cost_tracking + cost_avoidance
    # 终端成本只考虑状态跟踪
    ocp.model.cost_expr_ext_cost_e = (q_expr - np.zeros(q_expr.size()[0])).T @ W_e_diag @ (q_expr - np.zeros(q_expr.size()[0]))

4. 总结

本教程详细介绍了在ACADOS中为移动机器人MPC问题定义动力学模型，并配置两种主要非线性成本函数类型：NONLINEAR_LS和EXTERNAL。NONLINEAR_LS适用于具有最小二乘形式的成本，通常能提供更好的性能；而EXTERNAL则提供了极大的灵活性，可以处理任意复杂的非线性表达式。根据具体的优化目标和成本函数的结构，选择合适的成本类型是实现高效和鲁棒MPC的关键。通过灵活运用CasADi表达式，ACADOS能够帮助开发者构建出满足复杂控制需求的实时控制器。

更多高级用法和示例，可以参考ACADOS官方文档和GitHub仓库中的示例代码，例如 acados/examples/acados_python/pendulum_on_cart/ocp/ocp_example_cost_formulations.py，它展示了不同成本函数的具体实现。