PUAM560机械臂逆运动学实例-梳理

经过努力，对课本中PUAM560机械臂的运动学逆解算的实例进行了梳理，啥事都得动手试试才能学的深刻，整理一下我的理解和思路。由于首次接触逆解对于许多概念理解很不到位，不过随着学习的深入我相信我会越来越接近这个“真理”的。

我会先通过思维导图先简述一遍解题主干，最后附上源代码，大家运行源代码可以看到详细的说明。

同样是用mathematica推导和梳理，由于mathematica使用的不是很熟练，所以有些化简手算比用代码计算简单，因此在程序中就没实现，另一方面也是锻炼自己的对相关算法的理解了。整个程序用到了如下函数具体用法可见源代码：

ClearAll[];

Print[];

MatrixForm[];

Simplify[];

Collect[];

Solve[];

Inverse[];

课本上实例使用的纯代数的方法，而没有使用Pieper的解法，作者期望以此让我们学习到更多的解法。还要说明的是整个求解过程中并没有出现具体的参数。因此在实际应用过程中还会有许多问题（主要针对我这个菜鸟），需要深刻的认识到这一点。下面贴出求解关节角的思路，这同时反应出了Pieper解法的优越性！

从思维导图的下面开始一步步往上走：

其中cita是指关节转角后面的数字代表关节的序号，这与源代码的表示是相同的

其中T01表示坐标系1向坐标系0的映射其他以此类推，这个方程贯穿解题的整个过程，后面根据需要解的角会做不同的变化，其实也就是合并和移项。

首先解cita1和cita3：

通过解这个方程cita1和cita3就这么出来了！其中有一步三角恒等变换，需要手动计算，这是高中知识我觉得自己多练练是有好处的。还有这两个角度都有两个解，可以从思维导图中看到。

其次是cita2：

Cita2的求解属于整个求解过程中最复杂的一步了，最后解出的还是cita2和cita3的和的形式，不仅如此cita2还手cita1的影响，这就导致了cita2有四个解，思维导图的第二层可以看到他和cita1和cita3的关系。

接着是cita4：

Cita4由这个方程得出，这个cita4有些特殊，他会引发‘奇异位形’具体情况已经在源代码中说明

最后cita5和cita6将一起介绍：

先求cita5

再求cita6，这两步，步骤中规中矩简单明了，都是单解，但是式子很复杂（主要是项太多了），如果带入实际参数的话会简化很多。

最后在思维导图的顶部出现了8个分枝，代表机械臂有8个解。Cita2的四个解导致机械臂整体有四个解，腕部关节的翻转直接使解的个数翻倍，关于这个腕部关节的反转我在下面的视频中表达了我自己的理解。但是在实际应用中会通过各种条件限制选出最优的一个解

IMG_1104

下面贴上源代码大家自己粘贴到mathematical里即可看到详细说明：

