6轴机械臂（拟人臂+球形腕（三轴相交于一点））正逆运动学求解
1、DH坐标下机械臂参数
theta=[pi/10,pi/2,pi/4,-pi/4,pi/4,-pi/8];%关节角度
a=[0, 0.260, 0.025, 0, 0, 0];%连杆长度
d=[0, 0, 0, 0.280, 0, 0.072];%偏移距离alpha=[pi/2, 0, pi/2, -pi/2 , pi/2, 0];%RBR型6轴机械臂使用MATLAB的机器人工具箱函数可以画出该机械臂在指定关节角下的姿态，并且给出末端位置和姿态角，代码如下：
L1 = Link('d', 0.340, 'a', 0, 'alpha', pi/2 ,'standard' );
L2 = Link('d', 0, 'a', 0.260, 'alpha', 0 ,'standard' );
L3 = Link('d', 0 , 'a', 0.025, 'alpha', pi/2 ,'standard' );
L4 = Link('d', 0.280, 'a', 0, 'alpha', -pi/2 ,'standard' );
L5 = Link('d', 0, 'a', 0, 'alpha', pi/2 ,'standard');
L6 = Link('d', 0.072, 'a', 0, 'alpha', 0 ,'standard');theta=[-pi/2,pi*4/3,pi/4,pi/4,pi/4,-pi/4];%关节角度
STA_MZ204=SerialLink([L1,L2,L3,L4,L5,L6],'name','MZ204-STA'); %SerialLink 类函数
A = fkine(STA_MZ204,theta);//正运动学求解
figure(1),STA_MZ204.teach(theta,'eul');%eul为以欧拉角显示方向效果如下：
二、正运动学求解（MATLAB代码实现）
1、DH坐标系下的T矩阵计算
T=[cos(theta),-sin(theta)*cos(alpha),sin(theta)*sin(alpha),a*cos(theta);sin(theta),cos(theta)*cos(alpha),-cos(theta)*sin(alpha),a*sin(theta);0,sin(alpha),cos(alpha),d;0,0,0,1];上面是一个轴的变换矩阵，那么6轴机械臂的变换矩阵T06=T1*T2*T3*T4*T5*T6
2、根据T06矩阵求解末端位置和方向角，方向角为欧拉角（绕ZYZ轴旋转）和方向角（绕XYZ轴旋转）的求解方法不同
末端位置:px=T06(1,4),py=T06(2,4),pz=T06(3,4)
绕ZYZ轴旋转求解：
r11 = T(1,1);r21 = T(2,1);r31 = T(3,1);r12 = T(1,2);r22 = T(2,2);r32 = T(3,2);r13 = T(1,3);r23 = T(2,3);r33 = T(3,3);px = T(1,4);py = T(2,4);pz = T(3,4);beta_beta = atan2(sqrt(r31*r31+r32*r32),r33) ;%绕Ybeta_alpha = atan2(r23/sin(beta_beta),r13/sin(beta_beta));%先绕Zbeta_yeta = atan2(r32/sin(beta_beta),-r31/sin(beta_beta));%再绕zpos=[px,py,pz,beta_alpha,beta_beta,beta_yeta];绕XYZ轴旋转求解：
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);beta_y = atan2(nz,sqrt(nx*nx+ny*ny)) ;beta_z = atan2(ny/cos(beta_y),nx/cos(beta_y));beta_x = atan2(oz/cos(beta_y),az/cos(beta_y));pos=[px,py,pz,beta_x,beta_y,beta_z];三、逆运动学求解
逆运动学求解是由给定的末端姿态（位置+方向）求出各个关节的角度，下面将详细叙述求解步骤
1、以ZYZ轴为例，根据末端姿态pos（px,py,pz,theta_a,theta_b,theta_y）求解腕部位置（pwx,pwy,pwz），即三轴交点的位置。
这里要注意的一点是我令d1为0，如果你的机械臂d1不为0的话，你可以将d1等效为0求解，只需要将pz减去d1之后计算即可
pwx=px-cos(theta_a)*sin(theta_b)*d6;pwy=py-sin(theta_a)*sin(theta_b)*d6;pwz=pz-cos(theta_b)*d6;
2、根据腕部位置可以求解出前三个关节的角度，因为腕部的位置是由前三个轴决定的，与后三个轴无关，腕部位置等于第三个轴的末端位置，（令a3=d4，而d4=0）具体可以参考“机器人学建模规划与控制”这本书,不过这种方法只适用于机械臂的a3=0。当a3不为0时，我还是建议使用数学推导，根据具体情况来求解
上面的说法让你有些糊涂的话，我们可以直接用数学推导来说明，当你令d6=0时，机械臂的末端位置便是腕部位置，计算出d6=0时的px,py,pz如下：
px =d4*(cos(theta1)*cos(theta2)*sin(theta3) + cos(theta1)*cos(theta3)*sin(theta2)) + a2*cos(theta1)*cos(theta2) + a3*cos(theta1)*cos(theta2)*cos(theta3) - a3*cos(theta1)*sin(theta2)*sin(theta3)py =d4*(cos(theta2)*sin(theta1)*sin(theta3) + cos(theta3)*sin(theta1)*sin(theta2)) + a2*cos(theta2)*sin(theta1) + a3*cos(theta2)*cos(theta3)*sin(theta1) - a3*sin(theta1)*sin(theta2)*sin(theta3)pz =a2*sin(theta2) - d4*(cos(theta2)*cos(theta3) - sin(theta2)*sin(theta3)) + a3*cos(theta2)*sin(theta3) + a3*cos(theta3)*sin(theta2)从上面的px,py,pz计算结果我们可以发现，他只与theta1,theta2,theta3相关
我们对其进行化简令cos(theta2)=c2,cos(theta2+theta3)=c23得
pwx=px=d4c1s23+c1(a2c2+a3c23)
pwy=py=d4s1s23+s1(a2c2+a3c23)
pwz=pz=a2s2+a3s23-d4c23从上面可以推出前三个关节的角度theta3,theta2,theta1,主要是一些三角函数推导计算，就不详细说明了，直接上程序
pos2(1)为pwx,pos2(2)为pwy,pos2(3)为pwz
%关节角3求解
m3 = (pos2(1)^2+pos2(2)^2+pos2(3)^2-a(2)^2-a(3)^2-d(4)^2)/(2*a(2))
n3 = sqrt(d(4)^2+a(3)^2)
theta3(1,1) = atan2(m3/n3,sqrt(1-m3^2/n3^2))-atan2(a(3),d(4));
theta3(1,2) = atan2(m3/n3,-sqrt(1-m3^2/n3^2))-atan2(a(3),d(4));%关节角2求解
m2(1,1:2) = a(2)+a(3)*cos(theta3)+d(4)*sin(theta3)%m2(1)--theta3(1),m2(2)--theta3(2)
n2(1,1:2) = -a(3)*sin(theta3)+d(4)*cos(theta3)%n2(1)--theta3(1),n2(2)--theta3(2)theta2(1,1:2)=atan2(n2,m2)+atan2(pos2(3),sqrt(n2.*n2+m2.*m2-pos2(3)*pos2(3)));%theta2(1,1)--theta3(1,1),theta2(1,2)--theta3(1,2)
theta2(1,3:4)=atan2(n2,m2)+atan2(pos2(3),-sqrt(n2.*n2+m2.*m2-pos2(3)*pos2(3)));%theta2(1,3)--theta3(1,1),theta2(1,4)--theta3(1,2)
test2=sqrt(n2.*n2+m2.*m2-pos2(3)*pos2(3))
test3=atan2(pos2(3),-sqrt(n2.*n2+m2.*m2-pos2(3)*pos2(3)))
%关节角1求解
theta1(1,1)=atan2(pos2(2),pos2(1));if(pos2(2)>=0)theta1(1,2)=atan2(pos2(2),pos2(1))-pi;
elsetheta1(1,2)=atan2(pos2(2),pos2(1))+pi;
end3、根据计算出来的theta1,theta2,theta3,我们可以计算出R03，同时根据位姿我们可以计算出R06，那么就可以求解出R36，
R03是T03=T01*T12*T23的一部分（前三行，三列，表示方向变换部分），同理R06也是如此。
下面给出R03的计算代码
R03(1,1)=cos(theta1).*cos(theta2).*cos(theta3) - cos(theta1).*sin(theta2).*sin(theta3);
R03(2,1)=cos(theta2).*cos(theta3).*sin(theta1) - sin(theta1).*sin(theta2).*sin(theta3);
R03(3,1)=cos(theta2).*sin(theta3) + cos(theta3).*sin(theta2);R03(1,2)= sin(theta1);
R03(2,2)=-cos(theta1);
R03(3,2)=0;R03(1,3)=cos(theta1).*cos(theta2).*sin(theta3) + cos(theta1).*cos(theta3).*sin(theta2);
R03(2,3)=cos(theta2).*sin(theta1).*sin(theta3) + cos(theta3).*sin(theta1).*sin(theta2);
R03(3,3)=sin(theta2).*sin(theta3) - cos(theta2).*cos(theta3);根据末端姿态pos计算R06的代码如下
R06(1,1)=cos(pos(6))*cos(pos(5))*cos(pos(4))-sin(pos(4))*sin(pos(6));R06(2,1)=cos(pos(6))*cos(pos(5))*sin(pos(4))+cos(pos(4))*sin(pos(6));R06(3,1)=-sin(pos(5))*cos(pos(6));R06(1,2)=-sin(pos(6))*cos(pos(5))*cos(pos(4))-cos(pos(6))*sin(pos(4));R06(2,2)=-sin(pos(6))*cos(pos(5))*sin(pos(4))+cos(pos(6))*cos(pos(4));R06(3,2)=sin(pos(5))*sin(pos(6));R06(1,3)=cos(pos(4))*sin(pos(5));R06(2,3)=sin(pos(4))*sin(pos(5));R06(3,3)=cos(pos(5));因为R06=R03*R36;则R36=INV(R03)*R06,在MATLAB里可以用R36=R03/R06来计算。
接下来讲如何根据R36求解theta4,theta5,theta6
首先我们看一下R36的数学表达式
R36(1,1)=cos(theta4)*cos(theta5)*cos(theta6) - sin(theta4)*sin(theta6);R36(2,1)=cos(theta4)*sin(theta6) + cos(theta5)*cos(theta6)*sin(theta4);R36(3,1)= -cos(theta6)*sin(theta5);R36(1,2)= - cos(theta6)*sin(theta4) - cos(theta4)*cos(theta5)*sin(theta6);R36(2,2)= cos(theta4)*cos(theta6) - cos(theta5)*sin(theta4)*sin(theta6);R36(3,2)= sin(theta5)*sin(theta6);R36(1,3)=cos(theta4)*sin(theta5);R36(2,3)=sin(theta4)*sin(theta5);R36(3,3)=cos(theta5);根据R36的表达式我们便可以求出theta4,theta5,theta6
具体代码如下：
%theta5(0,pi),theta4(1,i)=atan2(R36(2,3),R36(1,3));theta5(1,i)=atan2(sqrt(R36(2,3)^2+R36(1,3)^2),R36(3,3));theta6(1,i)=atan2(R36(3,2),-R36(3,1));%theta5(-pi,0),theta4(2,i)=atan2(-R36(2,3),-R36(1,3));theta5(2,i)=atan2(-sqrt(R36(2,3)^2+R36(1,3)^2),R36(3,3));theta6(2,i)=atan2(-R36(3,2),R36(3,1));4、注意theta1,theta2,theta3的解一共有四组，分别是
//theta1(1,1)，theta2(1,1)，theta3(1,1)
//theta1(1,1)，theta2(1,2)，theta3(1,2)
//theta1(1,2)，theta2(1,3)，theta3(1,1)
//theta1(1,2)，theta2(1,4)，theta3(1,2)
每一组解对应的theta4,theta5,theta6有两组解
所以逆运动学的解一共有8组
6轴机械臂（拟人臂+球形腕（三轴相交于一点））正逆运动学求解相关推荐
- 修正逆解文章——六轴UR机械臂正逆运动学求解_MATLAB代码（标准DH参数表）
如下参考链接1的作者大大实现了UR5机械臂的正运动学和逆运动学的Matlab代码.但逆解部分在不同版本的Matlab中运行有错误. 本篇文章是MatlabR2016a下完成的,并说明一下原代码错误的原 ...
- UR机械臂正逆运动学求解
最近有个任务:求解UR机械臂正逆运动学,在网上参考了一下大家的求解办法,众说纷纭,其中有些朋友求解过程非常常规,但是最后求解的8组解,只有4组可用.在这里我介绍一个可以求解8组解析解的方法,供大家参考 ...
- matlab欧拉迭代,matlab机械臂正逆运动学求解问题，使用牛顿-欧拉迭代算法
代码复制的有问题,详细见本楼,谢谢. clc;clear; DR=pi/180; %time j = 1; for i = 0 : 0.1 : 2 %input theta= 45 * DR *(1+ ...
- UR5构型机械臂正逆运动学
前言 整理之前的一个项目,当时看着一个博客硬生生计算了差不多一个星期.尝试用MatLab符号推导工具箱化简一部分工作.我使用的大象机器人一款开源入门级协作机器人产品myCobot,开发文档十分完善,但 ...
- 五自由度机械臂正逆运动学算法（Ｃ语言+Matlab）
五自由度机械臂建模 学习代码都记录在个人github上,欢迎关注~ Matlab机器人工具箱版本9.10 机械臂还是原来的机械臂,之前用ROS做底层驱动,不需要写正逆运动学和相关算法就能得到一些简单的 ...
- 5-DOF机械臂课程设计（一）之正逆运动学
文章目录 基本要求 基本概念 关节坐标系的建立 正运动学求解 DH参数表的建立 逆运动学求解 多解下解的选取 源码下载 后续 基本要求 开发上位机程序,要求有良好的界面,能提供关节空间下和笛卡尔坐 ...
- ROS机械臂正逆运动学
这里做一个六轴机械臂用于正逆运动学实验. 这里其实一共只有3轴,只有3轴位置没有姿态.所以urdf文件里我在末端做了3个虚拟关节,以便将kdl的frame能够填满,使得齐次坐标变换是规则的. 1.ur ...
- Matlab机器人工具箱（3-1）：五自由度机械臂（正逆运动学）
01 正运动学:DH表示法 1955年, Denavit和Hartenberg在"ASME Journal of Applied Mechanic"发表了一篇论文,这篇论文介绍了一 ...
- Matlab机械臂综合仿真平台，包含运动学、动力学和控制。 MATLAB机器人仿真正逆运动学simulink轨迹规划 机械臂动力学控制等
Matlab机械臂综合仿真平台,包含运动学.动力学和控制. MATLAB机器人仿真正逆运动学simulink轨迹规划 机械臂动力学控制等 gui控制仿真平台PUMA机器人 robotics toolb ...
最新文章
- LIGA Stereo:基于双目3D检测的Lidar几何感知表示学习（ICCV2021）
- libevent 安装异常
- PHP7天前的时间戳
- 鼠标键盘唤醒计算机,除了按下电源按钮唤醒计算机，WIN10也可以使用鼠标或键盘来唤醒...
- HashMap的7种遍历方式
- 程序员基本功04JAVA的内存回收
- 带有Guice的富域模型
- 《Python编程从入门到实践》记录之Python函数定义、使用
- jQuery获取url地址
- Tensor:逐元素操作
- getComputedStyle
- BLM战略规划的核心是业务模式创新
- 英特尔超级计算机显卡,美国公布首台百亿亿次超级计算机！用上Intel Xe独立显卡...
- 在人工智能领域创业，需要明确可知的5个基本条件
- 垃圾分类之上传一张图片进行测试
- CSS line-height概念与举例
- python学习（二）变量和运算符
- 【1.9w字】彻底搞懂HTTP知识的面试题，建议精读收藏
- 什么是物联网？物联网如何工作？
- openresty+kong+konga+postgresql