101 lines
3.3 KiB
Mathematica
101 lines
3.3 KiB
Mathematica
|
% 所有路径数据定义
|
|||
|
|
paths = {
|
|||
|
|
[2,2; 5,5; 7,5; 11,9; 11,10; 10,11; 18,19; 19,19], 'A*';
|
|||
|
|
[2,2; 9,2; 9,6; 11,6; 11,10; 10,10; 10,12; 13,12; 13,13; 17,13; 17,14; 18,14; 18,18; 18,19; 19,19], 'Dijkstra';
|
|||
|
|
[2,2; 7,2; 10,6; 14,3; 18,7; 14,11; 10,13; 10,18; 15,17; 19,19], 'RTT_one';
|
|||
|
|
[2,2; 7,2; 10,6; 5,7; 3,11; 7,14; 11,12; 16,14; 16,19; 19,19], 'RTT_two';
|
|||
|
|
[2,2; 7,2; 10,6; 5,8; 2,12; 2,17; 7,17; 11,15; 16,16; 19,19], 'RTT_three';
|
|||
|
|
[2,2; 7,2; 10,6; 15,5; 11,9; 9,14; 13,17; 18,19; 19,19], 'RTT_four';
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
% 模拟参数
|
|||
|
|
v_normal = 10; % 直线速度 m/s
|
|||
|
|
a_turn = -2; % 拐弯减速加速度 m/s^2
|
|||
|
|
dt = 0.1; % 时间步长
|
|||
|
|
|
|||
|
|
% 遍历每一组路径
|
|||
|
|
for k = 1:size(paths, 1)
|
|||
|
|
points = paths{k,1};
|
|||
|
|
group_name = paths{k,2};
|
|||
|
|
|
|||
|
|
time = 0;
|
|||
|
|
time_series = [];
|
|||
|
|
speed_series = [];
|
|||
|
|
acc_series = [];
|
|||
|
|
total_distance = 0;
|
|||
|
|
total_time = 0;
|
|||
|
|
|
|||
|
|
for i = 1:size(points, 1) - 1
|
|||
|
|
p1 = points(i, :);
|
|||
|
|
p2 = points(i + 1, :);
|
|||
|
|
segment_vector = p2 - p1;
|
|||
|
|
segment_distance = norm(segment_vector);
|
|||
|
|
total_distance = total_distance + segment_distance;
|
|||
|
|
|
|||
|
|
if i > 1
|
|||
|
|
prev_vec = points(i, :) - points(i - 1, :);
|
|||
|
|
angle = acos(dot(prev_vec, segment_vector) / (norm(prev_vec) * norm(segment_vector)));
|
|||
|
|
delta_v = abs(v_normal * sin(angle / 2));
|
|||
|
|
|
|||
|
|
if delta_v > 0
|
|||
|
|
t_decel = delta_v / abs(a_turn);
|
|||
|
|
t_accel = t_decel;
|
|||
|
|
|
|||
|
|
% 减速段
|
|||
|
|
t = 0:dt:t_decel;
|
|||
|
|
v_decel = v_normal - abs(a_turn) * t;
|
|||
|
|
a_decel = -abs(a_turn) * ones(size(t));
|
|||
|
|
|
|||
|
|
% 加速段
|
|||
|
|
t2 = 0:dt:t_accel;
|
|||
|
|
v_accel = v_normal - delta_v + abs(a_turn) * t2;
|
|||
|
|
a_accel = abs(a_turn) * ones(size(t2));
|
|||
|
|
|
|||
|
|
% 拼接
|
|||
|
|
time_series = [time_series, time + t];
|
|||
|
|
speed_series = [speed_series, v_decel];
|
|||
|
|
acc_series = [acc_series, a_decel];
|
|||
|
|
time = time + t_decel;
|
|||
|
|
|
|||
|
|
time_series = [time_series, time + t2];
|
|||
|
|
speed_series = [speed_series, v_accel];
|
|||
|
|
acc_series = [acc_series, a_accel];
|
|||
|
|
time = time + t_accel;
|
|||
|
|
|
|||
|
|
total_time = total_time + t_decel + t_accel;
|
|||
|
|
end
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
% 匀速段
|
|||
|
|
t_segment = segment_distance / v_normal;
|
|||
|
|
t = 0:dt:t_segment;
|
|||
|
|
v_const = v_normal * ones(size(t));
|
|||
|
|
a_const = zeros(size(t));
|
|||
|
|
|
|||
|
|
time_series = [time_series, time + t];
|
|||
|
|
speed_series = [speed_series, v_const];
|
|||
|
|
acc_series = [acc_series, a_const];
|
|||
|
|
time = time + t_segment;
|
|||
|
|
total_time = total_time + t_segment;
|
|||
|
|
end
|
|||
|
|
|
|||
|
|
% 输出总路程与用时
|
|||
|
|
fprintf('%s：总路程 = %.2f 米，总用时 = %.2f 秒\n', group_name, total_distance, total_time);
|
|||
|
|
|
|||
|
|
% 绘图
|
|||
|
|
figure('Name', group_name, 'NumberTitle', 'off');
|
|||
|
|
subplot(2,1,1);
|
|||
|
|
plot(time_series, speed_series, 'b', 'LineWidth', 2);
|
|||
|
|
xlabel('时间 (s)');
|
|||
|
|
ylabel('速度 (m/s)');
|
|||
|
|
title([group_name ' - 速度-时间图']);
|
|||
|
|
grid on;
|
|||
|
|
|
|||
|
|
subplot(2,1,2);
|
|||
|
|
plot(time_series, acc_series, 'r', 'LineWidth', 2);
|
|||
|
|
xlabel('时间 (s)');
|
|||
|
|
ylabel('加速度 (m/s²)');
|
|||
|
|
title([group_name ' - 加速度-时间图']);
|
|||
|
|
grid on;
|
|||
|
|
end
|