미지수가 6개인 연립 방정식 풀이 오류
7 views (last 30 days)
Show older comments
function [Homogeneous_Matrix] = FWK(a,d,alpha,theta)
A_R_z = [cos(theta) -sin(theta) 0 0;
sin(theta) cos(theta) 0 0;
0 0 1 0;
0 0 0 1];
A_T_z = [1 0 0 0;
0 1 0 0;
0 0 1 d;
0 0 0 1];
A_T_x = [1 0 0 a;
0 1 0 0;
0 0 1 0;
0 0 0 1];
A_R_x = [1 0 0 0;
0 cos(alpha) -sin(alpha) 0;
0 sin(alpha) cos(alpha) 0;
0 0 0 1];
Homogeneous_Matrix = A_T_z*A_R_z*A_T_x*A_R_x;
end
clear; clc; close all;
% [-1.41582,-1.11201,1.12301,-1.56936,-1.56959,0.011107,-0.236656,0.64918,0.371584,3.12072,0.229297,0.00190622]
%%
syms d1 a2 a3 d4 d5 d6 theta1 theta2 theta3 theta4 theta5 theta6
% FYI : FWK(a,d,alpha,theta)
T01 = FWK(0, d1, pi/2, theta1);
T12 = FWK(a2, 0, 0, theta2);
T23 = FWK(a3, 0, 0, theta3);
T34 = FWK(0, d4, pi/2, theta4);
T45 = FWK(0, d5, -pi/2, theta5);
T56 = FWK(0, d6, 0, theta6);
H = T01*T12*T23*T34*T45*T56;
% Theta 1
P_05 = H * [0 0 -d6 0]';
psi = atan2(P_05(1,1),P_05(2,1));
phi = acos(d4/sqrt(P_05(2,1)^2+P_05(1,1)^2));
theta1 = pi/2 + psi +phi;
theta1_1 = pi/2 + psi +phi;
theta1_2 = pi/2 + psi -phi;
% Theta 5
T_10 = inv(T01);
T_16 = T_10 * H;
theta5 = acos((T_16(3,4)-d4)/d6);
theta5_1 = acos((T_16(3,4)-d4)/d6);
theta5_2 = -acos((T_16(3,4)-d4)/d6);
% Theta 6
theta6 = atan2((-T_16(2,3)/sin(theta5_1)),(T_16(1,3)/sin(theta5_1)));
% Theta 3
T_14 = (T_10* H)*inv(T45*T56);
P_13 = T_14* [0 -d4 0 0]';
theta3 = acos((norm(P_13)^2-a2^2-a3^2)/(2*a2*a3));
% Theta 2
T_65 = inv(T56);
T_54 = inv(T45);
T_14 = (T_10 * H) * T_65 * T_54;
P_13 = T_14 * [0 -d4 0 0]';
theta2 = -atan2(P_13(2), -P_13(1)) + asin((a3*sin(theta3))/norm(P_13));
% Theta 4
T_32 = inv(T23);
T_21 = inv(T12);
T_34 = T_32*T_21*T_14;
theta4 = atan2(T_34(2,1), T_34(1,1));
Encoder = [-1.41582,-1.11201,1.12301,-1.56936,-1.56959,0.011107] % Radian
eqn1 = theta1 == Encoder(1);
eqn2 = theta2 == Encoder(2);
eqn3 = theta3 == Encoder(3);
eqn4 = theta4 == Encoder(4);
eqn5 = theta5 == Encoder(5);
eqn6 = theta6 == Encoder(6);
[sol1,sol2,sol3,sol4,sol5,sol6 ] = solve([eqn1,eqn2,eqn3,eqn4,eqn5,eqn6],[d1 a2 a3 d4 d5 d6])
위 코드로 미지수 6개인 연립방정식을 풀려고 하는데, Unable to find explicit solution 에러가 계속 발생합니다.
에러에 대해서 검색해봐도 도움이 될 자료를 찾지 못했는데 해결 방법 아시는 분 도와주시면 정말 감사드리겠습니다.
0 Comments
Answers (0)
See Also
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!