成都定制网站设,达州建设机械网站,互联网品牌宣传推广服务公司,学校网站网页制作文章目录 PTPVT 插值说明 PTPVT 插值说明PVT Hermite插值PVT 三次多项式插值PT 插值Sin轨迹测试结果PVT Hermite插值结果PVT 三次多项式插值结果PT 插值结果 用户轨迹测试结果PVT Hermite插值结果PT 插值结果 PTPVT 插值说明
PT模式#xff1a; 位置-时间路径插值算法。 PVT模… 文章目录 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 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 trajPVT 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 trajPVT 三次多项式插值
# 三次多项式
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 trajPT 插值
# 使用 PT插值的形式
def PT(p0,p1,v, n):traj []t 0for i in range((n)):traj.append(p0 v * t)t t 0.001return trajSin轨迹测试结果
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(i1):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(i1):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 插值结果
