ICRA 2025 | 自动驾驶 & 机器人通用:基于 STL 的路径积分轨迹规划新方法

在机器人导航、自动驾驶这些领域，轨迹规划一直是核心难题。想象一下，让自动驾驶汽车既要精准到达目的地，又要全程避开障碍物，还得满足“在某个时间段内经过指定区域”这类复杂要求，传统方法要么算得慢，要么容易出错。最近，ICRA 2025上一篇关于轨迹规划的论文，给出了一套超高效的解决方案，今天就带大家搞懂这个能让机器人“聪明规划”的新方法。

论文信息

题目： Trajectory Planning with Signal Temporal Logic Costs using Deterministic Path Integral Optimization

基于确定性路径积分优化的信号时序逻辑代价轨迹规划

作者： Patrick Halder, Hannes Homburger, Lothar Kiltz, Johannes Reuter, Matthias Althoff

先搞懂：为啥传统轨迹规划不好用？

咱们先说说现状。想要让机器人、自动驾驶车按要求运动，得先把“要做什么、不能做什么”说清楚。信号时序逻辑（STL）就是干这个的——它能精准定义各种时空规则，比如“全程避开障碍物”“最终到达目标区域”“在10秒内经过指定路口”。

但问题来了：

1. 这些规则一旦复杂起来，比如又要避障又要按顺序经过多个区域，传统方法会出现“组合爆炸”，计算量呈指数级增长；
2. STL的核心计算指标（鲁棒性）存在不可微分、非凸的特点，基于梯度的优化方法要么得修改原始问题，要么算出来的结果不准；
3. 就算是一些采样类方法，也容易收敛慢，没法满足实时控制的需求。

简单说，传统方法要么算得慢，要么算不准，面对复杂任务根本扛不住。

核心突破：确定性路径积分优化，搞定STL代价难题

这篇论文最核心的创新，就是把模型预测路径积分（MPPI）控制框架，和STL代价结合起来，搞出了一套全新的轨迹规划方法。咱们不用懂复杂公式，就看它到底牛在哪：

创新点1：不用平滑近似，直接处理STL的“硬茬”

STL的鲁棒性度量是个“刺头”——不可微分、非凸，传统方法要么绕着走，要么把它“磨平”（平滑处理）再计算，结果自然会有偏差。

这篇论文的方法不搞这些花活，直接把STL鲁棒性度量当成代价函数的一部分，融入MPPI框架。不用修改原始问题，也不用做近似，从根源上保证了计算的准确性。

创新点2：状态增广+方差缩减，从“随机”到“精准”

MPPI原本是随机采样的方法，直接用会有偏差，没法得到确定性问题的精确解。作者想出了两个关键招：

• 状态增广：把机器人过去的状态信息都纳入当前状态里，解决了STL代价依赖完整轨迹的问题；
• 方差渐进缩减：迭代减小采样分布的方差和逆温度参数，让随机采样的结果慢慢收敛到确定性问题的精确解，彻底克服了标准MPPI的偏差问题。

创新点3：并行计算+提前终止，适配实时场景

这套方法继承了MPPI天然的并行性优势——能同时采样、评估大量轨迹，计算效率直接拉满。更贴心的是，算法支持“提前终止”，就算没算到最后，中间得到的解也能用，特别适合自动驾驶、机器人实时控制这类对时间敏感的场景。

方法总览：一步步找到最优轨迹

咱们用通俗的话拆解下算法的核心流程（对应论文里的算法1和图1）：

1. 先把原始的STL最优控制问题，通过“状态增广”转换成MPPI能处理的标准形式；
2. 初始化采样的方差和逆温度参数，开始迭代计算；
3. 每次迭代都采样大量轨迹，计算每条轨迹的代价（包括STL约束的满足程度）；
4. 用加权的方式，从采样轨迹里算出当前迭代的最优解；
5. 逐步缩小采样方差和逆温度，让解越来越精准；
6. 迭代到一定次数，就得到满足所有STL约束的最优轨迹。

（图1：基于PI的解决方案示例，目标是最终到达蓝色方框，同时始终避开灰色圆形障碍物。能清晰看到四个中间解的迭代过程，方差慢慢缩小，轨迹越来越精准）

实测见真章：三个难题，吊打传统方法

作者做了三个不同复杂度的实验，把新方法（PI）和混合整数规划（MIP）、梯度法（GRAD）、平滑梯度法（SGRAD）、进化策略（CMA-ES）这些主流方法比了个遍，结果超亮眼。

实验1：标量积分器——简单场景验证有效性

第一个实验是让一个简单的积分器系统，在10个时间步内最大化最终位置，同时要至少两个时间步状态低于1。

结果显示：除了GRAD方法，其他方法都能满足约束，但PI方法的收敛速度更快，计算时间远低于MIP这类方法。

（图2：PI求解器针对这个问题的解，能看到四次迭代的采样轨迹——随着迭代次数增加，采样的方差越来越小，轨迹最终收敛到最优解，完美满足“至少两个时间步低于1+最大化最终位置”的要求）

实验2：简单运动规划——避障+到达目标

第二个实验是双积分器系统的运动规划：要在15个时间步内到达蓝色矩形区域，同时全程避开灰色圆形障碍物。

结果：所有方法都能满足约束，但PI方法的收敛时间是最短的。CMA-ES因为要在两个最优解里做选择，计算时间明显变长；GRAD和SGRAD因为要处理非线性的避障约束，速度也远不如PI。

实验3：复杂车辆模型——多约束高难度任务

第三个实验最能体现实力：用复杂的车辆单轨模型，规划50个时间步的轨迹，要求包括：

• 全程待在指定区域内；
• 避开5个障碍物；
• 至少连续两个时间步经过3个任务区域（顺序不限）；
• 不同时出现在两个指定任务区域。

结果让人大吃一惊：

• PI方法只用了25.07秒就收敛，CMA-ES用了79.82秒；
• MIP、GRAD、SGRAD这些方法，在1小时的时间限制内，根本没算出可行解！

（图3：PI方法给出的复杂车辆模型轨迹解。能清楚看到车辆避开了所有障碍物，依次经过了三个任务区域，全程待在指定范围内，完美满足所有复杂的STL约束）

总结：不止于轨迹规划，未来潜力无限

这篇论文的价值，不只是提出了一个高效的轨迹规划方法，更重要的是解决了STL约束下最优控制的核心痛点：

• 不用修改原始问题，直接处理不可微分、非凸的STL代价；
• 收敛快、精度高，还能并行计算，适配实时应用；
• 在复杂非线性、高维问题上，秒杀传统方法。

未来，作者还计划把这套方法扩展到带优先级的STL规范，以及模型预测控制场景中。相信在自动驾驶、机器人导航、无人机控制这些领域，这套方法会成为解决复杂时空约束规划问题的“利器”。

最后提一句，论文的代码已经开源，感兴趣的小伙伴可以去看看。毕竟在实际应用中，能又快又准地搞定复杂轨迹规划，才是真的能落地、有价值的技术。