六自由度机械臂运动学分析与仿真

六自由度机械臂运动学分析与仿真相里燕妮杨小军（中国科学院西安光学精密机械研究所）摘要：针对安川弧焊工业机器人手臂MOTOMAN-MA1400的构型特点，采用D-H法建立了机械臂的连杆坐标系，得到了以关节角度为变量的正运动学方程，利用MATLAB进行正逆运动学计算以及机械臂末端点的轨迹规划。为了验证正逆运动学模型的正确性及直观地观察机械臂各部分运动情况，采用Pro-E建立了机械臂的三维实体模型。将角度变量值导入模型，开发了机械臂运动仿真平台。仿真结果与理论计算一致，从而验证了算法的正确性，并完成了机械臂的运动仿真，为机械臂末端位置的激光标定实验提供了理论参考。关键词：机械臂运动学仿真中图分类号：TN11文献标识码：AKinematicsanalysisandsimulationofasixDOFmanipulatorXIANGLIYan-niYANGXiao-jun(Xi'anInstituteofOpticsandPrecisionMechanicsofCAS,Xi'an710119,China)Abstract:ConsideringtheconfigurationcharacteristicsofMOTOMAN-MA1400industrialmanipulator,D-Hmethodwasusedtoestablishconnectinglinkcoordinatesystem,obtainingforwardkinematicsequationswithjointanglevariables.TheforwardandinversekinematicsandInordertoverifythecorrectnessoftheforwardandinversekinematicsandvisualizethemanipulatormovement,themanipulator3DphysicalmodelwasbuildedinPro-E.Angularvariableswereinputinthemodelandthesimulationplatformofmanipulatorwasdeveloped.Simulationresultswereconsistentwiththeoreticalcalculations.Thecorrectnessofthealgorithmwasverifiedandthemanipulatormovementsimulationwascompletedbasedontheplatform,whichcanprovideatheoreticalreferenceforthemanipulatorextremitycoordinatesinlasercalibrationexperiments.Keywords:manipulatorkinematicssimulation0引言随着科技的发展，机器人的应用越来越广泛。而机械臂作为机器人的主要执行装置，使其成为机器人技术的重要研究内容。本文主要以6自由度的MOTOMAN-MA1400机械臂为研究对象，机器人运动学描述了机器人关节与组成机器人的各刚体之间的运动关系,是机器人末端执行器的直角坐标空间与机器人关节空间之间进行相互转换的桥梁。它包括运动学正解和运动学逆解，其求解算法将直接影响机器人系统运动的精度和速度。机械臂运动学分析是实现机械臂运动控制与轨迹规划的基础，其中正运动学是最基本的问题，目前机械臂运动学模型[1]建立主要采用D-H法与旋量法。本文运动学方程采用D-H法建立，PeterCorke开发了基于MATLAB的机器人工具箱，对于机器人的运动学反解，采用机器人工具箱RoboticsToolbox通过程序自动求解，任意给定机械臂末端点在空间中的起点和终点坐标，求解运动学反解，再通过高阶多项式插值来获得各个关节期望角度变量、角速度和角加速度。采用Pro-E建立了机械臂的三维实体模型，将计算得到的角度变量导入实体模型中，通过仿真的方法复现机械臂运动，能够直观地监视手臂各部分的运动情况，也验证了算法的正确性。运动学分析也为机械臂末端位置的激光标定实验提供了理论参考。1机械臂实体建模采用Pro-E根据对MOTOMAN-MA1400机械臂的外围尺寸结构约束关系进行三维建模。根据机械臂的关节分布情况及构型特点，分为6个部分进行建模，作者简介：相里燕妮（1989-），女，硕士，陕西韩城人。研究领域：机器人运动与控制。Email:[e-mail].导师简介：杨小军，男，副研究员，硕士生导师。即：S轴（旋转）、L轴（下臂）、U轴（上臂）、R轴（手腕旋转）、B轴（手腕摆动）、T轴（手腕回转）。各个轴之间添加旋转约束，各部分严格按照机械臂的三维几何关系进行建模。完成各个杆件建模之后，依次对模型进行装配，最终得到的MOTOMAN-MA1400装配体如图1所示。图16自由度机械臂Pro-E三维模型Fig.1Pro-E3Dmodelof6-DOFmanipulator2机械臂运动学分析2.1机械臂连杆坐标系建立MOTOMAN-MA1400是具有6个旋转关节的6自由度机械臂，根据该机械臂的构型特点，采用D-H法[2]建立各关节处的坐标系。首先确定基坐标系的位置及手臂的初始位置和姿态，然后采用D-H法建立基坐标系与各关节处的坐标系。000ZYX表示基坐标系，原点位于第1关节轴线与第2关节轴线的公垂线与第1关节轴线的交点处，而后依次建立各关节处的坐标系，666ZYX为机械臂工具坐标系，机械臂坐标系如图2所示。图26自由度机械臂坐标系Fig.26-DOFmanipulatorcoordinatesystem2.2机械臂运动学正解完成连杆坐标系建立后，根据相邻关节坐标系间的关系可确定关节和连杆的D-H参数。如表1所示，其中、a、d为固定值，为变量。各个连杆的D-H参数见表1。表16自由度机械臂的D-H参数Tab.1theD-Hreferencesof6-DOFmanipulator连杆i[◦]i[◦]ia[m]id[m]转角范围[◦]11-900.150.45-170~+170221800.6140-90~+15533-900.20-175~+190449000.64-150~+15055900.030-45~+18066000.2-200~+200根据表1中的D-H参数，可以根据连杆的齐次变换矩阵式(1)1cossincossinsincossincoscoscossinsin0sincos0001iiiiiiiiiiiiiiiiiiiaaTd(1)将表1中的D-H参数带入式(1)中，计算相邻关节的变换矩阵。对于6个自由度的串联结构机械臂，各个连杆坐标系之间属于联体坐标关系，由6个连杆的D-H矩阵，则机械臂末端的位置和姿态可由式(2)求取。100065544332211060zzzzyyyyxxxxpaonpaonpaonTTTTTTT(2)式中：60T为机械臂末端执行器在基坐标系中的位姿矩阵，Tzyxppp为机械臂末端执行器在基坐标系中的位置，Tzyxnnn为机械臂末端执行器坐标系的6X轴在基坐标系中的方向矢量，Tzyxooo为机械臂末端执行器坐标系的6Y轴在基坐标系中的方向矢量，Tzyxaaa为机械臂末端执行器坐标系的6Z轴在基坐标系中的方向矢量。若任意给出各个连杆的关节角为[0pi/2pi/2000]，则可由式(2)求出机械臂末端执行器在基坐标系中的位姿矩阵：061.0000000.380001.000000001.00000.27600001.0000T2.3机械臂运动学反解运动学逆解[3]讨论上述运动学正解方程的反向问题，即求由笛卡尔空间到关节空间(即所有关节转角)的逆变换，以求解各个关节转角。关于机器人的逆向求解并没有唯一确定的通用算法可利用，本文在研究的MOTOMAN-MA1400型机器人的位置结构和姿态结构的特点后,采用一种避免大量逆矩阵相乘的方法，即用110121TT左乘式(2),用165T右乘式(2)得16560110121544332TTTTTTT(3)根据上式求解后六自由度串联关节型机器人的运动学逆问题有若干组解，须根据机器人的各项参数和工作环境，从中选择一组最合适的解。3MATLAB运动学仿真3.1MATLAB正解逆解仿真上文已对MOTOMAN-MA1400型机器人的运动学正解和逆解做了理论分析，为了验证理论计算的正确性，本文采用MATLAB的机器人工具箱RoboticsToolbox[4]通过程序自动求解，分别采用工具箱的link()、robot()、drivebot()、fkine()和ikine()函数并根据表1中的D-H参数建立MOTOMAN-MA1400机器人对象以及计算运动学正解和逆解。给定关节角度值为[0pi/2pi/2000]，通过MATLAB程序求解得到的运动学正解结果为：061.00000.00000.00000.38000.00001.00000.00000.00000.00000.00001.00000.27600001.0000T求解得到的运动学逆解结果为：Q’=[0.00001.57081.5708-0.0000-0.0000-0.0000]由上可知，理论计算与程序仿真求解完全相符，从而得到了相互验证。3.2MATLAB运动学仿真取机器人的初始角度值为0q=[000000]，终止位置的关节角度为zq=[pi/3pi/2pi/2000]，利用机器人工具箱中的transl()、jtraj()、ctraj()和plot()函数生成机械臂末端在X、Y、Z三个方向上由初始位置到终止位置位移矢量的运动变化轨迹，如图3和图4所示。00.10.20.30.40.50.60.70.80.9100.10.20.30.40.50.60.70.80.91图3机械臂末端轨迹规划Fig.3thetrajectoryplanningofthemanipulatorextremity00.20.40.60.81-0.200.20.40.60.20.40.60.81图4机械臂末端运行轨迹Fig.4thetrackofthemanipulatorextremity机械臂轨迹规划所采用的方法是高阶多项式插值法[5]，从图3可以发现，高阶多项式插值的运动轨迹平滑，易于实现。051000.511.5仿真时间t/s关节1的角位移/rad051000.050.10.150.2仿真时间t/s关节1的角速度/(rad/s)0510-0.1-0.0500.050.1仿真时间t/s关节1的角加速度/(rad/s2)051000.511.52仿真时间t/s关节2的角位移/rad051000.10.20.30.4仿真时间t/s关节2的角速度/(rad/s)0510-0.1-0.0500.050.1仿真时间t/s关节2的角加速度/(rad/s2)图5关节角度变量、角速度和角加速度Fig.5theanglevariables,anglevelocityandangleacceleration为了实现机械臂的各个关节在关节坐标系下的轨迹规划，首先利用逆运动学方程将路径点转换成关节角度值，然后分别对每一个关节变量映射成一个光滑时间函数,使之从起始点开始，依次通过所有路径点，最后达到目标点，采用高阶多项式插值法得到的关节空间下的前2个关节的关节角度变量、角速度和角加速度曲线如图5所示，由此可见，机械臂各关节均正常运动，能够平稳的实现由初始位置到达终止位置这一过程，表明了该六自由度机器人的结构参数设计合理，可以达到期望的目标。4Pro-E仿真平台为了验证正逆运动学模型的正确性及直观地观察机械臂各部分运动情况，在Pro-E中建立了机械臂的三维实体