串联机器人运动学分析与研究

周承仙，富 巍

(1. 厦门理工学院 电气工程与自动化学院，福建 厦门 361024；2. 福建省高校机器人工程研究中心，福建 厦门 361024)



串联机器人运动学分析与研究

周承仙1,2，富 巍1,2

(1. 厦门理工学院 电气工程与自动化学院，福建 厦门 361024；2. 福建省高校机器人工程研究中心，福建 厦门 361024)

为直观显示机器人路径规划的优化状况，采用D-H法则建立连杆坐标系和连杆参数，推导正运动学方程，实现逆运动学方程的封闭解. 利用LabWindows/CVI编程软件，结合OpenGL技术对机器人运动学正逆解进行了三维图形仿真，验证正逆运动学准确性和实时性，实时显示各关节轴运动的角度、 角速度及姿态. 仿真实验表明，末梢定位的误差小于6×10-5mm. 为验证仿真结果的可行性，将仿真生成的路径规划点输入到Googol GRB 3016型6自由度关节机器人测试，仿真结果与实际运行吻合.

串联机器人； 运动学； 三维仿真

0 引 言

正逆运动学分析在关节机器人研究中占有重要地位[1-2]. 为了能快速准确地对机器人进行运动学分析，优化机器人机构设计和轨迹规划，机器人仿真是一个重要环节，它涉及机器人本体机构学、 正逆运动学、 零部件建模以及三维仿真驱动控制算法等，是一项具有创新意义和实用价值的研究课题[3-4]. 机器人正向运动学一般采用齐次坐标变换法得到并且解是唯一的，而逆向运动学往往有多个解且分析方法较为复杂. 通常情况下，工业机器人都采用简单的机械模型来设计，以使其逆运动学问题可用封闭形式来表达. 用计算机对机器人系统进行运动仿真，是基于机器人理论和交互式计算机图形技术并用三维方式显示出来，用来检测机器人本体动态参数和路径规划. 目前机器人系统仿真需要解决的问题是缩小仿真与实物之间的误差. 国内外学者利用MATLAB中的Robotics Toolbox工具箱，实现了简单运动可视化运动仿真[5-10]，但在实际工程应用中其与仿真差别较大，且不方便嵌入式移植，仿真功能较弱，可扩展性差，缺乏具体的三维场景支持[11]. 大多数工业机器人公司都有独自的机器人仿真系统，这些机器人控制系统或仿真软件都是封闭式的，不利于研制初期开发者控制算法研究和路径优化. 一般用户都是通过示教来规划路径，示教点数是离散和不均匀的，故路径规划不平稳，精确度不高.

本文借助LabWindows/CVI与OpenGL技术把正逆运动学的解和轨迹规划算法融合在一起，利用D-H法建立6轴机器人运动学模型，递推正逆运动学求解，使用LabWindows和OpenGL的软件编程进行机械臂三维图形仿真，验证机器人数学模型求解正确性和路径优化. 将上位机编写仿真后的路径规划程序移植到其他嵌入式控制器上，避免在调试中发生失控造成机械臂相互碰撞甚至发生事故，实现输出不同机器能识别的离线程序.

1 机器人运动学D-H模型建立

Googol GRB 3016型6自由度机器人机械结构为关节1，2轴线垂直相交，关节2，3轴线平行，关节3，4轴线垂直相交，关节4，5，6轴线交于一点，符合Pieper原则，满足封闭解的条件[12]. 如图 1(b) 所示，建立连杆坐标系，确定坐标系中各轴的方向. 其中，di为两连杆的相对位置；θi为两连杆法线的夹角；ai为公发线长度；αi为关节扭角，连杆参数如表 1 所示.

图1 机器人本体及连杆坐标系

表1 机器人连杆参数

2 运动学方程正解

通过相邻机械臂坐标系的齐次变换矩阵，得到机械臂末端位置与姿态，即运动学方程的正向解[13]，由连杆坐标变化矩阵(1)得到每一个连杆和它相邻连杆变换矩阵为0T1～5T6.

(1)

把上述各矩阵相乘得0T6=

(2)

计算正运动方程的中间过程如下：关节2与关节3平行，先求出1T3,0T3,3T6，最后得出0T6，可得机械手的变换矩阵

(3)

式中：

其中, si=sinθi, ci=cosθi, s23=sin(θ2+θ3), c23=cos(θ2+θ3).

3 运动学方程逆解

机器人逆运动学分析是路径规划与优化的重要问题[14-16], 运动学逆解解法主要有代数法、 几何法和数值法[17-18]. 但由于机器人逆运动问题的复杂性和多样性，无法建立通用的解析算法[19]. 根据机器本体机构不同，建立坐标系不同，实现的算法也不一样. 逆解是从已知位姿[n o a p]求关节变量[θ1θ2θ3θ4θ5θ6]. 本文采用分离变量法，在方程两边乘变换矩阵逆矩阵，依次解出所有角度未知数.

