本文主要是介绍六轴机器人matlab写运动学逆解函数(改进DH模型),希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!
1.理论
本文采用的模型为之前博客“matlab机器人工具箱一般六轴的DH模型和改进DH模型建立与区别”里面的改进DH模型,参数不再重复给出。
基系与工具坐标系关系为:
bT0⋅(0T1⋅1T2⋅2T3⋅3T4⋅4T5⋅5T6)⋅6Te=bTe b T 0 ⋅ ( 0 T 1 ⋅ 1 T 2 ⋅ 2 T 3 ⋅ 3 T 4 ⋅ 4 T 5 ⋅ 5 T 6 ) ⋅ 6 T e = b T e
将逆运动学问题简化为:
0T1⋅1T2⋅2T3⋅3T4⋅4T5⋅5T6=bT−10⋅bTe⋅6T−1e 0 T 1 ⋅ 1 T 2 ⋅ 2 T 3 ⋅ 3 T 4 ⋅ 4 T 5 ⋅ 5 T 6 = b T 0 − 1 ⋅ b T e ⋅ 6 T e − 1
2.转换为下式求解
2T3⋅3T4⋅4T5=1T−12⋅0T−11⋅bT−10⋅bTe⋅6T−1e⋅5T−16 2 T 3 ⋅ 3 T 4 ⋅ 4 T 5 = 1 T 2 − 1 ⋅ 0 T 1 − 1 ⋅ b T 0 − 1 ⋅ b T e ⋅ 6 T e − 1 ⋅ 5 T 6 − 1
左边:
left =
[ cos(theta3)*cos(theta4)*cos(theta5) - sin(theta3)*sin(theta5), - cos(theta5)*sin(theta3) - cos(theta3)*cos(theta4)*sin(theta5), cos(theta3)*sin(theta4), a2 + a3*cos(theta3) - d4*sin(theta3)]
[ cos(theta3)*sin(theta5) + cos(theta4)*cos(theta5)*sin(theta3), cos(theta3)*cos(theta5) - cos(theta4)*sin(theta3)*sin(theta5), sin(theta3)*sin(theta4), d4*cos(theta3) + a3*sin(theta3)]
[ -cos(theta5)*sin(theta4), sin(theta4)*sin(theta5), cos(theta4), 0]
[ 0, 0, 0, 1]
右边:
right =
[ oz*sin(theta2)*sin(theta6) - nz*cos(theta6)*sin(theta2) + nx*cos(theta1)*cos(theta2)*cos(theta6) + ny*cos(theta2)*cos(theta6)*sin(theta1) - ox*cos(theta1)*cos(theta2)*sin(theta6) - oy*cos(theta2)*sin(theta1)*sin(theta6), ax*cos(theta1)*cos(theta2) - az*sin(theta2) + ay*cos(theta2)*sin(theta1), oz*cos(theta6)*sin(theta2) + nz*sin(theta2)*sin(theta6) - ox*cos(theta1)*cos(theta2)*cos(theta6) - nx*cos(theta1)*cos(theta2)*sin(theta6) - oy*cos(theta2)*cos(theta6)*sin(theta1) - ny*cos(theta2)*sin(theta1)*sin(theta6), px*cos(theta1)*cos(theta2) - pz*sin(theta2) - a1*cos(theta2) + py*cos(theta2)*sin(theta1)]
[ oz*cos(theta2)*sin(theta6) - nz*cos(theta2)*cos(theta6) - nx*cos(theta1)*cos(theta6)*sin(theta2) - ny*cos(theta6)*sin(theta1)*sin(theta2) + ox*cos(theta1)*sin(theta2)*sin(theta6) + oy*sin(theta1)*sin(theta2)*sin(theta6), - az*cos(theta2) - ax*cos(theta1)*sin(theta2) - ay*sin(theta1)*sin(theta2), oz*cos(theta2)*cos(theta6) + nz*cos(theta2)*sin(theta6) + ox*cos(theta1)*cos(theta6)*sin(theta2) + nx*cos(theta1)*sin(theta2)*sin(theta6) + oy*cos(theta6)*sin(theta1)*sin(theta2) + ny*sin(theta1)*sin(theta2)*sin(theta6), a1*sin(theta2) - pz*cos(theta2) - px*cos(theta1)*sin(theta2) - py*sin(theta1)*sin(theta2)]
[ ny*cos(theta1)*cos(theta6) - nx*cos(theta6)*sin(theta1) - oy*cos(theta1)*sin(theta6) + ox*sin(theta1)*sin(theta6), ay*cos(theta1) - ax*sin(theta1), ox*cos(theta6)*sin(theta1) - ny*cos(theta1)*sin(theta6) - oy*cos(theta1)*cos(theta6) + nx*sin(theta1)*sin(theta6), py*cos(theta1) - px*sin(theta1)]
[ 0, 0, 0, 1]
令左右两边相等,求出六个角。、
逆解顺序如下:
由于公式比较复杂我仅在此给出想法,各位可自行计算。
θ1 θ 1 :两式第3行第4列。
θ3 θ 3 :两式第1行第4列和第2行第4列。
θ2 θ 2 :两式第1行第4列和第2行第4列。
θ5 θ 5 :两式第1行第2列和第2行第2列。
θ4 θ 4 :两式第1行第2列和第2行第2列。
θ6 θ 6 :两式第3行第1列和第3行第3列。
3.MATLAB程序
分两个程序①主函数②function函数
①主函数
clear;
clc;
%建立机器人模型
% theta d a alpha offset
ML1=Link([0 0 0 0 0 ],'modified');
ML2=Link([0 0 0.180 -pi/2 0 ],'modified');
ML3=Link([0 0 0.600 0 0 ],'modified');
ML4=Link([0 0.630 0.130 -pi/2 0 ],'modified');
ML5=Link([0 0 0 pi/2 0 ],'modified');
ML6=Link([0 0 0 -pi/2 0 ],'modified');
modrobot=SerialLink([ML1 ML2 ML3 ML4 ML5 ML6],'name','modified');
modmyt06=mymodfkine(-pi/3,-pi/3,pi/3,-pi/4,pi/4,pi/6);
modmyikine=mymodikine(modmyt06)
②function函数
function [ikine_t]=mymodikine(Tbe)
% ti di ai-1 alphai-1
MDH=[0 0 0 0;0 0 0.180 -pi/2;0 0 0.600 0;0 0.630 0.130 -pi/2;0 0 0 pi/2;0 0 0 -pi/2];nx=Tbe(1,1); ny=Tbe(2,1); nz=Tbe(3,1); ox=Tbe(1,2); oy=Tbe(2,2); oz=Tbe(3,2); ax=Tbe(1,3); ay=Tbe(2,3); az=Tbe(3,3); px=Tbe(1,4); py=Tbe(2,4); pz=Tbe(3,4);d4=MDH(4,2);a1=MDH(2,3);a2=MDH(3,3);a3=MDH(4,3);d2=0;d3=0;f1=-pi/2;f3=-pi/2;f4=pi/2;f5=-pi/2;%t1(3,4)t11=-atan2(-py,px)+atan2((d2-d3)/sin(f1),((px*sin(f1))^2+(py*sin(f1))^2-(d2-d3)^2)^0.5);t12=-atan2(-py,px)+atan2((d2-d3)/sin(f1),-((px*sin(f1))^2+(py*sin(f1))^2-(d2-d3)^2)^0.5);%t3m3_1=pz*sin(f1);n3_1=a1-px*cos(t11)-py*sin(t11);m3_2=pz*sin(f1);n3_2=a1-px*cos(t12)-py*sin(t12);t31=-atan2(a2*a3/sin(f3),a2*d4)+atan2((m3_1^2+n3_1^2-a2^2-a3^2-d4^2)/sin(f3),((2*a2*d4*sin(f3))^2+(2*a2*a3)^2-(m3_1^2+n3_1^2-a2^2-a3^2-d4^2)^2)^0.5);t32=-atan2(a2*a3/sin(f3),a2*d4)+atan2((m3_1^2+n3_1^2-a2^2-a3^2-d4^2)/sin(f3),-((2*a2*d4*sin(f3))^2+(2*a2*a3)^2-(m3_1^2+n3_1^2-a2^2-a3^2-d4^2)^2)^0.5);t33=-atan2(a2*a3/sin(f3),a2*d4)+atan2((m3_2^2+n3_2^2-a2^2-a3^2-d4^2)/sin(f3),((2*a2*d4*sin(f3))^2+(2*a2*a3)^2-(m3_2^2+n3_2^2-a2^2-a3^2-d4^2)^2)^0.5);t34=-atan2(a2*a3/sin(f3),a2*d4)+atan2((m3_2^2+n3_2^2-a2^2-a3^2-d4^2)/sin(f3),-((2*a2*d4*sin(f3))^2+(2*a2*a3)^2-(m3_2^2+n3_2^2-a2^2-a3^2-d4^2)^2)^0.5);%t2m2_1=a2+a3*cos(t31)+d4*sin(f3)*sin(t31);n2_1=a3*sin(t31)-d4*sin(f3)*cos(t31);m2_2=a2+a3*cos(t32)+d4*sin(f3)*sin(t32);n2_2=a3*sin(t32)-d4*sin(f3)*cos(t32);m2_3=a2+a3*cos(t33)+d4*sin(f3)*sin(t33);n2_3=a3*sin(t33)-d4*sin(f3)*cos(t33);m2_4=a2+a3*cos(t34)+d4*sin(f3)*sin(t34);n2_4=a3*sin(t34)-d4*sin(f3)*cos(t34);t21=atan2(m3_1*m2_1+n2_1*n3_1,m3_1*n2_1-m2_1*n3_1);t22=atan2(m3_1*m2_2+n2_2*n3_1,m3_1*n2_2-m2_2*n3_1);t23=atan2(m3_2*m2_3+n2_3*n3_2,m3_2*n2_3-m2_3*n3_2);t24=atan2(m3_2*m2_4+n2_4*n3_2,m3_2*n2_4-m2_4*n3_2);%t5m5_1=-sin(f5)*(ax*cos(t11)*cos(t21)+ay*sin(t11)*cos(t21)+az*sin(f1)*sin(t21));n5_1=sin(f5)*(ax*cos(t11)*sin(t21)+ay*sin(t11)*sin(t21)-az*sin(f1)*cos(t21));m5_2=-sin(f5)*(ax*cos(t11)*cos(t22)+ay*sin(t11)*cos(t22)+az*sin(f1)*sin(t22));n5_2=sin(f5)*(ax*cos(t11)*sin(t22)+ay*sin(t11)*sin(t22)-az*sin(f1)*cos(t22));m5_3=-sin(f5)*(ax*cos(t12)*cos(t23)+ay*sin(t12)*cos(t23)+az*sin(f1)*sin(t23));n5_3=sin(f5)*(ax*cos(t12)*sin(t23)+ay*sin(t12)*sin(t23)-az*sin(f1)*cos(t23));m5_4=-sin(f5)*(ax*cos(t12)*cos(t24)+ay*sin(t12)*cos(t24)+az*sin(f1)*sin(t24));n5_4=sin(f5)*(ax*cos(t12)*sin(t24)+ay*sin(t12)*sin(t24)-az*sin(f1)*cos(t24));t51=atan2(((ay*cos(t11)-ax*sin(t11))^2+(m5_1*cos(t31)+n5_1*sin(t31))^2)^0.5,(m5_1*sin(t31)-n5_1*cos(t31))/(sin(f3)*sin(f4)));t52=atan2(-((ay*cos(t11)-ax*sin(t11))^2+(m5_1*cos(t31)+n5_1*sin(t31))^2)^0.5,(m5_1*sin(t31)-n5_1*cos(t31))/(sin(f3)*sin(f4)));t53=atan2(((ay*cos(t11)-ax*sin(t11))^2+(m5_2*cos(t32)+n5_2*sin(t32))^2)^0.5,(m5_2*sin(t32)-n5_2*cos(t32))/(sin(f3)*sin(f4)));t54=atan2(-((ay*cos(t11)-ax*sin(t11))^2+(m5_2*cos(t32)+n5_2*sin(t32))^2)^0.5,(m5_2*sin(t32)-n5_2*cos(t32))/(sin(f3)*sin(f4)));t55=atan2(((ay*cos(t12)-ax*sin(t12))^2+(m5_3*cos(t33)+n5_3*sin(t33))^2)^0.5,(m5_3*sin(t33)-n5_3*cos(t33))/(sin(f3)*sin(f4)));t56=atan2(-((ay*cos(t12)-ax*sin(t12))^2+(m5_3*cos(t33)+n5_3*sin(t33))^2)^0.5,(m5_3*sin(t33)-n5_3*cos(t33))/(sin(f3)*sin(f4)));t57=atan2(((ay*cos(t12)-ax*sin(t12))^2+(m5_4*cos(t34)+n5_4*sin(t34))^2)^0.5,(m5_4*sin(t34)-n5_4*cos(t34))/(sin(f3)*sin(f4)));t58=atan2(-((ay*cos(t12)-ax*sin(t12))^2+(m5_4*cos(t34)+n5_4*sin(t34))^2)^0.5,(m5_4*sin(t34)-n5_4*cos(t34))/(sin(f3)*sin(f4)));%t4if sin(t51)==0t41=0;else t41=atan2(((ay*cos(t11)-ax*sin(t11))*sin(f1)*sin(f5))/(-sin(t51)*sin(f3)),(-m5_1*cos(t31)-n5_1*sin(t31))/(sin(t51)));endif sin(t52)==0 t42=0;else t42=atan2(((ay*cos(t11)-ax*sin(t11))*sin(f1)*sin(f5))/(-sin(t52)*sin(f3)),(-m5_1*cos(t31)-n5_1*sin(t31))/(sin(t52)));endif sin(t53)==0 t43=0;else t43=atan2(((ay*cos(t11)-ax*sin(t11))*sin(f1)*sin(f5))/(-sin(t53)*sin(f3)),(-m5_2*cos(t32)-n5_2*sin(t32))/(sin(t53)));endif sin(t54)==0 t44=0;else t44=atan2(((ay*cos(t11)-ax*sin(t11))*sin(f1)*sin(f5))/(-sin(t54)*sin(f3)),(-m5_2*cos(t32)-n5_2*sin(t32))/(sin(t54)));endif sin(t55)==0t45=0;else t45=atan2(((ay*cos(t12)-ax*sin(t12))*sin(f1)*sin(f5))/(-sin(t55)*sin(f3)),(-m5_3*cos(t33)-n5_3*sin(t33))/(sin(t55)));endif sin(t56)==0 t46=0;else t46=atan2(((ay*cos(t12)-ax*sin(t12))*sin(f1)*sin(f5))/(-sin(t56)*sin(f3)),(-m5_3*cos(t33)-n5_3*sin(t33))/(sin(t56)));endif sin(t57)==0 t47=0;else t47=atan2(((ay*cos(t12)-ax*sin(t12))*sin(f1)*sin(f5))/(-sin(t57)*sin(f3)),(-m5_4*cos(t34)-n5_4*sin(t34))/(sin(t57)));endif sin(t58)==0 t48=0;else t48=atan2(((ay*cos(t12)-ax*sin(t12))*sin(f1)*sin(f5))/(-sin(t58)*sin(f3)),(-m5_4*cos(t34)-n5_4*sin(t34))/(sin(t58)));end%t6e1=nx*sin(t11)-ny*cos(t11);f1=ox*sin(t11)-oy*cos(t11);t61=atan2((cos(t41)*e1-cos(t51)*sin(t41)*f1),(cos(t41)*f1+cos(t51)*sin(t41)*e1));t62=atan2((cos(t42)*e1-cos(t52)*sin(t42)*f1),(cos(t42)*f1+cos(t52)*sin(t42)*e1));t63=atan2((cos(t43)*e1-cos(t53)*sin(t43)*f1),(cos(t43)*f1+cos(t53)*sin(t43)*e1));t64=atan2((cos(t44)*e1-cos(t54)*sin(t44)*f1),(cos(t44)*f1+cos(t54)*sin(t44)*e1));e2=nx*sin(t12)-ny*cos(t12);f2=ox*sin(t12)-oy*cos(t12);t65=atan2((cos(t45)*e2-cos(t55)*sin(t45)*f2),(cos(t45)*f2+cos(t55)*sin(t45)*e2));t66=atan2((cos(t46)*e2-cos(t56)*sin(t46)*f2),(cos(t46)*f2+cos(t56)*sin(t46)*e2));t67=atan2((cos(t47)*e2-cos(t57)*sin(t47)*f2),(cos(t47)*f2+cos(t57)*sin(t47)*e2));t68=atan2((cos(t48)*e2-cos(t58)*sin(t48)*f2),(cos(t48)*f2+cos(t58)*sin(t48)*e2));ikine_t=[t11 t21 t31 t41 t51 t61;t11 t21 t31 t42 t52 t62;t11 t22 t32 t43 t53 t63;t11 t22 t32 t44 t54 t64t12 t23 t33 t45 t55 t65;t12 t23 t33 t46 t56 t66;t12 t24 t34 t47 t57 t67;t12 t24 t34 t48 t58 t68];
4.运行结果
给定关节角为:
(-pi/3,-pi/3,pi/3,-pi/4,pi/4,pi/6)
正解得到末端位姿:
modmyt06
再用 modmyt06 m o d m y t 06 去进行逆解,结果如下:
共有8组解,正解回去位姿符合。
PS:逆解推荐用 arctan a r c t a n 反解角度,本程序适用于改进DH模型,仅用于学习,不适用于工业应用。
这篇关于六轴机器人matlab写运动学逆解函数(改进DH模型)的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!