没有合适的资源?快使用搜索试试~ 我知道了~
引言 最近在算法中想实现给定机器人关节起始末尾角度、角速度、角加速度,求解过程中时间t时刻的角度、角速度和角加速度,而不是返回整个插值曲线中的值,也不依赖采样频率。网上比较普遍的算法如多项式轨迹–五次多项式轨迹确实也给出了详细过程,但是经过一番功夫后发现是错误的,例如你将条件中的加速度设置的不为零时求解结果不正确(惨痛的经历,我说跳算法时老飞车呢)。所以我使用matlab符号求解器,自己完成了求解过程。 Matlab符号求解 clear clc syms a0 a1 a2 a3 a4 a5 x1 x2 y1 y2 dy1 dy2 ddy1 ddy2 //系数0 1 2 3 4 5
资源详情
资源评论
资源推荐
机器人;实时五次插值推导机器人;实时五次插值推导
引言引言
最近在算法中想实现给定机器人关节起始末尾角度、角速度、角加速度,求解过程中时间t时刻的角度、角速度和角加速度,
而不是返回整个插值曲线中的值,也不依赖采样频率。网上比较普遍的算法如多项式轨迹–五次多项式轨迹确实也给出了详细
过程,但是经过一番功夫后发现是错误的,例如你将条件中的加速度设置的不为零时求解结果不正确(惨痛的经历,我说跳算惨痛的经历,我说跳算
法时老飞车呢法时老飞车呢)。所以我使用matlab符号求解器,自己完成了求解过程。
Matlab符号求解符号求解
clear
clc
syms a0 a1 a2 a3 a4 a5 x1 x2 y1 y2 dy1 dy2 ddy1 ddy2
//系数0 1 2 3 4 5 时间 位置 速度 加速度
F = [a0+a1*x1+a2*x1^2+a3*x1^3+a4*x1^4+a5*x1^5==y1, //起始条件
a0+a1*x2+a2*x2^2+a3*x2^3+a4*x2^4+a5*x2^5==y2, //末尾条件
a1+2*a2*x1+3*a3*x1^2+4*a4*x1^3+5*a5*x1^4==dy1,
a1+2*a2*x2+3*a3*x2^2+4*a4*x2^3+5*a5*x2^4==dy2,
2*a2+6*a3*x1+12*a4*x1^2+20*a5*x1^3==ddy1,
2*a2+6*a3*x2+12*a4*x2^2+20*a5*x2^3==ddy2];
solve = solve(F,[a0 a1 a2 a3 a4 a5]);
simplify(solve.a0)
simplify(solve.a1)
simplify(solve.a2)
simplify(solve.a3)
simplify(solve.a4)
simplify(solve.a5)
通过上面的程序就可以求解出五次插值的六个系数了
五次插值五次插值Matlab代码代码
function [y,dy,ddy] = MyQuintic(x1,x2,y1,y2,dy1,dy2,ddy1,ddy2,t) //t是当前求解时刻
t = max(min(t,x2),x1); //确保时间t在插值范围内
a0 = (2*x1^5*y2 - 2*x2^5*y1 + 2*dy1*x1*x2^5 - 2*dy2*x1^5*x2 + 10*x1*x2^4*y1 - 10*x1^4*x2*y2 - ddy1*x1^2*x2^5 + 2*ddy1*x1^3*x2^4 - ddy1*x1^4*x2^3
+ ddy2*x1^3*x2^4 - 2*ddy2*x1^4*x2^3 + ddy2*x1^5*x2^2 - 10*dy1*x1^2*x2^4 + 8*dy1*x1^3*x2^3 - 8*dy2*x1^3*x2^3 + 10*dy2*x1^4*x2^2 -
20*x1^2*x2^3*y1 + 20*x1^3*x2^2*y2)/(2*(x1 - x2)^5);
a1 = -(2*dy1*x2^5 - 2*dy2*x1^5 - 2*ddy1*x1*x2^5 + 2*ddy2*x1^5*x2 - 10*dy1*x1*x2^4 + 10*dy2*x1^4*x2 + ddy1*x1^2*x2^4 + 4*ddy1*x1^3*x2^3 -
3*ddy1*x1^4*x2^2 + 3*ddy2*x1^2*x2^4 - 4*ddy2*x1^3*x2^3 - ddy2*x1^4*x2^2 - 16*dy1*x1^2*x2^3 + 24*dy1*x1^3*x2^2 - 24*dy2*x1^2*x2^3 +
16*dy2*x1^3*x2^2 - 60*x1^2*x2^2*y1 + 60*x1^2*x2^2*y2)/(2*(x1 - x2)^5);
a2 = -(ddy1*x2^5 - ddy2*x1^5 + 4*ddy1*x1*x2^4 + 3*ddy1*x1^4*x2 - 3*ddy2*x1*x2^4 - 4*ddy2*x1^4*x2 + 36*dy1*x1*x2^3 - 24*dy1*x1^3*x2 +
24*dy2*x1*x2^3 - 36*dy2*x1^3*x2 + 60*x1*x2^2*y1 + 60*x1^2*x2*y1 - 60*x1*x2^2*y2 - 60*x1^2*x2*y2 - 8*ddy1*x1^2*x2^3 + 8*ddy2*x1^3*x2^2 -
12*dy1*x1^2*x2^2 + 12*dy2*x1^2*x2^2)/(2*(x1 - x2)^5);
a3 = (ddy1*x1^4 + 3*ddy1*x2^4 - 3*ddy2*x1^4 - ddy2*x2^4 - 8*dy1*x1^3 + 12*dy1*x2^3 - 12*dy2*x1^3 + 8*dy2*x2^3 + 20*x1^2*y1 - 20*x1^2*y2 +
20*x2^2*y1 - 20*x2^2*y2 + 4*ddy1*x1^3*x2 - 4*ddy2*x1*x2^3 + 28*dy1*x1*x2^2 - 32*dy1*x1^2*x2 + 32*dy2*x1*x2^2 - 28*dy2*x1^2*x2 -
8*ddy1*x1^2*x2^2 + 8*ddy2*x1^2*x2^2 + 80*x1*x2*y1 - 80*x1*x2*y2)/(2*(x1 - x2)^5);
a4 = -(30*x1*y1 - 30*x1*y2 + 30*x2*y1 - 30*x2*y2 + 2*ddy1*x1^3 + 3*ddy1*x2^3 - 3*ddy2*x1^3 - 2*ddy2*x2^3 - 14*dy1*x1^2 + 16*dy1*x2^2 -
16*dy2*x1^2 + 14*dy2*x2^2 - 4*ddy1*x1*x2^2 - ddy1*x1^2*x2 + ddy2*x1*x2^2 + 4*ddy2*x1^2*x2 - 2*dy1*x1*x2 + 2*dy2*x1*x2)/(2*(x1 - x2)^5);
a5 = (12*y1 - 12*y2 - 6*dy1*x1 + 6*dy1*x2 - 6*dy2*x1 + 6*dy2*x2 + ddy1*x1^2 + ddy1*x2^2 - ddy2*x1^2 - ddy2*x2^2 - 2*ddy1*x1*x2 + 2*ddy2*x1*x2)/(2*
(x1 - x2)^5);
dat = t-x1;
y = a0+a1*dat+a2*dat^2+a3*dat^3+a4*dat^4+a5*dat^5;//位置
dy = a1+2*a2*dat+3*a3*dat^2+4*a4*dat^3+5*a5*dat^4;//速度
ddy = 2*a2+6*a3*dat+12*a4*dat^2+20*a5*dat^3; //加速度
end
可以很容易将其写为C或C++代码。
Matlab测试代码测试代码
clear
clc
x1 = 0;y1 = 0;dy1 = 0;ddy1 = 0;
x2 = 3;y2 = 5;dy2 = 0;ddy2 = 10;
data = [];mydata = [];
for t = x1:0.01:x2
[y,dy,ddy] = Quintic(x1,x2,y1,y2,dy1,dy2,ddy1,ddy2,t);
data = [data;y,dy,ddy];//上文提到的链接代码
[y,dy,ddy] = MyQuintic(x1,x2,y1,y2,dy1,dy2,ddy1,ddy2,t);
mydata = [mydata;y,dy,ddy];//自己推导的代码
end
figure(1)
weixin_38696836
- 粉丝: 3
- 资源: 932
上传资源 快速赚钱
- 我的内容管理 展开
- 我的资源 快来上传第一个资源
- 我的收益 登录查看自己的收益
- 我的积分 登录查看自己的积分
- 我的C币 登录后查看C币余额
- 我的收藏
- 我的下载
- 下载帮助
安全验证
文档复制为VIP权益,开通VIP直接复制
信息提交成功
评论0