本文章为系列文章,以IRB1600机器人为例,建立机器人正运动、逆运动学、控制系统模型,并在simulink中进行仿真,与理论计算结果进行对比验证
(一) irb1600机器人坐标系建立、正运动学计算与simulink验证
(二) 机器人逆运动学计算
(三) 机器人运动学控制系统仿真
机器人逆运动学计算有迭代法和解析法两种
迭代法涉及微分运动学,本质与理论力学中的虚功原理类似,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(·)代表正向运动学方程。
串联机械臂有逆运动学解析解的充分条件是满足Pieper准则。即如果机器人满足两个充分条件中的一个,就会得到封闭解,这两个条件是:
设末端位姿已知,为
根据正运动学求解过程,容易建立如下等式:
接下来详细说明每个关节角度的求解过程
将上述等式变换为
令(3,3)(3,4)元素相等,得到
(为简化书写,cos(theta1)=c1,sin(theta1)=s1,依次类推,下同)
将等式变换为
令(1,3)(3,3)(1,4)(3,4)元素相等
(该部分解算过程较长,用到和角公式、万能公式等,如想了解详细求解过程,请点击下载求解过程报告)
计算得到
根据上述结果,编写了逆运动学代码
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