基于三次B样条的六自由度液压机械臂轨迹规划

刘俊辉 ，周伟

(1.平顶山技师学院现代制造系，河南平顶山 467000； 2.郑州机械研究所有限公司，河南郑州 450001)

0 前言

当前，在自动化控制技术和计算机软硬件技术的推动下，工业机械臂的应用场景越来越多。工业机械臂在产品加工精度、产品生产的一致性、高强度作业时长、持续工作时长及作业安全性等方面，与传统的人工作业、半自动加工方式相比优势十分明显，显著提升了产品的加工精度和生产效率。六自由度机械臂经常被应用于高精度的加工场景，但由于大多数机械臂关节运动大都采用电机和减速器相结合的驱动方式，机械臂的负重比较小，无法适用于大力矩的输出场景，因此限制了机械臂应用范围的进一步拓展。与电机驱动关节相比，液压系统具有更大的负载能力和更稳定的传动平稳性，可以有效提高机械臂的负重比，并增加六自由度机械臂的应用场景。但六轴液压机械臂在使用中也存在一定弊端，包括液压系统的响应迟滞及由于液压关节系统给末端执行器带来的轨迹偏差。

关于液压关节机械臂轨迹偏差的修正问题，国内外专家和学者进行了广泛和深入的研究。文献[8]提出一种基于机械臂力矩计算的数学建模方法，通过控制运动中的机械臂各物理参数，动态修正末端执行器的轨迹偏差。但机械臂关节在高负载的场景会存在严重的扰动现象，各核心参数的提取难度较大，影响模型的精确性。文献[9]提出一种基于模糊自适应控制的轨迹修正方案，但该算法要求将实时的机械臂移动数据和轨迹修正数据传递给后台控制系统，计算复杂度和控制成本过高，无法推广到工业领域。文献[10]提出以仿生算法为基础的模糊免疫PID控制算法，并结合线性插值优化机械臂的运动轨迹，但该控制方案的控制精度稳定性低。分析现有控制算法的不足，并综合考虑控制成本、算法复杂度、控制精度及推广价值等因素，本文作者提出基于三次B样条的六自由度机械臂轨迹修正方案。三次B样条曲线修正的优势在于，它兼具了凸包性、三阶可微及控制参数表达相对简洁等优势，可实现对机械臂末端执行器速度、加速度和位置移动轨迹的周期性插补，有效控制轨迹偏差。

1 液压机械臂空间运动求解与轨迹误差分析

六自由度机械臂的控制系统通过输入液压摆缸关节的指令，控制机械臂完成复杂的期望动作。机械臂末端执行器的运动轨迹，受各液压关节空间坐标和位姿的影响。利用改进的D-H参数法，建立机械臂各液压关节的坐标系，并对各关节角作正向和逆向求解。对经典的D-H算法进行优化，针对每个液压关节单独构建坐标系，且保持所构建的坐标系与六自由度机械臂关节一一对应。经过优化和改进的笛卡尔空间内六轴机械臂的坐标设定如图1所示。

图1 改进D-H算法下的机械臂关节坐标空间标定

其中:和+1分别为液压机械臂关节和关节+1所对应的坐标系原点；+1为关节+1围绕轴的夹角；为液压机械臂关节对应连杆的扭角；为关节对应的连杆长度；+1为连杆+1所对应的偏置。如果给定某一关节对应的连杆长度，可对应求出其他机械臂液压关节对应的D-H参数。如果设为350 mm，则对应的其他关节参数如表1所示。

表1 液压机械臂参数(a3=350 mm)

依据表1可依次计算出相对于机械臂基座的各液压关节空间位姿，进而得到各关节空间位姿的变换矩阵。关节变换矩阵能够描述出机械臂的空间位姿的正向运动情况，用表示关节1的空间位姿矩阵，则有：

(1)

同理可分别求解出六自由度机械臂液压关节在笛卡尔空间内的位姿矩阵～。机械臂末端执行器的空间位姿由6个液压关节空间位姿决定，将6个关节空间位姿矩阵相乘即可得到末端执行器的空间位姿和运动轨迹。液压机械臂存在关节柔性，机械臂关节绕轴旋转会导致关节角增量的产生。每个关节角增量对应一个偏移误差矩阵，具体表示为

(2)

其中:为第个液压关节对应的旋转误差对应的偏离角。通过掌握六轴机械臂液压控制系统的周期性运动规律，能够有效控制转角误差，并抑制轨迹偏差。液压机械臂在关节结构设计上与传统电机驱动关节不同，其液压缸输出轴与连杆直接连接，液压缸的响应滞后问题会放大机械臂关节的转角误差。在固定的关节转角运动度数前提下，需要通过实时调节和不断的轨迹修正保持末端执行器的控制精度。

