6自由度机器人梯形速度控制直线插补算法研究

４ 自动化升级后的实际应用的情况
作为与多家糖厂的合作项目，我们对多款手动 型上悬式离心机进行了自动化技术升级。经反复试 验，及多榨季的运行，改造后的离心机手动及自动 模式运行时均相当稳定，显著提高了产品质量及生 产效率，且操作简单，大大降低了操作工的劳动强 度，取得很好的经济和社会效益。
参考文献：
本文对 ＲＢＴ－６Ｔ／Ｓ０２Ｓ 机器人进行了研究，通过 对其运动学分析，根据机器人参数得到其数学模型， 根据插补算法，完成位移增量及加、减速度段速度 速度值，通过运动学及雅可比逆运算，将末端位移 和速度转换成各关节的角度值和速度值。通过仿真 和实验结果表明算法可行。
参考文献：
［１］ 蔡自兴．机器人学［Ｍ］．北京：清华大学出版社，２０００．
（４）
插补的总步数 Ｎ 为
Ｎ＝Ｎ１＋Ｎ２＋ Ｎ３ （Ｎ１、Ｎ２、Ｎ３ 为加速段、匀速段 和减速段的插补步数）
在加速段距离 Ｌ１ 为
（１５）
于是可以实时计算各轴插补点坐标值
加速段的插补步数 Ｎ９２】 第 ３１ 卷 第 ９ 期 ２００９－０９
在 ６ 自由度机器人中利用 Ａ１，Ａ２，Ａ３，Ａ４，Ａ５，Ａ６ 根据 各连杆的变化即可求出雅可比矩阵。
当 ｑ 已知时，根据雅可比矩阵可推算出操作空 间的速度矢量ｘ。当机器人末端速度给定，可以根据 逆雅可比矩阵，求出各个关节速度。
２ 机器人直线实时插补算法
［１］ 韦建军．ＴＦ－１３５０ 全自动离心机气动系统设计［Ｊ］．液压与气 动杂志，２００１．
［２］ 韦建军． 一款高可靠性的蔗糖离心机进料量测控器［Ｊ］．机 械设计与制造杂志 ２００１，４．
【上接第 ９４ 页】
从机器人仿真结果和实际运行可以看出，实 际直线与仿真的直线吻合。直线插补算法满足要求。
４ 结束语
工业大学学报，２００５，２８（７）：７６０－７６３． ［７］ 石炜，程原野，李强．六连杆机器人运动学分析［Ｊ］．包头钢铁
学院学报，２００６，２５（４）：３６０－３６４． ［８］ 陈则仕，张秋菊．一种五轴联动机器人运动学建模与仿真
研究［Ｊ］．制造业自动化，２００５，（２７）１２：３６－３９． ［９］ 郑金兴，张铭钧．梯形速度控制便插补周期的实时插补算
图 ９ 机器人喷枪画出直线插补运动轨迹 【下转第 １３４ 页】
探测板 ３ 靠近吊蓝 ４ 内壁，吊蓝随轴 １ 转动，使进入 的糖膏因离心力作用均布在吊蓝内，随进料增多糖 膏增厚，糖膏线不断外移，最后与探测板接触，推 动探测板绕转轴转动，带动感应块运动，触发接近 开关 ２，发出料满信号，ＰＬＣ 输出关闸信号，进料闸 门在气缸驱动下关闭。调节探测板与吊蓝内壁之间 的距离可以控制进入的吊蓝的糖膏量。测量仪的检 测状态置位与复位由内部小气缸驱动完成。
（６） ｅｎｔ 为取整数部分减速段的插补步数为
（７）
匀速段的插补步数为
Ｎ ＝Ｎ－Ｎ －Ｎ （８） ２
１ ３
图 ４ 机器人轨迹控制过程
位置。通过连续的运动，实现要求的轨迹。 空间直线插补是机器人系统中不可缺少的基本
插补算法，在已知该直线始末两点的位置和姿态， 求各轨迹中间点（插补点）的位置和姿态。由于大 多数情况下机器人沿直线运动时，其姿态不变，因 此不需姿态插补。
当给定直线始末两点的坐标值 ｐ０（Ｘ０，Ｙ０，Ｚ０）、 ｐＥ（ＸＥ，ＹＥ，ＺＥ），如图 ５ 所示。机器人从点 ｐ０（Ｘ０，Ｙ０，Ｚ０） 经过加速、匀速、减速达到 ｐＥ（ＸＥ，ＹＥ，ＺＥ）。在固定插 补步长 ∆ｌ 的条件下，依据直线加减速算法，确定出 加速段、匀速段和减速段的插补次数，进而生成变 化的进给速度和相应的插补周期，最后形成各坐标 轴的运动增量。与通用固定插补周期的方法相比， 可以有效的控制精度，充分发挥各关节的加速能力， 缩短加减速时间。
置零、停止、仿真、运动模块。程序界面如图 ７ 所示。
图 ７ 平面内直线插补界面
给定直线始末两点的坐标值 ｐ０（４３０，－５０，６００）、ｐＥ （４３０，５０，６７０）及插补步长 １ｍｍ，速度ｖ＝１０ｍｍ·ｓ－１，加 速段 α 为 １００ｍｍ·ｓ－１ 其中 Ｘ０＝ＸＥ，运行仿真模块得 到仿真结果如图 ８ 所示，图中的直线表示经过直线 插补算法后的运行轨迹。在仿真验证正确后点击 “运行”按钮，得到机器人实际运动轨迹如图９所示。
第 ３１ 卷 第 ９ 期 ２００９－０９ 【９３】
使用齐次变换矩阵及雅可比逆矩阵求出各关节 的角度及相关速度段速度，完成控制。
３ 直线插补程序设计及控制实现
根据直线插补原理设计机器人直线插补控制程 序，流程图如图 ６ 所示，为了简便，我们讨论机器 人平面运动情况，将 Ｘ 轴坐标固定，使用坐标 Ｙ－Ｚ。
１ ＲＢＴ－６Ｔ／Ｓ０２Ｓ 机器人运动学分析 及速度分解
本文所研究的机器人为 ６ 自由度串联关节式机 器人 ＲＢＴ－６Ｔ／Ｓ０２Ｓ，由两块运动控制卡控制六个轴 运动，其结构如图 １ 所示，技术参数和连杆参数见 表 １、表 ２。
表 １ ＲＢＴ－６Ｔ／Ｓ０２Ｓ 机器人技术参数
表 ２ ＲＢＴ－６Ｔ／Ｓ０２Ｓ 机器人连杆参数
［２］ 刘极峰．机器人技术基础［Ｍ］．北京：高等教育出版社，２００６． ［３］ 何广平．关节型机器人［Ｍ］．北京：化学工业出版社，２００３． ［４］ 孙迪生．机器人控制技术［Ｍ］．北京：机械工业出版社，２００６． ［５］ 丁恭学．机器人控制研究［Ｍ］．杭州：浙江大学出版社，２００６． ［６］ 钱朋安，蔡济民．机器人运动学仿真教学软件研究［Ｊ］．合肥
方向矢量 ｏ 和接近矢量 ａ 的 ３ 个分量；（ｐｘ，ｐｙ，ｐｚ）是手 中心的位置矢量 ｐ 的分量。规定顺时针为正，逆时
针为负，ｃｉ＝ｃｏｓθｉ，ｓｉ＝ｓｉｎθｉ 相邻两连杆相对位姿的 Ａ 与关节角变量 θｉ 及连杆参数间关系分别为：
（２）
图 ２ ＲＢＴ－６Ｔ／Ｓ０２Ｓ 机器人运动结构简图
控制器通过屏蔽缆线与外部输入元件（传感 器）、输出元件（如气阀、水阀、汽阀线圈）相联。
图 ２ 自动模式下甲糖分离工艺与控制流程图
完成电气控制与机械部分的改造后，即可进行 试机。离心机在自动模式下甲糖分离工艺与控制流 程如图 ２ 所示。
循环开始时，ＰＬＣ 接受现场主令信号、速度检 测单元、位置传感器等送来的信号，经逻辑判断，然 后按编制好的工艺程序发出指令，控制电机的转速 及各执行机构。其中，洗网时间长短视筛网堵塞情 况调节；洗水阀通电时，ＰＬＣ 同时输出信号给蜜水 分离机构气阀，将机构从“原蜜”转换到“洗蜜”状 态；分离时间到后，根据速度来控制电机减档档位 的切换时刻，以免造成电机过热。运行时一般先进 行手动操作，机器运转正常后，在一个循环内在线 切入自动模式，进行全自动循环操作。
电机速度的检测：电机速度是控制过程中一个 重要参量，在自动模式运行时需要根据电机的转速 大小来控制速度档位及工序的切换。可采用脉冲计 数的方法测量。在多速电机顶部转轴处安装两块随 转轴转动的铁质感应块，接近开关发出感应信号， 由 ＰＬＣ 根据输入的脉冲计算出电机转速。
刹车的检测：卸料必须保证在可靠刹车后进 行。是否可靠刹车的检测方法是，刹车气缸动作信 号输出后，如果测速装置 １０ 秒（可调）内测量数值 不为零，则系统认定刹车失败。
（１２） （１３）
图 ５ 平面直线插补
其中 ｐ０、ｐＥ 是相对于基础坐标系的坐标值。设 α 为要求的机器人的加减速度值；ｖ 为要求的速度。 需插补直线长度为
各轴速度增量为
（１４）
结合式（１），可由 θｉ 计算出末端连杆的位姿，实 现运动学正解。反之，若给定末端连杆的位姿（ｎ，ｏ，
ａ，ｐ）可求出各关节变量 θｉ，实现运动学逆解。 机器人末端速度向量 ｘ 与关节角速度向量 ｑ 之
间的关系 Ｘ＝Ｊ（ｑ）ｑ ，Ｊ（ｑ）为雅可比矩阵。使用微分变
换法推出雅可比矩阵的第 ｉ 列 Ｊｌｉ 为
图 ８ 直线插补仿真轨迹
图 ６ 直线插补程序流程图
ＲＢＴ－６Ｔ／Ｓ０２Ｓ 机器人的实际运动由运动控制卡 控制。在 ＶＣ６．０ 环境下，将运动控制卡动态连接库 文件 ｓ６ｓ１ｖ１００．ｄｌｌ链接在 ＶＣ中，通过调用相关函数， 以对话框面向对象的方式编制系统数据输入、复位、
【９４】 第 ３１ 卷 第 ９ 期 ２００９－０９
（大连民族学院 机电信息工程学院，大连 １１６０２３） 摘 要：本文通过对 ６ 自由度关节机器人的运动学分析，设计了机器人梯形速度控制在空间内的直线
插补算法，通过固定一个坐标轴的方法设计了平面内 ６ 自由度机器人直线插补流程，用 ＶＣ＋＋ 语言编写了机器人直线插补程序，并通过仿真和实验进行验证，结果证明运动学算法是可行的。 关键词：关节机器人；运动学；直线插补 中图分类号：ＴＰ２４ 文献标识码：Ａ 文章编号：１００９－０１３４（２００９）０９－００９１－０４
图 １ 关节机器人 ＲＢＴ－６Ｔ／Ｓ０２Ｓ 结构图
收稿日期：２００９－０１－１３ 作者简介：刘长宏（１９６８ －），男，河南林州人，工程师 ，主要从事机电一体化研究。
第 ３１ 卷 第 ９ 期 ２００９－０９ 【９１】
据表 ２ 建立 ＲＢＴ－６Ｔ／Ｓ０２Ｓ 机器人运动结构简图 如图 ２ 所示，用 Ｄ－Ｈ 方法建立的坐标系［４，５］如图 ３ 所 示，其中 ｙ 轴按右手法则确定。
（３）
图 ３ Ｄ－Ｈ 方法坐标系的建立
在该坐标系下机器人末端装置（手）在坐标系 中的位置［６￣８］为
（１）
式中，Ａ 为描述一个连杆与相领下一个连杆间 相对位置的齐次变换矩阵，（ｎｘ，ｎｙ，ｎｚ）、（ｏｘ，ｏｙ，ｏｚ）、（ａｘ，ａｙ， ａｚ）表示机器人末端装置在基坐标系中的法线矢量ｎ、
目前，机器人基本操作方式是示教再现的，即 首先教机器人如何做，机器人就记住了这个过程， 于是它可以根据需要重复这个动作。为了保证运动 的平稳，在示教点之间采用插值或插补算法，插入 中间点，再用机器人逆运动学算法算出各个关节的 角度对机器人控制。具体流程如图 ４ 所示。
从图 ４ 所示的轨迹控制过程可以得知，每隔一 个时间间隔 Ｔｓ，插补出一轨迹点的坐标值，转换成 相应的关节角度值，加到位置伺服系统以实现这个