练习:

讨论平面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')
例(1)运算结果
例(2)运算结果

至此,计算结束。不过在例(2)验算结果与原矩阵有一定误差。(暂时不知道具体原因)