01 关节空间轨迹规划

绘制图为：

机械臂位置及其运动过程
关节位置&速度&加速度随时间变化曲线
末端执行器轨迹(x-y视图)
带轨迹标记的运动过程

% RoboticToolbox v10
% 关节空间轨迹规划
clcclear
%模型导入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]，则最后一个关节角度一直是0q2 = 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) %绘制关节角加速度随时间的变化，如图2grid 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平面内的末端轨迹，如图3xlabel('X轴(mm)');ylabel('Y轴(mm)')%title('末端执行器轨迹(x-y视图)')%带轨迹标记的运动过程figure('name','带轨迹标记的运动过程')plot3(p(:,1),p(:,2),p(:,3),'LineWidth',3)bot.plot(q)

02 笛卡尔空间轨迹规划

绘制的图形分别为

机械臂位置及其运动过程
关节转角随时间变化
末端轨迹 x-y视图
末端轨迹三维视图
各关节速度&加速度变化曲线
末端 x、y、z轴位置的变化曲线
末端 x、y、z轴速度&加速度的变化曲线

 clcclear
%模型导入mdl_5dof    du = pi/180;ra = 180/pi;%% 运动学轨迹t=[0:0.05:2];%两秒完成轨迹，步长0.05%产生位姿矩阵T1 = bot.fkine([-70 60 -50  10  30]*du);%10T2 = bot.fkine([-1.1278    0.2226    0.9737    -0.6219    0.1415]);%笛卡尔运动规划Ts=ctraj(T1,T2,length(t));for i=1:41q(i,:) = bot.ikine(Ts(i),'mask',[1 1 1 1 0 1]);endbot.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平面内的末端轨迹，如图3xlabel('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)-1v(i,:)=(q(i+1,:)-q(i,:))/delt_t;endplot(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)-1a(i+1,:)=(v(i+1,:)-v(i,:))/delt_t;endplot(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;endsubplot(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)-1v_c(i,:)=(p_c(i+1,:)-p_c(i,:))/delt_t;endplot(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)-1a_c(i+1,:)=(v_c(i+1,:)-v_c(i,:))/delt_t;endplot(t(1:40),a_c,'LineWidth',2)%绘制轨迹grid on;xlabel('时间(s)');ylabel('加速度(mm/s_2)')legend('p_x','p_y','p_z','location','northeastoutside')

03 五次多项式轨迹规划自己写的函数

function[q,qd,qdd]=FiveInterpolation(q0,qf,t,qd0,qdf,qdd0,qddf)if length(t)==1time_seq=(0:t-1)'/(t-1);time_duration=1;elset_max=max(t);t_min=min(t);time_duration=t_max-t_min;time_seq=(t-t_min)/(time_duration);time_seq=time_seq(:);endq0=q0(:);qf=qf(:);if length(q0)~=length(qf)error('输入矢量长度不一致')endif nargin==3qd0=zeros(length(q0),1);qdf=qd0;qdd0=zeros(length(q0),1);qddf=qdd0;elseif nargin==5qdd0=zeros(length(q0),1);qddf=qdd0;
%         if (1ength(qd0)~=length(q0))||(1ength(qdO)~=length(qdf))
%             error('输入矢量长度不一致')
%         endqd0=qd0(:);qdf=qdf(:);elseif nargin==7qdd0=qdd0(:);qddf=qddf(:);
%         if(1ength(qdd0)~=length(q0))||(1ength(qdd0)~=length(qddf))
%             error('输入矢量长度不一致')
%         endelseerror('输人参数数目不对')endF=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>=2coeff=[zeros(size(A)) 5*A 4*B 3*C 2*D E]';q=time_matrix*coeff/time_duration;endif nargout>=3coeff=[zeros(size(A)) zeros(size(A)) 20*A 12*B 6*C 2*D]';qdd=time_matrix*coeff/time_duration^2;end
end

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        00     1    0      0       0        0        0        00     0    2      0       0        0        0        01     t1   t1^2   t1^3    t1^4     t1^5     t1^6     t1^71     t2   t2^2   t2^3    t2^4     t2^5     t2^6     t2^71     tm   tm^2   tm^3    tm^4     tm^5     tm^6     tm^70     1    2*tm   3*tm^2  4*tm^3   5*tm^4   6*tm^5   7*tm^60     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);figurewaitforbuttonpress;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;

