PTPVT 插值说明

PTPVT 插值说明

PT模式： 位置-时间路径插值算法。
PVT模式： 位置-速度-时间路径插值算法。可以使用三次多项式或者 Hermite 算法进行插值。

PT算法对于点位距离比较小的运动或者低速度的运动比较合适。由于其很少的计算量，因此计算速度很快。
一般可以直接用在伺服驱动器中，比如说控制器的控制周期是 1ms, 那么伺服驱动器就可以在每 1ms 内的时间间隔内使用 PT 插值，可以 1 ms 内插值16个点位，以使得运动更加的精细。PT 插值的加速度是不连续的，存在突变。

PVT算法对于平滑轨迹和轨迹跟踪比较有用。位置轨迹点可以间隔很近，也可以间隔很大。例如：对于复杂的路径，那么点位需要间隔很近，这是为了防止两个点之间由于插值算法导致波动比较大；对于简单的路径，那么点位可以间隔很大。

PTPVT 插值说明

Hermite 插值满足在节点上等于给定函数值，而且在节点上的导数值也等于给定的导数值。对于高阶导数的情况，Hermite插值多项式比较复杂，在实际情况中，常常遇到的是函数值与一阶导数给定的情况。

当给定一阶导致一样的时候， Hermite 插值就和三次多项式插值得到的结果是一样的。
`

# 该函数只适用于两个点之间的时间间隔是 1 的插值
def PVT(p0,p1,v0,v1,n):# 两点三次Hermite曲线的时间参数方程# t 的范围就是【0， 1】之间,# t = 0, traj = p0, d_traj = v0# t = 1, traj = p1, d_traj = v1dt = 1/ntt = np.linspace(0, 1 - dt, n)traj = []for i in range(len(tt)):t = tt[i]H0 = 1 - 3*np.power(t, 2) + 2*np.power(t, 3)H1 = t - 2*np.power(t, 2)+ np.power(t, 3)H2 = 3*np.power(t, 2) - 2*np.power(t, 3)H3 = np.power(t, 3) - np.power(t, 2)traj.append(H0 * p0 + H1*v0 + H2*p1 + H3*v1)return traj

PVT Hermite插值

# 两点三次Hermite曲线的时间方程
# 当 t0 = 0, t1 = 1 的时候， PVT1 得到的结果和 PVT 的结果是一样的
def PVT1(p0, p1, v0, v1, t0, t1):t = t0traj = []n =  (int)(np.round(((t1 - t0)/0.001)))i = 0while i < n:alpha0 = (1 + 2*((t-t0) / (t1-t0))) * ((t-t1) / (t0-t1))**2alpha1 = (1 + 2*((t-t1) / (t0-t1))) * ((t-t0) / (t1-t0))**2beta0 =  (t-t0) * ((t-t1) / (t0-t1))**2beta1 =  (t-t1) * ((t-t0) / (t1-t0))**2traj.append(p0*alpha0 + p1*alpha1 + v0*beta0 + v1*beta1)t = t + 0.001i = i + 1return traj

PVT 三次多项式插值

# 三次多项式
def PVT2(p0,p1,v0,v1,n):traj = []t = 0for i in range((n)):T = n * 0.001h = p1 - p0a0 = p0a1 = v0a2 = (3*h - (2*v0 + v1)*T) / (T**2)a3 = (-2*h + (v0 + v1)*T) / (T**3)traj.append(a0 + a1*(t) + a2*(t)**2 + a3*(t)**3)t = t + 0.001return traj

PT 插值

# 使用 PT插值的形式
def PT(p0,p1,v, n):traj = []t = 0for i in range((n)):traj.append(p0 + v * t)t = t + 0.001return traj

Sin轨迹测试结果

import numpy as np
import time
import matplotlib
matplotlib.use("tkagg")
import matplotlib.pyplot as plt
from enum import Enum
from IPython import embed# 该函数只适用于两个点之间的时间间隔是 1 的插值
def PVT(p0,p1,v0,v1,n):# 两点三次Hermite曲线的时间参数方程# t 的范围就是【0， 1】之间,# t = 0, traj = p0, d_traj = v0# t = 1, traj = p1, d_traj = v1dt = 1/ntt = np.linspace(0, 1 - dt, n)traj = []for i in range(len(tt)):t = tt[i]H0 = 1 - 3*np.power(t, 2) + 2*np.power(t, 3)H1 = t - 2*np.power(t, 2)+ np.power(t, 3)H2 = 3*np.power(t, 2) - 2*np.power(t, 3)H3 = np.power(t, 3) - np.power(t, 2)traj.append(H0 * p0 + H1*v0 + H2*p1 + H3*v1)return traj# 两点三次Hermite曲线的时间方程
# 当 t0 = 0, t1 = 1 的时候， PVT1 得到的结果和 PVT 的结果是一样的
def PVT1(p0, p1, v0, v1, t0, t1):t = t0traj = []n =  (int)(np.round(((t1 - t0)/0.001)))i = 0while i < n:alpha0 = (1 + 2*((t-t0) / (t1-t0))) * ((t-t1) / (t0-t1))**2alpha1 = (1 + 2*((t-t1) / (t0-t1))) * ((t-t0) / (t1-t0))**2beta0 =  (t-t0) * ((t-t1) / (t0-t1))**2beta1 =  (t-t1) * ((t-t0) / (t1-t0))**2traj.append(p0*alpha0 + p1*alpha1 + v0*beta0 + v1*beta1)t = t + 0.001i = i + 1return traj# 三次多项式
def PVT2(p0,p1,v0,v1,n):traj = []t = 0for i in range((n)):T = n * 0.001h = p1 - p0a0 = p0a1 = v0a2 = (3*h - (2*v0 + v1)*T) / (T**2)a3 = (-2*h + (v0 + v1)*T) / (T**3)traj.append(a0 + a1*(t) + a2*(t)**2 + a3*(t)**3)t = t + 0.001return traj# 使用 PT插值的形式
def PT(p0,p1,v, n):traj = []t = 0for i in range((n)):traj.append(p0 + v * t)t = t + 0.001return traj# dt = 1
# n = 1000
# t = np.linspace(0, 20, 21)# dt = 0.1
# n = 100
# t = np.linspace(0, 20, 201)dt = 0.5
n = 500
t = np.linspace(0, 20, 41)X = 100*np.sin(0.2*np.pi*t)
Vx = 0.2*np.pi*100*np.cos(0.2*np.pi*t)time_list = np.linspace(0, 20, 20 *1000)
pos_list = 100*np.sin(0.2*np.pi*time_list)
Vel_list = 0.2*np.pi*100*np.cos(0.2*np.pi*time_list)count = 1
time = 0Xpvt= []
tpvt = []for i in range(len(X)):if(i>=1):q0 = X[i-1]q1 = X[i]v0 = Vx[i-1]v1 = Vx[i]n = int(round((t[i] - t[i-1]) * 1000))# PVT 是适用于 dt = 1 的测试数据，其他测试数据不适合# traj = PVT(q0,q1,v0,v1, n)# traj = PVT1(q0,q1,v0,v1,t[i-1],t[i])# traj = PVT2(q0,q1,v0,v1,n)v_end = (q1 - q0)/ (t[i] - t[i - 1])traj = PT(q0,q1,v_end,n)for k in range(len(traj)):Xpvt.append(traj[k])tpvt.append(time)time = time + 0.001count = count + 1plt.plot(t,X,"*",label = "give pos")
plt.plot(tpvt,Xpvt, label = "PVT")
plt.plot(time_list,pos_list, label = "sin")
plt.title("position PVT")
plt.legend()
plt.show()plt.plot(t, Vx,'.', label = 'give vel')
plt.plot(tpvt[:-1], np.dot(np.diff(Xpvt), 1000),label = 'PVT')
plt.plot(time_list, Vel_list, label = 'cos')
plt.title("velocity PVT")
plt.legend()
plt.show()

PVT Hermite插值结果

PVT 三次多项式插值结果

从这边的结果也可以看出， 两点三次 Hermite 插值和三次多项式插值得到的结果是一样的。

PT 插值结果

用户轨迹测试结果

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]]
X = [row[0] for row in data]
t = [row[1] for row in data]
time_list = np.arange(0, t[-1],0.001) count = 1
time = 0Xpvt= []
tpvt = []
v0 = 0
for i in range(len(X)):if(i>=1):q0 = X[i-1]q1 = X[i]v1 = (q1 - q0)/ (t[i] - t[i - 1])n = int(round((t[i] - t[i-1]) * 1000))# traj = PVT(q0,q1,v0,v1, n)# traj = PVT1(q0,q1,v0,v1,t[i-1],t[i])traj = PVT2(q0,q1,v0,v1,n)# traj = PT(q0,q1,v1,n)v0 = v1for k in range(len(traj)):Xpvt.append(traj[k])tpvt.append(time)time = time + 0.001count = count + 1plt.plot(t,X,"*",label = "give pos")
plt.plot(tpvt,Xpvt, label = "PVT")
plt.title("position PVT")
plt.legend()
plt.show()plt.plot(tpvt[:-1], np.dot(np.diff(Xpvt), 1000),label = 'PVT')
plt.title("velocity PVT")
plt.legend()
plt.show()

PVT Hermite插值结果

从这边可以看出， PT 插值不太适合直接用在控制器中， 更适合用在驱动器中。 而PVT插值的话，则需要用户提供时间间隔更加小一点的轨迹。

PT 插值结果