# 毕设(4)—关节空间轨迹规划（多项式）

## 三次多项式轨迹规划

$\left\{ \begin{array}{l} \theta(t)=a_0+a_1t+a_2t^2+a_3t^3 \\ \dot{\theta}(t)=a_1+2a_2t+3a_3t^2 \\ \ddot{\theta}(t)=2a_2+6a_3t \end{array} \right.$

$\left\{ \begin{array}{l} \theta(0)=q_1=a_0 \\ \theta(t)=q_2=a_0+a_1t+a_2t^2+a_3t^3 \\ \dot{\theta}(0)=v_1=a_1 \\ \dot{\theta}(t)=v_2=a_1+2a_2t+3a_3t^2 \end{array} \right.$

$\left\{ \begin{array}{l} a_0=q_1 \\ a_1=v_1 \\ a_2=\frac{-3q_1+3q_2-2v_1t-v_2t}{t^2} \\ a_3=\frac{2q_1-2q_2+v_1t+v_2t}{t^3} \end{array} \right.$

## 五次多项式轨迹规划

$\left\{ \begin{array}{l} \theta(t)=a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5 \\ \dot{\theta}(t)=a_1+2a_2t+3a_3t^2+4a_4t^3+5a_5t^4 \\ \ddot{\theta}(t)=2a_2+6a_3t+12a_4t^2+20a_5t^3 \end{array} \right.$

$\left\{ \begin{array}{l} \theta(0)=q_1=a_0 \\ \theta(t)=q_2=a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5 \\ \dot{\theta}(0)=v_1=a_1 \\ \dot{\theta}(t)=v_2=a_1+2a_2t+3a_3t^2+4a_4t^3+5a_5t^4 \\ \ddot{\theta}(0)=A_1=2a_2 \\ \ddot{\theta}(t)=A_2=2a_2+6a_3t+12a_4t^2+20a_5t^3 \end{array} \right.$

$\left\{ \begin{array}{l} a_0=q_1 \\ a_1=v_1 \\ a_2=\frac{A1}{2} \\ a_3=\frac{-20q_1+20q_2-12v_1t-8v_2t-3A_1t^2+A_2t^2}{2t^3} \\ a_4=\frac{30q_1-30q_2+16v_1t+14v_2t+3A_1t^2-2A_2t^2}{2t^4} \\ a_5=\frac{-12q_1+12q_2-6v_1t-6v_2t-A_1t^2+A_2t^2}{2t^5} \end{array} \right.$

## Matlab代码验证

test3.m

clear, clc, close all;

L(1) = Link([0 0 0 0 0 0], 'modified');
L(2) = Link([0 0 0 -pi / 2 0 0], 'modified');
L(3) = Link([0 0 135 0 0 0], 'modified');
L(4) = Link([0 0 147 0 0 0], 'modified');
L(5) = Link([0 131 61 -pi / 2 0 0], 'modified');

angle1 = [-pi / 6, -pi / 6, pi / 3, -pi / 6, pi / 4];
angle2 = [pi / 3, -pi / 60, pi / 30, -pi / 60, -pi / 4];
angleT = [angle1; angle2];

VT = [0, 0, 0, 0, 0; 0, 0, 0, 0, 0];
AT = [0, 0, 0, 0, 0; 0, 0, 0, 0, 0];

%[q, qd, qdd, t] = cubic_traj(angleT, VT, [0, 5], 100);
[q, qd, qdd, t] = quintic_traj(angleT, VT, AT, [0, 5], 100);

figure(1);
T = zeros(4, 4, size(q, 1));
for i = 1:size(q, 1)
T(:, :, i) = myfkine(q(i, :));
end
plot3(squeeze(T(1, 4, :)), squeeze(T(2, 4, :)), squeeze(T(3, 4, :)), 'r-', 'LineWidth', 2);
title('轨迹图');
hold on;
plot3(squeeze(T(1,4,1)),squeeze(T(2,4,1)),squeeze(T(3,4,1)), 'bo', 'MarkerSize', 7, 'LineWidth', 2);
hold on;
plot3(squeeze(T(1,4,size(q, 1))),squeeze(T(2,4,size(q, 1))),squeeze(T(3,4,size(q, 1))), 'bo', 'MarkerSize', 7, 'LineWidth', 2);
hold on;
robot.plot(q);
grid on;
hold on;

figure(2);
plot(t,qdeg);
title('角度图');
legend('q_1', 'q_2', 'q_3', 'q_4', 'q_5');
xlabel('t(s)');
ylabel('deg(°)');
grid on;
hold on;

figure(3);
plot(t, qddeg);
title('角速度图');
legend('q_1', 'q_2', 'q_3', 'q_4', 'q_5');
xlabel('t(s)');
ylabel('V(°/s)');
grid on;
hold on;

figure(4);
plot(t, qdddeg);
title('角加速度图');
legend('q_1', 'q_2', 'q_3', 'q_4', 'q_5');
xlabel('t(s)');
ylabel('A(°/s^2)');
grid on;
hold on;


 三次轨迹图 三次角度图 三次角速度图 三次角加速度图

 五次轨迹图 五次角度图 五次角速度图 五次角加速度图

posted @ 2022-06-02 13:26  Dragonet-Z  阅读(849)  评论(0编辑  收藏  举报