ACADOS中移动机器人MPC非线性代价函数实现咨询

算力阿峰

2026-7-2

问题描述

作为ACADOS与MPC的新手，当前任务是定义移动机器人动力学并通过ACADOS求解MPC问题。已用CasADi完成移动机器人模型定义，现需配置ACADOS求解器实现两类代价函数：

最小化实际轨迹与期望轨迹的误差（期望控制输入为0）
避障代价函数
原本考虑使用EXTERNAL代价类型，被告知应采用NONLINEAR_LS，特此咨询实现方法。

已完成的代码片段如下：

def mobile_robot_model():
    """
    Define a simple mobile robot model.

    Returns:
        x_vec (MX): Symbolic vector representing the state [x, y, v, theta].
        u (MX): Symbolic vector representing the control input [a, w].
        continuous_dynamics (Function): CasADi function for the continuous-time dynamics.
            Takes state vector and control input vectors as input and returns
            the vector representing the rates of change of the state variables.
    """

    model_name = 'mobile_robot'

    # Define symbolic variables (states)
    x = ca.MX.sym('x')
    y = ca.MX.sym('y')
    v = ca.MX.sym('v')
    theta = ca.MX.sym('theta')

    # Control
    a = ca.MX.sym('a')  # acceleration
    w = ca.MX.sym('w')  # angular velocity

    # Define state and control vectors
    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))

    # Create a CasADi function for the continuous-time dynamics
    continuous_dynamics = ca.Function(
        'continuous_dynamics',
        [states, controls],
        [ca.vcat(rhs)],
        ["state", "control_input"],
        ["rhs"]
    )

    f_impl = x_dot - continuous_dynamics(states, controls)

    model = AcadosModel()

    model.f_expl_expr = continuous_dynamics(states, controls)
    model.f_impl_expr = f_impl
    model.x = states
    model.xdot = x_dot
    model.u = controls
    model.p = []
    model.name = model_name

    return model

def create_ocp_solver():

    # Create AcadosOcp object
    ocp = AcadosOcp()

    # Set up the optimization problem
    model = mobile_robot_model()
    ocp.model = model

    # --------------------PARAMETERS--------------
    # constants
    nx = model.x.size()[0]
    nu = model.u.size()[0]
    ny = nx + nu
    ny_e = nx
    T = 30
    N = 100
    n_params = len(model.p)

    # Setting initial conditions
    ocp.dims.N = N
    ocp.dims.nx = nx
    ocp.dims.nu = nu
    # ocp.dims.ny = ny
    # ocp.dims.ny_e = ny_e
    ocp.solver_options.tf = T

    # initial state
    x_ref = np.zeros(nx)

    # Set initial condition for the robot
    ocp.constraints.x0 = x_ref

    # initialize parameters
    ocp.dims.np = n_params
    ocp.parameter_values = np.zeros(n_params)

    # ---------------------CONSTRAINTS------------------
    # Define constraints on states and control inputs
    ocp.constraints.lbu = np.array([-0.1, -0.3])  # Lower bounds on control inputs
    ocp.constraints.ubu = np.array([0.1, 0.3])    # Upper bounds on control inputs
    ocp.constraints.lu = np.array([100, 100, 1, 10])  # Upper bounds on states
    ocp.constraints.idxbu = np.array([0, 1])  # for indices 0 & 1

实现方案

ACADOS的NONLINEAR_LS代价类型将目标函数表示为非线性最小二乘形式：
$$J = \frac{1}{2}\sum_{k=0}^{N-1} | r(x_k, u_k) |{W}^2 + \frac{1}{2} | r_e(x_N) |{W_e}^2$$
其中$r$为阶段残差向量，$r_e$为终端残差向量，$W$、$W_e$为对角权重矩阵。我们只需将两类代价整合到残差向量中即可。

1. 修改模型定义，添加残差表达式

在mobile_robot_model()中添加阶段残差model.r和终端残差model.r_e，整合轨迹跟踪与避障逻辑：

