1、1,第3章 机器人运动学,运动学研究物体的位姿、速度和加速度之间的关系。,本章将介绍双轮移动机器人、三轮全向移动机器人和关节式机械臂的运动学问题。,双轮移动机器人运动学,平面轮式移动机器人,（x, y, q）表示双轮机器人位姿，v 表示机器人前进速度，表示机器人转动速度w，则,（3-1）,由（3-1）式可得运动学约束条件，,是所谓的“非完整约束”。物理含义是，机器人不能沿轮轴线方向横移。,设轮距为D，轮半径为r，两轮独立驱动时轮子转速wL，wR,则,（3-2）,2,给定期望的机器人前进速度v，转动速度w，则可以确定机器人的两轮转速为,（3-3）,因此，可以非常方便地通过控制电机的转速来控制机器
2、人移动和转动速度。,机器人位置估计,已知初始位姿为（x0, y0, q0），两轮转角增量为L和 R，则两轮移动距离分别为lR = rR和lL = rL，,机器人移动距离 l=(lR+lL)/2,方位角变化 q =(lR-lL)/D。,第n步机器人位姿可以按下面公式更新：,若已知机器人的初始位姿，根据该递推公式可以确定任意时刻机器人位姿，比较简单，但因积累误差大，所以长时间不可靠。,3,三轮全向移动机器人运动学,双轮移动机器人运动中最大的问题是不能横向移动，在实际应用中灵活性比较差。,全向移动轮是一种新的轮式移动机构，在大轮的边缘上布置若干小轮，使得机器人的移动方向不再限定于大轮所在的平面方向。
3、,全向移动轮,三轮全向移动机构,xoy是机器人坐标系，机器人的运动速度用vx、vy和w表示，三个全向轮的角速度分别用w1、w2和w3表示，v1、v2和v3分别表示三个全向轮轮心处的线速度。假设全向轮的半径为R，距运动机构中心的距离为L，则速度间关系为：,（3-5）,4,三个全向轮的角速度与机器人速度之间关系：,（3-6）,图3-4全向移动机构在场地坐标系中的位置,场地坐标系下的速度Vx、Vy和W 与机器人坐标系下机器人速度之间的变换关系如下：,可以非常方便地通过控制电机的转速来控制机器人在场地坐标系下的移动和转动速度。,5,平面机械臂运动学,平面机械臂,两连杆平面旋转关节机械臂，其结构由连杆长
4、度L1，L2和关节角1，2确定。,表示关节位置的变量1，2称为关节变量。,旋转关节变量用关节角表示，而移动关节变量用移动距离d表示。,机械手末端位置与关节角之间的关系为,其中 c1=cos1，c12=cos(1+2)，s1=sin1，s12=sin(1+2)。,采用矢量表示为 r = f( ),式中f 表示矢量函数，r =x,yT，=1, 2T。,从关节变量 求手爪位置 r 称为正运动学，,反之，从手爪位置r求关节变量 称为逆运动学。,6,逆运动学公式：,平面机械臂简图,OAB中 a 可以根据余弦定理确定,因此，可以得到 2 = -,1+和都可计算，因此1也是可以计算的。,因此，,逆运动学的解
5、一般不唯一，显然图中机械臂关于OB轴对称的位置也是逆运动学问题的一个解。,空间机械臂连杆描述,机械臂可以看成一系列刚体通过关节连接而成的链式运动机构。一般把这些刚体称为连杆，通过关节将相邻的连杆连接起来。旋转关节和移动关节是机械臂设计中经常采用的单自由度关节。,称基座为连杆0。第一个可移动连杆为连杆1，机械臂的最末端连杆为连杆n。为了使机械臂末端执行器可以在3维空间达到任意的位置和姿态，机械臂至少需要6个关节，因此，典型的工业机械臂一般都具有6个关节。,7,用一条空间直线表示关节的转轴（平移轴），连杆i 的运动可以用转轴i 和连杆i相对连杆i-1的转动角度i 来描述。下面给出几个连杆参数的定义
6、：,连杆长度，连杆两端关节轴线间公垂线的长度,连杆转角，过关节轴i-1做垂直于公垂线的平面，在该平面内做过垂足且平行于关节轴i 的直线。该直线与关节轴i-1的夹角定义为连杆转角。,连杆转角只在两个关节轴为空间异面直线的情况有意义,连杆偏距，关节轴i与相邻关节转轴(i-1和i+1) 公垂线间距离称为连杆偏距,关节角，两相邻连杆绕公共轴线旋转的角度称为关节角。,连杆描述,8,对于一个6关节机器人，需要18个参数就可以完全描述机械臂固定的运动学结构参数。如果机器人6个关节均为转动关节，18个固定参数可以用6组（ai-1, ai-1, di）表示。,机器人的每个连杆都可以用以上四个参数描述，其中连杆长
7、度和连杆转角描述连杆本身，连杆偏距和关节角描述连杆之间的连接关系。,对于转动关节，qi为关节变量，其它三个参数是常数；对于移动关节，di为关节变量，其它三个参数是常数。,这种用连杆参数描述机构运动学关系的规则称为（Devanit-Hartenberg） DH方法，连杆参数称为DH参数。,空间机械臂坐标系选择,为了获得机械臂末端执行器在3维空间的位置和姿态，需要在每个连杆上定义与连杆固连的坐标系来描述相邻连杆之间的位置关系。,根据固连坐标系所在连杆的编号对固连坐标系命名，如在固连在连杆i上的固连坐标系称为坐标系i。,9,固连在基座上的坐标系称为坐标系0。该坐标系在机械臂运动过程中保持固定，因此在
8、研究机械臂运动学问题时一般把坐标系0选为参考系，用来描述其它连杆坐标系的位置。,坐标系i选择示意图,因此，当关节1为转动关节时a0=0，a0=0，d1=0；当关节1为移动关节时a0=0，a0=0，q1 =0。,原则上参考系0可以任意设定，但为了简化描述，通常设定Z0轴沿关节轴1的方向，并且关节1的关节变量为0时参考系0与坐标系1重合。,对于中间连杆i，坐标系i的Zi轴与关节轴i重合，坐标系i的原点位于公垂线ai与关节轴i的交点处。Xi沿ai方向由关节i指向关节i+1，并按照右手系规则确定Yi，ai按右手定则绕Xi转角定义。,若ai =0，两Z轴相交，则选Xi垂于Zi和Zi+1 ，坐标系i的选择
9、不是唯一的。,10,坐标系i选择示意图,连杆坐标系中连杆参数确定,以上4个参数中，ai表示连杆长度只能取非负值；而其余3参数可以为正，也可以为负。,DH参数按以下方法确定：,ai =沿Xi轴，从Zi移动到Zi+1的距离；,ai =绕Xi轴，从Zi旋转到Zi+1的角度；,di =沿Zi轴，从Xi-1移动到Xi的距离；,qi =绕Zi轴，从Xi-1旋转到Xi的角度；,建立连杆坐标系的步骤,2. 找出关节轴i和i+1之间的公垂线或两个轴的交点，以两个轴的交点或公垂线与关节轴i的交点为坐标系i的原点。,1. 找出各关节轴，并标出轴的延长线。步骤2-5仅考虑两个相邻关节轴（i 和 i+1）和坐标系i。,
10、坐标系i选择示意图,6. 当第一个关节变量为0时坐标 系1与坐标系0重合。,3. 规定Zi沿关节轴i的方向。,4. 规定Xi沿公垂线指向关节轴i+1，若两个轴相交，规定Xi垂直于两轴所在的平面。,5. 按右手定则确定Yi轴。,对于坐标系n，原点位置可以在关节轴上任意选取， Xn的方向也是任意的。但在选择时应尽量使更多的连杆参数为0。,11,例3-1 如图3-9所示的平面三连杆机械臂，因为三个关节均为旋转关节，称为RRR（或3R）机构。请在该机构上建立连杆坐标系并写出DH参数。,平面3R机械臂,连杆坐标系布局,定义参考坐标系0，它固定在基座上，当第一个关节变量（q1）为0时坐标系1与坐标系0重合
11、，因此建立参考坐标系0如图所示，Z0轴与关节1的轴线重合且垂直于机械臂所在平面。,由于机械臂位于一个平面上，因此所有Z轴相互平行，且连杆偏距d和连杆转角a均为0。该机械臂的DH参数如表3-1所示。,表3-1 3R机械臂DH参数,12,13,空间机械臂运动学,本节将导出相邻连杆间坐标系变换的一般形式，然后将这些独立的变换联系起来求出连杆n相对连杆0的位置和姿态。,相邻连杆间坐标变换公式,建立 P、Q和R3个中间坐标系，其中i和i-1是固定在连杆 i 和 i-1 上的固连坐标系，如图3-13所示。,图3-13中间坐标系选择示意图,i-1 R，对应变换Rot(x, i-1 )；,1. 绕 Xi-1
12、轴旋转 i-1角,2. 沿 XR轴平移 ai-1,R Q，对应变换Trans(ai-1 ,0,0)；,3. 绕 ZQ轴旋转 i 角,Q P，对应变换Rot(z, i ),4. 沿 ZP 轴平移 di,P i，对应变换Trans(0 ,0, di),因为所有变换都是相对于动坐标系的，所以坐标系i和i-1之间的变换矩阵为：,= Rot(x,i-1)Trans(ai-1,0,0)Rot(z,i)Trans(0,0,di),14,其中各独立变换矩阵如下：,Rot(x,i-1)Trans(ai-1,0,0)Rot(z,i)Trans(0,0,di),连杆间的通用变换公式：,对于任意的n连杆机械臂，只要给
13、出各连杆的DH参数，即可以计算机械臂末端在固定坐标系0下表示的变换矩阵（位置和姿态）,因此，采用DH规则选择连杆坐标系，并用DH参数描述连杆，可以非常容易地获得机械臂的变换矩阵，关键是首先获得机械臂的DH参数描述。,15,例3-3 利用表3-1的DH参数计算个连杆的变换矩阵，并计算末端连杆现对固定坐标系的变换矩阵。,，,其中c123 = cos(q1+q2+q3)，s123 = sin(q1+q2+q3)。从最后一式可以看出，坐标系3的原点坐标与（3-10）式的结果完全相同。,16,PUMA560工业机器人运动学,右图所示PUMA560是一个6自由度工业机器人，所有关节均为转动关节。,图3-1
14、6,图3-15,机器人的连杆参数如表3-2所示。,图3-15和图3-16给出了所有关节角为零位时，连杆坐标系的分布情况。与大多数工业机器人一样，PUMA560关节4、5和6的轴线相交于同一点，且交点与坐标系4、5和6的坐标原点重合。后面将介绍如此设计的原因。,17,表3-2 PUMA560 连杆参数表,将相应的参数代入（3-16）得各连杆的变换矩阵如下：,将以上变换矩阵连乘即可得到,因为在第4章逆运动学求解需要，这里计算一些中间结果：,18,最终得到六个连杆坐标变换矩阵的乘积：,其中各元素值为：,上式即为PUMA560的运动学方程，给出了机器人末端坐标系6相对于基座固定坐标系0的位姿。显然，手
15、工计算6自由度机器人的运动学方程还是比较复杂的，但是，采用计算机编程实现运动学计算非常容易。只需要输入机器人的DH参数，再利用（3-16）式，六个矩阵连乘即可获得运动学方程式（3-24）。,坐标系的标准命名规则,图3-17 标准坐标系,为了分析处理方便，机器人和工作空间一般采用规范的命名，并采用“标准”的名字对各种坐标系命名。图3-17表示了5个坐标系，并给出了标准命名。,基坐标系B固连于机器人的基座上，就是上节介绍的坐标系0。在连杆描述时经常称之为连杆0。,基坐标系B,S一般固连于机器人工作台的一个角上，有时称之为任务坐标系，机器人的所有运动都是相对于工作台坐标系S执行的。工作台坐标系S通常
16、根据基坐标系B来确定，两个坐标系都是固定坐标系。,工作台坐标系S,W固连于机械臂末端连杆，因此也称为坐标系n。一般情况下腕部坐标系的原点位于机械臂的手腕上。 W也是相对于基坐标系B定义的。,腕部坐标系W,19,T一般固连于机器人所夹持工具的末端。工具坐标系T通常根据腕部坐标系W定义。,工具坐标系T,目标坐标系G,目标坐标系G是机器人移动工具时对期望工具位置的描述。在机器人运动结束时，工具坐标系T与目标坐标系G重合。目标坐标系G通常相对工作台坐标系S定义。,机器人完成期望操作的主要任务之一是对所夹持的工具进行定位。图3-17中虚线表示了坐标系间的描述关系，可以用变换矩阵表示这种描述关系：,工具坐
17、标系定位,1. 确定变换关系,2. 根据机器人完成期望操作任务,=,的要求得到变换方程,3. 采用逆运动学求解（下章介绍）期望的机械臂各关节变量。,4. 根据期望的关节变量控制机械臂运动，工具坐标系达到期望位姿。,20,第4 章 机器人逆运动学,已知机器人末端的位置和姿态（坐标变换矩阵）确定各关节变量的值是一个非常关键的问题。该问题就是机器人逆运动学将要解决的问题。,逆运动学问题的可解性,机械臂逆运动学问题就是已知,的数值，求各关节变量。,从（3-24）中求解关节变量q1, q2, , q6是一个非线性问题。,解的存在性,逆运动学问题解是否存在完全取决于机械臂的工作空间。所谓工作空间是指机械臂
18、末端执行器所能达到的空间位姿的集合。,当期望位姿位于机械臂的工作空间之外时，逆运动学问题无解。,期望机械臂末端达到B点,对于给定的机械臂，其工作空间是固定的。而对于少于6个自由度的机械臂，它在三维空间内不能达到全部位姿。所以通用工业机器人一般都设计成6个自由度。,21,多解问题,逆运动学求解的另一个问题是多解问题。,图4-2 平面机械臂，有两个解,如图4-2 所示的平面机械臂有两个解，虚线表示另外一个解。,逆运动学解的个数取决于机械臂关节的数量，同时与连杆参数和关节运动范围有关。PUMA560工业机器人一般存在8个解。,逆运动学问题解法,从运动学方程中求解关节变量q1, q2, , q6是一个
19、非线性方程组求解问题。而非线性方程组求解方法分为封闭（解析）解法和数值解法两大类。,数值解法不适用，一是机械臂操作需要频繁求解逆运动学问题，数值解法计算量比较大；二是数值解法不能保证求出全部解。所以逆运动学问题一般只采用封闭（解析）解法。,机器人逆运动学问题涉及一个复杂的非线性方程组求解，而从数学角度分析一般的非线性方程组经常没有封闭（解析）解。,对于机械臂逆运动学问题存在解决方案，因为机械臂是人造机构，只需将其设计成存在封闭解的结构即可解决该问题。,22,理论上已经证明，对于6自由度机械臂，存在封闭解的充分条件是有相邻的三个关节轴相交于一点。因此，已经设计出来的6自由度机械臂几乎都有三个相交
20、的关节轴，例如PUMA560的4、5、6轴交于一点。,欧拉变换解,式（2-40）给出了采用欧拉角表示的坐标变换，其逆问题是给定旋转矩阵Rzyz，确定对应的三个欧拉角。,假设给定的旋转矩阵表示，根据欧拉变换方程可得9个方程,可以由最后一个方程求出，再由倒数第二个方程求出最后根据倒数第四个方程计算出。,这种方法的不足是反正弦和反余弦解的不确定性和解的精度问题。,实际问题一般用双变量反正切函数计算相应的角度。,23,双变量反正切函数,atan2的精确程度对其整个定义域都是一样的。高级编程语言，如C和Matlab等，提供标准库函数供编程者调用。,双变量反正切函数,在三角函数求解时，通常采用双变量反正切
21、函数atan2(y, x)来确定角度。atan2提供二个自变量，即纵坐标和横坐标，见图4.3。,当 -时，由atan2反求角度过程中，同时检查y和x的符号来确定其所在象限。该函数也能检验什么时候x或y为0，并反求出正确的角度。,根据（2-39）和（2-40）式可知：,（4-2）,上式中，矩阵两边对应（2，3）元素相等得-axs+ayc=0tan=ay/ax，所以得的两个解,=atan2(ay,ax)，= +,24,，计算各关节变量q1, q2, , q6。,25,对应（1，3）和（3，3）元素相等得 sq= axc+ays，cq=az 所以有,=atan2(cax+say, az),对应（2，
22、1）和（2，2）元素相等得s= -nxs+nyc，c=-oxs+oyc所以有,=atan2(-nxs+nyc, -oxs+oyc),欧拉变换解计算过程并不复杂，该问题一般存在两个解。,当=0时，根据（4-1）知ay=ax=0，此时（4-3）将不能确定的值。此时对应绕Z轴连续做两次旋转，不能确定每次转的角度属于欧拉角奇异情况。,PUMA560逆运动学,将研究PUMA560的逆运动学封闭解，一般的6自由度工业机器人逆运动学可以参考该方法进行求解。,该问题是已知变换矩阵,各连杆坐标系变换关系如下：,与欧拉角求解类似，根据第3章PUMA560运动学（3-21）和（3-23）式得,其中（3-22）的最后
23、三个数如下：,（4-6）,令（4-6）式两边元素（2, 4）相等，得到,-pxs1+pyc1=d3,做三角恒等变换,px=cos，py=sin,其中,-cs1+sc1=d3/r sin(-1)=d3/r,所以,因此,最后1的解可以写为,26,（4-10）,（4-13）,采用类似的方法可以求得其它关节角的解可以写为,2=23-3 （ 4-17）,（4-19）,（4-22）,（4-24）,由于（4-10）式和（4-13）式的1和3各有两个解，另外机械臂腕关节“翻转”可以得到46的另一组解,（4-26）,因此，PUMA560的逆运动学问题共有8组解。,（4-10）,27,28,小结,机器人运动方程的表示用变换矩阵表示机械手的运动方程用转角（即欧拉角）变换序列表示运动姿态用横滚俯仰和偏转角表示运动姿态,机器人运动方程的求解,欧拉变换解 PUMA560机器人运动方程求解,
