机器人导航路径平滑算法：一条符合机器人运动限制的路径
文章转自 帅气的为为 https://zhuanlan.zhihu.com/p/364421182
介绍
机器人通过栅格地图进行路径规划时，根据静态障碍物得到全局路径，本阶段暂不考虑动态障碍物。通过各种路径规划算法，如 Dijkstra’s，A*，D-star，RRT等，规划出的路径都存在直线之间有急剧拐弯（曲率变化大）的问题。
尽管通过将八邻域改为更多邻域，如前文所述，能略微改善曲率变化急剧的问题，但是这样的路径仍然不适于机器人运动模型，尤其是非全向机器人，如阿克曼底盘，所以需要一条符合机器人运动限制的路径。
路径连续性
连续性包括两个部分：几何连续 ，参数连续 。
几何连续指路径的多个片段的起始点相连，而且切向量的方向相同。
参数连续指路径的多个片段的起始点相连，而且切向量的方向和大小相同。
如果两曲线在点 处的第 阶导数相等，那么该两曲线在点 处是 连续，同时也是 连续的，对于 。总之，曲线 的 阶导数 是连续的话，则是 连续曲线。
（a）不连续曲线段 （b）C^0连续 （c）C^1连续 （d）C^2连续
如上所述，参数连续意味着曲线和其参数化的平滑，而集合连续仅意味着机器人轨迹平滑。例如 连续意味着切向量连续，而 连续意味着斜率连续； 连续意味着加速度矢量连续，而 连续意味着曲率连续。对于机器人运动来说， 连续保持速度， 连续保持加速度。对于机器人路径规划来说，关键是路径是否 或者 连续。高阶连续如 主要处理面，用于 CAD/CAM 设计。
插值法
- 多项式插值
对于 个点 ，可以用拉格朗日（Lagrange）插值 ，其阶 ：
，其中 。
另一种是赫米特（Hermite）插值，定义 阶的 在 处为0：
， ，其中 。
插值法用于路径平滑十分原始，有两个缺点：计算复杂度高；Runge’s 现象。
2. 贝塞尔曲线
对于 个点 ，对应的贝塞尔曲线定义为：
，其中 ， 。
3. 三次样条曲线
样条曲线适合于对任意方程建模，对于低阶曲线也有较好的效果，能防止 Runge’s 现象。
有两个重要特点：用最小阶数就可以产生 近似；对于小曲率也是足够平滑。
考虑 区间内有序的 个点 ，以及相应的分析 ， 。因为该样条曲线是3阶的，其二阶导数必须是连续的，因此定义 ， ， ，于是有：
其中 ， ，并且
可以从 连续系统求得： 。
4. B样条曲线
给定 个实数 ， ， 阶的 B 样条曲线：
由基本的 阶 B 样条曲线 线性组成：
其中 称为控制点， 个控制点形成一个凸包（convex hull），对于 这些基本 B 样条曲线定义为：
当控制点的个数比阶数多一个时，例如 ， ，B 样条曲线退化为贝塞尔曲线。
不同的控制点和阶数
5. NURBS 曲线
直接给定义：
其中 是阶数， 是 B 样条曲线基本方程， 是控制点， 的权重 是齐次点 的最后一个维度。
特殊曲线
- Dubin’s 曲线
给定平面内两点和运动方向，Dubins 用圆弧和线段在给定曲率范围内找到连接各点的最短平滑路径，如图所示。
Durbin’s 曲线简单但是有效，适用于实时处理因为计算简单，可以用于回环线来满足各种不同限制。
首先介绍一个简单的车，如图所示。
它的 C 空间 ，定义为 ，车子运动方程如下：
时间间隔 极小时，车子大致走后轮指向。 趋近于0，意味着 ，因为 ， ，于是有：
要满足该限制，方程的解为： ， ， 为常数。
下面解关于 的方程， 为汽车转弯半径， ，因为 ，有
两边同时除以 ，因为 ，有
假定速度 和转向角 等同于动作参数 和 ，动作向量 ，汽车动力学方程为：
因为车子有最小转弯半径 ，而路径规划的任务是最小化起点到终点的车子运动曲线，如果 那么就没有曲率限制，走两点间直线距离最短，但实际上需要优化
，其中 是到达终点 的时间，如果没有到达则 。
因为速度恒定，运动方程可以简化为：
其中 属于区间 ，对于 ，以下结果都成立：
| Symbol | Steering: u |
|---|---|
| S | 0 |
| L | 1 |
| R | -1 |
在[1]中已经证明，在任何两个空间之间，Dubins 汽车的最短路径可以由不超过三个基本运动的组合表示，所以动作空间为 ，只有如下六个关键词是最优的：
为了更加准确，要表示每个基本动作的持续时间。对于 或 ，加入下标表示转角，对于 ，加入下标表示距离，因此 Dubins 曲线可以更加精确的表示为：
其中 ， ， 。如图所示两种可能情况。
也可以用 来代表曲线，即 或 ，那么6个词可以简化为仅2个词：
更准确的版本为：
当 时，分割如下图所示：
已有了关键信息，但是对于给定的 和 ，仍存在两个问题：
六个词中哪些可以生成最短路径？
对于某个词，相应下标的值 ， ， 是多少？
一个简单的方法就是穷举法，如图所示。
2. Clothoid
Clothoid，也被称为欧拉螺旋线或者 Cornu’s 螺旋线，表示在复平面：
其中 和 是菲涅尔函数，有时候也成为菲涅尔余弦积分和菲涅尔正弦积分。
当 时两个菲涅尔函数都趋近于 ，所以这个曲线逐渐趋近于第一象限 ，对称的来说，因为两个方程都是奇函数，这个曲线逐渐趋近于第一象限 。
Cornu’s 螺旋线的曲率和曲线距离成正比，因此车子的角加速度恒定，意味着驾驶员匀速转动方向盘即可，更加安全。
3. Hypocycloid
当一个半径 小圆在大一点的半径 大圆内滚动时，小圆上的某固定点产生：
其中 是峰点（曲线不可导的地方）的个数， 是两圆半径比即 ，例子如图所示
如果在 hypocycloid 内有 个峰点，小圆半径则为 ，因为小圆转 圈回到原点，也就生成 个峰点。
有一种新的方法成为 Smooth Hypocycloidal Paths（SHP），可以生成任何角度，该算法用来尽量保持直线路径，只平滑急转弯处，例子如图所示
4. Reeds-Shepp Curves
和 Dubins 相似，直接给运动方程：
其中 ， 。第一个变量 决定变速箱向前（ ）或向后（ ），为了简化假定 ， 对于所有 都成立。
因此词袋为：
其中 表示变速箱逆向，即向前到向后，或向后到向前。
下图举例 。
下表为 Reeds and Shepp 的48种不同曲线，然而第一行的 和 可以被剔除，因此只有46种。
5. Balkcom-Mason 曲线
将动作集定义为 ，优化方程
因此可以有四个基本运动：
可证明得出最短路径最多由五个基本部分组成，因此有9个词即：
准确形式由下表给出：
为了显示的更易懂，给出九种运动模型图：
当 ， 时，最优曲线如图所示：
[1] Dubins, L.E. (1957) On Curves of Minimal Length with a Constraint on Average Curvature, and with Prescribed Initial and Terminal Positions and Tangents. American Journal of Mathematics, 79, 497-516.