模拟无人机在三维景观上的飞行¶

所介绍的 Julia 语言代码实现了对无人驾驶飞行器（UAV）在生成的三维景观上飞行的模拟。该程序包括几个关键步骤。

1.使用三角函数和随机噪声生成景观，以创建逼真的地形。 2.建立无人机运动模型，考虑高度、速度和障碍物探测限制。 3.二维和三维轨迹可视化，以及考虑到探测到的障碍物的飞行动画。

该代码展示了数值方法、线性代数和交互式图形在 Julia 中的应用，使其在自主导航、机器人和地形分析任务中大显身手。

支持库¶

让我们添加辅助对象和库。

LinearAlgebra 连接线性代数运算（矩阵、向量、展开、规范等）的内置库。

plotlyjs() 激活用于交互式可视化的 PlotlyJS 后端。

using LinearAlgebra
plotlyjs()

Plots.PlotlyJSBackend()

生成三维景观¶

** 功能generate_landscape** 生成指定尺寸（width_m × length_m）的三维景观，并将其可视化为一个交互式曲面。

1.函数会创建一个坐标网格：x 和 y - 线性范围从 0 到宽度_m 和长度_m，步长为 1 米。 2.2. 然后填入高程（z）--使用正弦和余弦的组合来创建丘陵表面。 3. 同时添加随机噪音 (rand()) 以增加真实感。 4.4. 此外，该函数还为无人机定义了随机的起点和终点。(绿色--起点，红色--终点）。

关键参数 | 参数 | 类型/值 | 说明 | 参数 | 类型/值 | 说明 | 参数 | 类型/值 | 说明 | 参数 | 类型/值 | 说明 |---------------|--------------------|-----------------------------------| |width_m |Int | Landscape width (metres). |length_m |Int | Landscape length (metres) | 景观长度（米）。 |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.障碍物探测

无人机在 45°的视场（FOV）内以设定的距离扫描空间 (view_distance)：

检查视场中的各个点（12 个光束）。
如果障碍物的高度 (obstacle_height) 威胁到碰撞 (obstacle_height + min_height > текущая высота) ，则将其添加到列表visible_obstacles 。

3.避障 检测到障碍物时，可选择以下选项。

选项 1（50% 的几率）**：侧向移动（左/右在speed ）。
选项 2（50% 的几率）**：超过障碍物的最大高度 +min_height + 1.0 。

4.着陆

到达终点后

缓缓下降10 步至终点处的地势标高。

5.可视化

图 1**：带有起点（绿色）和终点（红色）标记的二维轨迹（XY）。
图 2**：按阶梯计算的海拔高度变化（Z）。

**主要参数 | 参数值说明 |----------|----------|----------| |min_height | -5 m | 离地形的最低高度 |max_height | 5 m | 最大飞行高度 |speed | 1 m/step | 飞行速度 |fov_angle | 45° | 障碍物探测视角 |view_distance | 10 m | 扫描范围 | 10 m

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 шагов

3.无人机飞行可视化¶

无人机飞行可视化算法

1.图形定制

后端**：使用gr() 进行高性能渲染。
帧速率**：默认为fps=10 （可通过参数调整）。

2.数据准备

景观**：分为坐标x, y 和高程z 。
优化**：创建字典xidx,yidx ，用于在景观网格中快速搜索无人机坐标索引。

3.基本图形

三维表面** (base_surface)：
- 颜色方案:terrain 。
- 起点（绿色）和终点（红色）标记。
二维地图** (base_2d)：
- 具有透明度的地形等高线图 (contour!)alpha=0.3 。
- 起点和终点。

4.飞行动画

每帧（i-th步路径）： 1.三维可视化：

无人机当前位置（红点）、
动态边界 (xlims!,ylims!) 以聚焦无人机周围区域、
标头显示距离地形的高度，箭头（↑/↓）表示高度增加/减少。 2.2D 可视化：
逐步显示飞行距离（蓝线）、
当前位置（红点）。 3.障碍：
如果在当前步骤中检测到障碍物，则在 3D 和 2D 中以黄叉 (:xcross) 标记。

五、动画装配

以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)

[ Info: Saved animation to /user/start/examples/codegen/tmp.gif

输出¶

这段代码是模拟无人机在随机地形上飞行的完整解决方案。让我们列举一下它的主要功能。 1.灵活的参数：您可以调整速度、飞行高度、视角和障碍物探测范围。 2.逼真的可视化效果：使用 3D 图形和动画将飞行轨迹可视化。 3.避障算法：无人机会动态调整路线，以避免碰撞。

该代码可作为更复杂模拟的基础，如城市环境中的自主飞行或地形测绘。进一步的开发可包括整合机器学习来优化路线或处理来自真实传感器的数据。