一种机械臂运动学研究及3D仿真平台构建

摘 要：针对川崎机械臂FS03N的构型特点，提出了一种逆运动学的求解方法。采用DH法建立了机械臂的连杆坐标系，得到正运动学方程，通过变量分离将机械臂姿态采用欧拉角表示，得到了机械臂位姿的一组广义坐标。通过对FS03N的构型分析，采用几何法与反变换法相结合的方法，以解的组合关系为基础，得到了机械臂的8组封闭解。建立了基于Matlab的机械臂算法验证与3D仿真运动平台，验证了逆运动学解算的正确性，为机械臂的轨迹与路径规划提供了前提条件。

关键词：机械臂；逆运动学；组合关系；封闭解；仿真验证

中图分类号：TD241；TD391 文献标志码：A 文章编号：1672-1098（2014）03-0039-06

机械臂运动学是机器人研究领域的基础课题，包括正运动学与逆运动学。正运动是指在已知机械臂各关节角度的情况下，求取末端执行器位于基础坐标系下的位姿；而逆运动学则是指在已知末端执行器位于基坐系下的位姿情况下，求取机械臂的各关节角度值。对于具有串联结构的机械臂，正运学求解相对容易，而对于逆运动学求解，由于机械臂的结构不同，其求解的复杂程度也不相同。从工程的应用角度出发，逆运动学求解更为重要。

上世纪80年代，Paul等采用的解析法对机械臂运动学进行求解，对于后来的运动学逆解问题具有指导性意义[1]。Primrose首次证明一般6自由度机械臂最多具有16组逆解[2]。Regnier等采于迭代法，能够计算出多种结构六自由度手臂的逆解[3]。于艳秋等采用有理数的方法求解了一般6自由度机器人手臂逆运动学问题，虽然保证了解的精度，但是却难以解决实时实现问题[4]。朱世强课题组从算法的实时性角度出发，采用矩阵分解和向量内积的方法，对课题组研制的钱江一号机械臂逆解进行了研究[5-6]。钱东海等基于旋量理论建立机械臂运动学模型，利用消元理论和Paden-Kahan子问题相结合的方法，提出了一种机械臂的逆运动学算法[7]。吕世增等将吴方法引入机械臂逆运动学求解中，通过特征列思想和旋量法相结合，并利用数学计算软件实现了逆运动学的求解[8]。

在机械臂的逆运动学求解过程中，解析法可以找到全部根，但是计算较为复杂；几何法虽然对一般机械臂不具备通用，但是形式简单易于理解；迭代法受初值选取问题约束，往往无法得到全部解；遗传算法[9-10]、神经网络[11]与Groebner基法[12]在理论上是可行的，但实时性不强，且存在解的精度与稳定性问题，这与要求实时性控制高的机械臂而言，具有很大的局限性，很少用于实际机械臂控制之中。

本文首先采用DH法建立该种机械臂的连杆坐标系，得到正运动学方程，并通过将姿态矩阵分离变量，得到机械臂包含六个参数的一组广义坐标。对于逆运动学求解，前3个关节采用几何法，后3个关节采用反变换法，通过给出解的组合关系的方法，计算得到该机械臂的8组解。开发基于Matlab的机械臂运动仿真平台，验证运动学逆解计算的正确性。

1 正运动学

1.1 坐标系建立

该工业机械臂共有6个旋转关节，如图1所示。根据机械臂的构型特点，采用DH法确定其各关节处的连杆坐标系，首先确定其基坐标系o0-x0y0z0，原点位于关节1轴线与关节2轴线所在水平平面的交点上，然后依次建立位于关节2-6处的坐标系，o6-x6y6z6为工具坐标系，完整的坐标系如图2所示。完成坐标系建立后，根据相邻杆件间坐标系的关系，确定其DH参数，如表1所示，其中θi为第i关节角度值，di为相邻关节间的杆件长度，ai为相邻关节间的杆件偏移量，αi为相邻坐标系间的扭转角。

1.2 正运动学方程

相邻杆件间的坐标变换矩阵为

将表1中的各参数分别代入式（1），可得6个矩阵T1、T2、T3、T4、T5、T6，将此6个矩阵依次相乘，便可得到机械臂从基坐标系至工具坐标系的坐标变换矩阵如式2所示。

式中包含了机械臂的位置与姿态信息，其中[n s a]为姿态矩阵共有9个元素，[px py pz]′为机械臂工具坐标系的位置。而工业机械臂一般采用一组完备的广义坐标（X Y Z O A T）进行表示，因此将式（2）中包含的姿态矩阵进行变换，采用欧拉角进行表示。其计算思想为，令手臂的姿态矩阵与欧拉角的转动矩阵相等如式（3）所示，通过对应元素相等，可以先求得第一个转动角度φ，在此基础上，对式（3）变量分离，进而求得θ与ψ，求解过程如下

最后可以得到机械臂的一组完备广义坐标（px， py， pz， φ，θ，ψ）。图1所示机械臂各关节角度为机械零，但是此时通过DH法建立的坐标系关节坐标却为（90，-90，90，0，0，0），因此在具体求解过程中，要进行加权处理。

2 逆运动学

逆运动学就是在已知机械臂末端执行器位姿的情况下，求解机械臂的各关节角度值，是正运动学的反过程。串联机械臂正运动学的解具有唯一性特点，而逆运动学的求解相对较为复杂，且存在多解与无解的可能性。该机械臂满足Pieper准则，可以得到最多8组逆运动学封闭解。在以下逆运动学求解过程中，各关节角度以机械臂的机械零为基准进行。

2.1 构型分析

结合图1与图2， 可以看出该机械臂的肩部始终位于o0-x0y0平面上； 大臂与小臂长度相同a2=d4，即肩、肘与腕三点构成一等腰三角形，从可操作度的角度出发，当大臂与小臂的之和为常数时，大臂与小臂的长度相同，使机械臂的可操作度达到最大值。无论机械臂的各关节如何运动，其肩、肘与腕构成的等腰三角形始终位于同一平面，该平面与水平面相互垂直。

2.2 求解θ1、θ2和θ3关节角

机械臂末端执行器位于基坐标系下的位姿为（px， py， pz， φ，θ，ψ），通过式（3）可以求得其姿态矩阵[n s a]，而后便可确定机械臂腕部位置坐标（xW， yW， zW）有式（8）和图3、图4。