机器人技术基础 课件 第7章 工业机器人运动规划 -3

第7章工业机器人运动规划机器人技术基础目录contants7.1机器人运动规划概述7.2关节空间的轨迹规划方法7.3操作空间的轨迹规划7.3操作空间的轨迹规划操作空间的轨迹规划有很多方法，最常用的是空间直线插补和空间圆弧插补，其他非直线和圆弧轨迹都可以用空间直线和圆弧去逼近，并通过合适的算法将误差控制在允许范围之内即可。如果仅从位置插补的角度讲，机器人操作空间的插补方式与数控加工的插补方式类似，但机器人除了末端位置，还需要对其末端姿态进行插补。操作空间的轨迹规划可以保证机器人末端在操作空间的运动轨迹符合期望，但有一些问题需要小心避免。第7章工业机器人运动规划7.3操作空间的轨迹规划7.3.1空间直线插补7.3.2空间圆弧插补7.3.3机器人的末端姿态插补7.3.4操作空间轨迹规划的几何问题7.3.1空间直线插补在操作空间中，机械臂末端轨迹上的每个节点包含了三个位置变量和三个姿态变量，关于姿态的插补在7.3.3中会详细介绍，这里先假设机械臂沿直线运动过程中姿态不变，所以只关心三个位置变量的直线插补即可。假设机械臂沿直线做匀速运动，操作空间的直线轨迹插补方法如下。7.3.1空间直线插补操作空间的直线轨迹插补方法如下。①首先确定机械臂末端在操作空间的运动速度v和插补时间ts。插补时间是机器人的伺服控制周期决定的，通常为毫秒级，比如10ms，20ms等。②计算始末点的直线长度③计算插补步数7.3.1空间直线插补④计算各轴的增量⑤计算各插补点坐标其中i=0，1，2，...，N。⑥最后根据机械臂D-H参数求逆解，获得插补点对应的关节角时间序列θ(t)。7.3.1空间直线插补例7.77.3.1空间直线插补例7.7解：①首先计算直线长度、步长以及插补步数各轴的增量为最后即可求出各插补点的坐标，具体计算比较繁琐，交给程序完成。其中i=0，1，2，...，142。7.3.1空间直线插补例7.7②RoboticsToolbox提供了球形6轴机械臂求逆解的函数ikine6s，利用下面几行程序就可以完成题目的要求，绘制的图形如下。7.3.1空间直线插补例7.7程序NCUT7_7a.m及其绘制的动画视频匀速直线插补7.3.1空间直线插补例7.7思考：例7.7假设整个过程机械臂末端一直是匀速运动的，如果要求起点和终点速度均为0，应如何做轨迹规划？7.3.1空间直线插补例7.7程序NCUT7_7b.m及其绘制的动画视频匀加减速直线插补第7章工业机器人运动规划7.3操作空间的轨迹规划7.3.1空间直线插补7.3.2空间圆弧插补7.3.3机器人的末端姿态插补7.3.4操作空间轨迹规划的几何问题7.3.2空间圆弧插补空间圆弧是指空间任意平面内的圆弧，即空间一般平面内的圆弧。空间圆弧的插补可分如下三步进行。（1）建立圆弧所在平面坐标系，将三维插补问题转换为二维插补问题（2）利用二维平面圆弧插补算法求出插补点坐标（xi，yi，zi）（3）将二维平面坐标系中的插补点坐标（xi，yi，zi），通过齐次坐标变换矩阵T再转化为参考坐标系下的插补点坐标（Xi，Yi，Zi）。7.3.2空间圆弧插补在圆弧所在平面上建立坐标系{R}，其Z轴垂直于圆弧平面，原点位于圆心。只要求出{R}对应的齐次变换矩阵T，则通过坐标变换，可将参考坐标系内已知的三点坐标转换为{R}坐标系的坐标，这时空间一般平面上的圆弧就变为XRORYR平面上的圆弧，从而将三维问题转化为二维问题。因此关键在于如何建立圆弧平面坐标系{R}，即如何确定平面的法向量作为坐标系的ZR轴，以及如何确定圆心坐标，从而将坐标系{R}原点置于圆心。（1）建立圆弧所在平面坐标系，将三维插补问题转换为二维插补问题

