《第3章机器人位姿方程剖析》由会员分享，可在线阅读，更多相关《第3章机器人位姿方程剖析（56页珍藏版）》请在金锄头文库上搜索。
1、第三章 机器人位姿方程,3-0 多刚体之间的位姿关系,1,2,n,1,2,n,S0描述S1：,S1描述S2 ：,Sn-1描述Sn ：,S0描述S1 ：,S1描述S2 ：,Sn-1描述Sn ：,1,2,n,一、位姿方程：,都是位姿方程,利用位姿方程可求解任意矩阵，例如,1,2,n,也可列出位姿方程：,二、矩阵方程变换图,0,1,n,3,2,n-1,1,n,3,2,(n-1),基本画法： 短线代表坐标系； 有向箭头线代表变换。,作用：巡线列位姿方程 任何两短线（坐标系）之间的有向箭头连线都可以通过已有的有向箭头连线（阵）描述，且和向量的矢量方程式类似； 巡有向箭头线方向走，与该有向箭头线所表示的矩
2、阵右乘 巡有向箭头线反方向走，与该有向箭头线所表示的矩阵的逆阵右乘,例如：n与n的连线 代表,作用：巡线列位姿方程 任何两短线（坐标系）之间的有向箭头连线都可以通过已有的有向箭头连线（阵）描述，且和向量的矢量方程式类似； 巡有向箭头线方向走，与该有向箭头线所表示的矩阵右乘 巡有向箭头线反方向走，与该有向箭头线所表示的矩阵的逆阵右乘,0,1,n,3,2,n-1,1,n,3,2,(n-1),练习：2与3的连线 代表,作用：巡线列位姿方程 任何两短线（坐标系）之间的有向箭头连线都可以通过已有的有向箭头连线（阵）描述，且和向量的矢量方程式类似； 巡有向箭头线方向走，与该有向箭头线所表示的矩阵右乘 巡有
3、向箭头线反方向走，与该有向箭头线所表示的矩阵的逆阵右乘,0,1,n,3,2,n-1,1,n,3,2,(n-1),例如：0与n的连线 代表,作用：巡线列位姿方程 任何两短线（坐标系）之间的有向箭头连线都可以通过已有的有向箭头连线（阵）描述，且和向量的矢量方程式类似； 巡有向箭头线方向走，与该有向箭头线所表示的矩阵右乘 巡有向箭头线反方向走，与该有向箭头线所表示的矩阵的逆阵右乘,0,1,n,3,2,n-1,1,n,3,2,(n-1),3-1 操作机两杆间位姿矩阵的建立,轴 i-1,轴 i,轴 i+1,杆 i,杆 i1,Oi,Oi-1,小结： 1.坐标系的建立： (1)zi轴在i杆的小标号轴线上；
4、(2)xi轴为i杆两个轴的公垂线，由小标号指向大标号。 2.D-H参数的规定： (1)构件几何尺寸 1) i杆的杆长ai为i杆公垂线长度，标量； 2) i杆的扭转角i为zi 轴到zi+1轴，绕xi轴，代数量； (2)构件运动、安装参数 i杆相对于i-1杆的转角i为xi-1轴到xi轴，绕zi轴，代数量； i为转动关节，则i运动参数； i为移动关节，则i安装参数； 2) i杆相对于i-1杆的距离di为xi-1轴到xi轴，沿zi轴，代数量。 i为转动关节，则di安装参数； i为移动关节，则di运动参数；,o2,o5,o6,o2,o5,o6,【例3-3 】设立图示之PUMA560机器人的六杆操作机的坐
5、标系，并标定D-H参数。,o2,o5,o6,o1,o3,o4,a3,d2,a2,d4,3-2 操作机位姿方程的正、逆解,正解：已知关节空间，求笛卡儿空间。,定义：,关节空间：关节变量的集合。,笛卡儿空间： 笛卡儿坐标系描述下的构件位姿的集合。,逆解：已知笛卡儿空间，求关节空间。,【例3-3 】，【例3-4 】试求如图3-8所示之PUMA560机器人的六杆操作机的正逆解,说明：,2、 令PUMA560几何参数d2=a3= 0就是MOTOMAN商用机器人的几何模型；（a2,d4),1、 PUMA560机器人的六杆操作机是几何参数最多的商用机器人；（a2,a3,d2,d4),3、 PUMA560操作
6、机位姿方程的正逆解的步骤和技巧具有普遍指导作用。,一、操作机位姿方程的正解,已知关节空间：,求笛卡儿空间：,因为,所以,现列出所有变换矩阵：,所以，运动学正解矩阵为：,细化出显式公式：,nx = c1c23(c4c5c6s4s6) s23s5c6+s1(s4c5c6+c4s6) ny = s1c23(c4c5c6s4s6) s23s5c6c1(s4c5c6+c4s6) nz = s23(c4c5c6s4s6) c23s5c6 ox = c1c23(c4c5c6s4s6) + s23s5c6+s1(c4c6 s4c5s6) oy= s1c23(c4c5s6s4c6) + s23s5s6c1(c4
7、c6 s4c5s6) oz = s23(c4c5c6s4s6) + c23s5c6 ax = c1(c23c4s5+s23c5) s1s4c5 ay = s1(c23c4s5+s23c5) + c1s4s5 az =s23c4s5 c23c5 px = c1a2c2 + a2c23 d4 s23 d2s1 py = s1a2c2 + a2c23 d4 s23 + d2c1 pz = a3s23 a2s2 d4c23,二、操作机位姿方程的逆解,已知关节空间：,求笛卡儿空间：,nx = c1c23(c4c5c6s4s6) s23s5c6+s1(s4c5c6+c4s6) nx = s1c23(c4c
8、5c6s4s6) s23s5c6c1(s4c5c6+c4s6) nz = s23(c4c5c6s4s6) c23s5c6 ox = c1c23(c4c5c6s4s6) + s23s5c6+s1(c4c6 s4c5s6) oy = s1c23(c4c5s6s4c6) + s23s5s6c1(c4c6 s4c5s6) oz = s23(c4c5c6s4s6) + c23s5c6 ax = c1(c23c4s5+s23c5) s1s4c5 ay = s1(c23c4s5+s23c5) + c1s4s5 az =s23c4s5 c23c5 px = c1a2c2 + a2c23 d4 s23 d2s1
9、 py = s1a2c2 + a2c23 d4 s23 + d2c1 pz = a3s23 a2s2 d4c23,已知,通过各种途径求解1 2 3 4 5 6 。,a2、a3、d2、d4已知。,6号坐标系原点O6（= O4 =O5 ）的位置坐标，只与位置机构有关，只与（1 2 3）有关，可首先独立解出（1 2 3） 。,6号坐标系姿态，与（1 2 3 4 5 6 ）都有关，必须在（1 2 3独立解出后方可解出（4 5 6 ） 。,技巧1：将待求量以到方程左边，使左边只有待求量， 而右边矩阵有对应的常数项，递推求解：,求出1,求出2,求出3,技巧1：将待求量以到方程左边，使左边只有待求量， 而右
10、边矩阵有对应的常数项，递推求解：,求出4,求出5和6,注意：通常上述递推并不需要作完，就可利用等号两端矩阵对应元素相等，求出全部的关节变量。,技巧1的应用：,求解顺序： 1 、 3 、 2 4 、 5 、 6,1求1 根据，由例3-3得：,(a),nx = c23c4c5c6 s4s6 s23s5c6 ny = s4c5c6 c4s6 nz = s23c4c5c6s4s6 c23s5c6 ox = c23c4c5c6+s4s6 + s23s5c6 oy = s4c5c6 c4s6 oz = s23c4c5c6+s6s6 + c23s5s6 ax = c23c5s5 s23c5 ay = s4s
11、5 az = s23c4s5 c23c5 px = a2c2 + a3c23 d4s23 py= d3 pz = a3c23 a2s2 d4s23,nx = c23c4c5c6 s4s6 s23s5c6 ny = s4c5c6 c4s6 nz = s23c4c5c6s4s6 c23s5c6 ox = c23c4c5c6+s4s6 + s23s5c6 oy = s4c5c6 c4s6 oz = s23c4c5c6+s6s6 + c23s5s6 ax = c23c5s5 s23c5 ay = s4s5 az = s23c4s5 c23c5 px = a2c2 + a3c23 d4s23 py= d
12、2 pz = a3c23 a2s2 d4s23,s1px+c1py = d2 (b),代入(b)式： c1s s1c = d2 / ,得：sin( ) = d2/,则：,令：px = cos ， py = sin,技巧2：作三角代换：,nx = c23c4c5c6 s4s6 s23s5c6 ny = s4c5c6 c4s6 nz = s23c4c5c6s4s6 c23s5c6 ox = c23c4c5c6+s4s6 + s23s5c6 oy = s4c5c6 c4s6 oz = s23c4c5c6+s6s6 + c23s5s6 ax = c23c5s5 s23c5 ay = s4s5 az =
13、 s23c4s5 c23c5 px = a2c2 + a3c23 d4s23 py= d2 pz = a3c23 a2s2 d4s23,2利用式（a）还可求3,c1px + s1py = a3c23 d4s23 + a2c2 pz = a3s23 + d4c23 + a2s2,将上两式左右平方相加，再与式（b）左右平方相加得：,a3c3 d4s3 = K (d),s1px+c1py = d2 (b),经三角代换后可得：,3求2,=,c1c23px + s1c23py s23pz a2c3 = a3 c1c23px + s1c23py c23pz + a2s3 = d4,将上两式联立求解s23，
14、c23，得,c1c23px + s1c23py s23pz a2c3 = a3 c1c23px + s1c23py c23pz + a2s3 = d4,A = c1px + s1py,4求4,=,axc1c23 + ays1c23 azs23 = c4s5 axs1 + ayc1 = s4s5,axc1c23 + ays1c23 azs23 = c4s5 axs1 + ayc1 = s4s5,5求5,ax(c1c23c4 + s1s4) + ay (s1c23c4 c1s4 ) az (s23c4) = s5 ax(c1c23) + ay (s1s23) + az (c23) = c5,6求6,s6 = nx(c1c23s4 s1c4) ny(s1c23s4 + c1c4) + nzs23s4 c6 = nx(c1c23c4 +
