Документация Engee
Notebook

Моделирование полета дрона над 3D-ландшафтом

Представленный код на языке Julia реализует симуляцию полета беспилотного летательного аппарата (БПЛА) над сгенерированным 3D-ландшафтом. Программа включает в себя несколько ключевых этапов.

  1. Генерация ландшафта с помощью тригонометрических функций и случайных шумов для создания реалистичного рельефа.
  2. Моделирование движения дрона с учетом ограничений по высоте, скорости и обнаружения препятствий.
  3. Визуализация траектории в 2D и 3D, а также анимация полета с учетом обнаруженных препятствий.

Код демонстрирует применение численных методов, линейной алгебры и интерактивной графики в Julia, что делает его полезным для задач автономной навигации, робототехники и анализа рельефа.

Вспомогательные библиотеки

Добавим вспомогательные объекты и библиотеки.

LinearAlgebra подключает встроенную библиотеку для линейных алгебраических операций (матрицы, векторы, разложения, нормы и т. д.).

plotlyjs() активирует бэкенд PlotlyJS для интерактивной визуализации.

In [ ]: 
using LinearAlgebra
plotlyjs()
Out[0]:
Plots.PlotlyJSBackend()

Генерация трёхмерного ландшафта

Функция generate_landscape выполняет генерацию 3D-ландшафта заданных размеров (width_m × length_m) и визуализирует его в виде интерактивной поверхности.

  1. Функция создает сетку координат: x и y — линейные диапазоны от 0 до width_m и length_m с шагом 1 метр.
  2. Далее она заполняет высоты (z) — использует комбинацию синуса и косинуса для создания холмистой поверхности.
  3. А также добавляет случайный шум (rand()) для реалистичности.
  4. Поимимо этого функция определяет случайные точки старта и финиша дрона. (зелёный — старт, красный — финиш)

Ключевые параметры

Параметр Тип/Значение Описание
width_m Int Ширина ландшафта (метры).
length_m Int Длина ландшафта (метры).
c=:terrain Symbol Цветовая карта для рельефа.
start/finish Vector{Float64} Координаты [x, y, z].
In [ ]: 
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 м Дальность сканирования
In [ ]: 
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))

Время полёта дрона: 109 шагов
Out[0]:

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-й шаг пути):

  1. 3D-визуализация:
    • текущая позиция дрона (красная точка),
    • динамические границы (xlims!, ylims!) для фокусировки на области вокруг дрона,
    • заголовок с высотой над рельефом и стрелкой (↑/↓), указывающей набор/снижение высоты.
  2. 2D-визуализация:
    • постепенное отображение пройденного пути (синяя линия),
    • текущая позиция (красная точка).
  3. Препятствия:
    • отмечаются желтыми крестами (:xcross) в 3D и 2D, если обнаружены на текущем шаге.

5. Сборка анимации

  • кадры генерируются с интервалом 1/speed (но не реже 1 шага),
  • результирующая анимация сохраняется как GIF с заданным fps.

Ключевые параметры

Параметр Значение/Тип Описание
fps 10 (по умолчанию) Частота кадров анимации (кадров/сек)
speed 1 (м/шаг) Скорость дрона, влияет на плавность анимации
layout @layout([a{0.7w} b]) Соотношение размеров 3D и 2D графиков (70% / 30%)
In [ ]: 
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)

[ Info: Saved animation to /user/start/examples/codegen/tmp.gif
Out[0]:
No description has been provided for this image

Вывод

Данный код представляет собой законченное решение для симуляции полета дрона над случайным ландшафтом. Перечислим его основные особенности.

  1. Гибкость параметров: можно настраивать скорость, высоту полета, угол обзора и дальность обнаружения препятствий.
  2. Реалистичная визуализация: используются 3D-графика и анимация для наглядного представления траектории.
  3. Алгоритм обхода препятствий: дрон динамически корректирует маршрут, избегая столкновений.

Этот код может служить основой для более сложных симуляций типа автономных полетов в городской среде или картографирования местности. Дальнейшее развитие может включать интеграцию машинного обучения для оптимизации маршрутов или обработку данных с реальных датчиков.