活动介绍
file-type

Matlab动画制作:绘制动态轨迹图像

版权申诉
1KB | 更新于2024-12-14 | 46 浏览量 | 0 下载量 举报 收藏
download 限时特惠:#14.90
1. MATLAB简介 MATLAB是一种高性能的数值计算环境和第四代编程语言,广泛应用于工程计算、数据分析、算法开发和仿真等领域。它提供了丰富的内置函数和工具箱,用于解决从简单的数学问题到复杂的工程问题。 2. MATLAB中的动画绘制 在MATLAB中,可以通过编程的方式绘制动画图像。动画是通过连续绘制一系列静态图像来形成的动态效果。MATLAB提供了多种函数来实现动画效果,比如plot、line、patch等,以及更高级的函数如getframe和movie等。 3. 动态图像与动态轨迹 动态图像通常指的是随时间变化的图像序列,这些图像序列可以表示时间序列数据,例如物理现象、化学反应过程、金融数据的变化等。动态轨迹特指在动态图像中表示对象移动路径的图像,通常用在数学建模、物理模拟、生物运动分析等领域。 4. 半径与圆的动画绘制 在绘制动态图像时,半径和圆的概念经常被用来表示特定的几何关系或物理运动。例如,可以使用MATLAB来模拟行星绕太阳旋转的轨迹(圆周运动),其中半径表示行星与太阳之间的距离,圆则表示行星运动的轨迹路径。这类动态图像通常涉及到极坐标系中的运动模拟。 5. MATLAB的动画应用 在MATLAB中,动画不仅可以用于教育和演示目的,还可以在科研中用于模拟实验结果。例如,在力学领域,可以模拟物体在各种力的作用下的运动轨迹;在生物学领域,可以模拟细胞分裂或动物迁徙的动态过程;在经济学中,可以模拟股票价格的波动。 6. MATLAB中的相关函数和命令 - plot:绘制二维图形; - line:在坐标轴上添加线段; - patch:绘制填充多边形; - getframe:捕获图形窗口的当前帧; - movie:播放图形序列作为动画; - comet:以尾巴拖动的方式绘制点的轨迹,使轨迹更加明显; - polarplot:在极坐标中绘制数据。 - hold on:保持当前图形,用于在同一坐标轴上绘制多个图形。 7. 使用MATLAB绘制动画的步骤 a) 初始化:设置图形窗口和参数。 b) 循环:通过循环结构实现动画效果,每次循环中计算新的数据点。 c) 绘图:使用MATLAB的绘图命令将新的数据点绘制在图形上。 d) 更新:更新图形窗口,显示新的图形状态。 e) 控制:提供暂停、继续、停止等控制按钮,以便用户操作动画播放。 8. 注意事项 在进行MATLAB动画绘制时,需要合理设置帧率(即每秒绘制的帧数),过低可能导致动画不流畅,过高则可能会导致CPU使用率过高。此外,绘制动画时应注意数据的更新和图形的刷新,避免出现画面闪烁等问题。 9. 结论 MATLAB提供了一个强大的平台,用于在科学和工程领域中创建和分析动态图像。通过理解如何使用MATLAB绘制动画,可以更有效地展示数据变化和模型行为,为科研、教育和演示提供有力的支持。

相关推荐

filetype

