（二）五次多项式轨迹规划

我什么都布吉岛

已于 2022-03-23 10:28:59 修改

阅读量2.9w

点赞数 44

分类专栏： # 运动规划文章标签：机器人算法

于 2020-04-25 20:10:23 首次发布

本文链接：https://blog.csdn.net/weixin_39258979/article/details/105755351

版权

运动规划专栏收录该内容

11 篇文章 75 订阅

订阅专栏

一、三次多项式轨迹规划的缺陷

上一篇文章说道，三次多项式轨迹规划只能够保证速度和位移连续，并不能保证加速度连续。加速度不连续将会对使电机抖动、甚至冲击。

二、轨迹规划中的五次多项式

我们对加速度数值进行指定，将会增加两个加速度方程，总计6个方程组。他们分别是：

起末位移方程组
起末速度方程组
起末加速度方程组
$\begin{aligned} & x(t)=c_0+c_1t+c_2t^2+c_3t^3+c_4t^4+c_5t^5\\ & v(t)=c_1+2c_2t+3c_3t^2+4c_4t^3+5c_5t^4\\ & a(t)=2c_2+6c_3t+12c_4t^2+20c_5t^3 \end{aligned} \tag{1}$
带入初始条件有：

$\begin{aligned} &x(t_s)=c_0+c_1t_s+c_2t_s^2+c_3t_s^3+c_4t_s^4+c_5t_s^5\\ &v(t_s)=c_1+2c_2t_s+3c_3t_s^2+4c_4t_s^3+5c_5t_s^4\\ &a(t_s)=2c_2+6c_3t_s+12c_4t_s^2+20c_5t_s^3\\ &x(t_e)=c_0+c_1t_e+c_2t_e^2+c_3t_e^3+c_4t_e^4+c_5t_e^5\\ &v(t_e)=c_1+2c_2t_e+3c_3t_e^2+4c_4t_e^3+5c_5t_e^4\\ &a(t_e)=2c_2+6c_3t_e+12c_4t_e^2+20c_5t_e^3 \end{aligned}\tag{2}$
写成矩阵形式：
$\begin{bmatrix} 1 & t_s & t_s^2&t_s^3&t_s^4&t_s^5 \\ 1 & t_e & t_s^2&t_s^3&t_s^4&t_s^5 \\ 0 & 1 & 2ts&3t_s^2&4t_s^3&5t_s^4\\ 0&1&2t_e&3t_e^2&4t_e^3&5t_e^4\\ 0&0&2&6t_s&12t_s^2&20t_s^3\\ 0&0&2&6t_e&12t_e^2&20t_e^3\\ \end{bmatrix} \begin{bmatrix}\tag{3} c_0 \\ c_1 \\ c_2 \\ c_3\\ c_4\\ c_5 \end{bmatrix}= \begin{bmatrix} x_s \\ x_e \\ v_s \\ v_e\\ a_s\\ a_e \end{bmatrix}$
记为 $T C = P$ ，则 $C=T^{-1}P$

三、Matlab仿真与实现

% 五次多项式插补
close all
clear;
clc;

% 0-1
t0=0; x0=30; v0=30;a0=10;
t1=5; x1=60; v1=40;a1=13;
[x01,v01,a01]=fivetimes_plan(t0,x0,v0,a0,t1,x1,v1,a1);

% 1-2
t2=13; x2=80; v2=10;a2=13;
[x12,v12,a12]=fivetimes_plan(0,x1,v1,a1,t2-t1,x2,v2,a2);

% 2-3
t3=20; x3=10; v3=0;a3=23;
[x23,v23,a23]=fivetimes_plan(0,x2,v2,a2,t3-t2,x3,v3,a3);


x=[x01 x12 x23];
v=[v01 v12 v23];
a=[a01 a12 a23];

figure(1);
subplot(3,1,1);
plot(x,'r','LineWidth',1.2);
axis([ t0*100 t3*100 -inf inf]);
ylabel('position')

subplot(3,1,2);
plot(v,'g','LineWidth',1.2)
axis([ t0*100 t3*100 -inf inf]);
ylabel('velocity')

subplot(3,1,3);
plot(a,'b','LineWidth',1.2);
axis([ t0*100 t3*100 -inf inf]);
ylabel('acceleration')
xlabel('time')

function [x,v,a]=fivetimes_plan(ts,start_x,start_v,start_a,te,end_x,end_v,end_a)
    para=[start_x,end_x,start_v,end_v,start_a,end_a]';
    Tran=[1,ts,ts^2,ts^3,ts^4,ts^5;
          1,te,te^2,te^3,te^4,te^5
          0,1,2*ts,3*ts^2,4*ts^3,5*ts^4;
          0,1,2*te,3*te^2,4*te^3,5*te^4;
          0,0,2,6*ts,12*ts^2,20*ts^3
          0,0,2,6*te,12*te^2,20*te^3];
    C=(inv(Tran))*para;
    c0=C(1);
    c1=C(2);
    c2=C(3);
    c3=C(4);
    c4=C(5);
    c5=C(6);
    x=[];v=[];a=[];
    for i=ts:0.01:te
        t=i;
        x=[x c0+c1*t+c2*t^2+c3*t^3+c4*t^4+c5*t^5];
        v=[v c1+2*c2*t+3*c3*t^2+4*c4*t^3+5*c5*t^4];
        a=[a 2*c2+6*c3*t+12*c4*t^2+20*c5*t^3];
    end
end

在这里插入图片描述
可以看出，五次多项式确实没有加速度突变，符合开始的设想。无论是三次还是五次多项式插值速度曲线并没有匀速段，这种规划方式不适用于笛卡尔空间的规划，因为大多数工业应用如涂胶、焊接都要求有匀速段，为了拥有匀速段，就有了后边的梯形和S型速度规划。