1、ppt精选版1第第5 5章章 轨迹规划轨迹规划（4学时）学时）学习目的：学习目的：1 理解轨迹规划原理理解轨迹规划原理2 学会用轨迹规划处理实际问题学会用轨迹规划处理实际问题学习内容：学习内容：1 轨迹规划原理轨迹规划原理 2 关节空间的轨迹规划关节空间的轨迹规划3 直角坐标空间的轨迹规划直角坐标空间的轨迹规划4 连续轨迹纪录连续轨迹纪录ppt精选版2定义：定义：如果规定一个机器人从如果规定一个机器人从A点经过点经过B点运动到点运动到C点而不点而不强调时间的概念，那么这一过程中的位形序列就构强调时间的概念，那么这一过程中的位形序列就构成了一条成了一条路径路径。如果我们强调到达其中任意一点的。如
2、果我们强调到达其中任意一点的时间，那么这就是一条时间，那么这就是一条轨迹轨迹。我们可以看出轨迹和。我们可以看出轨迹和路径的区别就在于路径的区别就在于轨迹依赖速度和加速度轨迹依赖速度和加速度。5.1 路径与轨迹路径与轨迹ppt精选版35.2 关节空间描述与直角空间描述关节空间描述与直角空间描述1 关节空间描述关节空间描述如果给定机器人运动的起点和终点，就可以利用逆如果给定机器人运动的起点和终点，就可以利用逆运动学方程计算出每个关节的矢量角度值；然后机运动学方程计算出每个关节的矢量角度值；然后机器人控制器驱动关节电机运动使机器人到达相应的器人控制器驱动关节电机运动使机器人到达相应的位置。这种以关节
3、角度的函数来描述机器人轨迹的位置。这种以关节角度的函数来描述机器人轨迹的方法称为关节空间法。方法称为关节空间法。特点：特点：在机器人运动的过程中，中间状态是不可知在机器人运动的过程中，中间状态是不可知的，但计算量较小，不会出现奇异点的，但计算量较小，不会出现奇异点。2 直角坐标空间描述直角坐标空间描述将轨迹分成若干段，使机器人的运动经过这些中间将轨迹分成若干段，使机器人的运动经过这些中间点，在每一点都求解机器人的关节变量，直到到达点，在每一点都求解机器人的关节变量，直到到达终点，如下图所示：终点，如下图所示：ppt精选版4特点：特点：路径可控且可预知，直观、容易看到机器人路径可控且可预知，直观
4、、容易看到机器人末端轨迹；但计算量大，容易出现奇异点末端轨迹；但计算量大，容易出现奇异点，如下图，如下图所示：所示：直角空间描述直角空间描述ppt精选版5轨迹穿过轨迹穿过机器人自机器人自身身关节值突变关节值突变ppt精选版65.3 轨迹规划的基本原理轨迹规划的基本原理 关节空间的轨迹规划关节空间的轨迹规划 1.计算起点和终点的关节变量，各关节都以最大计算起点和终点的关节变量，各关节都以最大角角 速度运动速度运动 特点：特点：轨迹不规则，末端走过的距离不均匀，且轨迹不规则，末端走过的距离不均匀，且各关节不是同时到达。各关节不是同时到达。ABppt精选版72.在在1的基础上对关节速率做归一化处理，
5、使各关节的基础上对关节速率做归一化处理，使各关节同时到达终点。同时到达终点。特点：特点：各关节同时到达终点，轨迹各部分比较均各关节同时到达终点，轨迹各部分比较均 衡，但所得路径仍然是不规则的。衡，但所得路径仍然是不规则的。ABppt精选版8二二 直角坐标空间轨迹规划直角坐标空间轨迹规划1.首先画出路径，接着将路径首先画出路径，接着将路径n等分等分(为了获得较好为了获得较好的沿循精度，的沿循精度，n越大越好越大越好)，分别计算到达各点所需，分别计算到达各点所需的关节变量。的关节变量。特点：特点：关节角非均匀变化，末端沿已知路径行走。关节角非均匀变化，末端沿已知路径行走。ppt精选版92.在在1的
6、基础上，考虑各关节的加速减速时间，为的基础上，考虑各关节的加速减速时间，为防止在加速期间轨迹落后于设想的轨迹，在划分分防止在加速期间轨迹落后于设想的轨迹，在划分分界点时，如果是直线轨迹，就按照方程划分。曲线界点时，如果是直线轨迹，就按照方程划分。曲线轨迹就相对复杂一些。轨迹就相对复杂一些。ppt精选版103.多点的情况多点的情况（1）从）从A向向B先加速，再匀速，接近先加速，再匀速，接近B时再减速，时再减速，从从B到到C再重复。为避免这一过程中不必要的停止再重复。为避免这一过程中不必要的停止动作，可将动作，可将B点两边的动作进行平滑过渡。机器点两边的动作进行平滑过渡。机器人先抵达人先抵达B点，
7、然后沿着平滑过渡的路径重新加点，然后沿着平滑过渡的路径重新加速，最终抵达并停止在速，最终抵达并停止在C点。点。ppt精选版11（2）考虑到由于采用了平滑过渡曲线，机器人经）考虑到由于采用了平滑过渡曲线，机器人经过的可能不是原来的过的可能不是原来的B点，可事先设定一个不同的点，可事先设定一个不同的B点，使曲线正好经过点，使曲线正好经过B点。点。ppt精选版12(3)在在B点前后各加过渡点点前后各加过渡点D,E，使得，使得B点落在点落在DE上。上。ppt精选版13三三 轨迹规划的分类轨迹规划的分类v1)对于点位作业机器人，需要描述它的起始状态和对于点位作业机器人，需要描述它的起始状态和目标状态。如
8、果用目标状态。如果用 表示工具坐标系的起始值，表示工具坐标系的起始值，表示目标值，就是表示这两个值的相对关系。表示目标值，就是表示这两个值的相对关系。这种运动称为点到点运动这种运动称为点到点运动(PTP)v2)对于弧焊、研磨、抛光等曲面作业，不仅要规定对于弧焊、研磨、抛光等曲面作业，不仅要规定起始点和终止点，还要规定中间整个运动过程。对起始点和终止点，还要规定中间整个运动过程。对于一段连续运动过程，理论上无法精确实现，实际于一段连续运动过程，理论上无法精确实现，实际上是选取一定数量上是选取一定数量(满足轨迹插补精度满足轨迹插补精度)的点作为中的点作为中间点，从而近似实现沿给定的路径运动。间点，
9、从而近似实现沿给定的路径运动。这种运动称为连续路径运动或轮廓运动这种运动称为连续路径运动或轮廓运动(CP)v3)障碍约束轨迹规划障碍约束轨迹规划0TfTppt精选版145.4 关节空间的轨迹规划关节空间的轨迹规划 0)(0)()()(322103fiffiitttttctctcct：这里初始和末端条件是一、一、三次多项式的轨迹规划三次多项式的轨迹规划我们假设机器人某一关节的运动方程是三次的我们假设机器人某一关节的运动方程是三次的ppt精选版15 从上例可以看出，若我们已知开始和终止时刻从上例可以看出，若我们已知开始和终止时刻的角度以及角速度，那么就可以求得的角度以及角速度，那么就可以求得 ，进
10、而求，进而求得关节的运动方程。得关节的运动方程。ic 0320322211322100221322103333fffifffffiitCtCCtCttCtCtCCtCttctccttctctcct得到：将初始和末端条件代入求一阶导数得到：对ppt精选版16 尽管每一个关节都是分别计算的，但是在实际尽管每一个关节都是分别计算的，但是在实际控制中，所有关节自始至终都是同步运动；控制中，所有关节自始至终都是同步运动；如果机器人初始和末端速度不为零，可以通过如果机器人初始和末端速度不为零，可以通过给定数据得到未知数值；给定数据得到未知数值；如果要求机器末端人依次通过两个以上的点，如果要求机器末端人依次
11、通过两个以上的点，则每一段求解出的边界速度和位置均可作为下一段则每一段求解出的边界速度和位置均可作为下一段的初始条件，其余相同；的初始条件，其余相同；位置、速度连续，但是加速度不连续。位置、速度连续，但是加速度不连续。ppt精选版17例例5.1：已知一个关节在已知一个关节在5秒之内从初始角秒之内从初始角30度运动度运动到终端角到终端角75度，使用三次多项式计算在第度，使用三次多项式计算在第1，2，3，4秒时关节的角度。（我们假设在开始和终止的瞬秒时关节的角度。（我们假设在开始和终止的瞬间关节的速度是间关节的速度是0)解：由题意可得到解：由题意可得到0)5(3)5(2)(0)(75)5()5()
12、5()(30)(232113322100ccctctcccctctfiofoi72.04.50303210cccc由此得到位置，速度和加速度的多项式方程如下：由此得到位置，速度和加速度的多项式方程如下：ppt精选版18 tttttttt32.48.1016.28.1072.04.530232 32.70)4(16.59)3(84.45)2(68.34)1(我们可以进一步画出关节的位置，速度和加速度曲线我们可以进一步画出关节的位置，速度和加速度曲线ppt精选版19可以看出，本例中需要的初始加速度为可以看出，本例中需要的初始加速度为10.8度度/秒秒2运动末端的角加速度为运动末端的角加速度为-10
13、.8度度/秒秒2。例题：例题：在例在例5.1的基础上继续运动，要求在其后的的基础上继续运动，要求在其后的3秒内关节秒内关节角到达角到达 。画出该运动的位置，速度和加速度曲线。画出该运动的位置，速度和加速度曲线。105思路点拨：思路点拨：可将第一运动段末端的关节位置和速度可将第一运动段末端的关节位置和速度作为下一运动段的初始条件。作为下一运动段的初始条件。ppt精选版20解：解：tttttttt332.1320)(666.620)(222.21075)(232 进而可以画出以下曲线进而可以画出以下曲线 可以求得其中3062)(32322213221033fitttCCttCtCCttCtCtCC
14、t 10575fi00fippt精选版21为保证为保证机器人机器人的加速的加速度不超度不超过其自过其自身能力，身能力，应考虑应考虑加速度加速度的限制。的限制。2max)()(4ififtt 根据此式可计算出达到目标所需根据此式可计算出达到目标所需要的时间要的时间ppt精选版22二、二、五次多项式轨迹规划五次多项式轨迹规划 同例同例5.1，若采用，若采用五五次多项式，若再已知初始次多项式，若再已知初始加速度和末端减速度均为加速度和末端减速度均为5 度度/秒秒2，其他条件不变，其他条件不变，试画出三条相应曲线。（边界条件变为试画出三条相应曲线。（边界条件变为6个）个）352432453423215
15、544332210201262)(5432)()(tctctccttctctctccttctctctctcct 根据这些方程，可以通过位置、速度和加速度根据这些方程，可以通过位置、速度和加速度边界条件计算出五次多项式的系数。边界条件计算出五次多项式的系数。ppt精选版23352432453423215544332210201262)(5432)()(tctctccttctctctccttctctctctcct 32432543230928.096.66.95)(232.032.28.45)(0464.058.06.15.230)(6.1307530ttttttttttttttccfoi：进而得
16、到如下运动方程中，得出：将初始和末端条件代入秒度秒度/0/0fi22/5/5秒度秒度fi 58.0041cc0464.05.252ccppt精选版24关节位置、速度和加速度图形关节位置、速度和加速度图形ppt精选版25三、抛物线过渡的线性运动轨迹三、抛物线过渡的线性运动轨迹 如果机器人关节以恒定速度运动，那么轨迹方程就相当于如果机器人关节以恒定速度运动，那么轨迹方程就相当于一次多项式，其速度是常数，加速度为一次多项式，其速度是常数，加速度为0，这说明在起点和终，这说明在起点和终点，加速度为无穷大，只有这样才可以瞬间达到匀速状态。但点，加速度为无穷大，只有这样才可以瞬间达到匀速状态。但很显然这是
17、不可能的，因此在起点和终点处，可以用抛物线来很显然这是不可能的，因此在起点和终点处，可以用抛物线来进行过渡。如图所示进行过渡。如图所示ppt精选版2622222102102212210)()(21)(0)(0)0()0()()(21)(cttcttctcccctctctcttccttctcctiii 显然，这个抛物线运动段的加速度是一常数，并在公共点显然，这个抛物线运动段的加速度是一常数，并在公共点A和和B上产生连续的速度。将边界条件代入抛物线段的方程，上产生连续的速度。将边界条件代入抛物线段的方程，得到：得到：从而给出抛物线段的方程为：从而给出抛物线段的方程为：假设假设ti和和 tf时刻对应
18、的起点和终点时刻对应的起点和终点位置为位置为 和和 ，抛物线与直线，抛物线与直线部分的过渡段在时间部分的过渡段在时间tb和和tf-tb处是处是对称的，因此可得：对称的，因此可得：ifppt精选版270221222fiABfABbfAbbfABbAbiAttttttctc显然，对于直线段，速度将保持为常值，它可以根据显然，对于直线段，速度将保持为常值，它可以根据驱动器的物理性能来加以选择。将零出速度、线性段驱动器的物理性能来加以选择。将零出速度、线性段常值速度常值速度 以及零末端速度代入以及零末端速度代入 和和 中，中，可以得到可以得到A、B点以及终点的关节位置和速度如下：点以及终点的关节位置和
19、速度如下：2221)(tctitct2)(ppt精选版28显然，显然，不能大于总时间不能大于总时间 的一半，否则在整个过程中的一半，否则在整个过程中将没有直线运动段而只有抛物线加速和抛物线减速段。将没有直线运动段而只有抛物线加速和抛物线减速段。由上式可以计算出对应的最大速度由上式可以计算出对应的最大速度 。应该。应该说明，如果运动段的初始时间不是说明，如果运动段的初始时间不是0而是而是 ，则可采用，则可采用平移时间轴的办法使初始时间为平移时间轴的办法使初始时间为0。终点的抛物线段是。终点的抛物线段是对称的，只是其加速度为负。因此可表示为：对称的，只是其加速度为负。因此可表示为：bfbbifbf
20、bifbtttttttctc222222fift/)(2max由上式可以求解过渡时间：由上式可以求解过渡时间：进而由上式可以解得过渡时间：进而由上式可以解得过渡时间：ffibttbtftatppt精选版29 bfftcttct22221其中bfbfbftttttttttt)()()()(2)(2 ppt精选版30例题：例题：若已知某关节以速度若已知某关节以速度 =10度度/秒在秒在5秒内从秒内从初始角初始角 运动到目的角运动到目的角 。求解所需的过渡。求解所需的过渡时间并绘制位置、速度和加速度曲线。时间并绘制位置、速度和加速度曲线。130i70f解：代入相应公式可得到解：代入相应公式可得到10
21、10530,12 ttsttfBBAAiffib的方程如下所示到由到由到由01010 tA10)5(10)5(5702 tt曲线如下图所示：曲线如下图所示：ppt精选版31ppt精选版325.5 直角坐标空间的轨迹规划直角坐标空间的轨迹规划 实际上，所有用于关节空间轨迹规划的方法实际上，所有用于关节空间轨迹规划的方法都可用于直角坐标空间的轨迹规划。最根本的差都可用于直角坐标空间的轨迹规划。最根本的差别在于，直角坐标空间轨迹规划必须反复求解逆别在于，直角坐标空间轨迹规划必须反复求解逆运动学方程来计算关节角，也就是说，对于关节运动学方程来计算关节角，也就是说，对于关节空间轨迹规划，规划函数生成的值
22、就是关节值，空间轨迹规划，规划函数生成的值就是关节值，而直角坐标空间轨迹规划函数生成的值是机器人而直角坐标空间轨迹规划函数生成的值是机器人末端手的位姿，他们需要通过求解逆运动学方程末端手的位姿，他们需要通过求解逆运动学方程化为关节量。化为关节量。ppt精选版33 以上过程可以简化为如下的计算循环：以上过程可以简化为如下的计算循环：在工业应用中，最实用的轨迹是点到点之间的直在工业应用中，最实用的轨迹是点到点之间的直线运动，但也经常碰到多目标点线运动，但也经常碰到多目标点(例如有中间点例如有中间点)间需间需要平滑过渡的情况。要平滑过渡的情况。为实现一条直线轨迹，必须计算起点和终点位姿为实现一条直线
23、轨迹，必须计算起点和终点位姿之间的变换，并将该变换划分许多小段。起点构型之间的变换，并将该变换划分许多小段。起点构型(1)将时间增加一个增量将时间增加一个增量t=t+(2)利用所选择的轨迹函数计算出手的位姿利用所选择的轨迹函数计算出手的位姿(3)利用机器人逆运动学方程计算出对应手位姿利用机器人逆运动学方程计算出对应手位姿的关节量的关节量(4)将关节信息送给控制器将关节信息送给控制器tppt精选版34和终点构型和终点构型 之间的总变换之间的总变换R可通过下面的方程进行可通过下面的方程进行计算：计算：iTfiiifiifTTRRTTTTRTT111 至少有以下三种不同方法可用来将该总变换化为至少有
24、以下三种不同方法可用来将该总变换化为许多的小段变换。许多的小段变换。(1)希望在起点和终点之间有平滑的线性变换，因希望在起点和终点之间有平滑的线性变换，因此需要大量很小的分段，从而产生了大量的微分运动。此需要大量很小的分段，从而产生了大量的微分运动。利用上一章导出的微分运动方程，可将末端手坐标系利用上一章导出的微分运动方程，可将末端手坐标系在每个新段的位姿与微分运动、雅可比矩阵及关节速在每个新段的位姿与微分运动、雅可比矩阵及关节速度通过下列方程联系在一起。度通过下列方程联系在一起。ppt精选版35dTTTTdTJDDoldnewDJD1这一方法需要进行大量的计算，并且仅当雅可比矩这一方法需要进
25、行大量的计算，并且仅当雅可比矩阵逆存在时才有效。阵逆存在时才有效。(2)在起点和终点之间的变换分解为一个平移和两在起点和终点之间的变换分解为一个平移和两个旋转。平移是将坐标原点从起点移动到终点，第个旋转。平移是将坐标原点从起点移动到终点，第一个旋转是将末端手坐标系与期望姿态对准，而第一个旋转是将末端手坐标系与期望姿态对准，而第二个旋转是手坐标系绕其自身周转到最终的姿态。二个旋转是手坐标系绕其自身周转到最终的姿态。所有这所有这3个变换同时进行。个变换同时进行。ppt精选版36(3)在起点和终点之间的变换在起点和终点之间的变换R分解为一个平移和一个分解为一个平移和一个K轴的旋转。平移仍是将坐标原点
26、从起点移动到终点，轴的旋转。平移仍是将坐标原点从起点移动到终点，而旋转则是将手臂坐标系与最终的期望姿态对准。两而旋转则是将手臂坐标系与最终的期望姿态对准。两个变换同时进行。个变换同时进行。ppt精选版376.78.08.0381014814xyxy例例5.6 一个两自由度平面机器人要求从起点一个两自由度平面机器人要求从起点(3,10)沿直沿直线运动到终点线运动到终点(8,14)。假设路径分为。假设路径分为10段，求出机器段，求出机器人的关节变量。每一根连杆的长度为人的关节变量。每一根连杆的长度为9英寸。英寸。解：直角坐标空间中起点和终点间的直线可描述为：解：直角坐标空间中起点和终点间的直线可描
27、述为：中间点的坐标可以通过将起点和终点的中间点的坐标可以通过将起点和终点的x,y坐标坐标之差简单地加以分割得到，然后通过求解逆运动学方之差简单地加以分割得到，然后通过求解逆运动学方程得到对应每个中间点的两个关节角。程得到对应每个中间点的两个关节角。或者或者ppt精选版38#XY131018.810923.510.419104.03410.819.5100.444.511.220.295.85511.621.390.965.51222.585.77612.424.180.186.512.82674.29713.228.267.8107.513.630.860.71181433.952.812pp
28、t精选版39ppt精选版40 133312221311181arccos162162arccosarctanCCSPCCPPCPPPyZzyyx例：例：一个一个3自由度机器人有两根连杆，每根连杆长自由度机器人有两根连杆，每根连杆长9英寸。假设定义坐标系使得当所有关节角均为英寸。假设定义坐标系使得当所有关节角均为0时手时手臂处于垂直向上状态。要求机器人沿直线从点（臂处于垂直向上状态。要求机器人沿直线从点（9，6，10）移动到点（）移动到点（3，5，8）。求）。求3各关节在每个中各关节在每个中间点的角度值，并绘制出这些角度值。已知：间点的角度值，并绘制出这些角度值。已知：ppt精选版41机器人及其
29、坐标系机器人及其坐标系ppt精选版42解：在求解本题时，可将起点与终点之间解：在求解本题时，可将起点与终点之间10等分。通等分。通过求解逆运动学方程既可算得每个中间点的关节角。过求解逆运动学方程既可算得每个中间点的关节角。123手坐标系及关节角手坐标系及关节角ppt精选版43机器人的关节角曲线机器人的关节角曲线ppt精选版445.6 连续轨迹记录连续轨迹记录v 有时机器人为了完成某些复杂的或轨迹难以用有时机器人为了完成某些复杂的或轨迹难以用直线或其他高次多项式来描述的任务，可以首先示直线或其他高次多项式来描述的任务，可以首先示教机器人如何运动，同时记录这些运动数据，然后教机器人如何运动，同时记
30、录这些运动数据，然后回放所记录的运动并执行该运动。在整个运动过程回放所记录的运动并执行该运动。在整个运动过程中都必须不断地采样并记录关节量，此后通过回放中都必须不断地采样并记录关节量，此后通过回放采样数据并驱动机器人的关节来驱动使机器人跟踪采样数据并驱动机器人的关节来驱动使机器人跟踪所记录的轨迹，从而完成所规划的任务。所记录的轨迹，从而完成所规划的任务。v 前面所讲的示教回放机器人前面所讲的示教回放机器人v v特点：特点：所需的编程和计算量较少，但需要精确执行、所需的编程和计算量较少，但需要精确执行、采样和记录所有的运动；每当部分运动需要改变时采样和记录所有的运动；每当部分运动需要改变时就必须重新示教编程。就必须重新示教编程。ppt精选版45v小结：小结：v轨迹规划既可在关节空间进行也可在直角坐标进轨迹规划既可在关节空间进行也可在直角坐标进行。在两种空间的许多方法可以通用。行。在两种空间的许多方法可以通用。v直角坐标空间的轨迹规划比较直观，但是较难计直角坐标空间的轨迹规划比较直观，但是较难计算和规划。对于已指定的路径（如直线）必须在算和规划。对于已指定的路径（如直线）必须在直角空间进行规划才能实现。直角空间进行规划才能实现。v如果没有指定机器人的路径，则关节空间的轨迹如果没有指定机器人的路径，则关节空间的轨迹规划更容易计算。规划更容易计算。
