五次多项式过渡的直线轨迹(Linear Trajectory with Polynomial Blends)

最新推荐文章于 2025-05-10 00:35:12 发布

原创

最新推荐文章于 2025-05-10 00:35:12 发布 · 1.8k 阅读

27 ·

CC 4.0 BY-SA版权

本文介绍使用MATLAB进行轨迹规划的方法，包括使机器人或机械臂精确通过所有预设点的五次多项式轨迹规划，以及仅接近但不严格通过中间点的优化轨迹规划，适用于自动机器和机器人应用。

文章目录

一、经过所有中间点(MATLAB代码)
二、靠近但不经过所有中间点(MATLAB代码)
三、参考文献

一、经过所有中间点(MATLAB代码)

clc;
clear;
close all;

%{
   
   
syms ts te ps pe vs ve as ae T real;
a = [1, 0, 0, 0, 0, 0
    0, 1, 0, 0, 0, 0
    0, 0, 2, 0, 0, 0
    1, T, T^2, T^3, T^4, T^5
    0, 1, 2*T, 3*T^2, 4*T^3, 5*T^4
    0, 0, 2, 6*T, 12*T^2, 20*T^3
    ] \ [ps; vs; as; pe; ve; ae];
a = [simplify(a(1)), simplify(a(2)), simplify(a(3)), simplify(a(4)), simplify(a(5)), simplify(a(6))]'
%}

t = [0, 2, 4, 8, 10]';
pos = [10, 20, 0, 30, 40]';
dt = 0.001;
n = length(t);
accelerationTimeRatio = 0.4 * ones(n - 1, 1);    %加速段/减速段的比率，(0, 1/2)之间

tArray = [];
posArray = [];
velArray = [];
accArray = [];
tArray = [tArray; t(1)];
posArray = [posArray; pos(1)];
velArray = [velArray; 0.0];
accArray = [accArray; 0.0];
for i = 1 : n - 1
    Ta = accelerationTimeRatio(i) * (t(i + 1) - t(i));
    Ts = 0.5 * Ta;
    
    %第一段五次多项式
    ts = t(i);
    te = t(i) + Ta;
    ps = pos(i);
    pe = (pos(i + 1) - pos(i)) / (t(i + 1) - 2.0 * Ts - t(i)) * (t(i) + Ta - (t(i) + Ts)) + pos(i);
    vs = 0.0;
    ve = (pos(i + 1) - pos(i)) / (t(i + 1) - 2.0 * Ts - t(i));
    as = 0.0;
    ae = 0.0;
    h = pe - ps;
    T = te - ts;
    
    a0 = ps;
    a1 = vs;
    a2 = 0.5 * as;
    a3 = (20.0 * h - (8.0 * ve + 12.0 * vs) * T - (3.0 * as - ae) * T^2) / (2.0 * T^3);
    a4 = (-30.0 * h + (14.0 * ve + 16.0 * vs) * T + (3.0 * as - 2.0 * ae) * T^2) / (2.0 * T^4);
    a5 = (12.0 * h - 6.0 * (ve + vs) * T - (as - ae) * T^2) / (2.0 * T^5);
    
    tt = (ts + dt : dt : te)';
    if abs(tt(end) - te) > 1.0e-8
        tt = [tt; te];
    end
    
    tArray = [tArray; tt];
    posArray = [posArray; a0 + (tt - ts) .* (a1 + (tt - ts) .* (a2 + (tt - ts) .* (a3 + (tt - ts) .* (a4 + a5 .* (tt - ts)))))];
    velArray = [velArray; a1 + (tt - ts) .* (2.0 * a2 + (tt - ts) .* (3.0 * a3 + (tt - ts) .* (4.0 * a4 + 5.0 * a5 .* (tt - ts))))];
    accArray = [accArray; 2.0 * a2 + (tt - ts) .* (6.0 * a3 + (tt - ts) .* (12.0 * a4 + 20.0 * a5 .* (tt - ts)))];
    
    %第二段直线
    ts = te;
    te = t(i + 1) - Ta;
    ps = pe;
    pe = (pos(i + 1) - pos(i)) / (t(i + 1)