How to Solve a System of Linear Equations?
3 次查看(过去 30 天)
显示 更早的评论
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.
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/497887/image.png)
Therefore we can write the transformation of p as:
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/497892/image.png)
- 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 个评论
回答(0 个)
另请参阅
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!