% 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型相切的结果,,给出完整代码,使其独立可完整运行
最新发布