1. Hermite插值计算


书接上回:机器人路径规划之分段三次 Hermite 插值(PCHIP)(上)[快速上手],在通过快速调用API实现功能之后,这节让我们来分析一下这个很有意思的插值过程。

上节说过,Hermite插值是一种不但要求插值多项式函数值与原函数值相同,同时还要求在节点处,插值多项式的一阶直甚至高阶的导数值也与被插函数的相应阶导数值相等的插值方法。那么具体的如何通过Hermite插值的方法进行插值计算呢?

现在我们通过一个简单的例子求一下Hermite多项式,先假设一个函数:

f(x)=x3+1

我们以点 (0, 1) 和 (1, 2) 以及两点的导数 f ' (0) = 0,f ' (1) = 3 来求Hermite插值多项式。

这里有 便可求得  了(参考Lagrange插值法, 是插值基函数):

已知

首先计算

P0(x) = (xx1)/(x0x1) = (x1)/(01) = 1x

P1(x) = (xx0)/(x1x0) = (x1)/(1−0) = x

接着计算

α0 = (12(x0)/(01)) (1x)2 = (1+2x)(1x)2 = 13x2+2x3

α1 = (12(x−1)/(1−0)) x2 = 3x22x3

计算

β0(x) = (x0)(1x)2 = x2x2+x3

β1(x) = (x−1)x2 = x3x2

计算

代入上面各式求得:

H3(x) = x3 + 1

至此,我们求得了开始设定的 f(x)。

2. 单调性判断


在上一节提到,我们希望对Hermite插值多项式进行分段处理,在一段单调的插值点上保持其插值曲线的单调性,接下来,我们就需要对插值点进行单调性判断了。

2.1 条件 1 — 数据点梯度的符号必须等于割线梯度的符号

我们在这里考虑单个三次多项式(例如,对于一对数据点)。 令 Δi 是连接两个数据点的割线的梯度,其中 Δi = pi+1 pi , 为了保持数据点的单调性,这个梯度的符号决定了我们是希望这个三次多项式是增加还是减少,并且由于我们希望这个三次多项式是单调的,所以这条曲线上所有点的梯度符号必须一致(并匹配 Δi 的符号)。

如果任一端点的梯度的符号与 Δi 的符号不同,那么我们知道曲线不会保持数据点的单调性。 例如,在下图中,数据点正在减少,因此我们希望多项式上所有点的梯度为负,而终点的梯度为正:

2.2 条件 2 — 一对相等的数据点值必须具有零数据点梯度

如果两个数据点具有完全相同的值,则连接这两个点的割线的梯度将恰好为零(例如 Δi = 0)。 在这种情况下,我们希望在两个数据点之间看到一条水平直线,因此端点梯度必须为零。 如果任一梯度不为零,则这两个点之间的曲线将不是单调的:

2.3 条件 3 — 当 αi + βi − 2 ≤ 0 时,条件1能保证单调性

现在让我们假设上述两个条件都为真,例如:

sign(Δi) = sign(δi) = sign(δi+1)

Δi ≠ 0

此时,可以方便地根据参数 t 重新排列三次Hermite多项式,代入 Δi 我们有:

g(t) = (δi + δi+1 − 2Δi) t3 + (−2δi − δi+1 + 3Δi)t2 + δit + pi

这个函数的一阶导数如下:

g' (t) = 3(δi + δi+1 − 2Δi) t2 + 2(−2δi − δi+1 + 3Δi) t + δi

接下来,我们需要考虑以下两种情况:

  • δi + δi+1 − 2Δi = 0 时:

g(t) 简化为二次方程,g' (t) 变为线性,因此要么是常数,要么增加,要么减少。 条件 sign(Δi) = sign(δi) = sign(δi+1) 已经可以阻止 g' (t) 改变符号,所以仅此条件就足以确保曲线保持单调。

  • δi + δi+1 − 2Δi ≠ 0 时:在这种情况下,导数 g' (t) 是二次方程,因此 g' (t) = 0 在 t 中最多有两个唯一解,为了保持单调性,我们需要确保在 t ∈ [0,1] 中没有解。

