PTPVT 插值说明

于 2024-12-29 20:24:49 发布
阅读量426 收藏 1
点赞数 6
分类专栏： 机器人轨迹规划 文章标签： Python PT PVT 插值
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/u014077947/article/details/144809309
版权

文章目录

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 插值结果

在这里插入图片描述
在这里插入图片描述

word 自动大写_如何更改Word的大写自动更正设置
culunyi0802的博客
09-19 1861
word 自动大写Microsoft Word’s AutoCorrect feature has built-in capitalization settings aimed at automatically fixing common mistakes like typing two capital letters at the beginning of a word or not capit...
word 设置目录中英文字母大写改为小写
weixin_40693897的博客
08-02 1210
选中目录右击“字体”，设置“小型大写字母”。 原创作者: ghostwritten...
word自动生成目录--小写字母变大写
sax_157001的博客
05-24 1万+
哪一级标题有出现小写字母变大写的情况就选中该目录 在工具栏中点击引用 点击目录下的倒三角，选择插入目录 在出现的对话框中选择右下角的修改 选择对话框左下角的格式—字体，去掉对话框中的小型大写字母 确定,完成
word英文大写问题解决方案
qq_36442947的博客
11-10 2493
word中打英文或者复制粘贴后全部都变成大写，切换大小写无法调整过来的情况解决方案 选中大写英文，右键，选择字体，在效果中设置为下图所示：
Word2007 生成目录英文变成大写的解决办法
热门推荐
Scooler
01-18 5万+
今天使用word2007对文章生成目录的时候发现目录中的英文字符都变成了大写，解决办法就是修改目录的样式 初步发现，目录一和目录二中的英文是变成了大写。 下面是修改方法： 在word的开始选项卡总找到样式，点击样式里右下角的小箭头。 打开样式列表，鼠标放到目录1上，后面会出现一个下拉箭头，点它 点击下拉箭头后，点击下拉列表里的修改，打开了修改样式对话框 在修改样
Word里面输入英文时全是大写
第四纪绿洲的专栏
03-23 5775
转自：http://www.qiao8.net/?p=1799 症状： 用户反映，编辑Word文档时突然间只能输入大写了。 解决： 打开格式→字体，在字体栏中选择Word中正在使用的字体，取消选择“全部大写字母”；或者直接在样式栏中选择“清除格式”即可。
word2007自动生成目录小写字母变大写
懒懒猪
05-23 1万+
最近写文档，用的是word2007，自动生成的目录总是把标题的小写字母换成大写，经过一天的查找实践，最后终于搞定了。 1. 在工具栏中点击引用 2. 点击目录下的倒三角，选择插入目录 3. 在出现的对话框中选择右下角的修改 4. 哪一级标题有出现小写字母变大写的情况就选择该目录：例如1级标题出现上述情况，就选择目录1，然后点击修改。 5. 选择对话框左下角的格式—字体，去掉对话框中的小型
word取消自动单词首字母大写
m0_70067011的博客
10-18 3855
情况说明：在word输入单词后首字母会自动变成大写取消单词首字母大写步骤：（1）点击菜单栏文件（2）点击“更多”——>“选项”（3）点击“校对”——>“自动更正选项”（4）取消“句首字母大写(S)”和“表格单元格的首字母大写(C)”的勾选——>点击“确定”
为什么word目录输入英文自动变成大写
05-17
这可能是因为在你的Word文档中启用了“首字母大写”功能。你可以按以下步骤取消该功能： 1. 选中你想要取消大写的文本； 2. 单击“Shift”和“F3”键，直至文本变成你想要的形式（全小写或首字母大写）。 如果你...
Word中自动生成目录：自动化避免繁琐
在Word文档的编辑过程中，自动目录的编制是一项重要的功能，它能够极大地提高文档管理的效率并减少错误。本文档主要介绍了如何在Word中使用自动目录，并阐述了其两大关键优势。 首先，自动目录的优势明显。一方面，...
Word文档自动目录设置与更新
"Word文档自动目录编制教程" 在Word文档中，自动目录是组织和管理长篇内容的有效工具，尤其适用于编写报告、论文或手册等。自动目录可以帮助读者快速找到文档的不同部分，同时也能在文档内容更改时进行自动更新，...
如何使word里英文小写变大写
最新发布
04-03
虽然问题不是关于数字而是英文大小写，但也许可以建议使用宏来处理更复杂的转换需求。 需要注意用户是否提到了具体的Word版本，但问题中没有说明，所以需要给出通用方法，适用于大多数版本。同时，检查是否有其他...
Word文档自动化目录制作教程：步骤详解与实例
首先，自动目录的重要性体现在两个方面：一是它能自动化处理目录的创建，避免了人工编目录可能产生的繁琐和错误；二是当文档内容发生变化，如页面调整时，目录会自动更新，确保始终反映最新的章节结构。设置自动目录...
word中将半角标点符号（如双引号等）转换为全角
傅斯年的写字台
08-02 1万+
百度经验:jingyan.baidu.com 通常在毕业论文中，规范的要求是：中文字符要求用宋体（或其他中文字体），西文字符要求用times new roman。 对此，我们通用的办法是在样式中，将“正文”的字体修改为相对应的中、西文字体。然而，在word2013中，有些标点符号（如双引号），会被自动识别为西文字体（或一半中文字体，一半西文字体）。在这种情况下，通过上述方法，将
word转pdf或者打印后题注编号变成大写
sheng_xinjun的博客
04-18 6446
最近在写毕业论文，遇到了这样一个问题：我的题注编号是由章节号和题注顺序决定的，比如 图2.3代表了这是第二章的第三张图，本来一切顺利，但是在转换为pdf后发现2.3变成了二.3。 解决方法：首先CTRL+A选中全文，之后CTRL+F11锁定域，之后再转换为pdf，就不会出现编号问题了。如果论文后续还需要修改再CTRL+SHITF+F11解锁域。 ...
word文档手写字母总会大写问题
pleboyzha的博客
05-10 369
word文档手写字母总会大写问题 1.点击文件 2.选择 “”选项“”，和编辑 3.把这个勾取消即可
查找一个目录下，所有文件中数字、字母(大小写不区分)、汉字、空格的个数、行数。
heymysweetheart的专栏
07-15 2391
汉字在java中占两个字符(char).
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

独坐寒江边

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值