基于直插补模型的机器人机械臂运动控制轨迹智能修正研究

刘 力

(延安职业技术学院机电工程系，陕西 延安 716000)

机器人是“工业4.0”时代的产物[1]，也是现代制造技术、信息技术、计算机科学技术、电气自动化控制技术、无线传感技术等多学科融合发展的成果[2]。随着机器人机械臂工作效率、控制精度的不断改善与提高，其在高精度工业制造、军工国防、航空航天、医疗卫生、农业生产等领域有着越来越广泛的应用[3]。机械臂移动轨迹规划与补偿控制会直接影响其最终的控制精度[4]，因此一直以来轨迹控制都是该领域的研究热点之一。机械臂轨迹控制首先要精确计算出机械臂末端执行器的空间坐标值、关节旋转角度偏差值，然后再利用插值补偿算法以理想的运动轨迹为基准[5]，实现对机械臂运动轨迹的实时修正。汇总国内外现有的研究成果可以发现，在机器人机械臂轨迹规划与智能修正方面主要采用PID(比例、积分、微分)控制、模糊控制、人工智能仿生控制及多项式插值控制等多种算法。

PID控制是工业控制领域的经典算法之一，具有算法简洁、稳定性强等优点，但PID控制在控制精度、适用性等方面有较强的局限性，不适用于复杂度较高的机械臂系统[6]；模糊控制是处理非线性问题的有效方案之一，但模糊控制的局限性在于模糊规则的制定与模糊隶属度函数的选择过于主观，往往会影响最终判定结果的客观性[7]；人工智能控制是近年来工业控制领域的研究热点和主流方向，人工智能控制多采用模拟人脑、人体神经网络、动物种群活动等特征，实现一种动态化的寻优和求解[8]，人工智能算法在故障检测领域的应用范围和应用效果远优于工业控制领域，但在全局控制过程中容易陷入局部最优解[9]；基于多项式的插值控制具有良好的控制精度，但整个控制过程的算法过于复杂，往往不具有实际应用价值或可操作性[10]。直线插补模型是一种精度极高的点群逼近方式[11]，本文将直线插补经典算法应用到机械臂轨迹控制领域，并对算法的插补方式进行优化，即采用多段逼近的方式，在不增加算法复杂度的情况下改善机械臂轨迹的智能修正控制精度。

1 机械臂动力学模型建立及正向运动求解

1.1 机械臂空间坐标分析

机器人机械臂运动轨迹的智能修正与实时纠偏是在机械臂动力学模型基础上完成的，轨迹的修正是在局部、小范围内进行的，如果运动中的轨迹偏差过大应重新调整系统程序。建立机械臂连杆坐标系时涉及到的系统参数包括连杆长度Li、旋转角度βi、连杆的偏距ηi、关节的角度θi，各参数的空间位置及变换关系如图1所示。

图1 机械臂连杆、关节变化示意图

确定了机械臂第i个关节在空间的坐标维度后，根据机械臂连杆与关节的空间位置关系，建立每一个关节和连杆的空间坐标系，并进行广义上的坐标变换。机械臂连杆和关节的运动方程可以基于坐标变换矩阵推导得到。例如从机械臂连杆(i-1)到连杆i的空间坐标矩阵变换过程如下：绕zi-1轴旋转βi角度，此时xi-1轴与xi轴旋转到同一个平面；zi-1轴在空间内平移ηi-1个单位的距离，使xi-1轴与xi轴重合；平移连杆(i-1)使其坐标系与连杆i的坐标系原点重合，此时旋转xi-1轴，使空间坐标轴zi-1与zi重合。通过机器人机械臂连杆和关节的坐标变换，可以得到空间范围内连杆与关节的齐次坐标变化矩阵Hi：

Hi=

(1)

(2)

机械臂正运动求解过程主要利用D-H矩阵求出各关节和连杆的齐次坐标变化矩阵，再利用式(2)确定机械臂末端执行器的空间位置和姿态，此时式(2)可以改写为：

(3)

式中：ζ,o,L,τ分别为关节旋转角度、坐标中心移动距离、机械连杆位移和关节偏置角度。