参考坐标系{0}与空间圆弧平面坐标系{R}的关系7.3.2空间圆弧插补已知到P1,P2,P3三点决定了圆弧所在平面，根据叉积的定义可知，向量P1P2和P2P3的叉积即为该平面的法向量方向（1）建立圆弧所在平面坐标系，将三维插补问题转换为二维插补问题

参考坐标系{0}与空间圆弧平面坐标系{R}的关系由圆的性质可知，线段P1P2和P2P3的中垂线交点即为圆心。所以首先需要求出两中垂线方程，再求其交点。7.3.2空间圆弧插补由于P1P2中垂线L1同时垂直于平面法向量n和向量P1P2，所以其方向向量可以通过叉积获得（1）建立圆弧所在平面坐标系，将三维插补问题转换为二维插补问题

参考坐标系{0}与空间圆弧平面坐标系{R}的关系中点A的坐标为所以，过中点A且法向量为的中垂线所在的直线方程为7.3.2空间圆弧插补同理，P2P3的中垂线所在的直线方程为（1）建立圆弧所在平面坐标系，将三维插补问题转换为二维插补问题

参考坐标系{0}与空间圆弧平面坐标系{R}的关系其中现在已经求出两条中垂线的方程，其交点即为圆心。7.3.2空间圆弧插补（1）建立圆弧所在平面坐标系，将三维插补问题转换为二维插补问题

参考坐标系{0}与空间圆弧平面坐标系{R}的关系7.3.2空间圆弧插补（1）建立圆弧所在平面坐标系，将三维插补问题转换为二维插补问题

