机械臂建模实战-七次多项式轨迹规划

下面我将用三个例子与大家一起学习七次多项式轨迹规划，其中第一个有问题，期待大家一起解决，难道仅仅是因为逆解的解法导致的吗？第一个模型为上一篇的GLUON-6L3机械臂

clear;clc;close all;
% DH建模部分theta、   d、       a、   alpha
L(1)=Link([  0,    0.1015,     0,        0],'modified');
L(2)=Link([  0,    0.0792,     0,    -pi/2],'modified');
L(3)=Link([  0,    -0.0792,   0.173,     0],'modified');
L(4)=Link([  0,    0.0792,    0.173,     0],'modified');
L(5)=Link([  0,    0.0792,     0,     pi/2],'modified');
L(6)=Link([  0,    0.0417,     0,    -pi/2],'modified');
 
L(2).offset=-pi/2; %设定关节变量偏移量
L(4).offset=pi/2;

robot= SerialLink(L, 'name', 'GLUON_6L3');  %建立机械臂模型
 
%调整坐标轴及视野
set(gca,'XLim',[-0.6,0.6]);  %将X轴范围设定为[-400,400]
set(gca,'YLim',[-0.6,0.6]);  %将X轴范围设定为[-500,500]
set(gca,'ZLim',[0,0.6]);      %将Z轴范围设定为[0,600]，最小值设定为0，可以消除模型下面的长杆
set(gca,'XDir','reverse');    %将x轴方向设置为反向
set(gca,'YDir','reverse');    %将Y轴方向设置为反向
set(gca,'View',[-85,10]);     %设定视野方向角和俯仰角
 
%设定关节变量范围
L(1).qlim=[deg2rad(-140) deg2rad(140)];
L(2).qlim=[deg2rad(-90)   deg2rad(90)];
L(3).qlim=[deg2rad(-140) deg2rad(140)];
L(4).qlim=[deg2rad(-140) deg2rad(140)];
L(5).qlim=[deg2rad(-140) deg2rad(140)];
L(6).qlim=[deg2rad(-360) deg2rad(360)];

t0 = 0;       %开始时刻，起点
t1 = 2;       %经过第二点
t2 = t1 + 4;  %经过第三点
tm = t2 + 4;  %到达目标点
 
t0_1 = 0:0.5:2;  %第一段时间
t1_2 = 0:0.5:4;  %第二段时间
t2_x = 0:0.5:4;  %第三段时间

%改变四个点的位置，轨迹将重新规划
aim0 = [0 0.121 0.527];     %起点
aim1 = [0.25 0.121 0.445];  %第一经过点
aim2 = [0.373 0.121 0.306]; %第二经过点
aimx = [0.425 0.121 0.102]; %终点
    
T0 = transl(aim0);   %转换为齐次矩阵
T1 = transl(aim1);
T2 = transl(aim2);
Tx = transl(aimx);

%求解四个位置的关节角度,这里都用ikine()逆解
theta0 = robot.ikine(T0); 
theta1 = robot.ikine(T1);
theta2 = robot.ikine(T2);
thetax = robot.ikine(Tx);

%初始条件
theta0_ = [0