How to Solve a System of Linear Equations?

1 次查看（过去 30 天）
ercan duzgun
ercan duzgun 2021-1-23
Hello dear MATLAB community. I am having difficulty while solving a system of linear equations. My MATLAB code is below. Could you give me any hint/suggestion about what is wrong with my code?
Quick explanation about my programme: Think a 6DOF Stewart Platform. For the moving platform, we can write the inverse kinematic equation as follow: OXYZ is the reference frame of the fixed platform. Oxyz is the reference frame of the moving platform. t is the distance vector between OXYZ and Oxyz (or in another meaning: the moving platform reference frame in fixed frame). p() is the position vectors of the vertices of the moving platform in the moving platform refernce frame Oxyz. R is the rotation matrix of the p in Oxyz. P is the coordinates of the vertices of the moving platform in OXYZ fixed reference frame.
Therefore we can write the transformation of p as:
.
  • Problem a) First I calculate P, for the known input values of .
  • Problem b) At second stage, I try to calculate R rotation matrix, for the known values of . (I use the same numeric values that I used in problem (a))
I can do the problem a. I am having problem at problem b. Could you give me any suggestion to solve R rotation matrix?
My MATLAB code is this:
clear all;clc;
p=[1 2 5;
1 4 10;
0 0 0];
% p consists of three points: p1,p2,p3.
% p=[p1,p2,p3]; p1=[p1x;p1y;p1z];p2=[p2x;p2y;p2z];p3=[p3x;p3y;p3z]
t=[0;0;50];
alpha=10*pi/180;beta=15*pi/180;gamma=20*pi/180;
Rx=[1 0 0;
0 cos(alpha) -sin(alpha)
0 sin(alpha) cos(alpha)];
Ry=[cos(beta) 0 sin(beta)
0 1 0
-sin(beta) 0 cos(beta)];
Rz=[cos(gamma) -sin(gamma) 0
sin(gamma) cos(gamma) 0
0 0 1];
R=Rz*Ry*Rx;
P=R*p+t
% End of Problem (a)
%%
% Solution for Problem (b)
syms r11 r21 r31 r12 r22 r32 r13 r23 r33 real
R=[r11 r12 r13;
r21 r22 r23;
r31 r32 r33];
P1x=P(1,1);P1y=P(2,1);P1z=P(3,1);
P2x=P(1,2);P2y=P(2,2);P2z=P(3,2);
P3x=P(1,3);P3y=P(2,3);P3z=P(3,3);
p1x=p(1,1);p1y=p(2,1);p1z=p(3,1);
p2x=p(1,2);p2y=p(2,2);p2z=p(3,2);
p3x=p(1,3);p3y=p(2,3);p3z=p(3,3);
p=[p1x p2x p3x;
p1y p2y p3y;
p1z p2z p3z];
tx=t(1,1);ty=t(2,1);tz=t(3,1);
eq11=vpa(p1x*r11 + p1y*r12 + p1z*r13 + tx - P(1,1))
eq12=vpa(p2x*r11 + p2y*r12 + p2z*r13 + tx - P(1,2))
eq13=vpa(p3x*r11 + p3y*r12 + p3z*r13 + tx - P(1,3))
eq21=vpa(p1x*r21 + p1y*r22 + p1z*r23 + ty - P(2,1))
eq22=vpa(p2x*r21 + p2y*r22 + p2z*r23 + ty - P(2,2))
eq23=vpa(p3x*r21 + p3y*r22 + p3z*r23 + ty - P(2,3))
eq31=vpa(p1x*r31 + p1y*r32 + p1z*r33 + tz - P(3,1))
eq32=vpa(p2x*r31 + p2y*r32 + p2z*r33 + tz - P(3,2))
eq33=vpa(p3x*r31 + p3y*r32 + p3z*r33 + tz - P(3,3))
sol=vpasolve(eq11,eq12,eq13,eq21,eq22,eq23,eq31,eq32,eq33)
sol.r11
sol.r12
sol.r13
sol.r21
sol.r22
sol.r23
sol.r31
sol.r32
sol.r33
R_cal=[ sol.r11 sol.r12 sol.r13;
sol.r21 sol.r22 sol.r23;
sol.r31 sol.r32 sol.r33]
ypr_in_degrees=R2ypr(R_cal)*180/pi;
function [YPR] = R2ypr(R)
r11=R(1,1);r12=R(1,2);r13=R(1,3);
r21=R(2,1);r22=R(2,2);r23=R(2,3);
r31=R(3,1);r32=R(3,2);r33=R(3,3);
roll_z =atan2(r21,r11);
pitch_y=atan2(-r31,sqrt(r32^2+r33^2));
yaw_x =atan2(r32,r33);
YPR=[yaw_x,pitch_y,roll_z];
YPR=eval(vpa(YPR));
end
I would like comment that: if I choose z axis values of p as non-zero, my codes works. (for example: p=[1 2 5;1 4 10; 5 6 8]).
However, if the z axis values of p is zero 0, the MATLAB can't compute the unknown values of R. And MATLAB gives this error: Reference to non-existent field 'r13'.
And I can't find any element of R matrix.
Could you suggest anything with my this codes, or if you have any idea about how to to find R matrix with its 9 element, your ideas are welcomed.
Thanks in advance.

请先登录，再回答此问题。

回答（0 个）

类别

在 Help Center 和 File Exchange 中查找有关 Multirate Signal Processing 的更多信息

标签

产品


版本

R2019b

Community Treasure Hunt

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

Start Hunting!

Translated by