一种机械臂运动学研究及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。

图4 关节1角度与坐标系关系结合该机械臂的构型特点分析与图3，可以看出点S、E、W与W′四点构成的平面始终与o0-x0y0平面垂直，以机械臂的机械零位为基准并结合关节1角度的定义（见图4～图6），可以得出第一关节的两组解为

图6 角度关系图由于S、E、W三点位于与o0-x0y0垂直的平面上，为了便于求解，将此空间中的三个点转换为平面问题进行求解。W点位于o0-x0y0z0坐标系下的坐标值已知，将该坐标值转化为如图5所示的S-x′ 0 y′ 0 z′ 0，从S-x′ 0 y′ 0 z′ 0至o0-x0y0 z0可以看作由一个平移与一个旋转组成，S-x′ 0 y′ 0 z′ 0先沿其y′ 0 轴平移-a1个单位，然后绕z′ 0 旋转-θ1角度，如图5所示，得到其旋转矩阵为

求取关节1角度值后，再进行关节3的求取，图6为坐标系转换后的关节角度关系图，其中E′为机械臂肘部可能的另一位置。结合图5与图6，求取关节3角度值有

2.3 求解θ4、θ5和θ6 关节角

2.4 解的对应关系

通过以上可以计算得到该机械臂的各关节角度值，共有可能的8组解。对于给定的一组位姿值（px， py，pz，φ，θ，ψ），通过式（9）与（10）可以求得第1关节的两组解，通过式（14）可以求得第3关节的两组解，通过式（17）可以求得第2关节的两组解，第2关节的两组解都与第1关节的解相关联，因此对于第1关节的两组解，第2关节共有四组解，且其取值与第3关节角度值的正负号相关联，因此前三个关节解的组合共有四种组合。对于后3个关节解的组合，共有两组，直接与前3个关节解相关联，因此对应每一组的前3个关节解，有两组后3个关节解的组合，因此得出了该机械臂的8组解组合的对应关系。为了形象地说明此8组解的组合关系，采用树形的形式加以表述，该机械臂的逆解树如图7所示。

为了验证FS03N机械臂运动学模型建立的正确性，开发了基于Matlab的机械臂运动仿真平台，图8为软件设计流程图，图9为构建的仿真平台。本仿真平台包含了正运动学与逆运动学验证，并且可以实时地观察到机械臂的运动情况。

逆运动学求解正确性的验证如下：先随机取机械臂的一组关节角度（30， 20， -70， -40， -30，-60），通过正运动学计算公式可以得到此时的机械臂位姿为（7.120 86，-63.756 69，416.680 73，-79.459 80，74.736 51，89.308 97），然后将此组位姿值作为机械臂逆运动学求解计算的输入值，可以计算得到该机械臂的8组解，而后将这8组解逐组输入正运动学求解的文本框内，每组得到一组与之对应的位姿值，每一组位姿与原输入的角度值得到的位姿进行比较，结果相同，从而验证了逆运动学算法的正确性，并且此8组解中包含原随机输入的一组关节角度值，如表2～表3所示。

4 结束语

本文针对川崎FS03N机械臂的运动学问题，采用DH方法建立了机械臂的关节坐标系，得到了正运动学方程，而后采用变量分离的办法，对位姿采用6个参数加以表示。对于逆运动学求解，前3个关节采用几何法求解，后3个关节采用反变换法求解，通过给出解的几何关系，得到了该种构型机械臂的8组封闭解。为了验证算法了正确性，开发了基于Matlab软件的运动仿真平台，不仅验证了算法的正确性更能直观地观察到机械臂的运动情况。本文采用的计算方法，可以为机械臂的运动控制提供相应的理论依据，开发的运动仿真平台为机械臂的轨迹规划与解决避碰问题提供了可视化的条件。

参考文献：

[1] PAUL R P， SHIMANO B E， MAYER G. Kinematics control equations for simple manipulators[J]. IEEE Transactions on System， Man and Cybernetics， 1981， 11（6）：449-455.

[2] E J F PRIMROSE.On the input-output equation of the general 7R mechanism[J]. Mechanism and Machine Theory， 1986， 21（6）：509-510.

[3] REGNIER S， OUEZDOU F B， BIDAUD P. Distributed method for inverse kinematics of all serial manipulators[J]. Mechanism and Machine Theory， 1997， 32（7）：855-867.

[4] 于艳秋， 廖启征. 基于有理数运算的一般6R机器人位置逆解算法[J]. 机械工程学报， 2005， 41（3）：229-233.

[5] 刘松国， 朱世强， 王宣银. 基于矩阵分解的一般6R机器人实时高精度逆运动学算法[J]. 机械工程学报， 2008， 44（11）：304-309.

[6] 刘华山， 朱世强， 吴剑波. 基于向量积的机器人实时性逆解算法[J]. 农业机械学报， 2009， 40（6）：212-216.

[7] 钱东海， 王新峰， 赵伟， 等. 基于旋量理论和Paden-Kahan子问题的6自由度机器人逆解算法[J]. 机械工程学报， 2009， 45（9）：72-76.

[8] 吕世增， 张大卫， 刘海年. 基于吴方法的6R机器人逆运动学旋量方程求解[J]. 机械工程学报， 2010， 46（17）：35-41.

[9] HUANG WUXIN， TAN SHILI. Inverse kinematics of compliant manipulator based on the immune genetic algorithm[C]// International conference on natural comutation. Ji'nan， China， 2008：390-394.

[10] 张智， 朱齐丹， 吴自新. 基于免疫遗传算法的机械手避碰逆解[J].系统仿真学报， 2007， 19（3）：514-518.

[11] V T JOVANOVIC， K KAZEROUNIAN.Using chaos to obtain global solutions in computational kinematics[J]. Journal of Mechanical， 1998， 120：299-304.

[12] 杭鲁滨， 王彦. 基于Groebner基法的一般串联6R机器人机构逆运动学分析[J]. 上海交通大学学报， 2004， 38（6）：853-856.

（责任编辑：李 丽）