55
3C机器人轨迹规划算法研宄与仿真
张建政董易
(上海福赛特机器人有限公司，上海200233)
摘要：随着国内人力成本的提高，传统的劳动密集型产业如3C产业，对自动化升级产生了巨大需求。本 文针对3C机器人的应用要求，对轨迹规划方法展开了研究。为了提高机器人效率，本文建立机器人在笛卡尔空 间及关节空间的轨迹规划及圆滑方法。为了验证轨迹规划方法的有效性，采取Madab-SolidWorks搭建联合仿真 环境，并对算法进行验证。结果表明，这套算法可以较好地使用在3C产品生产中。
关键词：3
C机器人运动学轨迹规划轨迹圆滑四元数
套轨迹规划算法。该算法包含直线指令（MoveL)、圆弧指 令（Circle)、关节空间运动指令（MoveJ)。为了提高机 器人运动节拍，两条指令之间通过轨迹圆滑提髙运动的节 拍，轨迹规划的算法结构如图2所示。
法兰
3C 是电脑（Computer)、通讯（Communication)和消费 性电子（Consumer Electronic)的合称。我国是3C产品的生 产大国，自主品牌和代工产品的产量占世界总产量的70%，并 且以非常迅猛的速度发展。但是，我国国内厂商自动化水平 较低，依旧依靠大量的劳动力进行人工操作。随着我国人口 红利的逐渐消失，3C厂商面临人力资源短缺和成本上升的难 题，对以工业机器人为代表的自动化设备产生了迫切需求。 工业机器人的广泛应用有助于生产企业降低人力成本，提 髙生产效率和产品质量，更快地应对市场的变化[1_2]。
3C产品产量巨大且更新速度快，零部件小且精密，生 产环境复杂。因此，在生产中对于机器人的速度、精度和 灵活度有更髙的要求。这不仅需要机器人硬件提供保证， 同时需要有一套合适的轨迹规划策略和算法。很多文献己 经对机器人轨迹规划方法展开了深入的理论研究[3_5]，但较 少针对产品及工艺进行开发。本文根据这些性能需求，对 产品的轨迹规划的策略和关键算法展开研宄，建立了一套 算法，并利用Matlab-Solidworks联合仿真验证其有效性。1 3
图1桌面型工业机器人3D模型
C机器人
面向3C行业的制造工艺要求，上海福赛特机器人公司 自主设计开发了一款桌面型工业机器人。该机器人主要采 用轻量化和一体化的设计思路，产品结构紧凑、外形美观， 满足髙速、髙精度运动的硬件条件。
该款机器人采用典型的6转动关节串联形式构型，具 有结构紧凑、占地空间小、动作灵活、工作空间大和末端 速度髙等优点，在自动化生产中通用化程度髙。机器人的 底座、转座、大臂、肘部、小臂、腕部和末端法兰等部分 通过转动关节连接，通过伺服电机及谐波减速机进行驱动， 3D模型如图1所示。末端法兰根据国家标准[6]制孔，便于 安装多种工具，以满足不同的工艺需求。2
轨迹规划策略和算法实现
由于3C产品的大多数零件小且精密，加工过程中要求 工业机器人动作灵活，轨迹准确且平滑，无冲击无抖动。 为了实现这一要求，本文重点讨论轨迹规划的架构及其中 的几个关键问题。2.1轨迹规划算法结构
本文针对3C产品生产中的工艺应用情境，开发出了一
图2
轨迹算法程序结构
三种不同的指令之间共有9种圆滑组合。由于每种指 令所需要的初始条件不同和指令之间的过渡方式不同，本文将其分为三类。
(1)直线指令之间的过渡。直线之间选择采用圆弧进行过渡。圆弧半径可根据过渡速度进行计算，轨迹类型简 单可控。
(2)
关节空间与直线、圆弧之间的过渡。由于关节空
间规划不要求在笛卡尔空间的具体位置，本身即可视为一种 过渡轨迹。和其他两种类型的轨迹衔接时，直线/圆弧轨迹 不再单独划出过渡过程，而是按照自身命令计算出初始或结 尾处关节的位置和速度，以作为关节的目标或初始值。
(3) 直线与圆弧之间的过渡、圆弧指令间的过渡。两
56
现代制造技术与裝备
2017第3期总第244期
条空间直线和圆弧，一般情况下并不共面，难以找到一个 简单曲线实现过渡，本文中采用关节空间过渡方法用来实 现两条笛卡尔空间轨迹的圆滑，圆滑效果好且实现方便。2.2四元数姿态平滑算法
在描述机器人位姿时，姿态的圆滑和角速度连续性是 轨迹规划中的难点。本文也重点关注如何解决此问题。两 个四元数姿态之间可以采用球面线性插值方法（Slerp)进 行插值，以实现最短。
sin汐
其中，中间控制变量ue [0，1]，0为两姿态间夹角。而当姿态的节点多于2个时，则需要通过球面样条插 值q（Squad)进行插值，以保证姿态圆滑，基本公式为：
(u) -Slerp{Slerp{qt, qi+l, u), Slerp{st, si+l, u), 2u{\\~u))
(2)
'log(d
) + log(d
)、
以
exp\\
-----------s----------
y
其中，Si为构造出的中间四元数。
本文提出采用多项式插值的方法分段建立起中间变量u 和时u间t的关系：
=fi(t),t^ [ti ti+l]
(4)
通过函数及其导数的连续，保证多段轨迹中姿态角、 角速度的连续。
2.3移动和转动协调方法
针对直线指令中的小位移大转动情况，更极端的情况 为定点转动。如果单独根据移动所需要时间计算，可能会 出现速度过髙甚至出错的情况。为了解决这种问题，本文 所采用的算法设定了参考的最大线速度和角速度，并比较 移动和转动所需要的时间，选取其中较大者进行规划。其中， 移动的时间可以根据距离、初速度、目标速度、末速度和 加速度信息，通过插值程序精确求出：
t =Interp(d, v〇, vobp vendJ a)
(5)
而转动所需要的时间则需要根据目标姿态和合理的角 速度进行估算。
为了保证转动和直线运动具有相近的运动节奏，算法 预设了量级相近的参考线速度和参考角速度在给 定线速度V时，自动给出隐含的角速度：
(6)
V
转动所需要的时间可简单估算为:
t = k^CO
⑴
其中，系数k可以根据实际需要进行调整。直线轨迹的程序流程如图3所示。2.4非共面轨迹间的过渡
两条连续的直线轨迹肯定共面，而直线与圆弧之间、 圆弧与圆弧之间则存在不共面的可能性，难以用简单的曲
线进行圆滑。因此，本文采用在关节空间内过渡的方法对 其进行圆滑，如图4所示。
MoveL
图3
直线轨迹程序流程图
直线轨迹由AB两点决定，圆弧轨迹由BCD三点决定， 四点并不共面。以B点为中心，划定半径为r的球形圆滑
边界。当轨迹进入圆滑边界时开始圆滑，与轨迹的交点B1 和B2为圆滑的切入、切出点。边界半径由速度和直线与圆 弧切线之间的夹角决定：
厂—
(8)
«maxtan(9
根据B1和B2点对应的各关节的位置和速度，分别作
为关节空间规划的起点和目标点状态。当直线轨迹规划到 B1时，机器人转以关节空间运动模式按三次曲线运动到 B2,从而实现轨迹的圆滑。3 3C产品生产工艺案例仿真
为了通过逼真的三维可视化效果验证这套规划算法的 有效性，本文采用Matlab和Solidworks联合仿真[7_8]的 方法对机器人进行仿真。Matlab和Simulink具有强大的 科学计算、编程和仿真能力，在工程界得到了广泛应用， 而SolidWorks是专业的建模软件，它们结合可以搭建出灵 活直观的三维仿真环境。
设计与研究
57
在SolidWorks中建立机器人装配体的3D模型后， 通过SimMechanics插件导出为.xml格式，并导入到 Simulink中。设置各个部件与关节之间的关系，模型从 Matlab程序计算结果中读取数据，输出机器人的3D模型动 作，同时显示各轴的位置、速度、加速度曲线，如图5所示。
搭建完成仿真环境后，本文通过几个典型的案例对算
4
法中的要点进行仿真验证。
定点转动圆滑仿真
为了验证移动和转动的协调性，本文选择小移动大转 动的一种极限情况一一定点转动作为案例进行仿真。三个 姿态两两之间的夹角为30°、56.4°和46.6°，仿真结果 如图6所示。其中，轨迹三维图中标示出了工具坐标系xyz 轴的指向。整个过程中，位置保持不动，姿态过渡圆滑， 角速度连续，且数值大小合理，关节的角速度、加速度在 限制范围内。
角速度角加速度
时间（S) 各关节加速度
时间（S)时间（S)
图6定点转动仿真
5
直线与圆弧轨迹过渡仿真
本文选择直线和圆弧的过渡作为最后一个算例。整个
过程中，直线轨迹和圆弧轨迹匀速过程的速度一致且恒定， 中间有一个关节空间的过渡，对应的各个关节速度连续， 加速度不超过限定值。
6
结语
针对3C行业制造工艺的实际需求，本文提出了一种新 型的工业机器人。该机器人外形美观，结构紧凑小巧，荷 重比高，运动快速，精度高。为了提高机器人在3C行业中 使用的效率，本文针对3C生产工艺开发了一套轨迹规划的 策略及算法，重点介绍了算法中的几个难点问题的解决方法， 并通过Matlab-Solidworks搭建联合仿真环境，用算例对算 法进行验证。结果表明，本文提出的机器人可以在3C行业 中用于多种工艺环境，满足高速、高精度需求，具有广阔的 应用前景，对小型工业机器人开发也具有一定的意义。
参考文献
[1] 梁文莉.机器人行业的新秀一一3C机器人[J].机器人技术与
应用，2015， (1) ：[2] 23-24.
赵凤娇，李彬，张彦云.机器人技术在3C产业中的应用[J]. 机器人技术与应用，2016，[3] (1) ： 24-26.
任敬轶，孙汉旭.一种新颖的笛卡尔空间轨迹规划方法[J]. 机器人，[4] 2002，24 (3).
刘宏伟，王效杰，梁艳阳，等.机器人末端执行器姿态轨迹规 划研宄[5] [J].机械设计与制造，2015，(4) ： 28-30.
徐海黎，解祥荣，庄健，等.工业机器人的最优时间与最优能 量轨迹规划[6] CN-GB.[J].机械工程学报，2010, 46 (9) : 19-25.工业机器人.机械接口，第.2部分，轴类[7] [S].2006.陈爱波，宁淑荣，陈五一，等基于MATLAB与SolidWorks的 六自由度机器人联合建模仿真[J].机电技术，2014，(6)： [8] 57-60.
杨林，李霆，黄桂武，等.6R机器人的联合仿真[J].机电工 程技术，2013，(4) ： 60-64.
for 3C Industrial Robot
Study and Simulation Trajectory Planning Algorithm ZHANG Jianzheng, DONG Yi
(Shanghai Foresight robotics Go,Ltd, Shanghai 200233) in traditional labor intensive industries, such as 3C industry, as the Abstract： The demands for automation upgrade is stressed labor cost rising. Aiming at the robot application in 3C industry, the trajectory planning algorithm study is conducted in this paper. In order to raise the efficiency, the trajectory planning and smoothing methods are derived. And Matlab-Simulink combined simulation approach is built to verify the derived methods. The results show that the trajectory planning method in this typical 3C production process.
paper is suitable in Key words： smoothing, quaternion
3C robot, kinematics, trajectory plan, trajectory
因篇幅问题不能全部显示，请点此查看更多更全内容