双臂机器人实时无碰撞运动规划

３双臂机器人实时无碰撞运动规划
３．１ 双臂机器人（１空间障碍物边界的离散 在基于ｃ空间的运动规划方法中，（１空间离散
单元数目对算法的效率有很大影响，合理的单元分
割应基于一定的优化标准“１．本文以左、右臂的末端 执行器位移作为标准．设机器人左、右臂的关节２固
定不动，关节】的角位移为△日，或者关节１固定不 动，关节２的角位移为△ｐ时，机器人末端执行器产
万方数据
第１ｌ期
丁富强，等：双臂ｓｃＡＲＡ，Ｉ、Ｅｓ机器人实时无碰撞运动规划
１６９３
使Ｒ。一Ｒｆ＋１； （６）若Ｐｊｊｌ一（丑，则左臂路径规划结束，路释点
为“Ｚ删ｉ，…，。ｈＧ¨若，ｉ·一６Ｒ，则右臂路径规划 结束，路径点为。％，。＆．…，。《ｒ，（；。；
（７），一，ｌ １，转步骤（３）． 需要指出的是，在步骤（ｊ）、（６）中，一旦机器人 的某个操作臂首先达到目标位置，则该操作臂将从 下一时刻起保持静止，井对另一操作臂形成固定障
原始路径建立后，采用基于扫描规则的优化方 法得到优化路径：判断当前节点与中间节点的连线 是否与障碍物相交，若不相交，则将运动路径简化为 当前节点到该中间节点的连线，否则将该中间节点 置为当前节点，然后再从新的当前节点出发进行扫 描，直到当前节点与目标节点的连线可以直接逛动 而不与障碍物相交为止．图４中的虚线表示原始路 径，实线为优化后得到的路径，其中“，和“。均为优 化后得到的中间节点，路径的优化既可以在原始路 径完全产生后再进行，也可在原始路径产生过程中 直接进行，由于双臂机器人可以提供并行运行功能， 本文在原始路径产生过程中直接进行路径优化．
双臂机器人实时无碰撞运动规划算法（ＲＦＭＡ） 如下：
（１）输入机器人左、右臂起始和终止无碰撞位 姿＾、ｒ．
（２）输出双臂机器人左、右臂实时无碰撞运动 路径．
（３）初始化．①设搜索得到的左、右臂中间节 点分别为咒、Ｋ，优化后的路径节点分别为“ｐ、“｝， 其中ｒ、厶和Ｒｃ为大于或等于零的整数；②计算 △目和△ｐ，建立左、右臂各自的离散ｃ空间，得到离 散（’空间中左臂起始和目标构形对应的节点ｓ。ＧＬ 以及右臂起始和目标构形对应的节点靠、ＧＲ．
臂构形对称时，它们彼此形成的ｃ空间障碍物形状 相似．
圉１ 叔臂ｓ（１ＡＲＡＴＥｓ机器人结构示意圈
Ｆｌｚ．１ Ａ ｓｋｅＩｆｈ ｏｆ ｔｈｅ ｃｏｎｆｌｇｕｒａｔｌ…ｔｒｕｃｌｕｒｅ ｏｆ
５ＣＡＲＡＴＦＳ ｒｏｂｏｔ
２双臂机器人Ｃ空间障碍物的确定
机器人ｃ空间为一广义空间口３．在ｃ空间中被 障碍物占据的构形称为ｃ窀问障碍物，ｃ空间的其 余部分称为自由ｃ空间．对机器人的两个手臂不划 分优先级．两个手臂在运动时互为障碍物．假定以 Ｌ Ａｒｍ、Ｒ—Ａｒｍ表示机器人的左、右臂，Ｌ Ａｒｍ对 Ｒ Ａｒｍ的运动约束构成的Ｒ—Ａｒｍ的ｃ空间障碍 物为Ｃ０。（Ｌ Ａｒｎｌ），而自由ｃ空间则为 （’ｎ（Ｉ—Ａｒｍ）；Ｒ Ａｒｍ对Ｌ Ａｒｍ的运动约束构成 的ｌ—Ａｒｍ的ｃ空间障碍物为Ｃ０，（Ｒ Ａｒｍ），自由 ｃ空间为ｃＦ。（Ｒ—Ａｒｍ），则左、右臂的ｃ空间分别
（Ｒｅｓｅａｒｃｈ ｌｎｓｔ．ｏｆ Ｒｏｂ。ｔｉｃｓ，Ｓｈａ“ｇｈａＩＪｉａｏｔｏ“ｇ Ｕｎｉｖ．，Ｓｈａ“ｇｈａｉ ２０００３０，Ｃｈｉｎａ）
Ａｂｓｌｒａｃｔ：Ｔｈｉｓ ｐａｐｅｒ ｃ。ｎｓｔｒｕｃｔｅｄ ｔｈｅ ｄｕａｌ ａｒｍ ｒｏｂｏｔ Ｃ—ｓｐａｃｅ。ｂｓｔａｃｌｅ ｂｏｕｎｄａｒｙ ｂａｓｅｄ ｏｎ ｔｈｅ ｒｅａｃｈａｂｌｅ ｍａｎｌｆｏｌｄ ａｎｄ ｃｏｎｔａｃｔ ｍａｎｉｆｏｌｄ．Ａｎｄ ｉｔ ｔｈｅｎ ｐｒｅｓｅｎｔｅｄ ａｎｄ ｓｉｍｕｌａｔｅｄ ｓｕｃｃｅｓｓｆｕＵｙ ｔｈｅ ａｌｇｏｒｉｔｈｍ。ｆ ｔｈｅ ｄｕａＩ ａｒｍ ｒｏｂｏｔ ｒｅａｌ—ｔｉｍｅ ｃ。ｌｌｉｓｉｏｎ ｆｒｅｅ ｍｏｔｉｏｎ ｐｌａｎ ｂｙ ｔｈｅ ｏｂｓｔａｃｌｅ ｂｏｕｎｄａ’ｙ ｄｌｓｃｒｅｔｉ２ａｔｉｏｎ ａｎｄ ｔｈｅ ｓｅａｒｃｈｉ“ｇ ａｌ ｇｏ＾ｔｈｍ． Ｋｅｙ ｗｏｒｄｓ：ｄｕａ卜ａｒｍ ＳＣＡＲＡＴＥＳ ｒｏｂｏｔ；Ｃ ８ｐａｃｅ ｏｂｓｔａｃｌｅ ｂｏｕｎｄａｒｙ；ｒｅａｌ＋ｔｌｍｅ ｃ。ｌｌｉｓｉｏｎ—ｆｒｅｅ ｍ。ｔｉｏｎ ｐｌａｎ
参考文献：
４双臂机器人无碰撞路径规划仿真
轴间距离卢２６０ ｍｍ．图中的虚线为机器人左、右臂
第】连杆的运动范围．双臂机器人碰撞发生的条件 是一个手臂的末端顶点与另一个手臂连杆的部分边 相碰撞．
收稿日期：２０。２ ｌｌ ２０ 基盒项目：国家自然科学基金（５９８７５０５０）和国家高技术研究发展计划（８６３）（８６３ ５１２ ９８２０—０４）资助项目
（２）
二拶
图３不同关节角的变化形成的末端执行器位移
Ｆ１９ ３
Ｔｈｅ ｅｎｄ—ｃｆｆｅｃｔｏｒ ｄｌｓｐ】ａｃｅｍｅｎｒ ｆｏｒｍｅｄ ｂｖ ｔｈｅ ｃｈａ“ｇｅ ｏｆ ｄｉｆｆｅｒｅｎｔ ｊｏｌｎＩｓ
万方数据
１ ６９２
上 海交通 大 学 学报
第３ ７卷
设机器人每次运动的时间间隔为７’，则在丁内
△ａ一＾ｖ…７’，两个关节角的变化量分别为
生相同的最大位移为△ｚ（见图３），则有
△口（ｆ．＋Ｚｂ）＝△脚ｂ一血
因此，在设定△ｚ后，可选取ｃ空间的划分单位△口、 △卢．将左、右臂的ｃ空间划分为ｍ×”个网格，并分
别满足：
ｎ ＝ ｍａｘ
一 ｍｎ
ｌ
ｍａｘ
：； ｎ ｌＩ ｍ ａｘ ｍ ａ ｘ 靠乱靠乱
｛｝ 一 ｍ ｎ
１
一 ｍｎ
２
一 ｍ ｎ 靠巩靠乱 ２ ＾＾） ／／ 郜埘邵卵
１双臂机器人碰撞几何特征
图１为双臂ｓｃＡＲＡＴＥｓ机器人的结构示意 图，双臂机器人左边的操作臂简称为左臂，右边的操 作臂简称为右臂；图中所标的４个角为双臂机器人 的构形角，它们分别满足以下参数范围：日ｎ∈［８０，
１８５ｊ，口。２∈［一２５，一１ ５０］，靠ｌ∈Ｌ一５，１００］，靠ｚ∈ ［２５，１ ５０］．将机器人各个操作臂简化为图中所示的 四边形，长度３１０ ｍｍ，宽５０ ｍｍ．左、右臂本体回转
为Fra Baidu bibliotek
ｃ’（Ｌ—Ａｒｍ）＝∞Ｌ（Ｒ Ａｒｍ）Ｕ（ｊＦＬ（Ｒ Ａｒｍ）ｌ …
（１（Ｒ Ａｒｍ）一ｃｚ凡（Ｉ，Ａ ｒｍ）ＵｃＦＫ（Ｉ，Ａｒｍ）Ｊ ｎ形障碍物在ｓｃＡＲＡ型操作臂的ｃ空间形成
一连通的ｃ空间障碍物“］，冈此凸形障碍物可通过 （１空间障碍物的边界确定．在计算ｃ空间障碍物的 边界时，采用文献［５］中所论述的接触流形和可达流 形理论．对如图ｌ所示的双臂机器人，表示Ｒ Ａｒｍ 的两个简单凸多边形和表示Ｉ—Ａｒｍ的两个简单凸 多边形可以通过互相接触分别形成不同条件下的接 触流形，则Ｒ Ａｒｍ的ｃ空间障碍物边界可以通过 Ｒ Ａｒｍ的可达流形与这两个接触流形的交获得． 同理，１一Ａｒｍ的ｃ空间障碍物也可通过Ｌ Ａｒｍ的 可达流形与Ｌ Ａｒｍ和Ｒ Ａｒｍ所形成的两个接触 流形的交获得．图２给出了靠，＝ｌｏｏ。，靠ｚ一５００和＆－ 一１００。。钆一８０。时所形成的左臂ｃ空间障碍物边界 曲线，边界曲线的右侧为所对应的ｃ空间自由区 域，左侧为ｃ空间障碍物本身．
机器人实时无碰撞运动规划算法．
关键词：双臂ｓｃＡＲＡＴＥＳ机器人；ｃ空间障碍物边界；实时无碰撞运动规划
中图分类号：ＴＩ’２４２
文献标识码：Ａ
Ｒｅａｌ—Ｔｉｍｅ ＣＯＩＩｉｓｉＯｎ—Ｆｒｅｅ ＭＯｔｉＯｎ ＰＩａｎ ｏｆ ｔｈｅ ＤｕａＩ—Ａｒｍ ＳＣＡＲＡＴＥＳ Ｒｏｂｏｔ
ＤＩＮ（；ＦＭ一‘ｉｉｎｎｇ， ＦＥＩ Ｙｎｎ—ｑｉ【ｍｇ，ＨＡＮ ｗ“一ｊｕｎ， ｚＨＡＯ Ｘｉ—ｋｎｇ
双臂ｓｃＡＲＡＴＥｓ机器人是近几年刚刚推出的 新喇装配机器人．当机器人执行任务时．两个操作臂 在同一丁作空间中运动，每个操作臂均是另一操作 臂的运动障碍物，两个机器人手臂都是在非结构化 的环境中运动，它们之间存在着碰撞的可能．对双臂 机器人无碰撞运动规划的研究，大部分集中在多机 器人协调方面，每个机器人都有自己的控制器，各个 操作臂之间靠另外建立的更高一级的层级控制器进 行协调规划和控制”４］．如何快速有效建立双臂机器 人无碰撞运动路径直接关系到双臂机器人的实际应 用前景．本文针对双臂机器人解决以下问题：给定双 臂机器人左、右臂末端的起始位置和终止位置，实时 规划出机器人双臂无碰撞最优运动路径．
文章编号：１ ００６—２４６７（２００３）１１—１６９０一０４
双臂ＳＣＡＲＡＴＥＳ机器人实时元碰撞运动规划
丁富强， 费燕琼， 韩卫军， 赵锡芳
（上海交通大学机器人研究所，上海２０００３０）
摘要：以双臂ＳＣＡＲＡＴＥＳ机器人为研究对象，基于可迭流形和接触流形建立了艇臂机器人ｆ
空间障碍物边界，并通过对障碍物边界的离散以厦对路径搜索算法的选择，提出并仿真实现了双臂
利用路径的启发性搜索算法构建左、右臂的原始路 径，然后基于扫描规则对所建立的原始路径进行优 化．
采用Ａ’算法进行原始路径搜索，取代价函数 ，（“）＝（１一叫）ｇ（“）十删＾（“）
其中：ｗ∈［ｏ，１］为权值，为了提高路径规则的在线
性能，设ｔ“＝ｏ．９９；ｇ‘“）＝＆△ａ＋句△卢，反映从起始 构形节点ｓ到当前节点“的总节点数变化所引起 的角度变化程度；＾（“）为启发函数表示在（、空间中 从当前节点“到目标节点Ｇ间的直线距离，该距离 也可用“到Ｇ间的两个关节角变化量来衡量．
凹一篇ｌ 聊一丁Ｊ ．。 彤…７’｝
㈥
式中：＾为速度比例系数；Ｖ。。一７ ７００ ｍｍ／ｓ为机器 人末端最大线速度．假定了１一ｌ ｍｓ，＾一】，则由式（３） 可以得到△８＝１．４。、△卢一２．８。，且左、右臂相应的ｃ 空间单元数为７５×４５．图４（ａ）描述了机器人左臂位 于钆＝ｌｌ ５。，钆一一８０。构形时所形成的右臂ｃ空 间障碍物边界及其离散结果．图中网格点的位鬣由 序列坐标（ｉ，Ｊ）所确定，其中ｉ—ｏ，１，…，ｍ；Ｊ—ｏ，Ｉ， …，ｎ；图中的离散点为边界曲线外部的节点．
时间大大缩短．
ｃ３ ±士 相
｛。五口
引对双臂机器人任务操作中运动规划的崮有问 题，利用可达流形和接触流形的边界曲线法建立了 双臂机器人ｃ空间障碍物边界，并通过对离散化后 的障碍物边界进行原始路径的搜索和优化，提出并 仿真实现ｒ双臂机器人实时无碰撞运动规划算法·
碍物，使其从此将在固定的ｃ空间障碍物中运动·
ＲＦＭＡ的具体步骤如下： （１）计算左、右臂分别沿ｓ。ＧＬ、ｓ。Ｇ。运动时在 丁内彼此间形成的ｆ空间障碍物，并进行相交性检 测．若无相交情况，则使左、右臂沿ｓ。抗、＆Ｇ。直接 运动到目标位置，程序结束； （２）令ｒ—Ｌｆ—Ｒｃ—ｏ，且使尸￡一“≯＝ｓＬ．蹦一 “＆＝ＳＲ； （３）计算左、右臂问的最近距离，建立左、右臂 的ｃ空间障碍物边界“、Ｑｉ，并获得离散边界数据 点集，用启发式搜索算法搜索左、右臂的中间节点 Ｐｉ＿。、Ｐｒｌ； （４）若ｒ＝ｏ．则执行步骤（７）； （５）若班‘尸；“与‘堆～Ｑ：１中的一个障碍物有 相交，则“｝“一Ｐ￡，并使上ｃ＝ｋ＋ｊ；者“｝玮ｕ与 Ｑ｝～Ｑｆｌ中的一个障碍物有相交，则“Ｐ１＝戌，并
８Ｒ∥）
（ｂ）两臂同时运动 ａ矗：臂对右臂运动约束构
成的ｃ空间障碍物边界 ·ｃ空间障碍物边界离散化
图４ 两个操作臂无碰撞路径的构造及其优化
Ｆ‘ｇ．４ Ｔｈｅ ｐａｔｈ ｃｏｎ８ｔｒｕｃｔｉｏｎ ａｎｄ ｏｐｔｉｎｌｌｚａ— ｔｊｏｎ ｏｆ Ｉｈｅ ｔｗｏ ｍａｎｌＤｕｌａｔｏｒｓ
３．２左、右臂无碰擅路径的建立 双臂机器人无碰撞路径规划分两步进行：首先
由罔２可见，机器人左、右臂的ｃ空间障碍物 是·连通的区域，左、右臂的每一个构形都对另外的 一个手臂产生两个ｃ空间障碍物．当机器人左、右
疗Ｌ１“。）
ａ）靠ｌ＝１００：目Ｒ２＝５０。
目Ｌ、＂１ （ｂ）口Ｒｌ＝１０旷，口趾＝８“１
图２机器人左臂ｆ空间障碍物边界
２（、一ｓｐｍ Ｆ１９
ｏｈｓｔａｃｌｅ ｂ‘Ⅲｎ【ｌａｒｙ ｏｆ Ｌ｝１ｅ ｄｕａｌ—ａｒｎｌ ｒｏｂｏ
作者简介：丁富强（１ ９７３一），男·河南话阳市人，博士生，研究方向为机器人自动化装配、运莉规划．费燕琼（联系人），女．讲师．博士
电话（Ｔｅｌ．）：０２ｌ一６２９３２６８６；Ｅ㈨ｌ：ｆｙｑ＠ｓＪｔｕ．ｅｄｕ．ｃｎ
万方数据
第．。期
丁富强，等：兰竺兰全！全三兰！兰兰尘苎竺兰兰兰兰竺兰竺—————兰
慈 鹚多蚀起瓣 起始位胃