Amy青梅1. 基于优化的移动机器人轨迹规划：从理论到实践
在移动机器人领域，轨迹规划是连接决策与执行的关键环节。传统基于采样的方法（如RRT*）虽然能保证全局最优性，但往往难以满足实时性要求；而基于优化的方法通过数学建模直接求解，在保证动态可行性的同时，能够实现毫秒级的响应速度。本文将深入解析基于优化的轨迹规划技术，特别聚焦约束条件下的解决方案。
1.1 约束轨迹优化的数学本质
约束轨迹优化问题可以抽象为一个带约束的最优控制问题：
$$
\begin{aligned}
\min_{z(t), T} &\quad \int_0^T v(t)^\top W v(t) dt + \rho(T) \
\text{s.t.} &\quad z^{(s)}(t) = v(t), \quad \forall t \in [0, T] \
&\quad \mathcal{G}(z(t), \dot{z}(t), \dots, z^{(s)}(t)) \leq 0 \
&\quad z(t) \in \mathcal{F} \
&\quad \text{边界条件约束}
\end{aligned}
$$
其中各参数含义：
- $z(t)$：轨迹参数化函数（通常为多项式）
- $T$：轨迹总时间
- $W$：权重矩阵，调节优化目标偏好
- $\mathcal{G}$：动力学约束函数
- $\mathcal{F}$：自由空间（避障约束）
实际工程中，95%以上的问题会采用5次或7次多项式参数化轨迹，因为它们在平滑性和计算复杂度之间取得了良好平衡。
2. 凸优化简化方法详解
2.1 问题凸化的核心技术路线
将非凸的原始问题转化为凸优化问题，需要解决四个核心挑战：
- 最优参数化形式未知 → 固定采用B样条或多项式基函数
- 时空自由度耦合 → 解耦为路径参数化和时间分配两步
- 动态可行性非凸 → 用凸包近似动力学约束
- 安全区域复杂 → 构建凸的飞行走廊(Flight Corridor)
2.2 飞行走廊构建实战
飞行走廊是连接起止点的一系列凸多面体（通常为立方体或棱柱），其构建流程：
- 初始路径搜索：使用A或RRT生成一条几何路径
- 空间膨胀：沿路径进行基于距离场的膨胀操作
- 凸分解：将复杂空间分解为凸多面体的并集
- 连续性保证：确保相邻走廊有足够重叠区域
python复制# 走廊生成伪代码示例
def build_flight_corridor(start, goal, obstacles):
path = A_star_search(start, goal, obstacles)
corridor = []
for waypoint in path:
free_space = inflate(calculate_clear_space(waypoint, obstacles))
convex_hull = quickhull(free_space)
corridor.append(convex_hull)
return merge_overlapping_regions(corridor)
2.3 多项式轨迹优化实现
采用分段多项式参数化时，第i段轨迹表示为：
$$
f_i(t) = \sum_{k=0}^N p_{i,k} \left(\frac{t-t_i}{\Delta t_i}\right)^k
$$
构建QP问题的关键步骤：
-
代价函数设计：通常最小化jerk或snap
$$ J = \sum_{i=1}^M \int_0^{\Delta t_i} \left| \frac{d^3 f_i}{dt^3} \right|^2 dt $$ -
约束矩阵构建：
- 连续性约束：$A_{eq}p = b_{eq}$
- 走廊约束：$A_{ineq}p \leq b_{ineq}$
-
时间归一化处理：将每段时间区间映射到[0,1]提高数值稳定性
3. 时空变形优化技术
3.1 时空联合优化框架
传统方法先固定时间分配再优化路径，而时空变形方法同步优化：
- 空间形变：调整路径几何形状
- 时间形变：重新参数化时间轴
优化变量扩展为：
$$ \min_{z(t), T(t)} J(z,T) $$
其中$T(t)$是单调递增的时间映射函数。
3.2 自适应时间分配策略
通过引入时间权重函数$w(t)$实现动态调速：
- 狭窄区域：增大$w(t)$ → 时间膨胀 → 低速通过
- 开阔区域：减小$w(t)$ → 时间压缩 → 高速通过
实际实现常采用对数-线性变换：
$$ \tau(t) = \frac{T}{1+e^{-k(t-t_0)}} $$
3.3 动力学约束处理技巧
速度/加速度约束通过边界检查实现：
matlab复制% MATLAB约束检查示例
function [c, ceq] = dynamics_constraints(p)
v_max = 2.0; a_max = 5.0;
[v, a] = compute_derivatives(p);
c = [max(abs(v)) - v_max;
max(abs(a)) - a_max];
ceq = [];
end
4. 工程实践关键问题
4.1 求解器选择基准测试
| 求解器 | 问题类型 | 最大变量数 | 典型求解时间 |
|---|---|---|---|
| OSQP | QP | 10^4 | <10ms |
| MOSEK | SOCP | 10^5 | ~50ms |
| IPOPT | NLP | 10^3 | ~100ms |
实测表明：对于50段多项式轨迹优化，OSQP在i7-11800H上平均求解时间仅3.2ms
4.2 数值稳定性保障措施
- 预处理：对变量进行标准化（零均值单位方差）
- 正则化：添加小量对角矩阵防止Hessian矩阵奇异
$$ H \leftarrow H + \epsilon I $$ - 条件数监控：实时检查矩阵条件数，超过阈值时触发重构
4.3 实时性优化方案
- 热启动：复用上一帧的解作为初始猜测
- 并行计算：使用OpenMP加速雅可比矩阵计算
- 增量更新：仅优化受环境变化影响的轨迹段
5. 典型问题与解决方案
5.1 轨迹震荡问题
现象：生成的轨迹出现高频抖动
原因：优化权重设置不当导致过拟合
解决方案：
- 增加jerk项权重
- 采用低通滤波后处理
- 引入运动学平滑项：
$$ J_{smooth} = \int \left( \frac{d^2 \kappa}{ds^2} \right)^2 ds $$
其中$\kappa$为曲率
5.2 狭窄通道穿越
挑战：严格的空间约束导致无解
创新方法：
- 引入安全裕度自适应调整：
$$ d_{safe} = \max(d_{min}, \frac{v^2}{2a_{max}}) $$ - 使用Bezier曲线精确满足约束
- 采用微分平坦空间规划
5.3 动态障碍物处理
实时响应流程：
- 感知层：检测障碍物运动状态
- 预测层：预估未来轨迹（常采用CV/CTRV模型）
- 规划层：构建时空安全区域：
$$ \mathcal{F}(t) = { x | \min_i |x-o_i(t)| > r_i } $$
6. 前沿进展与未来方向
6.1 学习增强的优化方法
- 学习型求解器：用GNN预测QP问题的初始解
- 约束神经网络：将约束编码为网络层
- 模仿学习：从专家演示中学习优化权重
6.2 分布式优化架构
多机协同规划框架：
- 顶层：分布式MPC处理全局任务
- 中层：基于ADMM的轨迹协商
- 底层：局部轨迹优化
6.3 硬件在环优化
利用GPU并行计算实现：
- 100Hz以上的实时规划
- 大规模多智能体轨迹优化
- 复杂动态环境的快速响应
在实际的移动机器人项目中，我们通常会采用分层架构：上层使用基于采样的方法生成全局路径，下层用本文介绍的优化方法进行局部调整。这种组合既保证了全局可行性，又能满足实时性要求。特别是在无人机和AGV应用中，这种方案已经证明了其可靠性——某物流AGV项目通过该技术将碰撞率降低至0.01次/千小时以下。