ACADOS中非线性成本函数的高效实现与配置

admin 百科 2025-12-13 13

本文详细阐述了在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

登录后复制

ACADOS中非线性成本函数的高效实现与配置-第2张图片-佛山资讯网