文章目录
- PTPVT 插值说明
- PTPVT 插值说明
- PVT Hermite插值
- PVT 三次多项式插值
- PT 插值
- Sin轨迹测试结果
- PVT Hermite插值结果
- PVT 三次多项式插值结果
- PT 插值结果
- 用户轨迹测试结果
- PVT Hermite插值结果
- PT 插值结果
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 = v1
dt = 1/n
tt = 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 = t0
traj = []
n = (int)(np.round(((t1 - t0)/0.001)))
i = 0
while i < n:
alpha0 = (1 + 2*((t-t0) / (t1-t0))) * ((t-t1) / (t0-t1))**2
alpha1 = (1 + 2*((t-t1) / (t0-t1))) * ((t-t0) / (t1-t0))**2
beta0 = (t-t0) * ((t-t1) / (t0-t1))**2
beta1 = (t-t1) * ((t-t0) / (t1-t0))**2
traj.append(p0*alpha0 + p1*alpha1 + v0*beta0 + v1*beta1)
t = t + 0.001
i = i + 1
return traj
PVT 三次多项式插值
# 三次多项式
def PVT2(p0,p1,v0,v1,n):
traj = []
t = 0
for i in range((n)):
T = n * 0.001
h = p1 - p0
a0 = p0
a1 = v0
a2 = (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.001
return traj
PT 插值
# 使用 PT插值的形式
def PT(p0,p1,v, n):
traj = []
t = 0
for i in range((n)):
traj.append(p0 + v * t)
t = t + 0.001
return 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 = v1
dt = 1/n
tt = 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 = t0
traj = []
n = (int)(np.round(((t1 - t0)/0.001)))
i = 0
while i < n:
alpha0 = (1 + 2*((t-t0) / (t1-t0))) * ((t-t1) / (t0-t1))**2
alpha1 = (1 + 2*((t-t1) / (t0-t1))) * ((t-t0) / (t1-t0))**2
beta0 = (t-t0) * ((t-t1) / (t0-t1))**2
beta1 = (t-t1) * ((t-t0) / (t1-t0))**2
traj.append(p0*alpha0 + p1*alpha1 + v0*beta0 + v1*beta1)
t = t + 0.001
i = i + 1
return traj
# 三次多项式
def PVT2(p0,p1,v0,v1,n):
traj = []
t = 0
for i in range((n)):
T = n * 0.001
h = p1 - p0
a0 = p0
a1 = v0
a2 = (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.001
return traj
# 使用 PT插值的形式
def PT(p0,p1,v, n):
traj = []
t = 0
for i in range((n)):
traj.append(p0 + v * t)
t = t + 0.001
return 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 = 0
Xpvt= []
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.001
count = count + 1
plt.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 = 0
Xpvt= []
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 = v1
for k in range(len(traj)):
Xpvt.append(traj[k])
tpvt.append(time)
time = time + 0.001
count = count + 1
plt.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 插值结果
![[Day 10]有序数组的平方](https://i-blog.csdnimg.cn/direct/0c2b70130e054641b687b2f077df602d.png)
