运动控制算法轨迹规划

1、轨迹规划器摄象机图像分析器力传感器操作臂运动学操作臂动力学机器人控制器任务规划器环境任务规划器qd(t)F(t)x(t)q(t)(txk压缩的数据I(k,e)机器人的轨迹规划机器人的轨迹规划1 工业机器人的轨迹规划工业机器人的轨迹规划.轨迹规划的一般性问题轨迹规划的一般性问题 常见的机器人作业有两种：常见的机器人作业有两种：点位作业（点位作业（PTP=point-to-point motion）连续路径作业（连续路径作业（continuous-path motion），或者称为轮廓运动），或者称为轮廓运动（contour motion）。）。 操作臂最常用的轨迹规划方法有两种：操作臂最常用的轨

2、迹规划方法有两种：第一种第一种是要求对于选定的轨迹结点（插值点）上的位姿、速是要求对于选定的轨迹结点（插值点）上的位姿、速度和加速度给出一组显式约束（例如连续性和光滑程度等），轨度和加速度给出一组显式约束（例如连续性和光滑程度等），轨迹规划器从一类函数（例如迹规划器从一类函数（例如n次多项式）选取参数化轨迹，对结次多项式）选取参数化轨迹，对结点进行插值，并满足约束条件。点进行插值，并满足约束条件。第二种第二种方法要求给出运动路径的解析式。方法要求给出运动路径的解析式。轨迹规划方法一般是在机器人的初始位置和目标位置之间用轨迹规划方法一般是在机器人的初始位置和目标位置之间用多项式函数来多项式函数来

3、“内插内插”或或“逼近逼近”给定的路径，并产生一系列的给定的路径，并产生一系列的控制点。控制点。a. 三次多项式插值三次多项式插值 关节空间法计算简单、容易。再者，不会发生机构的奇异性关节空间法计算简单、容易。再者，不会发生机构的奇异性问题。问题。2.关节轨迹的插值关节轨迹的插值 只给定机器人起始点和终只给定机器人起始点和终止点的关节角度。止点的关节角度。为了实现平稳运动，轨迹函数至少需要四个约束条件。即为了实现平稳运动，轨迹函数至少需要四个约束条件。即满足起点和终点的关节角度约束满足起点和终点的关节角度约束满足起点和终点的关节速度约束（满满足起点和终点的关节速度约束（满足关节速度的连续性要求

4、）足关节速度的连续性要求）解上面四个方程得：解上面四个方程得：注意：这组解只适用于关节起点、终点速度为零的运动情况。注意：这组解只适用于关节起点、终点速度为零的运动情况。 设只有一个自由度的旋转关节机械手处于静止状态时，设只有一个自由度的旋转关节机械手处于静止状态时， =150，要在，要在3s内平稳运动到达终止位置：内平稳运动到达终止位置： =750，并且在终，并且在终止点的速度为零。止点的速度为零。tttttttt64.260 .40)(32.130 .40)(44. 40 .200 .15)(232将上式的已知条件代入以下四个方程得四个系数：将上式的已知条件代入以下四个方程得四个系数：因此

5、得：因此得：b. b. 过路径点的三次多项式插值过路径点的三次多项式插值 方法是：方法是：把所有路径点都看成是把所有路径点都看成是“起点起点”或或“终点终点”，求解，求解逆运动学，得到相应的关节矢量值。然后确定所要求的三次多项逆运动学，得到相应的关节矢量值。然后确定所要求的三次多项式插值函数，把路径点平滑的连接起来。不同的是，这些式插值函数，把路径点平滑的连接起来。不同的是，这些“起点起点”和和“终点终点”的关节速度不再是零。的关节速度不再是零。由上式确定的三次多项式描述了起始点和终止点具有任意给定位由上式确定的三次多项式描述了起始点和终止点具有任意给定位置和速度的运动轨迹。剩下的问题就是如何

6、确定路径点上的关节置和速度的运动轨迹。剩下的问题就是如何确定路径点上的关节速度，有以下三种方法：速度，有以下三种方法：fft)()0(0此时的此时的速度约速度约束条件束条件变为：变为：同理可以求得此时的三次多项式系数：同理可以求得此时的三次多项式系数： （1） 根据工具坐标系在直角坐标空间中的瞬时线速度和角速根据工具坐标系在直角坐标空间中的瞬时线速度和角速度来确定每个路径点的关节速度度来确定每个路径点的关节速度 ；该方法工作量大。；该方法工作量大。 （2）为了保证每个路径点上的加速度连续，由控制系统按照为了保证每个路径点上的加速度连续，由控制系统按照此要求自动地选择路径点的速度。此要求自动地选

7、择路径点的速度。 （3）在直角坐标空间或关节空间中采用某种适当的启发式方在直角坐标空间或关节空间中采用某种适当的启发式方法，由控制系统自动地选择路径点的速度；法，由控制系统自动地选择路径点的速度； 对于方法对于方法（2），为了保证路径点处的加速度连续，可以设法，为了保证路径点处的加速度连续，可以设法用两条三次曲线在路径点处按照一定的规则联系起来，拼凑成所用两条三次曲线在路径点处按照一定的规则联系起来，拼凑成所要求的轨迹。其约束条件是：联接处不仅速度连续，而且加速度要求的轨迹。其约束条件是：联接处不仅速度连续，而且加速度也要连续。也要连续。 对于方法对于方法（3）， 这里所说的启发式方法很简单，

8、即假设用这里所说的启发式方法很简单，即假设用直线段把这些路径点依次连接起来，如果相邻线段的斜率在路径直线段把这些路径点依次连接起来，如果相邻线段的斜率在路径点处改变符号，则把速度选定为零；如果相邻线段不改变符号，点处改变符号，则把速度选定为零；如果相邻线段不改变符号，则选择路径点两侧的线段斜率的平均值作为该点的速度。则选择路径点两侧的线段斜率的平均值作为该点的速度。路径点上速度的自动生成ttAtDtCtBt00ABCD 如果对于运动轨迹的要求更为严格，约束条件增多，那么如果对于运动轨迹的要求更为严格，约束条件增多，那么三次多项式就不能满足需要，必须用更高阶的多项式对运动轨三次多项式就不能满足需

9、要，必须用更高阶的多项式对运动轨迹的路径段进行插值。例如，对某段路径的起点和终点都规定迹的路径段进行插值。例如，对某段路径的起点和终点都规定了关节的位置、速度和加速度（有六个未知的系数），则要用了关节的位置、速度和加速度（有六个未知的系数），则要用一个五次多项式进行插值。一个五次多项式进行插值。5544332210)(tatatatataatc、用抛物线过渡的线性插值、用抛物线过渡的线性插值 单纯线性插值将导致在结点处关节运动速度不连续，加速度单纯线性插值将导致在结点处关节运动速度不连续，加速度无限大。无限大。带抛物线过渡的线性插值（2）tthtf0f0h带抛物线过渡的线性插值（1）ttbtf

10、tf-tb0f0对于多解情况，如右图所示。加对于多解情况，如右图所示。加速度的值越大，过渡长度越短。速度的值越大，过渡长度越短。解决办法：解决办法：在使用线性插值时，在使用线性插值时，把每个结点的邻域内增加一段抛把每个结点的邻域内增加一段抛物线的物线的“缓冲区段缓冲区段”，从而使整，从而使整个轨迹上的位移和速度都连续。个轨迹上的位移和速度都连续。d、过路径点的用抛物线过渡的线性插值、过路径点的用抛物线过渡的线性插值 如图所示，某个关节在运动中设有如图所示，某个关节在运动中设有n个路径点，其中三个相个路径点，其中三个相邻的路径点表示为邻的路径点表示为j，k和和l，每两个相邻的路径点之间都以线性每

11、两个相邻的路径点之间都以线性函数相连，而所有的路径点附近则有抛物线过渡。函数相连，而所有的路径点附近则有抛物线过渡。（同样存在多（同样存在多解）解）多段带有抛物线过渡的线性插值轨迹t0lkj 如果要求机器人通过某个结点，同时速度不为零，怎么办？如果要求机器人通过某个结点，同时速度不为零，怎么办？可以在此结点两端规定两个可以在此结点两端规定两个“伪结点伪结点”，令该结点在两伪结点的，令该结点在两伪结点的连线上，并位于两过渡域之间的线性域上。连线上，并位于两过渡域之间的线性域上。2 移动机器人的轨迹规划移动机器人的轨迹规划 a.基于模型和基于传感器的路径规划基于模型和基于传感器的路径规划 基于模型

12、的方法有：基于模型的方法有：c-空间空间法、自由空间法、网格法、四叉法、自由空间法、网格法、四叉树法、矢量场流的几何表示法等。树法、矢量场流的几何表示法等。相应的搜索算法有相应的搜索算法有A*、遗传算法、遗传算法等。等。图中图中A区域的位置码区域的位置码 (Location Code:LC)为为3031。 BCD问：图中问：图中B，C，D区域的位置码区域的位置码 LC为为? b.全局路径规划（全局路径规划（Global Path Planning）和局部路径规划）和局部路径规划（Local Path Planning） 自主移动机器人的导航问题要解决的是：自主移动机器人的导航问题要解决的是：（

13、1）“我现在何处？我现在何处？”；（2）“我要往何处去？我要往何处去？”；（3）“要如何到该处去？要如何到该处去？”。 局部路径规划局部路径规划主要解决（主要解决（1）和（）和（3）两个问题，即机器人）两个问题，即机器人定位和路径跟踪问题；方法主要有：人工势场法定位和路径跟踪问题；方法主要有：人工势场法 、模糊逻辑算、模糊逻辑算法等法等 。 全局路径规划全局路径规划主要解决（主要解决（2），即全局目标分解为局部目），即全局目标分解为局部目标，再由局部规划实现局部目标。主要有：可视图法标，再由局部规划实现局部目标。主要有：可视图法 、环境分、环境分割法（自由空间法割法（自由空间法 、栅格法、栅格法 ）等）等 ；c.离线路径规划和在线路径规划离线路径规划和在线路径规划 离线路径规划离线路径规划是基于环境先验完全信息的路径路径规划。是基于环境先验完全信息的路径路径规划。完整的先验信息只能适用于静态环境，这种情况下，路径是离完整的先验信息只能适用于静态环境，这种情况下，路径是离线规划的；线规划的；在线路径规划在线路径规划是基于传感器信息的不确定环境的路是基于传感器信息的不确定环境的路径规划。在这种情况下，路径必须是在线规划的。径规划。在这种情况下，路径必须是在线规划的。2. 机器人