串联机器人运动学分析及轨迹规划

串联机器人运动学分析及轨迹规划
陈卓;苏卫华;李彬;秦晓丽
【摘要】Objective To select a safe, flexible and lightweight manipulator and to execute its kinematic analysis and trajectory planning, so as to improve the application of serial robot in the treatment of hazardous biochemical products. Methods The kinematic modeling was carried out for the manipulator, and the kinematic and inverse kinematic formulas were deduced and verified.Differential transform method was used to obtain the Jacobian matrix of the manipulator,and the trajectory planning was implemented for the manipulator at joint and cartesian spaces.Results An ideal motion curve was formed by the trajectory planning of serial robot to realize its stable and high-efficiency motion at spaces.Conclusion Serial robot is facilitated to execute the treatment of hazardous biochemical products, and references are provided to other researches on serial robot.%目的:针对串联机器人在危险生化品处理方面的应用,选取一种安全系数高、灵活轻量的机械臂并对其进行运动学建模和轨迹规划.方法:先对机械臂进行运动学建模,之后推导并验证正运动学和逆运动学公式,然后利用微分变换法求取机械臂的雅可比矩阵,最后对机械臂在关节空间和直角坐标空间进行轨迹规划.结果:通过对串联机器人的轨迹规划,可使其形成光滑连续的理想运动曲线,使串联机器人在空间上的运动更加平稳高效.结论:该研究有助于使串联机器人更好地应用于危险生化品处理的场景,也可以为串联机器人的其他相关研究提供参考.
【期刊名称】《医疗卫生装备》
【年(卷),期】2018(039)001
【总页数】6页(P5-10)
【关键词】串联机器人;运动学分析;雅可比矩阵;轨迹规划
【作者】陈卓;苏卫华;李彬;秦晓丽
【作者单位】天津理工大学机械工程学院,天津300384;军事科学院系统工程研究院卫勤保障技术研究所,天津300161;军事科学院系统工程研究院卫勤保障技术研究所,天津300161;天津理工大学机械工程学院,天津300384;军事科学院系统工程研究院卫勤保障技术研究所,天津300161
【正文语种】中文
【中图分类】R318;TP242
0 引言
近10 a，串联机器人在工业生产、日常生活和医疗服务等方面得到了广泛应用。

目前，利用不同的末端执行器，串联机器人可以实现不同的作业任务，如包装、码垛、抛光、喷漆、医疗等。

串联机器人具有快速灵活、高精度、多功用等优点，这些优点使其在无人化、智能化技术领域应用广泛。

到目前为止已经有很多针对串联机器人的理论研究，Chen等[1]提出了基于旋量理论的串联机器人逆运动学子问题求解算法，Kang等[2]研究了冗余度双臂机器人协同工作空间及轨迹规划，Rodríguez等[3]研究了双臂机器人在抓取过程中的避障算法和协调操作控制。

为了解决串联机器人逆运动学的多解优化问题，一些智能算法理论被应用其中，Hasan等[4]提出了一种基于人工神经网络（artificial neural network，ANN）
的自适应学习策略控制六自由度机械臂的运动，Tabandeh等[5]提出利用遗传算
法来优化求解串联机器人的逆运动学。

除此之外，中外学者关于串联机器人的应用创新研究也有很多，例如双臂烹饪机器人[6]、医疗手术机器人[7]等。

本文提出利用串联机器人处理危险生化品，从根源上避免操作人员的感染风险，并提高任务工作效率。

图1所示的是串联机器人夹取病毒样品架进行水浴灭活的操作。

图2所示的是双臂机器人协同操作提取样品的过程，整个过程需要对机械臂
进行精确的轨迹规划和运动控制。

本文对该串联机器人模型完成了运动学分析和轨迹规划，扩展了串联机器人在危险生化品处理方面的应用。

图1 水浴灭活操作
图2 双臂协作提取样品
1 运动学分析
1.1 结构参数建模
本文选取一款丹麦的UR机械臂来实现危险生化品的处理。