3.1 关节角度θ1和θ3

计算

(4)

由式(4)两边的元素(2,4)相等，得到

(5)

由式(4)两边的元素(1,4)和(3,4)相等，联立得到方程组

(6)

式(6)平方相加化简得a3c3+d4s3=k,其中

(7)

3.2 关节角度θ2和θ4

计算

(8)

由式(8)两边的元素(1,4)和(2,4)相等，联立得到方程组

m1=c1px+s1py-a1, m2=a3+a2c3,

m3=-d4-a2s3,

(9)

求得θ2+θ3=atan(m2pz-m1m3,m3pz+m1m2)，θ2=θ23-θ3，根据θ1,θ3各有两种解，可得θ2可能存在4组解.

由式(8)两边的元素(1,3)和(3,3)相等，联立得到方程组

(10)

3.3 关节角度θ5和θ6

计算

(11)

由式(11)两边的元素(1,3)和(3,3)相等，联立得到方程组

(12)

由式(11)两边的元素(2,1)和(2,2)相等，联立得到方程组

(13)

逆解可能存在多解、 无解、 无效解. 对于实时控制器来说，逆解计算的时间和空间复杂度较高，实际路径规划中，机械臂在多解情况下只能执行一组解. 在直角坐标系下路径有连续性，关节坐标系也具备连续性，根据各关节速度相邻角度值不超过一定阈值，再借助工作空间限制、 避免撞击、 总体行程最短来确定唯一解.

4 三维建模与正逆解验证

Googol GRB 3016型机器人是6自由度串联机器人，机械本体包括回转底座、 大臂、 小臂和手腕. 采用LabWindows/CVI进行上位机编程，结合OpenGL技术实现机器人控制算法和实时三维图形仿真. OpenGL是一个性能优良通用的三维图形库，调用灵活、 与开发软件平台无关，而LabWindows是以C语言为开发环境，可利用C语言进行数据采集、 处理、 控制算法实现和显示，并可将控制算法文本语言移植到嵌入式中.

4.1 运动学三维建模系统

先利用OpenGL技术建立机械臂三维模型，自底向上绘制各部分结构，仿真控制算法由Labwindows/CVI编程实现机械臂控制，对于焊接机器人来说，由其他传感器，如2D激光距离传感器获得在直角坐标系下路径规划，经过初步数据处理后作为机器人末梢位姿参数. 先判断是否在机械本体工作空间内，并求各关节角度唯一解，实时显示各轴的动态参数，便于对关节轴任意时刻姿态分析. 三维仿真图可以从不同角度观看路径规划的可行性，并显示各关节轴的角度和角速度，以便优化姿态平稳性控制，评估驱动电机本身的物理特性是否能够实现. 仿真系统根据逆解结果实时绘制出机器人手臂三维图形，并显示直角坐标和关节坐标, 便于研究者直观地判断机械臂多解和奇异点，验证逆解算法的合理性与实时性，软件界面如图 2 所示.

图2 三维图形仿真界面

4.2 正逆运动学验证与理论误差

当机器位于初始姿态(θ1=0°,θ2=90°,θ3=0°,θ4=180°,θ5=0°,θ6=0°)得到

(14)

变换矩阵0T6与GRB3016机器人本体初始位姿一致.

在逆解验证中，以末梢轨迹为螺旋上升曲线求各关节角曲线，末梢运动轨迹方程为

(15)

式中：T为运动一个圆周的周期，t为总仿真时间. 图 3 为直角坐标下末梢运动轨迹曲线.

图3 轨迹规划曲线

末梢运动为螺旋上升轨迹时，得到对应各关节角度与时间的曲线关系如图 4 所示.

利用机器人三维模型仿真系统，将逆解结果代入正运动方程得到末梢位置与原轨迹对比，分别得到在笛卡尔坐标系中X轴、Y轴、Z轴方向的理论误差，如图 5 所示.

图4 各关节轴运动曲线

图5 路径规划的理论误差

4.3 实验验证与误差

为验证仿真系统实际应用的可行性，设置机器人末梢水平移动，即末梢沿Y轴负方向移动800 mm，通过该仿真系统得到路径规划点保存为Googol机器人格式的TXT文件，移植到Googol机器人控制器目录下，启动机器人工作，电机和伺服器均为三洋公司产品，上位机通过串口可以与电机伺服器通信，实时读取各轴电机实际运动的绝对编码器数值，实验表明，仿真得到的规划路径与实际运行吻合. 图 6 为关节轴角度跟踪曲线，图 7 为瞬时误差，误差主要由控制输出电机加速度偏小，或控制周期太小引起的，适当调整这两个参数可以减小误差，但最终控制系统延后几个控制节拍可达到角度精准定位.

