机器人学基础—(XXXX蔡自兴)7-第七章 机器人轨迹规划

2010/6/3机电研究组－李挺((机电系机电系))LiTingLiTing（（李挺）李挺）LiTingLiTing’’sResearchandTeachingWebsResearchandTeachingWeb：：://liting.51.netTELTEL：：13861838060QQ13861838060QQ：：6936672746936672742010/6/3机电研究组－李挺（（20102010版本版本))2010/6/3机电研究组－李挺－李挺,controlledfashionistocauseeachjointtomoveasspecifiedbyasmoothfunctionoftime.Exactlyhowtocomputethesemotionfunctionsistheproblemoftrajectory.InordertomovetheendInordertomovetheend--effectoreffectorthroughspacefrompointAtothroughspacefrompointAtopointBwemustcomputeapointBwemustcomputeatrajectoryforeachjointtofollow.trajectoryforeachjointtofollow.Topics6Trajectorygeneration轨迹规划机器人学机器人学第七章第七章轨迹规划轨迹规划2010/6/3机电研究组－李挺§§7.17.1工业机器人的运动工业机器人的运动§§7.1.17.1.1点到点运动点到点运动（（pointpoint--toto--pointpoint））工业机器人：介绍关节空间的运动规划和机器人末端在笛卡尔空间的运动规划。移动机器人：介绍全局路径规划和局部路径规划方法。OZYX点到点运动只关心起始和目标位置点，对运动路径没有限制，所以在点到点运动中，机器人的末端在笛卡尔坐标系具有多条可能的轨迹。机器人各个关节的运动不需要联动，各个关节可以具有不同的运动时间。机器人学机器人学第七章第七章轨迹规划轨迹规划2010/6/3机电研究组－李挺§§7.1.27.1.2轨迹跟踪运动轨迹跟踪运动（（trajectorytrackingtrajectorytracking））轨迹跟踪运动，希望机器人的末端以特定的姿态沿给定的路径运动。为了保证机器人的末端处在给定的路径上，需要计算出路径上各点的位置，以及在各个位置点上机器人所需达到的姿态。上述计算路径上各点处的机器人位置与姿态的过程，称为机器人笛卡尔空间的路径规划。根据规划出的各个路径点处的机器人位置和姿态，利用逆向运动学求取机器人各个关节的目标位置，通过控制各个关节的运动，使机器人的末端到达各个路径点处的期望位姿。轨迹1轨迹2为了使机器人末端尽可能地接近期望轨迹，在进行机器人笛卡尔空间的路径规划时，两个路径点之间的距离应尽可能小。在进行关节空间的运动规划时，要使得各个关节具有相同的运动时间。机器人学机器人学第七章第七章轨迹规划轨迹规划2010/6/3机电研究组－李挺⎪⎪⎪⎪⎪⎪⎩⎪⎪⎪⎪⎪⎪⎨⎧⎪⎪⎩⎪⎪⎨⎧面向任务的动作系列高层规划成由多条基本运动轨迹组完成某一动作中层规划曲线拟合加工过多路径点的曲线拟合线性轨迹规划点定位基本运动轨迹低层规划规划分为三个层次:,::点定位点定位直线插补直线插补PUMA262PUMA262仿真仿真机器人学机器人学第七章第七章轨迹规划轨迹规划2010/6/3机电研究组－李挺§§7.27.2工业机器人的关节空间运动规划工业机器人的关节空间运动规划§§7.2.17.2.1关节空间运动规划关节空间运动规划为了控制机器人的关节空间运动量，并使关节运动轨迹平滑，关节运动平稳，需对机器人的关节运动进行规划。关节运动规划的内容，主要包括关节运动轨迹的选择和关节运动位置的插值。所谓关节位置的插值，是指对于给定关节空间的起始位置和目标位置，通过插值计算中间时刻的关节位置。q(t)qfq0t0tft关节的运动空间机器人学机器人学第七章第七章轨迹规划轨迹规划2010/6/3机电研究组－李挺(t)qtataa(t)qtatataaq(t))(tq)(qq)q(tq)q()t(qqfffff3223213322100062320,00,00+=++=+++=⎩⎨⎧====⎯→⎯⎯→⎯&&&&&对关节角作密化处理时刻§§7.2.27.2.233次多项式插值次多项式插值某关节从t0时刻的关节位置q0运动到tf时刻的关节位置qf，假设在t0和tf时刻机器人的速度均为0。q(t)qfq0t0tft利用3次多项式构造该轨迹，并根据控制周期计算各个路径点的期望关节位置。机器人学机器人学第七章第七章轨迹规划轨迹规划2010/6/3机电研究组－李挺⎪⎪⎪⎩⎪⎪⎪⎨⎧====⎪⎪⎩⎪⎪⎨⎧++==+++==)-q(qt-a)-q(qtaaqatataaatatataaqaqffffffffff03302210023211332210002303200tttqqttqtqqttqqtqtqfffffff)1)((6)()(2)(3)(023032020−−=−−−+=&机器人学机器人学第七章第七章轨迹规划轨迹规划2010/6/3机电研究组－李挺§§7.2.37.2.3过路径点过路径点33次多项式插值次多项式插值taa(t)qtataa(t)qtatataaq(t)q)(tqq)(qq)q(tq)q()t(qqffffff3223213322100006232,0,00+=++=+++=⎩⎨⎧====⎯→⎯⎯→⎯&&&&&&&对关节角作密化处理时刻⎪⎪⎪⎩⎪⎪⎪⎨⎧++====⎪⎪⎩⎪⎪⎨⎧++==+++==)qq(t1)-q(qt2-aqt1-qt2)--q(qt3aqaqat3at2aaqaqtatataaqaqf02f0f3f3ff0f0f2f201002f3f21f103f32f2f10f00&&&&&&&20320032)()(t)qq(t1)-q(qt2-tqt1-qt2)--q(qt3qtqt)qq(t1)-q(qt2-tqt1-qt2)--q(qt3tqqtqf02f0f3fff0f0f2ff02f0f3fff0f0f2f⎥⎥⎦⎤⎢⎢⎣⎡+++⎥⎥⎦⎤⎢⎢⎣⎡+=⎥⎥⎦⎤⎢⎢⎣⎡+++⎥⎥⎦⎤⎢⎢⎣⎡++=&&&&&&&&&&&机器人学机器人学第七章第七章轨迹规划轨迹规划2010/6/3机电研究组－李挺：（1）根据工件坐标系在直角坐标空间中的瞬时线速度和角速度来确定每个路径点的关节速度。（利用操作臂在此路径点上的逆雅可比，把该点的直角坐标速度“映射”为所要求的关节速度，要注意奇异点。但设置密集的速度点要耗费很大的工作量。）（2）在直角坐标空间或关节空间中采用适当的启发式方法，由控制系统自动地选择路径点的速度。（假设用直线段把这些路径点依次连接起来，如果相邻线段的斜率在路径点处改变符号，则把速度选定为零；如果相邻线段不改变符号，则选取路径点两侧的线段斜率的平均值作为该点的速度。）（3）为了保证每个路径点上的加速度连续，由控制系统按此要求自动地选择路径点的速度。（用两条三次多项式曲线在路径点处按一定规则联系起来，拼凑成所要求的轨迹。其约束条件是：联结处不仅速度连续，而且加速度也连续。）机器人控制系统最好同时具有（2）和（3）的功能确定路径点上的关节速度机器人学机器人学第七章第七章轨迹规划轨迹规划2010/6/3机电研究组－李挺：用启发式方法自动生成路径点上的速度机器人学机器人学第七章第七章轨迹规划轨迹规划2010/6/3机电研究组－李挺)p,p,p,gh(pMOVE_throun321L1i=设路径序号?,P,P,P2i1ii并判断是否超限取出将相邻三点++2i611i61i61),(,),(,),(INVERSE++θθθθθθLLL求逆解调用逆解子程序1i621i621+••••••），，（），式计算（界条件，启发由连续三个路径点及边θθθθθθLLf1iitPP点间运行时间由机器人速度，计算+1j＝设关节号为j3210a,a,a,aj）（关节的三次多项式系数计算6?j1jj+=10mst0,t=Δ=tttΔ+=1j=设关节号j33j22j1j0jjttatataatθθ时刻计算）由（+++=送伺服系统转换为驱动值将jjθθ6?j?ttf≥1?-ni=MOVEEND1jj+=1ii+=超限报警！NYNNN2010/6/3机电研究组－李挺§§7.2.47.2.4高阶多项式插值高阶多项式插值⎪⎪⎪⎪⎪⎪⎩⎪⎪⎪⎪⎪⎪⎨⎧−−+−−=−++++−=−−+−−====+++=++++=+++++=⎪⎩⎪⎨⎧======ffffffffffffffffffffffffttqqtqqqqattqqtqqqqattqqtqqqqaqaqaqatatataa(t)qtatatataa(t)qtatatatataaq(t)q)(tqq)(qq)(tqq)(qq)q(tq)q(5200054200043200030201003524324534232155443322100002)()66(12122)23()1614(30302)3()128(202022012625432,0,0,0&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&机器人学机器人学第七章第七章轨迹规划轨迹规划2010/6/3机电研究组－李挺[][][][][][]35200024200020000452000342000232000005520004420003320002000)()66(121210)23()1614(303063)3()128(20203)(2)()66(12125)23()1614(