【MATLAB】5DOFs机械臂关节空间轨迹规划

最新推荐文章于 2025-03-18 14:26:33 发布

None072

最新推荐文章于 2025-03-18 14:26:33 发布

阅读量4.1k

点赞数 8

分类专栏： # Matlab 文章标签： matlab

本文链接：https://blog.youkuaiyun.com/qq_43557907/article/details/124257382

版权

Matlab 专栏收录该内容

3 篇文章

订阅专栏

上一篇博客指路->【MATLAB】5DOFs机械臂运动学正逆解（MDH）
轨迹规划即在运动起始点和终止点之间插入中间点序列，包括关节空间轨迹规划和笛卡尔空间轨迹规划。关节空间轨迹规划把机器人的关节变量变换成对时间的函数，然后对角速度和角加速度进行约束。在此使用MATLAB的Robotics Toolbox工具箱中的jtraj函数对上述关节空间轨迹进行规划。jtraj函数需要传入的参数为起始点与目标点的五个关节角度与时间长度向量，下面的代码中设定采样时间为8s，步长0.1s，程序运行图如下：
在这里插入图片描述

直接贴代码：

clear;
close all;
clc;
%% 参数定义
%机械臂为5自由度机械臂
clear L;
%角度转换
angle=pi/180;  %角度转换
L1 = 106.45; L2 = 101.98; L3 = 95.05; L4 = 107.43;
%D-H参数表
theta1 = 0;   D1 = 0;     A1 = 0;     alpha1 = 0;       
theta2 = 0;   D2 = 0;     A2 = 0;     alpha2 = -pi/2;    
theta3 = 0;   D3 = 0;     A3 = L2;    alpha3 = 0;      
theta4 = 0;   D4 = 0;     A4 = L3;    alpha4 = 0;      
theta5 = 0;   D5 = 0;     A5 = 0;     alpha5 = -pi/2; 
theta6 = 0;   D6 = L4;    A6 = 0;     alpha6 = 0; %虚拟关节，代表末端执行器
%% DH法建立模型,关节转角，关节距离，连杆长度，连杆转角，关节类型（0转动，1移动）,关节范围

L(1) = Link([theta1, D1, A1, alpha1, 0], 'modified');L(1).qlim =[-180*angle, 180*angle];
L(2) = Link([theta2, D2, A2, alpha2, 0], 'modified');L(2).qlim =[-180*angle, 180*angle];
L(3) = Link([theta3, D3, A3, alpha3, 0], 'modified');L(3).qlim =[-180*angle, 180*angle];
L(4) = Link([theta4, D4, A4, alpha4, 0], 'modified');L(4).qlim =[-180*angle, 180*angle];
L(5) = Link([theta5, D5, A5, alpha5, 0], 'modified');L(5).qlim =[-180*angle, 180*angle];
L(6) = Link([theta6, D6, A6, alpha6, 0], 'modified');L(6).qlim =[-180*angle, 180*angle];
robot = SerialLink(L,'name','five');

%%
% 末端执行器末端点{P}的T矩阵
T_P25 = [1 0 0 0;0 1 0 0;0 0 1 L4;0 0 0 1];
% 世界坐标系{W}的T矩阵
T_02W= [1 0 0 0;0 1 0 0;0 0 1 L1;0 0 0 1];

%% {P}相对{W}的T矩阵
px = 180;py = -100; pz = 90; %给定第一个点A在世界坐标系的坐标
px2 = -100;py2 = 100; pz2 = 130;%给定第二个点B在世界坐标系的坐标
T_P2W = [1 0 0 px;0 0 1 py;0 1 0 pz;0 0 0 1];
T_P2W2 = [1 0 0 px2;0 0 1 py2;0 1 0 pz2;0 0 0 1];
T_520 = inv(T_02W)*T_P2W*inv(T_P25);
T_5202 = inv(T_02W)*T_P2W2*inv(T_P25);
% T_P20 = T_520*T_P25;
% T_P202 = T_5202*T_P25;
P_theta_ikine = DOF5_ikine(T_520);
P_theta_ikine2 = DOF5_ikine(T_5202);
theta1 = P_theta_ikine(1, 1:6);%取A角度的第一个解
theta2 = P_theta_ikine2(1, 1:6);%取B角度的第一个解

%% 关节空间运动轨迹
%8秒完成轨迹，步长0.1
t=[0:0.1:8];
[q,qt,qtt]=jtraj(theta1,theta2,t);
T = robot.fkine(q); %得到空间轨迹
p = transl(T);%轨迹的位移部分
%带轨迹标记的运动过程
figure('name','带轨迹标记的运动过程')
plot3(p(:,1),p(:,2),p(:,3),'LineWidth',3)
robot.plot(q)