在这种情况下,我们来探索一下 g' (t) 和 g' (t) 的两个正交属性:

    • 端点之间的割线梯度 Δi 是正还是负;
    • 导数 g' (t) 是凸函数还是凹函数。

case 1:Δi > 0,g' (t) 是凸函数,g' (t) = 0 在 t ∈ [0,1] 中无解,因此通过条件1就能满足单调性:

case 2:Δi > 0,g' (t) 是凹函数,g' (t) = 0 在 t ∈ [0,1] 中有解,因此不满足单调性:

case 3:Δi < 0,g' (t) 是凹函数,g' (t) = 0 在 t ∈ [0,1] 中无解,因此通过条件1就能满足单调性:

case 4:Δi < 0,g' (t) 是凸函数,g' (t) = 0 在 t ∈ [0,1] 中有解,因此不满足单调性:

因此,对于这四种情况中的两种情况,条件 1 已经保持了单调性。对于其余情况,从上面可以看出,大的梯度值会导致一阶导数改变符号,因此不再保持单调性。

总结一下:

  • 如果 δi + δi+1 − 2Δi > 0 且 Δi < 0,则保持单调性
  • 如果 δi + δi+1 − 2Δi < 0 且 Δi > 0,则保持单调性

接下来将整个条件除以 Δi 并让 αi = δi / Δi 以及 βi = δi+1/Δi ,便可以将上述两条和为一个条件:

  • 如果 αi + βi − 2 ≤ 0,则条件 1 确保保持单调性

我们将这个约束在图上绘制出来就会很清晰了,其中 α β 分别是 x 轴和 y 轴。 如果三次多项式的 αi βi 值在绿色区域内,那么三次多项式是单调的:

2.4 条件 4 — 当 αi + βi − 2 > 0 时,需要限制梯度 

上文分析了 αi + βi − 2 ≤ 0 的情况,那么在 αi + βi − 2 > 0 的其余三次多项式中,我们可以在上节的图中看到,如果端点梯度恰好足够极端,一阶导数可能会改变符号并产生非单调曲线。

在剩下的情况中,当一阶导数的局部极值在 0 ≤ t ≤ 1 时偏离 g' (t) = 0 时,就会发生这种情况。

为了确定何时发生这种情况,我们可以检查二阶导数:

g''(t) = 6(δi + δi+1 − 2Δi) t + 2(−2δi − δi+1 + 3Δi)

g''(t) = 0 并除以 Δi(并且替换为 α β ),我们可以找到出现该局部极值的 t 值:

t = (1/3) (2α + β − 3) / (α + 2β − 3)

  • 如果这个局部极值不在 (0, 1) 范围内,那么对于该范围内曲线上的所有点的一阶导数一定是单调的,因为条件 1 已经确保两个端点的一阶导数符号的一致性,一阶导数不会改变符号,因此在这种情况下曲线本身就是单调的。 由于 αi + βi − 2 > 0,当以下任一情况为真时,就会发生这种情况:

2α + β 3 ≤ 0

α + 2β 3 ≤ 0

通过在单调性图上绘制这些附加区域,我们可以清楚地看到这一点:

  • 如果这个局部极值在范围 (0, 1) 内,那么我们要确保它不会改变符号。 也就是说,局部极值的符号必须与 Δi 的符号一致(因为如果一阶导数改变了符号,我们的曲线就不是单调的)。

该极值处的一阶导数如下:

我们希望:

