1、1.3 机器人的位姿分析1.3.1 杆件坐标系的建立坐标系号的分配方法 机器人的各连杆通过关节连接在一起，关节有移动副与转动副两种。按从机座到末端执行器的顺序，由低到高依次为各关节和各连杆编号，如图示。机座的编号为杆件0，与机座相连的连杆编号为杆件1，依此类推。机座与连杆1的关节编号为关节1，连杆1与连杆2的连接关节编号为2，依此类推。各连杆的坐标系 Z 轴方向与关节轴线重合(对于移动关节，Z 轴线沿此关节移动方向)。末端执行器上的坐标系依据手爪手指的开合方向定。原点位于形心；Xn沿末端执行器手指组成的平面的法向；Yn垂直于手指；Zn的方向朝外指向目标。,第3次,各坐标系的方位的确定有两种方法
2、：一是一般方法，只要满足前述条件，对坐标系的各坐标轴的分配无任何特殊规定，后一坐标系向前一坐标系的坐标变换完全按照坐标变换方程进行。,二是D-H方法，它严格定义了每个坐标系的坐标轴，并对连杆和关节定义了4个参数。下面介绍这种方法。（目前最常用的方法）,转动关节的D-H坐标系 如图示。连杆 i 的坐标系的 Zi 轴位于 i+1 的转动关节轴线上；连杆i的两端关节轴线的公垂线为连杆 i 坐标系的 Xi 轴，方向指向下一个连杆；公垂线与 Zi 的交点为原点；坐标系的 Yi 轴由 Xi 和 Zi 确定。,杆件参数的意义- 和,关节Ai轴和Ai+1轴线公法线的长度 关节i轴线与i+1轴线在垂直于ai平面
3、内的夹角,串联关节，每个杆件最多与2个杆件相连，如Ai与Ai-1和 Ai+1相连。由运动学的观点来看，杆件的作用仅在于它能保持其两端关节间的形态不变。这种形态由两个参数决定，一是杆件的长度 ，一个是杆件的扭转角,Ai,Ai+1,杆件参数的意义- 和,是从第i-1坐标系的原点到Zi轴和i轴的交点沿i-1轴测量的距离 绕 Zi-1轴由i-1 轴转向i轴的关节角,确定杆件相对位置关系，由另外2个参数决定，一个是杆件的距离： ，一个是杆件的回转角：,Ai,Ai+1,Ai-1,坐标系的建立原则,Ai,Ai+1,Ai-1,为右手坐标系原点Oi：设在 与zi+1轴线的交点上 Zi轴：与zi+1关节轴重合，指
4、向任意 Xi轴：与公法线 重合，指向沿 由zi轴线指向zi+1轴线 Yi轴：按右手定则,沿 xi 轴， zi-1 轴与 xi 轴交点到 0i 的距离i 绕 xi 轴，由 zi-1 转向zidi 沿 zi-1 轴，zi-1 轴和 xi 交点至0i 1 坐标系原点的距离i 绕 zi-1 轴，由 xi-1转向 xi,有一种特殊情况，即连杆 i 的两端轴线平行。由于两平行轴线的公垂线存在多值，故无法确定连杆 i 的坐标系原点。这时，连杆 i 的坐标系原点由 di+1 确定。,特殊情况坐标系的建立原则,Oi Ai与Ai+1关节轴线的交点 Zi Ai+1轴线 Xi Zi和Zi-1构成的面的法线 Yi 右手
5、定则,两个关节轴相交,两个关节轴线平行,先建立 0i-1然后建立0i+1最后建立 0i,平动关节（棱柱联轴器）的D-H坐标系 对于图示平动关节，距离di成为关节变量。而平动关节轴的方向即为此联轴器移动的方向，但不同于转动关节的情况是该轴空间位置没有规定。对于联轴器来说，其长度ai没有意义，令其为零。平动关节轴的坐标系原点与下一个连杆原点重合。Zi-1 轴在关节 i+1 的轴线上。Xi-1轴平行于Zi-1轴器矢量与 Zi 矢量的交积。当 di =0时，定义该联轴器的位置为零。,1.广义连杆(D-H坐标)全为转动关节:Zi坐标轴;Xi坐标轴;Yi坐标轴;连杆长度ai;连杆扭角i;两连杆距离di;两
6、杆夹角i,全为转动关节:Zi坐标轴:沿着i+1关节的运动轴;Xi坐标轴:沿着Zi和Zi-1的公法线,指向离开Zi-1轴的方向;Yi坐标轴:按右手直角坐标系法则制定;连杆长度ai; Zi和Zi-1两轴心线的公法线长度;连杆扭角i: Zi和Zi-1两轴心线的夹角;两连杆距离di:相邻两杆三轴心线的两条公法线间的距离;两杆夹角i :Xi和Xi-1两坐标轴的夹角;,含移动关节:Zi坐标轴;Xi坐标轴;Yi坐标轴;连杆长度ai=0;连杆扭角i;两连杆距离di;两杆夹角i,含移动关节:Zi坐标轴: 沿着i+1关节的运动轴;Xi坐标轴: 沿着Zi和Zi-1的公法线,指向离开Zi-1轴的方向;Yi坐标轴: 按
7、右手直角坐标系法则制定;连杆长度ai; Zi和Zi-1两轴心线的公法线长度;（沿Xi轴，从Zi-1移动到Zi的距离）连杆扭角i: Zi和Zi-1两轴心线的夹角; （绕Xi轴，从Zi-1旋转到Zi的角度）两连杆距离di: 相邻两杆三轴心线的两条公法线间的距离; （沿Zi-1轴，从Xi-1移动到Xi的距离）两杆夹角i : Xi和Xi-1两坐标轴的夹角; （绕Zi-1轴，从Xi-1旋转到Xi的角度）,注意,固连于机器人基座（即连杆0）上的坐标系为坐标系0。通常作为参考坐标系。可任意设定，但为了简化，通常设定Z0轴沿关节轴1的方向，并且当关节变量为0时，设定参考坐标系0与坐标系1重合。,例 PUMA
8、560机器人如图示。建立D-H坐标系。,PUMA 560机器人结构简图,PUMA 560机器人的D-H参数表,举例：Stanford机器人,A1,A2,A3,A4,A5,A6,为右手坐标系原点Oi： Ai与Ai+1关节轴线的交点Zi轴：与Ai+1关节轴重合，指向任意 Xi轴： Zi和Zi-1构成的面的法线Yi轴：按右手定则,Li 沿 xi 轴， zi-1 轴与 xi 轴交点到 0i 的距离i 绕 xi 轴，由 zi-1 转向zidi 沿 zi-1 轴，zi-1 轴和 xi 交点至0i 1 坐标系原 点的距离i 绕 zi-1 轴，由 xi-1转向 xi,例 斯坦福机器人的结构示意图如图。建立D-
9、H坐标系。,斯坦福机器人结构示意图,1.3.2 连杆坐标系间的变换矩阵连杆坐标系间的齐次变换矩阵的表示方法 用 表示机器人将连杆n坐标系的坐标变换成连杆n1坐标系的坐标的齐次坐标变换矩阵，通常把上标省略，写成An。从连杆 i 到连杆 i1 的坐标变换矩阵Ai为Ai = Rot ( Z, i ) Trans ( ai , 0 , di ) Rot ( X, i),杆件坐标系间的变换过程 -相邻关节坐标系的齐次变换,将xi-1轴绕zi-1轴转i 角度，将其与xi轴平行；沿zi-1轴平移距离di ，使zi-1轴与zi轴重合；沿xi轴平移距离ai，使两坐标系原点及x轴重合；绕xi 轴转i角度，两坐标系
10、完全重合,进一步得出Ai=Rot ( Z, i ) Trans( ai , 0, di )Rot ( X, i ) 对联轴器的齐次坐标变换矩阵有 Ai=,机器人的运动学方程,D-H变换矩阵,1.4 机器人正向运动学 根据前面介绍的方法，欲研究机器人运动学首先应建立机器人各杆件的构件坐标系，从而得出齐次坐标变换矩阵Ai 。Ai 能描述连杆坐标系之间相对平移和旋转的齐次变换。A1 描述第一个连杆对于机身的位姿，A2 描述第二个连杆坐标系相对于第一个连杆坐标系的位姿，依此类推。 若有一个六连杆机器人，机器人末端执行器坐标系（即连杆坐标系6）的坐标相对于连杆 i 坐标系的齐次变换矩阵，用 iT6 表示
11、，即iT6= Ai+1 A6它描述第六个连杆对于相对于连杆 i 坐标系的位姿。机器人末端执行器相对于机身坐标系的齐次变换矩阵为0T6=A1 A2 A6式中： 0T6 常写成 T6。它描述第六个连杆 (末端执行器) 对于相对于连杆0(机座) 坐标系的位姿。,第4次,1.4.1 斯坦福机器人运动方程 以斯坦福机器人为例说明如何建立机器人的运动方程。 斯坦福机器人如图示。求 Ai ( i=1, 2, 3, 4, 5, 6 ) 及T6的表达式。D-H坐标系的建立（如图示，过程见上节）确定各连杆的D-H参数和关节变量（见表，过程见上节）根据齐次变换矩阵公式，可求得Ai 求机器人的运动方程T6=A1 A2
12、 A3 A4 A5 A6,，,，,d6,T6 =式中：nX=c1c2(c4c5c 6s4s6)s2s6c6s1(s4c5c6+c4s6)；nY=s1c2(c4c5c6s4s6)s2s5c6c1(s4c5c6+c4s4)；nZ=s2(c4c5c6s4s6)c2s5c6；oX=c1c2(c4c5s6+s4c6)+s2s5s6s1(s4c5s6+c4c5)；oY=s1c2(c4c5c6s4c6)+s2s5s6+c1(s4c5s6+c4c5)；oZ=s2(c4c5s6+s4c6)+c2s5c6；aX=c1(c2c4s5+s2c5)s1s4s5；aY=s1(c2c4s5+s2c5)+c1s4s5；aZ=
13、s2c4s5+c2c5；pX=c1s2d3s1d2 + fx(d6) ；pY=s1s2d3+c1d2 + fx(d6) ；pZ=c2d3 + fx(d6) 。注： ci = cos i； si = sini,1.4.2 PUMA 560 机器人运动方程 1) D-H坐标系的建立如前。 2) 确定各连杆的D-H参数和关节变量如前，设a3=0。 3) 求两杆之间的位姿矩阵Ai (略) 4) 求机器人的运动方程 T6 = nX=c1c23(c4c5c6s4s6)s23s6c6s1(s4c5c6+c4s 6)； nY=s1c23(c4c5c6s4s6)s23s5c6+c1(s4c5c6+c4s6)；
14、nZ=s23(c4c5c6s4s6)c2s5c6； oX=c1c23(c4c 5s6s4c6)+s23s5s6s1(s4c5s6+c4c6)； oY=s1c23(c4c5s6s4c6)+s23s5s6 +c1(s4c5c 6+c4c6)； oZ=s23(c4c5s6+s4c6)+c23s5s6； aX=c1(c23c4s5+s23c5)c1s4s5； aY=s1(c23c 4s5+s23c5)+c1s4s5； aZ=s23c4s5+c23c5； PX=c1d6(c23c4s5+s23c5)+s23d4 +a2c 2s1 (d6s4s5+d2) ； PY=s1d6(c23c4s5+s23c5)+
15、s23d4 +a2c 2 +c1 (d6s4s5+d2) ； PZ= d6(c23c5 s23c4s5)+c23d4 a2s 2 注：c23=cos(2+3)=c2c3s2s3；s23=sin(2+3)=c2s3s2c3,1.5 机器人逆向运动学 给定末端连杆的位姿计算相应关节变量的过程叫做运动学逆解。1.5.1 逆向运动学的解多解性机器人的运动学逆解具有多解性。图示，对于给定的位置与姿态，它具有两组解。 造成多解的原因是解反三角函数方程产生的。只有一组解与实际情况对应。剔除多余解的方法：(1) 根据关节运动空间来选择合适的解；(2) 选择一个最接近的解；(3) 根据避障要求选择合适的解；(4
16、) 逐级剔除多余解。可解性 能否求得机器人运动学逆解的解析式是机器人的可解性问题。转动和移动关节的机器人系统，在一个单一串联链中有6个自由度(或小于6)时是可解的。其通解是数值解，不是解析表达式。要使机器人有解析解，设计时就要使机器人的结构尽量简单，而且尽量满足有若干个相交的关节轴或许多i等于0、90的特殊条件。,用未知的逆变换逐次左乘，由乘得的矩阵方程的元素决定未知数，即用逆变换把一个未知数由矩阵方程的右边移到左边 求解这个未知数 把下一个未知数移到左边重复上述过程，直到解出所有解,运动学逆问题解法,Paul 等人提出的方法:,Paul 等人提出的方法,作业,1.14（P61),小节,先建立
17、D-H坐标系；确定各连杆的D-H参数和关节变量；根据齐次变换矩阵确定位姿矩阵；求解运动学方程；求运动学的逆解。,本章复习,旋转矩阵,(1.13),(1.14),(1.12),旋转矩阵的特点：(1) 主对角线上有一个元素为1，其余均为转角的余弦/正弦；(2) 绕轴转动的次序与元素1所在的行、列号对应；(3) 元素1所在的行、列，其它元素均为0；(4) 从元素1所在行起，自上而下，先出现的正弦为负，后出现的为正，反之依然。,平移矩阵（齐次坐标形式）,Trans(X，Y，Z) 称为平移齐次变换矩阵，又称平移算子。 第四列元素X、Y、Z分别表示沿坐标轴 X、Y、Z 的移动量,变换,算子左、右乘规则（P
18、37）若相对固定坐标系进行变换，则算子左乘；若相对动坐标系进行变换，则算子右乘。,转动关节的D-H坐标系 如图示。连杆 i 的坐标系的 Zi 轴位于 i+1 的转动关节轴线上；连杆i的两端关节轴线的公垂线为连杆 i 坐标系的 Xi 轴，方向指向下一个连杆；公垂线与 Zi 的交点为原点；坐标系的 Yi 轴由 Xi 和 Zi 确定。,含移动关节:Zi坐标轴: 沿着i+1关节的运动轴;Xi坐标轴: 沿着Zi和Zi-1的公法线,指向离开Zi-1轴的方向;Yi坐标轴: 按右手直角坐标系法则制定;连杆长度ai; Zi和Zi-1两轴心线的公法线长度;（沿Xi轴，从Zi-1移动到Zi的距离）连杆扭角i: Zi和Zi-1两轴心线的夹角; （绕Xi轴，从Zi-1旋转到Zi的角度）两连杆距离di: 相邻两杆三轴心线的两条公法线间的距离; （沿Zi-1轴，从Xi-1移动到Xi的距离）两杆夹角i : Xi和Xi-1两坐标轴的夹角; （绕Zi-1轴，从Xi-1旋转到Xi的角度）,机器人的运动学方程,D-H变换矩阵,机器人的逆向运动学给定末端连杆的位姿计算相应关节变量的过程叫运动学逆解；多解性；可解性；,