矩阵中的各项参数可利用式(1)和式(2)中的参数化简得出，在正向求解中只要当机器人机械臂各关节参数、关节旋转角度、连杆位移确定之后，末端执行器的空间位姿矩阵参数也随之确定。

1.2 基于直插补模型的机械臂轨迹修正

机械臂轨迹规划过程中，末端执行器的行进路径受到连杆位移、移动速度、加速度等多种参数的影响及状态约束。在笛卡尔空间坐标系内，机械臂按设定轨迹行进,不仅要考虑到躲避障碍物造成的轨迹偏移，还要关注关节奇异、连杆不连续、系统误差、控制信号延迟等因素的影响所导致的运动轨迹偏差。直线插补模型采用足够短的直线段拟合和纠正末端执行器的轨迹偏差，为满足纠偏模型弓高误差最小化要求，本文采用内接弦线法调整线段的长度，具体拟合过程如图2所示。

图2 内接弦线法拟合过程

图中点P1和P2是机械臂末端执行器运动轨迹上的任意两点，当弓高误差趋近于零时，P1和P2两点趋于重合，即线段P1P2的长度趋近于零。如果末端执行器运动轨迹出现偏差，则采用直线插补法拟合曲线，纠正原有的弧形轨迹路线。设行进轨迹曲线的曲率半径为r，则线段P1P2与弧线之间的最大弓高误差d为：

(4)

式中：L为末端执行器连杆长度。

最大弓高误差必须小于轨迹曲线的曲率半径离散精度κ：

r2-2rκ+κ2≤r2-(L/2)2

(5)

当离散精度κ很小时，忽略κ2，可以推导出：

(6)

随着拟合精度的不断提高，插入的线段PiPj的长度越来越小。将一组PiPj线段拟合成为新的轨迹曲线。在直线插补过程中通常以固定的时间周期逼近新的行进轨迹，但机械臂速度或加速度过大会导致弓高误差过大，从而使修正后的轨迹精度降低。直线插补程序以固定的插补周期为基础，在每个采样周期内都要根据机械臂的行进速度、加速度、位移总量确定出下一个插补周期的位移增量、角度增量信息。

在精插补过程中仍旧以图2所示的二维平面内的线段P1P2为例，其中P1为起点，P2为终点，给定一个从P1到P2方向的速度v，为重新拟合机械臂行进轨迹曲线，在线段P1P2上取一点Pk作为精插补点，取Pk+1为Pk的下一个精插补点，Δx和Δy分别为与x轴和y轴平行的进给向量分量，则Pk与Pk+1之间的距离ΔL为Δx与Δy之和。在整个插补周期T之内，直线段被分成n段，设线段P1P2的长度为D，则：

D=nΔL

(7)

采用连续小直线段插补不仅能够最大限度地降低最大弓高误差，也可以通过多次加速进给提高轨迹的拟合精度。采用基于多线段直插补的方式能够满足机械臂末端执行器精确移动要求，即使出现了移动轨迹偏差，通过对多个直线插补线段的曲线拟合，也可以连续修正轨迹的偏差。仍以6R机械臂为例，当末端执行器倾斜移动时需要同时对第1～4关节进行连续的空间补偿，以抵销关节之间移动所造成的关节轨迹偏差。计算合适的直线插补位移量，保证每个关节移动过程中弓高误差均被控制在较低的值，即拟合后的轨迹曲率与精度满足机械臂运行的精度要求。线段P1P2的轨迹拟合方程为：

(8)

式中：xP1,yP1和xP2，yP2为P1和P2点的坐标值；f(x,y)为轨迹坐标拟合方程。根据机械臂控制精度的要求，在线段P1P2插入(n-1)个分割点，将线段分成n段，则x轴和y轴方向上的进给向量分量Δx和Δy分别为：

(9)

其中第k个精插补点Pk的坐标值为：

(10)

如果在线段P1P2上插入m个精插补点，那么与线段P1P2相对应的机械臂移动轨迹总长度L′为：

(11)