def mobile_robot_model():
    """
    Define a simple mobile robot model with nonlinear least squares residuals.

    Returns:
        model (AcadosModel): Acados model with dynamics and residual expressions.
    """

    model_name = 'mobile_robot'

    # Define symbolic variables (states)
    x = ca.MX.sym('x')
    y = ca.MX.sym('y')
    v = ca.MX.sym('v')
    theta = ca.MX.sym('theta')

    # Control
    a = ca.MX.sym('a')  # acceleration
    w = ca.MX.sym('w')  # angular velocity

    # Define state and control vectors
    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))

    # Create a CasADi function for the continuous-time dynamics
    continuous_dynamics = ca.Function(
        'continuous_dynamics',
        [states, controls],
        [ca.vcat(rhs)],
        ["state", "control_input"],
        ["rhs"]
    )

    f_impl = x_dot - continuous_dynamics(states, controls)

    model = AcadosModel()

    model.f_expl_expr = continuous_dynamics(states, controls)
    model.f_impl_expr = f_impl
    model.x = states
    model.xdot = x_dot
    model.u = controls
    model.p = []
    model.name = model_name

    # ------------------- NONLINEAR LS RESIDUALS -------------------
    # 1. 轨迹跟踪残差：状态偏差 + 控制偏差（期望控制为0）
    tracking_res = ca.vertcat(states, controls)

    # 2. 避障残差：平滑惩罚机器人与障碍物的距离（示例：单个障碍物在(5,5)，安全距离1.0）
    obs_x = 5.0
    obs_y = 5.0
    safe_dist = 1.0
    dist_to_obs = ca.sqrt((x - obs_x)**2 + (y - obs_y)**2)
    # 使用tanh实现平滑近似，距离小于安全距离时残差增大，大于时残差趋近于0
    k_gain = 10.0  # 控制惩罚陡峭程度
    obs_res = ca.tanh(k_gain * (safe_dist - dist_to_obs))

    # 合并阶段残差
    model.r = ca.vertcat(tracking_res, obs_res)

    # 终端残差：仅考虑状态跟踪误差
    model.r_e = states

    return model

2. 配置OCP求解器的代价参数

在create_ocp_solver()中设置残差维度、权重矩阵与参考向量：

def create_ocp_solver():

    # Create AcadosOcp object
    ocp = AcadosOcp()

    # Set up the optimization problem
    model = mobile_robot_model()
    ocp.model = model

    # --------------------PARAMETERS--------------
    # constants
    nx = model.x.size()[0]
    nu = model.u.size()[0]
    ny = nx + nu + 1  # 阶段残差长度：跟踪残差(nx+nu) + 避障残差(1)
    ny_e = nx  # 终端残差长度：仅状态
    T = 30
    N = 100
    n_params = len(model.p)

    # Setting initial conditions
    ocp.dims.N = N
    ocp.dims.nx = nx
    ocp.dims.nu = nu
    ocp.dims.ny = ny
    ocp.dims.ny_e = ny_e
    ocp.solver_options.tf = T

    # initial state
    x_ref = np.zeros(nx)
    u_ref = np.zeros(nu)

    # Set initial condition for the robot
    ocp.constraints.x0 = x_ref

    # initialize parameters
    ocp.dims.np = n_params
    ocp.parameter_values = np.zeros(n_params)

    # ---------------------CONSTRAINTS------------------
    # 修正状态约束的正确写法
    ocp.constraints.lbu = np.array([-0.1, -0.3])  # 控制输入下界
    ocp.constraints.ubu = np.array([0.1, 0.3])    # 控制输入上界
    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.idxlx = np.arange(nx)
    ocp.constraints.idxux = np.arange(nx)

    # ---------------------COST FUNCTION (NONLINEAR LS)------------------
    # 参考向量：阶段参考为[x_ref, u_ref, 0]（避障残差目标为0，无惩罚）
    ocp.cost.y_ref = np.concatenate([x_ref, u_ref, np.array([0.0])])
    # 终端参考为x_ref
    ocp.cost.y_ref_e = x_ref

    # 权重矩阵：对角矩阵，调整各代价项优先级
    state_weights = np.array([10.0, 10.0, 1.0, 1.0])  # 状态跟踪权重
    control_weights = np.array([0.5, 0.5])            # 控制输入权重
    obs_weight = np.array([100.0])                    # 避障权重

    ocp.cost.W = np.diag(np.concatenate([state_weights, control_weights, obs_weight]))
    ocp.cost.W_e = np.diag(state_weights)

    # 设置代价类型为NONLINEAR_LS
    ocp.cost.cost_type = 'NONLINEAR_LS'
    ocp.cost.cost_type_e = 'NONLINEAR_LS'

    # ---------------------SOLVER OPTIONS------------------
    ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES'
    ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'  # 高斯牛顿近似适配非线性LS
    ocp.solver_options.integrator_type = 'ERK'
    ocp.solver_options.nlp_solver_type = 'SQP_RTI'  # 实时迭代模式适配MPC

    # 创建求解器
    ocp_solver = AcadosOcpSolver(ocp)

    return ocp_solver

关键说明

残差平滑性：避障残差使用tanh函数实现平滑惩罚，避免不连续函数导致求解器收敛困难。
权重调整：可根据实际需求修改权重矩阵，比如提高避障权重以优先保证安全，或提高状态权重以优化轨迹跟踪精度。
时变参考：若期望轨迹是时变的，可将期望状态放入模型参数model.p，在每次MPC迭代时更新ocp.parameter_values与ocp.cost.y_ref。

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

ACADOS中移动机器人MPC非线性代价函数实现咨询

1. 修改模型定义，添加残差表达式 #

2. 配置OCP求解器的代价参数 #

关键说明 #

1. 修改模型定义，添加残差表达式

2. 配置OCP求解器的代价参数

关键说明