matlab 五次多项式轨迹规划算法代码

以下是MATLAB中实现五次多项式轨迹规划算法的示例代码：

% 设置起始位置、目标位置、起始速度和目标速度
start_pos = 0;
target_pos = 10;
start_vel = 0;
target_vel = 0;

% 设置时间段
duration = 5; % 总运动时间

% 计算路径的时间步长
dt = 0.01; % 时间步长

% 计算路径的时间段数目
num_steps = duration / dt;

% 计算起始位置和目标位置之间的距离
distance = target_pos - start_pos;

% 生成时间向量
t = linspace(0, duration, num_steps);

% 初始化轨迹数组
trajectory = zeros(size(t));

% 计算五次多项式函数的系数
a0 = start_pos;
a1 = start_vel;
a2 = 0;
a3 = (20 * distance - (8 * target_vel + 12 * start_vel) * duration) / (2 * duration^3);
a4 = (30 * (start_vel + target_vel) * duration - (3 * target_vel + 27 * start_vel) * distance) / (2 * duration^4);
a5 = (-12 * (start_vel + target_vel) * distance + (target_vel + 11 * start_vel) * duration^2) / (2 * duration^5);

% 生成五次多项式轨迹
for i = 1:num_steps
    trajectory(i) = a0 + a1 * t(i) + a2 * t(i)^2 + a3 * t(i)^3 + a4 * t(i)^4 + a5 * t(i)^5;
end

% 绘制轨迹
plot(t, trajectory);
xlabel('时间');
ylabel('位置');
title('五次多项式轨迹规划');

上述代码中，根据给定的起始位置、目标位置、起始速度和目标速度，计算了五次多项式函数的系数，并使用这些系数生成了平滑的轨迹。最后，使用`plot`函数将生成的轨迹可视化出来。

请注意，此代码是一个简单的示例，实际应用中可能需要根据具体问题进行修改和优化，以满足特定的需求。