(一)三次多项式轨迹规划

一、什么是多项式?

其实多项式这个概念你在初中就见过(整式),他就是一个表达式: f ( x ) = a n x n + ⋯ + a 1 x + a 0 f(x)=a_nx^n+\cdots+a_1x+a_0 f(x)=anxn++a1x+a0。在这个式子中, a n , ⋯   , a 1 , a 0 a_n,\cdots,a_1,a_0 an,,a1,a0,这是数域中的数,也叫系数, x x x这里是变量。

在定义多项式次数时,其实大家可以把它和线性空间的维数联系到一起(虽然两者数值上差了一个1)。这是因为 { 1 , x , ⋯   , x n } \{1,x,\cdots,x^n\} {1,x,,xn}可以看作一个基,然后它张成的空间就是一元多项式的全体,当然有一个例外是0,也就是零多项式,它的次数我们定义为 − ∞ -\infty

多项式中的次数都是非负数,并且多项式是有限项。所以像是 x 1 3 x^{\frac{1}{3}} x31这样的式子就不是多项式。同样的,因为 a i a_i ai这种东西都是数域中的数,如果数域不同,可能也会影响它是否成为一个多项式和。比方说, x + π x+\pi x+π在有理数域中就不是多项式。

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

选择满足要求的运动学性质的物理量如,如:位移、速度和加速度其实就完成了一个轨迹的规划。以单轴关节角 x ( t ) x(t) x(t)为例,容易得:
x ( t ) = c 0 + c 1 t + c 2 t 2 + c 3 t 3 v ( t ) = c 1 + 2 c 2 t + 3 c 3 t 2 a ( t ) = 2 c 2 + 6 c 3 t \begin{aligned} &x(t)=c_0+c_1t+c_2t^2+c_3t^3\\ &v(t)=c_1+2c_2t+3c_3t^2\\ &a(t)=2c_2+6c_3t\\ \end{aligned} x(t)=c0+c1t+c2t2+c3t3v(t)=c1+2c2t+3c3t2a(t)=2c2+6c3t
三次多项式对四个运动量进行了规划(约束),显然x(t) v(t) a(t)均是连续函数:

  • 起始绝对量
  • 起始速度
  • 结束绝对量
  • 结束速度

每一个约束都对应一个方程,方程如下:
x ( t s ) = c 0 + c 1 t s + c 2 t s 2 + c 3 t s 3 = x s x ( t e ) = c 0 + c 1 t e + c 2 t e 2 + c 3 t e 3 = x e v ( t s ) = c 1 + 2 c 2 t s + 3 c 3 t s 2 = v s v ( t e ) = c 1 + 2 c 2 t e + 3 c 3 t e 2 = v e \begin{aligned} &x(t_s)=c_0+c_1t_s+c_2t_s^2+c_3t_s^3=x_s\\ &x(t_e)=c_0+c_1t_e+c_2t_e^2+c_3t_e^3=x_e\\ &v(t_s)=c_1+2c_2t_s+3c_3t_s^2=vs\\ &v(t_e)=c_1+2c_2t_e+3c_3t_e^2=ve \end{aligned} x(ts)=c0+c1ts+c2ts2+c3ts3=xsx(te)=c0+c1te+c2te2+c3te3=xev(ts)=c1+2c2ts+3c3ts2=vsv(te)=c1+2c2te+3c3te2=ve

写成矩阵形式:
[ 1 t s t s 2 t s 3 1 t e t e 2 t e 3 0 1 2 t s 3 t s 2 0 1 2 t e 3 t e 2 ] \begin{bmatrix} 1 & ts & ts^2&ts^3 \\ 1 & te & te^2&te^3 \\ 0 & 1 & 2ts&3ts^2\\ 0&1&2te&3te^2 \end{bmatrix} 1100tste11ts2te22ts2tets3te33ts23te2
到系数矩阵
T C = P C = T − 1 P \begin{aligned} &TC=P\\ &C=T^{-1}P \end{aligned} TC=PC=T1P

获得系数之后,轨迹就唯一确定了。即每一时刻都可以对应一个“位移量”。

Matlab实现如下,连续进行了三段曲线连续轨迹规划:0-1,1-2,2-3

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

% 0-1
t0=0; x0=30; v0=75;
t1=5; x1=60; v1=10;
[x01,v01,a01]=plan(t0,x0,v0,t1,x1,v1);

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

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


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]=plan(ts,start_x,start_v,te,end_x,end_v)
    para=[start_x,end_x,start_v,end_v]';
    Tran=[1,ts,ts^2,ts^3;1,te,te^2,te^3;0,1,2*ts,3*ts^2;0,1,2*te,3*te^2];
    C=(inv(Tran))*para;
    c0=C(1);
    c1=C(2);
    c2=C(3);
    c3=C(4);
    x=[];v=[];a=[];
    for i=ts:0.01:te
        t=i;
        x=[x c0+c1*t+c2*t^2+c3*t^3];
        v=[v c1+2*c2*t+3*c3*t^2];
        a=[a 2*c2+6*c3*t];
    end
end

结果如下:

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-HuOxbMIa-1587794722228)(https://note.youdao.com/yws/res/45267/BF15337FBB704B28B47A712C4655E2D7)]

虽然我们每一段的位移、速度和加速度都是连续的,但是显然对于加速度而言,出现了间断点,在这个例子里,瞬间加速度超过了 20 m m / s 2 20mm/s^2 20mm/s2,是否能够提供这样的加速度取决于硬件(电机)

©️2020 CSDN 皮肤主题: 游动-白 设计师:上身试试 返回首页