173 lines
5.9 KiB
Matlab
173 lines
5.9 KiB
Matlab
%% 五连杆机构腿长计算脚本
|
||
clear; clc;
|
||
|
||
%% 定义输入参数
|
||
% 连杆长度 (m)
|
||
l1 = 0.215; % 连杆1长度
|
||
l2 = 0.258; % 连杆2长度
|
||
l3 = 0.258; % 连杆3长度
|
||
l4 = 0.215; % 连杆4长度
|
||
l5 = 0.0; % 连杆5长度
|
||
|
||
% 关节角度 (弧度)
|
||
phi1 = 0.10; % 30度
|
||
phi4 = 2.96; % 45度
|
||
|
||
%% 调用腿长计算函数
|
||
fprintf('五连杆机构腿长计算\n');
|
||
fprintf('输入参数:\n');
|
||
fprintf(' 连杆长度: l1=%.3f, l2=%.3f, l3=%.3f, l4=%.3f, l5=%.3f (m)\n', l1, l2, l3, l4, l5);
|
||
fprintf(' 关节角度: phi1=%.3f (%.1f°), phi4=%.3f (%.1f°)\n', phi1, rad2deg(phi1), phi4, rad2deg(phi4));
|
||
|
||
try
|
||
% 计算各关键点位置
|
||
% B点 (连杆1末端)
|
||
x_B = l1 * cos(phi1);
|
||
y_B = l1 * sin(phi1);
|
||
|
||
% D点 (连杆4末端)
|
||
x_D = l5 + l4 * cos(phi4);
|
||
y_D = l4 * sin(phi4);
|
||
|
||
% 计算BD距离
|
||
BD_dist = sqrt((x_D - x_B)^2 + (y_D - y_B)^2);
|
||
|
||
% 检查三角形约束条件
|
||
if BD_dist > (l2 + l3) || BD_dist < abs(l2 - l3)
|
||
warning('无法形成闭合五连杆机构！BD距离=%.3f, 需要满足: %.3f < BD < %.3f', ...
|
||
BD_dist, abs(l2-l3), l2+l3);
|
||
leg_length = NaN;
|
||
foot_pos = [NaN, NaN];
|
||
return;
|
||
end
|
||
|
||
% 使用余弦定理求解phi2
|
||
cos_phi2_BD = (l2^2 + BD_dist^2 - l3^2) / (2 * l2 * BD_dist);
|
||
|
||
% 检查余弦值是否在有效范围内
|
||
if abs(cos_phi2_BD) > 1
|
||
warning('余弦值超出范围: %.3f', cos_phi2_BD);
|
||
leg_length = NaN;
|
||
foot_pos = [NaN, NaN];
|
||
return;
|
||
end
|
||
|
||
% 计算BD方向角
|
||
alpha_BD = atan2(y_D - y_B, x_D - x_B);
|
||
|
||
% 计算phi2 (两个解，选择合理的一个)
|
||
angle_offset = acos(cos_phi2_BD);
|
||
phi2_solution1 = alpha_BD + angle_offset;
|
||
phi2_solution2 = alpha_BD - angle_offset;
|
||
|
||
% 选择合理的解 (通常选择使机构在合理配置的解)
|
||
phi2 = phi2_solution1; % 可以根据实际需要调整选择逻辑
|
||
|
||
% 计算C点位置 (足端位置)
|
||
x_C = x_B + l2 * cos(phi2);
|
||
y_C = y_B + l2 * sin(phi2);
|
||
|
||
% 验证C点到D点距离是否等于l3
|
||
CD_dist = sqrt((x_D - x_C)^2 + (y_D - y_C)^2);
|
||
error_CD = abs(CD_dist - l3);
|
||
|
||
if error_CD > 1e-6
|
||
fprintf('警告: CD距离误差较大 (%.6f)\n', error_CD);
|
||
end
|
||
|
||
% 计算腿长 (假设为原点到C点的距离)
|
||
leg_length = sqrt(x_C^2 + y_C^2);
|
||
foot_pos = [x_C, y_C];
|
||
|
||
% 输出结果
|
||
fprintf('\n计算结果:\n');
|
||
fprintf(' B点位置: (%.3f, %.3f)\n', x_B, y_B);
|
||
fprintf(' C点位置: (%.3f, %.3f)\n', x_C, y_C);
|
||
fprintf(' D点位置: (%.3f, %.3f)\n', x_D, y_D);
|
||
fprintf(' phi2角度: %.3f rad (%.1f°)\n', phi2, rad2deg(phi2));
|
||
fprintf(' 腿长: %.3f m\n', leg_length);
|
||
fprintf(' BD距离: %.3f m\n', BD_dist);
|
||
fprintf(' CD验证: %.6f m (应该等于l3=%.3f)\n', CD_dist, l3);
|
||
|
||
%% 可视化结果
|
||
figure;
|
||
plot([0, x_B], [0, y_B], 'b-', 'LineWidth', 2); % 连杆1
|
||
hold on;
|
||
plot([x_B, x_C], [y_B, y_C], 'r-', 'LineWidth', 2); % 连杆2
|
||
plot([x_C, x_D], [y_C, y_D], 'g-', 'LineWidth', 2); % 连杆3
|
||
plot([l5, x_D], [0, y_D], 'm-', 'LineWidth', 2); % 连杆4
|
||
plot([0, l5], [0, 0], 'k-', 'LineWidth', 3); % 连杆5 (基座)
|
||
|
||
% 标记关键点
|
||
plot(0, 0, 'ko', 'MarkerSize', 8, 'MarkerFaceColor', 'k'); % A点
|
||
plot(x_B, y_B, 'bo', 'MarkerSize', 8, 'MarkerFaceColor', 'b'); % B点
|
||
plot(x_C, y_C, 'ro', 'MarkerSize', 8, 'MarkerFaceColor', 'r'); % C点
|
||
plot(x_D, y_D, 'go', 'MarkerSize', 8, 'MarkerFaceColor', 'g'); % D点
|
||
plot(l5, 0, 'ko', 'MarkerSize', 8, 'MarkerFaceColor', 'k'); % E点
|
||
|
||
% 标记腿长
|
||
plot([0, x_C], [0, y_C], 'k--', 'LineWidth', 1); % 腿长线
|
||
|
||
% 图形设置
|
||
grid on;
|
||
axis equal;
|
||
xlabel('X (m)');
|
||
ylabel('Y (m)');
|
||
title('五连杆机构示意图');
|
||
legend('连杆1', '连杆2', '连杆3', '连杆4', '基座', 'Location', 'best');
|
||
|
||
% 添加文本标注
|
||
text(0, 0-0.02, 'A', 'HorizontalAlignment', 'center');
|
||
text(x_B, y_B+0.02, 'B', 'HorizontalAlignment', 'center');
|
||
text(x_C, y_C+0.02, 'C(足端)', 'HorizontalAlignment', 'center');
|
||
text(x_D, y_D+0.02, 'D', 'HorizontalAlignment', 'center');
|
||
text(l5, 0-0.02, 'E', 'HorizontalAlignment', 'center');
|
||
text(x_C/2, y_C/2, sprintf('腿长=%.3fm', leg_length), 'HorizontalAlignment', 'center');
|
||
|
||
catch ME
|
||
fprintf('计算过程中出现错误: %s\n', ME.message);
|
||
leg_length = NaN;
|
||
foot_pos = [NaN, NaN];
|
||
end
|
||
|
||
%% 多组参数测试
|
||
fprintf('\n\n=== 多组参数测试 ===\n');
|
||
test_cases = [
|
||
pi/6, pi/4; % 30°, 45°
|
||
pi/4, pi/3; % 45°, 60°
|
||
-pi/6, pi/2; % -30°, 90°
|
||
0, pi/6; % 0°, 30°
|
||
];
|
||
|
||
for i = 1:size(test_cases, 1)
|
||
phi1_test = test_cases(i, 1);
|
||
phi4_test = test_cases(i, 2);
|
||
|
||
fprintf('\n测试 %d: phi1=%.1f°, phi4=%.1f°\n', i, rad2deg(phi1_test), rad2deg(phi4_test));
|
||
|
||
% 重复计算过程（简化版）
|
||
x_B_test = l1 * cos(phi1_test);
|
||
y_B_test = l1 * sin(phi1_test);
|
||
x_D_test = l5 + l4 * cos(phi4_test);
|
||
y_D_test = l4 * sin(phi4_test);
|
||
BD_dist_test = sqrt((x_D_test - x_B_test)^2 + (y_D_test - y_B_test)^2);
|
||
|
||
if BD_dist_test <= (l2 + l3) && BD_dist_test >= abs(l2 - l3)
|
||
cos_phi2_BD_test = (l2^2 + BD_dist_test^2 - l3^2) / (2 * l2 * BD_dist_test);
|
||
if abs(cos_phi2_BD_test) <= 1
|
||
alpha_BD_test = atan2(y_D_test - y_B_test, x_D_test - x_B_test);
|
||
angle_offset_test = acos(cos_phi2_BD_test);
|
||
phi2_test = alpha_BD_test + angle_offset_test;
|
||
x_C_test = x_B_test + l2 * cos(phi2_test);
|
||
y_C_test = y_B_test + l2 * sin(phi2_test);
|
||
leg_length_test = sqrt(x_C_test^2 + y_C_test^2);
|
||
fprintf(' 结果: 腿长=%.3f m, 足端位置=(%.3f, %.3f)\n', leg_length_test, x_C_test, y_C_test);
|
||
else
|
||
fprintf(' 结果: 余弦值超出范围\n');
|
||
end
|
||
else
|
||
fprintf(' 结果: 无法形成闭合机构\n');
|
||
end
|
||
end
|
||
|
||
fprintf('\n计算完成！\n'); |