subs 符号函数 在带入数值过程中提高计算速度的方法

来源

机器人 transformation matrix 中，使用了符号变量，在对关键变量使用数值带入计算时耗时很长。

计算方法

T_t2b_RPY_tmp 是 transformation matrix，包含关节变量 theta_R theta_P theta_Y

for 循环一个位置一个位置算

tic
BBB = zeros(4,4,prod(num_point));
p = 0;
for i = Roll
for j = Pitch
for k = Yaw
p = p + 1;
BBB(:,:,p) = double(subs(T_t2b_RPY_tmp, {theta_R theta_P theta_Y}, {i j k}));
end
end
end
toc

传递矩阵中的每个元素分开计算

tic
for i = 1:4
    for j = 1:4
        Result_Tmp = double(subs(T_t2b_RPY_tmp(i,j), {theta_R theta_P theta_Y}, {X Y Z}));
        T_ij = Resort_Result_Mesh_Sepa(Result_Tmp);
        T_RPY(i,j,:) =  T_ij;
    end
end
toc
isequal(T_AIO_RPY, T_RPY)


function Trans_Matrix_Resort = Resort_Result_Mesh_Sepa(T)
% 在 transformation matrix 每个元素 【单独】 计算中，subs 函数返回的是一个 mxnxp 矩阵，
% 具体取决于 meshgrid 的输入，在对应关系上，是 z轴 维度先计算，需要重新按 z轴 重新排列
% 数据，保证重拍后矩阵的 z维度变量 和 按 for-loop 可以一一对应
    
AAA = zeros(1,numel(T));
% tmp = [];
sz3 = size(T,3);
% tic
k = 0;
for j = 1:size(T, 2)
    for i = 1:size(T,1)
%         tmp=[tmp, reshape(double(T(i,j,:)), 1, [])];
        k = k + 1;
        AAA(sz3*(k-1)+1 : sz3*k) = reshape(double(T(i,j,:)), 1, []);
    end
end
% isequal(tmp, AAA)
% toc
Trans_Matrix_Resort = AAA;
    
end

subs 函数 直接计算 传递矩阵

p = 0;
for i = 1:length(Roll)
    for j = 1:length(Pitch)
        for k = 1:length(Yaw)
            p = p + 1;
            index_RPY(p,:) = [p, i,j,k];
        end
    end
end
[X, Y, Z] = meshgrid(Roll,Pitch,Yaw); 		% Roll,Pitch,Yaw 关节角度序列
tic
Result_Tmp = double(subs(T_t2b_RPY_tmp, {theta_R theta_P theta_Y}, {X Y Z}));
toc
% 检测对应关系然后还原旋转矩阵
num_point = [Roll_Point, Pitch_Point, Yaw_Point];
T_AIO_RPY = Resort_Result_Mesh_AIO(Result_Tmp, index_RPY, num_point);


function Trans_Matrix_Resort = Resort_Result_Mesh_AIO(T_Tmp, index, num_pt)
% 这个函数把 通过 meshgrid 方法 【一次】 求出的 transformation matrix 重新排列

% tic
for p = 1:size(index,1)
    i = index(p,2);
    j = index(p,3);
    k = index(p,4);
    
    % 由于 meshgrid 后维度上 Y 和 X 交换，详情看帮助，注意 demension 变化。
    % 此处需要交换 x y 坐标
    tmp = i;
    i = j;
    j = tmp;
    Trans_Matrix_Resort(:,:,p) = [
        T_Tmp(i,j,k),               T_Tmp(i,j+num_pt(1)*1,k),               T_Tmp(i,j+num_pt(1)*2,k),              T_Tmp(i,j+num_pt(1)*3,k)         
        T_Tmp(i+num_pt(2)*1,j,k),	T_Tmp(i+num_pt(2)*1,j+num_pt(1)*1,k),	T_Tmp(i+num_pt(2)*1,j+num_pt(1)*2,k),  T_Tmp(i+num_pt(2)*1,j+num_pt(1)*3,k)
        T_Tmp(i+num_pt(2)*2,j,k),	T_Tmp(i+num_pt(2)*2,j+num_pt(1)*1,k),	T_Tmp(i+num_pt(2)*2,j+num_pt(1)*2,k),  T_Tmp(i+num_pt(2)*2,j+num_pt(1)*3,k)
        T_Tmp(i+num_pt(2)*3,j,k),	T_Tmp(i+num_pt(2)*3,j+num_pt(1)*1,k),	T_Tmp(i+num_pt(2)*3,j+num_pt(1)*2,k),  T_Tmp(i+num_pt(2)*3,j+num_pt(1)*3,k)
    ];
end
% toc

end

上面计算还是符号运算，速度还是有限

直接把传递矩阵转成函数进行数值计算更快