编程作业

a) 题
b) 题
c) 题
d) 题

a) 题

函数：

1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2 %函数名：ZYX2Rotate
3 %功能：输入欧拉角，计算对应的旋转矩阵
4 %输入：a,b,c 对应 α，β，γ
5 %输出：旋转矩阵
6 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
7 function R_matrix = ZYX2Rotate(a,b,c)
8    a = deg2rad(a);
9    b = deg2rad(b);
10    c = deg2rad(c);
11    R_Z = [cos(a),-sin(a),0;sin(a),cos(a),0;0,0,1];
12    R_Y = [cos(b),0,sin(b);0,1,0;-sin(b),0,cos(b)];
13    R_X = [1,0,0;0,cos(c),-sin(c);0,sin(c),cos(c)];
14    R_matrix = R_Z*R_Y*R_X;
15 end

主程序：

1 clc;
2 clear;
3 a1=10;b1=20;c1=30;  %第一组 α，β，γ
4 R_matrix1 = ZYX2Rotate(a1,b1,c1);  %输出的旋转矩阵
5 disp('==========================================')
6 disp('第一组输入：');
7 disp(['   α = ',num2str(a1),',β = ',num2str(b1),',γ = ',num2str(c1)]);
8 disp('------------------------------------------')
9 disp('输出：');
10 disp(R_matrix1)
11 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
12 a2=30;b2=90;c2=-55;  %第二组 α，β，γ
13 R_matrix2 = ZYX2Rotate(a2,b2,c2);  %输出的旋转矩阵
14 disp('==========================================')
15 disp('第二组输入：');
16 disp(['   α = ',num2str(a2),',β = ',num2str(b2),',γ = ',num2str(c2)]);
17 disp('------------------------------------------')
18 disp('输出：');
19 disp(R_matrix2)
20 disp('==========================================')

输出：

1 ==========================================
2 第一组输入：
3   α = 10,β = 20,γ = 30
4 ------------------------------------------
5 输出：
6   0.9254   0.0180   0.3785
7   0.1632   0.8826   -0.4410
8   -0.3420   0.4698   0.8138
9
10 ==========================================
11 第二组输入：
12   α = 30,β = 90,γ = -55
13 ------------------------------------------
14 输出：
15   0.0000   -0.9962   0.0872
16   0.0000   0.0872   0.9962
17   -1.0000   -0.0000   0.0000
18
19 ==========================================

根据程序输出结果，可以知道

① 证明单位正交旋转矩阵的6个约束条件

记 的列向量为

则计算验证可知6个约束条件

均满足。

 
 ② 证明

已知 ，对其求逆可得

显然有

又因为

所以

b) 题

函数：

1 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2 %函数名：Rotate2ZYX
3 %功能: 输入旋转矩阵，计算对应的欧拉角
4 %输入: 旋转矩阵
5 %输出: a, b, c 对应 α，β，γ 的第一组解
6 %     a1,b1,c1 对应 α，β，γ 的第二组解
7 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
8 function [a,b,c,b1,a1,c1] = Rotate2ZYX(R_matrix)
9    b = rad2deg(atan2(-
R_matrix(3,1),sqrt(R_matrix(1,1)^2+R_matrix(2,1)^2)));
10    if abs(b) == 90
11        a = 0.0;
12        c = b/90*rad2deg(atan2(R_matrix(1,2),R_matrix(2,2)));
13    else
14        a = rad2deg(atan2(R_matrix(2,1)/cos(b),R_matrix(1,1)/cos(b)));
15        c = rad2deg(atan2(R_matrix(3,2)/cos(b),R_matrix(3,3)/cos(b)));
16    end
17    
18    b1 = rad2deg(atan2(-R_matrix(3,1),-
sqrt(R_matrix(1,1)^2+R_matrix(2,1)^2)));
19    if abs(b1) == 90
20        a1 = 0.0;
21        c1 = b1/90*rad2deg(atan2(R_matrix(1,2),R_matrix(2,2)));
22    else
23        a1 = rad2deg(atan2(R_matrix(2,1)/cos(b1),R_matrix(1,1)/cos(b1)));
24        c1 = rad2deg(atan2(R_matrix(3,2)/cos(b1),R_matrix(3,3)/cos(b1)));
25    end
26 end

主程序：

