Matlab/Simulink六自由度机器人运动学与控制系统仿真（二）【附源文件】

irb1600机器人逆运动学计算

本文章为系列文章，以IRB1600机器人为例，建立机器人正运动、逆运动学、控制系统模型，并在simulink中进行仿真，与理论计算结果进行对比验证
（一） irb1600机器人坐标系建立、正运动学计算与simulink验证
（二） 机器人逆运动学计算

（三） 机器人运动学控制系统仿真

---

---

一、机器人逆运动学计算常用方法

机器人逆运动学计算有迭代法和解析法两种

1.1迭代法

迭代法涉及微分运动学，本质与理论力学中的虚功原理类似，MATLAB中的机器人工具箱使用的就是迭代方法。具体原理可参考这篇知乎回答，写的很好~

MATLAB机器人工具箱中机器人逆解是如何求出来的？ - 知乎

也可以参考书籍《机器人学建模、规划与控制》97页中提到的基于雅克比转置矩阵的逆运动学算法，（这本书对机器人学的基础知识讲解的非常清楚，推荐~），英文版为：
Bruno S, Lorenzo S, Luigi V. Robotics:Modelling,Planning and Control[M]. Springer, 2009: 151-156
英文版下载地址：点我下载英文版。

算法框图如下，算法仅需要计算正运动学函数k(q)和J T(q)，相较于对逐点计算逆运动学计算，在做轨迹规划时该方法更具有应用价值。

其中xd表示期望的运动轨迹，xe表示实际运动的轨迹，e表示两者之间的跟踪误差，K为增益矩阵，通常为对角矩阵，k(·)代表正向运动学方程。

1.2解析法

串联机械臂有逆运动学解析解的充分条件是满足Pieper准则。即如果机器人满足两个充分条件中的一个，就会得到封闭解，这两个条件是：

• 三个相邻关节轴相交于一点；
• 三个相邻关节轴相互平行
  irb1600机器人满足第一个条件，因此能够得到封闭解，以下对irb1600机器人计算逆运动学封闭解过程进行说明。

二、用解析法求解机器人逆运动学

设末端位姿已知，为

根据正运动学求解过程，容易建立如下等式：

接下来详细说明每个关节角度的求解过程

2.1求解theta1

将上述等式变换为

令（3,3）（3,4）元素相等，得到

（为简化书写，cos(theta1)=c1,sin(theta1)=s1，依次类推，下同）

计算得到

2.2求解theta3

将等式变换为

令（1,3）（3,3）（1,4）（3,4）元素相等
（该部分解算过程较长，用到和角公式、万能公式等，如想了解详细求解过程，请点击下载求解过程报告）
计算得到

2.3求解theta2

同样使用

令（1,3）（1,4）元素相等，求解得到

2.4求解theta5

同样使用

令（3,3）元素相等，求解得到

2.5求解theta4

2.6求解theta6

同样使用

令（3,2）元素相等，求解得到

三、逆运动学计算代码

根据上述结果，编写了逆运动学代码

clear
clc
%oT为目标位姿
oT=[0.9395   -0.3422   -0.0144     882.7
    0.3335    0.9236   -0.1891     74.96
    0.0780    0.1728    0.9819     120.5
         0         0         0         1]
%rib1600机器人逆解运算

nx = oT(1, 1);ox = oT(1, 2);ax = oT(1, 3);px = oT(1, 4);
ny = oT(2, 1);oy = oT(2, 2);ay = oT(2, 3);py = oT(2, 4);
nz = oT(3, 1);oz = oT(3, 2);az = oT(3, 3);pz = oT(3, 4);

%%
% theta1 两个解
theta1_1 = atan((py-72.5*ay)/(px-72.5*ax));
theta1_2 = pi+atan((py-72.5*ay)/(px-72.5*ax));
%选解
if theta1_1>=-pi&&theta1_1<=pi %theta1的角度范围
   theta1=theta1_1;
else
   theta1=theta1_2;
end   
 
s1 = sin(theta1);
c1 = cos(theta1);

%%
% theta3 两个解
d1=72.5*(ax*c1+ay*s1)-(-135+px*c1+py*s1);
d2=72.5*az-(pz-481.5);
test=(d1^2+d2^2-475^2-600^2)/1200*475;
theta3_1 = asin((d1^2+d2^2-475^2-600^2)/(2*600*475));
theta3_2 = pi-theta3_1;
%选解
if theta3_1>=-pi&&theta3_1<=pi %theta3的角度范围
   theta3=theta3_1;
else
   theta3=theta3_2;
end   
 
s3 = sin(theta3);
c3 = cos(theta3);

%%
% theta2 两个解
alpha=asin(d1/sqrt(d1^2+d2^2));
theta2_1 = asin(-475*c3/sqrt(d1^2+d2^2))-alpha-theta3;
theta2_2 = pi-theta2_1;
%选解
if theta2_1>=0&&theta2_1<=pi %theta2的角度范围
   theta2=theta2_1;
else
   theta2=theta2_2;
end   
 
s2 = sin(theta2);
c2 = cos(theta2);

%%
% theta5 两个解
c23=cos(theta2+theta3);
s23=sin(theta2+theta3);
theta5_1 = acos(az*c23-s23*(ax*c1+ay*s1));
theta5_2 = 2*pi-theta5_1;
%选解
if theta5_1>=-pi&&theta5_1<=pi %theta5的角度范围
   theta5=theta5_1;
else
   theta5=theta5_2;
end   
 
s5 = sin(theta5);
c5 = cos(theta5);

%%
% theta4 两个解
theta4_1 = asin((ax*s1-ay*c1)/s5);
theta4_2 = pi-theta4_1;
%选解
if theta4_1>=-pi&&theta4_1<=pi %theta4的角度范围
   theta4=theta4_1;
else
   theta4=theta4_2;
end   
 
s4 = sin(theta4);
c4 = cos(theta4);

%%
% theta6 两个解
c23=cos(theta2+theta3);
s23=sin(theta2+theta3);
theta6_1 = asin((oz*c23-s23*(ox*c1+oy*s1))/s5);
theta6_2 = pi-theta6_1;

if theta6_1>=-pi&&theta6_1<=pi %theta6的角度范围
   theta6=theta6_1;
else
   theta6=theta6_2;
end 


theta_ikine = [theta1 theta2 theta3 theta4 theta5 theta6]

四、总结

代码运行，部分位姿计算可能有问题，但经过验算，大部分应该是OK的，欢迎大家讨论指正~

参考文献：
https://www.docin.com/p-506791675.html