Simulink model of the same is attached here.
Forward and Inverse Kinematics for robot
조회 수: 238 (최근 30일)
이전 댓글 표시
Hello,
Hope you are doing well.
I am verifying the output of my forward kinematics through inverse kinematics and the results are not as desired. As the output of my inverse kinematics is not coming out to be the same as the input of forward kinematics.
The D-H parameters of manipulator is given as:
alpha a theta d
Link 1 -90 0 theta1* d1
Link 2 0 a2 theta2* 0
Link 3 0 a3 theta3* 0
Functions used are:
Forward kinematics:
function ph = forward_kinematics(q)
%input: [q1 q2 q3]
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];
end
Inverse kinematics
function q = inv_kinematics(ph)
%input: [ph1 ph2 ph3]
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];
end
댓글 수: 4
답변 (3개)
Mohd Musharaf Hussain Sarwari
2021년 2월 13일
I WANT SCARA ROBOT forward-and-inverse-kinematics MATLAB CODE
댓글 수: 2
Krishna Akella
2019년 6월 28일
Hi Mohsina,
I don't know the answer to your question but I looked at your model and I have a few observations. In your model I assume the inputs to your forward kinematics function are the joint angle values and the output is the end effector location. And I assume the inputs to your inverse kinematics function is the end effector location and the outputs are the joint values.
If this is correct, then why would you not pass the output from forward kinematics back to your inverse kinematics function to validate that you are getting back the same joint values?
I made this change and it seems like the first output value matches.
The reason the other joint angles might still not be matching is because you could have multiple solutions to worry about. For example, in the computation of your inverse kinematics function, you have
r = sqrt(ph1^2+(ph3-d1)^2);
There could be two solutions to the sqrt function. A positive and a negative value. And MATLAB returns the positive value. Similar thing is true for other functions like acos, where multiple angles can give the same result. All these will result in multiple solutions to result in the same end effector location.
Regards,
Krishna
Ram Bodhe
2020년 5월 12일
function q = inv_kinematics(ph)
%input: [ph1 ph2 ph3]
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];
end
참고 항목
카테고리
Help Center 및 File Exchange에서 Robotics에 대해 자세히 알아보기
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!