puma560的运动学及matlab实现（正解+逆解）
表1 PUMA560机器人的连杆参数
|
关节i |
变化范围/(o) |
||||
|
1 |
90 |
0 |
0 |
0 |
-160~160 |
|
2 |
0 |
-90 |
0 |
149.09 |
-225~45 |
|
3 |
-90 |
0 |
431.8 |
0 |
-45~225 |
|
4 |
0 |
-90 |
20.32 |
443.07 |
-110~170 |
|
5 |
0 |
90 |
0 |
0 |
-100~100 |
|
6 |
0 |
-90 |
0 |
0 |
-266~266 |
正解源码
DEG = pi/180;
cta1=-70.4385
cta2=182.6918
cta3=-90.0000
cta4=-82.4708
cta5=-19.7387
cta6=-97.9933
T01=[cosd(cta1),-sind(cta1),0,0;
sind(cta1), cosd(cta1),0,0;
0,0,1,0;
0,0,0,1];
T02=T01*[cosd(cta2),-sind(cta2),0,0;
0,0,1, 149.09;
-sind(cta2),-cosd(cta2),0,0;
0,0,0,1] ;
T03=T02*[cosd(cta3),-sind(cta3),0,431.8;
sind(cta3), cosd(cta3),0,0;
0,0,1,0;
0,0,0,1];
T04=T03*[cosd(cta4),-sind(cta4),0,20.32;
0,0,1,433.07;
-sind(cta4),-cosd(cta4),0,0;
0,0,0,1];
T05=T04*[cosd(cta5),-sind(cta5),0,0;
0,0,-1,0;
sind(cta5), cosd(cta5), 0,0;
0,0,0,1];
T06=T05*[cosd(cta6),-sind(cta6),0,0;
0,0,1,0;
-sind(cta6),-cosd(cta6),0,0;
0,0,0,1];
O=T06*[0;0;0;1];
=====================================================
逆解源码
fid = fopen('inverseout.txt','w');%逆解的保存文件
%赋初值
T06 =[0.0000 1.0000 0.0000 -149.0900;
0.0000 -0.0000 1.0000 864.8700;
1.0000 0 -0.0000 20.3200;
0 0 0 1.0000] ;
a0=0; a1=0; a2=431.8; a3=20.32; a4=0; a5=0;
d1=0; d2=149.09; d3=0; d4=433.07; d5=0; d6=0;
n_x=T06(1); n_y=T06(2); n_z=T06(3);
o_x=T06(5); o_y=T06(6); o_z=T06(7);
a_x=T06(9); a_y=T06(10); a_z=T06(11);
p_x=T06(13); p_y=T06(14); p_z=T06(15);
disp(['八组解分别是：']);
for i=1:2
for j=1:2
for k=1:2
%求解theta1(为弧度)
sqr1=[sqrt(p_x^2+p_y^2-d2^2),-sqrt(p_x^2+p_y^2-d2^2)];
ta1=atan2(p_y,p_x)-atan2(d2,sqr1(i));
%求解theta3（弧度表示）
k1=(p_x^2+p_y^2+p_z^2-a2^2-a3^2-d2^2-d4^2)/(2*a2);
sqr3=[sqrt(a3^2+d4^2-k1^2),-sqrt(a3^2+d4^2-k1^2) ];
ta3=atan2(a3,d4)-atan2(k1,sqr3(j));
fs23=-((a3+a2*cos(ta3))*p_z)+(cos(ta1)*p_x+sin(ta1)*p_y)*(a2*sin(ta3)-d4);
sc23=(-d4+a2*sin(ta3))*p_z+(cos(ta1)*p_x+sin(ta1)*p_y)*(a2*cos(ta3)+a3);
ta23=atan2( fs23,sc23);
%求解theta2 （弧度表示）
ta2=ta23-ta3;
%求解theta4 （弧度表示）
fs4=[ -a_x*sin(ta1)+a_y*cos(ta1),a_x*sin(ta1)-a_y*cos(ta1)];
sc4=[ -a_x*cos(ta1)*cos(ta23)-a_y*sin(ta1)*cos(ta23)+a_z*sin(ta23),
a_x*cos(ta1)*cos(ta23)+a_y*sin(ta1)*cos(ta23)-a_z*sin(ta23)];
fprintf(fid,'%d,',sc4(1,1));
fprintf(fid,'\t');
fprintf(fid,'%d,',sc4(2,1));
fprintf(fid,'\t');
fprintf(fid,'%d,',fs4(1,1));
fprintf(fid,'\t');
fprintf(fid,'%d,',fs4(1,2));
fprintf(fid,'\t');
fprintf(fid,'\n');
ta4=atan2(fs4(k),sc4(k));
%求解theta5 （弧度表示）
fs5=-a_x*(cos(ta1)*cos(ta23)*cos(ta4)+sin(ta1)*sin(ta4))...
-a_y*(sin(ta1)*cos(ta23)*cos(ta4)-cos(ta1)*sin(ta4))...
+a_z*(sin(ta23)*cos(ta4));
sc5=a_x*(-cos(ta1)*sin(ta23))+a_y*(-sin(ta1)*sin(ta23))+a_z*(-cos(ta23));
ta5=atan2(fs5,sc5);
%求解theta6 （弧度表示）
fs6=-n_x*(cos(ta1)*cos(ta23)*sin(ta4)-sin(ta1)*cos(ta4))...
-n_y*(sin(ta1)*cos(ta23)*sin(ta4)+cos(ta1)*cos(ta4))...
+n_z*(sin(ta23)*sin(ta4));
sc6= n_x*(cos(ta1)*cos(ta23)*cos(ta4)+sin(ta1)*sin(ta4))*cos(ta5)...
-n_x*cos(ta1)*sin(ta23)*sin(ta5)...
+n_y*(sin(ta1)*cos(ta23)*cos(ta4)+cos(ta1)*sin(ta4))*cos(ta5)...
-n_y*sin(ta1)*sin(ta23)*sin(ta5)...
-n_z*(sin(ta23)*cos(ta4)*cos(ta5)+cos(ta23)*sin(ta5));
ta6=atan2(fs6,sc6);
%save
%将其化为角度
Theta=[ta1 ta2 ta3 ta4 ta5 ta6]./pi*180
end
end
end
关于C++版本的运动学正解和逆解的代码，可以在以下链接下载
https://download.csdn.net/download/kobesdu/12099712
puma560的运动学及matlab实现（正解+逆解）相关推荐
- (6.1)MATLAB机器人正、逆解中姿态求解的欧拉角的说明
1.欧拉角的类型: 例程中MATLAB机器人工具箱中采用的是世界坐标系的欧拉角(先旋转的在右边).如下: 利用机器人工具箱的.teach()函数进行可是化时的欧拉角如下: 2.正解算法中的欧拉角: 在 ...
- 6轴机器人运动学正解,逆解2
逆解 逆解计算方法可以参考以下书籍 机器人学导论--分析.系统及应用 电子工业出版社 机器人学导论第3版 机械工业出版社 机器人学建模.规划与控制 西安交通大学出版社 对于关节1,2,3可以从运动方程 ...
- IRB 1600-6/1.45 ABB 改进DH参数计算正解逆解
IRB 1600-6/1.45 ABB机器人正解逆解计算 一.参考文章 二.改进DH参数获取 三.fk推导 四.ik推导 五.测试验证 关节空间验证 附录-代码部分 一.参考文章 最近在项目中,需要通 ...
- 机器人正解逆解-附记
距机器人正解逆解的3篇文章已经有一年多时间 不知不觉浏览已有一万 在评论里看到一些问题 在此尝试解答 建议还是先看看相关的书中的计算方法 不知道计算方法而通过程序去理解如何计算 是比较难的 代码里面的 ...
- 工业机器人（4）-- Matlab Robot Toolbox运动学正、逆解
[Matlab Robotics Toolbox]robotics toolbox学习及使用记录,方便自己后面复习.改进. 基于Matlab R2019b 9.5; Peter Corke的Robot ...
- （6）六轴机械臂的运动学正、逆解
下面在前面的ur5机械臂的DH参数基础是对其正逆解进行求解,为了后面能在MATLAB中利用stl文件进行实际显示,这里以标准DH参数为例进行讲解.(修正DH参数在用plot3d函数是显示失败,不知道是 ...
- 6轴机器人运动学正解,逆解1
正解 给定机器人各关节的角度,计算出机器人末端的空间位置 逆解 已知机器人末端的位置和姿态,计算机器人各关节的角度值 常见的工业机器人 正解与逆解的求解需要相应的机器人运动方程,其中关键的就是DH参数 ...
- 六轴机械臂-正解+逆解+轨迹规划实现
之前,写了一篇博客,从坐标系的说明 -> D-H参数表的建立 -> 正解和逆解的整个算法推导过程整理了一篇博客https://blog.csdn.net/ymj7150697/articl ...
- 修正逆解文章——六轴UR机械臂正逆运动学求解_MATLAB代码（标准DH参数表）
如下参考链接1的作者大大实现了UR5机械臂的正运动学和逆运动学的Matlab代码.但逆解部分在不同版本的Matlab中运行有错误. 本篇文章是MatlabR2016a下完成的,并说明一下原代码错误的原 ...
最新文章
- js获取html代码中所有图片地址
- php多文件上传类源码,PHP单文件上传类或多文件上传类源码
- 事件冒泡和捕获的执行顺序
- 496. 下一个更大元素 I
- 计算机面试数据库基本知识,面试基础知识集合(python、计算机网络、操作系统、数据结构、数据库等杂记)...
- Ubuntu switch window switch terminal tab
- java中vi是什么意思_java中的public void是什么意思？
- SQL批量提交修改业务
- 有关数据库事务的一些理解－原生的Java的JDBC事务
- “UNIX的名字是我起的”——对话UNIX开发者Brian W. Kernighan
- 关于回复某个评论的功能小改进
- Mathtype 免安装 | office自带UnicodeMath和LaTeX编辑功能
- 【笔记】时间片轮转 RR 进程调度算法（Java 实现）
- 个人博客系统(附源码)
- hbase java客户端_【HBase教程】（八）HBase JAVA 客户端API使用
- matlab内建函数怎么不同颜色,matlab分段函数不同颜色绘图
- ROS安装教程（ubuntu16.04安装ROS Kinetic）
- 一只公鸡5块钱，一只母鸡3块钱，3只小鸡一块钱，一个农夫用100块钱买100只鸡（不许解方程），怎么实现？编写java程序。...
- 青龙羊毛——去闲转（搬砖）
- 创建vue-ssr项目
热门文章
- 有人相爱，有人夜里开车看海，有人leetcode第一题都做不出来。
- 融入动画技术的音乐可视化交互技术——Fun Beats
- 第四讲图：最短路径两种经典算法学习总结2
- 快手推出春茶节活动，短视频直播平台成茶产业带展示窗口
- 互斥锁机制，互斥锁与读写锁区别
- AJAX+PHP+MySQL搜索(亲测可用附源码)
- 【九州仙侠传H5魂骨版】linux手工外网端+架设视频教程+开服清档+运营后台+授权GM后台+H5三端
- [算法前沿]--002-ChatGPT对安全的影响和开源的LLM大模型资源汇总
- RocketMQ消息ACK机制
- The browser supports WebGL, but initialization failed