正解与逆解.docxVIP

Motoman-HP20机器人是由首钢莫托曼公司生产的一款工业机器人，最大负载20kg，在工作载荷内重复定位±0.06mm，主要用于搬运和焊接等。对机器人的逆运动学求解，根据D-H方法建立的坐标系如图所示，D-H参数表见下表1.表1 D-H参数表θdaα1θ1(0)0a1-902θ2(-90)0a21803θ3(0)0a3-904θ4(0)d40905θ5(90)00906θ6(180)d60180并得到各连杆的变换矩阵：A1=A3=A5=末端执行器相对于机器人参考坐标系的总变换矩阵：T=0T6=A1A2A3A4A5A6.令：T=nxnynz0oxo其中：nx=C1C23(C4C5C6+S4S6)-S1(S4C5C6-C4S6)+C1S23S5C6ny=S1C23(C4C5C6+S4S6)+C1(S4C5C6-C4S6)+S1S23S5C6nz=-S23(C4C5C6+S4S6)+C23S5C6ox=C1C23(C4C5S6-S4C6)-S1(S4C5S6+C4C6)+C1S23S5S6oy=S1C23(C4C5S6-S4C6)+C1(S4C5S6+C4C6)+S1S23S5S6oz=-S23(C4C5S6-S4C6)+C23S5S6ax=-C1C23C4S5+S1S4S5+C1S23C5ay=-S1C23C4S5-C1S4S5+S1S23C5az=S23C4S5+C23C5px=C1C23C4S5d6-S1S4S5d6+C1S23[-C5d6+d4]+a3C1C23+a2C1C2+a1C1py= C1C23C4S5d6-S1S4S5d6+C1S23[-C5d6+d4]+a3C1C23+a2C1C2+a1C1pz=-S23C4S5d6+C23[-C5d6+d4]-a3S23-a2S2式中S1表示sin(θ1)，C1表示cos(θ1)，S23表示sin(θ2-θ3)，C23表示cos(θ2-θ3)。求解θ1将式T=A1A2A3A4A5A6左乘A1-1，右乘A6-1，有：A2A3A4A5=A1-1TA6-1AA = =取第4列元素相等有：d4S23+a3C23+a2C2-d4C23+a3tan(θ1)=(d6ay+py)/(d6ax+px)θ1=Atan2(d6ay+py,d6az+px)求解θ3A3A4A5= A2-1A1-1TA6-1AA = = 有：C2E14+S2E24-a2=-d4S3+a3C3S2E14-C2E24=d4C3+a3S3将上两式平方后相加得：E142+E242=a22+a32+d42+2a2(a3C3-d4S3)即：（E142+E242 -a22+a32+d42）/2a2= a3C3-d4S3令：Q=（E142+E242 -a22+a32+d42）/2a2利用三角代换：a3=Rsinγ ； d4=Rcosγ，其中R=a32+d42 ；γ=Atan2(aQ= RsinγC3- RcosγS3=Rsin(γ-θ3)sin(γ-θ3)=Q/R , cos(γ-θ3)=?γ-θ3=Atan2(Q/R,?1-(θ3=γ- Atan2(Q/R,?1-(QR)2)= Atan2(a3,d求解θ2C2E14+S2E24-a2=-d4S3+a3C3=Q 同理，利用三角代换，:σ=Atan2(E14/E24)θ2=σ-Atan2(V, ?U2-V2)= Atan2(V，?U2-V各关节的姿态变化矩阵：R1=C1R4=C40R6=R1R2R3R4R5R6=KR3-1R2-1R1-1K=R4R5R6其中：K=RR等式两端元素（1，3），（2，3）相除，得到：S4S5/C4S5=(-axS1+ayC1)/[C23(axC1+ayS1)-azS23]当S5=0时，关节4和关节6在同一直线上，机器人退化为5自由度，θ4可任选，通常取当前值，即θ4=0当S5≠0时，tanθ4=(-axS1+ayC1)/[C23(axC1+ayS1)-azS23]θ4=Atan2(-axS1+ayC1, C23(axC1+ayS1)-azS23)求解θ6元素（3，1），（3，2）相除，得到：S5S6/S5C6= [S23(oxC1+oyS1)+ozC23]/[ S23(nxC1+nyS1)+nzC23]tanθ6= [S23(oxC1+oyS1)+ozC23]/[ S23(nxC1+nyS1)+nzC23]θ6=Atan2(S23(oxC1+oyS1)+ozC23, S23(nxC1+nyS1)+nzC23)求解θ5由元素（3，2）得：S5S6= S23(oxC1+oyS1)+ozC23由元素（3，3）得：C5= S23(axC1+ayS1)+azC23得到：tanθ5= [S