matlab练习程序（旋转矩阵、欧拉角、四元数互转）

发奋图强

欧拉角转旋转矩阵公式：

旋转矩阵转欧拉角公式：

旋转矩阵转四元数公式，其中1+r11+r22+r33>0：

四元数转旋转矩阵公式，q0^2+q1^2+q2^2+q3^2=1：

欧拉角转四元数公式：

四元数转欧拉角公式：


matlab代码如下：

1. clear all;
   
2. close all;
   
3. clc;
   
4. 
   
5. %欧拉角
   
6. x = 0.5;
   
7. y = 0.6;
   
8. z = 0.7;
   
9. Ang1 = [x y z];
   
10. 
    
11. %欧拉角转旋转矩阵
    
12. Rx = [1      0      0;
    
13.     0 cos(x) -sin(x);
    
14.     0 sin(x) cos(x)];
    
15. Ry = [cos(y)  0 sin(y);
    
16.     0       1      0;
    
17.     -sin(y) 0 cos(y)];
    
18. Rz = [cos(z) -sin(z) 0;
    
19.     sin(z) cos(z)  0;
    
20.     0      0       1];
    
21. R = Rz*Ry*Rx;
    
22. R1 = R;
    
23. 
    
24. %旋转矩阵转欧拉角
    
25. x = atan2(R(3,2),R(3,3));
    
26. y = atan2(-R(3,1),sqrt(R(3,2)^2+R(3,3)^2));
    
27. z = atan2(R(2,1),R(1,1));
    
28. Ang2 = [x y z];
    
29. 
    
30. %旋转矩阵转四元数
    
31. t=sqrt(1+R(1,1)+R(2,2)+R(3,3))/2;
    
32. q=[t (R(3,2)-R(2,3))/(4*t) (R(1,3)-R(3,1))/(4*t) (R(2,1)-R(1,2))/(4*t)];
    
33. Q1 = q;
    
34. 
    
35. %四元数转旋转矩阵
    
36. R=[ 2*q(1).^2-1+2*q(2)^2    2*(q(2)*q(3)-q(1)*q(4)) 2*(q(2)*q(4)+q(1)*q(3));
    
37.     2*(q(2)*q(3)+q(1)*q(4)) 2*q(1)^2-1+2*q(3)^2     2*(q(3)*q(4)-q(1)*q(2));
    
38.     2*(q(2)*q(4)-q(1)*q(3)) 2*(q(3)*q(4)+q(1)*q(2)) 2*q(1)^2-1+2*q(4)^2];
    
39. R2 = R;
    
40. 
    
41. %欧拉角转四元数
    
42. q = [cos(x/2)*cos(y/2)*cos(z/2) + sin(x/2)*sin(y/2)*sin(z/2) ...
    
43.     sin(x/2)*cos(y/2)*cos(z/2) - cos(x/2)*sin(y/2)*sin(z/2) ...
    
44.     cos(x/2)*sin(y/2)*cos(z/2) + sin(x/2)*cos(y/2)*sin(z/2) ...
    
45.     cos(x/2)*cos(y/2)*sin(z/2) - sin(x/2)*sin(y/2)*cos(z/2)];
    
46. Q2 = q;
    
47. 
    
48. %四元数转欧拉角
    
49. x = atan2(2*(q(1)*q(2)+q(3)*q(4)),1 - 2*(q(2)^2+q(3)^2));
    
50. y = asin(2*(q(1)*q(3) - q(2)*q(4)));
    
51. z = atan2(2*(q(1)*q(4)+q(2)*q(3)),1 - 2*(q(3)^2+q(4)^2));
    
52. Ang3 = [x y z];
    
53. 
    
54. Ang1
    
55. Ang2
    
56. Ang3
    
57. 
    
58. R1
    
59. R2
    
60. 
    
61. Q1
    
62. Q2

复制代码

clearlove

谢谢分享