ClearAll["Global`*"]
Print ["PUMA560运动学逆解算-纯代数法"]
Print["通过腕关节的位姿可以确定的齐次矩阵："]
T06 = {{r11,r12,r13,Px},{r21,r22,r23,Py},{r31,r32,r33,Pz},{0,0,0,1}};
MatrixForm[T06] ;
Print["希望通过下面的方程解出所有的关节角"]
MatrixForm[T06]==T01.T12.T23.T34.T45.T56
Print["说明一下上式中各个矩阵的来历：等式左边的T06是直接由腕关节（坐标系6）的位姿得到的，可以参考前面章节的旋转矩阵和转角的关系，这个矩阵属于已知量（在逆解的前提下）"];
Print["而等式右侧的一串矩阵是由DH参数得到的，它们属于未知量，包含cita1到cita6，这里的cita是指关节转角"];
Print["先拿cita1开刀"];
Print["将含有cita1的部分移到等号左边"];
Print["Inverse[T01].T06 = T12.T23.T34.T45.T56;"];
T01 = {{Cos[cita1],-Sin[cita1],0,0},{Sin[cita1],Cos[cita1],0,0},{0,0,1,0},{0,0,0,1}};
Print["**T01为："];
MatrixForm[T01]
Print["**T01的逆为："];
T01inv =Simplify[Inverse[T01]];
MatrixForm[T01inv]
Print["等号左侧为："];
MatrixForm[ T01inv.T06]
Print["等号右侧为："];
Print["所有元素都是在坐标系1中表示的"];
Print["这里的矩阵是套用前面章节的公式，这个公式的重点是最后一列"];
T16 = {{r11,r12,r13,Px},{r21,r22,r23,Py},{r31,r32,r33,Pz},{0,0,0,1}};
MatrixForm[T16]
Print["其中： Px = a2Cos[cita2]+a3Cos[cita2+cita3]-d4Sin[cita2+cita3]"];
Print["       Py = d3"];
Print["       Pz = -a3Sin[cita2+cita3]-a2Sin[cita2]-d4Cos[cita2+cita3]"];
Print["可以看到Py是常数"];
Print["--**>>>于是得到式1："];
MatrixForm[ T01inv.T06]==MatrixForm[T16]
Print["注意左边齐次矩阵中的PxPyPz是坐标系1在坐标系0中的表示，右边的PxPyPz是坐标系6在坐标系1中的表示"];
Print["令两边元素（2，4）相等："];
Print["Py Cos[cita1]-Px Sin[cita1] = Py"];
Print["防止产生误解可以写成等式2："];
Print["Py Cos[cita1]-Px Sin[cita1] = d3"];
Print["使用三角恒定变换："];
Print["令"];
Print["Px = \[Rho]Cos[\[CurlyPhi]];Py = \[Rho]Sin[\[CurlyPhi]]"];
Print["带入原式求解，这里必须要会手动计算，比用计算机简洁"];
Print[">>>>>>>>>>>>>>>>>>>>>>>>cita1 已经求出来了:)*********注意！！cita1会有两个解"];
Print["等式1的价值还没有开发完,还可以用来求cita3"];
Print["令等式两边（1，4）和（3，4）相等"];
Print["Px Cos[cita1]+Py Sin[cita1] = Px"];
Print["Pz = Pz"];
Print["或者写成"];
Print["--**>>>等式3"];
Print["Px Cos[cita1]+Py Sin[cita1] = a2 Cos[cita2]+a3 Cos[cita2+cita3]-d4 Sin[cita2+cita3]"];
Print["--**>>>等式4"];
Print["Pz = -a3 Sin[cita2+cita3]-a2 Sin[cita2]-d4 Cos[cita2+cita3]"];
equ2r = d3;
equ3r = a2 Cos[cita2]+a3 TrigExpand[Cos[cita2+cita3]]-d4 TrigExpand[Sin[cita2+cita3]];
equ4r = a3 TrigExpand[Sin[cita2+cita3]]+a2 Sin[cita2]+d4 TrigExpand[Cos[cita2+cita3]];
equ2l = Py Cos[cita1]-Px Sin[cita1];
equ3l = Px Cos[cita1]+Py Sin[cita1];
equ4l = -Pz;
Print["将等式2，3，4分别两边平方并相加"];
equ5r = equ4r^2+equ3r^2+equ2r^2;
equ5l = equ4l^2+equ3l^2+equ2l^2;
Simplify[equ5r]
Simplify[equ5l]
Print["--**>>>于是得到等式5（equ5）："];
a2^2+a3^2+d3^2+d4^2+2 a2 a3 Cos[cita3]-2 a2 d4 Sin[cita3]==Px^2+Py^2+Pz^2
Print["化简成与课本相等的形式：（equ5）"];
Print["a3 Cos[cita3]-d4 Sin[cita3] = K"];
Print["其中"];
k==((Px^2+Py^2+Pz^2-a2^2-a3^2-d3^2-d4^2)/(2 a2))
Print["显然只有一个变量cita3在等式5的左侧"];
(*Simplify[Solve[a3 Cos[cita3]-d4 Sin[cita3] == K,cita3]]*)
Print["和等式2的解法相同：使用三角恒等变形即可"];
Print["---------------********************>>>>>>>>.cita3求出来了:)*******同样因为根号的原因cita3 也有两个解"];Print["到这里算是结束了一部分了，本部分等式序号排到---等式5"];
Print["经过上面的运算cita1和cita3已经是常量了，下一个受害者是cita2"];
Print["继续对变换方程进行变形:"];
Print["*****：inv[T03(cita2)].T06 = T34(cita4).T45(cita5).T56(cita6)"];
Print["其中T03(cita2)的cita1和cita3是常数，T03(cita2)的变量只有一个即cita2:"];
Print["T06:"];
MatrixForm[T06]
T01 = {{Cos[cita1],-Sin[cita1],0,0},{Sin[cita1],Cos[cita1],0,0},{0,0,1,0},{0,0,0,1}};
T13 = {{Cos[cita2+cita3],-Sin[cita2+cita3],0,a2 Cos[cita2]},{0,0,1,d3},{-Sin[cita2+cita3],-Cos[cita2+cita3],0,-a2 Sin[cita2]},{0,0,0,1}};
T03 = T01.T13;
Print["T03:"];
MatrixForm[T03]
Print["T03的逆:"];
invT03 = Simplify[Inverse[T03]];
MatrixForm[invT03]
equ6l = invT03.T06;
Print["得到等式6左侧:"];
MatrixForm[equ6l]
equ6r = {{Cos[cita4] Cos[cita5] Cos[cita6]-Sin[cita4] Sin[cita6],-Cos[cita4] Cos[cita5] Cos[cita6]-Sin[cita4] Sin[cita6],-Cos[cita4] Sin[cita5],a3},{Sin[cita5] Cos[cita6],-Sin[cita5] Sin[cita6],Cos[cita5],d4},{-Sin[cita4] Cos[cita5] Cos[cita6]-Cos[cita4] Sin[cita6],Sin[cita4] Cos[cita5] Sin[cita6]-Sin[cita4] Cos[cita6],Sin[cita4] Sin[cita5],0},{0,0,0,1}};
Print["得到等式6右侧:"];
MatrixForm[equ6r]
Print["--**>>>得到等式6:"];
Print["inv[T03(cita2)].T06 = T36"];
Print["令等式6两边（1，4）和（2，4）元素相等"];
Print["--**>>>得到等式7:"];
Print["-a2 Cos[cita3]+Px Cos[cita1] Cos[cita2+cita3]+Py Cos[cita2+cita3] Sin[cita1]-Pz Sin[cita2+cita3] = a3;"];
Print["--**>>>得到等式8:"];
Print["-Pz Cos[cita2+cita3]+a2 Sin[cita3]-Px Cos[cita1] Sin[cita2+cita3]-Py Sin[cita1] Sin[cita2+cita3] = d4;"];
equ7l = -a2 Cos[cita3]+Px Cos[cita1] Cos[cita2+cita3]+Py Cos[cita2+cita3] Sin[cita1]-Pz Sin[cita2+cita3];
equ7r = a3;
equ8l = -Pz Cos[cita2+cita3]+a2 Sin[cita3]-Px Cos[cita1] Sin[cita2+cita3]-Py Sin[cita1] Sin[cita2+cita3];
equ8r = d4;Print["本部分等式序号排到---等式8"];
Print["----********>开始化简等式7和8"];
Print["在等式8中表示出c23(Cos[cita2+cita3],这是一种简化书写方式) "];
midequ5 =Solve[-Pz Cos[cita2+cita3]+a2 Sin[cita3]-Px Cos[cita1] Sin[cita2+cita3]-Py Sin[cita1] Sin[cita2+cita3]==d4,Cos[cita2+cita3]]
Print["将c23带入等式7 "];
midequ6 = -a2 Cos[cita3]+Px Cos[cita1]((-d4+a2 Sin[cita3]-Px Cos[cita1] Sin[cita2+cita3]-Py Sin[cita1] Sin[cita2+cita3])/Pz)+Py ((-d4+a2 Sin[cita3]-Px Cos[cita1] Sin[cita2+cita3]-Py Sin[cita1] Sin[cita2+cita3])/Pz) Sin[cita1]-Pz Sin[cita2+cita3];
Print["化简等式7 "];
Print["得出S23的表达式"];
midequ7 = Simplify[midequ6];
midequ8= Solve[midequ7==a3,Sin[cita2+cita3]]
Print["s23 的分母 "];
Print["可以很明显的配出平方 "];
Print["----********>s23的分母已经被表示出来了"];
Print["对于s23 的分子 = "];
Print["自己稍微化简下跟课本对应即可"];
midequ9 = Collect[-a3 Pz-d4 Px Cos[cita1]-a2 Pz Cos[cita3]-d4 Py Sin[cita1]+a2 Px Cos[cita1] Sin[cita3]+a2 Py Sin[cita1] Sin[cita3],{Pz,d4,a2 Sin[cita3]}]
Print["-----******>c23 的分子已经被表示出来了 "];
Print["因此： "];
s23 == (Pz (-a3-a2 Cos[cita3])+d4 (-Px Cos[cita1]-Py Sin[cita1])+a2 (Px Cos[cita1]+Py Sin[cita1]) Sin[cita3])/(Pz^2+(Px Cos[cita1]+Py Sin[cita1])^2)
Print["在等式7中表示出s23 "];
midequ9 =Solve[-a2 Cos[cita3]+Px Cos[cita1] Cos[cita2+cita3]+Py Cos[cita2+cita3] Sin[cita1]-Pz Sin[cita2+cita3]==a3,Sin[cita2+cita3]]
Print["将s23带入等式8左侧 "];
midequ10 =  -Pz Cos[cita2+cita3]+a2 Sin[cita3]-Px Cos[cita1] ((-a3-a2 Cos[cita3]+Px Cos[cita1] Cos[cita2+cita3]+Py Cos[cita2+cita3] Sin[cita1])/Pz)-Py Sin[cita1] ((-a3-a2 Cos[cita3]+Px Cos[cita1] Cos[cita2+cita3]+Py Cos[cita2+cita3] Sin[cita1])/Pz);
Print["得出c23的表达式 "];
midequ11 = Simplify[midequ10];
midequ12= Solve[midequ11==d4,Cos[cita2+cita3]]
Print["c23 的分母"];
Print["可以很明显的配出平方 "];
Print["c23 的分子 = "];
Print["自己稍微化简下跟课本对应即可"];
midequ9 = Collect[-d4 Pz+a3 Px Cos[cita1]+a2 Px Cos[cita1] Cos[cita3]+a3 Py Sin[cita1]+a2 Py Cos[cita3] Sin[cita1]+a2 Pz Sin[cita3],{Pz,a3,a2 Cos[cita3]}]
Print["因此："];
c23 == (a3 (Px Cos[cita1]+Py Sin[cita1])+a2 Cos[cita3] (Px Cos[cita1]+Py Sin[cita1])+Pz (-d4+a2 Sin[cita3]))/(Pz^2+(Px Cos[cita1]+Py Sin[cita1])^2)
Print["把上面的计算做一下总结： "];
Print["------------------------------------------------------------------------**> "];
Print["全都化简成和课本相同的结果（很显然）: "];
Print["S23 ="];
((-a3-a2 c3) Pz+(c1 Px+s1 Py)(a2 s3-d4))/(Pz^2+(c1 Px+s1 Py)^2 )
Print["c23 = "];
((a2 s3-d4) Pz-(a3+a2 c3)(c1 Px+s1 Py))/(Pz^2+(c1 Px+s1 Py)^2)
Print["这里为了方便使用了和课本一样的符号 "];
Print["求解很简单 "];
Print["其中c1,c3由于前面的计算这里都是常数，所以可以用反正切求出cita2和cita3的和 "];
Print["注意虽然cita1和cita3都在前面解出，但是不要忘了他们都有两个解，那么取决于cita1和cita3的cita2就会有4个解！！！！！！ "];
Print["记住它解的个数，后面还会用到 "];
Print["-------***************>>>>cita2求解完毕:)"];
Print["工作还在继续。。。。。。。。。。。。。。。。。"];
Print["下一位嘉宾是cita4！"];
Print["回到等式6，令等式两边（1，3）和（3，3）对应相等"];
Print["得到"];
r13 Cos[cita1] Cos[cita2+cita3]+r23 Cos[cita2+cita3] Sin[cita1]-r33 Sin[cita2+cita3] == -Cos[cita4] Sin[cita5]
r23 Cos[cita1]-r13 Sin[cita1] == Sin[cita4] Sin[cita5]
Print["注意到两个等式只有右边有个额外的变量cita5，但是不要紧，两式等号两边相除可以直接消去Sin[cita5]"];
Print["这一步要注意Sin[cita5]不能为零"];
Print["然后反正切函数直接求出cita4"];
Print["有两个情况需要讨论"];
Print["情况 1 ：cita5 = 0"];
Print["此时关节转轴6和4共线，作用到末端的转角是由cita4和cita6的和或者差造成的，课本上称为：奇异位形"];
Print["此时的cita4不论为何值都能通过cita6弥补，所以这种情况下的cita4任意选取"];
Print["情况 2 ：cita5 ！= 0"];
Print["套用反正切公式即可"];
Print["-------***************>>>>cita4求解完毕:)"];
Print["cita1，cita2,cita3,cita4都已经求出"];
Print["最后的cita5和cita6！！！！"];
Print["继续对变换方程做变形求等式9"];
T46 = {{Cos[cita5] Cos[cita6],-Cos[cita5] Sin[cita6],-Sin[cita5] ,0},{Sin[cita6],Cos[cita6],0,0},{Sin[cita5] Cos[cita6],-Sin[cita5] Sin[cita6],Cos[cita5],0},{0,0,0,1}};
Print["等式9右侧"];
MatrixForm[T46]
T34={{Cos[cita4],-Sin[cita4],0,a3},{0,0,1,d4},{-Sin[cita4],-Cos[cita4],0,0},{0,0,0,1}};
T04 = T03.T34;
MatrixForm[T04];
invT04 = Simplify[Inverse[T04]];
MatrixForm[invT04];
equ12l = invT04.T06;
Print["等式9左侧"];
MatrixForm[equ12l]
Print["--**>>>综上等式9："];
Print["inv[T04]T06 = T45.T56"];
Print["由等式9可以看到未知数cita5和cita6都在等号右侧"];
Print["令等号两边的元素（1，3）和（3，3）对应相等使用反正切函数可以轻易得到cita5的值"];
Print["再调整变换方程为"];
Print["inv[T05].T06 = T56"];
Print["使其两边元素（3，1）和（1，1）相等，同样使用反正切函数可得cita6"];
Print["求解cita5和cita6的步骤并不复杂，但是表达式很复杂，如果带入实际DH参数会简化很多"];
Print["至此已将将六自由度机械臂的六个关节角求出"];
Print["但是！！！！！！！"];
Print["还记得cita2 的求解过程吗？它有四个解也就是说整个机械臂的运动学逆解算有四个解？不！"];
Print["加上腕关节的反转，整个机械臂将会有8个解"];
Print["实际会根据关节限位，距离远近等因素舍去某些解，选取一个最优解"];
Print["这样才算完整的走一遍运动学逆解算！！！！"];

