文献阅读之路径平滑

发布时间：2024-08-26 05:15浏览次数： 来源于：网络

我之前的一篇博客介绍了如何规划平滑的过弯道的轨迹，本篇介绍如何在已知路径的前提下，对其平滑，同时还要满足机器人/车辆的运动学约束。这2篇不同之处在于前者同时关注路径和速度，后者只关注路径；前者已知环境信息，后者不仅已知环境信息，还已知初步规划出的可行无碰的路径。

silver bullet：文献阅读之如何规划一条让车平稳顺滑过弯道的轨迹

• 输入是path（来自于图搜索或采样式路径规划方法）和steer function，输出是符合车辆运动学约束的可行驶的path。分为2个步骤，a:使用梯度下降法，移动和插入顶点使得离障碍物更远、离障碍物近的地方路径点更密集；b:使用基于cost的short-cutting方法删除不必要的路径点。

• 对不同的motion planner（图搜索和采样式路径规划方法）和不同的steer function（Dubins, Reeds-Shepp等，即运动学约束）、不同的平滑方法进行对比测试，在运行时间、path长度、max steer、无碰方面效果很好。

• 算法的细节介绍比较清晰，smooth过程考虑了运动学约束，是通过考察2点间能否steer function可达实现的，即文中的2P-BVP。
• 给出了需要调整的参数及其意义，以及参数推荐值。
• 对应的源代码：https://github.com/eric-heiden/grips。其中的example给出了论文中的图片展示结果。
• 没考虑动态障碍物。

---

这篇文章主要介绍Junior的 Hybrid A* + conjugate gradient smoothing 的实现过程，和《Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments》这篇文章一起，是我介绍一系列“初步path + 优化平滑” 套路来解决非结构化环境中路径规划问题的开端。我在这里仅从代码实现角度介绍其使用的目标函数设计，细节请看原文。

• Voronoi Field计算公式如下：

• 优化的目标函数如下：

• ?Voronoi 项的偏导如下（KTH的代码在实现时没有考虑这一项，即最后一篇文章）：

• 障碍物项的偏导如下：

• ?curvature 项的偏导如下：（疑问：我按求导公式计算curvature的偏导时，下面3项的第一个符号是正而不是负，第二个符号是负。可是我的结果与Junior以及下文Enhanced...的结果都不同，为什么呢？）

---

这篇文章主要介绍在实现Junior的 Hybrid A* + conjugate gradient smoothing 时遇到的问题及提出的解决方法。

• 问题1：当给curvature项很大的权重时，或除curvature外其他项的权重为0时，平滑后的路径会有“kinks”（扭结，我也不知道翻译成什么才能更清晰传神一些了），如图1所示。
• 解决方法1：使用不同于Junior的curvature对x的偏导计算，公式如图2所示，不同在于每一项的正负符号（粗体加黑）。其实KTH的文章中也使用了不同的正负符号，但是在代码中却完全依照了Junior的公式。
• 效果1：如图3所示。（注：描述问题的图中，绿色是平滑前，红色是平滑后。而描述效果的图中，绿色是平滑前，红色是修正平滑方法前，蓝色是修正后）

• 问题2：当平滑的优化过程迭代次数较多时，path会逐渐“向内坍缩”，如图4-5所示。
• 解决方法2：添加2个新的迭代终止条件，分别是a. 当path的平均曲率由逐渐减小变成了逐渐变大；b. 当path中的某点曲率超过了最大曲率限制。
• 效果2：如图6所示。

• 问题3：平滑后的path在终点处不满足期望的朝向heading angle。如图7所示。
• 解决方法3：计算曲率时为每个点引入下标相关的权重，使用logistic function计算。
• 效果3：如图9所示。

---

这篇文章给出了实现代码 https://github.com/karlkurzer/path_planner，具有很大的参考价值，基本上是Junior思路的简化版本实现，没有使用共轭梯度下降法，而是使用了普通的梯度下降法。

https://github.com/karlkurzer/path_planner

• 优化的目标函数：

• ?障碍物项：?

• curvature项：

• smoothness项：

• Voronoi 项：

?

• 因为仿真测试只使用了障碍物项和smoothness项，没用Voronoi项和curvature项，可以发现平滑后的path有一些不必要的转折，如下图。说明代价函数还是需要调整权重的。

• 关于 karlkurzer/path_planner工程的解析和改进请见 梯度下降法实现路径平滑 一文。

karlkurzer/path_plannersilver bullet：梯度下降法实现路径平滑

下一篇：NBA视频_5
上一篇：“双循环”新发展格局下企业转型发展的机理、路径与政策建议