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

1FundamentalsofRoboticsFundamentalsFundamentalsofRoboticsofRoboticsJiangnanJiangnanUniversityUniversityTheSchoolofMechanicalEngineeringTheSchoolofMechanicalEngineeringTheDepartmentofTheDepartmentofMechatronicsMechatronics((机电系机电系))LiTingLiTing（（李挺）李挺）LiTingLiTing’’sResearchandTeachingWebsResearchandTeachingWeb：：：：13861838060QQ13861838060QQ：：6936672746936672742第第七七章章机器人轨迹规划机器人轨迹规划（（20122012版本版本))机器人学基础机器人学基础3第七章第七章机器人轨迹规划机器人轨迹规划7.17.1轨迹规划应该考虑的问题轨迹规划应该考虑的问题7.27.2关节轨迹的插值计算关节轨迹的插值计算7.37.3笛卡儿路径轨迹规划笛卡儿路径轨迹规划7.47.4规划轨迹的实时生成规划轨迹的实时生成7.57.5高级规划的任务和方法高级规划的任务和方法7.67.6小结小结机器人学基础机器人学基础4第七章第七章机器人轨迹规划机器人轨迹规划机器人学基础机器人学基础所谓轨迹，是指机械手在运动过程中的位移位移、速度速度和加速度加速度。而轨迹规划是根据作业任务的要求，计算出预期的运动轨迹。工业机器人：介绍关节空间的运动规划和机器人末端在笛卡尔空间的运动规划。移动机器人：介绍全局路径规划和局部路径规划方法。5§§7.17.1轨迹规划应该考虑的问题轨迹规划应该考虑的问题第七章第七章机器人轨迹规划机器人轨迹规划路径约束和障碍约束的组合把机器人的规划与控制方式划分为四类，如表7.1所示。表7.1操作臂控制方式障碍约束有无有离线无碰撞路径规划＋在线路径跟踪离线路径规划＋在线路径跟踪路径约束无位置控制＋在线障碍探测和避障位置控制6§§7.17.1轨迹规划应该考虑的问题轨迹规划应该考虑的问题第七章第七章机器人轨迹规划机器人轨迹规划轨迹规划器轨迹规划器可形象地看成为一个黑箱黑箱（见图7.1），其输入包括路径的设定和约束，输出的是机械手末端手部的位姿序列，表示手部在各离散时刻的中间位形。7.1图轨迹规划器框图7§§7.17.1轨迹规划应该考虑的问题轨迹规划应该考虑的问题第七章第七章机器人轨迹规划机器人轨迹规划机械手最常用的轨迹规划方法有两种：•第一种方法要求用户对于选定的转变结点对于选定的转变结点（插值点）上上的位姿、速度和加速度的位姿、速度和加速度给出一组显式给出一组显式约束约束（例如连续性和光滑程度等），轨迹规划轨迹规划器器从一类函数（例如n次多项式）中选取参数化轨迹，对结点进行插值，并满足约束条件对结点进行插值，并满足约束条件。•第二种方法要求用户给出运动路径的解析式给出运动路径的解析式；如为直角坐标空间中的直线路径，轨迹规划器轨迹规划器在关节空间或直角坐标空间中确定一条轨迹来确定一条轨迹来逼近预定的路径逼近预定的路径。8§§7.17.1轨迹规划应该考虑的问题轨迹规划应该考虑的问题第七章第七章机器人轨迹规划机器人轨迹规划•在第一种方法中，约束的设定和轨迹规划约束的设定和轨迹规划均在关节空间进行均在关节空间进行。因此可能会发生与障可能会发生与障碍物相碰碍物相碰。•第二种方法的路径约束是在直角坐标空间约束是在直角坐标空间中给定的中给定的，而关节驱动器是在关节空间中受控的。9⎪⎪⎪⎪⎪⎪⎩⎪⎪⎪⎪⎪⎪⎨⎧⎪⎪⎩⎪⎪⎨⎧面向任务的动作系列高层规划成由多条基本运动轨迹组完成某一动作中层规划曲线拟合加工过多路径点的曲线拟合线性轨迹规划点定位基本运动轨迹低层规划规划分为三个层次:,::点定位点定位直线插补直线插补PUMA262PUMA262仿真仿真§§7.17.1轨迹规划应该考虑的问题轨迹规划应该考虑的问题第七章第七章机器人轨迹规划机器人轨迹规划10§§7.17.1轨迹规划应该考虑的问题轨迹规划应该考虑的问题第七章第七章机器人轨迹规划机器人轨迹规划OZYX点到点运动（点到点运动（pointpoint--toto--pointpoint））点到点运动只关心起始和目标位置点，对运动路径没有限制。11轨迹跟踪运动（轨迹跟踪运动（trajectorytrackingtrajectorytracking））轨迹跟踪运动，希望机器人的末端以特定的姿态沿给定的路径运动。为了保证机器人的末端处在给定的路径上，需要计算出路径上各点的位置，以及在各个位置点上机器人所需达到的姿态。上述计算路径上各点处的机器人位置与姿态的过程，称为机器人笛卡尔空间的路径规划。根据规划出的各个路径点处的机器人位置和姿态，利用逆向运动学求取机器人各个关节的目标位置，通过控制各个关节的运动，使机器人的末端到达各个路径点处的期望位姿。轨迹1轨迹2为了使机器人末端尽可能地接近期望轨迹，在进行机器人笛卡尔空间的路径规划时，两个路径点之间的距离应尽可能小。在进行关节空间的运动规划时，要使得各个关节具有相同的运动时间。§§7.17.1轨迹规划应该考虑的问题轨迹规划应该考虑的问题第七章第七章机器人轨迹规划机器人轨迹规划12§§7.27.2关节轨迹的插值计算关节轨迹的插值计算第七章第七章机器人轨迹规划机器人轨迹规划对关节进行插值时，应满足一系列的约束条件，例如抓取物体时，手部运动方向（初始点），提升物体离开的方向（提升点），放下物体（下放点）和停止点等结点上的位姿、速度和加速度的要求；与此相应的各个关节位移、速度、加速度在整个时间间隔内连续性要求；其极值必须在各个关节变量的容许范围之内等。在满足所要求的约束条件下，可以选取不同类型的关节插值函数，生成不同的轨迹。13§§7.27.2关节轨迹的插值计算关节轨迹的插值计算§§7.2.17.2.1三三次多项式插值次多项式插值θ(t)θfθ0t0tft第七章第七章机器人轨迹规划机器人轨迹规划运动轨迹的描述可用起始点关节角度与终止点关节角度的一个平滑插值函数来表示，在t0＝0时刻的值是起始关节角度，在终端时刻tf的值是终止关节角度。显然，有许多平滑函数可作为关节插值函数，如图7.2所示。7.2图单个关节的不同轨迹曲线14对关节角作密化处理时刻)t(taa(t)tataa(t)tatataa(t))(t)()(t)(fffff⎯→⎯⎯→⎯+=++=+++=⎪⎩⎪⎨⎧==⎩⎨⎧==06232000003223213322100θθθθθθθθθθθ&&&&&为了实现单个关节的平稳运动，轨迹函数至少需要满足四个约束条件四个约束条件。利用3次多项式构造该轨迹，并根据控制周期计算各个路径点的期望关节位置。7.27.2关节轨迹的插值计算关节轨迹的插值计算§§7.2.17.2.1三次多项式插值三次多项式插值θ(t)θfθ0t0tft15⎪⎪⎪⎩⎪⎪⎪⎨⎧====⎪⎪⎩⎪⎪⎨⎧++==+++==)-(t-a)-(taaatataaatatataaaffffffffff03302210023211332210002303200θθθθθθθttttttttttfffffff)1)((6)()(2)(3)(023032020−−=−−−+=θθθθθθθθθ&7.27.2关节轨迹的插值计算关节轨迹的插值计算§§7.2.17.2.1三次多项式插值三次多项式插值求解约束条件：可得：16§§7.27.2关节轨迹的插值计算关节轨迹的插值计算§§7.2.27.2.2过路径点的过路径点的三三次多项式插值次多项式插值第七章第七章机器人轨迹规划机器人轨迹规划可以把所有路径点也看作是“起始点”或“终止点”，求解逆运动学，得到相应的关节矢量值。然后确定所要求的三次多项式插值函数，把路径点平滑地连接起来。但是，在这些“起始点”和“终止点”的关节运动速度不再是零。17§§7.2.27.2.2过路径点的三次多项式插值过路径点的三次多项式插值路径点上的关节速度可以根据需要设定，这样一来，确定三次多项式的方法与前面所述的完全相同，只是速度约束条件变为7.27.2关节轨迹的插值计算关节轨迹的插值计算对关节角作密化处理时刻)0(623200032232133221000ffffffttaa(t)tataa(t)tatataa(t))(t)(t)()(⎯→⎯⎯→⎯+=++=+++=⎩⎨⎧==⎩⎨⎧==θθθθθθθθθθθθθ&&&&&&&四个约束条件四个约束条件1823213322100032,0,0tataa(t)tatataa(t))(t)()(t)(ffff++=+++=⎩⎨⎧====θθθθθθθθθθ&&&&&⎪⎪⎪⎩⎪⎪⎪⎨⎧++====)(t1)-(t2-at1-t2)--(t3aaaf02f0f3f3ff0f0f2f20100θθθθθθθθθθ&&&&&20320032)()(t)(t1)-(t2-tt1-t2)--(t3tt)(t1)-(t2-tt1-t2)--(t3ttf02f0f3fff0f0f2ff02f0f3fff0f0f2f⎥⎥⎦⎤⎢⎢⎣⎡+++⎥⎥⎦⎤⎢⎢⎣⎡+=⎥⎥⎦⎤⎢⎢⎣⎡+++⎥⎥⎦⎤⎢⎢⎣⎡++=θθθθθθθθθθθθθθθθθθθθθ&&&&&&&&&&&§§7.2.27.2.2过路径点的三次多项式插值过路径点的三次多项式插值求得三次多项式的系数：⎪⎪⎩⎪⎪⎨⎧++==+++==2f3f21f103f32f2f10f00t3at2aaatatataaaθθθθ&&7.27.2关节轨迹的插值计算关节轨迹的插值计算19（1）根据工件坐标系在直角坐标空间中的瞬时线速度和角速度来确定每个路径点的关节速度。（利用操作臂在此路径点上的逆雅可比，把该点的直角坐标速度“映射”为所要求的关节速度，要注意奇异点。但设置密集的速度点要耗费很大的工作量。）（2）在直角坐标空间或关节空间中采用适当的启发式方法，由控制系统自动地选择路径点的速度。（假设用直线段把这些路径点依次连接起来，如果相邻线段的斜率在路径点处改变符号，则把速度选定为零；如果相邻线段不改变符号，则选取路径点两侧的线段斜率的平均值作为该点的速度。）（3）为了保证每个路径点上的加速度连续，由控制系统按此要求自动地选择路径点的速度。（用两条三次多项式曲线在路径点处按一定规则联系起来，拼凑成所要求的轨迹。其约束条件是：联结处不仅速度连续，而且加速度也连续。机器人控制系统最好同时具有（2）和（3）的功能三种方法确定路径点上的关节速度§§7.2.27.2.2过路径点的三次多项式插值过路径点的三次多项式插值7.27.2关节轨迹的插值计算关节轨迹的插值计算20θi0θAθBθCθDθEθθ14θ2θ3θAtBtCtDtEt方法二：用启发式方法自动生成路径点上的速度§§7.2.27.2.2过路径点的三次多项式插值过路径点的三次多项式插值7.27.2关节轨迹的插值计算关节轨迹的插值计算21过多路径点三次多项式规划流程图1i=设路径序号?,P,P,P2i1ii并判断是否超限取出将相邻三点++2i611i61i61),(,),(,),(INVERSE++θθθθθθLLL求逆解调用逆解子程序1i621i621+••••••），，（），式计算（界条件，启发由连续三个路径点及边θθθθθθLLf1iitPP,点间运行时间计算由机器人速度+1j＝设关节号为j3210a,a,a,aj）（关节的三次多项式系数计算1jj+=10mst0,t=Δ=tttΔ+=1j=设关节号j33j22j1j0jjttatataatθθ时刻计算）由（+++=1jj+=超限报警！NYNNN?6j)p,p,p,p(ghMove_throun321LAA?6j?ttf≥?1ni−Return1ii+=送伺服系统转换成驱动值将jjθθ22§§7.27.