自动驾驶核心技术：轨迹优化与Acados求解器实战指南
在机器人或自动驾驶中，轨迹优化是一个重要的内容。
- 为了让机器人按照特定底盘运动，我们需要引入运动学模型，常见的有单车模型，自行车模型，差分底盘模型。
- 为了让机器人避障，我们需要引入避障约束，有静态障碍物还有动态障碍物。
那么这些约束，是如何在优化问题中设置并求解的呢。通常我们会将其建模未最优控制问题，然后用一个求解器进行求解，比如商业免费的Acados求解器。
本文将会把优化问题的组成，acados求解器的使用都进行讲解，并且结合一个非常经典且工程化的项目T-MPC来讲解，T-MPC是一个拓扑驱动的轨迹优化算法，支持并行计算，可处理动态障碍物的预测轨迹，每一个模块都非常值得学习。
本文参考项目：https://github.com/tud-amr/mpc_planner
近期会将该项目移植到我的开源仿真器中(无需ros)，欢迎关注：https://github.com/ahrs365/navsim-local
让我们将优化问题分解为独立的、可组合的模块来看待：
优化问题 = 动力学模型 + 代价模块 + 约束模块
一、问题描述
标准MPC优化问题：
$$
\begin{aligned}
\min_{u_{0:N-1}, x_{0:N}} \quad & \sum_{k=0}^{N-1} \ell(x_k, u_k, p) + \ell_e(x_N, p) \
\text{subject to} \quad & x_0 = \bar{x}_0 \quad \text{(初始状态约束)} \
& x_{k+1} = f(x_k, u_k), \quad k=0,…,N-1 \quad \text{(动力学约束)} \
& h(x_k, u_k, p) \in [h_l, h_u], \quad k=1,…,N-1 \quad \text{(路径约束)} \
& u_k \in [u_l, u_u] \quad \text{(控制边界)} \
& x_k \in [x_l, x_u] \quad \text{(状态边界)}
\end{aligned}
$$
变量举例说明：
| 符号 | 维度 | 含义 | 自行车模型示例 |
|---|---|---|---|
| x k x_k xk | n x n_x nx | 状态变量 | [ x , y , ψ , v , δ , s ] T ∈ R 6 [x, y, \psi, v, \delta, s]^T \in \mathbb{R}^6 [x,y,ψ,v,δ,s]T∈R6 |
| u k u_k uk | n u n_u nu | 控制输入 | [ a , ω , slack ] T ∈ R 3 [a, \omega, \text{slack}]^T \in \mathbb{R}^3 [a,ω,slack]T∈R3 |
| p p p | n p n_p np | 参数 | 目标位置、障碍物、权重等 |
| N N N | - | 预测时域 | 例如 30 步 |
| ℓ ( ⋅ ) \ell(\cdot) ℓ(⋅) | - | 阶段代价 | 速度代价、转角代价、目标代价等 |
| ℓ e ( ⋅ ) \ell_e(\cdot) ℓe(⋅) | - | 终端代价 | 通常与阶段代价相同 |
| f ( ⋅ ) f(\cdot) f(⋅) | - | 动力学模型 | 自行车模型离散化 |
| h ( ⋅ ) h(\cdot) h(⋅) | n h n_h nh | 路径约束 | 障碍物避障约束 |
然我们按照这三个模块进行逐一解释。
1.运动学模型
常见的三种模型分别是差速，单车，自行车。每一种，可以根据所选择的状态量的不同，衍生出很多的版本，状态量越多，控制量就越高阶，直观体现就是规划的结果越平滑，但是会导致优化变量维度增加，求解难度增加。
运动学模型在标准MPC优化问题中代表的就是：
x k + 1 = f ( x k , u k ) , k = 0 , . . . , N − 1 (动力学约束) x_{k+1} = f(x_k, u_k), \quad k=0,...,N-1 \quad \text{(动力学约束)} xk+1=f(xk,uk),k=0,...,N−1(动力学约束)
2.代价模块
根据自己优化问题，选择合适的代价组合。比如：基础代价+目标点跟踪代价
在在标准MPC优化问题中代表的就是：
$$
\begin{aligned}
\min_{u_{0:N-1}, x_{0:N}} \quad & \sum_{k=0}^{N-1} \ell(x_k, u_k, p) + \ell_e(x_N, p) \
\end{aligned}
$$
| 文件名 | 类型 | 模块名 | 功能简述 |
|---|---|---|---|
mpc_base.py |
代价 | MPCBaseModule | 基础代价（惩罚控制输入和状态） |
goal_module.py |
代价 | GoalModule | 目标点跟踪代价 |
contouring.py |
代价 | ContouringModule | MPCC路径跟踪代价 |
curvature_aware_contouring.py |
代价 | CurvatureAwareContouringModule | CA-MPC曲率感知路径跟踪 |
path_reference_velocity.py |
代价 | PathReferenceVelocityModule | 路径参考速度（辅助模块） |
3.约束模块
可以分为障碍物避障约束和路径相关约束。根据不同的避障精度和场景需求，可以选择不同的避障约束。
| 文件名 | 类型 | 模块名 | 功能简述 |
|---|---|---|---|
ellipsoid_constraints.py |
约束 | EllipsoidConstraintModule | 椭圆障碍物避障约束 |
contouring_constraints.py |
约束 | ContouringConstraintModule | 路径边界约束 |
linearized_constraints.py |
约束 | LinearizedConstraintModule | 线性化避障约束 |
guidance_constraints.py |
约束 | GuidanceConstraintModule | T-MPC引导轨迹约束 |
gaussian_constraints.py |
约束 | GaussianConstraintModule | 高斯不确定性约束 |
scenario_constraints.py |
约束 | ScenarioConstraintModule | 场景优化约束 |
decomp_constraints.py |
约束 | DecompConstraintModule | DecompUtil静态约束 |
3.1 EllipsoidConstraintModule（椭圆避障）
文件: ellipsoid_constraints.py
理论基础：
将障碍物建模为椭圆，机器人建模为多个圆盘，约束形式：
$$
(p_{\text{disc}} - p_{\text{obs}})^T M (p_{\text{disc}} - p_{\text{obs}}) \geq 1
$$
其中椭圆矩阵：
M = R ( ψ ) T [ 1 / a 2 0 0 1 / b 2 ] R ( ψ ) M = R(\psi)^T \begin{bmatrix} 1/a^2 & 0 \\ 0 & 1/b^2 \end{bmatrix} R(\psi) M=R(ψ)T[1/a2001/b2]R(ψ)
- a a a: 长轴
- b b b: 短轴
- ψ \psi ψ: 朝向角
- R ( ψ ) R(\psi) R(ψ): 旋转矩阵
机器人圆盘模型：
●─────●─────●
disc0 disc1 disc2
每个圆盘相对车身有偏移
约束数量：
n h = n discs × n obstacles n_h = n_{\text{discs}} \times n_{\text{obstacles}} nh=ndiscs×nobstacles
参数（每个障碍物）：
ellipsoid_obst_{i}_x/y: 位置ellipsoid_obst_{i}_psi: 朝向ellipsoid_obst_{i}_major/minor: 长轴/短轴ellipsoid_obst_{i}_chi: 风险系数ellipsoid_obst_{i}_r: 半径
代码核心：
# 椭圆矩阵
ab = cd.SX(2, 2)
ab[0, 0] = 1.0 / (obst_major**2)
ab[1, 1] = 1.0 / (obst_minor**2)
R = rotation_matrix(obst_psi)
M = R.T @ ab @ R
# 圆盘位置
disc_x = pos_x + disc_offset * cd.cos(psi)
disc_y = pos_y + disc_offset * cd.sin(psi)
# 约束值
diff = cd.vertcat(disc_x - obst_x, disc_y - obst_y)
constraint_value = diff.T @ M @ diff
# 约束: constraint_value >= 1.0
约束边界：
- 下界:
1.0（必须在椭圆外） - 上界:
inf（无上界）
应用场景： - ✅ 静态障碍物避障
- ✅ 动态障碍物（预测轨迹）
- ✅ 非凸障碍物（多个椭圆组合）
3.2 ContouringConstraintModule（路径边界）
文件: contouring_constraints.py
理论基础：
约束机器人保持在路径边界内：
$$
\begin{aligned}
e_c + w_{\text{robot}} &\leq w_{\text{right}}(s) + \text{slack} \
-e_c + w_{\text{robot}} &\leq w_{\text{left}}(s) + \text{slack}
\end{aligned}
$$
其中：
- e c e_c ec: 轮廓误差
- w robot w_{\text{robot}} wrobot: 机器人宽度的一半
- w left/right ( s ) w_{\text{left/right}}(s) wleft/right(s): 路径左右边界宽度（样条函数）
几何意义：
← w_left(s) →|← w_right(s) →
═════════════╪═════════════
路径 │
● 机器人
═════════════╪═════════════
参数：
width_left{i}_a/b/c/d: 左边界样条width_right{i}_a/b/c/d: 右边界样条
约束数量：
n h = 2 (左边界 + 右边界) n_h = 2 \quad \text{(左边界 + 右边界)} nh=2(左边界 + 右边界)
代码核心：
# 轮廓误差
contour_error = path_dy_normalized * (pos_x - path_x) - path_dx_normalized * (pos_y - path_y)
# 边界宽度
width_left = Spline(params, "width_left", self.num_segments, s)
width_right = Spline(params, "width_right", self.num_segments, s)
# 机器人宽度
w_cur = model.width / 2.0
# 约束
constraints.append(contour_error + w_cur - width_right.at(s) - slack)
constraints.append(-contour_error + w_cur - width_left.at(s) - slack)
约束边界：
- 下界:
-inf - 上界:
0.0（必须满足 h ≤ 0 h \leq 0 h≤0）
应用场景：
- ✅ 车道保持
- ✅ 狭窄通道导航
- ✅ 结构化环境
3.3 LinearizedConstraintModule（线性化避障）
文件: linearized_constraints.py
理论基础：
将非线性避障约束线性化为半空间约束：
$$
\mathbf{a}^T p_{\text{disc}} \leq b + \text{slack}
$$
其中：
- a = [ a 1 , a 2 ] T \mathbf{a} = [a_1, a_2]^T a=[a1,a2]T: 法向量
- b b b: 偏移量
线性化过程：
障碍物
●
╲
╲ 法向量 a
╲
──────╲────── 分离超平面
╲
● 机器人
参数（每个圆盘、每个障碍物）：
disc_{disc_id}_lin_constraint_{obs_id}_a1/a2: 法向量disc_{disc_id}_lin_constraint_{obs_id}_b: 偏移
约束数量：
n h = n discs × n obstacles n_h = n_{\text{discs}} \times n_{\text{obstacles}} nh=ndiscs×nobstacles
代码核心：
# 圆盘位置
rotation_car = rotation_matrix(psi)
disc_relative_pos = np.array([disc_offset, 0])
disc_pos = pos + rotation_car.dot(disc_relative_pos)
# 线性约束
a1 = params.get(f"disc_{disc_id}_lin_constraint_{obs_id}_a1")
a2 = params.get(f"disc_{disc_id}_lin_constraint_{obs_id}_a2")
b = params.get(f"disc_{disc_id}_lin_constraint_{obs_id}_b")
constraints.append(a1 * disc_pos[0] + a2 * disc_pos[1] - (b + slack))
约束边界：
- 下界:
-inf - 上界:
0.0
应用场景：
- ✅ 实时性要求高（线性约束求解快）
- ✅ 与引导轨迹配合（T-MPC）
- ✅ 凸障碍物
3.4 GuidanceConstraintModule（T-MPC引导）
文件: guidance_constraints.py
理论基础：
T-MPC (Topology-Driven MPC) 的核心思想：
- 离线生成多条引导轨迹（不同拓扑）
- 在线优化时，约束在引导轨迹附近线性化
- 并行优化多条轨迹，选择最优
约束形式：
$$
\mathbf{a}i^T p \leq b_i, \quad i = 1, …, n{\text{obstacles}}
$$
与LinearizedConstraintModule的区别：
| 特性 | LinearizedConstraintModule | GuidanceConstraintModule |
|---|---|---|
| 线性化参考 | 当前状态 | 引导轨迹 |
| 更新频率 | 每次求解 | 引导轨迹更新时 |
| 拓扑 | 单一 | 多拓扑并行 |
代码核心：
class GuidanceConstraintModule(ConstraintModule):
def __init__(self, settings, constraint_submodule=None):
# 线性约束（引导）
self.constraints.append(LinearConstraints(max_obstacles=...))
# 底层约束（椭圆）
self.constraint_submodule = EllipsoidConstraintModule(settings)
self.constraints.append(self.constraint_submodule.constraints[-1])
应用场景：
- ✅ 复杂障碍物环境
- ✅ 多拓扑路径规划
- ✅ T-MPC算法
3.5 GaussianConstraintModule（高斯不确定性）
文件: gaussian_constraints.py
理论基础：
将障碍物运动建模为高斯分布，限制碰撞概率：
$$
P(\text{collision}) \leq \alpha
$$
其中 α \alpha α 是风险阈值。
约束形式：
$$
\mathbf{a}{ij}^T (p{\text{disc}} - p_{\text{obs}}) \geq r_{\text{combined}} + \text{erfinv}(1 - 2\alpha) \sqrt{2 \mathbf{a}{ij}^T \Sigma \mathbf{a}{ij}}
$$
其中：
- a i j \mathbf{a}_{ij} aij: 从障碍物到机器人的单位向量
- Σ \Sigma Σ: 障碍物位置的协方差矩阵
- erfinv \text{erfinv} erfinv: 误差函数的逆
- α \alpha α: 风险参数
几何意义：
障碍物（高斯分布）
───
╱ ╲
╱ ● ╲ ← 均值
│ ╱ ╲ │
╲ σ_x,σ_y ╱
╲ ╱
───
安全距离 = r + k*σ
其中 k = erfinv(1-2α)
参数（每个障碍物）：
gaussian_obst_{i}_x/y: 均值位置gaussian_obst_{i}_major/minor: 标准差gaussian_obst_{i}_risk: 风险阈值 α \alpha αgaussian_obst_{i}_r: 障碍物半径
代码核心：
# 协方差矩阵
Sigma = np.diag([sigma_x**2, sigma_y**2])
# 方向向量
diff_pos = disc_pos - obs_pos
a_ij = diff_pos / cd.sqrt(diff_pos.dot(diff_pos))
# 误差函数逆（手动实现）
x_erfinv = 1.0 - 2.0 * risk
z = cd.sqrt(-cd.log((1.0 - x_erfinv) / 2.0))
y_erfinv = (((1.641345311 * z + 3.429567803) * z - 1.624906493) * z - 1.970840454) / \
((1.637067800 * z + 3.543889200) * z + 1.0)
# 约束
constraints.append(a_ij.T @ diff_pos - b_ij - y_erfinv * cd.sqrt(2.0 * a_ij.T @ Sigma @ a_ij))
约束边界：
- 下界:
0.0 - 上界:
inf
应用场景：
- ✅ 动态障碍物（行人）
- ✅ 不确定性环境
- ✅ 概率安全保证
3.7 DecompConstraintModule（DecompUtil静态约束）
文件: decomp_constraints.py
理论基础：
使用 DecompUtil 库将自由空间分解为凸多面体，每个面表示为线性约束：
$$
\mathbf{a}i^T p \leq b_i, \quad i = 1, …, n{\text{halfspaces}}
$$
参数：
disc_{disc_id}_decomp_{i}_a1/a2/b: 第 i i i 个半空间
约束数量：
$$
n_h = n_{\text{discs}} \times n_{\text{max_constraints}}
$$
代码核心：
for disc_it in range(self.n_discs):
disc_pos = pos + rotation_car.dot(disc_relative_pos)
for index in range(self.max_constraints):
a1 = params.get(f"disc_{disc_id}_decomp_{index}_a1")
a2 = params.get(f"disc_{disc_id}_decomp_{index}_a2")
b = params.get(f"disc_{disc_id}_decomp_{index}_b")
constraints.append(a1 * disc_pos[0] + a2 * disc_pos[1] - (b + slack))
应用场景：
- ✅ 静态环境
- ✅ 复杂几何障碍物
- ✅ 与DecompUtil库配合
4.以TMPC举例
4.1 什么是T-MPC？
T-MPC (Topology-Driven Model Predictive Control) = 拓扑驱动的模型预测控制
核心思想：
- 并行优化多条轨迹（每条对应不同的避障策略）
- 使用引导约束（Guidance Constraints）确保轨迹多样性
- 选择代价最小的轨迹执行
传统MPC:
优化1条轨迹 → 执行
T-MPC:
优化N条轨迹（左绕、右绕、中间穿过...）
↓
选择最优轨迹 → 执行
4.2 应用场景
适用于：
- 动态环境中的避障
- 多种可行路径选择（如超车、绕行）
- 需要快速决策的场景
示例：
场景：前方有障碍物
传统MPC:
只优化1条轨迹（可能陷入局部最优）
T-MPC:
轨迹1: 从左侧绕过
轨迹2: 从右侧绕过
轨迹3: 减速等待
↓
选择代价最小的轨迹
4.3 代价和约束模块总结
（以单车模型为例）
| 模块 | 类型 | 作用 |
|---|---|---|
| MPCBaseModule | 代价 | 惩罚控制输入（加速度、角速度、速度） |
| ContouringModule | 代价 | 路径跟踪（轮廓误差、滞后误差） |
| PathReferenceVelocityModule | 代价 | 动态速度参考（可选） |
| GuidanceConstraintModule | 约束 | 引导约束（线性化避障） |
| EllipsoidConstraintModule | 约束 | 椭圆避障约束（子模块） |
4.3.1 控制输入代价（MPCBaseModule）
(1) 加速度惩罚
$$
J_a = \sum_{k=0}^{N-1} w_a \cdot a_k^2
$$
作用：
- 平滑加速度变化
- 减少能量消耗
- 提高乘坐舒适性
典型权重： w a = 0.1 ∼ 1.0 w_a = 0.1 \sim 1.0 wa=0.1∼1.0
(2) 角速度惩罚
$$
J_w = \sum_{k=0}^{N-1} w_w \cdot \omega_k^2
$$
作用：
- 平滑转向
- 避免急转弯
- 减少轮胎磨损
典型权重： w w = 0.1 ∼ 1.0 w_w = 0.1 \sim 1.0 ww=0.1∼1.0
(3) 速度跟踪代价（可选）
$$
J_v = \sum_{k=0}^{N-1} w_v \cdot (v_k - v_{\text{ref}})^2
$$
作用：
- 跟踪参考速度
- 在安全前提下保持速度
典型权重： w v = 0.5 ∼ 2.0 w_v = 0.5 \sim 2.0 wv=0.5∼2.0
4.3.2 路径跟踪代价（ContouringModule）
(1) 轮廓误差（Contour Error）
$$
e_c = \mathbf{t}^\perp \cdot (\mathbf{p} - \mathbf{p}_{\text{path}})
$$
其中：
- p = [ x , y ] T \mathbf{p} = [x, y]^T p=[x,y]T：机器人位置
- p path = [ x path ( s ) , y path ( s ) ] T \mathbf{p}_{\text{path}} = [x_{\text{path}}(s), y_{\text{path}}(s)]^T ppath=[xpath(s),ypath(s)]T：路径上最近点
- t ⊥ = [ − sin θ path , cos θ path ] T \mathbf{t}^\perp = [-\sin\theta_{\text{path}}, \cos\theta_{\text{path}}]^T t⊥=[−sinθpath,cosθpath]T：路径法向量
代价：
$$
J_{e_c} = \sum_{k=0}^{N-1} w_{e_c} \cdot e_{c,k}^2
$$
(2) 滞后误差（Lag Error）
$$
e_l = -\mathbf{t} \cdot (\mathbf{p} - \mathbf{p}_{\text{path}})
$$
其中：
- t = [ cos θ path , sin θ path ] T \mathbf{t} = [\cos\theta_{\text{path}}, \sin\theta_{\text{path}}]^T t=[cosθpath,sinθpath]T：路径切向量
代价：
$$
J_{e_l} = \sum_{k=0}^{N-1} w_{e_l} \cdot e_{l,k}^2
$$
4.3.3 引导约束（GuidanceConstraintModule）
T-MPC的关键创新：使用线性化约束引导多条轨迹。
工作流程：
1. 离线/在线生成引导轨迹（Guidance Trajectories）
- 轨迹1: 左侧绕行
- 轨迹2: 右侧绕行
- 轨迹3: 中间穿过
2. 对每条引导轨迹，线性化避障约束
- 在引导轨迹附近线性化椭圆约束
- 得到线性约束 Ax ≤ b
3. 并行优化N个MPC问题
- 每个MPC使用不同的线性化约束
- 保证轨迹多样性
4. 选择代价最小的轨迹
线性化过程
原始椭圆约束（非线性）：
$$
\frac{(x - x_{\text{obs}})2}{r_x2} + \frac{(y - y_{\text{obs}})2}{r_y2} \geq 1
$$
在引导轨迹点 ( x g , y g ) (x_g, y_g) (xg,yg) 处线性化：
$$
\nabla h(x_g, y_g) \cdot \begin{bmatrix} x - x_g \ y - y_g \end{bmatrix} \geq 0
$$
其中：
$$
\nabla h = \begin{bmatrix}
\frac{2(x_g - x_{\text{obs}})}{r_x^2} \
\frac{2(y_g - y_{\text{obs}})}{r_y^2}
\end{bmatrix}
$$
转换为标准形式：
$$
a_1 x + a_2 y \leq b
$$
其中：
$$
\begin{aligned}
a_1 &= -\frac{2(x_g - x_{\text{obs}})}{r_x^2} \
a_2 &= -\frac{2(y_g - y_{\text{obs}})}{r_y^2} \
b &= -(a_1 x_g + a_2 y_g)
\end{aligned}
$$
4.3.4 椭圆避障约束（EllipsoidConstraintModule）
作用：作为 GuidanceConstraintModule 的子模块，用于生成引导轨迹时的避障。
约束形式：
$$
\left| \mathbf{R}(\psi_{\text{obs}}) \cdot (\mathbf{p} - \mathbf{p}{\text{obs}}) \right|2^2 \geq (r{\text{robot}} + r{\text{obs}})^2
$$
其中：
- R ( ψ obs ) \mathbf{R}(\psi_{\text{obs}}) R(ψobs)：旋转矩阵（考虑障碍物朝向）
- r robot r_{\text{robot}} rrobot：机器人半径
- r obs r_{\text{obs}} robs：障碍物半径
5. 完整优化问题
5.1 数学形式
T-MPC优化问题（单个轨迹）：
$$
\begin{aligned}
\min_{x_{0:N}, u_{0:N-1}} \quad & J = \sum_{k=0}^{N-1} \Big( w_a a_k^2 + w_w \omega_k^2 + w_{e_c} e_{c,k}^2 + w_{e_l} e_{l,k}^2 - w_s s_k \Big) \
& \quad + w_v (v_k - v_{\text{ref}}(s_k))^2 \
\
\text{s.t.} \quad & x_{k+1} = f(x_k, u_k), \quad k=0,…,N-1 \quad \text{(动力学)} \
& a_1^{(i)} x_k + a_2^{(i)} y_k \leq b^{(i)}, \quad i=1,…,n_{\text{obs}}, \quad k=0,…,N-1 \quad \text{(线性化避障)} \
& x_0 = x_{\text{init}} \quad \text{(初始状态)} \
& u_{\min} \leq u_k \leq u_{\max} \quad \text{(控制边界)} \
& x_{\min} \leq x_k \leq x_{\max} \quad \text{(状态边界)}
\end{aligned}
$$
5.2 决策变量
单个时间步：
$$
z_k = [u_k, x_k] = [a_k, \omega_k, x_k, y_k, \psi_k, v_k, s_k]
$$
维度： n z = n u + n x = 2 + 5 = 7 n_z = n_u + n_x = 2 + 5 = 7 nz=nu+nx=2+5=7
完整优化问题（ N = 30 N = 30 N=30）：
$$
\text{决策变量数} = N \times n_z = 30 \times 7 = 210
$$
5.3 并行优化
T-MPC的关键：并行求解 M M M 个优化问题（ M M M 条引导轨迹）
优化问题1（左绕）:
min J1
s.t. 动力学 + 线性化约束1（左侧半空间）
优化问题2（右绕）:
min J2
s.t. 动力学 + 线性化约束2（右侧半空间）
优化问题3（中间）:
min J3
s.t. 动力学 + 线性化约束3（中间通道）
选择: J* = min(J1, J2, J3)
二、Acados求解器
1. Acados简介
1.1 什么是Acados？
Acados (A fast and embedded solvers for nonlinear optimal control) 是一个开源的快速嵌入式非线性最优控制求解器。
官网: https://docs.acados.org/
核心特性：
- ✅ 开源免费：MIT许可证
- ✅ 高性能：专为实时控制设计
- ✅ 代码生成：生成优化的C代码
- ✅ 多种算法：SQP、SQP-RTI等
- ✅ 自动微分：基于CasADi
1.2 Acados vs Forces Pro
| 特性 | Acados | Forces Pro |
|---|---|---|
| 许可 | 开源（MIT） | 商业（学术免费） |
| 性能 | ⭐⭐⭐⭐ | ⭐⭐⭐⭐⭐ |
| 易用性 | ⭐⭐⭐ | ⭐⭐⭐⭐⭐ |
| 文档 | 良好 | 优秀 |
| 支持 | 社区 | 官方 |
| 适用场景 | 学术、开源项目 | 商业、高性能需求 |
1.3 Acados求解的问题
标准OCP（Optimal Control Problem）：
$$
\begin{aligned}
\min_{x_{0:N}, u_{0:N-1}} \quad & \sum_{k=0}^{N-1} \ell(x_k, u_k, p) + \ell_e(x_N, p) \
\text{subject to} \quad & x_0 = \bar{x}_0 \
& x_{k+1} = f(x_k, u_k, p), \quad k=0,…,N-1 \
& h(x_k, u_k, p) \in [h_l, h_u], \quad k=0,…,N-1 \
& u_k \in [u_l, u_u] \
& x_k \in [x_l, x_u]
\end{aligned}
$$
符号说明：
- x k ∈ R n x x_k \in \mathbb{R}^{n_x} xk∈Rnx: 状态变量
- u k ∈ R n u u_k \in \mathbb{R}^{n_u} uk∈Rnu: 控制输入
- p ∈ R n p p \in \mathbb{R}^{n_p} p∈Rnp: 参数（目标、障碍物等）
- ℓ ( ⋅ ) \ell(\cdot) ℓ(⋅): 阶段代价（stage cost）
- ℓ e ( ⋅ ) \ell_e(\cdot) ℓe(⋅): 终端代价（terminal cost）
- f ( ⋅ ) f(\cdot) f(⋅): 动力学模型
- h ( ⋅ ) h(\cdot) h(⋅): 路径约束（path constraints）
2. 优化问题构建流程
2.1 整体架构
┌─────────────────────────────────────────────────────────┐
│ 第1步: Python配置 │
│ - generate_jackalsimulator_solver.py │
│ - 选择模块、设置参数 │
└─────────────────────────────────────────────────────────┘
↓
┌─────────────────────────────────────────────────────────┐
│ 第2步: 符号表达式构建 (CasADi) │
│ - 定义参数 (define_parameters) │
│ - 构建代价 (objective) │
│ - 构建约束 (constraints) │
│ - 构建动力学 (model.get_acados_dynamics) │
└─────────────────────────────────────────────────────────┘
↓
┌─────────────────────────────────────────────────────────┐
│ 第3步: Acados模型创建 (create_acados_model) │
│ - AcadosModel: 封装所有符号表达式 │
│ - AcadosOcp: 配置求解器选项 │
└─────────────────────────────────────────────────────────┘
↓
┌─────────────────────────────────────────────────────────┐
│ 第4步: 代码生成 (AcadosOcpSolver) │
│ - 自动微分 │
│ - 生成C代码 │
│ - 编译为共享库 │
└─────────────────────────────────────────────────────────┘
↓
┌─────────────────────────────────────────────────────────┐
│ 第5步: C++运行时 │
│ - 加载求解器 │
│ - 设置参数、初始状态 │
│ - 调用求解 │
│ - 提取结果 │
└─────────────────────────────────────────────────────────┘
2.2 关键文件
| 文件 | 作用 |
|---|---|
generate_jackalsimulator_solver.py |
入口脚本，选择配置 |
solver_generator/generate_acados_solver.py |
Acados求解器生成 |
solver_generator/solver_definition.py |
优化问题定义 |
solver_generator/solver_model.py |
动力学模型 |
mpc_planner_modules/scripts/*.py |
各种模块（代价、约束） |
3. 符号表达式构建
3.1 CasADi符号计算
CasADi 是Acados的符号计算引擎。
import casadi as cd
# 创建符号变量
x = cd.SX.sym('x', 3) # 3维状态向量
u = cd.SX.sym('u', 2) # 2维控制向量
p = cd.SX.sym('p', 5) # 5维参数向量
# 构建表达式
cost = x[0]**2 + x[1]**2 + u[0]**2
constraint = x[0]**2 + x[1]**2 - p[0]**2 # 圆形约束
# 自动微分
grad_x = cd.jacobian(cost, x) # ∂cost/∂x
hess_x = cd.hessian(cost, x) # ∂²cost/∂x²
3.2 动力学模型构建
文件: solver_generator/solver_model.py
class ContouringSecondOrderUnicycleModel(DynamicsModel):
def __init__(self):
super().__init__()
# 状态变量 (7维)
self.states = ["x", "y", "psi", "v", "theta_v", "a", "w"]
self.nx = 7
# 控制输入 (2维)
self.inputs = ["a", "w"] # 加速度、角加速度
self.nu = 2
def continuous_model(self, x, u):
"""
连续时间动力学: ẋ = f(x, u)
状态: x = [x, y, ψ, v, θ_v, a, ω]
控制: u = [u_a, u_ω]
"""
return np.array([
x[3] * cd.cos(x[2]), # ẋ = v·cos(ψ)
x[3] * cd.sin(x[2]), # ẏ = v·sin(ψ)
x[6], # ψ̇ = ω
x[5], # v̇ = a
cd.atan2(cd.sin(x[4]), cd.cos(x[4])), # θ̇_v (归一化)
u[0], # ȧ = u_a
u[1] # ω̇ = u_ω
])
def acados_symbolics(self):
"""创建Acados符号变量"""
x = cd.SX.sym("x", self.nx) # 状态
u = cd.SX.sym("u", self.nu) # 控制
z = cd.vertcat(u, x) # 组合 [u, x]
self.load(z)
return z
def get_acados_dynamics(self):
"""
返回显式和隐式动力学形式
显式: ẋ = f_expl(x, u)
隐式: f_impl(x, u, ẋ) = ẋ - f_expl(x, u) = 0
"""
# 创建状态导数符号
x_dot = cd.SX.sym("x_dot", self.nx)
# 显式动力学
f_expl = numpy_to_casadi(
self.continuous_model(
self._z[self.nu:], # x
self._z[:self.nu] # u
)
)
# 隐式动力学
f_impl = x_dot - f_expl
return f_expl, f_impl
关键点：
continuous_model: 定义 x ˙ = f ( x , u ) \dot{x} = f(x, u) x˙=f(x,u)f_expl: 显式形式，用于ERK积分器f_impl: 隐式形式，用于IRK积分器
3.3 代价函数构建
文件: solver_generator/solver_definition.py
def objective(modules, z, p, model, settings, stage_idx):
"""
构建阶段代价函数
参数:
modules: 模块管理器
z: 决策变量 [u, x]
p: 参数向量
model: 动力学模型
settings: 配置
stage_idx: 时间步索引
返回:
cost: CasADi符号表达式
"""
cost = 0.0
# 加载参数和状态
params = settings["params"]
params.load(p) # 将参数向量映射到命名参数
model.load(z) # 将决策变量映射到状态和控制
# 累加所有代价模块
for module in modules.modules:
if module.type == "objective":
cost += module.get_value(model, params, settings, stage_idx)
return cost
示例：GoalModule
# mpc_planner_modules/scripts/goal_module.py
class GoalObjective(Objective):
def get_value(self, model, params, settings, stage_idx):
# 获取当前位置（符号变量）
pos_x = model.get("x")
pos_y = model.get("y")
# 获取目标和权重（符号参数）
goal_x = params.get("goal_x")
goal_y = params.get("goal_y")
goal_weight = params.get("goal_weight")
# 构建代价表达式
dist_sq = (pos_x - goal_x)**2 + (pos_y - goal_y)**2
normalization = goal_x**2 + goal_y**2 + 0.01
cost = goal_weight * dist_sq / normalization
return cost # 返回CasADi符号表达式
3.4 约束构建
文件: solver_generator/solver_definition.py
def constraints(modules, z, p, model, settings, stage_idx):
"""
构建路径约束
返回:
constraints: CasADi符号表达式列表
"""
constraints = []
params = settings["params"]
params.load(p)
model.load(z)
# 收集所有约束模块
for module in modules.modules:
if module.type == "constraint":
for constraint in module.constraints:
constraints += constraint.get_constraints(
model, params, settings, stage_idx
)
return constraints
示例：EllipsoidConstraint
# mpc_planner_modules/scripts/ellipsoid_constraints.py
class EllipsoidConstraint:
def get_constraints(self, model, params, settings, stage_idx):
constraints = []
# 机器人位置
pos_x = model.get("x")
pos_y = model.get("y")
psi = model.get("psi")
# 对每个障碍物
for obs_id in range(self.max_obstacles):
# 障碍物参数
obst_x = params.get(f"ellipsoid_obst_{obs_id}_x")
obst_y = params.get(f"ellipsoid_obst_{obs_id}_y")
obst_psi = params.get(f"ellipsoid_obst_{obs_id}_psi")
obst_major = params.get(f"ellipsoid_obst_{obs_id}_major")
obst_minor = params.get(f"ellipsoid_obst_{obs_id}_minor")
# 构建椭圆矩阵 M
# M = R^T * diag(1/a^2, 1/b^2) * R
ab = cd.SX(2, 2)
ab[0, 0] = 1.0 / (obst_major**2)
ab[1, 1] = 1.0 / (obst_minor**2)
R = rotation_matrix(obst_psi)
M = R.T @ ab @ R
# 对每个碰撞检测圆盘
for disc_id in range(self.n_discs):
disc_offset = params.get(f"ego_disc_{disc_id}_offset")
# 计算圆盘位置
disc_x = pos_x + disc_offset * cd.cos(psi)
disc_y = pos_y + disc_offset * cd.sin(psi)
# 椭圆约束: (p - p_obs)^T M (p - p_obs)
diff = cd.vertcat(disc_x - obst_x, disc_y - obst_y)
constraint_value = diff.T @ M @ diff
constraints.append(constraint_value)
return constraints
def get_lower_bound(self):
# 约束下界: constraint_value >= 1.0
# 意味着机器人必须在椭圆外
return [1.0] * (self.max_obstacles * self.n_discs)
def get_upper_bound(self):
# 约束上界: constraint_value <= inf
return [np.inf] * (self.max_obstacles * self.n_discs)
约束的数学形式：
$$
h(x, u, p) = (p_{\text{disc}} - p_{\text{obs}})^T M (p_{\text{disc}} - p_{\text{obs}}) \in [1, \infty]
$$
其中：
- M = R ( ψ ) T diag ( 1 / a 2 , 1 / b 2 ) R ( ψ ) M = R(\psi)^T \text{diag}(1/a^2, 1/b^2) R(\psi) M=R(ψ)Tdiag(1/a2,1/b2)R(ψ)
- a , b a, b a,b 是椭圆的长轴和短轴
- R ( ψ ) R(\psi) R(ψ) 是旋转矩阵
4. Acados求解器配置
4.1 创建Acados模型
文件: solver_generator/generate_acados_solver.py
def create_acados_model(settings, model, modules):
"""
创建Acados OCP模型
返回:
acados_model: AcadosModel对象
"""
# 1. 创建模型对象
acados_model = AcadosModel()
acados_model.name = solver_name(settings) # 例如: "jackal_mpc_solver"
# 2. 创建符号变量
z = model.acados_symbolics() # z = [u, x]
# 3. 获取动力学
dyn_f_expl, dyn_f_impl = model.get_acados_dynamics()
# 4. 创建参数向量
params = settings["params"]
p = params.get_acados_p() # 符号参数向量
# 5. 构建约束
constr = cd.vertcat(*constraints(modules, z, p, model, settings, 1))
# 6. 构建代价
cost_stage = objective(modules, z, p, model, settings, 1)
cost_e = objective(modules, z, p, model, settings, settings["N"] - 1)
# 7. 填充Acados模型
acados_model.x = model.get_x() # 状态变量
acados_model.u = model.get_acados_u() # 控制输入
acados_model.xdot = model.get_acados_x_dot() # 状态导数
acados_model.f_expl_expr = dyn_f_expl # 显式动力学
acados_model.f_impl_expr = dyn_f_impl # 隐式动力学
acados_model.p = params.get_acados_parameters() # 参数
# 8. 设置代价
acados_model.cost_expr_ext_cost = cost_stage # 阶段代价
acados_model.cost_expr_ext_cost_e = cost_e # 终端代价
# 9. 设置约束
acados_model.con_h_expr = constr
return acados_model
AcadosModel的关键字段：
| 字段 | 类型 | 含义 |
|---|---|---|
x |
CasADi SX | 状态变量 x ∈ R n x x \in \mathbb{R}^{n_x} x∈Rnx |
u |
CasADi SX | 控制输入 u ∈ R n u u \in \mathbb{R}^{n_u} u∈Rnu |
xdot |
CasADi SX | 状态导数 x ˙ \dot{x} x˙ |
p |
CasADi SX | 参数 p ∈ R n p p \in \mathbb{R}^{n_p} p∈Rnp |
f_expl_expr |
CasADi SX | 显式动力学 f ( x , u , p ) f(x, u, p) f(x,u,p) |
f_impl_expr |
CasADi SX | 隐式动力学 F ( x , u , x ˙ , p ) = 0 F(x, u, \dot{x}, p) = 0 F(x,u,x˙,p)=0 |
cost_expr_ext_cost |
CasADi SX | 阶段代价 ℓ ( x , u , p ) \ell(x, u, p) ℓ(x,u,p) |
cost_expr_ext_cost_e |
CasADi SX | 终端代价 ℓ e ( x , p ) \ell_e(x, p) ℓe(x,p) |
con_h_expr |
CasADi SX | 路径约束 h ( x , u , p ) h(x, u, p) h(x,u,p) |
4.2 配置OCP求解器
def generate_acados_solver(modules, settings, model, skip_solver_generation):
# 1. 定义参数
params = AcadosParameters()
define_parameters(modules, params, settings)
settings["params"] = params
# 2. 创建Acados模型
model_acados = create_acados_model(settings, model, modules)
# 3. 创建OCP对象
ocp = AcadosOcp()
ocp.model = model_acados
# 4. 设置维度
ocp.dims.N = settings["N"] # 预测时域长度
# 5. 设置代价类型
ocp.cost.cost_type = "EXTERNAL" # 使用外部代价函数
# 6. 设置初始约束
ocp.constraints.x0 = np.zeros(model.nx) # 初始状态（运行时会更新）
# 7. 设置状态边界
ocp.constraints.lbx = model.lower_bound[model.nu:] # 状态下界
ocp.constraints.ubx = model.upper_bound[model.nu:] # 状态上界
ocp.constraints.idxbx = np.array(range(model.nx)) # 所有状态都有边界
# 8. 设置控制边界
ocp.constraints.lbu = model.lower_bound[:model.nu] # 控制下界
ocp.constraints.ubu = model.upper_bound[:model.nu] # 控制上界
ocp.constraints.idxbu = np.array(range(model.nu)) # 所有控制都有边界
# 9. 设置路径约束边界
ocp.constraints.lh = constraint_lower_bounds(modules) # h的下界
ocp.constraints.uh = constraint_upper_bounds(modules) # h的上界
# 10. 设置参数初值
ocp.parameter_values = np.zeros(model_acados.p.size()[0])
# 11. 设置时域
ocp.solver_options.tf = settings["N"] * settings["integrator_step"]
# 12. 配置求解器选项（详见下节）
configure_solver_options(ocp, settings)
# 13. 生成求解器
solver = AcadosOcpSolver(acados_ocp=ocp, json_file=json_file_name)
return solver
4.3 求解器选项详解
def configure_solver_options(ocp, settings):
"""配置Acados求解器选项"""
# ========== 积分器选项 ==========
ocp.solver_options.integrator_type = "ERK" # 显式Runge-Kutta
# 可选: "IRK" (隐式RK), "GNSF" (广义非线性结构)
ocp.solver_options.sim_method_num_stages = 4 # RK4: 4阶
ocp.solver_options.sim_method_num_steps = 3 # 每个时间步分3次积分
# ========== NLP求解器选项 ==========
ocp.solver_options.nlp_solver_type = "SQP" # 序列二次规划
# 可选: "SQP_RTI" (实时迭代), "DDP" (微分动态规划)
ocp.solver_options.hessian_approx = "EXACT" # 精确Hessian
# 可选: "GAUSS_NEWTON" (高斯-牛顿近似)
ocp.solver_options.regularize_method = "MIRROR" # 正则化方法
# 帮助收敛，避免奇异矩阵
ocp.solver_options.globalization = "FIXED_STEP" # 全局化策略
# 可选: "MERIT_BACKTRACKING" (回溯线搜索)
ocp.solver_options.qp_tol = 1e-5 # QP子问题容差
ocp.solver_options.tol = 1e-2 # NLP容差
# ========== QP求解器选项 ==========
ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM"
# 可选:
# - "FULL_CONDENSING_QPOASES": 全凝聚，适合小问题
# - "PARTIAL_CONDENSING_HPIPM": 部分凝聚，适合大问题
ocp.solver_options.qp_solver_iter_max = 50 # QP最大迭代次数
ocp.solver_options.qp_solver_warm_start = 2 # 热启动
# 0 = 冷启动, 1 = 原始热启动, 2 = 原始+对偶热启动
# ========== 代码生成选项 ==========
ocp.solver_options.print_level = 0 # 0 = 静默, 1 = 打印信息
ocp.code_export_directory = "acados/solver_name"
关键选项解释：
A. 积分器类型
| 类型 | 特点 | 适用场景 |
|---|---|---|
| ERK | 显式RK，快速 | 非刚性系统 |
| IRK | 隐式RK，稳定 | 刚性系统 |
| GNSF | 利用结构，最快 | 特殊结构系统 |
B. NLP求解器
| 类型 | 特点 | 性能 |
|---|---|---|
| SQP | 完整SQP，精确 | 高精度，慢 |
| SQP_RTI | 实时迭代，1次迭代 | 实时，快 |
SQP vs SQP_RTI：
SQP: 每次求解到收敛
┌─────┐
│ QP1 │ → 收敛? No → QP2 → ... → QPn → 收敛
└─────┘
SQP_RTI: 只做1次迭代
┌─────┐
│ QP1 │ → 直接返回
└─────┘
实时控制中，SQP_RTI更常用！
C. QP求解器
Full Condensing：
- 消除所有状态变量，只保留控制变量
- QP规模： n u × N n_u \times N nu×N
- 适合：小问题（N < 20）
Partial Condensing：
- 部分消除状态变量
- QP规模：介于两者之间
- 适合：大问题（N > 20）
4.4 松弛变量（Slack Variables）
# 可选：添加松弛变量，将硬约束变为软约束
add_slack = True
if add_slack:
nc = ocp.model.con_h_expr.shape[0] # 约束数量
# 为路径约束添加松弛
ocp.constraints.idxsh = np.array(range(nc))
# 松弛变量惩罚
penalty = 1.0e5
ocp.cost.zl = penalty * np.ones((nc,)) # 下界松弛惩罚
ocp.cost.zu = penalty * np.ones((nc,)) # 上界松弛惩罚
ocp.cost.Zl = penalty * np.ones((nc,)) # 二次惩罚
ocp.cost.Zu = penalty * np.ones((nc,))
松弛变量的作用：
原始硬约束：
$$
h(x, u) \geq h_l
$$
带松弛的软约束：
$$
\begin{aligned}
h(x, u) + s &\geq h_l \
s &\geq 0 \
\text{cost} &+= \text{penalty} \cdot s
\end{aligned}
$$
好处：
- ✅ 避免无解情况
- ✅ 提高鲁棒性
- ⚠️ 可能违反约束（但会被惩罚）
5. 代码生成与编译
5.1 Acados代码生成流程
# 生成求解器
solver = AcadosOcpSolver(acados_ocp=ocp, json_file=json_file_name)
内部流程：
┌─────────────────────────────────────────┐
│ 1. 符号表达式 → JSON │
│ - 保存所有符号表达式到JSON文件 │
└─────────────────────────────────────────┘
↓
┌─────────────────────────────────────────┐
│ 2. 自动微分 │
│ - 计算雅可比矩阵 ∂f/∂x, ∂f/∂u │
│ - 计算Hessian矩阵 ∂²L/∂x² │
│ - 使用CasADi的AD引擎 │
└─────────────────────────────────────────┘
↓
┌─────────────────────────────────────────┐
│ 3. C代码生成 │
│ - 生成求解器C代码 │
│ - 生成接口代码 │
│ - 使用Jinja2模板 │
└─────────────────────────────────────────┘
↓
┌─────────────────────────────────────────┐
│ 4. 编译 │
│ - gcc/clang编译 │
│ - 生成共享库 libacados_ocp_solver.so │
└─────────────────────────────────────────┘
5.2 求解器生成详细步骤
mpc_planner/
├── mpc_planner_jackalsimulator/ # Jackal机器人配置
│ ├── config/
│ │ └── settings.yaml # ← 主配置文件
│ └── scripts/
│ └── generate_jackalsimulator_solver.py # ← 生成脚本
│
├── solver_generator/ # 求解器生成器
│ ├── solver_model.py # 车辆模型定义
│ ├── generate_solver.py # 主生成函数
│ ├── generate_acados_solver.py # Acados求解器生成
│ ├── generate_cpp_files.py # C++代码生成
│ └── util/ # 工具函数
│
├── mpc_planner_modules/ # MPC模块
│ └── scripts/
│ ├── contouring.py # 路径跟踪模块
│ ├── ellipsoid_constraints.py # 椭圆避障模块
│ ├── guidance_constraints.py # 引导约束模块
│ └── ...
│
└── mpc_planner_solver/ # 生成的求解器（自动生成）
├── c_generated_code/ # Acados生成的C代码
├── include/ # C++头文件
└── src/ # C++源文件
（1）安装Acados
# 检查环境变量
echo $ACADOS_SOURCE_DIR
# 应输出: /path/to/acados
# 检查Python接口
python3 -c "from acados_template import AcadosOcp, AcadosOcpSolver; print('Acados OK')"
（2）配置settings.yaml 核心参数核心参数
name: "jackal" # 求解器名称
N: 30 # 预测时域长度
integrator_step: 0.2 # 积分步长 [s]
n_discs: 1 # 机器人圆盘数量
solver_settings:
solver: "acados" # 求解器类型 (acados/forces)
acados:
iterations: 10 # SQP迭代次数
solver_type: SQP_RTI # SQP_RTI (实时) 或 SQP (离线)
（3）设置模型、代价、约束
在generate_jackalsimulator_solver.py中的configuration_tmpc函数进行配置：
def configuration_tmpc(settings):
model, modules = configuration_no_obstacles(settings)
modules.add_module(GuidanceConstraintModule(
settings,
constraint_submodule=EllipsoidConstraintModule # This configures the obstacle avoidance used in each planner
))
# modules.add_module(GuidanceConstraintModule(settings, constraint_submodule=GaussianConstraintModule))
return model, modules
（4）运行脚本
python3 generate_jackalsimulator_solver.py
运行后，会在mpc_planner_solver中生成求解器c代码。会生成红色框里的代码。
（5）调用求解器
调用求解器已经写好，见mpc_planner_solver/src/acados_solver_interface.cpp，无需修改即可使用
有“AI”的1024 = 2048，欢迎大家加入2048 AI社区
更多推荐
26
0- 0
已为社区贡献1条内容
所有评论(0)