Python库中各种插值函数的使用

为了比较不同的插值函数，这边使用了两种数据来进行验证，一种是随机的数据，一种是从 cos 轨迹上取点进行插值。

import numpy as np
import matplotlib.pyplot as plt
import scipy.interpolate as spi
from scipy.interpolate import BSpline
from IPython import embed

数据1： 随机的数据

data = [[0, 0],
[1, 1],
[5, 2],
[10, 3],
[20, 4],
[40, 5],
[100, 6],
[101, 7],
[101, 8],
[101, 9],
[60, 10],
[40, 11],
[-100, 12],
[40, 13],
[50, 14],
[60, 15],
[70, 16],
[80, 17],
[90, 18],
[100, 19],
[110, 20],
[100, 21],
[90, 22],
[50, 23],
[10, 24],
[0, 25]]
Y = [row[0] for row in data]
X = [row[1] for row in data]
time_list = np.arange(0, X[-1],0.001) 

数据2： 从 cos轨迹中取值

X = np.linspace(0, 2*np.pi, 10)
Y = np.cos(X)
time_list = np.linspace(0, 2*np.pi, int(2*np.pi * 1000))

B样条

## B样条曲线： BSpline(t, c, k, extrapolate=True, axis=0)
## t:  B样条节点
## c： B样条系数
## k 表示 B样条曲线的阶次
## 该函数需要用户自己定义好 c 才能使用，没有下面的函数来的方便
# i_f = BSpline(X,Y, k = 2)

## 一维曲线的 B样条 表示，XY 是 Y = F(X) 的数据点
i_f = spi.splrep(X,Y, k=3) 
pos = spi.splev(time_list, i_f) 
vel = np.dot(np.diff(pos), 1000)
acc = np.dot(np.diff(vel), 1000)

三次样条

# 1：三次样条： 自然边界，边界点的二阶导数为0
# 2：三次样条： 固定边界，边界点导数给定为0
# 3：三次样条： 非节点边界，使边界点的三阶导与这两边界端点的邻近点的三阶导相等
## 自然边界
i_f = spi.CubicSpline(X, Y, bc_type= 'natural')
## 固定边界
# i_f = spi.CubicSpline(X, Y, bc_type= 'clamped')
## 非节点边界
# i_f = spi.CubicSpline(X, Y, bc_type= 'not-a-knot')
pos = i_f(time_list)
vel = np.dot(np.diff(pos), 1000)
acc = np.dot(np.diff(vel), 1000)

Akmia

i_f = spi.Akima1DInterpolator(X, Y)
pos = i_f(time_list)
vel = np.dot(np.diff(pos), 1000)
acc = np.dot(np.diff(vel), 1000)

画图显示

	plt.figure()
	plt.plot(t_given, q_given, 'ro', label='given pos')
	plt.plot(time_list, pos, 'r', label='pos')
	plt.plot(time_list[:-1], vel, 'b', label='vel')
	plt.plot(time_list[:-2], acc,  label='acc')
	plt.grid('on')
	plt.legend(loc='upper right')
	plt.xlabel('time[s]')
	plt.show()


	plt.figure()
	plt.plot(t_given, q_given, 'ro', label='given pos')
	plt.plot(time_list, pos, 'r', label='pos')
	plt.grid('on')
	plt.legend(loc='upper right')
	plt.xlabel('time[s]')
	plt.show()


	plt.figure()
	plt.plot(time_list[:-1], vel, 'b', label='vel')
	plt.grid('on')
	plt.legend(loc='upper right')
	plt.xlabel('time[s]')
	plt.show()

	plt.figure()
	plt.plot(time_list[:-2], acc, 'b', label='acc')
	plt.grid('on')
	plt.legend(loc='upper right')
	plt.xlabel('time[s]')
	plt.show()

数据1 的比较结果

位置比较示意图

从下面两个位置图中，可以很明显的看出， Akima 的插值能比较好的拟合给定的插值点，不会出现两个插值点之间有一个很大的波动。
这种轨迹波动的特性在实际应用的时候有可能会出现不好的结果，比如说引发机器人自身的碰撞。举例来说，对于一般的工业机器人而言， 2轴的关节极限是【50， -130】，当用户2轴的插值点为【48， 47】，两个点之间的时间间隔为1s,那么当出现图中这种数据波动的时候，那么就有可能会规划出超出关节极限的轨迹，或者说在运动过程中出现2轴和3轴发生自碰撞的现象。这种在实际应用的时候可以考虑限制用户下发的点位的时间间隔，或者轨迹规划完成之后，对轨迹进行检查以避免该问题。

