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

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