该机械臂具有质量轻、结构紧凑、运动灵活和精度准确等优点，其关节处采用模块化结构设计，方便维修替换。

UR机械臂具有6个自由度，由2个肩关节、1个肘关节、3个腕关节组成。

通过Solidworks软件画出UR机械臂装配图（如图3所示），由D-H法[8]建立
其相邻关节空间的数学模型（如图4所示）。

UR机械臂的D-H参数表见表1。

D-H参数反映了机械臂相邻关节的坐标交换关系。

图3 UR机械臂装配图
图4 UR机械臂数学模型
表1 UR机械臂D-H参数表序号旋转角θ/（°）关节偏移d/m m连杆长度a/m
m扭角α/（°）1 θ 1 0 0 9 0 2 θ 2 0 L 1 0 3 θ 3 0 L 2 0 4 θ 4 0 0 9 0 5 θ 5 0 0 9 0 6 θ 6 0 0 0
1.2 正运动学
正运动学是指在已知机械臂各关节变量的情况下，计算末端执行器的位置和姿态，它描述了机器人关节空间与笛卡尔坐标空间的关系。

通过D-H法，相邻坐标空间
的齐次变换矩阵可表示为nTn+1，Rot（z，θn+1）表示绕z 轴旋转θn+1角度，Trans（0，0，dn+1）代表沿 z轴平移 dn+1单位，Trans（an+1，0，0）表示沿 x 轴平移an+1单位，Rot（x，αn+1）代表绕x轴旋转αn+1角度，所有相邻坐标系的变换都是基于当前坐标系的，因此应该依次右乘变换矩阵，如公式（1）所示：
其中
其中，Cθn+1为cosθn+1，Sθn+1为sinθn+1；Cαn+1为cosαn+1，Sαn+1为sinαn+1。

对于六自由度串联机器人，从基坐标到末端执行器的齐次变换矩阵可以表示为0T6：
其中，[n，o，a]组成的三维矩阵代表了末端执行器的姿态，矢量P代表了末端执行器在空间的位置。

替换对应参数之后得到
其中，Ci=cosθi，Si=sinθi，C23=cos（θ2+θ3）=C2C3-S2S3，S23=sin
（θ2+θ3）=S2C3+C2S3，S234=sin（θ2+θ3+θ4），C234=cos
（θ2+θ3+θ4）。

（nx，ny，nz）T、（ox，oy，oz）T、（ax，ay，az）T分
别表示机械臂末端执行器在3个坐标方向上的姿态矢量，（px，py，pz）T表示
机械臂末端执行器相对于基坐标的位置矢量。

通过公式（3）所求得的变换矩阵，能够发现该串联机器人末端点位置取决于关节角度θ1~θ3，而末端姿态与关节角
θ4~θ6有关。

1.3 逆运动学
逆运动学是指在已知机械臂末端执行器位姿的情况下，解出各关节角度变量，它可以被视为是正运动学的反向求解过程。

与正运动学相比，逆运动学在实际中有更广泛的应用。

其求解方法有很多种，如利用代数法求解析形式的封闭解、利用几何法简化模型求封闭解、利用伪逆雅可比矩阵求数值解等。

由于六自由度串联机器人逆运动学解的高维非线性、求解困难的特点，因此需要寻找一种方法来快速有效地求解逆运动学。

在本文中，我们使用右乘逆矩阵的方法来解耦关节角度，最终求得末端位姿关于各关节角度的解析解。

将0T6矩阵右乘A-11之后得到
公式（16）左右两边都是4×4的齐次变换矩阵，包含未知变量θ1~θ6（6个关节旋转角度）和已知变量（机器人末端位姿），我们需要解耦关节角度变量。

由公式（16）的矩阵（3，4）元素对应相等，可得
因此，可求得的另一组可能角度值）。

将矩阵元素（1，4）和（2，4）平方相加并对应相等，可得
由公式（18）可求得C3，又已知则
由矩阵（3，3）元素对应相等可以得到
又已知则
以矩阵元素（3，2）除以（3，1）可求得θ6=arctan
在计算θ2和θ4之前，需要先求出θ234（θ234=θ2+θ3+θ4）。

