[论文解读] A Convex Optimization Approach to Smooth Trajectories for Motion Planning with Car-Like Robots

Zhijie Zhu, Edward Schmerling+1 more|arXiv (Cornell University)|Jun 2, 2015

Robotic Path Planning Algorithms参考文献 21被引用 26

一句话总结

本文提出了一种名为凸弹性平滑（Convex Elastic Smoothing, CES）的算法，这是一种针对轮式机器人快速且实时的轨迹平滑方法，通过交替进行基于凸优化的形状优化与速度优化。通过将轨迹建模为无碰撞管内的弹性带，并利用凸规划求解两个步骤，CES 在 800 毫秒内即可生成平滑且动力学可行的路径，显著提升了自动驾驶应用中的轨迹质量和可行性。

ABSTRACT

In the recent past, several sampling-based algorithms have been proposed to compute trajectories that are collision-free and dynamically-feasible. However, the outputs of such algorithms are notoriously jagged. In this paper, by focusing on robots with car-like dynamics, we present a fast and simple heuristic algorithm, named Convex Elastic Smoothing (CES) algorithm, for trajectory smoothing and speed optimization. The CES algorithm is inspired by earlier work on elastic band planning and iteratively performs shape and speed optimization. The key feature of the algorithm is that both optimization problems can be solved via convex programming, making CES particularly fast. A range of numerical experiments show that the CES algorithm returns high-quality solutions in a matter of a few hundreds of milliseconds and hence appears amenable to a real-time implementation.

研究动机与目标

• 解决基于采样的运动规划器为轮式机器人生成的轨迹存在锯齿状、动力学不可行的问题。
• 开发一种快速、实时的轨迹平滑算法，确保在转向半径和摩擦力等约束下的动力学可行性。
• 通过凸优化将形状优化与速度重参数化整合到一个高效统一的框架中。
• 通过确保计算时间低于 1 秒，实现自动驾驶车辆中的实际部署。
• 通过最小化整体曲率并实现速度优化，同时保持避障能力，从而提升轨迹质量。

提出的方法

• CES 算法采用两阶段迭代过程：首先是弹性形状优化，然后是速度优化，两者均被表述为凸优化问题。
• 形状优化通过在初始轨迹周围定义一个‘管状’区域，并最小化一个平滑性代价函数来实现，该函数对高曲率施加惩罚。
• 速度优化通过凸规划实现，以在满足加速度和转向半径等动态约束的前提下最大化速度。
• 该算法利用微分平坦性，将自行车模型的轨迹映射回半车辆模型，以实现更真实的车辆动力学。
• 气泡生成技术在初始路径周围创建一个无碰撞的管状区域，确保平滑后的轨迹保持避障。
• 该方法受弹性带规划的启发，但用凸公式替代了非凸优化，从而实现快速收敛。

实验结果

研究问题

• RQ1 基于凸优化的方法是否能在满足动态约束的前提下，实现对轮式机器人的实时轨迹平滑？
• RQ2 与传统的快捷方法（如摆线或贝塞尔样条）相比，CES 在路径长度和光滑性方面表现如何？
• RQ3 在复杂且障碍物密集的环境中，CES 算法的计算效率如何？
• RQ4 对于采样规划器生成的不准确或非动力学可行的初始轨迹，CES 的鲁棒性如何？
• RQ5 该算法能否在保持动力学可行性和光滑性的同时，有效优化轨迹上的速度？

主要发现

• CES 算法在复杂崎岖地形场景下，总计算时间为 798 毫秒，可在 800 毫秒内生成高质量且动力学可行的轨迹。
• 在麋鹿测试场景中，CES 生成的路径仅比基于摆线的解决方案长 0.17%，表现出高度的精度与光滑性。
• 与原始锯齿状轨迹相比，该算法显著减少了路径长度和行驶时间，提升了低层控制器的可跟踪性。
• 在两次迭代中，速度优化耗时 31 毫秒，形状优化耗时 203 毫秒，显示出强大的计算效率。
• 该方法对初始轨迹模型具有鲁棒性，在参考路径由更简单的模型（如单轨车模型）生成时仍表现良好。
• 该算法的计算时间与实时自动驾驶应用兼容，尤其在较简单的城市场景中表现更优。