利用Matlab机器人工具箱进行轨迹规划及插值。生成有4个中间点的两轴运动，通过增加加速时间，比较轨迹的不同；利用四元数进行三维空间姿态插值，并通过动画演示。请给出matalb代码

好的，以下是Matlab代码：

轨迹规划及插值：

% 定义起始和目标点
start_pt = [0, 0];
end_pt = [10, 10];

% 定义4个中间点
mid_pt1 = [2, 5];
mid_pt2 = [5, 2];
mid_pt3 = [7, 8];
mid_pt4 = [9, 3];

% 定义加速时间
acc_time = 0.5;

% 生成轨迹
t = linspace(0, 1, 100);
x_traj = cubicPolyTraj(start_pt(1), mid_pt1(1), mid_pt2(1), acc_time, t);
y_traj = cubicPolyTraj(start_pt(2), mid_pt1(2), mid_pt2(2), acc_time, t);
x_traj = [x_traj, cubicPolyTraj(mid_pt2(1), mid_pt3(1), mid_pt4(1), acc_time, t)];
y_traj = [y_traj, cubicPolyTraj(mid_pt2(2), mid_pt3(2), mid_pt4(2), acc_time, t)];
x_traj = [x_traj, cubicPolyTraj(mid_pt4(1), end_pt(1), end_pt(1), acc_time, t)];
y_traj = [y_traj, cubicPolyTraj(mid_pt4(2), end_pt(2), end_pt(2), acc_time, t)];

% 画出轨迹
plot(x_traj, y_traj, 'b', 'LineWidth', 2);
hold on;
plot(start_pt(1), start_pt(2), 'ro', 'MarkerSize', 10);
plot(end_pt(1), end_pt(2), 'go', 'MarkerSize', 10);
plot([mid_pt1(1), mid_pt2(1)], [mid_pt1(2), mid_pt2(2)], 'k--');
plot([mid_pt3(1), mid_pt4(1)], [mid_pt3(2), mid_pt4(2)], 'k--');
plot([mid_pt2(1), mid_pt3(1)], [mid_pt2(2), mid_pt3(2)], 'r--');
xlabel('X');
ylabel('Y');

四元数进行三维空间姿态插值：

% 定义起始和目标姿态
start_quat = [0.7071, 0, 0.7071, 0];
end_quat = [0.5, 0.5, 0.5, 0.5];

% 插值
t = linspace(0, 1, 100);
quat_interp = quatSlerp(start_quat, end_quat, t);

% 动画演示
for i = 1:length(quat_interp)
    R = quat2rotm(quat_interp(i, :));
    plot3([0, R(1,1)], [0, R(2,1)], [0, R(3,1)], 'r', 'LineWidth', 2);
    hold on;
    plot3([0, R(1,2)], [0, R(2,2)], [0, R(3,2)], 'g', 'LineWidth', 2);
    plot3([0, R(1,3)], [0, R(2,3)], [0, R(3,3)], 'b', 'LineWidth', 2);
    xlim([-1, 1]);
    ylim([-1, 1]);
    zlim([-1, 1]);
    pause(0.05);
    hold off;
end

希望能对你有帮助！