平面3连杆机器人正逆解

        对于平面连杆类机器人，课本、论文以及实际使用的机器人(SCARA机器人就是平面连杆机器人的变种)，基本上都是平面2连杆。即，给定平面坐标(x,y)，对应两组（theta1，theta2），通过选接就可以确定唯一解。实际上，由于作业空间等的需求，有时需要平面3连杆机器人来完成平面的定位，即给定平面坐标（x,y），需要求出唯一对应的(theta1、theta2、theta3)，对于这种系统，显然有无穷解。为了在只给定（x,y）的情况下，获取唯一对应的(theta1、theta2、theta3)，我们需要额外计算出一组条件，我采用的方法是：根据（x,y）计算并指定末端连杆的姿态。

结构与DH表

        平面3连杆机器人的结构和各坐标系定义如下（右手坐标系、右手旋转）：

        其中x0-y0叫做“基坐标系(robot frame)”，该坐标系作为参考坐标系，固定不动；x1-y1固连在第一关节，相对与x0-y0仅有转交角theta1；x2-y2固连在第二关节，相对与x1-y1有转交角theta2和连杆长度L1；x3-y3固连在第三关节，相对与x2-y2有转交角theta3和连杆长度L2；xtool-ytool为固连在第三连杆末端的工具坐标系，相对与x3-y3仅有连杆长度L3。

        根据上图，得到标准DH表如下：

	a	alpha	d	theta
1	0	0	0	theta1
2	L1	0	0	theta2
3	L2	0	0	theta3
tool	L3	0	0	0

运动学正解

        使用标准DH法建立DH模型之后，各坐标系之间的齐次变换矩阵使用如下公式求取：

        根据上述公式和DH参数，求取：、、、。各齐次矩阵相乘得到：

                                                                      

        则表示工具坐标系相对于基座标系的齐次变换矩阵，包含位置和姿态的相对关系，即为正解。

运动学逆解

        各个机器人的逆解计算没有统一方法，一般有几何法、代数法、雅可比迭代、非线性寻优等解法，使用几何法来求解平面3R机械臂逆解。因平面3R结构冗余，仅给定XY，无法唯一确定3关节转角度，本文解决方法为：

       通过给定XY，来指定末端工具坐标系姿态。如下图所示，3R末端距原点的长度为S，定义图中两向量夹角gamma满足如下关系：

                                                               

       则3R机械臂末端从最远处运行至原点，gamma变化范围为：0~180.

       逆解求解过程大致如下：

       1）根据给定（x,y），求S、gamma；

       2）根据(x,y)、gamma、L3，求第二连杆末端位置（x2,y2）;

       3）根据(x2,y2)，求theta1、theta2、（x1,y1），这里就是平面2连杆的方法，方法比较成熟，不再详细说明;

       4）根据（x1,y1）、(x2,y2)、（x,y），求theta3。

【note】:使用该方法，对于3连杆的长度关系有一定要求，具体的我没有推导（感兴趣的可以推导下），一般只要3条连杆长度差不多即可，该方法有唯一解，计算速度快；另外也可以进行迭代求解，计算量会稍大。