练习:
讨论平面3自由度3R机器人的姿态逆运动解(参考练习题二中的模型和参数)。已知固定长度参数:L1 = 4; L2 = 3和L3 = 2 (m).
a)用手推导,求这个机器人的姿态逆运动学解析解。
b)编写二一个matlab程序求解平面3R机器人的全部姿态逆运动学解,并将每一组的结果代入姿态正运动学matlab程序中验证所有结果。
解:3R机器人的逆运动学解在课本中有三种解法。在这我将采用代数方法求解。由练习题(二)我们可知3R机器人的末端相对于基坐标的姿态矩阵如下:
(注: )
首先求 ,将x于y进行平方和得:
由此我们可求出 ,利用反余弦公式可以得到两个解。将求得得 带入x、y得:
进行变换得:
从而我们可以解出 ,当 选择不同时,求出得 也不相同。
最后我们可以通过三个选择角度之和等于 ,从而求解出 。
除了代数法之外,我们还可以利用几何法和数值法来求解。
b)根据上面代数法的计算过程,编写matlab程序求解例题矩阵,并最后运用正运动学验证结果。
%定义末端坐标相对于基坐标得齐次矩阵
T0H = [1 0 0 9
0 1 0 0
0 0 1 0
0 0 0 1];
% T0H = [ 0.5 -0.866 0 7.5373
% 0.866 0.5 0 3.9266
% 0 0 1 0
% 0 0 0 1 ];
T3H = transl(2,0,0);
%过得计算得齐次矩阵
T = T0H*inv(T3H);
%定义相关参数
X = T(1,4); Y = T(2,4);
L1 = 4; L2 = 3; L3 = 2;
%开始求解角度;
C2 = (X^2+Y^2 -L1^2 -L2^2)/(2*L1*L2);
theta2 = acosd(C2);
%在0°和180°时只有一个解,其他位置有两个解
if theta2 == 0
else
theta2 = [theta2; -theta2];
end
K1 = L1 + L2*cosd(theta2);
K2 = L2*sind(theta2);
r = sqrt(K1.^2+K2.^2);
theta1 = atan2(Y./r,X./r) - atan2(K2,K1);
theta3 = rad2deg(atan2(T(2,1),T(1,1))) - theta1 - theta2;
theta = [theta1 theta2 theta3]
% 定义各个连杆,默认为转动关节
% theta d a alpha
L(1)=Link([ 0 0 0 0],'modified'); L(1).qlim=[-pi,pi];
L(2)=Link([ 0 0 4 0],'modified'); L(2).qlim=[-pi,pi];
L(3)=Link([ 0 0 3 0],'modified'); L(3).qlim=[-pi,pi];
L(4)=Link([ 0 0 2 0 1],'modified');%工具坐标{T}
Robot3 = SerialLink(L,'name','R3');
%验证矩阵
T03 = Robot3.fkine([theta(1,:),0],'deg')
至此,计算结束。不过在例(2)验算结果与原矩阵有一定误差。(暂时不知道具体原因)
评论(0)
您还未登录,请登录后发表或查看评论