《机器人技术-建模、仿真及应用》课件 第二章机器人运动学

ROBOT机器人技术——建模、仿真及应用机器人运动学第二章目录数学基础PART.1运动学分析PART.2数学基础PART.1位置与位姿齐次变化仿真实例数学基础位置描述对于直角坐标系{A}，空间任一点的位置可用3×1阶的列矢量来表示(也称位置矢量)：式中：Px、Py、Pz是点P在坐标系{A}中的三个位置坐标分量。数学基础位置描述向量可以由3个起始和终止的坐标来表示。P=（

PX

-OX

）

i

+（PY

-OY

）

j

+（

PZ

-OZ

）

k

若O为原点：式中：Px、Py、Pz是向量在坐标系{A}中的三个位置坐标分量。向量的3个分量也可写成矩阵形式。(PX=cos

，PY=cos

，PZ=cos)（，，J为机械臂的雅可比矩阵（6×n矩阵））（为关节速度对末端执行速度的3*n作用矩阵）雅可比矩阵取一个自由度为n的机械臂，其正运动学方程如下：（旋转矩阵R和位移矢量P都是关于变量

的矩阵方程）将末端执行器的线速度

和角速度

表示为所有关节速度

的函数：（为关节角速度对末端执行速度的3*n作用矩阵）两个方程的紧凑形式：数学基础矢量u、v、w的坐标方向用齐次坐标表示。例位置描述数学基础姿态描述规定空间某刚体B的方位，设一坐标系{B}与此刚体固连，物体相对于参考坐标系{A}的姿态相对于参考坐标系{A}的方向余弦组成的3×3矩阵旋转矩阵：用矢量两两之间的余弦则表示为：数学基础姿态描述对应于x、y或z轴做旋转角为的旋转变换，其旋转矩阵分别为：旋转矩阵应具有以下几个特点：1）3个主矢量两两垂直；2）9个元素中，只有3个是独立的；3）3个单位主矢量满足6个约束条件，即：4）旋转矩阵为正交矩阵，并且满足以下条件，即：数学基础齐次变化齐次坐标是指在原有三维坐标的基础上，增加一维坐标而形成四维坐标。如：空间点p的齐次坐标为p=(4,6,8,w)4、6、8分别对应p点在空间坐标系中的x、y、z轴坐标，w为其对应的比例因子。p=（4,6,8,1）和p=(8,12,16,2）表示的是同一个p点。当比例因子w≠0时p点的齐次坐标的形式是不唯一的当比例因子w=0时该齐次坐标表示某一向量如：x=（1，0，0，0）表示坐标系的x轴单位向量。y=（0，1，0，0）表示坐标系的y轴单位向量。z=（0，0，1，0）表示坐标系的z轴单位向量。对应于x、y、z轴做转角位

的旋转变换，分别可得：数学基础齐次变化平移齐次坐标变换

对已知矢量

进行平移变换所得的矢量

为：

旋转齐次坐标变换Rot表示旋转矩阵。齐次变换平移齐次变换动坐标系{A}相对于固定坐标系的X0、Y0、Z0轴作(–1，2，2)平移后到{A

}；动坐标系{A}相对于自身坐标系的X、Y、Z轴分别作(–1，2，2)平移后到{A

}。A的矩阵表达式如下。写出坐标系{A

}、{A

}的矩阵表达式。例齐次变换平移齐次变换动坐标系{A}相对于固定坐标系的X0、Y0、Z0轴作(–1，2，2)平移后到{A

}；动坐标系{A}相对于自身坐标系的X、Y、Z轴分别作(–1，2，2)平移后到{A

}。A的矩阵表达式如下。写出坐标系{A

}、{A

}的矩阵表达式。例动坐标系{A}的平移变换算子：齐次变换旋转齐次变换已知坐标系中点U的位置矢量U=[7321]T，绕Z轴旋转90°，再绕Y轴旋转90°，求旋转变换后所得的点W。例数学基础齐次变化平移与旋转齐次坐标组合变换根据平移齐次坐标变换和旋转齐次坐标变换，空间某点由矢量

描述，其中i、j、k分别为x、y、z轴上的单位矢量，然后对应于x、y、z轴做转角为的旋转变换，分别可得：齐次变换复合变换已知坐标系中点U的位置矢量U=[7321]T，将此点绕Z轴旋转90°，再绕Y轴旋转90°，最后再作4i-3j+7k的平移，求变换后所得的点E。例数学基础仿真实例平移坐标变换实例T0=transl(0,0,0)T1=transl(1,2,1)trplot(T0,'color','r')holdontrplot(T1,'color','g')axis([-33-33-33])tranimate(T0,T1)例代码：坐标系由原点(0,0,0)分别沿x、y、z轴平移1、2、1个单位。数学基础仿真实例平移坐标变换实例T0=rotz(0)T0=rotz(0)T1=rotz(pi/4)trplot(T0,'color','r')axis([-11-11-11]);oldontranimate(T0,T1,'color','b')例代码：坐标系在原点位置绕z轴旋转45°。数学基础仿真实例先平移再旋转实例T0=transl(0,0,0)T1=transl(1,2,1)trplot(T0,'color',‘r’)；holdon；trplot(T1,'color','g')axis([-33-33-33])tranimate(T0,T1)T2=T1T3=T2*trotz(pi/2)trplot(T2,'color','r')holdontrplot(T3,'color','g')axis([-33-33-33])tranimate(T2,T3)平移旋转例代码：坐标系从原点位置(0,0,0)先分别沿着x、y、z轴平移1、2、1个单位，再绕z轴逆时针旋转90°。数学基础仿真实例先旋转再平移实例T0=trotz(0)T1=trotz(pi/2)trplot(T0,'color','r’);holdon；trplot(T1,'color','g')tranimate(T0,T1)T2=T1T3=transl(1,2,1)*T2trplot(T2,'color','r’);holdon；trplot(T3,'color','g')axis([-33-33-33])tranimate(T2,T3)平移旋转例代码：坐标系从原点位置(0,0,0)先绕z轴逆时针旋转90°，再分别沿着x、y、z轴平移1、2、1个单位。数学基础仿真实例旋转和平移同时进行实例T1=transl(0,0,0)T2=transl(1,2,1)T3=trotz(pi/2)T4=T2*T3trplot(T1,'color','r')holdontrplot(T4,'color','g')axis([-33-33-33])tranimate(T4,'color','b')例代码：坐标系从原点位置分别沿着x、y、z轴平移1、2、1个单位，同时绕z轴逆时针旋转90°。运动学分析PART.2正运动学分析D-H参数法正运动学仿真逆运动学分析逆运动学仿真运动学分析正运动学分析机器人本体，是机器人赖以完成作业任务的执行机构。机械臂多采用关节式机械结构，一般具有6个自由度，其中3个用来确定末端执行器的位置，另外3个则用来确定末端执行装置的方向。机械臂上的末端执行装置可以根据操作需要换成焊枪、吸盘、扳手等作业工具。运动学分析D-H参数法连杆n坐标系（简称n系）坐标原点位于i关节轴线上，是关节i的关节轴线与i-1和i关节轴线公垂线的交点；Z轴与i关节轴线重合；X轴与公垂线重合，从关节i-1指向关节i；Y轴按右手螺旋法则确定。运动学分析D-H参数法每个连杆可以由四个参数所描述名称含义“

”号性质转角以

方向看，

和

之间的夹角右手法则常量长度沿着

方向，

和

之间的距离与

正向一致常量关节角以

方向看，

和

之间的夹角右手法则转动关节为变量移动关节为常量距离沿着

方向，

和

之间的距离沿

正向为正转动关节为常量移动关节为变量在D-H法分析中，连杆坐标系

相对于

的变换

称为连杆变换矩阵，连杆变换矩阵

相当于坐标系

经过以下变换得到：运动学分析D-H参数法1)绕

