变插值周期五次多项式机器人关节空间规划

涂 孔，张 华

(浙江理工大学机械与自动控制学院，杭州 310018)



变插值周期五次多项式机器人关节空间规划

涂 孔，张 华

(浙江理工大学机械与自动控制学院，杭州 310018)

为了控制机器人平稳、精确地到达示教点，对机器人关节空间五次多项式插值函数进行了研究，提出了变插值周期的插值方法。将该插值算法应用于四自由度工业机器人，建立了基于D-H (Denavit-Hartenberg)坐标系的正逆运动学方程，并在运动控制系统中实现算法。通过示教得到始末点位姿，调用算法程序控制机器人到达示教终点。采集各轴编码器反馈脉冲值并计算得到各轴频率和加速度曲线，使用加速度传感器和百分表分别测试末端执行器运动速度和机器人的重复定位精度。试验结果表明：各轴频率和加速度曲线符合五次多项式函数速度、加速度曲线变化趋势，末端执行器速度变化连续，机器人运行平稳，重复定位精度为0.01 mm。

五次多项式；变插值周期；D-H坐标系；运动学方程

0 引 言

人工作业常会出现产品质量不稳定、效率低等问题。随着劳动力成本的增加，机器人代替人工作业已成为将来的发展趋势。轨迹规划[1]是机器人运动控制的重要内容，合理的规划方法可以使机器人快速、准确、平稳地到达目标点。轨迹规划可在关节空间或笛卡尔空间中进行。在关节空间中规划时，无需进行大量反解计算，插值速度快，精度能够满足一般作业要求[2-3]。故研究关节空间轨迹规划具有重要意义。

机器人在启动和停止时不能产生冲击，在运动过程中需保持平稳。王霆等[4]采用抛物线拟合的线性插值函数对关节空间进行规划，这种方法虽然速度曲线连续，但是因加速度存在突变，容易造成机器振动。基于多项式插值的曲线比基于线性插值的曲线更加平滑，没有突变。[5]常用的多项式插值曲线有三次、五次曲线。三次多项式插值加速度曲线不平滑[6]，五次多项式插值函数可实现平滑的加速度曲线，能够保证机器人运动的平稳性和精度[7-9]。虽然多项式插值的次数越大插值函数曲线越平滑，但是约束条件和计算量也越大，对处理器性能要求也越高。[10]并且在用多项式拟合曲线时，多项式次数越高，拟合多项式在插值区间端点处越容易产生震荡，即龙格现象[11]。本文综合考虑机器人运动的平稳性及约束条件和对处理器的要求，将五次多项式插值函数作为研究对象。为实现关节空间五次多项式轨迹规划，通过改变插值周期的方法对多项式函数进行插值。该方法易于理解，便于实现，可以控制机器人平稳、精确地运动。本文将该算法应用在四自由度工业机器人上，以验证该算法实现效果。

1 五次多项式插值算法及实现

1.1 五次多项式系数求解

五次多项式插值通式为:

θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(1)

其中：t为时间，s；θ(t)为t时刻对应的关节角，(°)；a0，a1，…，a5为系数。为了求取多项式的系数，必须满足如下约束条件：

(2)

(3)

1.2 插值算法实现

伺服电机旋转的角度由控制器发出的脉冲个数决定，电机转速由发出的脉冲频率决定。故需将示教位姿经运动学反解得到的关节角换算成对应的脉冲值，关节角随时间的变化转化为脉冲值随时间的变化，通过控制脉冲发出的个数和频率来控制机器人运动到达示教点。所以式(1)可转化为：