速度比较示意图

从速度曲线中来，Akima的速度曲线没有其他插值曲线那么平滑。三次样条的固定边界的插值曲线起点和终点速度为0，其他插值曲线的起点和终点速度不为0。

加速度比较示意图

从加速度曲线中来，Akima的加速度曲线并不连续，存在突变，当加速度比较大的时候，有可能会引起机器人运动过程中的异响，或者引发机器人的速度保护。
三次样条的自然边界的插值曲线起点和终点加速度为0，其他插值曲线的起点和终点加速度不为0。

数据2 的比较结果

位置比较示意图

速度比较示意图

加速度比较示意图

全部代码如下

import numpy as np
import matplotlib.pyplot as plt
import scipy.interpolate as spi
from IPython import embed

use_data = 2
if use_data == 1:
	data = [[0, 0],
	[1, 1],
	[5, 2],
	[10, 3],
	[20, 4],
	[40, 5],
	[100, 6],
	[101, 7],
	[101, 8],
	[101, 9],
	[60, 10],
	[40, 11],
	[-100, 12],
	[40, 13],
	[50, 14],
	[60, 15],
	[70, 16],
	[80, 17],
	[90, 18],
	[100, 19],
	[110, 20],
	[100, 21],
	[90, 22],
	[50, 23],
	[10, 24],
	[0, 25]]
	Y = [row[0] for row in data]
	X = [row[1] for row in data]
	time_list = np.arange(0, X[-1],0.001) 

if use_data == 2:
	X = np.linspace(0, 2*np.pi, 10)
	Y = np.cos(X)
	time_list = np.linspace(0, 2*np.pi, int(2*np.pi * 1000))


b_i_f = spi.splrep(X,Y, k=3) 
b_pos = spi.splev(time_list, b_i_f) 
b_vel = np.dot(np.diff(b_pos), 1000)
b_acc = np.dot(np.diff(b_vel), 1000)


c1_i_f = spi.CubicSpline(X, Y, bc_type= 'natural')
c1_pos = c1_i_f(time_list)
c1_vel = np.dot(np.diff(c1_pos), 1000)
c1_acc = np.dot(np.diff(c1_vel), 1000)


c2_i_f = spi.CubicSpline(X, Y, bc_type= 'clamped')
c2_pos = c2_i_f(time_list)
c2_vel = np.dot(np.diff(c2_pos), 1000)
c2_acc = np.dot(np.diff(c2_vel), 1000)


c3_i_f = spi.CubicSpline(X, Y, bc_type= 'not-a-knot')
c3_pos = c3_i_f(time_list)
c3_vel = np.dot(np.diff(c3_pos), 1000)
c3_acc = np.dot(np.diff(c3_vel), 1000)

a_i_f = spi.Akima1DInterpolator(X, Y)
a_pos = a_i_f(time_list)
a_vel = np.dot(np.diff(a_pos), 1000)
a_acc = np.dot(np.diff(a_vel), 1000)


plt.figure()
plt.plot(X, Y, 'ro', label='given pos')
plt.plot(time_list, b_pos, label='B spline')
plt.plot(time_list, c1_pos, label='cubic natural')
plt.plot(time_list, c2_pos, label='cubic clamped')
plt.plot(time_list, c3_pos, label='cubic not-a-knot')
plt.plot(time_list, a_pos, label='Akima')
plt.grid('on')
plt.legend(loc='upper right')
plt.xlabel('time[s]')
plt.show()

plt.figure()
plt.plot(time_list[:-1], b_vel, label='B spline')
plt.plot(time_list[:-1], c1_vel, label='cubic natural')
plt.plot(time_list[:-1], c2_vel, label='cubic clamped')
plt.plot(time_list[:-1], c3_vel, label='cubic not-a-knot')
plt.plot(time_list[:-1], a_vel, label='Akima')
plt.grid('on')
plt.legend(loc='upper right')
plt.xlabel('time[s]')
plt.show()

plt.figure()
plt.plot(time_list[:-2], b_acc, label='B spline')
plt.plot(time_list[:-2], c1_acc, label='cubic natural')
plt.plot(time_list[:-2], c2_acc, label='cubic clamped')
plt.plot(time_list[:-2], c3_acc, label='cubic not-a-knot')
plt.plot(time_list[:-2], a_acc, label='Akima')
plt.grid('on')
plt.legend(loc='upper right')
plt.xlabel('time[s]')
plt.show()