robotics section 2 practice 2

a

题目

编写matlab程序, 当用户输入Z-Y-X欧拉角\(\alpha-\beta-\gamma\)和位置矢量\(^{A}P_{B}\)时, 计算齐次变换矩阵\(^{A}T_{B}\).

实验两个例子:
i. \(\alpha = 10^{\circ}, \beta = 20^{\circ}, \gamma = 30^{\circ}, ^{A}P_{B} = [1, 2, 3]^T\)
ii. \(\alpha = 0^{\circ}, \beta = 20^{\circ}, \gamma = 0^{\circ}, ^{A}P_{B} = [3, 0, 1]^T\)

解答

%%ZYX欧拉角+P转齐次变换矩阵T
%输入角度
R_z=input('Rotate around the z-axis in alpha =');
R_y=input('Rotate around the y-axis in beta =');
R_x=input('Rotate around the x-axis in gamma =');
%输入位置矢量
PBA_x = input ('X-axis distance = ');
PBA_y = input ('Y-axis distance = ');
PBA_z = input ('Z-axis distance = ');
%只要有一个输入为空，输出警告
if isempty(R_z)+isempty(R_y)+isempty(R_x)+isempty(PBA_x)+isempty(PBA_y)+isempty(PBA_z)>0
    fprintf('Error!');
else
    R=rotz(R_z)*roty(R_y)*rotx(R_x);%ZYX
    T = [R, [PBA_x; PBA_y; PBA_z]; 0 0 0 1];
    %T=SE3(R,[PBA_x PBA_y PBA_z]);%输出齐次变换矩阵
end

其中SE3函数是机器人工具箱中的函数, 用于生成齐次变换矩阵. 这里是直接手动构造的.

结果

b

题目

已知\(\beta=20^{\circ}\), \(^{A}P_{B}=[3,0,1]^{T}\)和\(P_{B}=[1,0,1]^{T}\), 计算\(^{A}P\), 画草图验证结果是否正确.

解答

%%ZYX欧拉角+P转齐次变换矩阵T
%输入角度
R_z=input('Rotate around the z-axis in alpha =');
R_y=input('Rotate around the y-axis in beta =');
R_x=input('Rotate around the x-axis in gamma =');
%输入位置矢量
PBA_x = input ('X-axis distance = ');
PBA_y = input ('Y-axis distance = ');
PBA_z = input ('Z-axis distance = ');
%只要有一个输入为空，输出警告
if isempty(R_z)+isempty(R_y)+isempty(R_x)+isempty(PBA_x)+isempty(PBA_y)+isempty(PBA_z)>0
    fprintf('Error!');
else
    R=rotz(R_z)*roty(R_y)*rotx(R_x);%ZYX
    T = [R, [PBA_x; PBA_y; PBA_z]; 0 0 0 1];
    %T=SE3(R,[PBA_x PBA_y PBA_z]);%输出齐次变换矩阵
    PB = [1; 0; 1; 1]; % 齐次坐标下的 PB 点
    PA = T * PB; % 变换后的坐标
end

结果

result_b

c

题目

编写一个matlab程序运用符号公式计算齐次变换矩阵的逆矩阵\(^{A}T_{B}^{-1}=^{B}T_{A}\). 将结果和数值数值函数\(inv()\)的结果进行比较. 证明两者相等. 对a中的两个例子进行验证.

解答

%%ZYX欧拉角+P转齐次变换矩阵T
%输入角度
R_z=input('Rotate around the z-axis in alpha =');
R_y=input('Rotate around the y-axis in beta =');
R_x=input('Rotate around the x-axis in gamma =');
%输入位置矢量
PBA_x = input ('X-axis distance = ');
PBA_y = input ('Y-axis distance = ');
PBA_z = input ('Z-axis distance = ');
%只要有一个输入为空，输出警告
if isempty(R_z)+isempty(R_y)+isempty(R_x)+isempty(PBA_x)+isempty(PBA_y)+isempty(PBA_z)>0
    fprintf('Error!');
else
    R=rotz(R_z)*roty(R_y)*rotx(R_x);%ZYX
    T = [R, [PBA_x; PBA_y; PBA_z]; 0 0 0 1];
    %T=SE3(R,[PBA_x PBA_y PBA_z]);%输出齐次变换矩阵
    %计算逆矩阵
    T_inv = inv(T);
    %符号计算逆矩阵
    R_inv = R'; % 旋转矩阵的逆矩阵
    x = -R_inv * [PBA_x; PBA_y; PBA_z]; % 位置矢量的逆矩阵
    T_inv_sym = [R_inv, x; 0 0 0 1];
    %验证两者是否相等
    disp('T_inv = ');
    disp(T_inv);
    disp('T_inv_sym = ');
    disp(T_inv_sym);
    disp('T_inv - T_inv_sym = ');
    disp(T_inv - T_inv_sym);
end

结果

d

题目

令\(^{A}T_{B}\)为a中i的解, \(^{B}T_{C}\)为a中ii的解.
i. 计算\(^{A}T_{C}\), 并用变换图说明关系. 对\(^{C}T_{A}\)作同样的处理.
ii. 已知i中的\(^{A}T_{C}\)和\(^{B}T_{C}\), 假定\(^{A}T_{B}\)未知, 计算它并与已知的答案进行比较.
iii. 已知i中的\(^{A}T_{C}\)和\(^{A}T_{B}\), 假定\(^{B}T_{C}\)未知, 计算它并与已知的答案进行比较.

解答

1. \[ ^{A}T_{C} = ^{A}T_{B} \cdot ^{B}T_{C} = \begin{bmatrix} R_{A} & P_{A} \\ 0 & 1 \end{bmatrix} \begin{bmatrix} R_{B} & P_{B} \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} R_{A} \cdot R_{B} & R_{A} \cdot P_{B} + P_{A} \\ 0 & 1 \end{bmatrix} \]

\[ ^{C}T_{A} = (^{A}T_{C})^{-1} = (^{A}T_{B} \cdot ^{B}T_{C})^{-1} = \begin{bmatrix} R_{A} \cdot R_{B} & R_{A} \cdot P_{B} + P_{A} \\ 0 & 1 \end{bmatrix}^{-1} = \begin{bmatrix} R_{B}^{T} & -R_{B}^{T} \cdot P_{B} \\ 0 & 1 \end{bmatrix} \]

2. \[ ^{A}T_{B} = ^{A}T_{C} \cdot ^{C}T_{B} = \begin{bmatrix} R_{A} \cdot R_{C} & R_{A} \cdot P_{C} + P_{A} \\ 0 & 1 \end{bmatrix} \begin{bmatrix} R_{C} & P_{C} \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} R_{A} & P_{A} \\ 0 & 1 \end{bmatrix} \]

3. \[ ^{B}T_{C} = ^{B}T_{A} \cdot ^{A}T_{C} = \begin{bmatrix} R_{B} & P_{B} \\ 0 & 1 \end{bmatrix} \begin{bmatrix} R_{A} & P_{A} \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} R_{C} & P_{C} \\ 0 & 1 \end{bmatrix} \]

TAB = [ 0.9254    0.0180    0.3785    1;
        0.1632    0.8826   -0.4410    2;
       -0.3420    0.4698    0.8138    3;
        0         0         0         1];

TBC = [ 0.9397     0    0.3420         3;
         0         1         0         0;
        -0.3420    0    0.9397         1;
         0         0         0         1];

TAC = TAB * TBC;

disp(TAC)

结果

result_d