同样，以矩阵元素（2，3）除以（1，3）求出θ234=或的另一组可能角度值）。

由矩阵元素（1，4）和（2，4）组成关于sinθ2和cosθ2的方程组：
通过计算解出：因此
最后，通过已知的θ234根据公式（21）求出θ4：
1.4 实验验证
在MATLAB软件中对逆运动学公式进行验证。

首先我们随机选取6个关节角值，并利用正运动学公式得到相应的末端执行器位置和姿态，然后将位姿矩阵作为逆运动学公式的输入，以验证所求得的关节角度值与初始角度值的一致性。

随机选取3组实验数据，第一组关节角度为 90、60、45、45、60 和90°，先通过正运动学公式获取末端位姿T1，然后利用逆运动学反解出各关节角度，能够发现第三组关节角度与初始角度一致（在Matlab下角度以弧度值表示）。

第二组随机选取的关节角度为 30、45、60、0、30和90°，同样的方法，此时得出末端位姿T2，逆运动学的第一组解与初始角度一致。

第三组随机选取的关节角度为 22.5、77、60、0、36和90°，同样的方法，得出此时末端位姿T3，逆运动学的第一组解与初始角度一致。

通过3组随机选取的实验角度，验证了所推导逆运动学解的正确性。

在MATLAB
中分别仿真出3组机械臂位姿，如图5所示。

2 雅可比矩阵求解
雅可比矩阵反映了单个关节的微分运动与整个机构微分运动的关系，它与机器人的奇异位形有很强联系，而奇异性是机器人运动性能评价的一个重要因素，因此本章节对雅可比矩阵进行了求解。

当机械臂机构移动到奇异位形时，自由度的减少使速度或加速度突然变化，导致机械臂稳定性丧失，因此解决奇异位形问题对提高机器人的稳定性和动态性具有重要意义。

通过分析机械臂奇异位形处的雅可比矩阵，可以得到约束条件。

当雅可比矩阵的行列式为零，即雅可比矩阵是一个奇异矩阵的情况时，机械臂正好会处于奇异位形。

关于机器人雅可比矩阵的求解有3种代表性方法：矢量积法[9]、微分变换法[10]和螺旋运动求解法[11]。

由于微分变换方法求解简单、计算方便，本文选取该方法来求解雅可比矩阵。

图5 3组机械臂位姿
对于旋转关节：TJ1i=（pxny-pynx），TJ2i=（pxoy-py ox），TJ3i=（pxay-pyax），TJ4i=nz，TJ5i=oz，TJ6i=az。

其中，TJ1i为雅可比矩阵中第1行第i 列元素。

对于移动关节：TJ1i=nz，TJ2i=oz，TJ3i=az，TJ4i=0，TJ5i=0，TJ6i=0。

利用正运动学公式求解末端位姿并替换对应参数，得到雅可比矩阵，用Jij表示矩阵第i行第j列元素。

第一列元素：J11＝-C6S5（C2L1+C23L2），J21＝S5S6（C2L1+C23L2），J31＝C5（C2L1+C23L2），J41＝C5C6S234－S6C234，J51＝-C6C234－
C5S6S234，J61＝S5S234。

第二列元素：J12＝L2C5C6S4-L1C3C4S6-L2C4S6+L1S3
S4S6+L1C3C5C6S4+L1C4C5C6S3，J22＝ L1C6S3S4-L1C3C4C6-L2C4C6-
L2C5S4S6-L1C3C5S4S6-L1C4C5S3S6，J32＝S5（L1S34+L2S4），J42＝C6S5，J52＝-S5S6，J62＝-C5。

第三列元素：J13＝L2C5C6S4-L2C4S6，J23＝-L2（C4C6+C5S4S6），J33＝
L2S5S4，J43＝C6S5，J53＝-S5S6，J63＝-C5。

第四列元素：J14＝0，J24＝0，J34＝0，J44＝C6S5，J54＝-S5S6，J64＝-C5。