在实际的机械臂直线插补轨迹控制中，以脉冲信号控制机械臂系统的伺服马达，为了与精插补点的拟合过程相一致，输入的脉冲信号数量必须与精插补点的数量m相对应，且恒为整数。清除机械臂控制系统中已经设定的运动轨迹，按照新拟合的轨迹调整连杆、关节的旋转角度、位移等，系统原设定的运动速度、关节加速度等参数也要做相应的调整，最后按新的轨迹运行。修正后的机械臂运功轨迹更趋近于理论值，从而使机械臂末端执行器的控制精度得到本质上的改善。

2 实验

2.1 实验平台

本文实验以6R模块化机器人机械臂为研究对象，使用无线传感器采集机械臂各关节的旋转角度和连杆位移，通过上位机实时观测与修正机械臂移动过程中的轨迹。上位机的配置为：Intel core i7处理器，工作主频3.6 GHz，运行内存和数据存储内存分别为8 GB和512 GB。6R机械臂的结构设计如图3所示。

图3 实验用6R机械臂系统结构图

机械臂各关节与连杆的接触点都安装了运动稳定装置和无线传感器，一方面能够减轻关节结构之间的相互影响与摩擦干扰，保证机械臂按照事先设定好的轨迹路线行进；另一方面如果机械臂在某一段轨迹由于复杂因素发生了偏移，传感器会在第一时间预警并将发生偏移的数据传递到上位机，以便及时纠偏。

2.2 机械臂运动轨迹修正效果验证

在上位机中安装MATLAB仿真软件，取如图4所示的P0-P1段机械臂末端执行器的行进曲线，对机械臂的运动过程进行仿真，其中P2～P5为行进轨迹起止点之间的中间点。

图4 机械臂末端行进轨迹图

机器人机械臂行进轨迹上P0～P5点的空间坐标见表1。

表1 机械臂轨迹采样点的空间坐标 单位:mm

机械臂关节终端传感器的数据反馈显示，机械臂末端执行装置从P2点开始出现轨迹偏离，即曲线P2P3、P3P4和P4P5均出现了不同程度的偏离。以P4P5段为例分析纠偏效果，如图5所示。

图5 直线插补曲线修正效果图

P4P5直线插补修正结果显示，直线插补拟合的14条线段与曲线的弓高误差最大值仅为0.01 mm，表明经过轨迹修整和线段拟合之后的机器人机械臂行进轨迹，更接近于理想状态下的行进轨迹。

2.3 机械臂运动轨迹偏差及控制复杂度对比

机械臂末端执行器的轨迹偏差由机械臂各个关节旋转角度及连杆位移的偏差导致，在轨迹智能修正过程中关节和连杆的角度、位移偏差会逐渐缩小，偏差值越小表明修正的效果越好。在图4所示的P0-P5段轨迹修复过程中，观测各关节和连杆的角度偏差，同时引入经典PID控制算法和模糊控制算法参与对比。关节旋转角度偏差值对比见表2。

表2 机械臂关节旋转角度偏差 单位:(°)

由表2可知，经过直线插补模型轨迹修正后的关节角度偏差相比传统PID控制和模糊控制修正方法的偏差值更低，具有更好的轨迹修正效果。修正算法的复杂度也是衡量算法性能的重要指标，在算法复杂度测试中引入人工智能体系中的深度学习算法和多项式插值算法进行轨迹修正，轨迹修正完成后各种算法的迭代循环次数如图6所示。

图6 修正算法的复杂度分析

由图可以看出，深度学习修正算法完成轨迹修正的迭代次数为37次，多项式插值迭代算法迭代次数达到了43次，都远高于直线插补修正方法，证明直线插补模型算法更加简洁。

3 结束语

机器人机械臂在运动中会出现偏离预定理想轨迹的现象，本文采用计算复杂度更低的直线插补模型纠正机械臂运动中的轨迹偏移。直线插补模型采用长度很小的直线段拟合和纠正机械臂关节旋转角度及连杆的位移偏差，并将最大弓高误差控制在允许的最小范围内，采用该模型拟合后的曲线更接近于理想轨迹。实验结果表明，直线插补模型的轨迹修正效果良好，计算的复杂度更低。