热门标签
热门文章
- 1nexus3 maven迁移仓库迁移_nexus3仓库迁移
- 2干货 | 详述 Elasticsearch 向量检索发展史
- 3Spring Boot 使用validation校验参数
- 4通过eXtplorer+cpolar，搭建个人云存储并实现访问内网服务器数据
- 5TestFlight 提交提示“处理你的请求时出错。请稍后再试“下载提示“所请求的app不可以或者不存在“,报错422_testflight怎么发布处理你的请求时出错。请稍后再试。
- 6算法系列——寻找旋转排序数组中的最小值(剑指offer)
- 7现在的00后，真是卷死了呀，辞职信已经写好了·····
- 8使用kubeadm的方式搭建k8s(1.18.2版本)高可用集群_kubeadm.yaml
- 9云时代，好用的数据迁移方案推荐_云产品及数据在生产运行环境下的不停机迁移方案
- 10chatgpt赋能python：Python量化交易：为什么Python是最好的选择_pyalgotrade optimizer cuda
当前位置: article > 正文
Matlab机器人工具箱（3-2）：五自由度机械臂（轨迹规划）_matlab 进行机械臂仿真时,如何画出末端轨迹
作者：我家自动化 | 2024-02-16 21:27:58
赞
踩
matlab 进行机械臂仿真时,如何画出末端轨迹
轨迹规划可以分为关节空间的轨迹规划和笛卡尔空间的轨迹规划
-
关节空间规划：用时小，计算量少
[q,qt,qtt]=jtraj(q1,q2,t);
返回关节的位置、速度、加速度
使用五次多项式插值 -
笛卡尔空间规划：对于直线、圆形等末端轨迹形状要求严格的场合
Ts=ctraj(T1,T2,length(t));
返回末端位姿
使用带抛物线过渡的直线规划（对位姿矩阵的3个位置量） -
所用函数：
p = transl(Ts);
轨迹的位移部分
01 关节空间轨迹规划
绘制图为：
- 机械臂位置及其运动过程
- 关节 位置&速度&加速度 随时间变化曲线
- 末端执行器轨迹(x-y视图)
- 带轨迹标记的运动过程
% RoboticToolbox v10 % 关节空间轨迹规划 clc clear %模型导入 mdl_5dof du = pi/180; %% 运动学轨迹 % 改 t=[0:0.1:8];%8秒完成轨迹，步长0.05 %产生位姿矩阵法1：直接给出关节角度 T1 = bot.fkine([20 50 -30 -25 -10]*du);%生成一个位姿 T2 = bot.fkine([70 10 -60 -50 30]*du);%生成一个位姿 %产生位姿矩阵法2：描述位置 %T1 = transl(0.2,0.2,0.2)*trotx(pi/4);%位移*旋转,创建齐次变换 %T2 = transl(0.2,-0.1,0.1)*trotx(pi/2); %T1 = transl(300)*trotz(pi/4);%位移*旋转,创建齐次变换 %T2 = transl(200)*trotz(pi/2); %关节角度 q1 = bot.ikine(T1,'mask',[1 1 1 1 0 1]); %如果是[1 1 1 1 1 0]，则最后一个关节角度一直是0 q2 = bot.ikine(T2,'mask',[1 1 1 1 0 1],'q0',q1); %关节空间运动规划 [q,qt,qtt]=jtraj(q1,q2,t); %笛卡尔运动规划 %Ts=ctraj(T1,T2,length(t)); bot.plot(q)%绘制轨迹 %% 画图 figure('name','关节随时间变化') subplot(3, 1, 1); plot(t, q,'LineWidth',1.5) %绘制关节角随时间的变化 grid on; xlabel('时间(s)');ylabel('关节角度(rad)') set(gca,'YLim',[-3 2]); set(gca,'YTick',[-3,-2:2:2]);%设置要显示坐标刻度 legend('关节1','关节2','关节3','关节4','关节5','location','northeastoutside') subplot(3,1,2); plot(t, qt,'LineWidth',1.5) %绘制关节角速度随时间的变化 grid on; xlabel('时间(s)');ylabel('角速度(rad/s)') set(gca,'YLim',[-0.3 0.3]); set(gca,'YTick',[-0.3:0.15:0.3]);%设置要显示坐标刻度 legend('关节1','关节2','关节3','关节4','关节5','location','northeastoutside') subplot(3, 1, 3); plot(t, qtt,'LineWidth',1.5) %绘制关节角加速度随时间的变化，如图2 grid on; xlabel('时间(s)');ylabel('角加速度(rad/s^2)') set(gca,'YLim',[-0.12 0.12]); set(gca,'YTick',[-0.12:0.06:0.12]);%设置要显示坐标刻度 legend('关节1','关节2','关节3','关节4','关节5','location','northeastoutside') %% %末端执行器轨迹(x-y视图) figure('name','末端执行器轨迹(x-y视图)') T = bot.fkine(q); %得到笛卡尔轨迹 p = transl(T);%轨迹的位移部分 plot(p(:,1),p(:,2),'LineWidth',1.5) %绘制xy平面内的末端轨迹，如图3 xlabel('X轴(mm)');ylabel('Y轴(mm)') %title('末端执行器轨迹(x-y视图)') %带轨迹标记的运动过程 figure('name','带轨迹标记的运动过程') plot3(p(:,1),p(:,2),p(:,3),'LineWidth',3) bot.plot(q)
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
02 笛卡尔空间轨迹规划
绘制的图形分别为
- 机械臂位置及其运动过程
- 关节转角随时间变化
- 末端轨迹 x-y视图
- 末端轨迹 三维视图
- 各关节 速度&加速度 变化曲线
- 末端 x、y、z轴位置的变化曲线
- 末端 x、y、z轴 速度&加速度 的变化曲线
clc clear %模型导入 mdl_5dof du = pi/180; ra = 180/pi; %% 运动学轨迹 t=[0:0.05:2];%两秒完成轨迹，步长0.05 %产生位姿矩阵 T1 = bot.fkine([-70 60 -50 10 30]*du);%10 T2 = bot.fkine([-1.1278 0.2226 0.9737 -0.6219 0.1415]); %笛卡尔运动规划 Ts=ctraj(T1,T2,length(t)); for i=1:41 q(i,:) = bot.ikine(Ts(i),'mask',[1 1 1 1 0 1]); end bot.plot(q)%绘制轨迹 %% figure('name','关节转角') plot(t,q,'LineWidth',2) %绘制关节角随时间的变化 grid on; xlabel('时间(s)');ylabel('关节角度(rad)') legend('关节1','关节2','关节3','关节4','关节5','location','northeastoutside') %% figure('name','末端执行器轨迹(x-y视图)') p = transl(Ts);%轨迹的位移部分 plot(p(:,1),p(:,2),'LineWidth',1.5) %绘制xy平面内的末端轨迹，如图3 xlabel('X轴(mm)');ylabel('Y轴(mm)') % title('末端执行器轨迹(x-y视图)') grid on; %% figure('name','T0-T1直线轨迹（三维）') plot2(p); xlabel('x轴/mm');ylabel('y轴/mm');zlabel('z轴/mm'); grid on; %% figure('name','v&a') % 速度 subplot(2,1,1) delt_t=t(2)-t(1); for i=1:length(t)-1 v(i,:)=(q(i+1,:)-q(i,:))/delt_t; end plot(t,[[0 0 0 0 0];v],'LineWidth',2)%绘制轨迹 grid on; xlabel('时间(s)');ylabel('关节角速度(rad/s)') legend('关节1','关节2','关节3','关节4','关节5','location','northeastoutside') % 加速度 subplot(2,1,2) delt_t=t(2)-t(1); a(1,:)=v(1)/delt_t; for i=1:length(v)-1 a(i+1,:)=(v(i+1,:)-v(i,:))/delt_t; end plot(t(1:40),a,'LineWidth',2)%绘制轨迹 grid on; xlabel('时间(s)');ylabel('关节角加速度(rad/s_2)') legend('关节1','关节2','关节3','关节4','关节5','location','northeastoutside') %% %% figure('name','cart_p') % x、y、z轴 末端执行器加速度 for i=1:length(t) p_c(i,:)=Ts(i).t; end subplot(3,1,1) plot(t,p_c(:,1),'LineWidth',2)%绘制轨迹 xlabel('时间(s)');ylabel('X轴位置(m/s)') subplot(3,1,2) plot(t,p_c(:,2),'LineWidth',2)%绘制轨迹 xlabel('时间(s)');ylabel('Y轴位置(m/s)') subplot(3,1,3) plot(t,p_c(:,3),'LineWidth',2)%绘制轨迹 xlabel('时间(s)');ylabel('Z轴位置(m/s)') figure('name','cart——v&a') % figure('name','cart_pv') % x、y、z轴 末端执行器速度 subplot(2,1,1) delt_t=t(2)-t(1); for i=1:length(t)-1 v_c(i,:)=(p_c(i+1,:)-p_c(i,:))/delt_t; end plot(t,[[0 0 0];v_c],'LineWidth',2)%绘制轨迹 grid on; xlabel('时间(s)');ylabel('速度(m/s)') legend('p_x','p_y','p_z','location','northeastoutside') % figure('name','cart_pa') % x、y、z轴 末端执行器加速度 subplot(2,1,2) delt_t=t(2)-t(1); a_c(1,:)=v_c(1,:)/delt_t; for i=1:length(v_c)-1 a_c(i+1,:)=(v_c(i+1,:)-v_c(i,:))/delt_t; end plot(t(1:40),a_c,'LineWidth',2)%绘制轨迹 grid on; xlabel('时间(s)');ylabel('加速度(mm/s_2)') legend('p_x','p_y','p_z','location','northeastoutside')
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
- 98
- 99
- 100
- 101
- 102
- 103
- 104
- 105
- 106
- 107
- 108
- 109
03 五次多项式轨迹规划自己写的函数
function[q,qd,qdd]=FiveInterpolation(q0,qf,t,qd0,qdf,qdd0,qddf) if length(t)==1 time_seq=(0:t-1)'/(t-1); time_duration=1; else t_max=max(t); t_min=min(t); time_duration=t_max-t_min; time_seq=(t-t_min)/(time_duration); time_seq=time_seq(:); end q0=q0(:); qf=qf(:); if length(q0)~=length(qf) error('输入矢量长度不一致') end if nargin==3 qd0=zeros(length(q0),1); qdf=qd0; qdd0=zeros(length(q0),1); qddf=qdd0; elseif nargin==5 qdd0=zeros(length(q0),1); qddf=qdd0; % if (1ength(qd0)~=length(q0))||(1ength(qdO)~=length(qdf)) % error('输入矢量长度不一致') % end qd0=qd0(:); qdf=qdf(:); elseif nargin==7 qdd0=qdd0(:); qddf=qddf(:); % if(1ength(qdd0)~=length(q0))||(1ength(qdd0)~=length(qddf)) % error('输入矢量长度不一致') % end else error('输人参数数目不对') end F=q0;E=qd0;D=qdd0/2; templ=qf-D-E-F; temp2=qdf-2*D-E; temp3=qddf/2-D; A=-2*(temp2-3*templ)+(temp3-temp2); B=temp2-3*templ-2*A; C=templ-B-A; time_matrix=[time_seq.^5 time_seq.^4 time_seq.^3 time_seq.^2 time_seq ones(size(time_seq))];%时间矩阵 coeff=[A B C D E F]';%系数矩阵 q=time_matrix*coeff; if nargout>=2 coeff=[zeros(size(A)) 5*A 4*B 3*C 2*D E]'; q=time_matrix*coeff/time_duration; end if nargout>=3 coeff=[zeros(size(A)) zeros(size(A)) 20*A 12*B 6*C 2*D]'; qdd=time_matrix*coeff/time_duration^2; end end
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
04 demo：七次多项式插值-关节轨迹规划
参考：Matlab Robotics ToolBox 实战 – 七次多项式取放轨迹规划
% v10 % 七次插值轨迹规划 close all; clc; mdl_puma560 t0 = 0;%开始时刻 t1 = 2;%提升结束时刻 t2 = t1 + 4;%平移结束时刻 tm = t2 + 3;%下降结束时刻 t0_1 = 0:0.2:2;%上升时间 t1_2 = 0:0.5:4;%平移时间 t2_x = 0:0.3:3;%下降时间 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];%存货点 T0 = transl(aim0); T1 = transl(aim1); T2 = transl(aim2); Tx = transl(aimx); theta0 = p560.ikine6s(T0,'rdf');%左臂、手肘朝下、手腕翻转(旋转180度) theta1 = p560.ikine6s(T1,'rdf'); theta2 = p560.ikine6s(T2,'rdf'); thetax = p560.ikine6s(Tx,'rdf'); %初始条件 theta0_ = [0 0 0 0 0 0];%初始位置速度 theta0__ = [0 0 0 0 0 0];%初始位置加速度 thetax_ = [0 0 0 0 0 0];%目标位置速度 thetax__ = [0 0 0 0 0 0];%目标位置加速度 Theta = [theta0' theta0_' theta0__' theta1' theta2' thetax' thetax_' thetax__']'; M = [1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 2 0 0 0 0 0 1 t1 t1^2 t1^3 t1^4 t1^5 t1^6 t1^7 1 t2 t2^2 t2^3 t2^4 t2^5 t2^6 t2^7 1 tm tm^2 tm^3 tm^4 tm^5 tm^6 tm^7 0 1 2*tm 3*tm^2 4*tm^3 5*tm^4 6*tm^5 7*tm^6 0 0 2 6*tm 12*tm^2 20*tm^3 30*tm^4 42*tm^5]; C = M^-1 * Theta;%第i列对应第i个关节的其次多项式系数 %计算关节各函数 tmietick = 0.1; T = 0: tmietick:9; %角度 Q = [ones(int16(9/tmietick)+1,1) T' (T.^2)' (T.^3)' (T.^4)' (T.^5)' (T.^6)' (T.^7)']*C; %速度 Qv =[zeros(int16(9/tmietick)+1,1) ones(int16(9/tmietick)+1,1) 2* T' 3*(T.^2)' 4*(T.^3)' 5*(T.^4)' 6*(T.^5)' 7*(T.^6)']*C; %加速度 Qa =[zeros(int16(9/tmietick)+1,1) zeros(int16(9/tmietick)+1,1) 2*ones(int16(9/tmietick)+1,1) 6*T' 12*(T.^2)' 20*(T.^3)' 30*(T.^4)' 42*(T.^5)']*C; %正运动学分析 Txy=p560.fkine(Q); %画轨迹 Tjtraj1=transl(Txy); x = Tjtraj1(:,1); y = Tjtraj1(:,2); z = Tjtraj1(:,3); figure waitforbuttonpress; 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(T,Q); title('关节位移'); xlabel('时间t/s'); ylabel('位移s/rad'); legend('关节1','关节2','关节3','关节4','关节5','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(T,Qv); title('关节速度'); xlabel('时间t/s'); ylabel('速度v/(rad/s)'); legend('关节1','关节2','关节3','关节4','关节5','location','northeastoutside' ); grid on; subplot(3,1,3); plot(T,Qa); title('关节加速度'); xlabel('时间t/s'); ylabel('加速度a/(rad/s^2)'); legend('关节1','关节2','关节3','关节4','关节5','location','northeastoutside' ); grid on;
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
- 98
- 99
- 100
- 101
- 102
- 103
- 104
- 105
- 106
- 107
- 108
- 109
- 110
- 111
05 智能算法+插补
为更好的解决实际问题，可以在多项式插值的基础上，结合 遗传算法，粒子群算法等智能算法
可以参考硕士论文《六自由度机器人运动控制及轨迹规划研究_韩冲》
参考：
声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/我家自动化/article/detail/96832
推荐阅读
相关标签