第五列元素：J15＝0，J25＝0，J35＝0，J45＝S6，J55＝C6，J65＝0。

第六列元素：J16＝0，J26＝0，J36＝0，J46＝0，J56＝0，J66＝1。

得到雅可比矩阵行列式的表达式：
det（J）=-L1L2S5（L1C2S3-L2S2+L2C32S2+L2C2C3S3）（22）
经过化简，令行列式的值等于0，可得
-L1L2S5S3（L1C2+L2C23）＝0
最终获得 3 种机械臂的奇异位形：S5＝0，S3＝0，L1C2+L2C23＝0。

3 工作空间分析
机器人的工作空间可以定义为在机械臂结构下末端执行器能够到达的所有三维空间位置。

为了保证机械臂末端能够运动到目标点，需要对其工作空间进行分析。

串联机器人工作空间的分析基于Monte Carlo法，其基本思想是：当关节角度随机分
布在允许范围内时，遍历由其决定的末端执行器位置集合就是机器人的工作空间，通过数值模拟将工作空间表示为点云的形式。

在1.2章节中已证明UR机械臂末端执行器位置（px，py，pz）T只与前3个关
节角度相关，在MATLAB中用函数rand（N，1）生成步长，前3个关节的N×1 维随机角度变量由θi=θimin+（θimax-θimin）×rand（N，1）表示，关节角
度范围θ1∈[-π，π]，θ2∈[0，π]，θ3∈[0，π]；后3个关节角变量与末端执行
器位置无关，用zeros（N，1）表示。

将其代入末端执行器位置矢量（px，py，pz）T中，生成描述工作空间的点云。

由于Monte Carlo法是趋近于生成随机点
数值，随机点越多越能更真实地反映机器人工作空间，但相对应的运算时间也会增加。

本文选取N=10 000；连杆尺寸L1=120，L2=100，单位均为mm。

在三维坐标空间下UR机器人工作空间如图6所示。

图6 UR机械臂工作空间
4 轨迹规划
轨迹不同于路径，它更注重于时间线的描述，路径上的同一点，当到达时间不同时，相应的轨迹也不同。

对串联机器人的轨迹规划可分为关节空间的轨迹规划和直角坐标空间的轨迹规划，其目标是规划出每个关节角位置、速度、加速度的光滑连续曲线，使机械臂的运动更加平稳高效。

4.1 关节空间轨迹规划
假设串联机器人某关节在时刻0的角度为θ0，在时刻tf运动到了角度θf，建立
时间与位置参数的多项式或抛物线函数并对其求导，使初始和末端的边界条件与已知条件相匹配，通过待定系数法求多项式或抛物线函数的相应参数，并规划出关节位置、速度和加速度的平滑连续轨迹曲线。

在本文中，我们选取五次多项式来进行关节空间的轨迹规划：
为求解五次多项式系数需要6个边界限定条件：
经过计算，解出待定系数：
假设关节初始角度为15°并在3 s后转到了75°，初始速度和终止速度均为0，初
始加速度为5°/s2，末端加速度为-5°/s2，则通过五次多项式的轨迹规划就可以将其位置、速度、加速度变化曲线表示出来，如图7所示。

图7 关节空间的轨迹规划
在图7中可以看出位置、速度、加速度曲线满足边界约束条件且曲线光滑连续，
利用所求得的五次多项式可以规划任意条件下机械臂的关节变化情况。

但在实际应用中，还需考虑机械臂关节角加速度不超过其自身运动极限的情况。

在计算到达目标点的最短时间时，必须考虑加速度的限制。

加速度最大允许值[12]为
在假设当中，加速度曲线的最大值为36°/s2，而通过公式（25）计算得到的加速
度最大允许值为40°/s2，因此满足加速度约束条件。

4.2 直角坐标空间轨迹规划
机械臂在处理危险生化品的过程中，其运动更多涉及的是直角坐标空间的轨迹规划。

本文研究了当机械臂末端沿一定轨迹运动时各关节角的变化规律，轨迹上每个点都对应末端执行器的位姿，必须通过逆运动学将末端位姿转换成各关节角度的变化。

