基于matlab的四轴机械臂运动学逆运算

大家好！这是我第一次写客博，肯定会有表达不清楚和有些东西没讲到，在这就提前跟大家说下，麻烦大家多多见谅！学生党的一次学习记录。也是自己在网上找到关于四轴机器臂的教程较少，希望能给像我一样在解四轴机器臂时遇到问题的人一点点帮助。

关于四轴机械臂轴的角度求解主要有两种，一种代数法（需要建立D-H表），一种几何法（画图）。

这里我选择代数法和几何法来对我的四轴机械臂进行计算，主要代数法在后续可以更方便地继承下去，几何法用来提高求解速度。

一、前期准备

1.机器人运动学基础学完，我这里推荐去b站看林沛群老师的机器人学。基本上学完运动学部分就可以做机器人的基本操作了。

2.matlab机器人工具箱，后面我主要用matlab来对机械臂进行仿真和求解（matlab yyds！！！）

3.机械臂的具体参数（D-H table）。我是先在solidworks上把机械臂的模型建立出来后，直接在 

SW上找出机械臂的D-H table。（左边是机械臂相关参数，右边是我每个轴的坐标系建立）

D-H Table
i	αi-1	ai-1	di	θi
1	0	0	79.05	（0°）
2	-90°	0	0	（0°）
3	0	76.89	0	（-135°）
4	0	74.15	0	（135°）
5	0	113	-14.09	（0°）

我的第0轴在世界坐标系上，所有第1轴相对于第0轴有个位移，个人觉得还是有必要的。然后我的第5轴其实就是物件相对于机械臂第4轴的附加轴，我是希望物件的x、z轴方向与世界坐标系的方向相同。每个轴都是在舵机转动的中心位置，手画的不要太在意。

如果D-H table不太会的话，可以跟着林沛群老师上课的ppt里面的图来。α和θ的正负号就是右手定则。（如图下图）

二、求解方法

1、多重解

对于机器臂末端确定为某个点时，机械臂可能存在多种位置情况，在我们运用代数求解就会发现，对某个角度求解的公式有开根号的情况，这样在一个角度就会有两个解，而还没求解的角度又基于以求的角度来求解，所以就会产生多个解的情况。所以在求解的过程中，我加入了几何法求解θ1，从而减少了复杂度（这种方法的解与我的机械结构有关）

2、求解方法

像上面所说在我的机械臂逆运动学求解中我使用了代数法和几何法。我认为我这样既方便了后续改进和提高了运算的速度。

首先我们先来了解下代数法对前三轴的求解方法，下面这个公式是整个计算的核心，也基本包含了我们整个计算需要的内容。

其中最左边的4×1矩阵为T04(第4轴相对于第0轴的转换矩阵）的ORG（位移矩阵)。