PUAM560机械臂逆运动学实例-梳理相关推荐

数值法求六轴机械臂逆运动学解
问题描述:已知工具坐标系相对于固定坐标系的期望位置和姿态,如何求解满足期望位姿的关节角? 本篇将通过基于雅克比矩阵的牛顿-拉夫森迭代法求运动学逆解. 牛顿 - 拉夫森法(Newton - Raphso ...
UR机械臂逆运动学解析解
UR机械臂逆运动学解析解 ur10机械臂的DH(修正)参数为 d1=0.1273;a2=-0.612;a3=-0.5723;d4=0.163941;d5=0.1157;d6=0.6922; 首先,推导 ...
柔性matlab机械臂运动空间代码,中正平和的机器人学笔记——2. 机械臂逆运动学（附MATAB代码）...
0. 预备知识(别跳过呀) 上一篇我们讲了正运动学,也就是通过齐次变换矩阵,知晓关节角后求得机械臂末端的位姿,并表示在基座标系下.这一篇我们就来讲一讲逆运动学问题--已知工具坐标系{T}相对于固定坐标 ...
【现代机器人学】基于指数积的机械臂逆运动学
0 引言基于指数积的正运动学现代机器人学名词概念有了以上的基础,我们现在利用指数积来对机器人的逆运动学进行求解,有一点需要注意,需要先对机器人进行指数积的正运动学建模,然后才能利 ...
关于机械臂逆运动学 jacobian矩阵
机器人逆运动学机器人的逆解问题比较复杂,为了说明问题,下面先以2自由度的机器人为例. 如图所示,已知机器人末端的坐标值(x,y) ,试利用x,y 表示根据图中的几何关系可知: 联立求 ...
【机器人1】基于POE公式的UR5机械臂正运动学建模求解与matlab仿真
基于PoE公式的UR5机械臂正运动学建模求解与matlab仿真基于PoE公式的UR5机械臂正运动学分析 1.1 运动旋量的螺旋释义(理论基础) 1.2 基于PoE公式的UR5机械臂正运动学模型 1. ...
Udacity机器人软件工程师课程笔记（十六）-机械臂仿真控制实例（其一）-Gazebo、RViz和Moveit！
机械臂仿真控制实例目录环境设置项目工具介绍 Gazebo (1)Gazebo组件 (2)Gazebo界面统一机器人描述格式(URDF) RViz Moveit! 1.环境设置对于此项目,使用 ...
Udacity机器人软件工程师课程笔记（十八）-机械臂仿真控制实例（其三）-KR210机械臂反向运动学
机械臂仿真控制实例(其二)-KR210正向运动学目录反向运动学概述为Kuka KR210创建IK解算器 1.反向运动学概述 KR210的最后三个关节是满足三个相邻的关节轴线在单点处相交的旋转关节 ...
中正平和的机器人学笔记——1. 机械臂正运动学（附MATLAB代码）
中正平和的机器人学笔记--1.正运动学 0. 何为运动学?何为正运动学? 运动学主要研究的就是机械臂的运动特性(位置.速度.加速度等),但是不考虑使机械臂产生运动时施加的力和力矩(这部分是动力学).而 ...

PUAM560机械臂逆运动学实例-梳理

PUAM560机械臂逆运动学实例-梳理相关推荐

最新文章

热门文章