% 反射镜系统光路轨迹分析(增强版) clear; clc; close all; % ========== 参数设置 ========== mirror_radius = 20; % 圆形镜子半径 N = 120; % 动画帧数 ray_length = 100; % 出射光线延伸长度 (cm) plot_trajectory = true; % 是否绘制轨迹 save_animation = false; % 是否保存动画为GIF % ========== 用户可修改的偏差参数 (单位:度) ========== deviation_angles = [ 0.5, -0.5; % M1 偏差 -0.5, 0.5; % M2 偏差 0.5, 0.5; % M3 偏差 -0.5, -0.5; % M4 偏差 0.0, 0.0 % M5 偏差 ]; % ========== 初始光路关键点 ========== M1 = [0, 0, 0]; % 镜子1中心 M2 = [0, 0, 100]; % 镜子2中心 M3 = [100, 0, 100]; % 镜子3中心 M4 = [100, 0, 200]; % 镜子4中心 M5 = [0, 0, 200]; % 镜子5中心 start_pt = [-100, 0, 0]; % 光线起始点 % ========== 旋转矩阵函数 ========== Rx = @(theta) [1, 0, 0; 0, cosd(theta), -sind(theta); 0, sind(theta), cosd(theta)]; Ry = @(theta) [cosd(theta), 0, sind(theta); 0, 1, 0; -sind(theta), 0, cosd(theta)]; Rz = @(theta) [cosd(theta), -sind(theta), 0; sind(theta), cosd(theta), 0; 0, 0, 1]; % ========== 应用偏差到法向量 ========== M1_n0 = [-sqrt(2)/2, 0, sqrt(2)/2]‘; M2_n0 = [-sqrt(2)/2, 0, sqrt(2)/2]’; M3_n0 = [-sqrt(2)/2, 0, sqrt(2)/2]‘; M4_n0 = [sqrt(2)/2, 0, sqrt(2)/2]’; M5_n0 = [sqrt(2)/2, 0, sqrt(2)/2]'; M1_n = apply_deviation(M1_n0, deviation_angles(1,:)); M2_n = apply_deviation(M2_n0, deviation_angles(2,:)); M3_n = apply_deviation(M3_n0, deviation_angles(3,:)); M4_n = apply_deviation(M4_n0, deviation_angles(4,:)); M5_n = apply_deviation(M5_n0, deviation_angles(5,:)); % ====================================================================== % 图1:镜子5绕x轴旋转(带偏差) % ====================================================================== fig1 = figure(‘Position’, [50, 100, 1000, 800], ‘Name’, ‘镜子5绕x轴旋转(带偏差)’); hold on; grid on; axis equal; view(45, 30); xlabel(‘X (cm)’); ylabel(‘Y (cm)’); zlabel(‘Z (cm)’); title(‘镜子5绕x轴旋转 - 带偏差光路轨迹’,‘FontSize’,16,‘FontWeight’,‘bold’); % 绘制固定元素 draw_mirror_circle(M1, M1_n, mirror_radius, [0.8, 0.8, 0.9], ‘M1’); draw_mirror_circle(M2, M2_n, mirror_radius, [0.9, 0.8, 0.8], ‘M2’); draw_mirror_circle(M3, M3_n, mirror_radius, [0.8, 0.9, 0.8], ‘M3’); draw_mirror_circle(M4, M4_n, mirror_radius, [0.9, 0.8, 0.9], ‘M4’); % 绘制旋转轴和轨迹平面 plot3([-50, 50], [0, 0], [200, 200], ‘m–’, ‘LineWidth’, 2); text(50, 0, 200, ‘旋转轴 (x轴)’, ‘FontSize’, 12, ‘Color’, ‘m’); [Y, Z] = meshgrid(linspace(-100,100,10), linspace(100,300,10)); surf(zeros(size(Y)), Y, Z, ‘FaceAlpha’, 0.1, ‘EdgeColor’, ‘k’, ‘FaceColor’, ‘cyan’); text(0, 0, 100, ‘轨迹平面 (yz平面)’, ‘FontSize’, 12, ‘Color’, ‘b’); % 计算初始光路 [path, reflection_pts, final_spot] = calculate_light_path(start_pt, [M1; M2; M3; M4; M5], … [M1_n, M2_n, M3_n, M4_n, M5_n], ray_length); plot_light_path(path, reflection_pts, ‘r-b-g-m-c’, 2); % 初始化轨迹存储 phi_range = linspace(0, 2*pi, N); circle5 = zeros(N, 3); % 光斑轨迹 % 初始状态绘制 draw_mirror_circle(M5, M5_n, mirror_radius, [1, 0.6, 0.6], ‘M5’); plot3(final_spot(1), final_spot(2), final_spot(3), ‘go’, ‘MarkerSize’, 8, ‘MarkerFaceColor’, ‘g’); % 动画展示 if save_animation filename1 = ‘rotation_mode1.gif’; end for i = 1:2:N % 删除动态元素 delete(findobj(gca, ‘Tag’, ‘moving_point’)); delete(findobj(gca, ‘Tag’, ‘rotating_mirror’)); delete(findobj(gca, ‘Tag’, ‘current_light_path’)); phi = phi_range(i); n5_rot = Rx(rad2deg(phi)) * M5_n; % 绘制旋转后的镜子 draw_mirror_circle(M5, n5_rot, mirror_radius, [1, 0.6, 0.6], 'rotating_mirror'); % 计算当前光路 [current_path, current_reflection_pts, current_spot] = calculate_light_path(start_pt, [M1; M2; M3; M4; M5], ... [M1_n, M2_n, M3_n, M4_n, n5_rot], ray_length); circle5(i, :) = current_spot; % 绘制当前光斑 plot3(current_spot(1), current_spot(2), current_spot(3), 'o', ... 'MarkerSize', 12, 'MarkerFaceColor', 'g', 'Tag', 'moving_point'); % 绘制当前光路 plot_light_path(current_path, current_reflection_pts, 'r-b-g-m-c', 2, 'current_light_path'); % 捕获帧用于GIF if save_animation frame = getframe(fig1); im = frame2im(frame); [imind, cm] = rgb2ind(im, 256); if i == 1 imwrite(imind, cm, filename1, 'gif', 'Loopcount', inf, 'DelayTime', 0.1); else imwrite(imind, cm, filename1, 'gif', 'WriteMode', 'append', 'DelayTime', 0.1); end end pause(0.05); end % 绘制光斑轨迹 if plot_trajectory plot3(circle5(:,1), circle5(:,2), circle5(:,3), ‘b-’, ‘LineWidth’, 2); % 轨迹分析 trajectory_analysis(circle5, '镜子5旋转轨迹分析'); end % 添加图例和偏差信息 legend(‘M1’, ‘M2’, ‘M3’, ‘M4’, ‘旋转轴’, ‘轨迹平面’, ‘初始光路’, ‘M5’, ‘初始光斑’, … ‘旋转镜子’, ‘当前光斑’, ‘当前光路’, ‘光斑轨迹’, ‘Location’, ‘eastoutside’); deviation_text = sprintf(‘镜子偏差:\nM1: Yaw=%.1f°, Pitch=%.1f°\nM2: Yaw=%.1f°, Pitch=%.1f°\nM3: Yaw=%.1f°, Pitch=%.1f°\nM4: Yaw=%.1f°, Pitch=%.1f°\nM5: Yaw=%.1f°, Pitch=%.1f°’, … deviation_angles(1,1), deviation_angles(1,2), … deviation_angles(2,1), deviation_angles(2,2), … deviation_angles(3,1), deviation_angles(3,2), … deviation_angles(4,1), deviation_angles(4,2), … deviation_angles(5,1), deviation_angles(5,2)); annotation(‘textbox’, [0.05, 0.7, 0.2, 0.2], ‘String’, deviation_text, … ‘BackgroundColor’, ‘white’, ‘FontSize’, 10); axis([-150 150 -150 150 0 300]); % ====================================================================== % 图2:镜子2-5整体绕z轴旋转(带偏差) % ====================================================================== fig2 = figure(‘Position’, [900, 100, 1000, 800], ‘Name’, ‘整体绕z轴旋转(带偏差)’); hold on; grid on; axis equal; view(45, 30); xlabel(‘X (cm)’); ylabel(‘Y (cm)’); zlabel(‘Z (cm)’); title(‘镜子2-5整体绕z轴旋转 - 带偏差光路轨迹’,‘FontSize’,16,‘FontWeight’,‘bold’); % 绘制固定元素 draw_mirror_circle(M1, M1_n, mirror_radius, [0.8, 0.8, 0.9], ‘M1’); % 绘制旋转轴和光屏 plot3([0, 0], [0, 0], [-50, 300], ‘m–’, ‘LineWidth’, 2); text(0, 0, 300, ‘旋转轴 (z轴)’, ‘FontSize’, 12, ‘Color’, ‘m’); [X, Y] = meshgrid(linspace(-150,150,10), linspace(-150,150,10)); surf(X, Y, 300*ones(size(X)), ‘FaceAlpha’, 0.1, ‘EdgeColor’, ‘k’, ‘FaceColor’, ‘cyan’); rectangle = [-100, -100, 300; 100, -100, 300; 100, 100, 300; -100, 100, 300]; patch(‘Vertices’, rectangle, ‘Faces’, [1,2,3,4], … ‘FaceColor’, ‘cyan’, ‘FaceAlpha’, 0.1, ‘EdgeColor’, ‘b’); text(0, 0, 310, ‘光屏平面 (z=300)’, ‘FontSize’, 12, ‘Color’, ‘b’); % 初始化轨迹存储 theta_range = linspace(0, 2*pi, N); final_spots = zeros(N, 3); % 光斑轨迹 screen_hits = false(N, 1); % 记录是否击中光屏 % 动画展示 if save_animation filename2 = ‘rotation_mode2.gif’; end for i = 1:2:N % 删除动态元素 delete(findobj(gca, ‘Tag’, ‘rotating_mirrors’)); delete(findobj(gca, ‘Tag’, ‘current_light_path’)); delete(findobj(gca, ‘Tag’, ‘final_spot’)); theta = theta_range(i); % 计算旋转后的位置和法向量 R_z = [cos(theta), -sin(theta), 0; sin(theta), cos(theta), 0; 0, 0, 1]; M2_rot = (R_z * (M2 - M1)')' + M1; M3_rot = (R_z * (M3 - M1)')' + M1; M4_rot = (R_z * (M4 - M1)')' + M1; M5_rot = (R_z * (M5 - M1)')' + M1; M2_n_rot = R_z * M2_n; M3_n_rot = R_z * M3_n; M4_n_rot = R_z * M4_n; M5_n_rot = R_z * M5_n; % 绘制旋转的镜子 draw_mirror_circle(M2_rot, M2_n_rot, mirror_radius, [0.9, 0.8, 0.8], 'rotating_mirrors'); draw_mirror_circle(M3_rot, M3_n_rot, mirror_radius, [0.8, 0.9, 0.8], 'rotating_mirrors'); draw_mirror_circle(M4_rot, M4_n_rot, mirror_radius, [0.9, 0.8, 0.9], 'rotating_mirrors'); draw_mirror_circle(M5_rot, M5_n_rot, mirror_radius, [1, 0.6, 0.6], 'rotating_mirrors'); % 计算当前光路 mirror_positions = [M1; M2_rot; M3_rot; M4_rot; M5_rot]; mirror_normals = [M1_n, M2_n_rot, M3_n_rot, M4_n_rot, M5_n_rot]; [path, reflection_pts, final_spot] = calculate_light_path(start_pt, mirror_positions, mirror_normals, ray_length); final_spots(i, :) = final_spot; % 检查是否击中光屏 screen_hits(i) = (abs(final_spot(1)) <= 100 && abs(final_spot(2)) <= 100 && abs(final_spot(3) - 300) < 1); % 绘制当前光路 plot_light_path(path, reflection_pts, 'r-b-g-m-c', 2, 'current_light_path'); % 绘制最终光斑(根据是否击中光屏使用不同颜色) if screen_hits(i) plot3(final_spot(1), final_spot(2), final_spot(3), 'go', ... 'MarkerSize', 10, 'MarkerFaceColor', 'g', 'Tag', 'final_spot'); else plot3(final_spot(1), final_spot(2), final_spot(3), 'ro', ... 'MarkerSize', 10, 'MarkerFaceColor', 'r', 'Tag', 'final_spot'); end % 捕获帧用于GIF if save_animation frame = getframe(fig2); im = frame2im(frame); [imind, cm] = rgb2ind(im, 256); if i == 1 imwrite(imind, cm, filename2, 'gif', 'Loopcount', inf, 'DelayTime', 0.1); else imwrite(imind, cm, filename2, 'gif', 'WriteMode', 'append', 'DelayTime', 0.1); end end pause(0.05); end % 绘制光斑轨迹 if plot_trajectory % 分离击中光屏和未击中的点 hit_indices = find(screen_hits); miss_indices = find(~screen_hits); % 绘制击中光屏的轨迹 if ~isempty(hit_indices) plot3(final_spots(hit_indices,1), final_spots(hit_indices,2), final_spots(hit_indices,3), 'g-', 'LineWidth', 2); end % 绘制未击中光屏的轨迹 if ~isempty(miss_indices) plot3(final_spots(miss_indices,1), final_spots(miss_indices,2), final_spots(miss_indices,3), 'r-', 'LineWidth', 2); end % 轨迹分析 trajectory_analysis(final_spots, '整体旋转轨迹分析'); % 显示击中率 hit_rate = sum(screen_hits) / N * 100; text(0, 0, 320, sprintf('光屏击中率: %.1f%%', hit_rate), 'FontSize', 12, 'Color', 'b', 'HorizontalAlignment', 'center'); end % 添加图例 legend(‘M1’, ‘旋转轴’, ‘光屏平面’, ‘光屏边界’, ‘M2’, ‘M3’, ‘M4’, ‘M5’, … ‘当前光路’, ‘击中光屏’, ‘未击中光屏’, ‘光斑轨迹(击中)’, ‘光斑轨迹(未击中)’, ‘Location’, ‘eastoutside’); annotation(‘textbox’, [0.05, 0.7, 0.2, 0.2], ‘String’, deviation_text, … ‘BackgroundColor’, ‘white’, ‘FontSize’, 10); plot3([0, 0], [0, 0], [290, 310], ‘c-’, ‘LineWidth’, 2); text(0, 0, 310, ‘光屏平面 (z=300)’, ‘FontSize’, 12, ‘Color’, ‘b’); axis([-150 150 -150 150 0 350]); % ========== 增强功能:轨迹分析 ========== function trajectory_analysis(points, title_str) % 轨迹分析函数 figure(‘Name’, title_str); subplot(2,2,1); plot(points(:,1), points(:,2), ‘b.’); title(‘XY平面投影’); xlabel(‘X’); ylabel(‘Y’); axis equal; subplot(2,2,2); plot(points(:,1), points(:,3), 'r.'); title('XZ平面投影'); xlabel('X'); ylabel('Z'); axis equal; subplot(2,2,3); plot(points(:,2), points(:,3), 'g.'); title('YZ平面投影'); xlabel('Y'); ylabel('Z'); axis equal; subplot(2,2,4); plot3(points(:,1), points(:,2), points(:,3), 'm-'); title('3D轨迹'); xlabel('X'); ylabel('Y'); zlabel('Z'); grid on; axis equal; sgtitle(title_str, 'FontSize', 14, 'FontWeight', 'bold'); % 计算轨迹范围 x_range = max(points(:,1)) - min(points(:,1)); y_range = max(points(:,2)) - min(points(:,2)); z_range = max(points(:,3)) - min(points(:,3)); fprintf('\n%s:\n', title_str); fprintf('X方向范围: %.2f cm\n', x_range); fprintf('Y方向范围: %.2f cm\n', y_range); fprintf('Z方向范围: %.2f cm\n', z_range); fprintf('轨迹中心: (%.2f, %.2f, %.2f)\n', mean(points(:,1)), mean(points(:,2)), mean(points(:,3))); end % ========== 辅助函数 ========== function n_dev = apply_deviation(n0, angles) % 应用水平和俯仰偏差到法向量 Ry = @(theta) [cosd(theta), 0, sind(theta); 0, 1, 0; -sind(theta), 0, cosd(theta)]; Rx = @(theta) [1, 0, 0; 0, cosd(theta), -sind(theta); 0, sind(theta), cosd(theta)]; n_dev = Rx(angles(2)) * Ry(angles(1)) * n0; n_dev = n_dev / norm(n_dev); end function [path, reflection_pts, final_spot] = calculate_light_path(start_pt, mirror_positions, mirror_normals, ray_length) path = start_pt; reflection_pts = zeros(5, 3); current_point = start_pt; incident_dir = (mirror_positions(1,:) - current_point)'; incident_dir = incident_dir / norm(incident_dir); for i = 1:5 mirror_center = mirror_positions(i,:)'; mirror_normal = mirror_normals(:,i); denom = dot(incident_dir, mirror_normal); if abs(denom) < 1e-10 t = 1e10; else t = dot(mirror_center - current_point', mirror_normal) / denom; end reflection_point = current_point' + t * incident_dir; reflection_pts(i,:) = reflection_point'; path = [path; reflection_point']; reflection_dir = incident_dir - 2 * dot(incident_dir, mirror_normal) * mirror_normal; reflection_dir = reflection_dir / norm(reflection_dir); current_point = reflection_point'; incident_dir = reflection_dir; end final_spot = reflection_pts(5,:) + reflection_dir' * ray_length; path = [path; final_spot]; end function draw_mirror_circle(center, normal, radius, color, tag) theta = linspace(0, 2pi, 100); v = null(normal’); points = repmat(center, 100, 1) + radius(cos(theta)‘*v(:,1)’ + sin(theta)‘*v(:,2)’); patch('Vertices', points, 'Faces', 1:100, ... 'FaceColor', color, 'EdgeColor', 'k', ... 'FaceAlpha', 0.7, 'LineWidth', 1.5, 'Tag', tag); end function plot_light_path(path, reflection_pts, colors, width, tag) if nargin < 5, tag = ‘’; end color_list = strsplit(colors, ‘-’); plot3([path(1,1), reflection_pts(1,1)], ... [path(1,2), reflection_pts(1,2)], ... [path(1,3), reflection_pts(1,3)], ... [color_list{1}, '-'], 'LineWidth', width, 'Tag', tag); for i = 1:4 plot3([reflection_pts(i,1), reflection_pts(i+1,1)], ... [reflection_pts(i,2), reflection_pts(i+1,2)], ... [reflection_pts(i,3), reflection_pts(i+1,3)], ... [color_list{i+1}, '-'], 'LineWidth', width, 'Tag', tag); end plot3([reflection_pts(5,1), path(end,1)], ... [reflection_pts(5,2), path(end,2)], ... [reflection_pts(5,3), path(end,3)], ... [color_list{end}, '-'], 'LineWidth', width, 'Tag', tag); end 报错 镜子5旋转轨迹分析: X方向范围: 49.58 cm Y方向范围: 199.92 cm Z方向范围: 283.39 cm 轨迹中心: (-0.21, 1.67, 89.91) 警告: 忽略额外的图例条目。 位置:legend>process_inputs (第 592 行) 位置: legend>make_legend (第 319 行) 位置: legend (第 263 行) 位置: xuanzhuan4 (第 135 行) 整体旋转轨迹分析: X方向范围: 30.19 cm Y方向范围: 30.38 cm Z方向范围: 297.36 cm 轨迹中心: (4.27, 2.07, 144.94) 警告: 忽略额外的图例条目。 位置:legend>process_inputs (第 592 行) 位置: legend>make_legend (第 319 行) 位置: legend (第 263 行) 位置: xuanzhuan4 (第 277 行)

filetype

%% 机械臂笛卡尔空间直线轨迹控制 - 优化法与RST法对比 clear; clc; close all; % =========================================== % 参数定义 % =========================================== % 机械臂参数 (Modified D-H, 单位: mm) a = [0, 0, 500, 500, 0, 0]; alpha = [0, -pi/2, 0, -pi/2, pi/2, -pi/2]; d = [400, 0, 0, 0, 0, 100]; % 关节限位(增加安全裕度) joint_limits = [ -pi+0.1, pi-0.1; % 关节1 -pi/2+0.1, pi/2-0.1; % 关节2 -pi/2+0.1, pi/2-0.1; % 关节3 -pi+0.1, pi-0.1; % 关节4 0.1, pi-0.1; % 关节5 -pi+0.1, pi-0.1 % 关节6 ]; % 目标位置 (mm) P_start = [300; -200; 600]; P_end = [-400; 300; 400]; % 目标姿态 (旋转矩阵) R_start = [1, 0, 0; 0, 1, 0; 0, 0, 1]; % 垂直向下 R_end = [1, 0, 0; 0, 1, 0; 0, 0, 1]; % 保持相同姿态 % 轨迹参数 total_time = 5; % 总时间(秒) time_step = 0.01; % 时间步长(秒) time_vector = 0:time_step:total_time; num_points = length(time_vector); % 误差权重 pos_error_weight = 0.7; rot_error_weight = 0.3; % =========================================== % 核心函数定义 % =========================================== % 正向运动学 function T = forward_kinematics(q, a, alpha, d) T = eye(4); % 预计算三角函数值 ct = cos(q); st = sin(q); ca = cos(alpha); sa = sin(alpha); for i = 1:length(q) Ti = [ct(i), -st(i)*ca(i), st(i)*sa(i), a(i)*ct(i); st(i), ct(i)*ca(i), -ct(i)*sa(i), a(i)*st(i); 0, sa(i), ca(i), d(i); 0, 0, 0, 1]; T = T * Ti; end end % 逆运动学目标函数(优化法) function error = ik_objective(q, T_target, a, alpha, d, pos_weight, rot_weight) T_current = forward_kinematics(q, a, alpha, d); % 位置误差 pos_error = norm(T_current(1:3,4) - T_target(1:3,4)); % 方向误差(使用四元数方法) R_current = T_current(1:3,1:3); R_target = T_target(1:3,1:3); % 转换为四元数 q_current = rotm2quat_custom(R_current); q_target = rotm2quat_custom(R_target); % 四元数点积 dot_product = abs(dot(q_current, q_target)); % 确保点积在有效范围内 dot_product = min(max(dot_product, -1.0), 1.0); angle_error = 2 * acos(dot_product); % 加权总误差 error = pos_weight * pos_error + rot_weight * angle_error; end % RST解析法逆运动学 function q_sols = inverse_kinematics_RST(T_target, a, alpha, d) % 提取目标位置和姿态 P_target = T_target(1:3,4); R_target = T_target(1:3,1:3); % 关节角度解集 q_sols = zeros(6, 8); % 最多8组解 % 计算关节1的两个可能解 theta1_1 = atan2(P_target(2), P_target(1)); theta1_2 = atan2(-P_target(2), -P_target(1)); % 计算关节3的两个可能解 for i = 1:2 if i == 1 theta1 = theta1_1; else theta1 = theta1_2; end % 计算关节2和3的位置 x = P_target(1)*cos(theta1) + P_target(2)*sin(theta1) - a(1); z = P_target(3) - d(1); % 计算关节3的角度 D = (x^2 + z^2 - a(3)^2 - a(4)^2) / (2*a(3)*a(4)); D = max(min(D, 1), -1); % 确保在有效范围内 theta3_1 = atan2(sqrt(1-D^2), D); theta3_2 = atan2(-sqrt(1-D^2), D); for j = 1:2 if j == 1 theta3 = theta3_1; else theta3 = theta3_2; end % 计算关节2的角度 k1 = a(3) + a(4)*cos(theta3); k2 = a(4)*sin(theta3); theta2 = atan2(z, x) - atan2(k2, k1); % 计算前三个关节的变换矩阵 T03 = forward_kinematics([theta1, theta2, theta3], a(1:3), alpha(1:3), d(1:3)); % 计算手腕姿态 R03 = T03(1:3,1:3); R36 = R03' * R_target; % 提取欧拉角(ZYZ) if abs(R36(3,3)) > 1e-6 theta4 = atan2(R36(2,3), R36(1,3)); theta5 = atan2(sqrt(R36(1,3)^2 + R36(2,3)^2), R36(3,3)); theta6 = atan2(R36(3,2), -R36(3,1)); else % 奇异位置处理 theta4 = 0; if R36(3,3) > 0 theta5 = 0; else theta5 = pi; end theta6 = atan2(R36(1,2), R36(1,1)); end % 存储解 sol_index = (i-1)*4 + (j-1)*2 + 1; q_sols(:, sol_index) = [theta1; theta2; theta3; theta4; theta5; theta6]; % 另一组手腕解 sol_index = sol_index + 1; q_sols(:, sol_index) = [theta1; theta2; theta3; theta4+pi; -theta5; theta6+pi]; end end end % 选择最优解(考虑关节限位和前一点位置) function q_opt = select_optimal_solution(q_sols, q_prev, joint_limits) min_cost = inf; q_opt = q_prev; % 默认返回前一点位置 for i = 1:size(q_sols, 2) q_candidate = q_sols(:, i); valid = true; % 检查关节限位 for j = 1:6 if q_candidate(j) < joint_limits(j,1) || q_candidate(j) > joint_limits(j,2) valid = false; break; end end if valid % 计算与前一点的差异 diff = norm(q_candidate - q_prev); % 选择最小差异的解 if diff < min_cost min_cost = diff; q_opt = q_candidate; end end end end % 球面线性插值 (SLERP) function q_interp = custom_slerp(q1, q2, t) % 归一化输入四元数 q1 = q1 / norm(q1); q2 = q2 / norm(q2); % 计算点积 cos_theta = dot(q1, q2); % 处理方向 if cos_theta < 0.0 q1 = -q1; cos_theta = -cos_theta; end % 线性插值阈值 linear_threshold = 0.99995; if cos_theta > linear_threshold % 线性插值 q_interp = (1 - t) * q1 + t * q2; q_interp = q_interp / norm(q_interp); return; end % 计算角度 theta = acos(cos_theta); sin_theta = sin(theta); % 计算插值系数 ratio1 = sin((1 - t) * theta) / sin_theta; ratio2 = sin(t * theta) / sin_theta; q_interp = ratio1 * q1 + ratio2 * q2; q_interp = q_interp / norm(q_interp); end % 旋转矩阵转四元数 function q = rotm2quat_custom(R) tr = trace(R); if tr > 0 S = sqrt(tr + 1.0) * 2; qw = 0.25 * S; qx = (R(3,2) - R(2,3)) / S; qy = (R(1,3) - R(3,1)) / S; qz = (R(2,1) - R(1,2)) / S; elseif (R(1,1) > R(2,2)) && (R(1,1) > R(3,3)) S = sqrt(1.0 + R(1,1) - R(2,2) - R(3,3)) * 2; qw = (R(3,2) - R(2,3)) / S; qx = 0.25 * S; qy = (R(1,2) + R(2,1)) / S; qz = (R(1,3) + R(3,1)) / S; elseif (R(2,2) > R(3,3)) S = sqrt(1.0 + R(2,2) - R(1,1) - R(3,3)) * 2; qw = (R(1,3) - R(3,1)) / S; qx = (R(1,2) + R(2,1)) / S; qy = 0.25 * S; qz = (R(2,3) + R(3,2)) / S; else S = sqrt(1.0 + R(3,3) - R(1,1) - R(2,2)) * 2; qw = (R(2,1) - R(1,2)) / S; qx = (R(1,3) + R(3,1)) / S; qy = (R(2,3) + R(3,2)) / S; qz = 0.25 * S; end q = [qw, qx, qy, qz]; q = q / norm(q); % 归一化 end % 四元数转旋转矩阵 function R = quat2rotm_custom(q) q = q / norm(q); qw = q(1); qx = q(2); qy = q(3); qz = q(4); R = [1 - 2*qy^2 - 2*qz^2, 2*qx*qy - 2*qz*qw, 2*qx*qz + 2*qy*qw; 2*qx*qy + 2*qz*qw, 1 - 2*qx^2 - 2*qz^2, 2*qy*qz - 2*qx*qw; 2*qx*qz - 2*qy*qw, 2*qy*qz + 2*qx*qw, 1 - 2*qx^2 - 2*qy^2]; end % 笛卡尔空间直线轨迹生成 function [P_traj, R_traj] = generate_cartesian_trajectory(P_start, P_end, R_start, R_end, t) % 位置线性插值 P_traj = (1 - t) .* P_start + t .* P_end; % 姿态球面线性插值 (SLERP) num_points = length(t); R_traj = zeros(3, 3, num_points); % 转换为四元数 q_start = rotm2quat_custom(R_start); q_end = rotm2quat_custom(R_end); % 预计算所有插值点 for i = 1:num_points t_ratio = t(i); q_interp = custom_slerp(q_start, q_end, t_ratio); R_traj(:, :, i) = quat2rotm_custom(q_interp); end end % 创建机械臂图形对象 function [h_links, h_joints, h_end] = create_robot_plot(q, a, alpha, d, color, line_width) % 计算各关节位置 n_joints = length(q); positions = zeros(3, n_joints + 1); positions(:,1) = [0;0;0]; % 基座 T = eye(4); for i = 1:n_joints ct = cos(q(i)); st = sin(q(i)); ca = cos(alpha(i)); sa = sin(alpha(i)); Ti = [ct, -st*ca, st*sa, a(i)*ct; st, ct*ca, -ct*sa, a(i)*st; 0, sa, ca, d(i); 0, 0, 0, 1]; T = T * Ti; positions(:,i+1) = T(1:3,4); end % 创建图形对象 hold on; h_links = gobjects(1, n_joints); h_joints = gobjects(1, n_joints+1); % 绘制连杆 for i = 1:n_joints h_links(i) = plot3([positions(1,i), positions(1,i+1)], ... [positions(2,i), positions(2,i+1)], ... [positions(3,i), positions(3,i+1)], ... [color '-'], 'LineWidth', line_width); end % 绘制关节 for i = 1:(n_joints+1) h_joints(i) = plot3(positions(1,i), positions(2,i), positions(3,i), ... [color 'o'], 'MarkerSize', 8, 'MarkerFaceColor', color); end % 绘制末端 h_end = plot3(positions(1,end), positions(2,end), positions(3,end), ... 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r'); end % 更新机械臂姿势 function update_robot_plot(h_links, h_joints, h_end, q, a, alpha, d) % 计算各关节位置 n_joints = length(q); positions = zeros(3, n_joints + 1); positions(:,1) = [0;0;0]; % 基座 T = eye(4); for i = 1:n_joints ct = cos(q(i)); st = sin(q(i)); ca = cos(alpha(i)); sa = sin(alpha(i)); Ti = [ct, -st*ca, st*sa, a(i)*ct; st, ct*ca, -ct*sa, a(i)*st; 0, sa, ca, d(i); 0, 0, 0, 1]; T = T * Ti; positions(:,i+1) = T(1:3,4); end % 更新连杆位置 for i = 1:n_joints set(h_links(i), 'XData', [positions(1,i), positions(1,i+1)], ... 'YData', [positions(2,i), positions(2,i+1)], ... 'ZData', [positions(3,i), positions(3,i+1)]); end % 更新关节位置 for i = 1:(n_joints+1) set(h_joints(i), 'XData', positions(1,i), ... 'YData', positions(2,i), ... 'ZData', positions(3,i)); end % 更新末端位置 set(h_end, 'XData', positions(1,end), ... 'YData', positions(2,end), ... 'ZData', positions(3,end)); end % 计算关节位置(用于范围计算) function positions = compute_joint_positions(q, a, alpha, d) n_joints = length(q); positions = zeros(3, n_joints + 1); positions(:,1) = [0;0;0]; % 基座 T = eye(4); for i = 1:n_joints ct = cos(q(i)); st = sin(q(i)); ca = cos(alpha(i)); sa = sin(alpha(i)); Ti = [ct, -st*ca, st*sa, a(i)*ct; st, ct*ca, -ct*sa, a(i)*st; 0, sa, ca, d(i); 0, 0, 0, 1]; T = T * Ti; positions(:,i+1) = T(1:3,4); end end % 动态坐标轴范围计算 function [x_lim, y_lim, z_lim] = calculate_dynamic_limits(q_traj, a, alpha, d, margin_ratio) % 计算整个轨迹中机械臂关节的最大最小位置 min_x = inf; max_x = -inf; min_y = inf; max_y = -inf; min_z = inf; max_z = -inf; for i = 1:size(q_traj, 2) % 计算当前配置下的关节位置 positions = compute_joint_positions(q_traj(:, i), a, alpha, d); % 更新极值 min_x = min(min_x, min(positions(1, :))); max_x = max(max_x, max(positions(1, :))); min_y = min(min_y, min(positions(2, :))); max_y = max(max_y, max(positions(2, :))); min_z = min(min_z, min(positions(3, :))); max_z = max(max_z, max(positions(3, :))); end % 计算范围并添加边距 x_range = max_x - min_x; y_range = max_y - min_y; z_range = max_z - min_z; margin_x = x_range * margin_ratio; margin_y = y_range * margin_ratio; margin_z = z_range * margin_ratio; x_lim = [min_x - margin_x, max_x + margin_x]; y_lim = [min_y - margin_y, max_y + margin_y]; z_lim = [min_z - margin_z, max_z + margin_z]; % 确保最小范围 min_size = 100; % mm if diff(x_lim) < min_size center = mean(x_lim); x_lim = [center - min_size/2, center + min_size/2]; end if diff(y_lim) < min_size center = mean(y_lim); y_lim = [center - min_size/2, center + min_size/2]; end if diff(z_lim) < min_size center = mean(z_lim); z_lim = [center - min_size/2, center + min_size/2]; end end % =========================================== % 主程序:笛卡尔空间直线轨迹控制(两种方法对比) % =========================================== fprintf('===== 机械臂笛卡尔空间直线轨迹控制 =====\n'); fprintf('===== 优化法与RST解析法对比 =====\n'); tic; % 生成笛卡尔空间轨迹 t_ratio = time_vector / total_time; [P_traj, R_traj] = generate_cartesian_trajectory(P_start, P_end, R_start, R_end, t_ratio); % 预分配内存 q_traj_optim = zeros(6, num_points); % 优化法轨迹 q_traj_RST = zeros(6, num_points); % RST法轨迹 actual_traj_optim = zeros(3, num_points); actual_traj_RST = zeros(3, num_points); % 初始猜测 initial_guess = mean(joint_limits, 2); % ===================== % 优化法求解 % ===================== fprintf('\n[优化法] 求解轨迹点...\n'); options = optimoptions('fmincon', ... 'Display', 'off', ... 'Algorithm', 'interior-point', ... 'MaxIterations', 200, ... 'StepTolerance', 1e-6, ... 'ConstraintTolerance', 1e-6); % 求解起始点逆运动学 T_start_mat = [R_start, P_start; 0 0 0 1]; q_prev = fmincon(@(q)ik_objective(q, T_start_mat, a, alpha, d, pos_error_weight, rot_error_weight), ... initial_guess, [], [], [], [], joint_limits(:,1), joint_limits(:,2), [], options); q_traj_optim(:, 1) = q_prev; % 串行求解逆运动学 progress_step = floor(num_points/10); for i = 2:num_points T_target = [R_traj(:, :, i), P_traj(:, i); 0 0 0 1]; % 使用前一点的结果作为初始值 q_current = fmincon(@(q)ik_objective(q, T_target, a, alpha, d, pos_error_weight, rot_error_weight), ... q_prev, [], [], [], [], joint_limits(:,1), joint_limits(:,2), [], options); q_traj_optim(:, i) = q_current; q_prev = q_current; % 显示进度 if mod(i, progress_step) == 0 fprintf('%d%% ', round(i/num_points*100)); end end fprintf('完成!\n'); % 计算优化法实际末端轨迹 for i = 1:num_points T = forward_kinematics(q_traj_optim(:, i), a, alpha, d); actual_traj_optim(:, i) = T(1:3,4); end % 计算优化法轨迹误差 position_errors_optim = vecnorm(actual_traj_optim - P_traj, 2, 1); fprintf('\n[优化法] 轨迹误差统计:\n'); fprintf('平均位置误差: %.4f mm\n', mean(position_errors_optim)); fprintf('最大位置误差: %.4f mm\n', max(position_errors_optim)); fprintf('最小位置误差: %.4f mm\n', min(position_errors_optim)); % ===================== % RST解析法求解 % ===================== fprintf('\n[RST解析法] 求解轨迹点...\n'); % 求解起始点逆运动学 T_start_mat = [R_start, P_start; 0 0 0 1]; q_sols = inverse_kinematics_RST(T_start_mat, a, alpha, d); q_prev = select_optimal_solution(q_sols, initial_guess, joint_limits); q_traj_RST(:, 1) = q_prev; % 串行求解逆运动学 for i = 2:num_points T_target = [R_traj(:, :, i), P_traj(:, i); 0 0 0 1]; % 求解所有可能解 q_sols = inverse_kinematics_RST(T_target, a, alpha, d); % 选择最优解 q_current = select_optimal_solution(q_sols, q_prev, joint_limits); q_traj_RST(:, i) = q_current; q_prev = q_current; % 显示进度 if mod(i, progress_step) == 0 fprintf('%d%% ', round(i/num_points*100)); end end fprintf('完成!\n'); % 计算RST法实际末端轨迹 for i = 1:num_points T = forward_kinematics(q_traj_RST(:, i), a, alpha, d); actual_traj_RST(:, i) = T(1:3,4); end % 计算RST法轨迹误差 position_errors_RST = vecnorm(actual_traj_RST - P_traj, 2, 1); fprintf('\n[RST解析法] 轨迹误差统计:\n'); fprintf('平均位置误差: %.4f mm\n', mean(position_errors_RST)); fprintf('最大位置误差: %.4f mm\n', max(position_errors_RST)); fprintf('最小位置误差: %.4f mm\n', min(position_errors_RST)); fprintf('\n总计算时间: %.2f 秒\n', toc); % =========================================== % 可视化结果(两种方法对比) % =========================================== % 创建可视化图形 fig = figure('Name', '优化法 vs RST解析法', 'Position', [100, 100, 1400, 800]); % 1. 3D轨迹对比 subplot(2,3,1); h_desired = plot3(P_traj(1,:), P_traj(2,:), P_traj(3,:), 'k--', 'LineWidth', 1.5); hold on; h_optim = plot3(actual_traj_optim(1,:), actual_traj_optim(2,:), actual_traj_optim(3,:), 'b-', 'LineWidth', 1.5); h_RST = plot3(actual_traj_RST(1,:), actual_traj_RST(2,:), actual_traj_RST(3,:), 'r-', 'LineWidth', 1.5); h_start = plot3(P_start(1), P_start(2), P_start(3), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g'); h_end = plot3(P_end(1), P_end(2), P_end(3), 'mo', 'MarkerSize', 10, 'MarkerFaceColor', 'm'); title('3D轨迹对比'); xlabel('X (mm)'); ylabel('Y (mm)'); zlabel('Z (mm)'); legend([h_desired, h_optim, h_RST, h_start, h_end], ... {'期望轨迹', '优化法', 'RST法', '起点', '终点'}, 'Location', 'best'); grid on; axis equal; view(3); % 2. 位置误差对比 subplot(2,3,2); hold on; h_error_optim = plot(time_vector, position_errors_optim, 'b-', 'LineWidth', 1.5); h_error_RST = plot(time_vector, position_errors_RST, 'r-', 'LineWidth', 1.5); title('位置误差对比'); xlabel('时间 (秒)'); ylabel('位置误差 (mm)'); legend([h_error_optim, h_error_RST], {'优化法', 'RST法'}, 'Location', 'best'); grid on; % 3. 关节角度对比(关节1) subplot(2,3,3); hold on; h_joint1_optim = plot(time_vector, q_traj_optim(1,:), 'b-', 'LineWidth', 1.5); h_joint1_RST = plot(time_vector, q_traj_RST(1,:), 'r-', 'LineWidth', 1.5); title('关节1角度对比'); xlabel('时间 (秒)'); ylabel('角度 (rad)'); legend([h_joint1_optim, h_joint1_RST], {'优化法', 'RST法'}, 'Location', 'best'); grid on; % 4. 关节角度对比(关节2) subplot(2,3,4); hold on; h_joint2_optim = plot(time_vector, q_traj_optim(2,:), 'b-', 'LineWidth', 1.5); h_joint2_RST = plot(time_vector, q_traj_RST(2,:), 'r-', 'LineWidth', 1.5); title('关节2角度对比'); xlabel('时间 (秒)'); ylabel('角度 (rad)'); legend([h_joint2_optim, h_joint2_RST], {'优化法', 'RST法'}, 'Location', 'best'); grid on; % 5. 关节角度对比(关节3) subplot(2,3,5); hold on; h_joint3_optim = plot(time_vector, q_traj_optim(3,:), 'b-', 'LineWidth', 1.5); h_joint3_RST = plot(time_vector, q_traj_RST(3,:), 'r-', 'LineWidth', 1.5); title('关节3角度对比'); xlabel('时间 (秒)'); ylabel('角度 (rad)'); legend([h_joint3_optim, h_joint3_RST], {'优化法', 'RST法'}, 'Location', 'best'); grid on; % 6. 误差统计对比 subplot(2,3,6); stats_optim = [mean(position_errors_optim), min(position_errors_optim), max(position_errors_optim)]; stats_RST = [mean(position_errors_RST), min(position_errors_RST), max(position_errors_RST)]; bar_data = [stats_optim; stats_RST]'; bar_handle = bar(bar_data); set(gca, 'XTickLabel', {'平均误差', '最小误差', '最大误差'}); title('误差统计对比'); ylabel('误差 (mm)'); legend('优化法', 'RST法', 'Location', 'best'); grid on; % 添加数值标签 for i = 1:3 text(i-0.18, bar_data(i,1), sprintf('%.3f', bar_data(i,1)), ... 'HorizontalAlignment', 'center', ... 'VerticalAlignment', 'bottom'); text(i+0.18, bar_data(i,2), sprintf('%.3f', bar_data(i,2)), ... 'HorizontalAlignment', 'center', ... 'VerticalAlignment', 'bottom'); end % =========================================== % 动画演示(两种方法对比) % =========================================== fprintf('\n准备动画演示...\n'); % 计算动画显示范围 margin_ratio = 0.2; q_traj_combined = [q_traj_optim, q_traj_RST]; [x_lim, y_lim, z_lim] = calculate_dynamic_limits(q_traj_combined, a, alpha, d, margin_ratio); % 创建动画图形 anim_fig = figure('Name', '机械臂运动对比动画', 'Position', [100, 100, 1200, 600], 'Color', 'white'); % 优化法动画子图 subplot(1,2,1); hold on; grid on; axis equal; view(3); xlabel('X (mm)'); ylabel('Y (mm)'); zlabel('Z (mm)'); xlim(x_lim); ylim(y_lim); zlim(z_lim); title('优化法'); % 绘制期望轨迹 plot3(P_traj(1,:), P_traj(2,:), P_traj(3,:), 'b--', 'LineWidth', 1); plot3(P_start(1), P_start(2), P_start(3), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g'); plot3(P_end(1), P_end(2), P_end(3), 'mo', 'MarkerSize', 10, 'MarkerFaceColor', 'm'); % 创建实际轨迹线 h_actual_traj_optim = plot3(actual_traj_optim(1,1), actual_traj_optim(2,1), actual_traj_optim(3,1), 'r-', 'LineWidth', 1.5); % 创建机械臂图形对象 [h_links_optim, h_joints_optim, h_end_optim] = create_robot_plot(q_traj_optim(:,1), a, alpha, d, 'b', 1.5); % 添加标题 h_title_optim = title(sprintf('优化法 (t=%.2fs, 误差=%.2fmm)', 0, position_errors_optim(1))); % RST法动画子图 subplot(1,2,2); hold on; grid on; axis equal; view(3); xlabel('X (mm)'); ylabel('Y (mm)'); zlabel('Z (mm)'); xlim(x_lim); ylim(y_lim); zlim(z_lim); title('RST解析法'); % 绘制期望轨迹 plot3(P_traj(1,:), P_traj(2,:), P_traj(3,:), 'b--', 'LineWidth', 1); plot3(P_start(1), P_start(2), P_start(3), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g'); plot3(P_end(1), P_end(2), P_end(3), 'mo', 'MarkerSize', 10, 'MarkerFaceColor', 'm'); % 创建实际轨迹线 h_actual_traj_RST = plot3(actual_traj_RST(1,1), actual_traj_RST(2,1), actual_traj_RST(3,1), 'r-', 'LineWidth', 1.5); % 创建机械臂图形对象 [h_links_RST, h_joints_RST, h_end_RST] = create_robot_plot(q_traj_RST(:,1), a, alpha, d, 'r', 1.5); % 添加标题 h_title_RST = title(sprintf('RST法 (t=%.2fs, 误差=%.2fmm)', 0, position_errors_RST(1))); % 添加全局标题 sgtitle('机械臂直线运动对比动画', 'FontSize', 16); % 动画参数 animation_step = 2; % 每2帧显示一次 frame_rate = 30; % 目标帧率 % 动画循环 fprintf('开始动画演示...\n'); for i = 1:animation_step:num_points % 优化法更新 subplot(1,2,1); set(h_actual_traj_optim, 'XData', actual_traj_optim(1,1:i), ... 'YData', actual_traj_optim(2,1:i), ... 'ZData', actual_traj_optim(3,1:i)); update_robot_plot(h_links_optim, h_joints_optim, h_end_optim, q_traj_optim(:,i), a, alpha, d); set(h_title_optim, 'String', sprintf('优化法 (t=%.2fs, 误差=%.2fmm)', time_vector(i), position_errors_optim(i))); % RST法更新 subplot(1,2,2); set(h_actual_traj_RST, 'XData', actual_traj_RST(1,1:i), ... 'YData', actual_traj_RST(2,1:i), ... 'ZData', actual_traj_RST(3,1:i)); update_robot_plot(h_links_RST, h_joints_RST, h_end_RST, q_traj_RST(:,i), a, alpha, d); set(h_title_RST, 'String', sprintf('RST法 (t=%.2fs, 误差=%.2fmm)', time_vector(i), position_errors_RST(i))); % 控制帧率 draw_time = toc; pause_time = max(0, 1/frame_rate - draw_time); if pause_time > 0 pause(pause_time); end tic; % 重置计时器 % 刷新图形 drawnow; end fprintf('动画演示完成!\n'); % 保存结果 save('robot_trajectory_comparison.mat', 'q_traj_optim', 'q_traj_RST', ... 'actual_traj_optim', 'actual_traj_RST', ... 'position_errors_optim', 'position_errors_RST'); saveas(fig, 'trajectory_comparison.png'); saveas(anim_fig, 'robot_animation_comparison.png'); (改错)

filetype

输出的折纸图存为gif:% main_multiple_folds.m - 多折痕曲纹折纸机构运动学建模 % Multi-fold curved origami mechanism kinematic modeling clear; clc; close all %% 参数设置 - Parameter Settings % 思路:基于单个折痕的扩展逻辑,将折痕横向扩展到多条 % Concept: Extend single fold logic to multiple parallel folds % 生成扇形角度 - Generate sector angles theta_deg = []; N_vertices =13; % 每条折痕的顶点数 - Number of vertices per fold N_folds =1; % 折痕数量 - Number of folds % %直纹折纸 % %为每个顶点生成扇形角度 - Generate sector angles for each vertex % theta_deg(:,1) = 95*ones(N_vertices,1) + rand(N_vertices,1)*0; % theta_deg(:,2) = 85*ones(N_vertices,1) + rand(N_vertices,1)*0; % theta_deg(:,3) = 60*ones(N_vertices,1) + rand(N_vertices,1)*0; % theta_deg(:,4) = 120*ones(N_vertices,1) + rand(N_vertices,1)*0; % theta_deg(:,3) = 180*ones(N_vertices,1) - theta_deg(:,1); % 条件(2) % theta_deg(:,4) = 360*ones(N_vertices,1) - (theta_deg(:,1)+theta_deg(:,2)+theta_deg(:,3)); %曲纹折纸(常曲率) % s=30; % ds=2; % N_vertices=s/ds+1; % kl=0.25;kr=-0.25;%可折展 % phl=10/180*pi;phr=10/180*pi; % rr=1/abs(kr);rl=1/abs(kl); % dthr=2*atan(ds/(2*rr));dthl=2*atan(ds/(2*rl)); % theta_deg(:,1)=rad2deg(pi/2+sign(kr)*(dthr/2)+phr)*ones(N_vertices,1); % theta_deg(:,2)=rad2deg(pi/2+sign(kr)*(dthr/2)-phr)*ones(N_vertices,1); % theta_deg(:,3)=rad2deg(pi/2-sign(kl)*(dthl/2)+phl)*ones(N_vertices,1); % theta_deg(:,4)=rad2deg(pi/2-sign(kl)*(dthl/2)-phl)*ones(N_vertices,1); %曲纹折纸(变曲率) s=15; B=30;%离散程度 ds=s/B; N_vertices=B+1; for i=1:N_vertices kl(i)=(0.05*ds*(i-1))^3.1; kr=kl; phl=0/180*pi;phr=phl; rr(i)=1/abs(kr(i));rl(i)=1/abs(kl(i)); dthr(i)=2*atan(ds/(2*rr(i)));dthl(i)=2*atan(ds/(2*rl(i))); theta_deg(i,1)=rad2deg(pi/2+sign(kr(i))*(dthr(i)/2)+phr); theta_deg(i,2)=rad2deg(pi/2+sign(kr(i))*(dthr(i)/2)-phr); theta_deg(i,3)=rad2deg(pi/2-sign(kl(i))*(dthl(i)/2)+phl); theta_deg(i,4)=rad2deg(pi/2-sign(kl(i))*(dthl(i)/2)-phl); end %% th0=theta_deg(1,1);th1 = theta_deg(1,2);th2 = theta_deg(1,3); I=1e-10;j=I:1:160; %% 主循环参数 - Main loop parameters for in =1:size(j,2) % % 折叠角度参数 - Folding angle parameter i_total=j(in); % 基本参数设置 - Basic parameter setup %l_initial=[1,0.6,1]*0.1; % 初始长度 - Initial lengths l_initial=[1,ds,1]*0.1; % 初始长度 - Initial lengths %l_initial=[1 ,1.5,1]*0.1; % 初始长度 - Initial lengths c_scale = 1; % 缩放因子 - Scaling factor %曲纹折纸不适合非等距,因为改变了每条折痕曲率 rho_initial_deg = i_total; % 初始折叠角度 - Initial folding angle sigma_geom_choice = -1; % 几何选择参数 - Geometry choice parameter % 转换为弧度 - Convert to radians theta_rad = deg2rad(theta_deg); rho_central_rad = deg2rad(i_total); %% 计算初始折痕 - Calculate initial fold [P_center_0, P_left_0, P_right_0, actual_N_vertices] = calculate_3d_strip_geometry(... theta_rad, rho_central_rad, l_initial, c_scale, sigma_geom_choice, N_vertices); %% 多折痕扩展 - Multiple fold expansion % 存储所有折痕的数据 - Store data for all folds P_center_all = cell(N_folds, 1); % 中心线点 - Center line points P_left_all = cell(N_folds, 1); % 左边界点 - Left boundary points P_right_all = cell(N_folds, 1); % 右边界点 - Right boundary points % 第一条折痕(原始折痕)- First fold (original fold) P_center_all{1} = P_center_0; P_left_all{1} = P_left_0; P_right_all{1} = P_right_0; % 生成多条折痕 - Generate multiple folds for fold_idx = 2:N_folds % 获取前一条折痕作为参考 - Get previous fold as reference PR_prev_center = P_right_all{fold_idx-1}; PR_prev_left = P_center_all{fold_idx-1}; %% 计算前一条折痕的角度参数 - Calculate angle parameters for previous fold PR_th2_ = zeros(1, length(PR_prev_center)-1); PR_th3_ = zeros(1, length(PR_prev_center)-1); % a=P_right_0(:,2)-P_right_0(:,1); % b=P_center_0(:,1)-P_right_0(:,1); % dot_product = dot(a, b); % norm_a = norm(a); % norm_b = norm(b); % th10 = acosd(dot_product / (norm_a * norm_b)); a=PR_prev_center(:,1)-PR_prev_center(:,2); b=PR_prev_left (:,2)-PR_prev_center(:,2); dot_product = dot(a, b); norm_a = norm(a); norm_b = norm(b); th10 = 180-acosd(dot_product / (norm_a * norm_b)); for i_ = 1:length(PR_prev_center)-1 % 计算向量夹角 - Calculate vector angles A = (PR_prev_center(:,i_+1) - PR_prev_center(:,i_)); B = (PR_prev_left(:,i_) - PR_prev_center(:,i_)); dot_product = dot(A, B); norm_A = norm(A); norm_B = norm(B); cos_theta(i_) = dot_product / (norm_A * norm_B); PR_th2_(i_) = acos(cos_theta(i_)); end for i_ = 2:length(PR_prev_center) A = (PR_prev_center(:,i_-1) - PR_prev_center(:,i_)); B1 = (PR_prev_left(:,i_) - PR_prev_center(:,i_)); dot_product1 = dot(A, B1); norm_B1 = norm(B1); cos_theta(i_) = dot_product1 / (norm_A * norm_B1); PR_th3_(i_) = acos(cos_theta(i_)); end % c=P_left_0(:,end-1)-P_left_0(:,end); d=P_center_0(:,end)-P_left_0(:,end); dot_product = dot(c, d); norm_c = norm(c); norm_d = norm(d); th00 = acosd(dot_product / (norm_c * norm_d)); %% 计算新折痕的角度参数 - Calculate new fold angle parameters PR_th2 = rad2deg(PR_th2_); PR_th0 =180- PR_th2; %% 生成新折痕的右边界 - Generate right boundary for new fold % 计算第一个点的位置 - Calculate position of first point A = (PR_prev_center(:,2) - PR_prev_center(:,1)); B = (PR_prev_left(:,1) - PR_prev_center(:,1)); % 计算法向量 - Calculate normal vector PR_normal_Vector = cross(A, B); if rem(fold_idx,2)==1 sign_flag = -1; PR_th1 = th1; PR_th0(N_vertices )=theta_deg(N_vertices,1); else sign_flag = 1; PR_th1 = th10; PR_th0(N_vertices )=PR_th0(N_vertices-1 ); end % 旋转法向量 - Rotate normal vector PR_right_normal_Vector = (RotationofRandL(PR_prev_center(:,1), ... PR_prev_center(:,1) + PR_normal_Vector, A, deg2rad(sign_flag*i_total))) - PR_prev_center(:,1)'; % 计算旋转后的点 - Calculate rotated point PR_right_ = RotationofRandL(PR_prev_center(:,1), PR_prev_center(:,2), ... PR_right_normal_Vector/norm(PR_right_normal_Vector), -deg2rad(PR_th1)); % 新折痕的右边界点 - Right boundary points for new fold PR_right = zeros(3, N_vertices); PR_right(:,1) = PR_prev_center(:,1) + l_initial(3) *... (PR_right_' - PR_prev_center(:,1)) / norm((PR_right_' - PR_prev_center(:,1))); % 迭代计算其余点 - Iteratively calculate remaining points for i = 2:N_vertices A = (PR_right(:,i-1) - PR_prev_center(:,i-1)); B = (PR_prev_center(:,i) - PR_prev_center(:,i-1)); PR_normal_Vector = cross(A, B); PR_right_normal_Vector = PR_normal_Vector / norm(PR_normal_Vector); PR_right_1 = RotationofRandL(PR_prev_center(:,i), PR_prev_center(:,i-1), ... PR_right_normal_Vector / norm(PR_right_normal_Vector), deg2rad(PR_th0(i))); PR_right(:,i) = PR_prev_center(:,i) + l_initial(3) * c_scale^(i-1) * ... (PR_right_1' - PR_prev_center(:,i)) / norm((PR_right_1' - PR_prev_center(:,i))); end % 存储新折痕数据 - Store new fold data P_center_all{fold_idx} = PR_prev_center; P_left_all{fold_idx} = PR_prev_left; P_right_all{fold_idx} = PR_right; % 打印进度信息 - Print progress information fprintf('第 %d 条折痕计算完成 - Fold %d calculation completed\n', fold_idx, fold_idx); end %% 可视化所有折痕 - Visualize all folds figure(122); clf; % 清除图形 - Clear figure hold on; axis equal; % 颜色映射 - Color mapping colors1 = hsv(N_folds); colors2 = jet(N_folds); for fold_idx = 1:N_folds P_center_curr = P_center_all{fold_idx}; P_left_curr = P_left_all{fold_idx}; P_right_curr = P_right_all{fold_idx}; if fold_idx == 1 first_point = P_center_curr(:,1); % 绘制坐标系标记 % X轴(红色) quiver3(first_point(1), first_point(2), first_point(3), 0.1, 0, 0, ... 'Color', 'r', 'LineWidth', 2, 'MaxHeadSize', 1.5, 'AutoScale', 'off'); % Y轴(绿色) quiver3(first_point(1), first_point(2), first_point(3), 0, 0.1, 0, ... 'Color', 'g', 'LineWidth', 2, 'MaxHeadSize', 1.5, 'AutoScale', 'off'); % Z轴(蓝色) quiver3(first_point(1), first_point(2), first_point(3), 0, 0, 0.1, ... 'Color', 'b', 'LineWidth', 2, 'MaxHeadSize', 1.5, 'AutoScale', 'off'); % 添加点标记 plot3(first_point(1), first_point(2), first_point(3), 'ko', ... 'MarkerSize', 5, 'MarkerFaceColor', 'y'); end % 绘制每个折痕的面板 - Draw panels for each fold for i = 1:N_vertices-1 % 左面板坐标 - Left panel coordinates x_l = [P_center_curr(1,i) P_center_curr(1,i+1) P_left_curr(1,i+1) P_left_curr(1,i) P_center_curr(1,i)]; y_l = [P_center_curr(2,i) P_center_curr(2,i+1) P_left_curr(2,i+1) P_left_curr(2,i) P_center_curr(2,i)]; z_l = [P_center_curr(3,i) P_center_curr(3,i+1) P_left_curr(3,i+1) P_left_curr(3,i) P_center_curr(3,i)]; % 右面板坐标 - Right panel coordinates x_r = [P_center_curr(1,i) P_center_curr(1,i+1) P_right_curr(1,i+1) P_right_curr(1,i) P_center_curr(1,i)]; y_r = [P_center_curr(2,i) P_center_curr(2,i+1) P_right_curr(2,i+1) P_right_curr(2,i) P_center_curr(2,i)]; z_r = [P_center_curr(3,i) P_center_curr(3,i+1) P_right_curr(3,i+1) P_right_curr(3,i) P_center_curr(3,i)]; % 绘制和填充面板 - Draw and fill panels % plot3(x_l, y_l, z_l, '.-', 'LineWidth', 2, 'Color', colors1(fold_idx,:)); % plot3(x_r, y_r, z_r, '-', 'LineWidth', 2, 'Color', colors2(fold_idx,:)); % plot3(x_l, y_l, z_l, '.'); % plot3(x_r, y_r, z_r, '*' ); % % 填充颜色 - Fill color fill3(x_l, y_l, z_l, colors1(fold_idx,:)); fill3(x_r, y_r, z_r, colors2(fold_idx,:)); end end axis off % % 设置图形属性 - Set figure properties % xlabel('X 坐标 - X Coordinate'); % ylabel('Y 坐标 - Y Coordinate'); % zlabel('Z 坐标 - Z Coordinate'); % title(sprintf('多折痕曲纹折纸机构 (%d 条折痕) - Multi-fold Curved Origami (%d folds)', N_folds, N_folds)); % % grid on; % material shiny; % light('Position',[0 0 0]); % alpha(1); % 设置透明度 %view(3) view(0,90); % % 添加图例 - Add legend % legend_entries = cell(N_folds, 1); % for fold_idx = 1:N_folds % legend_entries{fold_idx} = sprintf('折痕 %d - Fold %d', fold_idx, fold_idx); % end %% 验证几何一致性 - Verify geometric consistency fprintf('\n=== 几何一致性验证 - Geometric Consistency Verification ===\n'); for fold_idx = 1:N_folds P_center_curr = P_center_all{fold_idx}; P_right_curr = P_right_all{fold_idx}; % 计算共面性检验 - Calculate coplanarity test triple_scalars = zeros(1, length(P_center_curr)-1); for i_ = 1:length(P_center_curr)-1 P1 = P_center_curr(:,i_); P2 = P_center_curr(:,i_+1); P3 = P_right_curr(:,i_); P4 = P_right_curr(:,i_+1); % 计算混合积 - Calculate scalar triple product v1 = P2 - P1; v2 = P3 - P1; v3 = P4 - P1; triple_scalars(i_) = dot(v1, cross(v2, v3)); end % 输出验证结果 - Output verification results max_deviation = max(abs(triple_scalars)); fprintf('折痕 %d: 最大共面偏差 = %.2e - Fold %d: Max coplanarity deviation = %.2e\n', ... fold_idx, max_deviation, fold_idx, max_deviation); if max_deviation < 1e-10 fprintf(' ✓ 几何一致性良好 - Good geometric consistency\n'); else fprintf(' ⚠ 存在几何偏差 - Geometric deviation detected\n'); end end %% 输出总结信息 - Output summary information fprintf('\n=== 计算总结 - Calculation Summary ===\n'); fprintf('总折痕数量 - Total number of folds: %d\n', N_folds); fprintf('每条折痕顶点数 - Vertices per fold: %d\n', N_vertices); fprintf('计算完成 - Calculation completed successfully\n'); %% === 添加代码:整合所有点坐标到单一矩阵 === % 计算总点数 (每条折痕有3类点 * N_vertices个顶点) totalPoints = N_folds * N_vertices * 3; % 初始化坐标矩阵 (3行: x,y,z; totalPoints列) allPointsMatrix = zeros(3, totalPoints); % 当前索引指针 currentIdx = 1; % 遍历所有折痕 for foldIdx = 1:N_folds % 当前折痕的点集 P_center = P_center_all{foldIdx}; P_left = P_left_all{foldIdx}; PR_right = P_right_all{foldIdx}; % 将中心点添加到矩阵 allPointsMatrix(:, currentIdx:currentIdx+N_vertices-1) = P_left; currentIdx = currentIdx + N_vertices; % 将左边界点添加到矩阵 allPointsMatrix(:, currentIdx:currentIdx+N_vertices-1) = P_center; currentIdx = currentIdx + N_vertices; % 将右边界点添加到矩阵 allPointsMatrix(:, currentIdx:currentIdx+N_vertices-1) = PR_right; currentIdx = currentIdx + N_vertices; end allPointsMatrix=allPointsMatrix'; for i=2:N_folds if i==2 for j=1:2*N_vertices allPointsMatrix((i-1)*3*N_vertices,:)=[]; end else for j=1:2*N_vertices allPointsMatrix((i+1)*N_vertices,:)=[]; end end end a1=allPointsMatrix(:,1); a2=allPointsMatrix(:,2); allPointsMatrix1=allPointsMatrix(:,1:2); % save('shuju1.txt','a1','-ascii') % save('shuju2.txt','a2','-ascii') % save('shujuf1.mat','allPointsMatrix') % dlmwrite('shujuf1.txt', allPointsMatrix1, 'delimiter', ',', 'precision', '%.6f'); % %% === 顶点夹角计算 === % if N_folds == 1 % fprintf('\n=== 单折痕顶点夹角计算 ===\n'); % vertex_angles = zeros(1, N_vertices); % 存储每个顶点的夹角 % % for i_vertex = 1:N_vertices % % 获取当前顶点的三个点 % P_c = P_center_0(:, i_vertex); % P_l = P_left_0(:, i_vertex); % P_r = P_right_0(:, i_vertex); % % % 计算两个向量:中间顶点到左侧顶点 和 中间顶点到右侧顶点 % vec_left = P_l - P_c; % vec_right = P_r - P_c; % % % 计算点积和模 % dot_product = dot(vec_left, vec_right); % norm_left = norm(vec_left); % norm_right = norm(vec_right); % % % 避免除以零 % if norm_left < 1e-10 || norm_right < 1e-10 % fprintf('顶点 %d: 向量长度为零,无法计算夹角\n', i_vertex); % vertex_angles(i_vertex) = NaN; % continue; % end % % % 计算夹角(弧度) % cos_theta = dot_product / (norm_left * norm_right); % cos_theta = max(min(cos_theta, 1), -1); % 限制在[-1,1]范围内 % angle_rad = acos(cos_theta); % angle_deg = rad2deg(angle_rad); % % vertex_angles(i_vertex) =180- angle_deg; % fprintf('顶点 %d: 左侧-中心-右侧夹角 = %.2f°\n', i_vertex, angle_deg); % end % % % 可视化顶点夹角变化 % figure(201); % plot(S, vertex_angles, 'ro-', 'LineWidth', 1); % xlabel('(t-1)𝛥𝑠'); % ylabel('𝜌(°)'); % % title('单折痕顶点夹角分布 (左侧-中心-右侧)'); % grid on; % ylim([0 180 ]); % % % % % 保存夹角数据 % angle_data = [S; vertex_angles]'; % save('vertex_angles.txt', 'angle_data', '-ascii'); % fprintf('顶点夹角数据已保存到 vertex_angles.txt\n'); % end % %% === 折叠角计算 === % if N_folds == 1 % fprintf('\n=== 单折痕折叠角计算 ===\n'); % folding_angles = zeros(1, N_vertices-1); % 存储每个折痕段的折叠角 % % for i_seg = 1:N_vertices-1 % % 获取当前折痕段的四个关键点 % P_center_i = P_center_0(:, i_seg); % P_center_i1 = P_center_0(:, i_seg+1); % P_left_i = P_left_0(:, i_seg); % P_right_i = P_right_0(:, i_seg); % % % 计算左侧面板的法向量 (使用三点: 中心i, 中心i+1, 左侧i) % v1_left = P_center_i1 - P_center_i; % v2_left = P_left_i - P_center_i; % normal_left = cross(v1_left, v2_left); % normal_left = normal_left / norm(normal_left); % 归一化 % % % 计算右侧面板的法向量 (使用三点: 中心i, 中心i+1, 右侧i) % v1_right = P_center_i1 - P_center_i; % v2_right = P_right_i - P_center_i; % normal_right = cross(v2_right, v1_right); % 注意向量顺序保证方向一致 % normal_right = normal_right / norm(normal_right); % % % 计算法向量夹角 (0~180度) % cos_angle = dot(normal_left, normal_right); % cos_angle = max(min(cos_angle, 1), -1); % 限制在[-1,1]范围内 % angle_rad = acos(cos_angle); % angle_deg = rad2deg(angle_rad); % % % 确定折叠角方向 (0°=完全折叠, 180°=完全展开) % % 通过检查法向量与折痕向量的关系确定方向 % ref_vector = cross(normal_left, normal_right); % if dot(ref_vector, v1_left) < 0 % folding_angle = angle_deg; % 锐角折叠 % else % folding_angle = 360 - angle_deg; % 钝角折叠 % end % % % 确保折叠角在0~180范围内 % if folding_angle > 180 % folding_angle = folding_angle - 360; % elseif folding_angle < 0 % folding_angle = abs(folding_angle); % end % % folding_angles(i_seg) = folding_angle; % % fprintf('折痕段 %d: 折叠角 = %.2f°\n', i_seg, folding_angle); % end % % % 可视化折叠角变化 % figure(200); % plot(1:N_vertices-1, folding_angles, 'bo-', 'LineWidth', 2); % xlabel('折痕段索引'); % ylabel('折叠角 (°)'); % %title('单折痕折叠角分布'); % grid on; % ylim([0 180]); % % % 保存折叠角数据 % folding_data = [1:N_vertices-1; folding_angles]'; % save('folding_angles.txt', 'folding_data', '-ascii'); % fprintf('折叠角数据已保存到 folding_angles.txt\n'); % end %% === 计算卷绕程度=== fprintf('\n=== 计算卷绕程度 - Calculate Modified Winding Degree ===\n'); % 只考虑中间折痕(中心线)的点 center_points = P_center_0(1:2, :)'; % 提取中心线的x,y坐标 (N_vertices x 2) % 找到x坐标绝对值最大的顶点 [~, idx_x] = max(abs(center_points(:,1))); x_value = center_points(idx_x, 1); % 找到y坐标绝对值最大的顶点 [~, idx_y] = max(abs(center_points(:,2))); y_value = center_points(idx_y, 2); % 计算卷绕程度(y_max/x_max的绝对值) winding_degree(in) = abs(y_value / x_value); % 输出结果 fprintf('中心线x坐标绝对值最大的顶点: (x, y) = (%.6f, %.6f)\n', x_value, center_points(idx_x,2)); fprintf('中心线y坐标绝对值最大的顶点: (x, y) = (%.6f, %.6f)\n', center_points(idx_y,1), y_value); fprintf('卷绕程度 = |%.6f / %.6f| = %.6f\n', y_value, x_value, winding_degree); end fprintf('\n多折痕曲纹折纸机构建模完成!- Multi-fold curved origami modeling completed!\n'); figure(4) plot(j,winding_degree,'bo-', 'LineWidth', 1); xlabel('𝜌(°)'); ylabel('卷曲程度ω'); % title('单折痕顶点夹角分布 (左侧-中心-右侧)'); grid on; %ylim([0 180 ]);

weixin_42653672
  • 粉丝: 121
上传资源 快速赚钱