图6 关节轴角度跟踪曲线

图7 关节角跟踪瞬时误差

5 结 论

本文从6轴机器人的运动学算法理论推导着手，实现机器人正逆运动学建模与仿真，根据D-H法则建立了6轴机器人运动学模型. 并根据连杆坐标系和连杆参数计算相应坐标变换矩阵，利用矩阵变换求出末端执行器运动方程的正逆解问题. 上位机采用LabWindows实现控制算法、 正逆解求解、 路径规划等，结合OpenGL技术实现三维显示，实时显示6轴机器人三维图形，显示各轴运动学的动态参数，如角度、 角速度和姿态等参数，从而可以估算各轴所需驱动电机大小. 将路径规划的仿真数据移植到机器人中验证，实现控制算法从仿真到控制实物的可行性，在控制算法中设置时间测试点，测量完成一次逆解需要的时间，保证逆解的实时性. 动态显示各轴位姿，根据机械本体约束，可以得到机器人有效工作空间，协助研制人员了解机器人工作空间的形态和极限，验证机械机构合理性与控制算法的可行性，便于机器人本体研制、 控制参数调试以及在焊接和喷涂应用中路径规划等问题.

[1]王伟，刘立冬，王刚，等. 基于四元数表示法的机器人基坐标系标定方法[J]. 北京航空航天大学，2015，41(3)：411-417. Wang Wei，Liu Lidong，Wang Gang，et al. Calibration method of robot base frame by quaternion form[J]. Journal of Beijing University of Aeronautics and Astronautics，2015，41(3)：411-417. (in Chinese)

[2]罗天洪，熊小辉. 六自由度重载装配机械手运动学分析及求解[J]. 重庆交通大学学报(自然科学版)，2009，28(6)：1126-1130. Luo Tianhong, Xiong Xiaohui. Kinematics analysis and solution of heavy manipulator with 6-DOF[J]. Journal of ChongQing JiaoTong University，2009，28(6)：1126-1130.(in Chinese)

[3]倪振松，廖启征，吴莘馨. 基于四元数矩阵与Groebner基的6R机器人运动学逆解算法[J]. 清华大学学报(自然科学版)，2013，53(5)：683-687. Ni Zhensong, Liao Qizheng, Wu Xinxin. General 6R robot inverse solution algorithm based on a quaternion solution algorithm based on a quaternion matrix and a Groebner base[J]. Journal of Tsinghua University(Science & Technology)，2013，53(5)：683-687. (in Chiese)

[4]白云飞，丛明，杨小磊，等. 基于6参数模型的6R串联机器人运动学参数辨识[J]. 机器人，2015，37(4)：486-492. Bai Yunfei，Cong Ming，Yang Xiaolei，et al. Kinematic parameter identification for 6R serial robots based on a 6-parameter model[J]. Robot，2015，37(4)：486-492.(in Chinese)

[5]李宪华，张雷刚，宋韬，等. 基于能力地图的机器人模块化手臂构型分析[J]. 华中科技大学学报，2015，43(12)：17-20. Li Xianhua，Zhang Leigang，Song Tao，et al. Configuration analysis of robot modular arm based on capability map[J]. Journal of Huazhong University of Science & Technology (Natural Science Edition)，2015，43(12)：17-20.(in Chinese)

[6]李劲松，赵连玉，刘振忠. 基于MATLAB/SimMechanics工业机器人惯量前馈仿真研究[J]. 天津理工大学学报，2015，31(1)：21-24. Li Jinsong，Zhao Lianyu，Liu Zhenzhong. Research of industrial robot inertia feedforward control simulation based on MATALB/SimMechanics[J]. Joural of Tianjin University of Technolog，2015，31(1)：21-24.(in Chinese)

[7]Piltan F, Taghizadegan A, Sulaiman N B. Modeling and control of four degrees of freedom surgical robot manipulator using MATLAB/SIMULINK[J]. International Journal of Hybrid Information Technology，2015，8(11)：47-78.

[8]Alshamasin M S, Ionescu F, Al-Kasasbeh R T. Modelling and simulation of a SCARA robot using solid dynamics and verification by MATLAB/Simulink[J]. International Review on Computers and Software，2012，7(3)：928-936.

[9]宁淑荣，郭希娟. MATLAB 在并联机器人机构仿真中的应用[J]. 系统仿真学报，2004，16(10)：2273-2275. Ning Shurong，Guo Xijuan. The application of Matlab in simulation of the parallel mechanism[J]. Journal of System Simulation，2004，16(10)：2273-2275.(in Chinese)

