puma560 353多项式规划轨迹

图南_SEU

已于 2023-10-29 18:29:08 修改

阅读量596

点赞数 3

文章标签：算法 matlab

于 2023-10-29 18:28:14 首次发布

本文链接：https://blog.csdn.net/m0_63842364/article/details/134105813

版权

利用matlab仿真，353多项式规划6自由度puma机器人的运动轨迹

I.353多项式的轨迹规划

1.边界条件的整理

aim0 = [0,-0.5,-0.5];%取货点
aim1 = [0,-0.5,0.2];%提升点
aim2 = [-0.5,0.5,0.2];%下落点
aimx = [-0.5,0.5,-0.5];%存货点
theta0_d = [0 0 0 0 0 0];%初始位置速度
thetaf_d = [0 0 0 0 0 0];%目标位置速度
theta1_d = [0 0 0 0 0 0];
theta2_d = [0 0 0 0 0 0];
theta1_dd = [0 0 0 0 0 0];
theta2_dd = [0 0 0 0 0 0];

2.求解转移过程中的关节角

T0 = transl(aim0);
T1 = transl(aim1);
T2 = transl(aim2);
Tx = transl(aimx);

theta1 = p560.ikine6s(T1,'rdf');
theta2 = p560.ikine6s(T2,'rdf');
thetaf = p560.ikine6s(Tx,'rdf');

3.求轨迹未知系数

公式详见尼库的机器人学导论

