Matlab化简机械臂正运动学表达式（采用DH矩阵推导）

我们在求解机械臂的正运动学的时候，对于在写论文的过程，我们需要对最终的位姿矩阵进行相应的化简，比如把sin(q1) 写成 S1,cos(q1) 写成 C1等等之类，之前我在处理这些表达式的时候，都是一个一个手动去替换，一个一个 ctrl+F,对于六自由度的机械臂来说真心好累。
　　今天我利用Matlab 中的一些基本语句实现了对位姿矩阵的自动化简。
　　一般在DH参数中 theta 和 alpha 会有角度的变化，一般都是 加减 pi/2，对于alpha 我在DH函数中进行了处理。而对于theta 的角度变化，在DH 函数中不好进行处理，所以需要在求出位姿矩阵之后统一化简处理。

%% 采用DH法计算机械臂的正运动学
%% 输入为 角度 单位是 弧度
function T=FK_DH_S()

syms  q S C real;
syms  qq_1 qq_2 qq_3 qq_4 qq_5 qq_6 real;
syms   a2 a3  real;
syms   S1 S2 S3 S4 S5 S6 C1 C2 C_3 C4 C5 C6 real ;

A1 = DH (qq_1, 0, 0, 0);
A2 = DH (qq_2, 0, a2, pi/2);
A3 = DH (qq_3-pi/2, 0, a3, 0);

T = A1*A2*A3;
T_temp=T;

n=3; %机械臂自由度的个数
% theta 常见的角度组合
SC_simplify=[sin(q) sin(q+pi/2) sin(q-pi/2) cos(q) cos(q+pi/2) cos(q-pi/2)] ;
% theta常见角度的正余弦变换
sc_simplify=[S -C C C -S S];

qq=[qq_1 qq_2 qq_3 qq_4 qq_5 qq_6];

sc_simplify_temp=sc_simplify;

for i=1:1:n
   SC_simplify_temp=subs(SC_simplify,q,qq(i));
    for j=1:1:6
      sc_simplify_temp(j)=sprintf('%s%d',sc_simplify(j),i);
      T=subs(T, SC_simplify_temp(j),sc_simplify_temp(j));
    end
end

disp(T);
disp(T_temp);

end



function A = DH(theta_z, d_z, a_x, alpha_x)

A_R_z = [cos(theta_z) -sin(theta_z) 0 0;
         sin(theta_z) cos(theta_z) 0 0;
         0 0 1 0;
         0 0 0 1];

A_T_z = [1 0 0 0;
         0 1 0 0;
         0 0 1 d_z;
         0 0 0 1];

A_T_x = [1 0 0 a_x;
         0 1 0 0;
         0 0 1 0;
         0 0 0 1];

if alpha_x==0
     A_R_x = [1 0 0 0;
         0 1 0  0;
         0 0 1 0;
         0 0 0 1];
 elseif alpha_x==pi/2
       A_R_x = [1 0 0 0;
         0 0 -1  0;
         0 1 0 0;
         0 0 0 1];
 elseif alpha_x==-pi/2
       A_R_x = [1 0 0 0;
         0 0 1  0;
         0 -1 0 0;
         0 0 0 1];
 else
     A_R_x = [1           0             0          0;
         0      cos( alpha_x)     -sin( alpha_x) 0;
         0       sin( alpha_x)    cos( alpha_x) 0;
         0      0                 0           1];
 end

A = A_R_z*A_T_z*A_T_x*A_R_x;
end

结果如下：

T =
 
[ -S3*(S1*S2 - C1*C2),  C3*(S1*S2 - C1*C2), C1*S2 + C2*S1, C1*C2*a2 - S3*a3*(S1*S2 - C1*C2) - S1*S2*a2]
[  S3*(C1*S2 + C2*S1), -C3*(C1*S2 + C2*S1), S1*S2 - C1*C2, C1*S2*a2 + C2*S1*a2 + S3*a3*(C1*S2 + C2*S1)]
[                  C3,                  S3,             0,                                       C3*a3]
[                   0,                   0,             0,                                           1]
 
 
T_temp =
 
[ cos(qq_3 - pi/2)*(cos(qq_1)*cos(qq_2) - sin(qq_1)*sin(qq_2)), -sin(qq_3 - pi/2)*(cos(qq_1)*cos(qq_2) - sin(qq_1)*sin(qq_2)), cos(qq_1)*sin(qq_2) + cos(qq_2)*sin(qq_1), a3*cos(qq_3 - pi/2)*(cos(qq_1)*cos(qq_2) - sin(qq_1)*sin(qq_2)) + a2*cos(qq_1)*cos(qq_2) - a2*sin(qq_1)*sin(qq_2)]
[ cos(qq_3 - pi/2)*(cos(qq_1)*sin(qq_2) + cos(qq_2)*sin(qq_1)), -sin(qq_3 - pi/2)*(cos(qq_1)*sin(qq_2) + cos(qq_2)*sin(qq_1)), sin(qq_1)*sin(qq_2) - cos(qq_1)*cos(qq_2), a3*cos(qq_3 - pi/2)*(cos(qq_1)*sin(qq_2) + cos(qq_2)*sin(qq_1)) + a2*cos(qq_1)*sin(qq_2) + a2*cos(qq_2)*sin(qq_1)]
[                                             sin(qq_3 - pi/2),                                              cos(qq_3 - pi/2),                                         0,                                                                                               a3*sin(qq_3 - pi/2)]
[                                                            0,                                                             0,                                         0,                                                                                                                 1]
 

从结果中可以看出，化简的结果都是正确的。