function zhengyundongxue( )
%已知关节角求变换矩阵
a=[0,-0.42500,-0.39225,0,0,0];
d=[0.089159,0,0,0.10915,0.09465,0.08230];
alpha=[pi/2,0,0,pi/2,-pi/2,0];
T01=T_para(theta(1),d(1),a(1),alpha(1));
T12=T_para(theta(2),d(2),a(2),alpha(2));
T23=T_para(theta(3),d(3),a(3),alpha(3));
T34=T_para(theta(4),d(4),a(4),alpha(4));
T45=T_para(theta(5),d(5),a(5),alpha(5));
T56=T_para(theta(6),d(6),a(6),alpha(6));
T=T01*T12*T23*T34*T45*T56;
end
function T = T_para(theta,d,a,alpha)
T=[ccc(theta),-sss(theta)*ccc(alpha),sss(theta)*sss(alpha),a*ccc(theta);
sss(theta),ccc(theta)*ccc(alpha),-ccc(theta)*sss(alpha),a*sss(theta);
0,sss(alpha),ccc(alpha),d;
0,0,0,1];
end
function sss=sss(a)
% sss=sin(a/180*pi);
sss=sin(a);
end
function ccc=ccc(a)
% ccc=cos(a/180*pi);
ccc=cos(a);
end
function theta=niyundongxue(T)
%变换矩阵T已知
%SDH:标准DH参数表求逆解(解析解)
%部分DH参数表如下,需要求解theta信息
a=[0,-0.42500,-0.39225,0,0,0];
d=[0.089159,0,0,0.10915,0.09465,0.08230];
alpha=[pi/2,0,0,pi/2,-pi/2,0];% alpha没有用到,故此逆解程序只适合alpha=[pi/2,0,0,pi/2,-pi/2,0]的情况!
nx=T(1,1);ny=T(2,1);nz=T(3,1);
ox=T(1,2);oy=T(2,2);oz=T(3,2);
ax=T(1,3);ay=T(2,3);az=T(3,3);
px=T(1,4);py=T(2,4);pz=T(3,4);
%求解关节角1
m=d(6)*ay-py; n=ax*d(6)-px;
theta1(1,1)=atan2(m,n)-atan2(d(4),sqrt(m^2+n^2-(d(4))^2));
theta1(1,2)=atan2(m,n)-atan2(d(4),-sqrt(m^2+n^2-(d(4))^2));
%求解关节角5
theta5(1,1:2)=acos(ax*sin(theta1)-ay*cos(theta1));
theta5(2,1:2)=-acos(ax*sin(theta1)-ay*cos(theta1));
%求解关节角6
mm=nx*sin(theta1)-ny*cos(theta1); nn=ox*sin(theta1)-oy*cos(theta1);
theta6=atan2(mm,nn)-atan2(sin(theta5),0);
%求解关节角3
mmm=d(5)*(sin(theta6).*(nx*cos(theta1)+ny*sin(theta1))+cos(theta6).*(ox*cos(theta1)+oy*sin(theta1))) ...
-d(6)*(ax*cos(theta1)+ay*sin(theta1))+px*cos(theta1)+py*sin(theta1);
nnn=pz-d(1)-az*d(6)+d(5)*(oz*cos(theta6)+nz*sin(theta6));
theta3(1:2,:)=acos((mmm.^2+nnn.^2-(a(2))^2-(a(3))^2)/(2*a(2)*a(3)));
theta3(3:4,:)=-acos((mmm.^2+nnn.^2-(a(2))^2-(a(3))^2)/(2*a(2)*a(3)));
%求解关节角2
mmm_s2(1:2,:)=mmm;
mmm_s2(3:4,:)=mmm;
nnn_s2(1:2,:)=nnn;
nnn_s2(3:4,:)=nnn;
s2=((a(3)*cos(theta3)+a(2)).*nnn_s2-a(3)*sin(theta3).*mmm_s2)./ ...
((a(2))^2+(a(3))^2+2*a(2)*a(3)*cos(theta3));
c2=(mmm_s2+a(3)*sin(theta3).*s2)./(a(3)*cos(theta3)+a(2));
theta2=atan2(s2,c2);
%整理关节角1 5 6 3 2
theta(1:4,1)=theta1(1,1);theta(5:8,1)=theta1(1,2);
theta(:,2)=[theta2(1,1),theta2(3,1),theta2(2,1),theta2(4,1),theta2(1,2),theta2(3,2),theta2(2,2),theta2(4,2)]';
theta(:,3)=[theta3(1,1),theta3(3,1),theta3(2,1),theta3(4,1),theta3(1,2),theta3(3,2),theta3(2,2),theta3(4,2)]';
theta(1:2,5)=theta5(1,1);theta(3:4,5)=theta5(2,1);
theta(5:6,5)=theta5(1,2);theta(7:8,5)=theta5(2,2);
theta(1:2,6)=theta6(1,1);theta(3:4,6)=theta6(2,1);
theta(5:6,6)=theta6(1,2);theta(7:8,6)=theta6(2,2);
%求解关节角4
theta(:,4)=atan2(-sin(theta(:,6)).*(nx*cos(theta(:,1))+ny*sin(theta(:,1)))-cos(theta(:,6)).* ...
(ox*cos(theta(:,1))+oy*sin(theta(:,1))),oz*cos(theta(:,6))+nz*sin(theta(:,6)))-theta(:,2)-theta(:,3);
end
L1 = Link('d', 0.089159, 'a', 0, 'alpha', pi/2 ,'standard' );
L2 = Link('d', 0, 'a', -0.42500, 'alpha', 0 ,'standard' );
L3 = Link('d', 0, 'a', -0.39225, 'alpha', 0 ,'standard' );
L4 = Link('d', 0.10915, 'a', 0, 'alpha', pi/2 ,'standard' );
L5 = Link('d', 0.09465, 'a', 0, 'alpha', -pi/2 ,'standard');
L6 = Link('d', 0.08230, 'a', 0, 'alpha', 0 ,'standard');
robot_UR5=SerialLink([L1,L2,L3,L4,L5,L6],'name','UR5'); %SerialLink 类函数
robot_UR5.display(); %Link 类函数
robot_UR5.teach(); %可以自由拖动的关节角度
theta=[1,1/2,1,1,1,1];%没有什么含义,随便取的
% robot_UR5.plot(theta); %SerialLink 类函数
%利用机器人工具箱求正、逆运动学
A=fkine(robot_UR5,theta);
AA=ikine(robot_UR5,A);
%利用刚写的函数求正、逆运动学
B=zhengyundongxue(theta);
BB=niyundongxue(B);
f_ikine001_ABBirb-120型机器人_运动学正解_源码
版权申诉
9 浏览量
2021-10-04
00:21:54
上传
评论 2
收藏 2KB ZIP 举报
kikikuka
- 粉丝: 67
- 资源: 4773
最新资源
- VIVADO中UART IP核使用
- 【深度学习实际案例解析】深度学习实际案例解析
- 封装swagger组件,提供全新UI以及无状态登录接口调用解决方案
- 小龙坎支局2024年4月渠道积分核对数据.xlam
- onlyoffice搭建及与alist使用的view.html
- Quadcopter-UAV-attitude-estimation-linux常用命令大全demo
- Quadcopter-UAV-attitude-estimation-based-on-数据库课程设计
- pbdlib-python-master.zip
- 43904245495352013_base.apk
- 基于springboot+vue + redis的工作流审批系统
资源上传下载、课程学习等过程中有任何疑问或建议,欢迎提出宝贵意见哦~我们会及时处理!
点击此处反馈
评论0