2 基于三次B样条的液压机械臂轨迹修正

六自由度机械臂能够完成更复杂和更高精度的指令动作，但现有机械臂在行进轨迹插值模式下，很难提高对轨迹局部的控制稳定性。B样条函数的理论基础源于贝塞尔曲线，在保留贝塞尔曲线的几何不变性等优点的同时，B样条函数还能够有效地对机械臂轨迹作局部规划与优化。

2.1 三次B样条函数的构造

在六自由度液压机械臂运动过程中，如果某个液压关节位置序列点的空间坐标发生偏差，仅会影响到运动轨迹的一部分，若对偏差及时修正则并不会影响到整个机械臂的运动轨迹控制精度。次B样条函数()可以表示为

(3)

式中：,()为控制顶点对应的次B样条基函数，B样条基函数可由德布尔递推公式得出;为个型值点中第个控制顶点；为B样条函数的节点向量。当迭代次数为时，控制顶点的数量满足如下关系式：

=-+1

(4)

式中：为机械臂液压关节的数量，在区间[,++1]内，控制顶点位置的变化并不会导致区域内机械臂轨迹出现偏差。B样条函数将液压机械臂的移动轨迹分割为多段曲线，基函数,()在分段曲线上满足连续可微的要求，B样条将折线控制在凸包范围内，()函数上的控制点-、-+1、…、都包括在凸包内，控制顶点可以表示为

(5)

当取值为3时，三次B样条函数()可以表示为

(6)

按照D-H参数表(表1)相关数据，可以计算出当前状态下液压机械臂各关节的空间位姿和末端执行器的空间位置坐标，在笛卡尔空间范围内同时能够监测到出现轨迹偏差的区域，并描绘出相关的示教点，生成对应的三次B样条曲线。将规划出的三次B样条曲线作为笛卡尔空间内的机械臂目标轨迹，目的是利用曲线的平滑功能控制机械臂各关节、末端执行器的位置偏差，并控制液压机械臂的移动速度、加速度。所构造的三次B样条节点向量满足公式(4)的数量要求，由于取值为3，所以三次B样条中间顶点的间距为1(-2)，则B样条函数的节点向量表示为

(7)

根据B样条曲线中当前节点向量的分布情况，可确定出相关的函数参数。针对三次B样条曲线拟合的特征而言，每相邻的曲线共用控制点，以控制点为中心构造首末端点。具体步骤如下：要求出相邻示教点的距离，并累计求弦长之和；根据弦长的比例进一步分析出示教点的分布情况；依据公式(7)求出B样条函数节点向量的表达式。

2.2 液压机械臂空间轨迹过渡与插值

三次B样条曲线函数是关于向量的表达式，应根据机械臂的规划速度和实际位移，得到向量随时间变化的数值序列，进而获取三次B样条曲线函数的空间位置点。由B样条曲线函数的特性可知，当节点向量为0或1时，其一阶导数为0且在该B样条曲线上一定存在极值点。液压机械臂的末端执行器在通过B样条曲线极值点时需要控制速度，并按照修正后的轨迹行进，末端执行器在曲率极值点的最大通过速度为

(8)

式中：为三次B样条曲线的曲率半径；为机械臂运动中存在的最大法向量。以曲率极值点为中心对B样条曲线作分段规划，如果曲线中包含多个速度控制点，以该段规划的末速度作为下一段的初速度，机械臂轨迹运动中对速度、加速度的控制是避免轨迹偏差过大及后续插值的基础。根据控制系统的最大法向量加速度识别出三次B样条函数曲线上的最优限速点，并对曲线作分段处理。三次B样条曲线的空间轨迹过渡过程描述如图2所示。

图2 三次B样条曲线空间过渡过程描述

在图2中，、、和4个控制点能够确定一条三次B样条曲线，机械臂的运动轨迹包含直线、过渡区和直线3个部分。利用曲线的曲率极值确定液压机械臂轨迹的限速点并进行分段规划，以曲线为过渡区，确定机械臂末端执行器在该曲线段初始速度和末速度。曲线段内理论上存在+1个节点，即在该区间内存在一个二阶导函数()，可以构成三角函数多项式，则函数()是三次B样条函数()的插值函数。在给定的[0,π/2]区间内，4个控制点对应的插值函数一阶导数值分布为、、和，则用三角函数来描述插值函数为

()=cos+sin+(sin-sin)+

(cos-cos)

(9)

