matlab - 机器人 MATLAB 的正向和反向运动学

Question

希望你一切顺利。

我正在通过反向运动学验证我的正向运动学的输出，但结果并不理想。由于我的反向运动学的输出与正向运动学的输入不同。

机械手的DH参数如下：

链接：alpha、a、theta、d

链接 1：-90 0 theta1* d1

链接 2：0 a2 theta2* 0

链接 3：0 a3 theta3* 0

使用的函数有：

逆运动学

function q = inv_kinematics(ph)
%input in radians: [ph1 ph2 ph3] = [0.1 0.2 0.4]
l1 = 0.05;
l2 = 0.28;
l3 = 0.2;
d1 = 0.03;
ph1 = ph(1);
ph2 = ph(2);
ph3 = ph(3);
r=sqrt(ph1^2+(ph3-d1)^2);
alpha=acos((r^2+l2^2-l3^2)/(2*r*l2))
q = zeros(3,1);
q1=atan2(ph2,ph1);
q2=atan2(ph1,ph3-d1)-alpha;
q3=atan2(ph1-l2*sin(q2),ph3-d1-l2*cos(q2))-q2;
q=[q1;q2;q3];
%output: q = [1.107 -0.265 1.314]
end

正向运动学：

function ph  = forward_kinematics(q)
%input: [q1 q2 q3] = [1.107 -0.265 1.314]
l2 = 0.28;
l3 = 0.2;
d1 = 0.03;
q1 = q(1);
q2 = q(2);
q3 = q(3);
ph = zeros(3,1);
ph1 = l2*cos(q1)*cos(q2)+l3*cos(q1)*cos(q2+q3);
ph2 = l2*sin(q1)*cos(q2)+l3*sin(q1)*cos(q2+q3);
ph3 = d1-l2*sin(q2)-l3*sin(q2+q3);
ph=[ph1;ph2;ph3];
%output: p = [0.1655 0.3308 -0.07005] 
end