目录
1、程序和计算
(1)程序打包下载
程序包含:工具箱正反解计算、手写算法正反解计算。
工程链接:6轴机器人手写正反解Matlab算法.rar-制造文档类资源-CSDN下载
(2)计算结果
①正解结果
②逆解结果
2、使用工具箱 正反解计算
clear;
clc;
%建立机器人模型
%alpha:连杆扭角;?
%a:连杆长度;?
%theta:关节转角;?
%d:关节距离;?
%offset:偏移
% theta d a alpha offset
L1=Link([0 0.4 0.025 pi/2 0 ]); %定义连杆的D-H参数
L2=Link([pi/2 0 0.56 0 0 ]);
L3=Link([0 0 0.035 pi/2 0 ]);
L4=Link([0 0.515 0 pi/2 0 ]);
L5=Link([pi 0 0 pi/2 0 ]);
L6=Link([0 0.08 0 0 0 ]);
robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','BiPan'); %连接连杆,机器人取名BiPan
%theta=[0,0,0,0,0,0]; %指定的关节角
theta=[0,pi/2,0,0,pi,0]; %指定的关节角
p=robot.fkine(theta) %fkine正解函数,根据我们给定的关节角theta,求解出末端位姿p
q=robot.ikine(p) %ikine逆解函数,根据我们给定的末端位姿p,求解出关节角q
robot.plot([0,pi/2,0,0,pi,0]); %输出机器人模型,后面的六个角为输出时的theta姿态
teach(robot); %调整各个关节角度
运行结果
3、手写正解算法
(1)手写正解计算方法1
(推荐使用方法2,方法1是根据参数简化的)
改进D-H模型
%改进DH模型——手写正解(T01-T65矩阵内部都是经过化简得)
function [T06]=mymodfkine(theta1,theta2,theta3,theta4,theta5,theta6)
% ti di ai-1 alphai-1
MDH=[theta1 0 0 0;
theta2 0 0.180 -pi/2;
theta3 0 0.600 0;
theta4 0.630 0.130 -pi/2;
theta5 0 0 pi/2;
theta6 0 0 -pi/2];
T01=[cos(MDH(1,1)) -sin(MDH(1,1)) 0 MDH(1,3);
cos(MDH(1,4))*sin(MDH(1,1)) cos(MDH(1,4))*cos(MDH(1,1)) -sin(MDH(1,4)) -MDH(1,2)*sin(MDH(1,4));
sin(MDH(1,4))*sin(MDH(1,1)) sin(MDH(1,4))*cos(MDH(1,1)) cos(MDH(1,4)) MDH(1,2)*cos(MDH(1,4));
0 0 0 1];
T12=[cos(MDH(2,1)) -sin(MDH(2,1)) 0 MDH(2,3);
cos(MDH(2,4))*sin(MDH(2,1)) cos(MDH(2,4))*cos(MDH(2,1)) -sin(MDH(2,4)) -MDH(2,2)*sin(MDH(2,4));
sin(MDH(2,4))*sin(MDH(2,1)) sin(MDH(2,4))*cos(MDH(2,1)) cos(MDH(2,4)) MDH(2,2)*cos(MDH(2,4));
0 0 0 1];
T23=[cos(MDH(3,1)) -sin(MDH(3,1)) 0 MDH(3,3);
cos(MDH(3,4))*sin(MDH(3,1)) cos(MDH(3,4))*cos(MDH(3,1)) -sin(MDH(3,4)) -MDH(3,2)*sin(MDH(3,4));
sin(MDH(3,4))*sin(MDH(3,1)) sin(MDH(3,4))*cos(MDH(3,1)) cos(MDH(3,4)) MDH(3,2)*cos(MDH(3,4));
0 0 0 1];
T34=[cos(MDH(4,1)) -sin(MDH(4,1)) 0 MDH(4,3);
cos(MDH(4,4))*sin(MDH(4,1)) cos(MDH(4,4))*cos(MDH(4,1)) -sin(MDH(4,4)) -MDH(4,2)*sin(MDH(4,4));
sin(MDH(4,4))*sin(MDH(4,1)) sin(MDH(4,4))*cos(MDH(4,1)) cos(MDH(4,4)) MDH(4,2)*cos(MDH(4,4));
0 0 0 1];
T45=[cos(MDH(5,1)) -sin(MDH(5,1)) 0 MDH(5,3);
cos(MDH(5,4))*sin(MDH(5,1)) cos(MDH(5,4))*cos(MDH(5,1)) -sin(MDH(5,4)) -MDH(5,2)*sin(MDH(5,4));
sin(MDH(5,4))*sin(MDH(5,1)) sin(MDH(5,4))*cos(MDH(5,1)) cos(MDH(5,4)) MDH(5,2)*cos(MDH(5,4));
0 0 0 1];
T56=[cos(MDH(6,1)) -sin(MDH(6,1)) 0 MDH(6,3);
cos(MDH(6,4))*sin(MDH(6,1)) cos(MDH(6,4))*cos(MDH(6,1)) -sin(MDH(6,4)) -MDH(6,2)*sin(MDH(6,4));
sin(MDH(6,4))*sin(MDH(6,1)) sin(MDH(6,4))*cos(MDH(6,1)) cos(MDH(6,4)) MDH(6,2)*cos(MDH(6,4));
0 0 0 1];
T06=T01*T12*T23*T34*T45*T56;
(2)手写正解计算方法2
和《5、matlab串联机器人DH改进型-手写正解函数(推荐)》手写正解计算相同。
改进D-H模型
%改进型D-H建模
function [T06]=myfkine2(theta1,theta2,theta3,theta4,theta5,theta6)
% theta d a alpha
% 关节转角 关节距离 连杆长度 连杆扭角
% MDH=[theta1 0.4 0.025 pi/2;
% theta2 0