sign(g'(t)) = sign(Δi)

这意味着我们的曲线必须满足以下条件才能是单调的:

α - [(2α + β − 3)2 / (α + 2β − 3)] / 3 ≥ 0

将此约束添加到我们的单调性图中,为我们提供了一个完整的单调性区域——任何 αi βi 值落在此范围内的三次多项式都是单调的:

此时,我们可以通过限制 αi βi (以及 δiδi+1)的值来确保它们在这个安全区域内,从而使任何的三次 Hermite 多项式单调。

3. 代码实现


接下来我们先抛去上一篇文章中的 scipy 库函数,仅通过numpy库来用Python实现一下分段三次Hermite插值多项式,这里直接贴函数的代码(未加维数判断和数据合法性判断,仅适用于二维合法数据)

  • 函数输入:data 为插值点 (x, y) 组成的二维数组,frequence 为采样频率(即输出曲线中首尾两插值点之间的采样点个数)
  • 函数输出:插值之后的二维数组,此时的 x 是以 frequence 为采样频率的new_x,此时的 y 是根据new_x对应的函数值。
def Cubic_Hermite_Spline(data, frequence):

    h = np.diff(data[:,0])
    delta = np.diff(data[:,1]) / np.tile(h, (1, 1))
    delta = delta[0,:]
    slopes = np.zeros(np.size(data[:,1]))

    # get slopes
    n = len(data[:,0])
    if n == 2:
        slopes = np.tile(delta[0], np.size(data[:,1]))
    else:
        aa = np.sign(delta[:-1]) * np.sign(delta[1:])
        k = np.where(aa == 1)[0]
        h = np.diff(data[:,0])
        hs = h[k] + h[k+1]
        w1 = (h[k] + hs) / (3*hs)
        w2 = (hs + h[k+1]) / (3*hs)
        dmax = np.maximum(abs(delta[k]), abs(delta[k+1]))
        dmin = np.minimum(abs(delta[k]), abs(delta[k+1]))
        slopes = np.zeros(np.size(data[:,1]))
        slopes[k+1] = dmin / (w1 * (delta[k]/dmax) + w2 * (delta[k+1]/dmax))
        slopes[0] = 0

    # Hermite spline
    data_num = data.shape[0]
    data_len = np.round((data[-1,0])*frequence).astype(int)+1
    time_stamp = np.linspace(0,data_len-1,data_len)/frequence
    Cubic_Hermit = np.zeros((data_len,2)) 
    Cubic_Hermit[-1,1] = data[-1,1]
    print(data[-1,1])
    Cubic_Hermit[-1,1] = data[-1,1] # fill in the end point
    Cubic_Hermit[:,0] = time_stamp  # fill in the tims stamp
    for i in range(data_num-1):
        t_diff = data[i+1,0] - data[i,0] # time difference between two key points
        
        # calculation of K matrix of for the neighbour frames 
        K = np.matrix([[2/(t_diff**3),-2/(t_diff**3),1/(t_diff**2),1/(t_diff**2)]
        ,[-3/(t_diff**2),3/(t_diff**2),-2/(t_diff),-1/(t_diff)],[0,0,1,0],[1,0,0,0]])

        # adjustment of the raw key point data
        f_mat = np.matrix([[data[i,1],data[i+1,1],slopes[i],slopes[i+1]]]).T
        f_mat_adj = f_mat

        # a_i, b_i, c_i, d_i of the polynomial for the current interval
        abcd = K*f_mat_adj
        # current interval spline data number
        cur_data_num = np.round((data[i+1,0]-data[i,0])*frequence).astype(int)
        cur_time_range = np.linspace(data[i,0],data[i+1,0]-1/frequence, cur_data_num)
        cur_CHS_index = np.round(cur_time_range*frequence).astype(int)
        cur_time_range = cur_time_range - data[i,0]

        Cubic_Hermit[cur_CHS_index,1] = (cur_time_range**3)*abcd[0,0] \
            + (cur_time_range**2)*abcd[1,0] + cur_time_range*abcd[2,0] + abcd[3,0]
    return Cubic_Hermit


完整代码如下:

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

frequency = 100

data = np.array([(0, 4),(1, 3),(2, 4), (3, 6), (5, 7), (6, 5), (8, 10), (11, 1)])

def Cubic_Hermite_Spline(data, frequence):

    h = np.diff(data[:,0])
    delta = np.diff(data[:,1]) / np.tile(h, (1, 1))
    delta = delta[0,:]
    slopes = np.zeros(np.size(data[:,1]))

    # get slopes
    n = len(data[:,0])
    if n == 2:
        slopes = np.tile(delta[0], np.size(data[:,1]))
    else:
        aa = np.sign(delta[:-1]) * np.sign(delta[1:])
        k = np.where(aa == 1)[0]
        h = np.diff(data[:,0])
        hs = h[k] + h[k+1]
        w1 = (h[k] + hs) / (3*hs)
        w2 = (hs + h[k+1]) / (3*hs)
        dmax = np.maximum(abs(delta[k]), abs(delta[k+1]))
        dmin = np.minimum(abs(delta[k]), abs(delta[k+1]))
        slopes = np.zeros(np.size(data[:,1]))
        slopes[k+1] = dmin / (w1 * (delta[k]/dmax) + w2 * (delta[k+1]/dmax))
        slopes[0] = 0

    # Hermite spline
    data_num = data.shape[0]
    data_len = np.round((data[-1,0])*frequence).astype(int)+1
    time_stamp = np.linspace(0,data_len-1,data_len)/frequence
    Cubic_Hermit = np.zeros((data_len,2)) 
    Cubic_Hermit[-1,1] = data[-1,1]
    print(data[-1,1])
    Cubic_Hermit[-1,1] = data[-1,1] # fill in the end point
    Cubic_Hermit[:,0] = time_stamp  # fill in the tims stamp
    for i in range(data_num-1):
        t_diff = data[i+1,0] - data[i,0] # time difference between two key points
        
        # calculation of K matrix of for the neighbour frames 
        K = np.matrix([[2/(t_diff**3),-2/(t_diff**3),1/(t_diff**2),1/(t_diff**2)]
        ,[-3/(t_diff**2),3/(t_diff**2),-2/(t_diff),-1/(t_diff)],[0,0,1,0],[1,0,0,0]])

        # adjustment of the raw key point data
        f_mat = np.matrix([[data[i,1],data[i+1,1],slopes[i],slopes[i+1]]]).T
        f_mat_adj = f_mat

        # a_i, b_i, c_i, d_i of the polynomial for the current interval
        abcd = K*f_mat_adj
        # current interval spline data number
        cur_data_num = np.round((data[i+1,0]-data[i,0])*frequence).astype(int)
        cur_time_range = np.linspace(data[i,0],data[i+1,0]-1/frequence, cur_data_num)
        cur_CHS_index = np.round(cur_time_range*frequence).astype(int)
        cur_time_range = cur_time_range - data[i,0]

        Cubic_Hermit[cur_CHS_index,1] = (cur_time_range**3)*abcd[0,0] \
            + (cur_time_range**2)*abcd[1,0] + cur_time_range*abcd[2,0] + abcd[3,0]
    return Cubic_Hermit

f_hermite = Cubic_Hermite_Spline(data, 100)
f_hermite_scipy = interpolate.PchipInterpolator(data[:,0], data[:,1])
x_new = np.linspace(data[:,0].min(), data[:,0].max(), frequency)
plt.plot(data[:,0], data[:,1], "o", label="Data")
plt.plot(x_new, f_hermite_scipy(x_new), label="Pchip_scipy")
plt.plot(f_hermite[:,0], f_hermite[:,1], label="Pchip")
plt.ylabel("y")
plt.xlabel("x")
plt.legend()
plt.show()

结果如下:

可以看出,除了首末点外,我们自己写的 Pchip 函数和 scipy 中的 Pchip 函数几乎一模一样,这是因为在我们的函数中,考虑到机器人的启动和停止,分别对首末点的导数设置为0,而 scipy 中首末点并没有这么做,如此看来,我们自己写的 Pchip函数反而对机器人的路径插值更加友好。


[参考资料]:

  1. Fritsch F N, Carlson R E. Monotone Piecewise Cubic Interpolation[J]. SIAM Journal on Numerical Analysis, 1980, 17(2): 238-246.
  2. Lekkas A M, Fossen T I. Integral LOS path following for curved paths based on a monotone cubic Hermite spline parametrization[J]. IEEE Transactions on Control Systems Technology, 2014, 22(6): 2287-2301.
  3. James Bird - Monotone Cubic Interpolation: https://jbrd.github.io/2020/12/27/monotone-cubic-interpolation.html