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 '20 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 '20 at 2:24
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$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

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