机器人；实时五次插值推导

引言

最近在算法中想实现给定机器人关节起始末尾角度、角速度、角加速度，求解过程中时间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)
plot(data(:,1));
hold on
plot(data(:,2));
plot(data(:,3));
figure(2)
plot(mydata(:,1));
hold on
plot(mydata(:,2));
plot(mydata(:,3));

运行效果

可以看到自己推导的结果，符合自己设定的初始与结束条件。

网上流传的系数计算，结果差的很多。

如果喜欢请点个赞吧。