Моделирование полета дрона над 3D-ландшафтом
Представленный код на языке Julia реализует симуляцию полета беспилотного летательного аппарата (БПЛА) над сгенерированным 3D-ландшафтом. Программа включает в себя несколько ключевых этапов.
- Генерация ландшафта с помощью тригонометрических функций и случайных шумов для создания реалистичного рельефа.
- Моделирование движения дрона с учетом ограничений по высоте, скорости и обнаружения препятствий.
- Визуализация траектории в 2D и 3D, а также анимация полета с учетом обнаруженных препятствий.
Код демонстрирует применение численных методов, линейной алгебры и интерактивной графики в Julia, что делает его полезным для задач автономной навигации, робототехники и анализа рельефа.
Вспомогательные библиотеки
Добавим вспомогательные объекты и библиотеки.
LinearAlgebra подключает встроенную библиотеку для линейных алгебраических операций (матрицы, векторы, разложения, нормы и т. д.).
plotlyjs() активирует бэкенд PlotlyJS для интерактивной визуализации.
using LinearAlgebra
plotlyjs()
Генерация трёхмерного ландшафта
Функция generate_landscape
выполняет генерацию 3D-ландшафта заданных размеров (width_m × length_m) и визуализирует его в виде интерактивной поверхности.
- Функция создает сетку координат: x и y — линейные диапазоны от 0 до width_m и length_m с шагом 1 метр.
- Далее она заполняет высоты (z) — использует комбинацию синуса и косинуса для создания холмистой поверхности.
- А также добавляет случайный шум (rand()) для реалистичности.
- Поимимо этого функция определяет случайные точки старта и финиша дрона. (зелёный — старт, красный — финиш)
Ключевые параметры
Параметр | Тип/Значение | Описание |
---|---|---|
width_m |
Int |
Ширина ландшафта (метры). |
length_m |
Int |
Длина ландшафта (метры). |
c=:terrain |
Symbol |
Цветовая карта для рельефа. |
start/finish |
Vector{Float64} |
Координаты [x, y, z]. |
function generate_landscape(width_m, length_m)
x = 0:1:width_m
y = 0:1:length_m
z = [10(sin(xi / 15) + cos(yi / 15)) + 5rand() + 10 for xi in x, yi in y]
z = reverse(z, dims=(1, 2))
random_yi_start = rand(1:length(y))
random_yi_finish = rand(1:length(y))
xi_start = 10
xi_finish = length(x)-10
start = [x[xi_start], y[random_yi_start], z[xi_start, random_yi_start]]
finish = [x[xi_finish], y[random_yi_finish], z[xi_finish, random_yi_finish]]
plt = surface(x, y, z', c=:terrain, xlabel="X (м)", ylabel="Y (м)", zlabel="Z (м)", title="3D ландшафт", legend=false)
scatter!(plt, [start[1]], [start[2]], [start[3]+3], marker=(:circle), color=:green)
scatter!(plt, [finish[1]], [finish[2]], [finish[3]+3], marker=(:circle), color=:red)
display(plt)
return x, y, z, start, finish
end
landscape = generate_landscape(100, 100);
Симуляция поведения дрона
Алгоритм поведения дрона в симуляции
1. Основная логика движения
Дрон перемещается от стартовой точки (start
) к финишной (finish
) по прямой траектории, соблюдая ограничения:
- скорость: дрон перемещается на фиксированное расстояние (
speed
) за шаг. - высота: поддерживает высоту не ниже
min_height
над рельефом и не вышеmax_height
. - коррекция границ: не вылетает за пределы ландшафта (
clamp
).
2. Обнаружение препятствий
Дрон сканирует пространство в поле зрения (FOV) 45° с заданной дистанцией (view_distance
):
- проверяет точки в секторе обзора (12 лучей).
- если высота препятствия (
obstacle_height
) угрожает столкновением (obstacle_height + min_height > текущая высота
), добавляет его в списокvisible_obstacles
.
3. Обход препятствий
При обнаружении препятствий есть следующие варианты.
- Вариант 1 (50% chance): смещается вбок (влево/вправо на
speed
). - Вариант 2 (50% chance): поднимается выше максимальной высоты препятствия +
min_height + 1.0
.
4. Посадка
По достижении финиша
- плавно снижается за 10 шагов до высоты рельефа в конечной точке.
5. Визуализация
- График 1: 2D-траектория (XY) с отметками старта (зеленый) и финиша (красный).
- График 2: изменение высоты (Z) по шагам.
Ключевые параметры
Параметр | Значение | Описание |
---|---|---|
min_height |
-5 м | Минимальная высота над рельефом |
max_height |
5 м | Максимальная высота полета |
speed |
1 м/шаг | Скорость перемещения |
fov_angle |
45° | Угол обзора для обнаружения препятствий |
view_distance |
10 м | Дальность сканирования |
function simulate_drone(landscape, min_height, max_height, speed; fov_angle=45.0, view_distance=10.0)
x, y, z, start, finish = landscape
drone_path = [copy(start)]
drone_heights = [start[3]]
obstacles = [[]]
current_pos = copy(start)
for _ in 1:1000
if norm(current_pos[1:2] .- finish[1:2]) < speed
break
end
direction = normalize(finish[1:2] .- current_pos[1:2])
next_pos = current_pos[1:2] .+ direction * speed
next_pos[1] = clamp(next_pos[1], x[1], x[end])
next_pos[2] = clamp(next_pos[2], y[1], y[end])
xi = argmin(abs.(x .- next_pos[1]))
yi = argmin(abs.(y .- next_pos[2]))
terrain_height = z[xi, yi]
new_z = clamp(max(terrain_height + min_height, current_pos[3]), -Inf, max_height)
visible_obstacles = Tuple{Float64, Float64, Float64}[]
for angle in range(-fov_angle/2, fov_angle/2, length=12)
rad = deg2rad(angle)
dir = [direction[1]*cos(rad) - direction[2]*sin(rad),
direction[1]*sin(rad) + direction[2]*cos(rad)]
check_pos = next_pos .+ dir * view_distance
xi_check = argmin(abs.(x .- check_pos[1]))
yi_check = argmin(abs.(y .- check_pos[2]))
if 1 ≤ xi_check ≤ size(z, 1) && 1 ≤ yi_check ≤ size(z, 2)
obstacle_height = z[xi_check, yi_check]
if obstacle_height + min_height > new_z
push!(visible_obstacles, (x[xi_check], y[yi_check], obstacle_height))
end
end
end
if !isempty(visible_obstacles)
max_obstacle = maximum(obs[3] for obs in visible_obstacles)
if max_obstacle + min_height > new_z
if rand() < 0.5
next_pos[1] = clamp(next_pos[1] + rand([-1.0, 1.0]) * speed, x[1], x[end])
else
new_z = max_obstacle + min_height + 1.0
end
end
end
current_pos = [next_pos[1], next_pos[2], new_z]
push!(drone_path, copy(current_pos))
push!(drone_heights, new_z)
push!(obstacles, visible_obstacles)
end
xi_end = argmin(abs.(x .- finish[1]))
yi_end = argmin(abs.(y .- finish[2]))
final_ground = z[xi_end, yi_end]
for _ in 1:10
new_z = drone_path[end][3] - (drone_path[end][3] - final_ground) / 10
push!(drone_path, [finish[1], finish[2], new_z])
push!(drone_heights, new_z)
push!(obstacles, [])
end
println("Время полёта дрона: $(length(drone_path)) шагов")
return drone_path, drone_heights, obstacles, start, finish
end
speed = 1
drone_path, drone_heights, obstacles, start, finish = simulate_drone(landscape, -5, 5, speed)
path_x = [p[1] for p in drone_path]
path_y = [p[2] for p in drone_path]
path_z = [p[3] for p in drone_path]
t = 1:length(drone_path)
plt1 = plot(path_x, path_y, lw=2, c=:blue, xlabel="X (м)", ylabel="Y (м)", title="Путь дрона", legend=false)
scatter!(plt1, [path_x[1]], [path_y[1]], marker=:circle, markersize=8, color=:green)
scatter!(plt1, [path_x[end]], [path_y[end]], marker=:circle, markersize=8, color=:red)
plt2 = plot(t, path_z, lw=2, c=:purple, xlabel="Шаги", ylabel="Высота (м)", title="Высоты дрона", legend=false)
scatter!(plt2, [t[1]], [path_z[1]], marker=:circle, markersize=8, color=:green)
scatter!(plt2, [t[end]], [path_z[end]], marker=:circle, markersize=8, color=:red)
plot(plt1, plt2, layout=(1, 2), size=(1000, 400))
3. Визуализация полёта дрона
Алгоритм визуализации полета дрона
1. Настройка графики
- Бэкенд: используется
gr()
для высокопроизводительной отрисовки. - Частота кадров: по умолчанию
fps=10
(регулируется параметром).
2. Подготовка данных
- Ландшафт: разделение на координаты
x, y
и высотыz
. - Оптимизация: создаются словари
xidx
,yidx
для быстрого поиска индексов координат дрона в сетке ландшафта.
3. Базовые графики
- 3D-поверхность (
base_surface
):- цветовая схема
:terrain
. - отметки старта (зеленый) и финиша (красный).
- цветовая схема
- 2D-карта (
base_2d
):- контурный график рельефа (
contour!
) с прозрачностьюalpha=0.3
. - точки старта и финиша.
- контурный график рельефа (
4. Анимация полета
Для каждого кадра (i
-й шаг пути):
- 3D-визуализация:
- текущая позиция дрона (красная точка),
- динамические границы (
xlims!
,ylims!
) для фокусировки на области вокруг дрона, - заголовок с высотой над рельефом и стрелкой (↑/↓), указывающей набор/снижение высоты.
- 2D-визуализация:
- постепенное отображение пройденного пути (синяя линия),
- текущая позиция (красная точка).
- Препятствия:
- отмечаются желтыми крестами (
:xcross
) в 3D и 2D, если обнаружены на текущем шаге.
- отмечаются желтыми крестами (
5. Сборка анимации
- кадры генерируются с интервалом
1/speed
(но не реже 1 шага), - результирующая анимация сохраняется как GIF с заданным
fps
.
Ключевые параметры
Параметр | Значение/Тип | Описание |
---|---|---|
fps |
10 (по умолчанию) |
Частота кадров анимации (кадров/сек) |
speed |
1 (м/шаг) |
Скорость дрона, влияет на плавность анимации |
layout |
@layout([a{0.7w} b]) |
Соотношение размеров 3D и 2D графиков (70% / 30%) |
gr()
function visualize_flight(landscape, drone_path, drone_heights, obstacles, speed, start, finish; fps=10)
x, y, z = landscape
xidx = Dict(px => argmin(abs.(x .- px)) for px in unique(p[1] for p in drone_path))
yidx = Dict(py => argmin(abs.(y .- py)) for py in unique(p[2] for p in drone_path))
base_surface = surface(x, y, z', c=:terrain, legend=false, aspect_ratio=:auto, axis=false)
base_2d = plot(aspect_ratio=1, legend=false, axis=false)
contour!(base_2d, x, y, z', c=:terrain, alpha=0.3)
scatter!(base_surface, [start[1]], [start[2]], [start[3]], marker=:circle, markercolor=:green, label=false)
scatter!(base_surface, [finish[1]], [finish[2]], [z[end, end]], marker=:circle, markercolor=:red, label=false)
scatter!(base_2d, [start[1]], [start[2]], marker=:circle, markercolor=:green)
scatter!(base_2d, [finish[1]], [finish[2]], marker=:circle, markercolor=:red)
anim = @animate for i in 1:length(drone_path)
plt1 = deepcopy(base_surface)
plt2 = deepcopy(base_2d)
px, py, pz = drone_path[i]
scatter!(plt1, [px], [py], [pz], markersize=5, markercolor=:red, marker=:circle)
xlims!(plt1, px - 10, px + 10)
ylims!(plt1, py - 10, py + 10)
xi, yi = xidx[px], yidx[py]
height_above = pz - z[xi, yi]
arrow = i == 1 ? "↑" : (height_above > (drone_path[i - 1][3] - z[xidx[drone_path[i - 1][1]], yidx[drone_path[i - 1][2]]]) ? "↑" : "↓")
title!(plt1, "Высота дрона: $(floor(height_above)) м $arrow")
if i > 1
path = drone_path[1:i]
plot!(plt2, [p[1] for p in path], [p[2] for p in path], linewidth=2, linecolor=:blue)
end
scatter!(plt2, [px], [py], markersize=5, markercolor=:red)
if !isempty(obstacles[i])
obs_x = [o[1] for o in obstacles[i]]
obs_y = [o[2] for o in obstacles[i]]
obs_z = [o[3] for o in obstacles[i]]
scatter!(plt1, obs_x, obs_y, obs_z, markersize=3, markercolor=:yellow, marker=:xcross)
scatter!(plt2, obs_x, obs_y, markersize=3, markercolor=:yellow, marker=:xcross)
end
plot(plt1, plt2, layout = @layout([a{0.7w} b]), size=(900, 300))
end every max(1, round(Int, 1/speed))
gif(anim, fps=fps)
end
visualize_flight(landscape, drone_path, drone_heights, obstacles, speed, start, finish; fps=5)
Вывод
Данный код представляет собой законченное решение для симуляции полета дрона над случайным ландшафтом. Перечислим его основные особенности.
- Гибкость параметров: можно настраивать скорость, высоту полета, угол обзора и дальность обнаружения препятствий.
- Реалистичная визуализация: используются 3D-графика и анимация для наглядного представления траектории.
- Алгоритм обхода препятствий: дрон динамически корректирует маршрут, избегая столкновений.
Этот код может служить основой для более сложных симуляций типа автономных полетов в городской среде или картографирования местности. Дальнейшее развитие может включать интеграцию машинного обучения для оптимизации маршрутов или обработку данных с реальных датчиков.