ACADOS中非线性成本函数的实现与配置-Python教程-PHP中文网

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

本文旨在深入探讨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 model

登录后复制

3. 配置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, model

登录后复制

4. 实现非线性成本函数

现在我们来详细讨论如何实现两种主要的非线性成本函数。

4.1 非线性最小二乘成本 (NONLINEAR_LS)

当成本函数可以表示为残差向量的L2范数平方（即 $\sum ||y(x, u) - y_{ref}||_W^2$）时，应使用 NONLINEAR_LS 类型。ACADOS能够利用这种结构来计算Gauss-Newton Hessian，从而提高求解效率。

结构:

Natural Language Playlist

探索语言和音乐之间丰富而复杂的关系，并使用 Transformer 语言模型构建播放列表。

查看详情

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, model

登录后复制

4.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, model

登录后复制

5. 综合配置示例

现在我们将上述成本函数配置集成到 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

登录后复制

以上就是ACADOS中非线性成本函数的实现与配置的详细内容，更多请关注php中文网其它相关文章！