P(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(4)

其中：P(t)为t时刻对应的脉冲值。将方程离散化，在各插值点处的脉冲值为：

P(i)=a0+a1(iΔt)+a2(iΔt)2+a3(iΔt)3+a4(iΔt)4+a5(iΔt)5

(5)

其中：i为插值次数。Δt为插值周期，s。各插值点处的理论频率为：

f(i)=(P(i)-P(i-1))/Δt

(6)

脉冲输出形式采用PWM波输出，PWM是占空比可调的矩形波，非常适合电机的控制。控制系统的主控芯片为STM32F407。该芯片有丰富的定时器资源，大部分定时器都含有PWM输出通道，只需配置相关寄存器就可以输出PWM波，通过调整PWM波产生的周期可以调整脉冲输出频率。PWM波输出的实际脉冲频率值f由式(7)计算：

(7)

其中：fCLK为时钟源频率，Hz；ARR为控制脉冲周期的寄存器值。

由式(7)可知，当fCLK一定时，将式 (6)计算出的理论频率f(i)代入式(7)可得ARR的值，将ARR的值加载到定时器上即可输出对应的脉冲频率。但由于ARR寄存器是整型类型，故实际输出的脉冲频率会大于或等于理论脉冲频率，且理论频率越大，产生的误差越大。若每隔固定插补周期去改变输出脉冲频率，这就使单个插值周期内产生的脉冲数和目标脉冲数不等，从而造成脉冲个数输出的误差。

本文采用变插值周期的插值方法来保证单个插值周期输出的脉冲个数，即当到达各插值点脉冲值时计算下一个插值点所需频率值，通过控制插值点处脉冲值来调整插值周期。为了便于计算，式(5)以Δt代入计算，使插值点脉冲值始终落在曲线上，不会出现累计误差，实际插值时间是一个变化的接近Δt的数。

采用变插值周期的方法，不仅适用于五次多项式插值函数，也适用于任意已知解析式的简单插值函数。该方法控制简单，易于实现。

2 插值算法应用

2.1 四自由度机器人运动学方程

机器人运动学方程的求解是轨迹规划和控制的基础。本文研究的四自由度工业机器人有4个关节，分别为上下轴移动关节、摆臂轴转动关节、伸缩轴移动关节和校正轴转动关节。该机器人是一个圆柱坐标型机器人，4个关节均由伺服电机驱动。

运用D-H参数法构建机器人的坐标系如图1所示。图中d1为上下轴关节变化量，mm；θ1为摆臂轴关节变化量,(°)；d2为伸缩轴关节变化量,mm；θ2为校正轴关节变化量,(°)；L1=400 mm；L2=800 mm；L3=150 mm。

图1 四自由度工业机器人D-H坐标系

在图1坐标系下，机器人末端执行器中心在基坐标系0中的位姿矩阵为：

(8)

四自由度机器人的D-H参数见表1。

表1 四自由度工业机器人D-H参数

由D-H参数可得相邻两连杆的相对位姿矩阵分别为：

(9)

(10)

(11)

(12)

(13)

由式(8)-(13)可得机器人末端执行器正运动学方程为：

(14)

由正运动学方程(14)可解得逆运动学方程为：

(15)

本文用向量F(d1,θ1,d2,θ2)表示由逆运动学方程计算得到的解。

2.2 插值流程图

实现四自由度机器人位姿控制的五次多项式插值流程如图2所示。

图2 五次多项式插值流程

3 试 验

3.1 试验平台简介

为验证插值算法实现的效果，搭建了如图3所示的测试平台。由示教器示教得到起点、终点位姿。分两部分测试。第一部分由百分表测试重复定位精度，同时，在控制系统中编写程序采集各轴控制电机编码器反馈的脉冲值，并计算得到脉冲频率和加速度值。第二部分由加速度传感器采集末端执行器的速度，经数据采集仪处理后在电脑上实时显示。

1.四自由度工业机器人；2.加速度传感器；3.百分表；4.示教器；5.数据采集仪；6.电脑图3 试验平台

3.2 试验数据和分析

由示教器示教得到起点位置矢量p1(948.4962，-193.6945，420)，法线矢量n1(0.9397， -0.3420 ,0)，终点位置矢量p2(1194.6982，237.1066，420)，法线矢量n2(0.9397， 0.3420 ,0)。设定起点到终点运行时间为1 s，起点和终点速度、加速度为零。

由示教起点和示教终点的位姿经逆运动学计算后可得起点解向量F1(20，-10, 20，-10)，终点解向量F2(20，10, 270，10)。由解向量可知上下轴关节参数没有变化。

每隔10 ms采集伸缩、摆臂、校正轴编码器反馈的脉冲数，并计算各轴脉冲频率和加速度值，其曲线如图4所示。由图4可以看出，所实现的机器人各轴频率和加速度曲线符合五次多项式理论函数速度和加速度曲线的变化趋势。频率变化曲线平滑，加速度变化曲线没有突变但存在较小的波动。伸缩轴频率和加速度曲线较其他轴变化幅度大，这和示教起点到终点伸缩轴关节参数变化较大相符。图4中各轴运行时间为1 s ,起点和终点处速度为零，可达到同时启停。

图4 伸缩、摆臂、校正轴的频率、加速度变化曲线

加速度传感器测得末端执行器x、y、z方向速度变化如图5所示。由图5可以看出，末端执行器速度曲线总体平滑，z方向速度在0 mm/s附近波动。产生波动的原因由机器人杆件弹性变形及测量误差等因素造成。试验过程中，机器人能够平稳地运行。

图5 末端执行器x、y、z方向速度曲线

机器人在示教起点和示教终点之间往复运动20次，百分表测得重复定位精度为±0.01 mm，且试验数据无累计误差，可满足一般工业要求。

4 结 论

本文给出了关节空间五次多项式插值详细的实现过程，并提出了一种变周期插值方法，该方法易于实现，控制简单，无累计误差，适用于任意已知解析式的简单插值函数。本文推导了一种四自由度工业机器人的运动学方程，为具有相似结构的机器人提供了参考。将该算法应用于四自由度工业机器人上，试验表明，机器人运动平稳，重复定位精度为±0.01mm。同时也验证了五次多项式插值算法可平稳地控制机器人运动。

[1] 刘极峰,易际明.机器人技术基础[M].北京:高等教育出版社,2003:81-83.

[2] 李伟光,许阳钊.搬运机器人的轨迹规划[J].组合机床与自动化加工技术,2010(5):83-85.

[3] 李万莉,陈熙巍,茹兰.基于SimMechanics的4自由度机器人的轨迹规划和仿真系统设计[J].中国工程机械学报,2008,6(2):144-148.

[4] 王霆,管小清.抛物线轨迹规划在工业机器人中的应用[J].机电产品开发与创新,2012,25(3):18-24.

[5] 屠海斌,张侃健,方仕雄,等.四自由度机器人轨迹规划算法研究与实现[J].工业控制计算机,2015,28(5):120-125.

[6] 孙亮,马江,阮晓钢.六自由度机械臂轨迹规划与仿真研究[J].控制工程,2010,17(3):388-392.

[7] 黎波,严骏,郭刚,等.挖掘机工作装置复杂轨迹跟踪实验[J].农业机械学报,2012,43(12):39-43.

[8] 刘凉,陈超英.考虑关节摩擦的并联机器人平滑轨迹规划[J].机械工程学报,2014,50(19):9-15.

[9]GOSSELINC,FOUCAULTS.Dynamicpoint-to-pointtrajectoryplanningofatwo-DOFcable-suspendedparallelrobot[J].IEEETransactionsonRobotics,2014,30(3):728-736.

[10] 刘鹏,张永贵,刘晨荣,等.Motomanup50机器人的轨迹规划及仿真研究[J].机械制造与自动化,2016(3):162-164.

[11] 寿媛,陈豫眉.利用MQ拟插值解决高次插值所出现的龙格现象[J].洛阳师范学院学报,2016,35(5):6-9.

[12] 蔡自兴.机器人学[M].北京:清华大学出版社,2000:46-47.

(责任编辑： 康 锋)

Robot Joint Space Planning Based on Quintic Polynomial of Variable Interpolation Cycle

TUKong,ZHANGHua

(Faculty of Mechanical Engineering & Automation, Zhejiang Sci-Tech University, Hangzhou 310018, China)

In order to control robot to reach the teaching point smoothly and accurately, the quintic polynomial interpolating function of robot joint space was studied, and this paper proposed an interpolation method of variable interpolation cycle. This interpolation algorithm was applied to a 4-DOF (4 degrees of freedom) industrial robot, and the direct and the inverse kinematics equations were established based on D-H (Denavit-Hartenberg) coordinate system. The interpolation algorithm was implemented in the motion control system. The poses of starting point and termination point were obtained by teaching, then the algorithm program was called to control the robot to reach the terminal point of teaching. The encoder feedback pulse values of each axis were collected to draw the joint frequency and acceleration curve. The acceleration sensor and dial indicator were used to test the velocity of terminal manipulator and repeated positioning accuracy of the robot. The experiment results showed that the joint frequency and acceleration curve conform to the change trend of quintic polynomial velocity and acceleration curve; the velocity change of terminal manipulator is continuous, and the robot moves smoothly; the repeated positioning accuracy is 0.01 mm.

quintic polynomial; variable interpolation cycle; D-H coordinate system; kinematics equation

10.3969/j.issn.1673-3851.2017.05.012

2016-09-20 网络出版日期：2017-01-03

国家自然科学基金项目(51307151)；浙江理工大学科研启动基金项目(13022155-Y)；机械工程浙江省高校重中之重(一级)学科优秀青年人才培养基金项目(ZSTUME01B07)；浙江省科技厅公益技术研究工业项目(2017C31036)

涂 孔(1990-)，男，安徽郎溪人，硕士研究生，主要从事机器人控制方面的研究。

张 华，E-mail: zhanghua@zstu.edu.cn

TP242.2

A

1673- 3851 (2017) 03- 0376- 05