本文详细阐述了在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 是期望的零控制输入。
CNB 云原生构建平台的 Git 操作技能，支持代码克隆、提交、推送、分支管理、Merge Request 管理、流水线触发与结果读取。初次使用需收集用户的 Git 用户名和邮箱。
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 ocp3.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，它展示了不同成本函数的具体实现。
