机器人路径规划之分段三次 Hermite 插值(PCHIP)(上)

在机器人的路径规划中针对离散采样点做插值计算生成平滑的曲线轨迹也是挺重要的一部分,本文主要引出一下目前使用较多也是个人觉得挺好用的一个插值方法——分段三次 Hermite 插值(PCHIP),并附上Python和Matlab的代码实现。

1. 插值

假设我们希望对离散的数据点 (x1, y1), …, (xn, yn) 来近似一个变量 f(x) 的连续函数,此处我们需要该连续函数完全经过各个数据点,这就是插值(另一种根据离散数据点求近似曲线的方法是拟合,拟合则是得到最接近的结果,强调最小方差的概念,不需要完全经过所有的点)。

我们先将一组离散点画在二维图中:

最简单的插值方法是在每对数据点之间定义分段线性函数(线性插值):

在Python中的实现很简单,可以使用 scipy 库中的 interpolate.interp1d 函数:(完整代码见最后)

x = [0, 1, 2, 3, 5, 6, 8,  11]
y = [4, 3, 4, 6, 7, 5, 10, 1]
x_new = np.linspace(0, 11, 100)
f_linear = interpolate.interp1d(x, y, kind = 'linear')
plt.plot(x_new, f_linear(x_new), label="linear")

线性插值易于实现且评估速度快,通常是计算机图形学中的合适选择。但是线性插值并不会产生平滑的曲线,想象一下机械臂沿着近似函数曲线以恒定速度运动,不连续的导数会导致机械臂在每个数据点瞬间改变方向,而不是平滑地改变方向,在实际的应用中是非常危险的。

为了纠正这个问题,可以在每对数据点之间选择一个高阶多项式,我们可以编写这个多项式的梯度,以确保整体近似函数是连续的,并且具有连续的导数。这里的插值多项式的方法有很多选择,比如拉格朗日插值法,牛顿插值法,埃尔米特(Hermite)插值,三次样条插值等。

我们先用三次B样条曲线插值看一下效果:

x = [0, 1, 2, 3, 5, 6, 8,  11]
y = [4, 3, 4, 6, 7, 5, 10, 1]
x_new = np.linspace(0, 11, 100)
f_cubic = interpolate.interp1d(x, y, kind = 'cubic')
plt.plot(x_new, y_bspline, label="B-spline")

我们可以看出,整条函数曲线变得更加的平滑——整体近似既是连续的,又具有连续的一阶导数,它解决了线性插值情况下梯度的突然变化。 但这也引入了一个问题——曲线现在超出了我们的数据点值的范围。

想象一下,在机械臂或者移动机器人的运动中,这中插值曲线会使得运动更加平滑,但是这种数据的超调却让我们无法接受:在运动过程中机器人跑到了设定目标点的范围之外了。

我们仔细观察一下曲线可以看出,超调的原因在于,离散点的极值并不是插值曲线的极值,此时,有一种插值方法的优势就凸显出来了。

2. Hermite插值法

Hermite插值不但要求插值多项式函数值与原函数值相同,同时还要求在节点处,插值多项式的一阶直甚至高阶的导数值也与被插函数的相应阶导数值相等。

简单点说就是,我们不但可以定义插值点的坐标,还可以定义每个插值点的导数。

这时我们可以用Python中 scipy 包的函数实现一下:

x = [0, 1, 2, 3, 5, 6, 8,  11]
y = [4, 3, 4, 6, 7, 5, 10, 1]
x_new = np.linspace(0, 11, 100)
f_hermite = interpolate.KroghInterpolator(x, y)
plt.plot(x_new, f_hermite(x_new), label="hermite")

好像效果比三次样条还差,不过回想一下我们似乎没有对插值点定义导数

重新做一边,将插值点的极点导数设置为0:

(通过在x数组中重复填写x值,对应的y值填写相应的导数值)

x = [0, 1, 1, 2, 3, 5, 5, 6, 8, 8, 11, 11]
y = [4, 3, 0, 4, 6, 7, 0, 5, 10, 0, 1, 0]
x_new = np.linspace(0, 11, 100)
f_hermite = interpolate.KroghInterpolator(x, y)
plt.plot(x_new, f_hermite(x_new), label="hermite")

emmmmmm,效果一言难尽,让我们放大看一下

可以看出,在我们设定点上,导数确实为0,但是同时也出现了“龙格现象(Runge phenomenon)”,所谓龙格现象,可以简单的解释为插值多项式的震荡。

通过下图,我们可以看出这个问题出现的原因——如图4个插值点在y中是严格增加的,结果曲线却不是:

3. 分段三次 Hermite 插值

此时,我们想一下,是否可以通过某种方法来保持插值点和曲线的单调性来解决这个问题,答案是肯定的,先说结论:通过分段三次 Hermite 插值 (PCHIP) 进行替代,可以有效的实现我们想要的结果。

在Python中我们可以简单的通过 scipy 库来实现:

x = [0, 1, 2, 3, 5, 6, 8,  11]
y = [4, 3, 4, 6, 7, 5, 10, 1]
x_new = np.linspace(0, 11, 100)
f_pchip = interpolate.PchipInterpolator(x, y)
plt.plot(x_new, f_pchip(x_new), label="pchip")

分段三次 Hermite 插值多项式的具体实现方法也挺有意思,在下一节中我会来介绍一下关于PCHIP的理论部分的内容。


接下来附上全部的测试代码:

Python:

import numpy as np
import matplotlib.pyplot as plt
from scipy import interpolate

x = [0, 1, 2, 3, 5, 6, 8,  11]
y = [4, 3, 4, 6, 7, 5, 10, 1]
x_h = [0, 1, 1, 2, 3, 5, 5, 6, 6,  8, 8, 11, 11]
y_h = [4, 3, 0, 4, 6, 7, 0, 5, 0, 10, 0, 1, 0]
x_new = np.linspace(0, 11, 100)

f_linear = interpolate.interp1d(x, y, kind = 'linear')
f_quadratic = interpolate.interp1d(x, y, kind = 'quadratic')
f_cubic = interpolate.interp1d(x, y, kind = 'cubic')
f_hermite = interpolate.KroghInterpolator(x, y)
f_pchip = interpolate.PchipInterpolator(x, y)

plt.plot(x, y, "o", label="data")
plt.plot(x_new, f_linear(x_new), label="linear")
plt.plot(x_new, f_quadratic(x_new), label="quadratic")
plt.plot(x_new, f_cubic(x_new), label="cubic")
plt.plot(x_new, f_hermite(x_new), label="hermite")
plt.plot(x_new, f_pchip(x_new), label="pchip")
plt.ylabel("y")
plt.xlabel("x")
plt.legend()
plt.show()

Matlab:
Matlab的代码实现就更简单了,用自身的函数即可:

x = [0, 1, 2, 3, 5, 6, 8,  11];
y = [4, 3, 4, 6, 7, 5, 10, 1];
x_new = [0:0.1:11];
f_pchip = pchip(x, y, x_new);
plot(x, y, 'o', x_new, f_phip, 'r-')

[参考资料]:
1.scipy 的 interp1d API 说明:scipy.interpolate.interp1d
2.scipy 的 KroghInterpolator API 说明:scipy.interpolate.KroghInterpolator
3.Github 上 James Bird 老哥的文档:Monotone Cubic Interpolation