当前位置：首页 > news >正文

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

news 来源：原创 2024/5/12 13:48:34

来源

机器人 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:4for j = 1:4Result_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];endend
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
% tocend