미지수가 6개인 연립 방정식 풀이 오류

7 views (last 30 days)
Sang-Won Bang
Sang-Won Bang on 19 Aug 2020
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 에러가 계속 발생합니다.
에러에 대해서 검색해봐도 도움이 될 자료를 찾지 못했는데 해결 방법 아시는 분 도와주시면 정말 감사드리겠습니다.

Answers (0)

Categories

Find more on Just for fun in Help Center and File Exchange

Products


Release

R2018a

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!