（1）这样T04就是我们第一个需要算出来的内容，从机械臂的坐标系统图中可以知道T04可以通过T45（目标物件相对于第4轴的转换矩阵）、TW5(目标物件相对于世界坐标系的转换矩阵）和TW0(第0轴相对于世界坐标系的转换矩阵）来求解。

这里可以用matlab来计算，节省时间。

syms x y z;
Two = [1,0,0,0;
       0,1,0,0;
       0,0,1,79.05;
       0,0,0,1];
T45 = [1,0,0,113;
       0,0,-1,-14.09;
       0,1,0,0;
       0,0,0,1];
Tw5 = [1,0,0,x;
       0,1,0,y;
       0,0,1,z;
       0,0,0,1];
T04 = inv(Two)*Tw5*inv(T45)

其中[x-113;y;z-4657/50;1]就是我的T04里的ORG（位移矩阵）。

（2）下面我们来求出我们等号左边的

在机器人运动学中，改进版的D-H table中所有的

所以我们直接把我们D-H table里面 i 的值带进这个公式就可以直接求出第i轴相对第i-1轴的转换矩阵。（注意，我在这里就犯了错误，其实在新版的D-H table中，第i轴的α和a都已经是i-1了，所以可以 把第 i 行的数据直接代进就行。）同样为了方便计算，我们还是用matlab。

syms theta1 theta2 theta3 theta4;
alpha=[0,-90,0,0];%α列的所有值，下面同理
a=[0,0,76.90,74.15];
d=[0,0,0,0];
T1 = mat(theta1,alpha(1),a(1),d(1));
T2 = mat(theta2,alpha(2),a(2),d(2));
T3 = mat(theta3,alpha(3),a(3),d(3));
T4 = mat(theta4,alpha(4),a(4),d(4));
disp(T1);
function T=mat(ti,ali_1,ai_1,di) %输入thetai alphai-1 ai-1 di，返回转换矩阵
T=[cosd(ti),-sind(ti),0,ai_1;
   sind(ti)*cosd(ali_1),cosd(ti)*cosd(ali_1),-sind(ali_1),-sind(ali_1)*di;
   sind(ti)*sind(ali_1),cosd(ti)*sind(ali_1),cosd(ali_1),cosd(ali_1)*di;
   0,0,0,1];
end

>> scdn
[cos((pi*theta1)/180), -sin((pi*theta1)/180), 0, 0]
[sin((pi*theta1)/180),  cos((pi*theta1)/180), 0, 0]
[                   0,                     0, 1, 0]
[                   0,                     0, 0, 1]

返回结果是这样的，由于在函数里我用的是cosd、sind它们返回的是角度值，所以(pi*theta1)/180可以直接看成theta1。这样我们T01、T12、T23、T34都全部求解出来了，P34_ORG就是T34的位移矩阵，与上面同理。

（3）我们将T23*P23_ORG的结果可以看成一个关于θ3的函数f(θ)，并且我们要把每个f(θ)写出来。

可以按照公式进行求解，由于我上面已经把T23、T34的具体值算出来的，所以我还是选择用matlab来算，因为matlab太方便啦！

T4org = [T4(1,4);T4(2,4);T4(3,4);T4(4,4)];
F = T3 *T4org

>>F =
 
(1483*cos((pi*theta3)/180))/20 + 769/10
         (1483*sin((pi*theta3)/180))/20
                                      0
                                      1

然后我们一部到位，剩下都是与上面同理的，到g(θ2,θ3)的函数与T01相乘结束。我们同样用公式也可以吧g(θ2,θ3)的表达式求出来就行。

G = simplify(T2*F) %simplify是用来对公式化简的，非常方便

>>G =
 
  (1483*cos((pi*(theta2 + theta3))/180))/20 + (769*cos((pi*theta2)/180))/10
                                                                          0
- (1483*sin((pi*(theta2 + theta3))/180))/20 - (769*sin((pi*theta2)/180))/10
                                                                          1
 

（4）然后我们可以回到开头吧T04的ORG结合到我们化简的最后一步

在我这个机械臂系统中，

r = F(1,1)^2+F(2,1)^2+F(3,1)^2;
r=simplify(f)
>>r =
 
(1140427*cos((pi*theta3)/180))/100 + 4564733/400

这样我们就求出了只包含了θ3未知数的方程组，所以

（x^2+y^2+z^2-11411.8325>11404.27），其实这样求出的θ3是有限制的，但是将就能用，如果以后我知道了其他的解法我再补充。我们知道了θ3后就可以代进g(θ)里面对θ2求解了。

（5）在上面我们可以知道，我们将θ3代入f(θ)，就能求出f1和f2的值。对于这种公式有个专门求解的公式。

我们把公式里面的值带入就可以得到θ2的结果了

（6）对θ1的求解我们用更方便的解法——几何法。将第5轴（物件）的x,y坐标投影到世界坐标系上，x，y值为第5轴（物件）相对于世界坐标系的投影值。那么θ1可以直接用反正切求解

（7）这样我们就吧θ1、θ2和θ3全部求解出来了。下面我们就需要利用上面三个角度来求解θ4。

在（1）中，我们用

求出了T04，再借用T04=T01*T12*T23*T34，等号左边已经求解出来了，而等号右边T01、T12、T23分别是带有未知数θ1、θ2、θ3的矩阵，我们把已经求出来的θ1、θ2、θ3带入到它们之中就可以求出T03，即T04 = T03*T34，而T34是θ4的矩阵。在（2）中我提到了转换矩阵的通用公式，当我们求出T34的具体值后，可以利用公式进行反推，这里我取了T34（1，1）对θ4反余弦求解，那么剩下的就交给matlab了！

theta1 = atand(y/x);
theta2 = 2*atand((f1+sqrt(f1^2+f2^2-z^2))/(f2-z));%只取了一种情况
theta3 = -acosd(((x.^2+y.^2+z.^2)-11411.8325)/11404.27);
T03 = simplify(T1*T2*T3);
T34 = inv(T03)*T04;%T34矩阵的数可以被全部求解出来
theta4 = acosd(T34(1,1)); %由T34的推导公式可知
disp(T34);

这里我只写了θ4的运算过程，在算的时候还要考虑θ2的多重解的情况。还有就是要考虑θ1的情况，每个人情况不一样，我这就先不说了。还有就是归0处理，θ2和θ3在matlab求解出来的值常常有小数点后很多位，比如（θ2 = -0.0045），这种情况在计算θ4时需要转换为0。

3 、角度选择

由于我们在前面求出了多种解，所以我们还需要确定哪种解可以用在我的机械臂上。（在这个点中，我只能按照自己在求解过程中的遇到的情况来讲解。）

我遇到的一个问题是，θ3与我实际机械臂（实物）转动角度差135°，θ2和θ4则与实际转动角度互为相反数。所以大家在求解出角度后，最好在matlab上验证下求解的角度与实际角度是否存在一个固定的偏差。

还有一个就是要考虑最后实物每个轴的转动角度是否与你在matlab仿真中的轴的角度旋转方向相同，我就出现了θ2、θ4转动角度刚好相反的情况。

最后还要对算出的解根据实际电机所能转动的角度给定阈值。判断电机是否能后到达该角度。

回到正题，在解出的两个解中要选择一个正确且轨迹最短的解，我这里选择的是判断求解出的两个θ2与当前机械臂所处位置的θ2的差值大小来选择哪一组解，因为在机器人运动学中，机器臂位置改变主要考前三轴的转动来决定的。在我实践中是没用发现问题的。

三、MATLAB模拟仿真

在matlab中对我们自己设计的机械臂进行仿真，需要用到matlab机器人工具箱，这个工具真的很强大，我也只学到一点点，够用。

工具箱的具体安装教程，大家就自己搜索，这里我就直接使用我们需要的知识。

L(1)=Link('revolute','d',79.05,'a',0,'alpha',0,'modified');
L(2)=Link('revolute','d',0,'a',0,'alpha',-pi/2,'offset',0,'modified');
L(3)=Link('revolute','d',0,'a',76.89,'alpha',0,'offset',(-135/180)*pi,'modified');
L(4)=Link('revolute','d',0,'a',74.15,'alpha',0,'offset',(135/180)*pi,'modified');
L(5)=Link('revolute','d',-14.09,'a',113,'alpha',pi/2,'offset',0,'modified');
Four_dof_mod = SerialLink(L,'name','4-dof');
Four_dof_mod.base=transl(0,0,0);
Four_dof_mod.teach; 

首先我们需要在matlab中建立每个连接点（Link）的参数，其实也就是把我们之前写的D-H table转换成matlab代码。其中L(1)-L(5)就是每个轴的名称，然后（‘revolute’）表示这个连接点为转动副（移动副为‘Prismatic’）。然后转动副的θ是不确定的，这里我们就需要为它设置一个初始值（‘offset’），比如你可以设定你机械臂电机初始状态存在的位置。然后其余的（‘d’），（‘a’），（'alpha'）就直接照抄就行。

不知道大家有没有注意到我的角度是用弧度制表示的，因为机器人工具箱本身的原因，我们在机器人工具箱里面的所有角度都需要用弧度制。

因为我的D-H table是改进型的，所以我们在建立连接点时，需要申明该连接点为（‘modified’）。

这里我机械臂为4轴，但建立了5个连接点，是因为这样可以把末端抓取的中心点包括进来，并且提高我们仿真的可视化，第5个点就是我们末端抓取物件的点。

Four_dof_mod = SerialLink(L,'name','4-dof')中，SerialLink(L,'name','4-dof')将5个连接点（Link）连接起来，组件了整个机器人的系统，并且把这个系统命名为‘4-dof’。然后就把这个系统的内容存储到Four_dof_mod中，便于我们后续对其进行操作。

Four_dof_mod.base=transl(0,0,0)   可以理解为设置机器人系统的世界坐标系的位置，也是设置机器人系统的底座位置。

Four_dof_mod.teach 机器人示教，生成一个GUI窗口，可以对我们创建的机器人进行操作控制。

当然，我们上面求解了那么久，不就是想验证和仿真到底对不对吗。

这里要用到xxx.plot()函数，将我们求解出来的角度按顺序放入plot中,示教的GUI窗口将自动移动到相应的角度。基本可以看到示教出来目标位置，与我们求解的目标位置相近（不用看左上角的X:Y:Z的值，它这里没用自动计算出最后的位置坐标）

想从示教上看到算出的位置坐标，就只能在左下角输入求解的角度，他就能显示数来了，可以看到求解出来的坐标与我们设置的坐标相近，证明我们求解的角度没问题。

四、结语 

这篇文字中，我只是分享下自己在算机器人逆运动学的一些心得和方法，该解法也有一定的局限性，我希望能给像我一样刚接触机器人运动学的新手提供一种求解思路，如果大家也有其他的好方法，希望能够一起交流学习下。

本来想把实物的运动效果也放上的，但这篇文章是放假回家写的，所以等以后再看实际效果图了。