1、1第第7 7章章 机器人路径规划机器人路径规划在机器人完成指定任务时，需要规划机器人在空间中的期望运动在机器人完成指定任务时，需要规划机器人在空间中的期望运动轨迹轨迹或者或者路径路径。路径和轨迹是两个相似但含义不同的概念，机器人运动的路径描述机器人的位路径和轨迹是两个相似但含义不同的概念，机器人运动的路径描述机器人的位姿随空间的变化，而机器人运动的轨迹描述机器人的位姿随时间的变化。姿随空间的变化，而机器人运动的轨迹描述机器人的位姿随时间的变化。所谓轨迹是指机器人每个自由度的位置、速度和加速度的时间历程。所谓轨迹是指机器人每个自由度的位置、速度和加速度的时间历程。本章将介绍移动机器人路径规划和机
2、械臂的轨迹规划问题。本章将介绍移动机器人路径规划和机械臂的轨迹规划问题。u移动机器人路径规划移动机器人路径规划 移动机器人路径规划的任务移动机器人路径规划的任务:已知机器人初始位姿、给定机器人的目标位已知机器人初始位姿、给定机器人的目标位姿，在存在障碍的环境中规划一条无碰撞、时间（能量）最优的路径。姿，在存在障碍的环境中规划一条无碰撞、时间（能量）最优的路径。若已知环境地图，即已知机器人模型和障碍模型，可采用基于模型的路径规划。若已知环境地图，即已知机器人模型和障碍模型，可采用基于模型的路径规划。若机器人在未知或动态环境中移动，机器人需要向目标移动、同时需要使若机器人在未知或动态环境中移动，机
3、器人需要向目标移动、同时需要使用传感器探测障碍，称为基于传感器的路径规划。用传感器探测障碍，称为基于传感器的路径规划。为了简化问题描述，假定机器人为两个自由度，即只考虑机器人的位置，为了简化问题描述，假定机器人为两个自由度，即只考虑机器人的位置，不考虑其姿态。不考虑其姿态。2图图7-17-1障碍物扩张法路径规划障碍物扩张法路径规划 任务是规划一条路径，使得机任务是规划一条路径，使得机器人从起点达到目标点（终点），器人从起点达到目标点（终点），同时不与环境中的障碍发生碰撞。同时不与环境中的障碍发生碰撞。以平面全向移动机器人为例，以平面全向移动机器人为例，假设机器人为半径为假设机器人为半径为r的圆
4、形机构。的圆形机构。首先，由于机器人可以全方向移动，所以可以忽略移动机器人的方向（姿首先，由于机器人可以全方向移动，所以可以忽略移动机器人的方向（姿态的自由度）。态的自由度）。其次，因为能用园表示机器人，所以可把障碍物沿径向扩张其次，因为能用园表示机器人，所以可把障碍物沿径向扩张r的宽度，同的宽度，同时将机器人收缩成一个点（如图时将机器人收缩成一个点（如图7-1所示）。所示）。因此，移动机器人路径规划可以简化为在扩张了障碍物的地图上，点机器因此，移动机器人路径规划可以简化为在扩张了障碍物的地图上，点机器人的路径规划问题。人的路径规划问题。3u人工势场方法人工势场方法 人工势场的基本思想是构造目
5、标位置引力场和人工势场的基本思想是构造目标位置引力场和障碍物周围斥力场共同作用下的人工势场。障碍物周围斥力场共同作用下的人工势场。搜索势函数的下降方向来寻找无碰撞路径。搜索势函数的下降方向来寻找无碰撞路径。下面给出各种势场的定义下面给出各种势场的定义 目标引力场目标引力场 212attgoalEK-ppp7-1 其中其中p是机器人位置，是机器人位置，pgoal是目标位置，是目标位置，K是引力常数。是引力常数。图图7-27-2机器人受力示意图机器人受力示意图障碍物斥力场障碍物斥力场 2001120obsrepobsdEdelse-ppppp 7-2其中其中pobs是障碍物位置，是障碍物位置，d0
6、表示障碍物的影响范围，表示障碍物的影响范围，h是斥力常数。是斥力常数。根据（根据（7-17-1）式，机器人受到的引力表示为）式，机器人受到的引力表示为 attattgoalEK-Fppp 7-34图图7-27-2机器人受力示意图机器人受力示意图 根据（根据（7-27-2）式，机器人在障碍物的影响）式，机器人在障碍物的影响范围内受到的斥力表示为范围内受到的斥力表示为 3011obsrepobsobsd-ppFppppp7-4 可得机器人所受合力为：可得机器人所受合力为：Ftotal=Fatt+Fobs (7-5)这样，我们就在环境地图中定义了机器人的引力场这样，我们就在环境地图中定义了机器人的引
7、力场 因此，机器人的路径规划问题被转化为点在引力场中的运动问题。因此，机器人的路径规划问题被转化为点在引力场中的运动问题。而点在引力场中的运动问题在物理学和数学中已经研究得非常清楚，可以而点在引力场中的运动问题在物理学和数学中已经研究得非常清楚，可以比较方便地进行求解。比较方便地进行求解。算法优点：算法优点：简单方便，可以实时规划控制，并能考虑多个障碍，连续移动。简单方便，可以实时规划控制，并能考虑多个障碍，连续移动。规划的路径比较平滑安全。规划的路径比较平滑安全。5算法缺点：算法缺点：规划算法是局部最优算法规划算法是局部最优算法 复杂多障碍环境中可能出现复杂多障碍环境中可能出现局部极值点，即
8、在非目标点达局部极值点，即在非目标点达到平衡状态而停滞。到平衡状态而停滞。因而不能规划出达到目标点的路径。因而不能规划出达到目标点的路径。前面只介绍了基本的人工势场方法。近年来，针对基本人工势场方法的前面只介绍了基本的人工势场方法。近年来，针对基本人工势场方法的的不足，人们提出了许多改进的人工势场方法。的不足，人们提出了许多改进的人工势场方法。图图7-37-3栅格法路径规划示意图栅格法路径规划示意图u栅格法栅格法 栅格法的基本思想：栅格法的基本思想：将机器人工作空间划分为多个简将机器人工作空间划分为多个简单区域，称为栅格。单区域，称为栅格。若栅格内没有障碍物称为自由栅格，若栅格内没有障碍物称为
9、自由栅格，否则称障碍栅格。否则称障碍栅格。将栅格编号，机器人路径规划就是将栅格编号，机器人路径规划就是搜索由起点到目标点的自由栅格组成的搜索由起点到目标点的自由栅格组成的连通域。连通域。可以用栅格序号表示，再将栅格序号转换成机器人空间的实际坐标，令可以用栅格序号表示，再将栅格序号转换成机器人空间的实际坐标，令机器人按此路径运动。机器人按此路径运动。图图7-37-3给出了栅格法路径规划的示意图。给出了栅格法路径规划的示意图。6栅格法路径规划步骤：栅格法路径规划步骤：1.1.建立栅格。将机器人和目标点间区域划分栅格，大小与机器人相关。建立栅格。将机器人和目标点间区域划分栅格，大小与机器人相关。2.
10、2.障碍地图生成。标注障碍栅格和自由栅格。障碍地图生成。标注障碍栅格和自由栅格。3.3.搜索无障碍最优路径，搜索无障碍最优路径，AAAA*搜索算法，遗传算法，人工势场，蚁群算法等。搜索算法，遗传算法，人工势场，蚁群算法等。优点：优点：若存在最优路径，算法得当一定可以得到问题最优解。若存在最优路径，算法得当一定可以得到问题最优解。有成熟的路径搜索算法使用。有成熟的路径搜索算法使用。缺点：缺点：栅格粒度影响较大。划分细时，存贮大和搜索时间长。栅格粒度影响较大。划分细时，存贮大和搜索时间长。得到的是折线，需要光滑处理。得到的是折线，需要光滑处理。7u机械臂路径规划机械臂路径规划 在实际问题当中，一般
11、用工具坐在实际问题当中，一般用工具坐标系标系T相对工作台坐标系相对工作台坐标系S的运动的运动来描述机械臂的运动。来描述机械臂的运动。当用工具坐标系当用工具坐标系T相对工作台坐标系相对工作台坐标系S的运动来描述机械臂的路径时，的运动来描述机械臂的路径时，使得路径规划与具体的机械臂、末端执行器和工件相分离。使得路径规划与具体的机械臂、末端执行器和工件相分离。这种规划方法具有通用性，适合不同的机械臂和工具，同时也适用于运这种规划方法具有通用性，适合不同的机械臂和工具，同时也适用于运动的工作台（如传送带）。动的工作台（如传送带）。在进行机械臂路径规划时，经常需要规划运动的细节，而不是简单地指定在进行机
12、械臂路径规划时，经常需要规划运动的细节，而不是简单地指定期望的终端位姿。期望的终端位姿。例如，一个完整的操作由若干步组成，每一步的都有期望的位姿，或者在例如，一个完整的操作由若干步组成，每一步的都有期望的位姿，或者在机械臂运动过程中需要规避障碍等。机械臂运动过程中需要规避障碍等。解决该问题的方法是在规划的路径中增加一系列的中间点。解决该问题的方法是在规划的路径中增加一系列的中间点。为了完成整个运动，工具坐标系必须通过中间点所描述的一系列过渡位姿。为了完成整个运动，工具坐标系必须通过中间点所描述的一系列过渡位姿。称路径的起点、中间点和终点为称路径的起点、中间点和终点为路径点路径点。通常都期望机械
13、臂的运动过程是平滑的，因此一般要求规划的路径是光滑通常都期望机械臂的运动过程是平滑的，因此一般要求规划的路径是光滑的，至少具有连续的一阶导数，甚至要求二阶导数也是连续的。的，至少具有连续的一阶导数，甚至要求二阶导数也是连续的。一阶导数对应机械臂的运动速度，二阶导数对应加速度。一阶导数对应机械臂的运动速度，二阶导数对应加速度。光滑性要求就是要使机械臂的运动更加平稳，避免突然的剧烈加速或者光滑性要求就是要使机械臂的运动更加平稳，避免突然的剧烈加速或者减速产生冲击作用而影响机械臂的运动精度并加剧机构的磨损。减速产生冲击作用而影响机械臂的运动精度并加剧机构的磨损。8u关节空间规划方法关节空间规划方法
14、前面介绍了机械臂的期望运动一般由指定的路径点来描述，其中的每个前面介绍了机械臂的期望运动一般由指定的路径点来描述，其中的每个点都代表工具坐标系点都代表工具坐标系T相对工作台坐标系相对工作台坐标系S的位姿。的位姿。我们可以采用第我们可以采用第4 4章介绍的逆运动学方法获得这些路径点对应的关节角度。章介绍的逆运动学方法获得这些路径点对应的关节角度。规定机械臂的关节同步运动，即每个关节角都同时达到路径点期望的角度。规定机械臂的关节同步运动，即每个关节角都同时达到路径点期望的角度。上述规定就是在相邻路径点之间每个关节的运行时间都是相等的。上述规定就是在相邻路径点之间每个关节的运行时间都是相等的。这样，
15、我们可以独立规划每个关节的轨迹、关节之间没有影响。这样，我们可以独立规划每个关节的轨迹、关节之间没有影响。因此，机械臂轨迹规划问题可以分解为因此，机械臂轨迹规划问题可以分解为n个独立的单关节轨迹规划问题。个独立的单关节轨迹规划问题。单区间三次多项式插值单区间三次多项式插值 任务就是确定函数任务就是确定函数q q(t)，使其在，使其在t=0时刻的值为关节角的初始位置，在时刻的值为关节角的初始位置，在t=tf时刻的值为关节角的目标位置。时刻的值为关节角的目标位置。另外，一般要求在初始时刻和终止时刻关节的速度均为零。另外，一般要求在初始时刻和终止时刻关节的速度均为零。因此，关节轨迹规划在数学上就是满
16、足因此，关节轨迹规划在数学上就是满足4 4个约束条件的函数插值问题。个约束条件的函数插值问题。多项式插值比较简单，同时便于计算，因此常用来解决函数插值问题。满足多项式插值比较简单，同时便于计算，因此常用来解决函数插值问题。满足4 4个约束条件的多项式函数插值函数是三次多项式。个约束条件的多项式函数插值函数是三次多项式。9位置约束：位置约束：0(0)()fftqqqq7-8 速度约束：速度约束：(0)0()0ftqq7-9 关节角轨迹可以用三次多项式表示为：关节角轨迹可以用三次多项式表示为：230123()taa ta ta tq7-10 关节角速度和加速度轨迹可以表示为：关节角速度和加速度轨迹
17、可以表示为：212323()23()26taa ta ttaa tqq7-11 把四个约束条件式把四个约束条件式7-87-8和式和式7-97-9代入代入到式到式7-107-10和式和式7-117-11得：得：00230123121230023ffffffaaata ta taaa ta tqq7-12 其解为：其解为：0010aaq2023033()2()ffffatatqqqq-满足约束条件的三次多项式：满足约束条件的三次多项式：230002332()()()fffftttttqqqqqq-例例7-1 7-1 假设一个具有单旋转关节单自由度机器人，处于静止状态时，关节角假设一个具有单旋转关节
18、单自由度机器人，处于静止状态时，关节角q q 1515o o。期望在。期望在2 2秒内平滑移动到关节角秒内平滑移动到关节角q q 7575o o的目标位置，并在目标位置的目标位置，并在目标位置处于静止状态。求满足约束条件的三次多项式，并画出关节角位置、速度处于静止状态。求满足约束条件的三次多项式，并画出关节角位置、速度和加速度随时间变化的曲线。和加速度随时间变化的曲线。10解：解：tf=2，位置和速度约束分别为：，位置和速度约束分别为：0(0)15()75fftqqqq(0)0()0ftqq代入到代入到 7-147-14式得式得关节角轨迹：关节角轨迹：23()154515tttq-230002
19、332()()()fffftttttqqqqqq-关节角速度和加速度轨迹为：关节角速度和加速度轨迹为：2()9045()9090tttttqq-图图7-47-4关节角位置、速度和加速度随时间变化的曲线关节角位置、速度和加速度随时间变化的曲线可以看出，起点和终点的角度值等于指定角度，且速度为零，加速度线性变化。可以看出，起点和终点的角度值等于指定角度，且速度为零，加速度线性变化。11u具有中间点的三次多项式插值具有中间点的三次多项式插值 一般情况下，机械臂需要连续经过若干中间点，因此需要建立满足这些约一般情况下，机械臂需要连续经过若干中间点，因此需要建立满足这些约束的插值函数。若仍然采用三次多项
20、式插值函数，则必须采用分段插值方法。束的插值函数。若仍然采用三次多项式插值函数，则必须采用分段插值方法。即在相邻路径点组成的每个区间内进行三次多项式插值，同时要求在两段曲线即在相邻路径点组成的每个区间内进行三次多项式插值，同时要求在两段曲线的连接处满足一定的光滑条件。比较简单的做法是，指定中间点关节角的位置的连接处满足一定的光滑条件。比较简单的做法是，指定中间点关节角的位置和速度，这样每个区间可以独立进行插值计算。和速度，这样每个区间可以独立进行插值计算。这样与单区间三次多项式插值基本类似，只是这样与单区间三次多项式插值基本类似，只是7-97-9式的速度约束一般不为零，式的速度约束一般不为零，
21、而是指定的速度：而是指定的速度：0(0)()fftqqqq满足约束条件的三次多项式系数的解满足约束条件的三次多项式系数的解 0010aaqq200230032321()21()()fffffffffatttattqqqqqqqq-例例7-2假设一个具有单旋转关节单自由度机器人，假设一个具有单旋转关节单自由度机器人，起始点和终止点速度为零，且位置满足起始点和终止点速度为零，且位置满足 q0=15o，qf=45o。设置一个中间点，位置和速度分别为。设置一个中间点，位置和速度分别为 qm=75o和和 。10deg/smq-假设两段区间的长度均为假设两段区间的长度均为2 2秒。求满足约束条件的分段三次
22、多项式，并画出关秒。求满足约束条件的分段三次多项式，并画出关节角位置、速度和加速度随时间变化的曲线。节角位置、速度和加速度随时间变化的曲线。12解：先计算起始点到中间点的三次多项式。其中解：先计算起始点到中间点的三次多项式。其中tf=2，位置和速度约束分别为：，位置和速度约束分别为：0(0)15()75fftqqqq(0)0()10ftqq-关节角轨迹：关节角轨迹：23()155017.5tttq-关节角速度和加速度轨迹为：关节角速度和加速度轨迹为：2()10052.5()100 105tttttqq-中间点到终止点的中间点到终止点的关节角轨迹：关节角轨迹：23()75 1012.55tttt
23、q-关节角速度和加速度轨迹为：关节角速度和加速度轨迹为：2()102515()2530tttttqq-13u具有抛物线拟合的线性插值具有抛物线拟合的线性插值 连接相邻两个路径点的最简单曲线是连接相邻两个路径点的最简单曲线是直线，因此希望采用线性插值，但线性插直线，因此希望采用线性插值，但线性插值在连接点处速度不连续。值在连接点处速度不连续。获得速度连续光滑曲线的方法是在直线段两端采用抛物线拟合段。因为获得速度连续光滑曲线的方法是在直线段两端采用抛物线拟合段。因为抛物线是二次函数，所以在拟合段内加速度为常数。抛物线是二次函数，所以在拟合段内加速度为常数。假设两端抛物线拟合段的加速度数值相等（符号
24、相反），如图所示，满足假设两端抛物线拟合段的加速度数值相等（符号相反），如图所示，满足条件的解不唯一，但每个解都是关于时间中点条件的解不唯一，但每个解都是关于时间中点tm和位置中点和位置中点q qm对称的。对称的。抛物线和直线的连接点处的速度相同，而整个直线段内速度是常值，所以抛物线和直线的连接点处的速度相同，而整个直线段内速度是常值，所以mbbmbtttqqqq-其中其中q qb是是tb时刻的角度值，时刻的角度值，是拟合段加速度值，是拟合段加速度值，是直线段速度值。是直线段速度值。qq2012bbtqqq7-187-19 将将7-197-19式代入到式代入到7-187-18式，并且注意到式，
25、并且注意到tm=t/2，q qm=(q q0+q qf)/2可以得到以下关系式可以得到以下关系式200bbftttqqqq-t是期望运行时间。是期望运行时间。04()/ftqqq-通常先选择加速度，在计通常先选择加速度，在计算时间算时间 tb14u具有中间点的三次样条插值具有中间点的三次样条插值 那么能否得到不需要指定中间点的速度，同时在整个运行时间内位置、速那么能否得到不需要指定中间点的速度，同时在整个运行时间内位置、速度和加速度都是连续的插值函数？度和加速度都是连续的插值函数？前面介绍的具有中间点的三次多前面介绍的具有中间点的三次多项式插值方法需要指定中间点的速度，项式插值方法需要指定中间
26、点的速度，同时在中间点处的加速度不连续。同时在中间点处的加速度不连续。答案是肯定的，方法就是采用答案是肯定的，方法就是采用样条插值样条插值技术。技术。下面针对关节角轨迹规划问题，给出常用的三次样条插值函数的定义。下面针对关节角轨迹规划问题，给出常用的三次样条插值函数的定义。在机械臂运行区间在机械臂运行区间0,tf上取上取n+1个时间节点个时间节点0=t0 t1 t2 tn-1 tn=tf 给出这些点处关节角位置函数的给出这些点处关节角位置函数的n+1个个值（路径点）值（路径点）q qi，i=0,1,2,n。要求。要求构造一个三次样条插值函数构造一个三次样条插值函数q q(t)，满足以下条件：，
27、满足以下条件：1）q q(ti)=q qi，i=0,1,2,n。2）在每个子区间）在每个子区间ti,ti+1上，上，q q(ti)是三次多项式。是三次多项式。3）q q(t)在整个运行区间在整个运行区间0,tf上具有二阶连续导数。上具有二阶连续导数。从上面的定义可以发现，样条插值函数从上面的定义可以发现，样条插值函数q q(t)是分段三次多项式，即在每个子是分段三次多项式，即在每个子区间上都是一个三次多项式。区间上都是一个三次多项式。确定样条插值函数确定样条插值函数q q(t)只需要只需要n+1个个路径点路径点qi值，而不需要其导数值，而不需要其导数(速度速度)值，值，且在整个机械臂运行时间内
28、且在整个机械臂运行时间内样条插值函数样条插值函数q q(t)的二阶导数都是连续的。的二阶导数都是连续的。15u三次样条插值函数三次样条插值函数q q(t)的存在性的存在性 因为每个区间都是一个三次多因为每个区间都是一个三次多项式，因此有项式，因此有4个未知量，有个未知量，有n个子个子区间，所以未知量个数共有区间，所以未知量个数共有4n 个。个。每个区间两个端点的函数值是事先指定的，所以有每个区间两个端点的函数值是事先指定的，所以有2n 个约束，个约束，n-1个中间点处的一阶和二阶导数（速度和加速度）连续，所以有个中间点处的一阶和二阶导数（速度和加速度）连续，所以有2(n-1)个约束。个约束。约
29、束的个数为约束的个数为2n+2(n-1)=4n-2因此，单从三次样条插值函数因此，单从三次样条插值函数q q(t)的定义不能唯一确定函数本身。的定义不能唯一确定函数本身。对于机械臂轨迹规划问题，一般要求初始点和终止点的速度（一阶导数）为对于机械臂轨迹规划问题，一般要求初始点和终止点的速度（一阶导数）为零，恰好补充两个约束条件。因此，三次样条插值函数零，恰好补充两个约束条件。因此，三次样条插值函数q q(t)可以唯一确定。可以唯一确定。2310111213()taa ta ta tq2320212223()taa ta ta tq 下面以具有一个中间点的关节角轨迹规划问题为例验证三次样条插值方法
30、，下面以具有一个中间点的关节角轨迹规划问题为例验证三次样条插值方法，假定两段区间的长度均为假定两段区间的长度均为tf，指定中间点关节角的位置。两段函数假设为，指定中间点关节角的位置。两段函数假设为解为：解为：10011212031300(1239)/(4)(835)/(4)mffmffaaatatqqqqqqq-2021022203230(33)/(4)(1266)/(4)(853)/(4)mffmffmffaatatatqqqqqqqqq-16例例7-3假设一个具有单旋转关节单自由度假设一个具有单旋转关节单自由度机器人，起始点和终止点速度为零，且位机器人，起始点和终止点速度为零，且位置满足置
31、满足 q q0=15o，q qf=45o。设置一个中间。设置一个中间点，位置为点，位置为q qm=75o。假设两段区间的长。假设两段区间的长度均为度均为2秒。求满足约束条件的三次样条秒。求满足约束条件的三次样条插值多项式，并画出关节角位置、速度和插值多项式，并画出关节角位置、速度和加速度随时间变化的曲线。加速度随时间变化的曲线。17u笛卡尔空间规划方法笛卡尔空间规划方法 前面介绍的关节空间规划方法可以保前面介绍的关节空间规划方法可以保证机械臂能够达到中间点和目标点。但是证机械臂能够达到中间点和目标点。但是连接这些点的中间路径在笛卡尔空间可能连接这些点的中间路径在笛卡尔空间可能是非常复杂的，其复
32、杂程度取决于机械臂是非常复杂的，其复杂程度取决于机械臂的运动学结构。的运动学结构。如果我们关心机械臂在笛卡尔空间的整个路径，而不仅仅是关键点，如如果我们关心机械臂在笛卡尔空间的整个路径，而不仅仅是关键点，如期望工具直线运动、画圆等，就需要采用笛卡尔空间规划方法。期望工具直线运动、画圆等，就需要采用笛卡尔空间规划方法。在笛卡尔空间规划路径要比在关节空间规划路径困难，所以除非必要，一在笛卡尔空间规划路径要比在关节空间规划路径困难，所以除非必要，一般均采用关节空间路径规划方法。般均采用关节空间路径规划方法。由于机械臂驱动能力总是有限的，所以对关节的速度和加速度需要进行限制。由于机械臂驱动能力总是有限的，所以对关节的速度和加速度需要进行限制。若规划的轨迹不满足要求，可以通过增加运行时间的方法来减小关节的速度若规划的轨迹不满足要求，可以通过增加运行时间的方法来减小关节的速度和加速度。和加速度。18
