机器人位姿方程.pptVIP

axc1c23 + ays1c23 ? azs23 = ?c4s5 ?axs1 + ayc1 = s4s5 只要s5≠0，便可求出θ4 当s5＝0时，机械手处于奇异形位。 此时，关节轴4和6重合，只能解出θ4与θ6的和或差。 奇异形位可以由上式中atan2的两个变量是否都接近零来判别。若都接近零，则为奇异形位，否则，不是奇异形位。 在奇异形位时，可任意选取θ4的值，再计算相应的θ6 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 + s1s4)c5 ? c1s23s5]+ny[(s1c23c4 ? c1s4)c4 ? s1s23s5]?nz(s23c4c5 + c23s5) 问题： c1s? ? s1c? = d3 / ? 得： sin(? ? ? ) = d3/? 可解出θ1角 或 为什么用双变量反正切函数atan2(x,y)？ 采用反正、余弦函数计算θ ，会带来如下问题： 1）由于绝对值相同的正负角度的余弦相等，如cosθ＝cos（-θ），因此不能确定反余弦的结果是在那个象限； 反正切函数θ＝tan－1（x / y）所在的象限空间由自变量的分子和分母的符号确定，如果采用正切表达式，就不难确定角所在的象限。 y x y ＋ x 正切函数所在象限 θ x y ＋ x y ＋ x y ＋ atan2(x,y) 可在[-? ,?)内唯一确定出角度。 2）当sinθ接近于0时，求出的角度φ和Ψ是不精确的。 反正切函数比反余旋函数在接近0°时的精度高： 《计算方法》：已知x和函数y=f(x),求y时，计算误差： 接近－?，值域(－?,－1)， 值域过大，精度不稳定 接近1，值域(0,－1]，值域很小，精度稳定 当?接近0°时,x接近1 当?接近0°时,x接近0 注意： 在求解关节变量过程中如出现反正切函数的分子和分母太小，则计算结果误差会很大，应重新选择矩阵元素建立新的方程组再进行计算，直到获得满意的结果为止。 同样，如果计算结果超出了机械手关节的运动范围，也要重新计算，直到符合机械手关节的运动范围。 由于机械手各关节变量的相互耦合，后面计算的关节变量与前面的关节变量有关。当前面关节变量的计算结果发生变化时，后面关节变量计算的结果也会发生变化，所以运动方程的逆解不是唯一的。 应该根据机械手的组合形态和各关节的运动范围，经过多次反复计算，从中选择一组合理解。 由此可见，求解机械手的逆运动方程是一个十分复杂的过程。 对着Z1正向看，O0、O4、O2为逆时针排列，?1就是取了正号； O0、O4、O2为顺时针排列，?1就是取了负号。 对着Z3正向看，O1、O4、O3为逆时针排列，?3就是取了正号； O2、O4、O3为顺时针排列，?3就是取了负号。 ?3取正负号的原则： 保证?3的绝对值较小。 ?1取正负号的原则： 保证?1的绝对值较小。 多值问题 a3 ① ② ③ ④ ⑤ ⑥ Ⅰ Ⅱ Ⅲ Ⅳ Ⅴ Ⅵ z1 z2 z5 z4 z6 o2 o5 o6 o1 o3 o4 x4 x5 x6 z3 x3 x2 x1 z0 x0 o0 d2 a2 d4 对着Z1正向看，O0、O4、O2为逆时针排列，?1就是取了正号； O0、O4、O2为顺时针排列，?1就是取了负号。 a3 ① ② ③ ④ ⑤ ⑥ Ⅰ Ⅱ Ⅲ Ⅳ Ⅴ Ⅵ z1 z2 z5 z4 z6 o2 o5 o6 o1 o3 o4 x4 x5 x6 z3 x3 x2 x1 z0 x0 o0 d2 a2 d4 对着Z3正向看，O1、O4、O3为逆时针排列，?3就是取了正号； O2、O4、O3为顺时针排列，?3就是取了负号。 例4：画出图示肘机械手的D-H坐标系，确定D-H参数，并写出Ai矩阵。 解：1) D-H坐标系的建立 2) 确定各连杆的D-H参数和关节变量 z1 z2 z3 z4 z5、z6 x a3 a4 a2 z0 x0 y0 x1 肘机械手的D-H参数表 ?Link Variable α a d cosα sinα 1