05 智能算法+插补

为更好的解决实际问题，可以在多项式插值的基础上，结合 遗传算法，粒子群算法等智能算法

可以参考硕士论文《六自由度机器人运动控制及轨迹规划研究_韩冲》

参考：

https://max.book118.com/html/2017/1221/145290217.shtm

Matlab机器人工具箱（3-2）：五自由度机械臂（轨迹规划）相关推荐

基于matlab的mk2三自由度机械臂轨迹规划及控制器仿真设计
基于matlab的mk2三自由度机械臂轨迹规划及控制器仿真设计(报告+ppt) 摘要:本文的研究对象为EEZYbotARM MK2三自由度机械臂,分析了其机械结构,建立D-H参数表,同时在MATLA ...
Matlab机器人工具箱（3-3）：五自由度机械臂（动力学）
动力学主要分为牛顿-欧拉法和拉格朗日法牛顿-欧拉法: 向外递推速度与角速度,向内迭代计算力与力矩拉格朗日方程: 根据能量思想,从标量(拉格朗日方程)得到动力学方程先计算动能与势能,再构造拉格朗日 ...
Matlab机器人工具箱（3-1）：五自由度机械臂（正逆运动学）
01 正运动学:DH表示法 1955年, Denavit和Hartenberg在"ASME Journal of Applied Mechanic"发表了一篇论文,这篇论文介绍了一 ...
五自由度机械臂正逆运动学算法（Ｃ语言+Matlab）
五自由度机械臂建模学习代码都记录在个人github上,欢迎关注~ Matlab机器人工具箱版本9.10 机械臂还是原来的机械臂,之前用ROS做底层驱动,不需要写正逆运动学和相关算法就能得到一些简单的 ...
使用ROS MoveIt!控制真实五自由度机械臂
文章目录使用ROS MoveIt!控制diy五自由度机械臂写在前面修改功能包中相关文件编写myrobot_controllers.yaml文件修改launch文件 myrobot_drive ...
基于MATLAB的关节型六轴机械臂轨迹规划仿真（2021实测完整代码）
我是一个目录基于MATLAB的关节型六轴机械臂轨迹规划仿真 1 实验目的 2 实验内容 2.1标准D-H参数法 2.2实验中使用的Matlab函数 3实验结果 4 全部代码基于MATLAB的关节型 ...
采用rrt进行机械臂轨迹规划得到轨迹的算法步骤
采用RRT进行机械臂轨迹规划得到轨迹的算法步骤如下: 定义机械臂的状态空间和操作空间,其中状态空间包括机械臂的关节角度.末端执行器的位置和姿态等信息,操作空间包括机械臂的关节空间和笛卡尔空间. 初始化 ...
Matlab机器人工具箱（3-4）：五自由度机械臂（计算力矩控制方法与roblocks）
01 roblocks使用方法在命令行输入roblocks 打开机器人工具箱的模块库使用'roblocks'命令打开simulink 机器人模块时提示版本过低的解决办法: ① 打开帮助–选择小齿轮 ...
五轴机械臂实现视觉抓取--realsense深度相机和五自由度机械臂
前言:要实现视觉抓取,首先需要实现机械臂的驱动,深度相机的目标识别,能够反馈位置. 1.实现机械臂在ROS层的控制 2.基于深度相机目标物体的空间坐标反馈,需要知道摄像头中物体的像素坐标系到大地坐标系 ...
机械臂轨迹规划篇（一）MATLAB测试三次多项式样条插值
MATLAB三次多项式样条插值前言机械臂运动空间三次多项式插值原理 MATLAB仿真测试前言最近这一段时间有点忙,所以博客这方面也更的少了,不过我有学到新的知识,还是会尽量更的,由于之前一直 ...

Matlab机器人工具箱（3-2）：五自由度机械臂（轨迹规划）

01 关节空间轨迹规划

02 笛卡尔空间轨迹规划

03 五次多项式轨迹规划自己写的函数

04 demo：七次多项式插值-关节轨迹规划

05 智能算法+插补

Matlab机器人工具箱（3-2）：五自由度机械臂（轨迹规划）相关推荐

最新文章

热门文章