PTPVT 插值说明

独坐寒江边2024-12-30 17:00

文章目录

    • [PTPVT 插值说明](#PTPVT 插值说明)
  • [PTPVT 插值说明](#PTPVT 插值说明)
    • [PVT Hermite插值](#PVT Hermite插值)
    • [PVT 三次多项式插值](#PVT 三次多项式插值)
    • [PT 插值](#PT 插值)
    • Sin轨迹测试结果
      • [PVT Hermite插值结果](#PVT Hermite插值结果)
      • [PVT 三次多项式插值结果](#PVT 三次多项式插值结果)
      • [PT 插值结果](#PT 插值结果)
    • 用户轨迹测试结果
      • [PVT Hermite插值结果](#PVT Hermite插值结果)
      • [PT 插值结果](#PT 插值结果)

PTPVT 插值说明

PT模式： 位置-时间路径插值算法。

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

PT算法对于点位距离比较小的运动或者低速度的运动比较合适。由于其很少的计算量，因此计算速度很快。

一般可以直接用在伺服驱动器中，比如说控制器的控制周期是 1ms, 那么伺服驱动器就可以在每 1ms 内的时间间隔内使用 PT 插值，可以 1 ms 内插值16个点位，以使得运动更加的精细。PT 插值的加速度是不连续的，存在突变。

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

PTPVT 插值说明

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

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

`

python 复制代码 
# 该函数只适用于两个点之间的时间间隔是 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插值

python 复制代码 
# 两点三次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 三次多项式插值

python 复制代码 
# 三次多项式
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 插值

python 复制代码 
# 使用 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轨迹测试结果

python 复制代码 
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 插值结果


用户轨迹测试结果

python 复制代码 
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 插值结果


相关推荐
lennon_jlu37 分钟前
1.4 java反射机制 简单的java反射机制实践
java·开发语言·python
hakesashou1 小时前
在vscode中编写Python的详细步骤
ide·vscode·python
从以前1 小时前
解析 World Football Cup 问题及其 Python 实现
开发语言·python·算法
_.Switch1 小时前
FastAPI 响应模型与自定义响应
开发语言·前端·数据库·python·fastapi·命令模式
dowhileprogramming1 小时前
Python 中常见的数据结构之一嵌套字典
前端·数据结构·python
赵谨言2 小时前
基于 Python 虎扑网站的 NBA 球员大数据分析与可视化
经验分享·python·毕业设计
火云牌神2 小时前
[python]实现可以自动清除过期条目的缓存
开发语言·python·缓存
孟郎郎2 小时前
OSError: [WinError 126] 找不到指定的模块 backend_with_compiler.dll
人工智能·python·llm·conda·torch
赵谨言2 小时前
基于 Python 大数据的购物车智能推荐与分析系统
经验分享·python·毕业设计
暗碳9 小时前
cloudns二级免费域名python更新ipv6 dns记录
开发语言·python
热门推荐
01Matlab|参与辅助服务的用户侧储能优化配置及经济分析02半导体应用系统一些小知识收集(strip&wafer mapping,EAP&scada)03HCIA-datacom数通题库和录播视频资料04渗透测试之SQLMAP工具详解 kali自带SQLmap解释 重点sqlmap --tamper 使用方式详解 搞完你就很nice了05【漏洞复现】CVE-2015-3337 Arbitrary File Reading06优化手机性能，解决卡顿问题：关闭这3个微信开关，释放内存空间07Docker--Bitnami/redis08新手学习VR全景需要知道的几个问题09玄机平台应急响应—webshell查杀10ubuntu22.04.5本地apt源部署