1
$\begingroup$

Currently working on a quadcopter simulation. I have a desired thrust vector,

t =sin(30)cos(45)a1+sin(30)sin(45)a2+cos(30)a3

and desired yaw angle,

psi = 45. 

Because we are working with an under-actuated system, I am trying to solve for the rotation matrix that aligns the vector:b3 = [0,0,1] (direction of thrust in the body-fixed frame of reference) with the direction of t. Here is my code:

t = [sind(30) * cosd(45), sind(30) * sind(45), cosd(30)];

b3 = [0;0;1];

psi = 45;

syms phi theta; 

eqn = [[cosd(psi)*cosd(theta)-sind(phi)*sind(psi)*sind(theta), - 
cosd(phi)*sind(psi), cosd(psi)*sind(theta)+cosd(theta)*sind(phi)*sind(phi);
cosd(theta)*sind(psi)+cosd(psi)*sind(phi)*sind(theta), cosd(phi)*cosd(psi), 
sind(psi)*sind(theta)-cosd(theta)*sind(phi)*cosd(psi);
 -cosd(phi)*sind(theta), sind(phi), cosd(phi)*cosd(theta)] 
* b3 == t/norm(t)];

S = solve(eqn,[phi theta]);
disp(S.phi)
disp(S.theta)`

The problem is that it outputs an empty symbolic struct, with no warnings or errors otherwise: Output

What I am doing wrong? It is an issue with my theory, my implementation, or both?

$\endgroup$
2
  • $\begingroup$ I believe you have an error in your rotation matrix. Namely the upper right element contains sind(phi)*sind(phi). The rest of your rotation matrix looks like $Z_1\,X_2\,Y_3$ as shown here, so instead of sind(phi)*sind(phi) I believe you should use sind(phi)*sind(psi). $\endgroup$
    – fibonatic
    Mar 18, 2020 at 11:57
  • $\begingroup$ Thank you. I believe this is correct, although was able to find an analytical solution which was much faster. Will post below. $\endgroup$
    – Max Lester
    Mar 20, 2020 at 2:24

2 Answers 2

0
$\begingroup$

Analytic Solution:

t = [sind(30) * cosd(45), sind(30) * sind(45), cosd(30)];
b = [0;0;1];
psi = deg2rad(45);

v = cross(b,t);
s = norm(v);
c = dot(b,t);

phiB = psi;
uB = b;
uBhat = vector2ssMat(uB);

I = eye(3);

rotA = I+vector2ssMat(v)+(vector2ssMat(v)^2)*((1-c)/s^2);
rotB = I*cos(phiB)+uB*transpose(uB)*(1-cos(phiB))+uBhat*sin(phiB);

Rdes = rotA*rotB;
disp(Rdes);
$\endgroup$
0
$\begingroup$

I made the same mistake, actually u need to write the thrust vector t as column vector instead of the row vector. u can simply replace t with t' at all occurrences, it will solve the issue.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.