轴旋转，使得与平行，如图a)所示；2)沿轴移动，使得与在同一直线上，如图b）11所示；3)绕轴旋转，使得转到与平行，如图c）所示；4)沿轴移动，使得连杆坐标系的原点与的原点11重合，如图d)所示。由此可得旋转变换矩阵为：D-H法矩阵变换过程运动学分析D-H参数法如图所示的平面三连杆机构，已知手臂长

、和，关节变量、和，试求末端执行器位姿矩阵。例i1000200300解：建立机械臂各杆的坐标系，列出D-H参数。运动学分析D-H参数法如图所示的平面三连杆机构，已知手臂长

、和，关节变量、和，试求末端执行器位姿矩阵。例==运动学分析正运动学仿真调用MATLAB机器人工具箱，使用D-H参数法设置三连杆机械臂杆长分别为30、50、40；关节角、连杆偏距、连杆转角都为0。例a1=30;a2=50;a3=40;L(1)=Link([00a10])L(2)=Link([00a20])L(3)=Link([00a30])robot=SerialLink(L)teach(robot)代码：运动学分析正运动学仿真调用MATLAB机器人工具箱，使用D-H参数法设置三连杆机械臂连杆1的杆长为30，连杆转角为90°，关节角为0°，连杆偏距为0；连杆2的杆长为50，连杆转角为0，关节角为0°，连杆偏距为20；连杆3的杆长为40，连杆转角都为0，关节角为0°，连杆偏距为0。例L_1=30;L_2=50;L_3=40;L(1)=Link([00L_1pi/2])L(2)=Link([020L_20])L(3)=Link([00L_30])Robot=SerialLink(L);teach(Robot)代码：运动学分析逆运动学分析实质：已知BTH求解θ，从而确定与末端位置有关的所有关节的位置——实际工程问题已知操作机杆件的几何参数，给定操作机末端执行器相对于参考坐标系的期望位置和姿态（位姿），操作机能否使其末端执行器达到这个预期的位姿？若能达到，那么操作机是否存在不同形态可满足条件？逆向运动学逆运动学分析可解性解的存在问题取决于操作末端的工作空间（Workspace）工作空间：操作臂末端执行器所能到达的范围，取决于机器人结构、杆件参数或手部位姿。工作域外逆解不存在具有转动和移动关节的机器人，在单一串联链中共有个6自由度或小于6个自由度时是可解的。通解是数值解，非解析表达式，是利用数值迭代原理求解得到，计算量比求解析解大得多。要使机器人有解析解，设计时就要使机器人的结构尽量简单，而且尽量满足连续三个旋转关节的旋转轴交会于一点，或连续三个关节轴互相平行的充分条件。（Pieper准则）逆向运动学多解性对于给定位置与姿态，具有多组解。造成运动学逆解多解是由于解反三角函数方程产生的。PUMA560机器人的四个逆解避免碰撞的一个可能实现的解逆运动学分析逆向运动学逆运动学分析求解方法逆解形式求解方法闭式解close-formsolution用解析函数式表示解求解速度快代数法几何法数值解numericalsolution利用迭代性质求解求解速度慢数值法逆向运动学逆运动学分析代数法根据正运动学分析，设机械臂腕关节的位置坐标为姿态角

，机械臂执行端坐标为

。基于D-H坐标系的机械臂运动学方程如下：平面三连杆机械臂代数法求逆运动学可知：逆向运动学逆运动学分析由矩阵两边对应相等，结合上式，可得腕部坐标

的表达式为：即：上式有解的条件是等式右边值的区间为[-1,1]，如果此约束条件不满足，则表明目标点超出了机械臂的可达工作空间，其逆运动学方程无解。代数法逆向运动学逆运动学分析代数法假设目标点在机械臂的工作空间内，则：由式

和式

可得：上式的求解应用了双变量反正切公式，用

计算根据

和

的符号来判别求得的角所在的象限。根据

带入式，

得：逆向运动学逆运动学分析代数法进而可求得：结合求出的

与，可得：

则三个关节角运用代数法全部解出。逆向运动学逆运动学分析几何法平面三连杆机械臂几何法求逆运动学所示，杆长

，杆长、坐标系1的原点、坐标系的原点的连线组成三角形，由余弦定理可得：由，得计算图中的

和逆向运动学逆运动学分析几何法即：结合

和，得即坐标3能够达到相同位置时，连杆机构的另一种可能情况，此时则有：逆向运动学逆运动学仿真封闭式解法：以KUKAKR5