第8章+机器人控制技术运动轨迹.ppt

8 1引言8 2目标物体的描述8 3任务的描述8 4视觉8 5程序8 6传送带跟踪8 7位置之间的运动8 8关节运动8 9笛卡尔运动8 10本章小结 第八章运动轨迹Chapter MotionTrajectories 本章是机器人运动控制的基础 它分为四个主要部分 第一部分 利用齐次坐标变换构造任务第二部分 基于时间坐标轨迹的运动控制描述第三部分 关节坐标运动的描述第四部分 笛卡儿运动控制描述 8 1引言 Introduction 任何刚性物体都能够用一个与该物体固定相联的坐标系来描述 给出该物体的图形表示及其坐标系统 只要说明该坐标系统的位置和方向 就足以在任何位置和方位上复现这个物体 如图8 1中的销钉 它的轴位于z轴上 半径为0 5 长度为6 8 2目标物体的描述 ObjectDescription 利用齐次变换来描述一个任务 任务内容是抓取如图8 1所示的一些销钉 然后把它们插入一个装配部件的孔中 见图8 2 图8 2任务的描述 8 3任务的描述 TaskDescription z 规定机械手末端执行器 手爪 的一系列位置Pn 见图8 3 就能把这一任务描述为相应于这些编号位置的机械手运动和动作的序列 图8 3末端执行器的位置 MOVEP1接近销钉MOVEP2移动到销钉的位置GRASP抓住销钉MOVEP3垂直提起销钉MOVEP4按一定角度接近孔眼MOVEP5接触到孔眼时停止MOVEP6调整销钉的位置MOVEP7插入销钉RELEASE松开销钉MOVEP8离开 下面通过规定机械手的结构来确定任务结构 我们用三个变换的乘积描述机械手 从而任务描述中的位置就由下式取代MOVEpn MOVEZT6E 8 1 其中Z 表示机械手相对于任务坐标系的位置 T6 表示机械手末端相对于机械手坐标的位置 E 表示末端执行器 手爪 相对于机械手末端坐标的位置 按照上述描述 机械手的位置由Z来确定 任务的执行就是改变抓手的位置 现在利用下列符号来描述任务的变化 P销钉在基坐标中的位置 H带有两孔眼的金属块在基坐标中的位置 HHRi金属块上第i个孔相对H坐标系的位置 PPG抓取销钉的抓手相对于销钉的位置 PPA抓手接近销钉 PPD抓手提起销钉 HRPHA销钉接近第i个孔眼 HRPCH销钉接触孔眼 HRPAL销钉开始插入 HRPN插入后的销钉 现在 任务可由一系列变换式来描述 由此解出机械手的控制输入T6 这些变换式如下 P1 ZT6E PPA接近销钉P2 ZT6E PPG到达抓取销钉的位置GRASP抓取销钉P3 ZT6E PPDPG提起销钉P4 ZT6E HHRiPHAPG接近第i个孔眼P5 ZT6E HHRiPCHPG接触第i个孔眼P6 ZT6E HHRiPALPG插入销钉P7 ZT6E HHRiPNPG插入完成RELEASE松开手爪P8 ZT6E HHRiPNPA回到起始位置 任务位置变换图如图8 4所示 尽管这样表示可能显得复杂 但是说明了任务的基本结构 而且每一个变换表示了一个独立的情况 相应的P H和Z坐标系如图8 5所示 由图8 5可知 Z为机械手坐标系 它定位在肩关节上 因此工作坐标系 基坐标系 位于机械手坐标的位置为T6pz 50由于机械手不能到达它自己的基座 所以我们把它放在基坐标系原点的后面 这样T6px 30 使T6py 0 并且保持两个坐标系的平行 图8 5任务坐标系P H和Z 由图8 5可知 8 2 下面通过相对于机械手末端的变换来定义末端执行器 我们沿着这样的表示习惯 末端执行器的z轴指向执行任务的方向 而y轴表示手爪的开合方向 于是如图8 6所示的抓手就可描述为 图8 6手爪变换 8 3 我们已经在图8 1中描述了销钉 现在再看一下带有两个孔眼的金属块H H的正视图如图8 7所示 借助于变换矩阵HRi i 1 2 是孔眼的序号 来描述它的特征 HR1 HR2 8 4 8 5 注意 式 8 4 和式 8 5 是分别沿H坐标的x轴旋转 90 再平移后得到 最重要的变换是销钉插入一个孔眼 见图8 8 销钉的z轴必须与孔眼的轴一致 由于销钉具有圆柱的对称性 x y轴的方向就可任意了 最后一个变换必须按照手爪在销钉上的部位来确定 见图8 9 8 6 8 7 现在我们通过示教方式利用机械手来确定前面的变换关系 将末端执行器放在销钉上面 处于它的抓取位置 图8 3中的P2 可得到下列变换式 ZT6E PPG P2 8 8 上式可确定PP ZT6EPG 1 8 9 手爪返回到靠近销钉的位置P1 于是有ZT6E PPA P1 8 10 从而确定了PAPA P 1ZT6E 8 11 关于P的起始点可这样确定 把手爪中的销钉提起 移到起始位置P3 于是有ZT6E PPDPG P3 8 12 由上式确定了PDPD P 1ZT6EPG 1 8 13 金属块H的位置由下式确定ZT6E HHR1PNPG P7 8 14 解出HH ZT6E HR1PNPG 1 8 15 销钉开始插入 PAL 见图5 10 由下式确定ZT6E HHR1PALPG P6 8 16 从而PAL HHR1 1ZT6EPG 1 8 17 接触孔眼的销钉位置PCH以及接近触点的点PHA可确定为PCH HHR1 1ZT6EPG 1 P5 8 18 PHA HHR1 1ZT6EPG 1 P4 8 19 这样就定义所有的变换 问题 式 8 17 8 19 三个表达式完全相同 为什么能描述三个不同的工位点 在求取任务执行过程中的销钉位置时 我们是把机械手放在销钉的上方 处于正确的抓取位置 然后求解变换式P 在实际系统中 我们还可以通过视觉获得销钉等物体的位置 假如有一个位置固定的摄像机 它能把销钉的方位表示为相对于它自身的坐标系的一个齐次变换 比如说PC 那么销钉相对于基坐标的位置就可表示为P CAMPC 8 20 其中CAM是一个变换 表示摄像机坐标系在基坐标中的位置 在通过机械手确定P之后 CAM的值能通过摄取一幅图象来得到PC 从而解得CAM PPC 1 8 21 摄像机的输入通过READ读取得到 READ CAMERA PC P CAM PC而P是要被确定的 我们用标识符来表示程序中的变换 号表示矩阵相乘 号表示与逆阵相乘 8 4视觉 Vision 现在可以利用求解T6的变换式 见图8 4 来确定任务程序中从P1到P8的每个位置 为了把机械手移到P1 要求ZT6E PPA 8 22 这就意味着T6 Z 1PPAE 1 8 23 要到达P2则需ZT6E PPG 8 24 如此等等 8 5程序 Program 在我们用变换表达式代替程序中P1 P8的各个位置时 先定义两个变量COORD和TOOL 它分别表示工作坐标系的一般表达式和抓手坐标系的一般表达式 然后 把所有的位置表达式写成如下形式T6TOOL COORDPOS 8 25 对于第一步运动 COORD和TOOL定义为COORD Z P TOOL E 因而第一步运动就是MOVEPA 现在的位置是相对于销钉来确定的 在销钉被抓取之后 我们把TOOL规定为TOOL E PG上式起到这样的作用 把机械手的末端重新规定为销钉的末端 插入两个销钉的程序就变为 TOOL E 固定抓手FORI 1 2DOBEGINREAD CAMERA PC 销钉位置P CAM PC 建立PCOORD Z P 相对于销钉的位置MOVEPA 接近MOVEPG 移至销钉上方GRASP 抓取TOOL E PC 抓手在销钉末端MOVEPD 起始点 HT HR I 孔眼的位置COORD Z H HT 相对于孔眼的坐标MOVEPHA 接近孔眼MOVEPCH 接触孔眼MOVEPAL 调整销钉MOVEPN 插入销钉RELEASE 松开销钉COORD Z H HT PN 相对于销钉的坐标TOOL EMOVEPA脱离销钉END 当销钉放在传送带上执行任务时 需要建立一个传送带与工作坐标的变换CONV S 它是传送带位置S的函数 然后根据传送带的坐标变换CONV S 和销钉相对传送带的位置CONVP来确定销钉在工作坐标系中的位置PP CONV S CONVP 8 26 这样 所有的位置都可描述为相对于传送带的齐次变换 如果物体OBJ放在传送带上 就可以利用传送带变换写出它的变换表达式ZT6E CONV S OBJFG 8 27 其中CONV某个标量S的函数 描述传送带的位置 CONVOBJ相对于传送带描述物体 OBJF描述装配部件方向的特征 FG利用装配部件特征描述末端执行器 8 6传送带跟踪 Conveyortracking 如果传送带上的物体停止在某个位置 而末端执行器处于抓取位置 OBJ就可确定为OBJ CONV 1 S ZT6E FG 1 8 28 在任务执行过程中 T6变成传送带变量的函数T6 Z 1CONV S OBJFGE 1 8 29 如果不断地对T6求值 并转换成关节角度 解逆运动方程 同时机械手跟随这些角度 那么机械手就会跟踪运动着的物体 如果传送带停止运动 机械手也会停止动作 这样 执行任务的程序就可写成 TOOL E固定抓手FORI 1 2DOBEGINREAD CAMERA PC 插入销钉位置P CONV S CAM P 建立PCOORD Z CONV S P 相对于销钉的坐标MOVEPA 接近MOVEPC 在销钉上方GRASP 抓取销钉TOOL E PG 抓手在销钉末端MOVEPD 起始位置 HT HR I 孔眼的位置COORD Z H HT 相对于孔眼的坐标MOVEPHA 接近孔眼MOVEPCH 接触孔眼MOVEPAI 调整销钉MOVEPN 插入RELEASE 松开销钉COORD Z H HT PN 相对于销钉的坐标TOOL E MOVEPA 离开销钉END 机械手的任务位置可以表示为一般形式T6TOOL COORDPOS 8 30 其中T6表示机械手六个关节的变换 TOOL变换表达式 描述手爪端点 或运动受到控制的物体 COORD变换表达式 表示工作坐标系 POS其余的变换表达式 描述所期望的手爪端点或物体的位置 为了到达所确定的机械手任务的任何位置 机械手的一系列位置运动可由下列方程确定T6TOOL1 COORD1POS1T6TOOL2 COORD2POS2 8 31 T6TOOL3 COORD3POS3 8 7位置之间的运动 MotionBetweenPositions 虽然任务位置的序列确定了任务 但并没有说明机械手是如何从一个位置移动到另一个位置的 利用下一个坐标系统和抓手 另外再定义一个POS变换 就可以完成上述工作 在位置1有1T6TOOL1 COORD1 s t 0 1POS1 8 32 下面利用终点位置TOOL2和COORD2再写一个变换表达式1T6TOOL2 COORD2 s t 0 2POS1 8 33 由这两个方程我们能解得利用COORD2和TOOL2定义的对于位置1的变换2POS12POS1 COORD2 1 s t 0 COORD1 s t 0 1POS1TOOL1 1TOOL2 8 34 于是 任何两点i和i 1之间的运动就是从T6 COORD2 s t 0 2POS1TOOL2 1 8 35 到T6 COORD2 s t 0 2POS2TOOL2 1 8 36 的运动 注意 在这些方程中 COORDi 1是运动坐标系统变量 S 的函数 当然 要把机械手从一个位置移到下一个位置可以有许多方法 但是 每一个实际系统必须提供连续的位置和连续的速度 为了防止振荡和蠕动 还要求连续的加速度 以从位置到位置的运动时间Ti为基准 我们可以画出直线坐标 角坐标或者关节坐标 作为时间的函数 图8 11是一个典型的坐标图 由于并不需要在每一个中间点停留 可以先对这条轨迹进行线性近似 如图8 12所示 但是 由于速度和加速度在所有的轨迹定义点上都是不连续的 因而我们并不能实现这样一条线性轨迹 在从一段轨迹过渡到另一条轨迹时为了保证速度 位置和加速度的连续性 可以在区间 tacc t tacc上的位置定义成一种合适的时间函数f t 在一段轨迹终了之前 就开始从这段轨迹向下一个tacc过渡 而在新的轨迹终了之后 就完成了tacc的过渡 考察图8 13所示轨迹 在对轨迹路径段的时间进行估计时 我们假设在时间tacc内把机械手从静止状态加速度到最大速度 在路径段之间的过渡时间为2tacc 如果把f t 选择为一个多项式 由于过渡阶段的对称性 实际上只需要一个四次多项式 q a4t4 a3t3 a2t2 a1t a0 8 37 其中q表示广义的位置 关节的直线位移或角位移 通过求导并运用约束条件 求出式 8 37 中各项系数 可以得到表示过渡期间的位置q 速度以及加速度的函数 8 38 8 39 8 40 约束条件 其中 C C B 8 41 B A B 8 42 8 43 在t tacc时刻过渡结束 已知值由下列各式给定q Ch B 8 44 8 45 8 46 8 47 根据确定的加速时间tacc 我们可以得到使整个机械手能实现的坐标轨迹 见图8 14 规划机械手的全部运动并非必要 我们只需要提前考虑一个位置 当时间t T1 tacc时 就根据下列赋值句 以便开始向下一路径段过渡 T1 T2 A X 当前位置B C C D C C B B A B t TACC 重新启动时刻 本节 我们要考察机械手的关节运动 此时 运动变量q 和由式 8 38 8 47 给定 它们表示关节坐标 在旋转关节的情况下 表示关节角度 i 而在滑动关节的情况下 则表示关节位移di 在一次过渡开始时 如在图8 13的A点 来描述这种类型的运动 并且假设 在t T1 tacc时刻的当前关节坐标为J 当前路径终点C的相应坐标为JC 如果D点用一个运动坐标系统COORD S 来表示 那么就令S S1 对COORD S 求值 S1为S在当前时刻t T1 tacc的值 从而JD Solve COORD S1 POSTOOL 1 8 48 其中 Solve为求解关节坐标 即解逆运动方程 的函数 它是以一个变换表达式为自变量 8 8关节运动 JointMotion 于是 在向D点运动的过程中 每个关节的运动时间就由下式求得 ti JDi JCi vi 8 49 其中 Vi为关节i的最大速度 路径段的时间取为T2 max ti 2tacc 8 50 然后 对完整的路径段进行赋值T1 T2 JA J 当前位置JB JC JC JD JC JC JB JB JA JB T Tacc 重新起动时刻再利用式 8 38 8 48 把JA JB JB代替A B B 就可求解机械手关节的定位点 然而 我们还没有考虑由于运动坐标系COORD S 的影响 为此 可再对相对于终点D的关节坐标进行求值 我们称所求的关节坐标为JC2 此时t T1 在这一时刻 我们把运动坐标变量S估计为S2 S t T1JC2 Solve COORD S2 POSTOOL 1 8 51 计算诸定位点J时 如果简单地给J附加一个修正项以说明C点的运动 那么在机械手达到C点时 我们就会得到追踪C点的运动 J J JC2 JC S S1 S2 S1 说明C点运动的这一修正变化导致了一个比较次要的问题 在过渡的开始点A 我们利用下式不连续地改变了速度 其中 S0为S t 0 对于B点 可将表达式修改为JB JB JC2 JC S0 S1 S2 S1 如果初始条件保持不变 机械手仍然会象原先一样向B点运动 如果初始条件不能保持 那我们还是要规定速度的连续性 因为机械手要追踪C点的运动 所以在修改B点的值以后 还必须重新计算 JB和 JC 最后 在t T1 tacc时刻接近C点时 我们处于定位点J 对C点求值得到JC JC JC2 JC S3 S1 S2 S1 其中 最终的路径段赋值句就变为JA J 当前位置JB JC JC2 JC S3 S1 S2 S1 JC Solve COORD S1 PDS TOOL T1 T2 JC2 Solve COORD S2 PDS TOOL JB JB JC2 JC S0 S1 S2 S1 JC JC JB JB JA JB t TACC重新起动时刻 笛卡尔运动可以非常简单地推广到圆柱坐标 球坐标以及其他正交坐标系统 如同关节坐标运动 轨迹线段是在由齐次变换式描述的位置之间定义的 笛卡尔运动与关节坐标运动的差别在于笛卡尔运动 运动在笛卡尔坐标中是直观的 关节坐标运动 运动在关节坐标中是线性的 笛卡尔运动的优点 轨迹段端点之间的运动很直观 容易定义 因而特别适合于最初和最终的轨迹线段 笛卡尔运动的缺点 它需要对机械手的定位点进行不断的求值 把它变换成各个关节坐标的运动 8 9笛卡尔运动 CartesianMotion 现在我们来研究机械手通过一次平移和两次转动时的情况 第一次转动是使抓手对准目标的方向 第二次转动则控制抓手具有合适的抓取姿态 定向 利用两次转动 关节运动的范围就比较简单 而且也可以预测 从点1到点2的运动可以用一个驱动变换D 来表示 这是一个与 有关的运动函数T6 COORD2 S 2POS1D TOOL2 1 8 52 其中 t T t是从运动开始起算的时间 而T是整个运动的总时间 当开始运动时 0 我们要求D 0 I I为单位变换 在运动终了时 1 我们要求2POS2 2POS1D 1 8 53 8 9 1位置之间的运动 MotionBetweenPositions 从而D 1 2POS1 12POS2 8 54 如果令2POS1 P1 而2POS2 P2 并且把P1和P2的各列表示为向量P1n P10 P1a P1p以及P2n P2a P2p就有 然后对P1进行符号求逆 再右乘P2 得到 8 57 平移沿着连接P1和P2的直线进行并且由变换T 来表示 第一次转动的作用是将抓手指向接近矢量P1a 再从P1旋转到P2位置上的接近矢量P2a 这次旋转是绕矢量k进行的 而k是通过P1和y轴围绕Z轴旋转一个角度 而取得的 因此矢量P1k就由下列给出 8 58 这一旋转可表示成Ra 而由式 2 70 给出 其中k由上式确定 第二次旋转是把表示抓手定向矢量P10 从P1旋转成P2位置上的定向矢量P20 即为Ro 于是 D 就表示为D T Ra Ro 8 59 其中T Ra和Ro具有下列形式 式中x y和z表示从P1到P2的平移分量 8 60 8 62 8 63 8 64 式中 Ra 表示绕P1k的角度为 的旋转 见式8 58 8 61 绕接近矢量的旋转Ro 只是绕Z轴的一次旋转 可由式 2 34 给定 Ro 表示绕抓手的接近矢量角度为 的旋转 式中 8 65 D 的右边三列为令 1 我们就可按下列步骤求解x y z 和 用Ro 1 1Ra 1 1同时右乘式8 59的两边 使右边一列的元素与式8 57的元素相等 我们就得到平移变换T的x y z的值 D 1 Ro 1 1Ra 1 1 T 1 8 69 x p1n p2p p1p 8 70 y p1o p2p p1p 8 71 z p1a p2p p1p 8 72 8 68 用Ro 1 1右乘式 8 59 的两边 然后用T 1 左乘两边 使第二 三 四列的元素与式 8 57 的元素相等 我们就可求解 和 从而有 8 77 及 8 78 即为 8 80 最后 先用T 1 1 再用Ra 1 1左乘式 8 59 的两边 得到Ra 1 1T 1 1D 1 Ro 1 8 79 再使上式两边的两个矩阵中第二行第一列 第二行第二列的元素对应相等 就可得到 8 81 8 82 在上小节中 我们利用驱动变换D 控制的一次平移和两次旋转来描述路径与运动本节要描述路径段之间的过渡 描述方式与在关节坐标中路径过渡的情况 8 8节 相似 这就是说 假定机械手在T1 tacc时刻处于由JA描述的位置 向JB运动 在T1时刻达到JB 再向位置POSC运动 POSC由下列描述T6CTOOL COORD S POSC 8 84 其中T6C是当机械手位于POSC时T6的值 T6 COORD2 S 2POS1D TOOL2 1 8 83 8 9 2路径段之间的过渡 TransitionsBetweenPathsSegments 首先 先求出分别由JA和JB表示T6的两个值 T6A和T6B然后解出相应的位置变换POSA和POSBPOSA COORD S t tacc 1T6ATOOL 8 85 POSB COORD S t 0 1T6BTOOL 8 86 从B点到C点的运动可用一个驱动变换DBC来描述DBC 1 POSB 1POSC 8 87 T6 COORD S POSBDBC TOOLC 1 8 88 用另一个驱动变换DBA描述从B点返回A点的运动DBA 1 POSB 1POSA 8 89 根据上述两个驱动变换表达式 描述从A点经过B点到达C点的运动可这样进行 把D 的 x y z 和 从xA yA zA A和 A经过零点到达xC yC zC C和 C 这样就可以把坐标画成时间的函数 如图8 15所示 现在 我们可以定义 B和 C 并利用式 8 38 8 47 来描述过渡问题以及向着C点的直线运动 驱动变换的坐标由下式给定 在t tacc时刻 过渡结束 各个定位点由下式给定 8 98 8 94 8 95 8 96 8 97 如果 对于两个驱动变换并不相同 那么就必须在过渡期间把它由 A变成 C 我们可以作一线性变化 例5 1 这个例子说明机械手从点POS1经过POS2到达POS3的运动 其中 如图8 16所示 在从点POS1到POS2的运动过程中 我们利用式 8 70 8 82 来确定x y z 和 x 0y 0z 0 我们假定机械手正在从POS1接近POS2 而 0 9 驱动变换于是就由式 8 68 确定而机械手位置为这样 机械手的运动就是从当前位置开始 经过POS2 到达POS3分别相应于第8 9 2节中的位置A B C 驱动变换DBA通过从B到A的运动来确定 驱动参数xA yA zA A A和 A可由式 8 70 8 82 求得 其中B POS1 A POS2 xA 1 00yA 0zA 0 从B点到C点的运动由驱动变换DBC描述 这时 再一次使用 8 70 8 82 我们得到xC 0yC 0zC 0 xA 1 00yA 0zA 0现在 我们能根据从A到C的过渡 求得其中任何一点的运动情况 图8 17 例如 当t 0 T1 10tacc 由式 5 94 确定为0 5 那么式 5 91 就给出各个分量的值 当t tacc 根据式 5 98 0 1 运动变量则由式 8 94 确定为 当t T1 tacc 0 9时 有 按照这种运动方式 机械手实际上并不经过轨迹上任何一点 而是从这些点的附近经过 见图8 18中的B点 机械手仅当停止在某一轨迹点时 它才经过该点 如果试图使机械手以非零速度经过一些中间轨迹点 那么 在所有的轨迹极值点处