1 clc;
2 clear;
3 a1=10;b1=20;c1=30;  %第一组 α，β，γ
4 R_matrix = ZYX2Rotate(a1,b1,c1);  %输出的旋转矩阵
5 [A1,B1,C1,A2,B2,C2] = Rotate2ZYX(R_matrix);  %得到的两组解
6 disp('==========================================')
7 disp('第一组输入：');
8 disp(['   α = ',num2str(a1),',β = ',num2str(b1),',γ = ',num2str(c1)]);
9 disp('------------------------------------------')
10 disp('输出--旋转矩阵：');
11 disp(R_matrix)
12 disp('输出--反解：');
13 disp(['   第一组解：α = ',num2str(A1),',β = ',num2str(B1),',γ =
',num2str(C1)])
14 disp(['   第二组解：α = ',num2str(A2),',β = ',num2str(B2),',γ =
',num2str(C2)])
15
16 a1=30;b1=90;c1=-55;  %第一组 α，β，γ
17 R_matrix = ZYX2Rotate(a1,b1,c1);  %输出的旋转矩阵
18 [A1,B1,C1,A2,B2,C2] = Rotate2ZYX(R_matrix);  %得到的两组解
19 disp('==========================================')
20 disp('第二组输入：');
21 disp(['   α = ',num2str(a1),',β = ',num2str(b1),',γ = ',num2str(c1)]);
22 disp('------------------------------------------')
23 disp('输出--旋转矩阵：');
24 disp(R_matrix)
25 disp('输出--反解：');
26 disp(['   第一组解：α = ',num2str(A1),',β = ',num2str(B1),',γ =
',num2str(C1)])
27 disp(['   第二组解：α = ',num2str(A2),',β = ',num2str(B2),',γ =
',num2str(C2)])
28 disp('==========================================')

输出：

1 ==========================================
2 第一组输入：
3   α = 10,β = 20,γ = 30
4 ------------------------------------------
5 输出--旋转矩阵：
6   0.9254   0.0180   0.3785
7   0.1632   0.8826   -0.4410
8   -0.3420   0.4698   0.8138
9
10 输出--反解：
11   第一组解：α = 10,β = 20,γ = 30
12   第二组解：α = 160,β = -170,γ = -150
13 ==========================================
14 第二组输入：
15   α = 30,β = 90,γ = -55
16 ------------------------------------------
17 输出--旋转矩阵：
18   0.0000   -0.9962   0.0872
19   0.0000   0.0872   0.9962
20   -1.0000   -0.0000   0.0000
21
22 输出--反解：
23   第一组解：α = 0,β = 90,γ = -85
24   第二组解：α = 90,β = -150,γ = 125
 25 ==========================================

c) 题

主程序:

1 clc;
2 clear;
3 a=0;b=20;c=0;  %α，β，γ
4 R_matrix = ZYX2Rotate(a,b,c);  %输出的旋转矩阵
5 P_B = [1;0;1];
6 P_A = R_matrix*P_B;
7 angle = rad2deg(acos(dot(P_A,P_B)/(norm(P_A)*norm(P_B))));
8 disp('P_A:')
9 disp(P_A);
10 disp('P_B 和 P_A 的夹角：')
11 disp(angle)
12 arrow3([0 0 0],[P_A(1),P_A(2),P_A(3)],'b2',4,5);
13 hold on;
14 arrow3([0 0 0],[P_B(1),P_B(2),P_B(3)],'r2',4,5);
15 grid on;
16 xlim([0 3])
17 ylim([0 1])
18 zlim([0 1.2])

输出：

1 P_A:
2   1.2817
3         0
4   0.5977
5
6 P_B 和 P_A 的夹角：
7   20.0000
d) 题

工具箱的安装与使用：Matlab Robotics Toolbox（10.4）下载、安装、使用教程

主程序：

1 clc;
2 clear;
3 disp('a) 欧拉角 -> 旋转矩阵')
4 a1=10;b1=20;c1=30;  %α，β，γ
5 R1=rpy2r(a1,b1,c1);
6 disp('R1:')
7 disp(R1)
8 a2=30;b2=90;c2=-55;  %α，β，γ
9 R2=rpy2r(a2,b2,c2);
10 disp('R2:')
11 disp(R2)
12 disp('b) 旋转矩阵 -> 欧拉角')
13 euler_o1 = tr2rpy(R1,'deg');
14 disp('R1 对应的欧拉角：');
15 disp(['   α = ',num2str(euler_o1(1)),',β = ',num2str(euler_o1(2)),',γ =
',num2str(euler_o1(3))]);
16 euler_o2 = tr2rpy(R2,'deg');
17 disp('R2 对应的欧拉角：');
18 disp(['   α = ',num2str(euler_o2(1)),',β = ',num2str(euler_o2(2)),',γ =
',num2str(euler_o2(3))]);
19 disp(' ');
20 disp('c) 绕轴旋转');
21 P_B = [1;0;1];
22 P_A = roty(20)*P_B;
23 disp('P_A:');
24 disp(P_A);

输出结果并验证：

1 a) 欧拉角 -> 旋转矩阵

2 R1:
3   0.8138   -0.4410   0.3785
4   0.4698   0.8826   0.0180
5   -0.3420   0.1632   0.9254
6
7 R2:
8         0   0.9962   0.0872
9         0   0.0872   -0.9962
10   -1.0000         0         0
11
12 b) 旋转矩阵 -> 欧拉角
13 R1 对应的欧拉角：
14   α = 10,β = 20,γ = 30
15 R2 对应的欧拉角：
16   α = 0,β = 90,γ = -85
17
18 c) 绕轴旋转
19 P_A:
20   1.2817
21         0
22   0.5977