机器人位姿方程学习指导书.pptVIP

第三章 机器人位姿方程 §3-0 多刚体之间的位姿关系 一、操作机位姿方程的正解 二、操作机位姿方程的逆解 nx = c1[c23(c4c5c6?s4s6) ? s23s5c6]+s1(s4c5c6+c4s6) nx = s1[c23(c4c5c6?s4s6) ? s23s5c6]?c1(s4c5c6+c4s6) nz = ? s23(c4c5c6?s4s6) ? c23s5c6 ox = c1[c23(?c4c5c6?s4s6) + s23s5c6]+s1(c4c6 ? s4c5s6) oy = s1[c23(?c4c5s6?s4c6) + s23s5s6]?c1(c4c6 ? s4c5s6) oz = ? s23(?c4c5c6?s4s6) + c23s5c6 ax = ? c1(c23c4s5+s23c5) ? s1s4c5 ay = ? s1(c23c4s5+s23c5) + c1s4s5 az =s23c4s5 ? c23c5 px = c1[a2c2 + a2c23 ? d4 s23] ? d2s1 py = s1[a2c2 + 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：将待求量以到方程左边，使左边只有待求量， 而右边矩阵有对应的常数项，递推求解： 求出?4 求出?5和?6 注意：通常上述递推并不需要作完，就可利用等号两端矩阵对应元素相等，求出全部的关节变量。 技巧1的应用： ① ?1 ② ?3 ③ ?2 ④ ?4 ⑤ ?5 ⑥ ?6 求解顺序： ① ?1 、② ? 3 、 ③ ?2 ④ ?4 、⑤ ?5 、 ⑥ ?6 1．求?1 根据，由例3-3得： (a) n?x = c23[c4c5c6 ? s4s6] ? s23s5c6 n?y = ?s4c5c6 ? c4s6 n?z = ?s23[c4c5c6?s4s6] ? c23s5c6 o?x = ?c23[c4c5c6+s4s6] + s23s5c6 o?y = s4c5c6 ? c4s6 o?z = s23[c4c5c6+s6s6] + c23s5s6 a?x = ?c23c5s5 ? s23c5 a?y = s4s5 a?z = s23c4s5 – c23c5 p?x = a2c2 + a3c23 ? d4s23 p?y= d3 p?z = ? a3c23 ? a2s2 ? d4s23 n?x = c23[c4c5c6 ? s4s6] ? s23s5c6 n?y = ?s4c5c6 ? c4s6 n?z = ?s23[c4c5c6?s4s6] ? c23s5c6 o?x = ?c23[c4c5c6+s4s6] + s23s5c6 o?y = s4c5c6 ? c4s6 o?z = s23[c4c5c6+s6s6] + c23s5s6 a?x = ?c23c5s5 ? s23c5 a?y = s4s5 a?z = s23c4s5 – c23c5 p?x = a2c2 + a3c23 ? d4s23 p?y= d2 p?z = ? a3c23 ? a2s2 ? d4s23 ?s1px+c1py = d2 (b) 代入(b)式： c1s? ? s1c? = d2 / ? 得：sin(? ? ? ) = d2/? 则： 令：px = ?cos? ， py = ?sin? 技巧2：作三角代换： 当 时，则指定的O6点不在工作空间内。 n?x = c23[c4c5c6 ? s4s6] ? s23s5c6 n?y = ?s4c5c6 ? c4s6 n?z = ?s23[c4c5c6?s4s6] ? c23s5c6 o?x = ?c23[c4c5c6+s4s6] + s23s5c6 o?y = s4c5c6 ? c4s6 o?z = s23[c4c5c6+s6s6] + c23s5s6 a?x = ?c23c5s5 ? s23c5 a?y = s4s5 a?z = s23c4s5 – c23