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

Question

希望你一切都好。

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

机械手的D-H参数为：

链接：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