以峰值点为中心,将三次B样条曲线分割成两个部分，且保证型值点在所分割的两段曲线内可导，根据端点的斜率可求解出控制点的斜率。插值后的B样条曲线光滑度提升，且能够保证B样条插值函数在二阶、三阶连续。在笛卡尔空间内，六自由度液压机械臂接收控制系统指令后会出现一定延迟和局部轨迹偏差，针对轨迹偏差的特点，通过构造三次B样条函数修正机械臂末端执行器的轨迹偏差，并在局部轨迹范围内保证关节运动参数调整的连贯性。

3 实验结果与分析

3.1 实验环境与参数设定

在实验室的仿真环境下，验证三次B样条算法对于机械臂轨迹控制的效果。六自由度机械臂选用URur液压协作式机械臂，具有较高的负重比和灵活性，实验现场如图3所示。

图3 六自由度液压机械臂轨迹控制实验现场

实验中机械臂的轨迹路径和标准控制数据集采用自建的方式，其中液压机械臂关节和连杆控制系统的软硬件配置如表2所示。

表2 液压机械臂控制系统软硬件参数

实验中随机选定50个液压机械臂目标位置,分为5组，其中4组数据作为训练集，1组数据作为测试集，测试中的最大迭代次数设定为700，最低训练误差调整至10×10m。

3.2 机械臂轨迹规划结果分析

液压机械臂理论上的规划轨迹会全部通过选定的50个目标位置点，但在机械臂实际运行中由于液压关节的迟滞,位置轨迹会发生偏差。为验证所提算法的性能，利用机器人工具箱构建完整的机械臂对象，并与传统算法进行对比。在笛卡尔空间范围内构建6个液压关节的初始位姿坐标系；借助MATLAB仿真系统构建D-H参数矩阵，并实时地显示出机械臂的位姿和轨迹在、和面的偏离情况，如图4—图6所示。

由图4—图6可知：所提出的三次B样条算法能够有效实现3个轴向的轨迹纠偏，轨迹规划处理后所筛选到的轨迹坐标点，都能够相对准确地落到理论规划的轨迹路径上；而经过3种传统轨迹规划与纠偏算法处理后，有一部分机械臂的轨迹坐标点与理论规划曲线之间存在一定程度的偏差，还存在较大的优化空间。

图4 xz平面各算法的轨迹纠偏情况 图5 yz平面各算法的轨迹纠偏情况

图6 xy平面各算法的轨迹纠偏情况

根据各液压关节在最优行进路径点上的角度，确定最佳的时间间隔，并实时控制和纠正机械臂的轨迹偏差。各液压关节之间在型值点的标准数据，如表3所示。

表3 六自由度机械臂各液压关节的型值点标准数据

以机械臂液压关节型值点的标准数据为基准，分析在不同的运动轨迹修正算法下，10个测试目标位置点机械臂各关节最大力矩、最大角速度及最大加速度的偏差情况，结果如表4—表7所示。

表4 三次B样条算法下关节运动指标偏差

表5 力矩属性建模算法下关节运动指标偏差

表6 模糊自适应算法下关节运动指标偏差

表7 模糊免疫PID算法下关节运动指标偏差

在机械臂目标测试点的轨迹纠偏中，经过三次B样条算法修正后各关节的力矩偏差、角速度偏差和加速度偏差都被控制在较低范围内。以液压机械臂的关节3为例，力矩偏差保持在9.7 N·mm，角速度偏差和加速度偏差分别为0.13(°)/s和0.90(°)/s，与其他3种轨迹纠偏方法相比具有较为明显的优势。最后，验证轨迹纠偏中各控制算法的收敛性，基于MATLAB软件得到的仿真结果如图7所示。

图7 不同算法轨迹纠偏的收敛性能对比

三次B样条轨迹算法在400次迭代之内就完成了收敛，具有更高的轨迹调整效率。其他3种轨迹修正方法的迭代次数均超过了550次，其中模糊自适应算法甚至在最高迭代次数范围没有完成收敛。结果表明：三次B样条算法在液压机械臂轨迹规划中兼具了行进轨迹纠偏的准确性和迭代效率，能够有效解决液压机械臂的响应迟滞问题。

4 结束语

配备液压关节的六自由度机械臂具有更高的负重比，能够使工业机械臂的应用领域得到拓展，但液压关节存在明显的响应时滞问题，需要实时纠正机械臂轨迹出现的偏差问题。本文作者通过构建三次B样条函数，对机械臂空间轨迹进行插值和纠偏处理，可改善液压机械臂轨迹出现的局部偏差问题。实验结果也从液压关节力矩偏差、加速度偏差等角度，验证了文中提出的轨迹纠偏算法的有效性和适用性。