Theta = [theta0',theta0_d',theta1',theta1',theta1_d',theta1_d',theta1_dd'...
theta2',theta2',theta2_d',theta2_d',theta2_dd',thetaf',thetaf_d']';
%a10 a11 a12 a13 a20 a21 a22 a23 a24 a25 a30 a31 a32 a33
M=[1 0 0 0 0 0 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 0 0 0 0 0
1 t1 t1^2 t1^3 0 0 0 0 0 0 0 0 0 0
0 0 0 0 1 0 0 0 0 0 0 0 0 0
0 1 2*t1 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 1 0 0 0 0 0 0 0 0
0 0 0 0 0 0 2 0 0 0 0 0 0 0
0 0 0 0 1 t2 t2^2 t2^3 t2^4 t2^5 0 0 0 0
0 0 0 0 0 0 0 0 0 0 1 0 0 0
0 0 0 0 0 1 2*t2 3*t2^2 4*t2^3 5*t2^4 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 1 0 0
0 0 0 0 0 0 2 6*t2 12*t2^3 20*t2^4 0 0 0 0
0 0 0 0 0 0 0 0 0 0 1 t3 t3^2 t3^3
0 0 0 0 0 0 0 0 0 0 0 1 2*t3 3*t3^2
];
m = pinv(M);
C = m * Theta;

4.运行情况

5.源代码

close all;
clc;
mdl_puma560

t1 = 5;
t2 = 5;
t3 = 5;

aim0 = [0,-0.5,-0.5];%取货点
aim1 = [0,-0.5,0.2];%提升点
aim2 = [-0.5,0.5,0.2];%下落点
aimx = [-0.5,0.5,-0.5];%存货点
theta0_d = [0 0 0 0 0 0];%初始位置速度
thetaf_d = [0 0 0 0 0 0];%目标位置速度
theta1_d = [0 0 0 0 0 0];
theta2_d = [0 0 0 0 0 0];
theta1_dd = [0 0 0 0 0 0];
theta2_dd = [0 0 0 0 0 0];

T0 = transl(aim0);
T1 = transl(aim1);
T2 = transl(aim2);
Tx = transl(aimx);

theta1 = p560.ikine6s(T1,'rdf');
theta2 = p560.ikine6s(T2,'rdf');
thetaf = p560.ikine6s(Tx,'rdf');


Theta = [theta0',theta0_d',theta1',theta1',theta1_d',theta1_d',theta1_dd'...
theta2',theta2',theta2_d',theta2_d',theta2_dd',thetaf',thetaf_d']';
%a10 a11 a12 a13 a20 a21 a22 a23 a24 a25 a30 a31 a32 a33
M=[1 0 0 0 0 0 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 0 0 0 0 0
1 t1 t1^2 t1^3 0 0 0 0 0 0 0 0 0 0
0 0 0 0 1 0 0 0 0 0 0 0 0 0
0 1 2*t1 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 1 0 0 0 0 0 0 0 0
0 0 0 0 0 0 2 0 0 0 0 0 0 0
0 0 0 0 1 t2 t2^2 t2^3 t2^4 t2^5 0 0 0 0
0 0 0 0 0 0 0 0 0 0 1 0 0 0
0 0 0 0 0 1 2*t2 3*t2^2 4*t2^3 5*t2^4 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 1 0 0
0 0 0 0 0 0 2 6*t2 12*t2^3 20*t2^4 0 0 0 0
0 0 0 0 0 0 0 0 0 0 1 t3 t3^2 t3^3
0 0 0 0 0 0 0 0 0 0 0 1 2*t3 3*t3^2
];
m = pinv(M);
C = m * Theta;

tmietick = 0.1;

T = 0: tmietick:5;

Q1 = [ones(int16(5/tmietick)+1,1) T' (T.^2)' (T.^3)' (0*T.^0)' (0*T.^1)' (0*T.^2)' (0*T.^3)' (0*T.^4)' (0*T.^5)' (0*T.^0)' (0*T.^1)' (0*T.^2)' (0*T.^3)']*C;
Q1d = [(0*T.^0)' (1*T.^0)' (2*T.^1)' (3*T.^2)' (0*T.^0)' (0*T.^1)' (0*T.^2)' (0*T.^3)' (0*T.^4)' (0*T.^5)' (0*T.^0)' (0*T.^1)' (0*T.^2)' (0*T.^3)']*C;
Q1dd = [(0*T.^0)' (0*T.^0)' (2*T.^0)' (6*T.^1)' (0*T.^0)' (0*T.^1)' (0*T.^2)' (0*T.^3)' (0*T.^4)' (0*T.^5)' (0*T.^0)' (0*T.^1)' (0*T.^2)' (0*T.^3)']*C;
Q2 = [(0*T.^0)' (0*T.^1)' (0*T.^2)' (0*T.^3)' (T.^0)' (T.^1)' (T.^2)' (T.^3)' (T.^4)' (T.^5)' (0*T.^0)' (0*T.^1)' (0*T.^2)' (0*T.^3)']*C;
Q2d = [(0*T.^0)' (0*T.^1)' (0*T.^2)' (0*T.^3)' (0*T.^0)' (T.^0)' (2*T.^1)' (3*T.^2)' (4*T.^3)' (5*T.^4)' (0*T.^0)' (0*T.^1)' (0*T.^2)' (0*T.^3)']*C;
Q2dd = [(0*T.^0)' (0*T.^1)' (0*T.^2)' (0*T.^3)' (0*T.^0)' (0*T.^0)' (2*T.^0)' (6*T.^1)' (12*T.^2)' (20*T.^3)' (0*T.^0)' (0*T.^1)' (0*T.^2)' (0*T.^3)']*C;
Q3 = [(0*T.^0)' (0*T.^1)' (0*T.^2)' (0*T.^3)' (0*T.^0)' (0*T.^1)' (0*T.^2)' (0*T.^3)' (0*T.^4)' (0*T.^5)' (T.^0)' (T.^1)' (T.^2)' (T.^3)']*C;
Q3d = [(0*T.^0)' (0*T.^1)' (0*T.^2)' (0*T.^3)' (0*T.^0)' (0*T.^1)' (0*T.^2)' (0*T.^3)' (0*T.^4)' (0*T.^5)' (0*T.^0)' (T.^0)' (2*T.^1)' (3*T.^2)']*C;
Q3dd = [(0*T.^0)' (0*T.^1)' (0*T.^2)' (0*T.^3)' (0*T.^0)' (0*T.^1)' (0*T.^2)' (0*T.^3)' (0*T.^4)' (0*T.^5)' (0*T.^0)' (0*T.^0)' (2*T.^0)' (6*T.^1)']*C;
Q = [Q1;Q2;Q3];
Qd = [Q1d;Q2d;Q3d];
Qdd = [Q1dd;Q2dd;Q3dd];
Txy=p560.fkine(Q);
TT = [T,T,T]; 

%画轨迹
Tjtraj1=transl(Txy);
x = Tjtraj1(:,1);
y = Tjtraj1(:,2);
z = Tjtraj1(:,3);
figure
plot3(x,y,z,'b');%轨迹图像
hold on;
%画出四个过程点
[x0,y0,z0] = ellipsoid(aim0(1),aim0(2),aim0(3),0.05,0.05,0.05);
[x1,y1,z1] = ellipsoid(aim1(1),aim1(2),aim1(3),0.05,0.05,0.05);
[x2,y2,z2] = ellipsoid(aim2(1),aim2(2),aim2(3),0.05,0.05,0.05);
[xx,yx,zx] = ellipsoid(aimx(1),aimx(2),aimx(3),0.05,0.05,0.05);
surf(x0,y0,z0) %画起始点
surf(x1,y1,z1) %画提升点
surf(x2,y2,z2) %画下降点
surf(xx,yx,zx) %画目标点

hold on;
%画轨迹图
p560.plot(Q);

%画关节位置、速度、加速度曲线
figure
subplot(3,1,1);
% plot(T,Q(:,1));
plot(TT,Q);
title('关节位移');
xlabel('时间t/s');
ylabel('位移s/rad');
legend('关节1','关节2','关节3','关节4','关节5','关节6','location','northeastoutside' );
str=[ '\leftarrow' '(' num2str(t1) ',' num2str(theta1(1)) ')'];
text(t1,theta1(1),cellstr(str));
str=[ '\leftarrow' '(' num2str(t2) ',' num2str(theta2(1)) ')'];
text(t2,theta2(1),cellstr(str));
grid on;
subplot(3,1,2);
plot(TT,Qd);
title('关节速度');
xlabel('时间t/s');
ylabel('速度v/(rad/s)');
legend('关节1','关节2','关节3','关节4','关节5','关节6','location','northeastoutside' );
grid on;
subplot(3,1,3);
plot(TT,Qdd);
title('关节加速度');
xlabel('时间t/s');
ylabel('加速度a/(rad/s^2)');
legend('关节1','关节2','关节3','关节4','关节5','关节6','location','northeastoutside' );
grid on;