当末端运动轨迹为直线时，采用插值法在轨迹中插入坐标点，并分别计算各个点处的逆运动学解，最后生成关节角度变化曲线。

如果只考虑末端执行器的位置变化，则从初始位置到结束位置的线性插值只与前3个关节的变化有关。

以1.4章节中前2组实验数据为例，当机械臂从直角坐标空间T1运动到T2，前3个关节角的数值变化情况和角度变化曲线如图8所示。

图8 直角坐标空间的轨迹规划
5 结语
本文对危险生化品处理串联机器人进行了运动学分析和轨迹规划，将其应用在特殊领域代替人完成危险复杂的操作，同时提高工作效率和操作精度。

我们建立了UR 机械臂的数学模型并进行了运动学分析，通过计算雅可比矩阵找出了该串联机器人的3种奇异位形，应用MATLAB软件对其工作空间进行了仿真，轨迹规划采用五次多项式和插值法得到光滑连续的理想曲线。

本文的理论研究和计算对串联机器人在危险生化品处理方面的应用起到了推动作用，同时也为串联机器人的其他相关研
究提供了参考。

[参考文献]
【相关文献】
[1]CHEN Q C，ZHU S Q，WANG X Y，et al.Inverse kinematics sub-problem solution algorithm for serial robot based on screw theory[J].Journal of Zhejiang University：Engineering Science，2014，48（1）：8-14.
[2]KANG S Z，WU H T，LI Y，et al.Coordinated workspace analysis and trajectory planning of redundant dual-arm robot[C]//2016 13th International Conference on Ubiquitous Robots and Ambient Intelligence（URAI），August 19-22，2016，Xi′an，China.New York：IEEE，2016：178-183.
[3]RODRÍGUEZ C，MONTAÑO A，SUÁREZ R.P lanning manipulation movements of a
dual-arm system considering obstacle removing[J].Robot Auton Syst，2014，62（12）：1 816-1 826.
[4]HASAN A T，HAMOUDA A M S，ISMAIL N，et al.An adaptive-learning algorithm to solve the inverse kinematics problem of a 6 DOF serial robot manipulator[J].Adv Eng Softw，2006，37（7）：432-438.
[5]TABANDEH S，CLARK C M，MELEK W.A genetic algorithm approach to solve for multiple solutions of inverse kinematics using adaptive niching and clustering[C]//2006 IEEE Congress on Evolutionary Computation，July 16-21，2006，Vancouver，
Canada.New York：IEEE，2006：1 815-1 822.
[6]ZHAI J X，YAN W X，FU Z，et al.Kinematic analysis of a dual-arm humanoid cooking robot[C]//2012 International Conference on Mechatronics and Automation（ICMA），August 5-8，2012，Chengdu，China.NewYork：IEEE，2012：249-254.
[7]TAYLOR R H.A perspective on medical robotics[J].Proceedings of the IEEE，2006，94（9）：1 652-1 664.
[8]HARTENBERG R S，DENAVIT J.A kinematic notation for lower pair mechanisms based
on matrices[J].J Appl Mech，1955，77（2）：215-221.
[9]TRANQUILLI P，GLANDON S R，SARSHAR A，et al.Analytical Jacobian-vector products for the matrix-free time integration of partial differential equations[J].J Comput Appl Math，2017，310：213-223.
[10]FU G，FU J，XU Y，et al.Accuracy enhancement of five-axis machine tool based on
differential motion matrix：geometric error modeling，identification and compensation[J].Int J Mach Tool Manu，2015，89：170-181.
[11]MAHONEY A W，ABBOTT J J.Generating rotating magnetic fields with a single permanent magnet for propulsion of untethered magnetic devices in a lumen[J].IEEE Trans Robot，2014，30（2）：411-420.
[12]LIN Z，ZEMAN V，PATEL R V.On-line robot trajectory planning for catching a moving object[C]//1989 IEEE International Conference on Robotics and Automation，May 14-19，1989，Scottsdale，USA.New York：IEEE，1989：1 726-1 731.。