机器人轨迹仿真用三次B样条插值

三次B样条插值可用于机器人轨迹规划中速度、加速度、加加速度的实现

%均匀三次B样条曲线插值

V = load（‘con_point‘）;%控制点坐标
V = V.‘;
n = length（V）;%控制点个数

%---画出图形---
hold on
%控制多边形，蓝色，线
%plot（ V（:1） V（:2） ‘black‘ ）;

%V（:2）=V（:2）/180*pi; %转化成弧度

%画出三次B样条曲线，角度位移
for i=1:（n-3）; %n-3段
    u=0:0.1:1; % 参数u；

    k0=1.0./6.*（1-u）.^3; % 基函数k0；
    k1=1.0./6.*（3.*u.^3-6.*u.^2+4）; % 基函数k1；
    k2=1.0./6.*（-3.*u.^3+3.*u.^2+3.*u+1）; % 基函数k2；
    k3=1.0./6.*u.^3; % 基函数k3；

    t=k0.*V（i1）+k1.*V（i+11）+k2.*V（i+21）+k3.*V（i+31）; % 横坐标时间t；
    dis=k0.*V（i2）+k1.*V（i+12）+k2.*V（i+22）+k3.*V（i+32）; % 纵坐标角度位移dis；

    if i==1
        temp = t（1）;
    end
    
    t（:） = t（:） - temp;%调整时间从0开始
    
    cnt（i1） = t（1）;
    cnt（i2） = dis（1）;  
    if i==（n-3）
        cnt（i+11） = t（11）;
        cnt（i+12） = dis（11）;
    end
    
    a1 = -0.5.*V（i2）+1.5.*V（i+12）-1.5.*V（i+22）+0.5.*V（i+32）;
    a2 = V（i2）-2.*V（i+12）+V（i+22）;
    a3 = 0.5.*V（i+22）-0.5*V（i2）;
    b1 = -0.5.*V（i1）+1.5.*V（i+11）-1.5.*V（i+21）+0.5.*V（i+31）;
    b2 = V（i1）-2.*V（i+11）+V（i+21）;
    b3 = 0.5.*V（i+21）-0.5*V（i1）;
    c1 = 2 * b1;
    c2 = b2;
    d1 = 2 * a1;
    d2 = a2;
    r1 = （d1.*u+d2）.*（b1.*u.^2+b2.*u+b3） - （c1*u+c2）.*（a1*u.^2+a2*u+a3）;
    r2 = （ b1*u.^2+b2*u+b3 ）.^3;
    r3 = d1*（ b1*u.^2+b2*u+b3 ）.^2 - 3*（d1*u+d2）.*（c1*u+c2）.*（b1*u.^2+b2*u+b3） + 3*（a1*u.^2+a2*u+a3）.*（ （c1*u+c2）.^2 ） - （a1*u.^2+a2*u+a3）.*（b1*u.^2+b2*u+b3）*c1;
    r4 = （ b1*u.^2+b2*u+b3 ）.^5;
    
    vel = （a1.*u.^2+a2.*u+a3） ./ （b1.*u.^2+b2.*u+b3）;%速度vel
    acc = r1./r2;%加速度acc
    jer = r3./r4;%加加速度jer
    
    if i>1
        x（2） = t（1）;
        y（2） = jer（1）;
        plot（xy‘:b‘）;
    end
    x（1） = t（11）;
    y（1） = jer（11）;
    
    plot（tdis tvel tacc tjer‘b‘）;
end

%legend（‘位移‘‘速度‘‘加速度‘‘加加速度‘）;

plot（ cnt（:1） cnt（:2） ‘r.‘ ）;

xlabel（‘时间/s‘）;
title（‘关节6‘）;

gtext（‘\leftarrow 位移‘）;
gtext（‘\leftarrow 速度‘）;
gtext（‘\leftarrow 加速度‘）;
gtext（‘\leftarrow 加加速度‘）;

hold off;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

----------- ---------  ---------- -----  ----

                 2293                    3