% 2024高教社杯全国大学生数学建模竞赛 A题 问题四 % 板凳龙调头路径优化与速度控制 clear; clc; close all; %% 参数设置 pitch = 0.55; % 螺距 (m) b_spiral = pitch / (2 * pi); % 螺线参数 v0 = 1; % 龙头速度 (m/s) R_turn = 4.5; % 调头区域半径 (m) theta0 = 32 * pi; % 初始角度（第16圈起始点） % 板凳尺寸 (m) dragon_head_length = 3.41; % 龙头把手间距 dragon_body_length = 2.20; % 龙身/龙尾把手间距 % 舞龙队节数 num_sections = 223; % 223节板凳 num_points = num_sections + 1; % 总点数 (龙头前把手 + 各节后把手) % 时间设置 dt = 0.1; % 时间步长 time_vector = -100:dt:100; num_time_points = length(time_vector); % 调头路径参数（来自论文优化结果） R1 = 2.8620; % 大圆半径 R2 = 1.4310; % 小圆半径 O1 = [-1.3916, -0.3876]; % 大圆圆心 O2 = [1.7825, 2.5029]; % 小圆圆心 phi1 = 2.4866; % 大圆弧度 phi2 = 3.2669; % 小圆弧度 % 计算调头时间 t_arc1 = phi1 * R1 / v0; % 第一段圆弧时间 t_arc2 = phi2 * R2 / v0; % 第二段圆弧时间 t_turn_total = t_arc1 + t_arc2; % 总调头时间 fprintf('优化参数: R1=%.4fm, R2=%.4fm, φ1=%.4frad, φ2=%.4frad\n', R1, R2, phi1, phi2); fprintf('大圆圆心: (%.4f, %.4f), 小圆圆心: (%.4f, %.4f)\n', O1(1), O1(2), O2(1), O2(2)); fprintf('调头总时间: %.4fs, 调头路径长度: %.4fm\n', t_turn_total, R1*phi1 + R2*phi2); %% 预计算弧长积分函数 (螺旋段) arc_length_func = @(theta) (b_spiral/2) * (theta .* sqrt(1 + theta.^2) + asinh(theta)); % 计算初始弧长s0 (t=-100s) s0 = arc_length_func(theta0); fprintf('初始总弧长: %.6f m\n', s0); %% 轨迹函数 G(t) function pos = computeG(t, b_spiral, s0, v0, params) % 解析参数 R1 = params.R1; R2 = params.R2; O1 = params.O1; O2 = params.O2; gamma1 = params.gamma1; gamma2 = params.gamma2; omega1 = params.omega1; omega2 = params.omega2; t_arc1 = params.t_arc1; t_arc2 = params.t_arc2; % 弧长函数 (螺旋段) arc_length_func = @(theta) (b_spiral/2) * (theta .* sqrt(1 + theta.^2) + asinh(theta)); % 盘入曲线 (t ≤ 0) if t <= 0 s_current = s0 + t * v0; % 注意符号修正 theta = solveTheta(s_current, arc_length_func); r = b_spiral * theta; pos = [r * cos(theta), r * sin(theta)]; % 盘出曲线 (t > 总调头时间) elseif t > t_arc1 + t_arc2 t_eff = t - (t_arc1 + t_arc2); s_current = s0 + t_eff * v0; % 从入口重新计算弧长 theta = solveTheta(s_current, arc_length_func); r = b_spiral * theta; pos = [r * cos(theta + pi), r * sin(theta + pi)]; % 中心对称 % 调头区域 else if t <= t_arc1 % 第一段圆弧 angle = gamma1 - omega1 * t; pos = O1 + R1 * [cos(angle), sin(angle)]; else % 第二段圆弧 angle = gamma2 + omega2 * (t - t_arc1); pos = O2 + R2 * [cos(angle), sin(angle)]; end end end % 二分法求解theta (给定弧长) function theta = solveTheta(s_target, arc_length_func) theta_low = 0; theta_high = 1000; % 足够大的上界 tol = 1e-10; max_iter = 100; for iter = 1:max_iter theta_mid = (theta_low + theta_high) / 2; s_mid = arc_length_func(theta_mid); if abs(s_mid - s_target) < tol break; end if s_mid < s_target theta_low = theta_mid; else theta_high = theta_mid; end end theta = theta_mid; end %% 计算龙头轨迹 fprintf('计算龙头轨迹...\n'); % 初始化位置和速度 head_positions = zeros(num_time_points, 2); % 设置参数结构体 params.R1 = R1; params.R2 = R2; params.O1 = O1; params.O2 = O2; params.gamma1 = atan2(O1(2), O1(1)) + pi/2; % 大圆弧起始角度 params.gamma2 = params.gamma1 - phi1 + pi; % 小圆弧起始角度 params.omega1 = v0 / R1; % 角速度1 params.omega2 = v0 / R2; % 角速度2 params.t_arc1 = t_arc1; params.t_arc2 = t_arc2; % 计算位置 for i = 1:num_time_points t = time_vector(i); head_positions(i, :) = computeG(t, b_spiral, s0, v0, params); end %% 计算所有把手位置 (递推法) fprintf('计算各把手位置...\n'); all_positions = zeros(num_time_points, num_points, 2); % 初始化龙头位置 all_positions(:, 1, :) = head_positions; % 递推计算各把手位置 for t_idx = 1:num_time_points for point_idx = 2:num_points % 确定与前一把手的距离 if point_idx == 2 d = dragon_head_length; % 龙头 else d = dragon_body_length; % 龙身/龙尾 end % 前一把手位置 prev_pos = squeeze(all_positions(t_idx, point_idx-1, :))'; % 牛顿法求解当前把手位置 theta_guess = atan2(prev_pos(2), prev_pos(1)); % 初始估计 tol = 1e-10; max_iter = 50; for iter = 1:max_iter r = b_spiral * theta_guess; x = r * cos(theta_guess); y = r * sin(theta_guess); f = (x - prev_pos(1))^2 + (y - prev_pos(2))^2 - d^2; % 导数计算 dx_dtheta = b_spiral * (cos(theta_guess) - theta_guess * sin(theta_guess)); dy_dtheta = b_spiral * (sin(theta_guess) + theta_guess * cos(theta_guess)); f_prime = 2*(x - prev_pos(1))*dx_dtheta + 2*(y - prev_pos(2))*dy_dtheta; % 更新theta delta = f / f_prime; theta_new = theta_guess - delta; % 检查收敛 if abs(delta) < tol break; end theta_guess = theta_new; end % 保存位置 r_final = b_spiral * theta_new; all_positions(t_idx, point_idx, :) = [r_final * cos(theta_new), r_final * sin(theta_new)]; end end %% 计算各把手速度 (基于运动学约束的递推方法) fprintf('计算各把手速度 (运动学约束递推)...\n'); all_velocities = zeros(num_time_points, num_points); % 速度大小 all_velocity_vectors = zeros(num_time_points, num_points, 2); % 速度矢量 for t_idx = 1:num_time_points % 计算每个把手的切线方向（速度方向） tangent_directions = zeros(num_points, 2); for point_idx = 1:num_points pos = squeeze(all_positions(t_idx, point_idx, :)); x = pos(1); y = pos(2); theta = atan2(y, x); % 计算螺旋线切线方向 dx_dtheta = b_spiral * (cos(theta) - theta * sin(theta)); dy_dtheta = b_spiral * (sin(theta) + theta * cos(theta)); tangent_vec = [dx_dtheta, dy_dtheta]; tangent_directions(point_idx, :) = tangent_vec / norm(tangent_vec); end % 计算每个板凳的轴线方向 axis_directions = zeros(num_points-1, 2); for point_idx = 1:num_points-1 pos_current = squeeze(all_positions(t_idx, point_idx, :)); pos_next = squeeze(all_positions(t_idx, point_idx+1, :)); axis_vec = pos_next' - pos_current'; axis_directions(point_idx, :) = axis_vec / norm(axis_vec); end % 计算每个把手的夹角γ gamma_angles = zeros(num_points, 1); for point_idx = 1:num_points if point_idx < num_points % 计算速度方向与轴线方向的夹角 cos_gamma = dot(tangent_directions(point_idx, :), axis_directions(point_idx, :)); gamma_angles(point_idx) = acos(cos_gamma); else % 最后一个把手，使用前一个把手的夹角 gamma_angles(point_idx) = gamma_angles(point_idx-1); end end % 递推计算速度大小 velocities = zeros(num_points, 1); velocities(1) = v0; % 龙头速度 for point_idx = 1:num_points-1 velocities(point_idx+1) = velocities(point_idx) * cos(gamma_angles(point_idx)) / cos(gamma_angles(point_idx+1)); end % 存储速度 all_velocities(t_idx, :) = velocities; % 计算速度矢量 for point_idx = 1:num_points speed = velocities(point_idx); direction = tangent_directions(point_idx, :); all_velocity_vectors(t_idx, point_idx, :) = speed * direction; end end %% 碰撞检测 fprintf('进行碰撞检测...\n'); collision_detected = false; collision_time = 0; % 板凳尺寸 bench_width = 0.3; % 板凳宽度 bench_length_head = 3.41; % 龙头板凳长度 bench_length_body = 2.20; % 龙身板凳长度 for t_idx = 1:num_time_points for i = 1:num_points-1 % 遍历每个板凳 % 获取板凳中心点 if i == 1 bench_center = squeeze(all_positions(t_idx, i, :))' + 0.5 * (squeeze(all_positions(t_idx, i+1, :))' - squeeze(all_positions(t_idx, i, :))'); else bench_center = squeeze(all_positions(t_idx, i, :))' + 0.5 * (squeeze(all_positions(t_idx, i+1, :))' - squeeze(all_positions(t_idx, i, :))'); end % 计算板凳方向向量 if i == 1 bench_dir = squeeze(all_positions(t_idx, i+1, :))' - squeeze(all_positions(t_idx, i, :))'; else bench_dir = squeeze(all_positions(t_idx, i+1, :))' - squeeze(all_positions(t_idx, i, :))'; end bench_dir = bench_dir / norm(bench_dir); % 计算板凳旋转矩阵 bench_angle = atan2(bench_dir(2), bench_dir(1)); rot_matrix = [cos(bench_angle), -sin(bench_angle); sin(bench_angle), cos(bench_angle)]; % 检查与其他板凳的碰撞 for j = max(1, i-5):min(num_points-1, i+5) % 只检查附近的板凳 if j == i, continue; end % 跳过自身 % 获取目标板凳中心点 if j == 1 target_center = squeeze(all_positions(t_idx, j, :))' + 0.5 * (squeeze(all_positions(t_idx, j+1, :))' - squeeze(all_positions(t_idx, j, :))'); else target_center = squeeze(all_positions(t_idx, j, :))' + 0.5 * (squeeze(all_positions(t_idx, j+1, :))' - squeeze(all_positions(t_idx, j, :))'); end % 将目标点转换到当前板凳坐标系 rel_pos = target_center - bench_center; local_pos = rot_matrix' * rel_pos'; % 检查是否在板凳范围内 if j == 1 bench_len = bench_length_head; else bench_len = bench_length_body; end if abs(local_pos(1)) < bench_len/2 + 0.1 && abs(local_pos(2)) < bench_width/2 + 0.1 collision_detected = true; collision_time = time_vector(t_idx); fprintf('碰撞检测: 时间 %.2fs, 板凳 %d 与 %d\n', collision_time, i, j); break; end end if collision_detected, break; end end if collision_detected, break; end end if ~collision_detected fprintf('无碰撞检测\n'); end %% 寻找最大速度并调整龙头速度 fprintf('分析速度分布...\n'); max_speeds = zeros(1, num_points); for i = 1:num_points max_speeds(i) = max(all_velocities(:, i)); end [max_overall_speed, max_speed_idx] = max(max_speeds); fprintf('最大速度: %.4f m/s (出现在把手 %d)\n', max_overall_speed, max_speed_idx); % 调整龙头速度 if max_overall_speed > 2 v0_adjusted = 2 / max_overall_speed * v0; fprintf('为保证速度不超过2m/s，需要将龙头速度调整为: %.4f m/s\n', v0_adjusted); else v0_adjusted = v0; fprintf('所有把手速度均不超过2m/s，无需调整龙头速度\n'); end %% 可视化结果 figure('Position', [100, 100, 1200, 800]); % 轨迹图 subplot(2, 2, 1); hold on; % 绘制螺旋路径 theta_vals = linspace(0, 32*pi, 1000); x_vals = b_spiral * theta_vals .* cos(theta_vals); y_vals = b_spiral * theta_vals .* sin(theta_vals); plot(x_vals, y_vals, 'b-', 'LineWidth', 1); % 绘制调头圆弧 theta_arc1 = linspace(params.gamma1, params.gamma1 - phi1, 100); arc1_x = O1(1) + R1 * cos(theta_arc1); arc1_y = O1(2) + R1 * sin(theta_arc1); plot(arc1_x, arc1_y, 'r-', 'LineWidth', 2); theta_arc2 = linspace(params.gamma2, params.gamma2 + phi2, 100); arc2_x = O2(1) + R2 * cos(theta_arc2); arc2_y = O2(2) + R2 * sin(theta_arc2); plot(arc2_x, arc2_y, 'r-', 'LineWidth', 2); % 标记调头区域 theta_circle = linspace(0, 2*pi, 100); circle_x = R_turn * cos(theta_circle); circle_y = R_turn * sin(theta_circle); plot(circle_x, circle_y, 'g--', 'LineWidth', 1); % 标记特殊时间点 special_times = [-100, -50, 0, 50, 100]; colors = {'ro', 'go', 'mo', 'co', 'yo'}; for i = 1:length(special_times) [~, idx] = min(abs(time_vector - special_times(i))); plot(head_positions(idx, 1), head_positions(idx, 2), colors{i}, 'MarkerSize', 8, 'MarkerFaceColor', colors{i}(1)); end title('板凳龙运动轨迹'); xlabel('x (m)'); ylabel('y (m)'); grid on; axis equal; legend('螺旋路径', '调头曲线', '调头区域', '位置标记', 'Location', 'best'); hold off; % 速度分布图 subplot(2, 2, 2); plot(1:num_points, max_speeds, 'b-o', 'LineWidth', 1.5); xlabel('把手编号'); ylabel('最大速度 (m/s)'); title('各把手最大速度分布'); grid on; % 标记特殊点 special_point_indices = [1, 2, 52, 102, 152, 202, 224]; hold on; for i = 1:length(special_point_indices) idx = special_point_indices(i); plot(idx, max_speeds(idx), 'ro', 'MarkerSize', 8, 'MarkerFaceColor', 'r'); end hold off; % 速度随时间变化图（龙头和最大速度把手） subplot(2, 2, 3); [~, max_speed_handle] = max(max_speeds); plot(time_vector, all_velocities(:, 1), 'b-', 'LineWidth', 1.5); hold on; plot(time_vector, all_velocities(:, max_speed_handle), 'r-', 'LineWidth', 1.5); xlabel('时间 (s)'); ylabel('速度 (m/s)'); title('速度随时间变化'); legend('龙头', sprintf('把手 %d (最大速度)', max_speed_handle)); grid on; % 调头区域放大图 subplot(2, 2, 4); hold on; plot(arc1_x, arc1_y, 'r-', 'LineWidth', 2); plot(arc2_x, arc2_y, 'r-', 'LineWidth', 2); plot(circle_x, circle_y, 'g--', 'LineWidth', 1); % 绘制特定时间点的板凳龙形态 show_times = [0, t_arc1, t_arc1 + t_arc2/2, t_turn_total]; colors = {'bo', 'go', 'mo', 'co'}; for i = 1:length(show_times) [~, idx] = min(abs(time_vector - show_times(i))); for j = 1:10:num_points % 每隔10个把手绘制一个 pos = squeeze(all_positions(idx, j, :)); plot(pos(1), pos(2), colors{i}, 'MarkerSize', 4, 'MarkerFaceColor', colors{i}(1)); end end title('调头区域放大图'); xlabel('x (m)'); ylabel('y (m)'); grid on; axis equal; xlim([-5, 5]); ylim([-5, 5]); hold off; %% 输出关键结果 fprintf('\n=== 关键结果 ===\n'); fprintf('最短调头路径长度: %.4f m\n', R1*phi1 + R2*phi2); fprintf('调头总时间: %.4f s\n', t_turn_total); fprintf('最大速度: %.4f m/s (发生在把手 %d)\n', max_overall_speed, max_speed_idx); fprintf('调整后的龙头速度: %.4f m/s\n', v0_adjusted); if collision_detected fprintf('碰撞发生时间: %.2f s\n', collision_time); else fprintf('无碰撞发生\n'); end % 保存结果到文件 results = struct(); results.optimized_path_length = R1*phi1 + R2*phi2; results.turnaround_time = t_turn_total; results.max_speed = max_overall_speed; results.max_speed_handle = max_speed_handle; results.adjusted_head_speed = v0_adjusted; results.collision_detected = collision_detected; results.collision_time = collision_time; save('bench_dragon_results.mat', 'results'); fprintf('结果已保存到 bench_dragon_results.mat\n');在上面代码的基础上，优化图片的输出，要求画出盘入和盘出的运动轨迹的图，并且可以画出S型相切的结果，，给出完整代码，使其独立可完整运行