求两条轨迹间的hausdorff距离_轨迹规划之位置插补

假设我们有一个向量的序列

，如果我们想对这个序列进行插值，那么 我们可以分别对每一对向量

和

进行插值，然后将插值的曲线连接起来，也就 是我们所说的样条

（Spline）:

很明显，这个曲线虽然是连续的，但是它的一阶导数（切线）在切换插值向量时都不是连续的．为了解决这个问题，我们最常使用的就是Bézier曲线．直接使用一个四次Bézier曲线（因为有五个点）来生成这个近似曲线．但是Bézier曲线只会经过初始点与最终点（插值），一 般不会经过中间的控制点（近似），所以这样求出来的曲线虽然是可导的，但是插值 曲线不会经过中间的三个向量：

为了解决这个问题，我们可以分段对每两个向量

和

之间使用Bézier曲线进 行插值，之后将所有的曲线（样条）连接起来．因为我们需要让曲线的一阶导数（或 者说曲线的趋势）连续，我们还需要知道它们的前一个向量

和后一个向量

， 并且用它们生成两个控制点

和

来控制曲线的趋势．我们会使用

和

作为 端点（曲线会经过这两个点），

和

中间的控制点，使用一个三次Bézier曲 线（Cubic BézierCurve，四个点）来近似这个两个向量之间的插值．

在我们的例子中，因为我们一共有四对向量

，我们会使 用四个三次Bézier曲线对这五个点进行插值．对于三次Bézier曲线所产生 的样条，如果想让最终的插值曲线达到

连续，则需要让前一个样条在

的控制点与当前样条在

的控制点分别处于最终曲线在

处切线对等的两侧：

在上面的曲线中，蓝色的线就是曲线在点

处的切线，红色的点就是三次Bézier曲线的控制点，分别处于切线对等的两侧．对于两个端点

和

，我们直接将这两个向量的控制点取为它们本身（这不是唯一的做法，但这样是可行的），最终得到一个平滑的曲线．我们希望将类似的逻辑带到四元数的超球面上，得到四元数序列的插值的方法，但在此之前我们需要了解如何构造一个三次Bézier曲线.

如何构造一个三次Bézier曲线:

Bspline/B样条曲线：

根据控制点来生成曲线的。

常见的三种曲线关系如下：

曲线中的每个控制点都会影响整个曲线的形状。

克服了Bezier曲线的一些缺点，Bezier曲线的每个控制点对整条曲线都有影响，也就是说，改变一个控制点的位置，整条曲线的形状都会发生变化，而B样条中的每个控制点只会影响曲线的一段参数范围，从而实现了局部修改。

和

都是多项式参数曲线，不能表示一些基本的曲线，比如圆，所以引入了

来解决这个问题。

非均匀有理B样条函数(

)：

自由曲线插补；

机器人的运动轨迹一般都是圆弧或直线，而对复杂的运动往往用样条函数进行拟合。

核心算法：

递推作为基函数

Cox-deBoor递推算法：
In the mathematical subfield of numerical analysis de Boor's algorithmis a fast[vague][quantify] and numerically stable algorithm for evaluating spline curves in B-spline form. It is a generalization of de Casteljau's algorithm for Bézier curves. The algorithm was devised by Carl R. de Boor. Simplified, potentially faster variants of the de Boor algorithm have been created but they suffer from comparatively lower stability.

其中：
单个

称为节点

多个

序列组成

称为节点向量

（一行多列）

这是一个最简单的基函数（分段常函数），存在函数值不连续。

它是一个分段多项式，高阶的情况如下：

结论：

其他位置为

；

它为分段

次多项式；

更高阶的基函数，其反映真实情况的波函数会越不粗糙。

举个梨子：

其中，

，

因为分母

为区间长度，为不确定的数，

同理可得：

又因为此时，

所以，可以得出结论：

在

和

处被压成了0，在

处都被调成了

，其图形为一个以

为三个顶点的“三角形”，

保证了函数的连续性。

推广到高阶的公式：

中，

同样是条两条直线方程围成的“三角形”。

：为

次规范的

基函数，最高次数为

满足关系

其中：

为节点数；

为控制点个数；

为基函数的最高次数。

方程为：

其中，

为控制点坐标，控制多边形。

举个梨子，如果输入控制点个数

个，那么

，则

，节点数

本来输入的是6个控制点，但是，只要通过给真正的变量

设置不一样的步长，输出的可是几百上千个点，用点与点间的直线，形成的折现去近似、拟合曲线。

仿真的结果如图：

直线插补：

适用于：末端在p2p间运动轨迹是直线。
较简单，不做赘述。

圆弧插补：

适用于：根据两端点间的插补数字信息，计算出逼近实际圆弧的点集。
可以证明，等闲得不行再码出来。

---

如果有帮助的话，请点赞或者关注支持一下~