[10]王宇燕，邓高寿，马田，等. 基于波碰撞算法的机器人路径规划研究[J]. 中北大学学报(自然科学版)，2014，35(3)：286-292. Wang Yuyan，Deng Gaoshou，Ma Tian，et al. Robot path planning based on wave collision algorithm[J]. Journal of North University of China(Natural Science Edition)，2014，35(3)：286-292.(in Chinese)

[11]黄玉钏，曲道奎，徐方. 真空并联机器人轨迹规划仿真[J]. 计算机仿真，2012，29(12)：211-215. Huang Yuchuan，Qu Daokui，Xu Fang. Numerical simulation for vacuum-parallel robots trajectory planning[J]. Computer Simulation，2012，29(12)：211-215. (in Chinese)

[12]Paul R，Shimano B，Mayer G. Differential kinematic control equations for simple manipulators[J]. IEEE Transactions on SMC，1981，11(6)：449-455.

[13]John J. Craig. 机器人学导论[M]. 第三版. 贠超，译. 北京：机械工业出版社，2014.

[14]钱夔，宋爱国，章华涛，等. 基于自适应模糊神经网络的机器人路径规划方法[J]. 东南大学学报，2012，42(4)：637-642. Qian Kui, Song Aiguo, Zhang Huatao, et al. Path planning for mobile robot based on adaptive fuzzy neural network[J]. Journal of Southeast University(Natural Science Edition)，2012，42(4)：637-642.(in Chinese)

[15]洪宇翔，都东，潘际銮，等. 基于轨迹动态规划的移动机器人焊道自动跟踪[J]. 焊接学报，2015，36(10)：25-28. Hong Yuxiang，Du Dong，Pan Jiluan，et al. Seam tracking based on dynamic trajectory planning for a mobile welding robot[J]. Transactions of the China Welding Institution，2015，36(10)：25-28. (in Chinese)

[16]杜婧，丛明，温海营，等. 冗余驱动仿下颌运动机器人的机构设计及轨迹规划[J]. 机器人，2015，37(1)：43-52. Du Jing，Cong Ming，Wen Haiying，et al. Mechanism design and trajectory planning of a redundantly actuated parallel robot for jaw movement[J]. Robot，2015，37(1)：43-52.(in Chinese)

[17]黄昔光，廖启征. 空间6R串联机器人机构位置逆解新算法[J]. 北京航空航天大学学报，2010，36(3)：295-298. Huang Xiguang，Liao Qizheng. New algorithm for inverse kinematics of 6R serial robot mechanism[J]. Journal of Beijing University of Aeronautics and Astronautics，2010，36(3)：295-298. (in Chinese)

[18]胡杰，张铁. 基于SCARA机器人的运动学分析及关节解耦[J]. 机床与液压，2011，39(21)：28-32. Hu Jie，Zhang Tie. Kinematic analysis and joint decoupling for SCARA robot[J]. Machine Tool & Hydraulics，2011，39(21)：28-32. (in Chinese)

[19]卢伟，宋爱国，蔡健荣，等. 柑橘采摘机器人结构设计及运动学算法[J]. 东南大学学报，2011，41(1)：637-642. Lu Wei，Song Aiguo，Cai Jianrong，et al. Structural design and kinematics algorithm research for orange harvesting robot[J]. Journal of Southeast University(Natural Science Edition)，2011，41(1)：637-642. (in Chinese)

Kinematic Analysis and Research of Serial Robot

ZHOU Cheng-xian1,2, FU Wei1,2

(1. School of Electric Engineering & Automation，Xiamen University of Technology，Xiamen 361024，China；2. Robotics Engineering Research Center in the Universities of Fujian Province, Xiamen 361024, China)

In order to display the optimization condition of robot path planning visually, D-H rule was adopted to establish the link coordinate frame and link parameters, then the forward kinematics equation can be derived and the closed-form solution of inverse kinematics equation can be realized. In addition, by conducting 3D graphical simulation for the kinematics inverse and forward solutions using LabWindows/CVI programming software, the accuracy and real-time performance of positive and athwart kinematics were verified, and thus the angle, angular velocity, and posture of each joint axis movement can be real-time displayed. The simulation results show that peripheral location error is less than 6×10-5mm. To verify the feasibility of simulation results, the path planning points produced in simulation were input into Googol GRB 3016 articulated robot with 6 degrees of freedom for pilot run, indicating that the simulation results are consistent with practicle computational results.

serial robot； kinematics； three-dimensional stimulation

2016-02-16 基金项目：福建省自然科学基金资助项目(2014J01254)； 福建省教育厅A类项目(JA14232)； 福建省科技计划重大重点项目(2014H0048)

周承仙(1981-)，男，硕士，实验师，主要从事机器人应用与光电检测技术的研究.

1673-3193(2016)05-0522-08

TP242.3

A

10.3969/j.issn.1673-3193.2016.05.016