参考坐标系{0}与空间圆弧平面坐标系{R}的关系7.3.2空间圆弧插补（1）建立圆弧所在平面坐标系，将三维插补问题转换为二维插补问题7.3.2空间圆弧插补（2）利用二维平面圆弧插补算法求出插补点坐标7.3.2空间圆弧插补（2）利用二维平面圆弧插补算法求出插补点坐标7.3.2空间圆弧插补（3）将{R}坐标系中的插补点坐标（xi，yi，zi），通过齐次坐标变换矩阵T再转化为参考坐标系下的插补点坐标（Xi，Yi，Zi）。通过以上三步，就完成了过空间三点的圆弧的插补。7.3.2空间圆弧插补例7.87.3.2空间圆弧插补例7.87.3.2空间圆弧插补例7.87.3.2空间圆弧插补例7.87.3.2空间圆弧插补例7.8（2）利用二维平面圆弧插补算法求出插补点坐标7.3.2空间圆弧插补例7.8通过以上三步，就完成了过空间三点的圆弧的插补，所绘制的插补点如右图所示。请参考程序NCUT7_8.m及其讲解视频第7章工业机器人运动规划7.3操作空间的轨迹规划7.3.1空间直线插补7.3.2空间圆弧插补7.3.3机器人的末端姿态插补7.3.4操作空间轨迹规划的几何问题7.3.3机器人的末端姿态插补7.3.3机器人的末端姿态插补（2）欧拉角法还可以采用一组带有顺序的旋转角对姿态进行描述，即欧拉角方式。欧拉角按照绕不同轴的旋转顺序可以分为多种，比如依次绕X,Y,Z或Y,X,Z或Z,Y,X轴旋转等等。7.3.3机器人的末端姿态插补（2）欧拉角法7.3.3机器人的末端姿态插补（2）欧拉角法欧拉角能够较好地解决末端姿态的规划问题，但由于欧拉角存在万向死锁问题，而且欧拉角的旋转顺序对于最后的姿态影响很大，无法同时对三个欧拉角进行规划，因此并没有得到广泛应用。陀螺仪正常状态万向死锁状态（少了一个自由度）7.3.3机器人的末端姿态插补（3）等效轴角坐标系表示法采用欧拉角法时默认从一个姿态到另一个姿态的变换，需要绕三个轴旋转三个角度。但数学上可以证明，任何两个姿态的变换，都可以通过绕空间某个轴旋转某个角度来获得。这个转换过程在数学上采用了四元数法，对四元数有兴趣可以查阅相关资料做进一步了解。等效轴角插补法就是建立在这个理论基础之上的。7.3.3机器人的末端姿态插补（3）等效轴角坐标系表示法7.3.3机器人的末端姿态插补（3）等效轴角坐标系表示法7.3.3机器人的末端姿态插补例7.9在例7.8圆弧位置轨迹规划的基础上，添加机械臂的姿态插补。要求机械臂经过三点时，其末端Z轴指向圆弧中心，其Y轴沿着该点的切线方向，如图所示；请借助RoboticsToolbox求插补点逆解，求出插补点对应的6个关节角_时间序列，并绘制出Puma560沿着该圆弧运动的动画。例7.97.3.3机器人的末端姿态插补解：主要分以下三步来求解该问题。①确定关键点的位姿矩阵例7.97.3.3机器人的末端姿态插补②等效轴角法进行姿态插补首先将上一步求得的关键点位姿矩阵转换为四元数表达法，接着在四元数空间进行插补，最后再将插补好的插补点四元数转换回齐次矩阵。需要利用RoboticsToolbox工具箱的函数完成上述步骤，主要是语句如下：例7.9q1=UnitQuaternion(T1);%表达位姿的齐次矩阵T1转换为四元数q1q2=UnitQuaternion(T2);%表达位姿的齐次矩阵T2转换为四元数q2q3=UnitQuaternion(T3);%表达位姿的齐次矩阵T3转换为四元数q3qq1=erp(q2,N1);%利用四元数法进行插补，从q1绕等效轴旋转到q2，共N1个姿态qq2=erp(q3,N2);%利用四元数法进行插补，从q2到绕等效轴旋转到q3共N2个姿态qq=[qq1qq2];%将两段插补点连在一起Tqq=qq.SE3;%将四元数qq转化为代表旋转变换的齐次矩阵Tqq%将末端位置坐标及姿态矩阵合成为完整的末端位姿齐次矩阵Tc=transl([x(k),y(k),z(k)])*double(Tqq(k));7.3.3机器人的末端姿态插补③求逆解对插补好的末端位姿求逆解，得到插补点对应的6个关节角_时间序列，然后就可以将动画绘制出来。主要是语句如下：例7.9%求puma560机器人末端位姿Tc对应的逆解qcqc(k,:)=p560.ikine6s(Tc);%绘制机器人沿圆弧运动的轨迹p560.plot(qc)参考程序NCUT7_9.m及程序讲解视频第7章工业机器人运动规划7.3操作空间的轨迹规划7.3.1空间直线插补7.3.2空间圆弧插补7.3.3机器人的末端姿态插补7.3.4操作空间轨迹规划的几何问题7.3.4操作空间轨迹规划的几何问题（1）不可到达的中间点图中直线AB代表一条焊缝，我们希望Puma560末端沿着该焊缝运动从而完成该焊接工作，但事实上它是无法做到的。这是因为其工作空间的中心有一个空心圆柱，如图（b）所示。也就是说该圆柱内的点机械臂末端均无法到达。7.3.4操作空间轨迹规划的几何问题图中直线AB代表一条焊缝，我们希望Puma560末端沿着该焊缝运动从而完成该焊接工作，但事实上它是无法做到的。这是因为其工作空间的中心有一个空心圆柱，如图（b）所示。也就是说该圆柱内的点机械臂末端均无法到达。（1）不可到达的中间点7.3.4操作空间轨迹规划的几何问题在操作空间做直线轨迹规划时，看似两个端点A、B均在工作空间之内，人们往往会误以为该直线上的所有点机械臂末端均可到达。在操作空间做轨迹规划时，需要特别关注轨迹上是否有中间点落在该空心圆柱之内，如果确有点落入其中，就必须重新规划路径，比如调整机械臂和焊接台之间的距离，使焊缝远离空心圆柱（比如图中虚线的位置），从而确保机械臂末端可以到达直线上所有点。此外由于各关节都有运动范围限制，机器人的工作空间并非完美的球形，做轨迹规划时一定要充分了解特定机器人工作空间的形状和大小，确保轨迹上所有点都是机械臂可以到达的。7.3.4操作空间轨迹规划的几何问题思考：在例7.8和例7.9中，如果我们将三点坐标修改为如图所示，那么Puma560末端还能到达圆弧上所有点吗？为什么？7.3.4操作空间轨迹规划的几何问题在机械臂的工作空间中，如果期望机械臂末端在奇异点附近以较大速率运行，可能会导致很大的问题。这是因为机械臂沿着直线路径接近某奇异位姿时，其对应的一个或多个关节速率会猛增，甚至增加至无穷大，而关节都是由电机驱动的，电机能够输出的速率有限，不可能为关节提供指定的速率，所以末端执行器也无法在奇异点附近按照期望速度运行。（2）奇异点附近的高关节速率7.3.4操作空间轨迹规划的几何问题（2）奇异点附近的高关节速率机械臂运行轨迹逐渐靠近奇异点7.3.4操作空间轨迹规划的几何问题（2）奇异点附近的高关节速率要求机械臂末端以2m/s的速度沿直线方向运行，插补时间为20ms，在操作空间做直线插补并通过求逆解得到各关节转角，据此可以并绘制各两关节位置和速度图。很明显，在奇异点附近关节速率急剧增大。两关节在奇异点附近的位置和速度7.3.4操作空间轨迹规划的几何问题（2）奇异点附近的高关节速率7.3.4操作空间轨迹规划的几何问题（2）奇异点附近的高关节速率如何解决这类问题？首先还是要尽量避免轨迹经过机械臂的奇异点，如果实在难以避免，则必须减小机械臂末端在奇异点附近的运行速度，比如从上面要求的2m/s降低到0.5m/s，从而将各关节速度控制在容许范围之内。7.3.4操作空间轨迹规划的几何问题（3）逆解的合理选择问题在第三章中介绍过，对于机械臂工作空间内的任一点都至少有一组逆解，对于大多数点还存在多组逆解。也就是说机械臂能以不同姿态到达同一个位置。仍以二关节机械臂为例，除奇异点外其工作空间内的点均具有两组逆解。如图所示，机械臂末端沿直线运动时，有两种姿态可以到达A点，一种上位一种下位。7.3.4操作空间轨迹规划的几何问题（3）逆解的合理选择问题我们希望其在运动过程中一直采用类似的姿态。如果在上一个插补点还是上位，运行到下一点就变成了下位，就会导致关节位置发生突变，这必然会对机器人系统造成冲击。因此，在操作空间轨迹规划时，应该从当前点的多个逆解中选择出与上一组逆解最接近的一组，从而避免相邻两组关节角发生突变，最大限度保证运行连续丝滑。7.3.4操作空间轨迹规划的几何问题从以上分析可以看到，在操作空间做轨迹规划可谓处处是雷不得不防。工业机器人一般都同时具有关节空间和操作空间的轨迹规划功能，通常默认使用关节空间轨迹规划，只有在非常必要时才使用操作空间轨迹规划，以便最大程度降低出问题的概率。（3）逆解的合理选择问题（2）奇异点附近的高关节速率（1）不可到达的中间点本章小结本章主要讨论了工业机器人的轨迹规划相关的内容。在第一节中首先明确运动规划包含路径规划与轨迹规划。路径是一个空间概念，它描述的是机器人位姿随空间的变化；轨迹则在路径基础上引入了时间，所以它是一个时空概念，描述的是机器人位姿随时间的变化。路径规划时，常用工具坐标系相对于工件坐标系的运动来描述作业路径