一、问题描述

如右图所示的三自由度机械臂，关节1和关节2相互垂直，关节2和关节3相互平行。如图所示，所有关节均处于初始状态。要求: (1) 定义并标注出各关节的正方向； (2) 定义机器人基坐标系｛0｝及连杆坐标系｛1｝，｛2｝，｛3｝； (3) 求变换矩阵 , , ； (4) 根据末端腕部位置 (x, y, z) 返求出对应关节，，； (5) 利用软件绘制出机器人模型的三维点线图，并控制机器人腕部沿半径r=0.1的圆弧运动.

二、任务求解

2.1建立坐标系

2.2 变换矩阵

2.2.1 变换求解

（1）连杆坐标系｛1｝——基坐标系｛0｝ 原点重合，可绕z轴任意旋转 （2）连杆坐标系｛2｝——连杆坐标系｛1｝ 先绕x轴旋转90°，再绕新得到的y轴旋转90°，然后沿新得到的y轴平移，最后得到的坐标系可绕z轴任意旋转 3）连杆坐标系｛3｝——连杆坐标系｛2｝ 绕z轴旋转-90°，再沿新得到的y轴平移，最后得到的坐标系可绕z轴任意旋转

2.2.2 物理意义求解

变换矩阵的前三列，每列值对应的数为变换坐标系的坐标轴x、y、z在基坐标系中的坐标位置，第四列为变换坐标系的原点在基坐标系中的坐标位置，第四行为齐次补行数据，则可根据坐标系的位置直接列出下式：可见，两种方式的结果表达式一致。

2.2.3 变换矩阵终解

2.3 逆运动学求解

2.3.1 矩阵逆推导

由连杆坐标系｛3｝到基座坐标系｛0｝的齐次矩阵可以表示为末端执行器的位置在基座坐标系｛0｝中的描述为末端执行器的位置在基座坐标系｛0｝中的描述为关系为：根据矩阵对应元素相等，由MATLAB计算可得可得下面等式：

x=-(7*cos(a2)*sin(a1))/25-(7*cos(a2)*sin(a1)*sin(a3))/20(7*cos(a3)*
sin(a1)*sin(a2))/20
y=(7*cos(a1)*cos(a2))/25+(7*cos(a1)*cos(a2)*sin(a3))/20+(7*cos(a1)*cos(a3)*sin(a2))/20
z=(7*sin(a2))/25 + (7*sin(a2)*sin(a3))/20 - (7*cos(a2)*cos(a3))/20 + 47/100

由则：由对应元素相等，得：

(x*cos(a1))/(cos(a1)^2 + sin(a1)^2) + (y*sin(a1))/(cos(a1)^2 + sin(a1)^2)=0

 (y*cos(a1))/(cos(a1)^2 + sin(a1)^2) - (x*sin(a1))/(cos(a1)^2 + sin(a1)^2)=(7*cos(a2))/25+(7*cos(a2)*sin(a3))/20+(7*cos(a3)*sin(a2))/20

z=(7*sin(a2))/25 + (7*sin(a2)*sin(a3))/20 - (7*cos(a2)*cos(a3))/20 + 47/100

综上可得，

2.3.2 几何推导

在x0y平面，将末端执行器的轨迹投影到该平面，蓝色表示原位置，橘色代表移动后的位置，黑色坐标系为基坐标系，如下图：
基于Python的3R机器人运动仿真由图易见，为末端执行器投影到xoy平面x与y的夹角，（逆时针为正）, 在y0z平面，将末端执行器的轨迹投影到该平面，蓝色表示原位置，橘色代表移动后的位置，黑色坐标系为基坐标系，如下图：将末端执行器的轨迹投影到下图所示平面，蓝色表示原位置，橘色代表移动后的位置，黑色坐标系为基坐标系，如下图：构建直角三角形，可得：综上，得各关节角与坐标位置的关系为：