% 问题2求解:机器人逆向运动学 % 从初始构型q0开始,通过迭代找到使末端执行器位置为pt、旋转矩阵为Rt的构型q
% 清空工作区和命令窗口 清楚; CLC;
% 定义D-H参数 % 按照题目中表1给出的参数设置 % 其中h = e = f = d = 1 h = 1; e = 1; f = 1; d = 1;
% 定义D-H参数表:theta, d, a, alpha % 注意:alpha需要转换为弧度 DH = [0, h, 0, -pi/2; 0, 0, e, 0; 0, 0, 0, -pi/2; 0, f, 0, pi/2; 0, 0, 0, -pi/2; 0, d, 0, 0];
% 定义目标位置和旋转矩阵(从问题1的结果获取) % 这里硬编码问题1的结果 pt = [1.5535; -1.8248; 0.3505]; Rt = [-0.7249, -0.0435, 0.6875; -0.3605, 0.8744, -0.3248; -0.5870, -0.4833, -0.6495];
% 定义初始构型q0 q0 = [-2*pi/3, -pi/3, 0, -pi/6, 0, 0]';
% 定义迭代参数 max_iterations = 1000;% 最大迭代次数 error_threshold = 1e-5;% 误差阈值 damping_factor = 0.5;% 阻尼因子,增大以提高稳定性 step_size = 0.2;% 步长因子,减小以提高稳定性
% 初始化 q = q0; 迭代 = 0; 错误 = [];% 用于存储每次迭代的误差 q_history = q';% 用于存储每次迭代的构型
% 迭代求解 while iteration < max_iterations % 增加迭代计数 迭代 = 迭代 + 1;
% 计算当前构型下的末端执行器位置和姿态 [p, R] = forward_kinematics(q, DH);
% 计算位置误差 pos_error = pt - p; pos_error_norm = norm(pos_error);
% 简化姿态误差的计算方式 % 使用旋转矩阵的Frobenius范数作为误差度量 rot_error_norm = norm(Rt - R, 'fro');
% 结合位置和姿态误差 error_vector = [pos_error; reshape(Rt - R, 9, 1)]; total_error = norm(error_vector);
% 记录误差 errors(iteration, :) = [pos_error_norm, rot_error_norm, total_error];%disp('唯一原创作者:B站北辰single 唯一原创地址:https://www.yuque.com/u42168770/qv6z0d/sdl2bw8pmgz75vv5 倒卖必究(全网巡查)')
% 打印当前迭代信息 if mod(iteration, 10) == 0 fprintf('迭代 %d: 位置误差 = %.6f, 姿态误差 = %.6f\n', iteration, pos_error_norm, rot_error_norm);%disp('唯一原创作者:B站北辰single 唯一原创地址:https://www.yuque.com/u42168770/qv6z0d/sdl2bw8pmgz75vv5 倒卖必究(全网巡查)') end
% 检查是否达到收敛条件 if total_error < error_threshold disp('迭代收敛成功!'); break; end
% 计算雅可比矩阵 J = compute_jacobian(q, DH);
% 使用阻尼最小二乘法计算雅可比矩阵的伪逆 J_pinv = J' * inv(J * J' + damping_factor^2 * eye(size(J, 1)));
% 简化误差向量,只使用位置误差和旋转矩阵差异 simple_error = [pos_error; reshape(Rt - R, 9, 1)];
% 计算构型的变化量 delta_q = step_size * J_pinv * simple_error;
% 更新构型 q = q + delta_q;
% 记录构型历史 q_history = [q_history; q']; 结束
% 打印最终结果 disp(['总迭代次数: ', num2str(iteration)]); disp('最终构型q:'); disp(q); disp('目标位置pt:'); disp(pt); disp('最终位置p:'); disp(p); disp('位置误差:'); disp(norm(pt - p)); disp('目标旋转矩阵Rt:'); disp(Rt); disp('最终旋转矩阵R:'); disp(R); disp('旋转矩阵误差(Frobenius范数):'); disp(norm(Rt - R, 'fro'));
% 保存结果到Excel文件 % result_table = table(q, pt, p, pt-p, 'VariableNames', {'最终构型', '目标位置', '实际位置', '位置误差'});%disp('唯一原创作者:B站北辰single 唯一原创地址:https://www.yuque.com/u42168770/qv6z0d/sdl2bw8pmgz75vv5 倒卖必究(全网巡查)') % writetable(result_table, '问题2_计算结果.xlsx');
% 可视化误差变化 figure('位置', [100, 100, 800, 600], '颜色', '白色'); semilogy(1:iteration, errors(:, 1), 'r-', 'LineWidth', 2, 'DisplayName', '位置误差'); 坚持; semilogy(1:iteration, errors(:, 2), 'g-', 'LineWidth', 2, 'DisplayName', '姿态误差'); semilogy(1:iteration, errors(:, 3), 'b-', 'LineWidth', 2, 'DisplayName', '总误差');%disp('唯一原创作者:B站北辰single 唯一原创地址:https://www.yuque.com/u42168770/qv6z0d/sdl2bw8pmgz75vv5 倒卖必究(全网巡查)') xlabel('迭代次数', 'FontSize', 14); ylabel('误差(对数刻度)', 'FontSize', 14); title('逆运动学迭代过程中的误差变化', 'FontSize', 16); 网格开启; legend('位置', '最佳'); saveas(gcf, '问题2_误差变化.png'); print('问题2_误差变化', '-dpng', '-r300');
% 可视化关节角度变化 figure('位置', [100, 100, 800, 600], '颜色', '白色'); joint_labels = {'关节1', '关节2', '关节3', '关节4', '关节5', '关节6'}; for i = 1:6%disp('唯一原创作者:B站北辰single 唯一原创地址:https://www.yuque.com/u42168770/qv6z0d/sdl2bw8pmgz75vv5 倒卖必究(全网巡查)') 子情节(3, 2, i); plot(0:迭代, q_history(:, i), 'LineWidth', 2); xlabel('迭代次数', 'FontSize', 12); ylabel('角度(弧度)', 'FontSize', 12); title(['关节 ', num2str(i), ' 角度变化'], 'FontSize', 14); 网格开启; 结束 saveas(gcf, '问题2_关节角度变化.png'); print('问题2_关节角度变化', '-dpng', '-r300');
% 可视化初始和最终机器人构型 figure('位置', [100, 100, 800, 600], '颜色', '白色'); 子情节(1, 2, 1); visualize_robot(q0, DH, '初始构型'); 子情节(1, 2, 2); visualize_robot(q, DH, '最终构型'); saveas(gcf, '问题2_构型对比.png'); print('问题2_构型对比', '-dpng', '-r300');
% 可视化迭代过程中的机器人构型变化 figure('位置', [100, 100, 800, 600], '颜色', '白色'); % 选择一些关键帧显示 num_frames = min(5, iteration+1); frame_indices = round(linspace(1, size(q_history, 1), num_frames)); 问题 对于 i = 1:num_frames 子情节(2, 3, i); idx = frame_indices(i); visualize_robot(q_history(idx, :)', DH, ['迭代 ', num2str(idx-1)]);%disp('唯一原创作者:B站北辰single 唯一原创地址:https://www.yuque.com/u42168770/qv6z0d/sdl2bw8pmgz75vv5 倒卖必究(全网巡查)') 结束 saveas(gcf, '问题2_迭代过程.png'); print('问题2_迭代过程', '-dpng', '-r300');
% 可视化轨迹 figure('位置', [100, 100, 800, 600], '颜色', '白色'); % 计算每个构型下的末端执行器位置 end_effector_trajectory = 零(大小(q_history, 1), 3); 对于 i = 1:size(q_history, 1) [p_i, ~] = forward_kinematics(q_history(i, :)', DH); end_effector_trajectory(i, :) = p_i'; 结束 问题 % 绘制轨迹 plot3(end_effector_trajectory(:, 1), end_effector_trajectory(:, 2), end_effector_trajectory(:, 3), 'b-', 'LineWidth', 2); 坚持; plot3(end_effector_trajectory(1, 1), end_effector_trajectory(1, 2), end_effector_trajectory(1, 3), 'ro', 'MarkerSize', 10, 'LineWidth', 2); plot3(end_effector_trajectory(end, 1), end_effector_trajectory(end, 2), end_effector_trajectory(end, 3), 'go', 'MarkerSize', 10, 'LineWidth', 2);%disp('唯一原创作者:B站北辰single 唯一原创地址:https://www.yuque.com/u42168770/qv6z0d/sdl2bw8pmgz75vv5 倒卖必究(全网巡查)') plot3(pt(1), pt(2), pt(3), 'k*', 'MarkerSize', 12, 'LineWidth', 2); xlabel('X轴', 'FontSize', 14); ylabel('Y轴', 'FontSize', 14); zlabel('Z轴', 'FontSize', 14); title('末端执行器轨迹', 'FontSize', 16); 网格开启; legend('轨迹', '起点', '终点', '目标点', '位置', '最佳'); saveas(gcf, '问题2_末端执行器轨迹.png'); print('问题2_末端执行器轨迹', '-dpng', '-r300');问题 问题 % 定义正向运动学函数 函数 [p, R] = forward_kinematics(q, DH) % 计算从基座坐标系到末端执行器坐标系的齐次变换矩阵 T = 眼睛 (4);
for i = 1:6 theta = q(i) + DH(i, 1); d = DH(i, 2); a = DH(i, 3); alpha = DH(i, 4);
% 计算齐次变换矩阵
A = [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta);
sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta);
0, sin(alpha), cos(alpha), d;%disp('唯一原创作者:B站北辰single 唯一原创地址:https://www.yuque.com/u42168770/qv6z0d/sdl2bw8pmgz75vv5 倒卖必究(全网巡查)')
0, 0, 0, 1];
T = T * A;
end
% 提取位置向量和旋转矩阵 p = T(1:3, 4); R = T(1:3, 1:3); 结束
% 定义计算雅可比矩阵的函数 函数 J = compute_jacobian(q, DH) % 计算各个关节坐标系的位置和旋转轴 T = 眼睛 (4); joint_positions = 零 (7, 3);% 包括基座原点和6个关节 z_axes = 零 (6, 3);% 各个关节的z轴(旋转轴)
% 计算各个关节的位置和z轴 for i = 1:6 % 记录当前的z轴(旋转轴) z_axes(i, :) = T(1:3, 3)';
% 记录关节位置
joint_positions(i, :) = T(1:3, 4)';
% 计算变换矩阵
theta = q(i) + DH(i, 1);
d = DH(i, 2);
a = DH(i, 3);
alpha = DH(i, 4);
A = [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta);
sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta);%disp('唯一原创作者:B站北辰single 唯一原创地址:https://www.yuque.com/u42168770/qv6z0d/sdl2bw8pmgz75vv5 倒卖必究(全网巡查)')
0, sin(alpha), cos(alpha), d;
0, 0, 0, 1];
T = T * A;
end
% 记录末端执行器位置 joint_positions(7, :) = T(1:3, 4)';
% 计算雅可比矩阵 J = zeros(12, 6); % 使用更大的雅可比矩阵,包含位置和旋转矩阵的所有元素
% 位置部分的雅可比(前3行) for i = 1:6 % 计算速度雅可比 J(1:3, i) = cross(z_axes(i, :)', T(1:3, 4) - joint_positions(i, :)'); end
% 姿态部分的雅可比(简化处理,直接使用数值微分近似) epsilon = 1e-6; % 微小扰动 [~, R_base] = forward_kinematics(q, DH); % 基准旋转矩阵
for i = 1:6 q_perturbed = q; q_perturbed(i) = q_perturbed(i) + epsilon; [~, R_perturbed] = forward_kinematics(q_perturbed, DH);
% 旋转矩阵差异除以epsilon,作为数值导数近似
dR_dq = (R_perturbed - R_base) / epsilon;
% 将旋转矩阵差异展平为向量
J(4:12, i) = reshape(dR_dq, 9, 1);
end 结束
% 辅助函数:可视化机器人构型 功能 visualize_robot(q, DH, title_str) % 计算各个关节的位置 joint_positions = 零 (7, 3);% 包括基座原点和6个关节%disp('唯一原创作者:B站北辰single 唯一原创地址:https://www.yuque.com/u42168770/qv6z0d/sdl2bw8pmgz75vv5 倒卖必究(全网巡查)') T = 眼睛 (4);
for i = 1:6 theta = q(i) + DH(i, 1); d = DH(i, 2); a = DH(i, 3); alpha = DH(i, 4);
% 计算齐次变换矩阵
A = [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta);
sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta);
0, sin(alpha), cos(alpha), d;
0, 0, 0, 1];
T = T * A;
joint_positions(i+1, :) = T(1:3, 4)';
end
% 绘制机器人的连杆 plot3(joint_positions(:, 1), joint_positions(:, 2), joint_positions(:, 3), 'k-', 'LineWidth', 2); hold on;
% 绘制各个关节 scatter3(joint_positions(:, 1), joint_positions(:, 2), joint_positions(:, 3), 100, 'r', 'filled');%disp('唯一原创作者:B站北辰single 唯一原创地址:https://www.yuque.com/u42168770/qv6z0d/sdl2bw8pmgz75vv5 倒卖必究(全网巡查)')
% 设置图形属性 title(title_str, 'FontSize', 14); xlabel('X轴', 'FontSize', 12); ylabel('Y轴', 'FontSize', 12); zlabel('Z轴', 'FontSize', 12); grid on; axis equal; view(30, 20);
% 清除hold状态 hold off;%disp('唯一原创作者:B站北辰single 唯一原创地址:https://www.yuque.com/u42168770/qv6z0d/sdl2bw8pmgz75vv5 倒卖必究(全网巡查)') 结束 这是问题2的代码,是基于问题1写出来的,但是问题1 的代码已经做出优化,下面我将给初问题1优化后的代码,请根据修改后的代码对图中的问题2进行解答 问题1优化后的代码 % 问题1求解:机器人正向运动学 % 计算6R机器人末端执行器的位置p_6^0和旋转矩阵R_6^0
% 清空工作区和命令窗口 清楚; CLC; 全部关闭;
% 定义D-H参数 % 按照题目中表1给出的参数设置 % 其中h = e = f = d = 1 h = 1; e = 1; f = 1; d_param = 1;% 避免与MATLAB内置函数d冲突
% 定义D-H参数表 % 根据表1:theta为关节变量,这里只定义d, a, alpha % d参数 d_values = [h, 0, 0, f, 0, d_param]; % a参数 a_values = [0, e, 0, 0, 0, 0]; % alpha参数(弧度) alpha_values = [-pi/2, 0, -pi/2, pi/2, -pi/2, 0];
% 定义机器人构型q(关节角度) q = [-pi/3, -pi/6, -pi/6, pi/3, pi/6, pi/3]';
% 计算每个关节的齐次变换矩阵 A = 单元格 (6, 1);% 用于存储各个变换矩阵
% 计算各个齐次变换矩阵 对于 i = 1:6 θ = q(i);% 关节角度 d = d_values(i); a = a_values(i); 阿尔法 = alpha_values(i);
% 计算齐次变换矩阵(标准DH参数) A{i} = [cos(theta), -sin(theta)*cos(alpha), sin(theta)sin(alpha), acos(theta); sin(theta), cos(theta)*cos(alpha), -cos(theta)sin(alpha), asin(theta); 0, sin(alpha), cos(alpha), d; 0, 0, 0, 1]; 结束
% 计算从基座坐标系到末端执行器坐标系的总变换矩阵 T0_6 = 眼睛 (4);% 初始化为单位矩阵 对于 i = 1:6 T0_6 = T0_6 * A{i}; 结束
% 提取位置向量p_6^0和旋转矩阵R_6^0 p0_6 = T0_6(1:3, 4); R0_6 = T0_6(1:3, 1:3);
% 将结果赋给pt和Rt pt = p0_6; Rt = R0_6;
% 打印结果 fprintf('末端执行器的位置向量pt:\n'); fprintf('x = %.4f\n', pt(1)); fprintf('y = %.4f\n', pt(2)); fprintf('z = %.4f\n', pt(3)); fprintf('\n末端执行器的旋转矩阵Rt:\n'); disp(Rt);
% 验证旋转矩阵的正交性 fprintf('\n验证旋转矩阵的正交性:\n'); fprintf('det(Rt) = %.6f (应该为1)\n', det(Rt)); fprintf('Rt * Rt'' - I 的范数 = %.6e (应该接近0)\n', norm(Rt * Rt' - eye(3)));
% ========== 改进的机器人可视化部分 ==========
% 创建更专业的机器人可视化 figure('位置', [100, 100, 1000, 800], '颜色', '白色');
% 计算各个关节的位置和坐标系 joint_positions = 零 (7, 3);% 包括基座原点和6个关节 T_frames = 单元格 (7, 1);% 存储各个坐标系的变换矩阵 T_frames{1} = 眼睛 (4);% 基座坐标系
T_current = 眼睛 (4); 对于 i = 1:6 T_current = T_current * A{i}; joint_positions(i+1, :) = T_current(1:3, 4)'; T_frames{i+1} = T_current; 结束
% 计算机器人的工作范围,用于优化视图 x_range = [min(joint_positions(:,1)), max(joint_positions(:,1))]; y_range = [min(joint_positions(:,2)), max(joint_positions(:,2))]; z_range = [min(joint_positions(:,3)), max(joint_positions(:,3))];
% 1.绘制机器人基座(改进:使用更紧凑的基座) base_size = 0.2;% 减小基座尺寸 base_height = 0.08;% 减小基座高度 [X_base, Y_base, Z_base] = 圆柱体(base_size, 20); Z_base = Z_base * base_height - base_height; surf(X_base, Y_base, Z_base, 'FaceColor', [0.3 0.3 0.3], 'EdgeColor', 'none'); 坚持;
% 添加基座底板 填充3(X_base(1,:), Y_base(1,:), Z_base(1,:), [0.2 0.2 0.2]);
% 2.绘制连杆(使用圆柱体表示) link_radius = 0.04;% 稍微减小连杆半径 joint_radius = 0.06;% 稍微减小关节半径
% 绘制每个连杆 对于 i = 1:6 start_pos = joint_positions(i, :); end_pos = joint_positions(i+1, :);
% 计算连杆方向和长度 link_vec = end_pos - start_pos; link_length = norm(link_vec);
if link_length > 0.01 % 只绘制有长度的连杆 % 创建圆柱体 [X_cyl, Y_cyl, Z_cyl] = cylinder(link_radius, 20); Z_cyl = Z_cyl * link_length;
% 计算旋转矩阵,使圆柱体对齐连杆方向
if link_length > 0
z_axis = link_vec(:) / link_length; % 确保是列向量
% 找到一个垂直于z_axis的向量
if abs(z_axis(3)) < 0.9
x_axis = cross([0; 0; 1], z_axis);
else
x_axis = cross([1; 0; 0], z_axis);
end
x_axis = x_axis / norm(x_axis);
y_axis = cross(z_axis, x_axis);
R_link = [x_axis(:), y_axis(:), z_axis(:)]; % 确保每个都是列向量
% 变换圆柱体顶点
for j = 1:size(X_cyl, 2)
for k = 1:size(X_cyl, 1)
point = R_link * [X_cyl(k,j); Y_cyl(k,j); Z_cyl(k,j)] + start_pos(:);
X_cyl(k,j) = point(1);
Y_cyl(k,j) = point(2);
Z_cyl(k,j) = point(3);
end
end
% 绘制连杆
surf(X_cyl, Y_cyl, Z_cyl, 'FaceColor', [0.7 0.7 0.7], 'EdgeColor', 'none');
end
end end
% 3. 绘制关节(使用球体表示) for i = 1:7 [X_sphere, Y_sphere, Z_sphere] = sphere(20); X_sphere = X_sphere * joint_radius + joint_positions(i, 1); Y_sphere = Y_sphere * joint_radius + joint_positions(i, 2); Z_sphere = Z_sphere * joint_radius + joint_positions(i, 3);
% 根据关节类型选择颜色 if i == 1 || i == 7 joint_color = [0.8 0.2 0.2]; % 基座和末端为红色 else joint_color = [0.2 0.2 0.8]; % 其他关节为蓝色 end
surf(X_sphere, Y_sphere, Z_sphere, 'FaceColor', joint_color, 'EdgeColor', 'none'); 结束
% 4.绘制坐标系(改进:增强末端执行器坐标系的可视化) 比例 = 0.15;% 坐标轴长度 for i = [1, 7] % 显示基座和末端坐标系 如果 i == 1 T_i = 眼睛 (4); frame_label = '基座'; axis_scale = 刻度; axis_width = 2; 还 T_i = T_frames{7}; frame_label = '末端执行器'; axis_scale = 比例 * 1.5;% 末端坐标系更大 axis_width = 4;% 末端坐标轴更粗 结束
origin = T_i(1:3, 4);
% 绘制坐标轴 % X轴 - 红色 quiver3(origin(1), origin(2), origin(3), ... T_i(1,1)*axis_scale, T_i(2,1)*axis_scale, T_i(3,1)*axis_scale, ... 'r', 'LineWidth', axis_width, 'MaxHeadSize', 0.5); % Y轴 - 绿色 quiver3(origin(1), origin(2), origin(3), ... T_i(1,2)*axis_scale, T_i(2,2)*axis_scale, T_i(3,2)*axis_scale, ... 'g', 'LineWidth', axis_width, 'MaxHeadSize', 0.5); % Z轴 - 蓝色 quiver3(origin(1), origin(2), origin(3), ... T_i(1,3)*axis_scale, T_i(2,3)*axis_scale, T_i(3,3)*axis_scale, ... 'b', 'LineWidth', axis_width, 'MaxHeadSize', 0.5);
% 添加坐标轴标签 if i == 7 % 只为末端执行器添加轴标签 text(origin(1) + T_i(1,1)axis_scale1.1, ... origin(2) + T_i(2,1)axis_scale1.1, ... origin(3) + T_i(3,1)axis_scale1.1, 'X_6', ... 'FontSize', 10, 'FontWeight', 'bold', 'Color', 'r'); text(origin(1) + T_i(1,2)axis_scale1.1, ... origin(2) + T_i(2,2)axis_scale1.1, ... origin(3) + T_i(3,2)axis_scale1.1, 'Y_6', ... 'FontSize', 10, 'FontWeight', 'bold', 'Color', 'g'); text(origin(1) + T_i(1,3)axis_scale1.1, ... origin(2) + T_i(2,3)axis_scale1.1, ... origin(3) + T_i(3,3)axis_scale1.1, 'Z_6', ... 'FontSize', 10, 'FontWeight', 'bold', 'Color', 'b'); end
% 添加标签 text(origin(1)+0.1, origin(2)+0.1, origin(3)+0.1, frame_label, ... 'FontSize', 12, 'FontWeight', 'bold', 'BackgroundColor', 'white'); 结束
% 5.添加末端执行器工具(简化的夹爪模型) % 绘制简单的夹爪 gripper_length = 0.15; gripper_width = 0.08; gripper_origin = pt; gripper_z_dir = rt(:,3);% 工具方向沿Z轴 gripper_x_dir = rt(:,1); gripper_y_dir = rt(:,2);
% 夹爪基座 gripper_base = gripper_origin + gripper_z_dir * 0.05; [X_grip_base, Y_grip_base, Z_grip_base] = 圆柱体 (0.05, 10); Z_grip_base = Z_grip_base * 0.05; % 变换到正确位置 对于 j = 1:size(X_grip_base, 2) 对于 k = 1:大小(X_grip_base, 1) 点 = Rt * [X_grip_base(k,j);Y_grip_base(k,j);Z_grip_base(k,j)] + gripper_origin; X_grip_base(k,j) = 点 (1); Y_grip_base(k,j) = 点 (2); Z_grip_base(k,j) = 点(3); 结束 结束 surf(X_grip_base, Y_grip_base, Z_grip_base, 'FaceColor', [0.5 0.5 0.5], 'EdgeColor', 'none');
% 夹爪手指 finger1_start = gripper_base + gripper_y_dir * gripper_width/2; finger1_end = finger1_start + gripper_z_dir * gripper_length; finger2_start = gripper_base - gripper_y_dir * gripper_width/2; finger2_end = finger2_start + gripper_z_dir * gripper_length;
% 绘制夹爪手指 plot3([finger1_start(1), finger1_end(1)], ... [finger1_start(2), finger1_end(2)], ... [finger1_start(3), finger1_end(3)], ... 'k-', 'LineWidth', 4); plot3([finger2_start(1), finger2_end(1)], ... [finger2_start(2), finger2_end(2)], ... [finger2_start(3), finger2_end(3)], ... 'k-', 'LineWidth', 4);
% 6.添加关节标签 对于 i = 2:6 文本(joint_positions(i,1)+0.05, joint_positions(i,2)+0.05, joint_positions(i,3)+0.05, ... sprintf('J%d', i-1), 'FontSize', 10, 'BackgroundColor', '白色'); 结束
% 7.绘制工作空间参考网格(改进:根据机器人实际范围调整) % 计算合适的网格范围 grid_margin = 0.3; grid_x_min = x_range(1) - grid_margin; grid_x_max = x_range(2) + grid_margin; grid_y_min = y_range(1) - grid_margin; grid_y_max = y_range(2) + grid_margin;
[X_grid, Y_grid] = meshgrid(grid_x_min:0.2:grid_x_max, grid_y_min:0.2:grid_y_max); Z_grid = 零(大小(X_grid)) - base_height;% 将网格放在基座底部 mesh(X_grid, Y_grid, Z_grid, 'EdgeColor', [0.8 0.8 0.8], 'FaceAlpha', 0);
% 8.添加末端执行器的投影和参考线 % 绘制末端位置到地面的投影线 plot3([pt(1), pt(1)], [pt(2), pt(2)], [pt(3), -base_height], ... 'k:', 'LineWidth', 2); % 绘制地面投影点 plot3(pt(1), pt(2), -base_height, 'ko', 'MarkerSize', 8, 'MarkerFaceColor', 'k'); % 添加投影坐标标注 文本(pt(1)+0.05, pt(2)+0.05, -base_height, ... sprintf('投影(%.2f, %.2f)', pt(1), pt(2)), ... 'FontSize', 9, 'BackgroundColor', 'white');
% 设置图形属性 title('6R工业机器人三维模型 - 末端执行器坐标系', 'FontSize', 16, 'FontWeight', 'bold'); xlabel('X (m)', '字体大小', 14); ylabel('Y (m)', '字体大小', 14); zlabel('Z (m)', '字体大小', 14);
% 设置光照 照明 Gouraud; light('位置', [2 2 5], '样式', '本地'); light('位置', [-2 -2 5], '样式', '本地'); 材料暗淡;
% 设置视角(改进:根据机器人实际范围自适应调整) 轴相等; % 计算合适的显示范围 view_margin = 0.4; x_lim = [x_range(1)-view_margin, x_range(2)+view_margin]; y_lim = [y_range(1)-view_margin, y_range(2)+view_margin]; z_lim = [-0.2, z_range(2)+view_margin]; 轴([x_lim y_lim z_lim]); 视图(45, 30); 网格开启; 盒子打开;
% 添加图例说明 annotation('textbox', [0.02 0.85 0.15 0.1], ... 'String', {'图例:', '红色: 基座/末端', '蓝色: 旋转关节', '灰色: 连杆'}, ... 'BackgroundColor', 'white', 'EdgeColor', 'black', 'FontSize', 10);
% 保存高质量图像 saveas(gcf, '问题1_改进的机器人3D模型.png');
% 创建简化的函数来绘制机器人 draw_robot = @(joint_pos) draw_robot_func(joint_pos, link_radius, joint_radius, base_size, base_height, x_lim, y_lim, z_lim, Rt, pt);
% 创建侧视图 figure('Position', [100, 100, 800, 600], 'Color', 'white'); draw_robot(joint_positions); title('6R工业机器人 - 侧视图', 'FontSize', 16); xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); axis equal; grid on; view(90, 0); % 侧视图 lighting gouraud; light('Position', [2 2 5], 'Style', 'local'); material dull; axis([x_lim y_lim z_lim]); saveas(gcf, '问题1_机器人侧视图.png');
% 创建俯视图 figure('Position', [100, 100, 800, 600], 'Color', 'white'); draw_robot(joint_positions); title('6R工业机器人 - 俯视图', 'FontSize', 16); xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)'); axis equal; grid on; view(0, 90); % 俯视图 lighting gouraud; light('Position', [0 0 5], 'Style', 'local'); material dull; axis([x_lim y_lim z_lim]); saveas(gcf, '问题1_机器人俯视图.png');
% 保存结果到文本文件 fileID = fopen('问题1_计算结果.txt', 'w'); fprintf(fileID, '6R机器人正向运动学计算结果\n'); fprintf(fileID, '=====================================\n\n'); fprintf(fileID, 'D-H参数:\n'); fprintf(fileID, 'h = %.1f, e = %.1f, f = %.1f, d = %.1f\n\n', h, e, f, d_param); fprintf(fileID, '关节角度 q (度):\n'); for i = 1:6 fprintf(fileID, 'q%d = %.2f°\n', i, q(i)*180/pi); end fprintf(fileID, '\n末端执行器位置 pt:\n'); fprintf(fileID, 'x = %.4f\n', pt(1)); fprintf(fileID, 'y = %.4f\n', pt(2)); fprintf(fileID, 'z = %.4f\n', pt(3)); fprintf(fileID, '\n末端执行器旋转矩阵 Rt:\n'); for i = 1:3 fprintf(fileID, '[%.4f %.4f %.4f]\n', Rt(i,1), Rt(i,2), Rt(i,3)); end fprintf(fileID, '\n验证:\n'); fprintf(fileID, 'det(Rt) = %.6f\n', det(Rt)); fprintf(fileID, '||Rt * Rt'' - I|| = %.6e\n', norm(Rt * Rt' - eye(3))); fclose(fileID);
fprintf('\n计算完成!结果已保存到文件。\n'); fprintf('生成的图像文件:\n'); fprintf('- 问题1_改进的机器人3D模型.png\n'); fprintf('- 问题1_机器人侧视图.png\n'); fprintf('- 问题1_机器人俯视图.png\n'); fprintf('- 问题1_计算结果.txt\n');
% 定义绘制机器人的辅助函数 function draw_robot_func(joint_positions, link_radius, joint_radius, base_size, base_height, x_lim, y_lim, z_lim, Rt, pt) % 绘制基座 [X_base, Y_base, Z_base] = cylinder(base_size, 20); Z_base = Z_base * base_height - base_height; surf(X_base, Y_base, Z_base, 'FaceColor', [0.3 0.3 0.3], 'EdgeColor', 'none'); hold on;
% 添加基座底板 fill3(X_base(1,:), Y_base(1,:), Z_base(1,:), [0.2 0.2 0.2]);
% 绘制连杆 for i = 1:6 start_pos = joint_positions(i, :); end_pos = joint_positions(i+1, :);
% 计算连杆方向和长度
link_vec = end_pos - start_pos;
link_length = norm(link_vec);
if link_length > 0.01 % 只绘制有长度的连杆
% 创建圆柱体
[X_cyl, Y_cyl, Z_cyl] = cylinder(link_radius, 20);
Z_cyl = Z_cyl * link_length;
% 计算旋转矩阵,使圆柱体对齐连杆方向
if link_length > 0
z_axis = link_vec(:) / link_length; % 确保是列向量
% 找到一个垂直于z_axis的向量
if abs(z_axis(3)) < 0.9
x_axis = cross([0; 0; 1], z_axis);
else
x_axis = cross([1; 0; 0], z_axis);
end
x_axis = x_axis / norm(x_axis);
y_axis = cross(z_axis, x_axis);
R_link = [x_axis(:), y_axis(:), z_axis(:)]; % 确保每个都是列向量
% 变换圆柱体顶点
for j = 1:size(X_cyl, 2)
for k = 1:size(X_cyl, 1)
point = R_link * [X_cyl(k,j); Y_cyl(k,j); Z_cyl(k,j)] + start_pos(:);
X_cyl(k,j) = point(1);
Y_cyl(k,j) = point(2);
Z_cyl(k,j) = point(3);
end
end
% 绘制连杆
surf(X_cyl, Y_cyl, Z_cyl, 'FaceColor', [0.7 0.7 0.7], 'EdgeColor', 'none');
end
end
end
% 绘制关节 for i = 1:7 [X_sphere, Y_sphere, Z_sphere] = sphere(20); X_sphere = X_sphere * joint_radius + joint_positions(i, 1); Y_sphere = Y_sphere * joint_radius + joint_positions(i, 2); Z_sphere = Z_sphere * joint_radius + joint_positions(i, 3);
% 根据关节类型选择颜色
if i == 1 || i == 7
joint_color = [0.8 0.2 0.2]; % 基座和末端为红色
else
joint_color = [0.2 0.2 0.8]; % 其他关节为蓝色
end
surf(X_sphere, Y_sphere, Z_sphere, 'FaceColor', joint_color, 'EdgeColor', 'none');
end
% 绘制末端执行器坐标系 origin = pt; axis_scale = 0.2; axis_width = 3;
% X轴 - 红色 quiver3(origin(1), origin(2), origin(3), ... Rt(1,1)*axis_scale, Rt(2,1)*axis_scale, Rt(3,1)*axis_scale, ... 'r', 'LineWidth', axis_width, 'MaxHeadSize', 0.5); % Y轴 - 绿色 quiver3(origin(1), origin(2), origin(3), ... Rt(1,2)*axis_scale, Rt(2,2)*axis_scale, Rt(3,2)*axis_scale, ... 'g', 'LineWidth', axis_width, 'MaxHeadSize', 0.5); % Z轴 - 蓝色 quiver3(origin(1), origin(2), origin(3), ... Rt(1,3)*axis_scale, Rt(2,3)*axis_scale, Rt(3,3)*axis_scale, ... 'b', 'LineWidth', axis_width, 'MaxHeadSize', 0.5);
% 绘制工作空间参考网格 grid_margin = 0.3; grid_x_min = x_lim(1) + grid_margin; grid_x_max = x_lim(2) - grid_margin; grid_y_min = y_lim(1) + grid_margin; grid_y_max = y_lim(2) - grid_margin;
[X_grid, Y_grid] = meshgrid(grid_x_min:0.2:grid_x_max, grid_y_min:0.2:grid_y_max); Z_grid = zeros(size(X_grid)) - base_height; mesh(X_grid, Y_grid, Z_grid, 'EdgeColor', [0.8 0.8 0.8], 'FaceAlpha', 0); end
% 额外创建一个紧凑视图,更好地展示机器人结构 figure('位置', [100, 100, 1000, 800], '颜色', '白色');
% 重新绘制机器人(使用相同的参数) draw_robot(joint_positions);
% 添加更多细节 % 绘制末端执行器的工作点 plot3(pt(1), pt(2), pt(3), 'ko', 'MarkerSize', 10, 'MarkerFaceColor', 'yellow'); 文本(pt(1)+0.1, pt(2)+0.1, pt(3)+0.15, sprintf('末端位置\n(%.2f, %.2f, %.2f)', pt(1), pt(2), pt(3)), ... 'FontSize', 10, 'BackgroundColor', 'yellow', 'EdgeColor', 'black', 'HorizontalAlignment', 'center');
% 绘制从基座到末端的轨迹线(虚线) plot3(joint_positions(:,1), joint_positions(:,2), joint_positions(:,3), ... 'k--', 'LineWidth', 1.5, '颜色', [0.5 0.5 0.5]);
% 绘制末端执行器的方向指示器(大箭头) arrow_scale = 0.3; arrow_origin = pt; arrow_end = pt + rt(:,3) * arrow_scale;% 沿Z轴方向 % 使用patch创建3D箭头 [cone_x, cone_y, cone_z] = 圆柱体([0.03 0], 20); cone_z = cone_z * 0.1; % 变换锥体到正确位置 对于 j = 1:size(cone_x, 2) 对于 k = 1:size(cone_x, 1) 点 = Rt * [cone_x(k,j);cone_y(k,j);cone_z(k,j)] + arrow_end - Rt(:,3)*0.1; cone_x(k,j) = 点 (1); cone_y(k,j) = 点(2); cone_z(k,j) = 点 (3); 结束 结束 surf(cone_x, cone_y, cone_z, 'FaceColor', [1 0.5 0], 'EdgeColor', 'none');
% 绘制箭头杆 plot3([arrow_origin(1), arrow_end(1)-Rt(1,3)*0.1], ... [arrow_origin(2), arrow_end(2)-Rt(2,3)*0.1], ... [arrow_origin(3), arrow_end(3)-rt(3,3)*0.1], ... 'Color', [1 0.5 0], 'LineWidth', 4);
% 添加工具方向标签 text(arrow_end(1), arrow_end(2), arrow_end(3)+0.05, '工具方向', ... 'FontSize', 10, 'FontWeight', 'bold', 'HorizontalAlignment', 'center');
% 设置紧凑的视角 title('6R工业机器人三维模型 - 紧凑视图', 'FontSize', 16, 'FontWeight', 'bold'); xlabel('X (m)', '字体大小', 14); ylabel('Y (m)', '字体大小', 14); zlabel('Z (m)', '字体大小', 14);
% 设置光照 照明 Gouraud; light('位置', [2 2 5], '样式', '本地'); light('位置', [-2 -2 5], '样式', '本地'); 材料暗淡;
% 使用更紧凑的显示范围 轴相等; 轴([x_lim y_lim z_lim]); 视图(35, 25);% 稍微调整视角 网格开启; 盒子打开;
% 添加更详细的图例 legend_str = { '图例:', '━━━━━━━━━━', '● 红色球: 基座/末端', '● 蓝色球: 旋转关节', '▬ 灰色柱: 连杆', '--- 虚线: 关节轨迹', '★ 黄色: 末端位置', '→ 橙色: 工具方向', 'XYZ: 坐标系' }; annotation('TextBox', [0.02 0.70 0.18 0.20], ... '字符串', legend_str, ... 'BackgroundColor', 'white', 'EdgeColor', 'black', 'FontSize', 9, ... 'FontName', 'FixedWidth');
% 添加机器人参数信息 param_str = { '机器人参数:', '━━━━━━━━━━', sprintf('h = %.1f m', h), sprintf('e = %.1f m', e), sprintf('f = %.1f m', f), sprintf('d = %.1f m', d_param) }; annotation('textbox', [0.82 0.75 0.16 0.15], ... '字符串', param_str, ... 'BackgroundColor', 'white', 'EdgeColor', 'black', 'FontSize', 9, ... 'FontName', 'FixedWidth');
% 添加末端执行器姿态信息 euler_angles = rotm2eul(Rt, 'ZYX') * 180/pi;% 转换为欧拉角(度) pose_str = { '末端姿态:', '━━━━━━━━━━', sprintf('X: %.2f m', pt(1)), sprintf('Y: %.2f m', pt(2)), sprintf('Z: %.2f m', pt(3)), '━━━━━━━━━━', sprintf('Rx: %.1f°', euler_angles(3)), sprintf('Ry: %.1f°', euler_angles(2)), sprintf('Rz: %.1f°', euler_angles(1)) }; annotation('textbox', [0.82 0.45 0.16 0.25], ... '字符串', pose_str, ... 'BackgroundColor', 'white', 'EdgeColor', 'black', 'FontSize', 9, ... 'FontName', 'FixedWidth');
saveas(gcf, '问题1_机器人紧凑视图.png');
% 创建专门的末端执行器细节视图 figure('位置', [100, 100, 800, 800], '颜色', '白色');
% 绘制末端执行器局部放大图 子情节(2,2,[1,3]); 坚持;
% 绘制末端关节 [X_sphere, Y_sphere, Z_sphere] = 球体 (30); X_sphere = X_sphere * joint_radius * 1.5 + pt(1); Y_sphere = Y_sphere * joint_radius * 1.5 + pt(2); Z_sphere = Z_sphere * joint_radius * 1.5 + pt(3); surf(X_sphere, Y_sphere, Z_sphere, 'FaceColor', [0.8 0.2 0.2], 'EdgeColor', 'none');
% 绘制末端执行器坐标系(更大更清晰) axis_scale = 0.3; 原点 = pt;
% X轴 - 红色 quiver3(原点(1), 原点(2), 原点(3), ... Rt(1,1)*axis_scale, Rt(2,1)*axis_scale, Rt(3,1)*axis_scale, ... 'r', 'LineWidth', 5, 'MaxHeadSize', 0.8); 文本(origin(1) + Rt(1,1)axis_scale1.2, ... 原点(2) + Rt(2,1)axis_scale1.2, ... origin(3) + Rt(3,1)axis_scale1.2, 'X_6', ... 'FontSize', 14, 'FontWeight', '粗体', '颜色', 'r');
% Y轴 - 绿色 quiver3(原点(1), 原点(2), 原点(3), ... Rt(1,2)*axis_scale, Rt(2,2)*axis_scale, Rt(3,2)*axis_scale, ... 'g', 'LineWidth', 5, 'MaxHeadSize', 0.8); 文本(origin(1) + Rt(1,2)axis_scale1.2, ... origin(2) + Rt(2,2)axis_scale1.2, ... origin(3) + Rt(3,2)axis_scale1.2, 'Y_6', ... 'FontSize', 14, 'FontWeight', '粗体', '颜色', 'g');
% Z轴 - 蓝色 quiver3(原点(1), 原点(2), 原点(3), ... Rt(1,3)*axis_scale, Rt(2,3)*axis_scale, Rt(3,3)*axis_scale, ... 'b', 'LineWidth', 5, 'MaxHeadSize', 0.8); 文本(origin(1) + Rt(1,3)axis_scale1.2, ... origin(2) + Rt(2,3)axis_scale1.2, ... origin(3) + Rt(3,3)axis_scale1.2, 'Z_6', ... 'FontSize', 14, 'FontWeight', '粗体', '颜色', 'b');
% 绘制参考坐标系(基座坐标系) ref_scale = 0.15; ref_origin = pt - [0.3; 0.3; 0.3]; 箭袋3(ref_origin(1), ref_origin(2), ref_origin(3), ref_scale, 0, 0, ... 'r', 'LineWidth', 2, 'LineStyle', '--', 'MaxHeadSize', 0.5); 箭袋3(ref_origin(1), ref_origin(2), ref_origin(3), 0, ref_scale, 0, ... 'g', 'LineWidth', 2, 'LineStyle', '--', 'MaxHeadSize', 0.5); 箭袋3(ref_origin(1), ref_origin(2), ref_origin(3), 0, 0, ref_scale, ... 'b', 'LineWidth', 2, 'LineStyle', '--', 'MaxHeadSize', 0.5); text(ref_origin(1)-0.05, ref_origin(2)-0.05, ref_origin(3)-0.05, '基座坐标系', ... 'FontSize', 10, 'BackgroundColor', 'white');
% 设置视图 title('末端执行器坐标系细节', 'FontSize', 14, 'FontWeight', '粗体'); xlabel('X (m)');ylabel('Y (m)');zlabel('Z (m)'); 轴相等; 网格开启; 视图(45, 30); 照明 Gouraud; light('位置', [2 2 5], '样式', '本地'); 材料暗淡; % 设置局部视图范围 轴([Pt(1)-0.4 Pt(1)+0.4 Pt(2)-0.4 Pt(2)+0.4 Pt(3)-0.4 Pt(3)+0.4]);
% 添加旋转矩阵显示 子情节(2,2,2); 轴关闭; text(0.1, 0.9, '末端执行器旋转矩阵 R_6^0:', 'FontSize', 12, 'FontWeight', 'bold'); matrix_str = sprintf(['[%6.3f %6.3f %6.3f]\n' ... '[%6.3f %6.3f %6.3f]\n' ... '[%6.3f %6.3f %6.3f]'], ... Rt(1,1), Rt(1,2), Rt(1,3), ... Rt(2,1), Rt(2,2), Rt(2,3), ... Rt(3,1), Rt(3,2), Rt(3,3)); 文本(0.1, 0.5, matrix_str, 'FontSize', 11, 'FontName', 'FixedWidth');
% 添加位置向量显示 子情节(2,2,4); 轴关闭; 文本(0.1, 0.9, '末端执行器位置 p_6^0:', 'FontSize', 12, 'FontWeight', '粗体'); pos_str = sprintf(['[%6.3f]\n' ... '[%6.3f]\n' ... '[%6.3f]'], pt(1), pt(2), pt(3)); 文本(0.1, 0.6, pos_str, 'FontSize', 11, 'FontName', 'FixedWidth');
% 添加欧拉角信息 text(0.1, 0.3, sprintf('欧拉角 (ZYX):\nRx: %.1f°\nRy: %.1f°\nRz: %.1f°', ... euler_angles(3), euler_angles(2), euler_angles(1)), ... 'FontSize', 11);
saveas(gcf, '问题1_末端执行器细节.png');
fprintf('\n额外生成了视图:\n'); fprintf('- 问题1_机器人紧凑视图.png\n'); fprintf('- 问题1_末端执行器细节.png\n');