Сообщество Engee
Engee Community
/
Примеры и проекты
/
Моделирование полета дрона (облет ориентиров)

Моделирование полета дрона (облет ориентиров)

Автор
avatar-bbobbybbobby
Notebook 
]add LinearAlgebra

   Resolving package versions...
   Installed SciMLJacobianOperators ─ v0.1.8
   Installed EnzymeCore ───────────── v0.8.12
   Installed MLDataDevices ────────── v1.11.1
   Installed DiffEqBase ───────────── v6.183.1
   Installed PreallocationTools ───── v0.4.31
   Installed FiniteDiff ───────────── v2.28.0
   Installed Roots ────────────────── v2.2.8
   Installed DelayDiffEq ──────────── v5.58.0
   Installed ControlSystems ───────── v1.14.0
  No Changes to `~/.project/Project.toml`
  No Changes to `~/.project/Manifest.toml`

]add Colors

   Resolving package versions...
   Installed SciMLJacobianOperators ─ v0.1.8
   Installed MLDataDevices ────────── v1.11.1
   Installed EnzymeCore ───────────── v0.8.12
   Installed FiniteDiff ───────────── v2.28.0
   Installed DiffEqBase ───────────── v6.183.1
   Installed PreallocationTools ───── v0.4.31
   Installed DelayDiffEq ──────────── v5.58.0
   Installed Roots ────────────────── v2.2.8
   Installed ControlSystems ───────── v1.14.0
    Updating `~/.project/Project.toml`
⌅ [5ae59095] + Colors v0.12.11
  No Changes to `~/.project/Manifest.toml`

using LinearAlgebra, Colors
Plots.PlotlyJSBackend()
const deltaH_mark=10

10
const θcam = 30
const steps_around_group = 24
const max_steps_around = 72
const min_radius_flyby = 1.0
const plateau = 10
const MaxSteps = 1000
const pract_ceiling = 33.0
const minh_over_terrain = 3.0
const descend_speed = 2.0
const steps_descend = 10

WARNING: redefinition of constant Main.pract_ceiling. This may fail, cause incorrect answers, or produce other errors.

10
function generate_landscape(width_m, length_m)
    X = 0:1:width_m
    Y = 0:1:length_m

    Z = [10(sin(xi / 10) + cos(yi / 15)) + 5rand() + plateau 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] + minh_over_terrain]
    Finish = [X[xi_finish], Y[random_yi_finish], Z[xi_finish, random_yi_finish]]

    plt1 = surface(X, Y, Z', c=:terrain, xlabel="X (м)", ylabel="Y (м)", zlabel="Z (м)",
                            title="3D ландшафт", size=(800,800), legend=false, camera = (15, 85))
    scatter!(plt1, [Start[1]], [Start[2]], [Start[3]+deltaH_mark], marker=(:diamond), markersize=5, color=:orange)
    scatter!(plt1, [Finish[1]], [Finish[2]], [Finish[3]+deltaH_mark], marker=(:xcross), markersize=5, color=:red)
#    println(typeof(plt1))
    landscape = X, Y, Z, Start, Finish
#    return x, y, z, start, finish, plt
    return landscape, plt1
end

generate_landscape (generic function with 1 method)
function generate_refpoints(ref_count, plt, ls_x, ls_y, ls_z,
                            refstartX=last(ls_x)÷5, refstartY=last(ls_y)÷5,
                            refendX=last(ls_x)-refstartX, refendY=last(ls_y)-refstartY)
    RefPoints = Tuple{Int64, Int64, Float64}[]
    
    for _ in 1:ref_count
        xi = rand(refstartX:refendX)
        yi = rand(refstartY:refendY)
        zi = ls_z[xi, yi]
        push!(RefPoints, (xi, yi, zi))
        scatter!(plt, [xi], [yi], [zi+deltaH_mark], marker=(:circle), color=:yellow)
    end
#    println("refpoints:", refpoints)
#    println("Type of refpoints:", typeof(refpoints))
    return RefPoints
end

generate_refpoints (generic function with 5 methods)
function put_start_finish(X, Y, Z, plt)
    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]]
    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 Start, Finish
end

LScape, plt = generate_landscape(150, 150)
Xls, Yls, Zls, Start, Finish = LScape
maxdepth = Base.minimum(Zls)
maxheight = Base.maximum(Zls)
RefPts = generate_refpoints(45, plt, Xls, Yls, Zls)
#println("Type of refs:", typeof(RefPts))
display(plt)
println("max_h=", maxheight)
println("max_d=", maxdepth)

max_h=34.85883187223033
max_d=-9.879538550047247

Дрон имеет бортовую камеру, смотрящую вертикально вниз, с заданным углом обзора 2θ, которая отслеживает попадание ориентиров в поле своего обзора. Если по пути движения дрона камера засекает ориентир, дрон изменяет траекторию так, чтобы облететь этот ориентир по оптимальной траектории (не уменьшая высоту полета, производит облет по оптимальному радиусу в оптимальном направлении). После облета ориентир добавляется в список обнаруженных, и дрон далее продолжает полет в направлении цели по первоначальному алгоритму облета препятствий. Требуется рассчитывать область пересечения поверхностей обзора камеры дрона и ландшафта в каждой точке траектории дрона, а также проверять, принадлежат ли координаты ориентира рассчитанной области пересечения по компонентам (x, y), хотя на практике такая задача вообще отсутствует - камера либо видит ориентир, либо нет. Задача также включает в себя подзадачу нахождения затенений ландшафтом низкорасположенных ориентиров. Представляет интерес поведение дрона при близком расположении ориентиров или попадании нескольких в поле камеры.

Упрощенный вариант вычисления поля камеры (не реализован)

Для координаты z ориентира вычисляется вероятность его обнаружения, заданная соотношением P(h) = Pmin * (1/Pmin)^(h/hmax), где hmax=maxheight - maxdepth, а h = z_ref - maxdepth. Все координаты ориентиров проверяются на предмет попадания внутрь поля обзора в плоскости (x,y) с учетом вероятности их обнаружения. Вероятность 1 соответствует диаметру обзора в основании конуса с координатой z_ref = maxheight ориентира. Pmin - вероятность обнаружения ориентира на максимальной глубине ландшафта.

(x_ref - x0)^2 + (y_ref - y0)^2 <= (1/4) * [2cone_r*(z0 - z_ref)/cone_h]^2 * P(h)

Обнаруженный ориентир облетается дроном по оптимальной траектории. Задача - получить наиболее гладкую траекторию по (x,y,z) компонентам.

function camera_beam(x0, y0, z0, θ)
    n=128
    
    cone_h = z0-maxdepth
    tanθ = 0
#    try
    if (Base.abs(θ) < 89.0)
        tanθ = tand(θ)
    else
#    catch
#        println("Поймана ошибка: ", e)
        return nothing
    end

    cone_r = cone_h*tanθ

    cone_base = LinRange(0, cone_r, n)
    z = LinRange(0, 1, n)
#    z = LinRange(0, cone_h, n)
    ϕ = LinRange(0, 2π, n)
    
    println("cone_h=", cone_h)
    println("cone_r=", cone_r)
    println("cone_base=", cone_base)
    println("z=", z)

    x1 = x0.+ [u * cone_r * cos(v) for u in z, v in ϕ]
    y1 = y0.+ [u * cone_r * sin(v) for u in z, v in ϕ]
    z1 = z0.+ [-cone_h*u for u in z, _ in ϕ]
#    z1 = z0.+ [-u for u in z, _ in ϕ]
    cone_plt = Plots.surface(x1, y1, z1, c=:jet, legend=false, size=(600,600), cbar=:none)
#    return z1
end

#cone_surf_z = camera_beam(30,40,50,θcam)

camera_beam (generic function with 1 method)
using Interpolations

function calc_refpoints_visible(ownposition, Z_ls, θ, Refs::AbstractVector{<:Tuple})
    n = 256
    
    x0, y0, z0 = ownposition
    z = LinRange(maxheight, maxdepth, n)
    θ = Base.abs(θ)

    refpts_detected = Tuple{Int64, Int64}[]
    pt_cntr = 0
    for t in Refs
#        println("Ref[]= ", t)
        Sref = sqrt((t[1] - x0)^2 + (t[2] - y0)^2)
#        println("Sref=", Sref)

        if (z0 < t[3])  # if drone position is lower than z-coordinate of a refpoint
            continue    # then don't take this refpoint
        end
        
        tanψ = +Inf
        try
            tanψ = Sref / (z0-t[3])
        catch
            continue
        end
        
#        println("tanψ=",tanψ)
        if (Base.abs(tanψ) < tand(θ)) && (Sref > 0)
            cosϕ = (t[1]-x0) / Sref
            sinϕ = (t[2]-y0) / Sref

#            println("cosϕ=", cosϕ)
            for i in 1:n
#                println("z[i]=", z[i])
                Sk = (z0 - z[i]) * tanψ
                xk = x0 + Sk*cosϕ
                yk = y0 + Sk*sinϕ
                xk1 = trunc(Int64, floor(xk))
                xk2 = trunc(Int64, ceil(xk))
                yk1 = trunc(Int64, floor(yk))
                yk2 = trunc(Int64, ceil(yk))
#                println(xk1, " ", yk1)
#                println(xk2, " ", yk2)

                Vicinity = [Z_ls[xk1, yk1] Z_ls[xk2, yk1];
                            Z_ls[xk1, yk2] Z_ls[xk2, yk2]]
                itp = interpolate(Vicinity, BSpline(Linear()))
                if (xk1 == xk2) && (yk1 == yk2)
                    idxX = 1
                    idxY = 1
                elseif (xk1 == xk2)
                    idxX = 1
                    idxY = 1+Base.abs((yk-yk1)/(yk2-yk1))
                elseif (yk1 == yk2)
                    idxY = 1
                    idxX = 1+Base.abs((xk-xk1)/(xk2-xk1))
                else
                    idxX = 1+Base.abs((xk-xk1)/(xk2-xk1))
                    idxY = 1+Base.abs((yk-yk1)/(yk2-yk1))
                end

                vzLS = itp(idxX, idxY)
                
                if (z[i] <= vzLS)
#                    println("(xk1,yk1):", xk1, " ", yk1)
#                    println("(xk2,yk2):", xk2, " ", yk2)

                    dist = sqrt((xk - t[1])^2 + (yk - t[2])^2)
                    if (dist < sqrt(2))
                        push!(refpts_detected,(t[1], t[2])) #store (x,y) coordinates of a refpoint
#                        println("--> pt_cntr[i]: ", pt_cntr, "[", i, "]", " caught refpt[", t[1],",", t[2], "]")
                        pt_cntr += 1
                    else
#                        println("xk=", xk)
#                        println("yk=", yk)
#                        println("dist = ", dist)
                        println("Landscape hedge blocks a refpt[$(t[1]),$(t[2])]\n")
                    end
                    
#                    println("Vicinity=", Vicinity)
#                    println("vzLS=", vzLS)
#                    println("z[i]=", z[i])
#                    println("\n")
                    break
                end
            end
        else
#            println("Contitions are not met!\n")
        end
    end
    return refpts_detected
end

calc_refpoints_visible (generic function with 1 method)
function Calc_Radius_Increment( radius, curr_pos, RefCenter, RefPts::AbstractVector{<:Tuple} )
    radius_flyby_inc = 0.0
    curr_posX, curr_posY = curr_pos
    ref_cenX, ref_cenY = RefCenter
    
    RefCen_vec = [ref_cenX - curr_posX, ref_cenY - curr_posY]

#    RefPts_XY = first.(RefPts)
    
    for t in RefPts
        ref_point = first.(t)
#        println("ref_point=", ref_point)
        EachRefPt_vec = [ref_point[1] - curr_posX, ref_point[2] - curr_posY]
        dot_pr = dot(RefCen_vec, EachRefPt_vec)
        if (dot_pr < 0)
            radius_flyby_inc = 1.2 * radius / steps_around_group
            println("radius flyby increment", radius_flyby_inc)
            break
        end
    end
    
    return radius_flyby_inc
end

Calc_Radius_Increment (generic function with 1 method)
function calc_Distances( arrA::AbstractVector{<:Tuple}, pos )
    Distances = sqrt.( (first.(arrA) .- first(pos)).^2 + (last.(arrA) .- last(pos)).^2 )
    return Distances
end

calc_Distances (generic function with 1 method)
function calcGroup_CenterXY(Pts_group::AbstractVector{<:Tuple})
    radius = 0.0
    Group_Center = Tuple{Float64, Float64}
    
    if !isempty(Pts_group)
        RefPts_grp_xy = first.(Pts_group) # extract x,y coorduinates
        RefPts_grp_x = map(t -> t[1], RefPts_grp_xy)
        RefPts_grp_y = map(t -> t[2], RefPts_grp_xy)
        Group_Center = (sum(RefPts_grp_x)/length(RefPts_grp_xy),
                         sum(RefPts_grp_y)/length(RefPts_grp_xy)) # found a geometric center of a group of points or one point

        if length(Pts_group) > 1 # fly around a group, calculate radius of flying around
            println("<c> Group_Center=", Group_Center)
#            println("<c> first.(Pts_group)=", first.(Pts_group))
                
            tmp = first.(Pts_group)
            RefPts_distances = sqrt.( (first.(tmp) .- first(Group_Center)).^2 +
                                      (last.(tmp) .- last(Group_Center)).^2 )
                
#            println("<c> RefPts_distances=", RefPts_distances)
            radius = 1.1 * maximum(RefPts_distances)
            println("<c> flying radius=", radius)
        end
#    else
#        println("ERROR array Pts_group size")
    end
    
    return radius, Group_Center
end

calcGroup_CenterXY (generic function with 1 method)
function Get_Refpts_byRefGroup( AllRefPts::AbstractVector{<:Tuple}, group )
    RefPts_grp = Tuple{Tuple{Int64, Int64}, Int64}[] #  === DEBUG === option for get a group quickly
    RefPts_center = Tuple{Float64, Float64}
    
    flying_radius = 0.0
#    println("AllRefPts=", AllRefPts)
#    println("get by group:", group)

    if !isempty(AllRefPts)
        for p in AllRefPts
            if last(p) == group  # if the group of RefPts[i] is equal to given
                push!(RefPts_grp, p)
            end
        end

        flying_radius, RefPts_center = calcGroup_CenterXY(RefPts_grp)
#        println("RefPts_grp = ", RefPts_grp, "\n")
    end

    return RefPts_grp, RefPts_center, flying_radius
end

Get_Refpts_byRefGroup (generic function with 1 method)
function Get_Refpts_byRefPts( AllRefPts::AbstractVector{<:Tuple}, Detected_Refs::AbstractVector{<:Tuple}, Flown_gr::AbstractVector{<:Int64}, curr_pos )
    RefPts_grp = Tuple{Tuple{Int64, Int64}, Int64}[] #  === DEBUG === option for get a group quickly
    RefPts_center = Tuple{Float64, Float64}

    flying_radius = 0.0
#    println("AllRefPts=", AllRefPts)
    
    if !isempty(AllRefPts)
        RefPts_XY = first.(AllRefPts)

        for p in Detected_Refs
            if (p in RefPts_XY)
#                println("Get by coords: ", p)
                idx = findall(x -> x == p, RefPts_XY)
#                println("idx=", idx)

                if (length(idx) == 1) # each detected point must have been added earlier in one group only!
                    group = last(AllRefPts[idx[1]])
                    
                    if isempty(Flown_gr)
                        point = (p, last(AllRefPts[idx[1]]) )
                        push!( RefPts_grp, point )
                    elseif !(group in Flown_gr)
                        point = (p, last(AllRefPts[idx[1]]) )
                        push!( RefPts_grp, point )
                    end
                else
                    println("!!! ERROR of Refpoints Group numbering")
                end

            end
        end

        if ( length(RefPts_grp) > 1 )
#            println("RefPts_grp=", RefPts_grp)
            Groups = last.(RefPts_grp)
            elem1 = Groups[1]
#            println("------->Groups = ", Groups)
           
            if !all(x -> x==elem1, Groups) # if there are different groups in array, then keep only the clothest point and its group
                Distances = calc_Distances(first.(RefPts_grp), curr_pos)
#                println("Distances = ", Distances)
                
                idx = argmin(Distances)
                
                grp = last(RefPts_grp[idx])
                RefPts_grp = filter!( x -> (last(x)==grp), RefPts_grp)
#            else
#                println("ALL ELEMENTS ARE EQUAL!")
            end
        end
        
        flying_radius, RefPts_center = calcGroup_CenterXY(RefPts_grp)
        
#        if !isempty(RefPts_grp)
#            println("RefPts_grp = ", RefPts_grp, "\n")
#        end
    end

    return RefPts_grp, RefPts_center, flying_radius
end

Get_Refpts_byRefPts (generic function with 1 method)
function simulate_drone(Landscape, h_over_lscape, service_ceiling, speed; fov_angle=45.0, view_distance=10.0)
    if (h_over_lscape * service_ceiling <= 0) || (service_ceiling <= plateau) # nonsense conditions
        return
    end
    X, Y, Z, Start, Finish = Landscape
    
    if (Start[3] > service_ceiling)
        println("!!! Flight started too high: ceiling = $(service_ceiling), started at $(Start[3])")
    end
    
    drone_path = [copy(Start)]
    drone_heights = [Start[3]]
    gnd_height_below = [Start[3]]
    obstacles = [[]]
    dir_around = [0.0, 0.0]
    direction = [0.0, 0.0]
    curr_direction = [0.0, 0.0]
    curr_speed = speed
    #stack_of_dir = []

    All_RefPts = Tuple{Tuple{Int64, Int64}, Int64}[]
    RefPts_Group = Tuple{Tuple{Int64, Int64}, Int64}[]
    RefPtsGr_center = Tuple{Float64, Float64}
    FlownGroups = Int64[]
    
    curr_group = ref_group = 0
    radius_flyby = 0.0
    radius_flyby_inc = 0.0
    min_refpt_dist = -Inf
    Φr = 0.0
    steps_around = 0
    
    current_pos = copy(Start)
    for _ in 1:MaxSteps
        if norm(current_pos[1:2] .- Finish[1:2]) < curr_speed
            break
        end

        direction = normalize(Finish[1:2] .- current_pos[1:2]) # always estimate targer direction
        Detected_Refs = calc_refpoints_visible(current_pos, Z, θcam, RefPts)
        
        if !isempty(Detected_Refs)  # if refpoints have been detected
            
            ref_points_xy = first.(All_RefPts)                     # extract coordinates x,y from array of All_RefPts detected before
            
            added_newpts = 0
            
            for i in 1:length(Detected_Refs)                       # by one check if coordinates of the detected points are already in All_RefPts
                if ((Detected_Refs[i] in ref_points_xy) == false)  # if these refpoints are new ones
                    added_newpts += 1
                    push!(All_RefPts, (Detected_Refs[i], ref_group+1)) # push them by one into All_RefPts with (ref_group+1) for all of them
                end
            end
            
            if (added_newpts > 0)
                ref_group += 1
            end
            
            if (curr_group == 0) # if fly towards the target, then can process detected points
                if (added_newpts > 0) # get new refpoints by their group
                    RefPts_Group, RefPtsGr_center, radius_flyby = Get_Refpts_byRefGroup(All_RefPts, ref_group)
                    
                    if (added_newpts != length(RefPts_Group))
                        println("!!! ERROR of extracting Group of refpoints")
                    end
                else # get refpoints that were former detected by coords (and get their group)
                    RefPts_Group, RefPtsGr_center, radius_flyby = Get_Refpts_byRefPts(All_RefPts, Detected_Refs, FlownGroups, (current_pos[1], current_pos[2]))
                end
            end #else continue our flying around detected group of refpoints
        else
#            println("RefPts_Group=", RefPts_Group)
        end

        if (curr_group == 0)
            
            next_pos = current_pos[1:2] .+ direction * curr_speed  # calc next_pos as we wouldn't have refpopints
            next_pos[1] = clamp(next_pos[1], X[1], X[end])
            next_pos[2] = clamp(next_pos[2], Y[1], Y[end])
        
            if (length(RefPts_Group) > 0)  # must fly around something
                dist_to_refcenter = sqrt( (current_pos[1] - RefPtsGr_center[1])^2 + (current_pos[2] - RefPtsGr_center[2])^2 )  # found distance to ref. center
            
                if (min_refpt_dist < 0)  # set the first actual value of distance from current position to the center of refpoints
                    min_refpt_dist = dist_to_refcenter
                elseif (dist_to_refcenter < min_refpt_dist) # if distance is decreasing, then approach as close as possible
                    min_refpt_dist = dist_to_refcenter # update the shortest distance to center
                else #the distance to ref. center is increasing again
                    if (length(RefPts_Group) == 1) # fly around a single refpoint
                        if (min_refpt_dist >= min_radius_flyby) # only if it is little aside from us, not too close
                            curr_group = last.(RefPts_Group)[1] # set a new curr_group, so this group becomes now the actually flying around
                            radius_flyby = min_refpt_dist
                            println("--> Start now flying around single refpoint ", RefPts_Group[1])
                        else # otherwise if we are too close to the center of refpoints/single refpoint
                            empty!(RefPts_Group) # clear the ignored reference point
                            min_refpt_dist = -Inf
                            println("--> Ignored a single refpoint - too close!")
                        end
                    else # otherwise if flying around a group of refpoints
                        if (radius_flyby <= min_radius_flyby)
                            empty!(RefPts_Group) # clear the ignored reference point
                            min_refpt_dist = -Inf
                            println("--> Ignored a group of refpoints - too close!")                        
                        else #if (dist_to_refcenter >= radius_flyby) # if the distance from current position to ref. center exceeded the flyby radius
                            curr_group = last.(RefPts_Group)[1] # set a new curr_group, so this group becomes now the actually flying around
                            
                            radius_flyby_inc = Calc_Radius_Increment(radius_flyby, (current_pos[1], current_pos[2]), RefPtsGr_center, RefPts_Group)
                            
                            println("--> Start now flying around a group of refpoints with center in ", RefPtsGr_center)
                        end
                    end
                end

                if (curr_group > 0) #if flying around is started, find isparameters
                    min_refpt_dist = -Inf #reset min distance to the ref. center
                    Φr = 2π / steps_around_group
                    
                    ref_cen = [RefPtsGr_center[1], RefPtsGr_center[2], 0]
                    dir_xy = [direction[1], direction[2], 0]
                    curr_pos_xy = copy(current_pos)
                    curr_pos_xy[3] = 0
                    
                    cross_product = cross(dir_xy, ref_cen .- curr_pos_xy)
                    if (cross_product[3] < 0)
                        Φr = -Φr
                        println(">>> Clockwise flight")
                    else
                        println("<<< Counterclockwise flight")
                    end
                    steps_around = 0
                end
            end
            curr_direction = direction
        end
        
        if (curr_group > 0)  # if fly around is started,
            radius_flyby += radius_flyby_inc

            curr_speed = 2π*radius_flyby / steps_around_group

            dir_around = [curr_direction[1]*cos(Φr) - curr_direction[2]*sin(Φr),
                          curr_direction[1]*sin(Φr) + curr_direction[2]*cos(Φr)]
            dir_around = normalize(dir_around)
            dot_product = dot(dir_around, direction)
#            println("dot_product=", dot_product)
            
            steps_around += 1
            next_pos = current_pos[1:2] .+ dir_around[1:2] * curr_speed  # calc next_pos as we wouldn't have refpopints
            next_pos[1] = clamp(next_pos[1], X[1], X[end])
            next_pos[2] = clamp(next_pos[2], Y[1], Y[end])
            curr_direction = dir_around

            if (norm(direction) * norm(curr_direction) - dot_product < 0.005) || (steps_around > max_steps_around) # about 5 degrees for cos(x)
                push!(FlownGroups, curr_group)
                curr_group = 0
                radius_flyby_inc = 0.0
                curr_speed = speed
                empty!(RefPts_Group)
                println("Flown around in $(steps_around) steps\n")
                steps_around = 0
                continue
            end
            
        end
        
        xi = argmin(abs.(X .- next_pos[1])) # find next position of drone on the grid
        yi = argmin(abs.(Y .- next_pos[2]))
        terrain_height = Z[xi, yi] # terrain height in the next planned position
        
        if (current_pos[3] >= service_ceiling) # still flying higher than our ceiling permits (e.g. started there)
#            new_z = min(current_pos[3], terrain_height + h_over_lscape)
            if (terrain_height + h_over_lscape < service_ceiling)
                new_z = terrain_height + h_over_lscape
            else
                new_z = service_ceiling
            end
#            new_z = clamp( max(current_pos[3], terrain_height + h_over_lscape), -Inf, service_ceiling )
        else
            new_z = clamp( min(terrain_height + h_over_lscape, current_pos[3]), -Inf, service_ceiling )
        end
        
        visible_obstacles = Tuple{Float64, Float64, Float64}[]
        
        for angle in range(-fov_angle/2, fov_angle/2, length=12)
            rad = deg2rad(angle)
            
            dir = [curr_direction[1]*cos(rad) - curr_direction[2]*sin(rad),
                   curr_direction[1]*sin(rad) + curr_direction[2]*cos(rad)]
            
            for b in range(1.0, view_distance, length=2*trunc(Int64, floor(view_distance)))
                check_pos = next_pos .+ dir * b
            
                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 > new_z # obstacle is higher than our ceiling
                        push!(visible_obstacles, (X[xi_check], Y[yi_check], obstacle_height))
                    end
                end
            end
        end

        if !isempty(visible_obstacles)
#            if (current_pos[3] >= service_ceiling)
#                min_obstacle = minimum(obs[3] for obs in visible_obstacles)
#            else
                max_obstacle = maximum(obs[3] for obs in visible_obstacles)
                min_obstacle = minimum(obs[3] for obs in visible_obstacles)
            
                if (max_obstacle + h_over_lscape > service_ceiling)
#=
                    orth_dir = curr_direction
                    if rand() < 0.5
                        orth_dir = [-curr_direction[2] curr_direction[1]]
                    else
                        orth_dir = [curr_direction[2] -curr_direction[1]]
                    end
                    
                    next_pos = current_pos[1:2] .+ orth_dir * curr_speed  # move orthogonal instead of towards the curr_direction
                    next_pos[1] = clamp(next_pos[1], X[1], X[end])
                    next_pos[2] = clamp(next_pos[2], Y[1], Y[end])
=#
                    next_pos[1] = clamp(next_pos[1] + rand([-0.7, 0.7]) * curr_speed, X[1], X[end])
                    next_pos[2] = clamp(next_pos[2] + rand([-0.7, 0.7]) * curr_speed, Y[1], Y[end])
                    new_z = clamp( current_pos[3] + descend_speed, -Inf, service_ceiling )
                elseif max_obstacle + h_over_lscape > new_z
                    new_z = clamp( max_obstacle + h_over_lscape, -Inf, service_ceiling )
                end
#            end
        end
        
        current_pos = [next_pos[1], next_pos[2], new_z]
        push!(drone_path, copy(current_pos))
        push!(drone_heights, new_z)
        
        xi = argmin(abs.(X .- next_pos[1])) # find next position of drone on the grid
        yi = argmin(abs.(Y .- next_pos[2]))
        push!(gnd_height_below, Z[xi,yi])

        push!(obstacles, visible_obstacles)
    end
    
    if (length(drone_path) > MaxSteps)
        println("CRASHED - Fuel is over!!!")
    else
        xi_end = argmin(abs.(X .- Finish[1]))
        yi_end = argmin(abs.(Y .- Finish[2]))
        final_ground = Z[xi_end, yi_end]

        for _ in 1:steps_descend
            new_z = drone_path[end][3] - (drone_path[end][3] - final_ground) / steps_descend
            push!(drone_path, [Finish[1], Finish[2], new_z])
            push!(drone_heights, new_z)
            push!(gnd_height_below, final_ground)
            push!(obstacles, [])
        end
    end
    println("Время полёта дрона: $(length(drone_path)) шагов")
    println("FlownGroups: ", FlownGroups)
    println("All visible Ref. points: ", All_RefPts)
    
    return drone_path, drone_heights, gnd_height_below, obstacles, Start, Finish, All_RefPts
end

println("Start position of drone: ", Start)
println( "Terrain height at start = ", Zls[trunc(Int64, Start[1]), trunc(Int64, Start[2])] )
speed = 1
drone_path, drone_heights, gnd_height, obstacles, start, finish, refs = simulate_drone(LScape, minh_over_terrain, pract_ceiling, 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]

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=:diamond, markersize=6, color=:orange)
scatter!(plt1, [path_x[end]], [path_y[end]], marker=:xcross, markersize=6, color=:red)

#plt2 = plot(t, path_z, lw=2, c=:purple, xlabel="Шаги", ylabel="Высота (м)", title="Высоты дрона", legend=false)
plt2 = plot(path_x, path_y, path_z, lw=2, c=:purple, xlabel="X (м)", ylabel="Высота (м)", title="Путь дрона в 3D", legend=false, camera = (-15, 5))

scatter!(plt2, [path_x[1]], [path_y[1]], [path_z[1]], marker=:diamond, markersize=4, color=:orange)
scatter!(plt2, [path_x[end]], [path_y[end]], [path_z[end]], marker=:xcross, markersize=4, color=:red)

for p in 1:length(RefPts)
    scatter!(plt1, [RefPts[p][1]], [RefPts[p][2]], marker=:circle, markersize=3, color=:yellow)
    scatter!(plt2, [RefPts[p][1]], [RefPts[p][2]], [RefPts[p][3]], marker=:circle, markersize=3, color=:yellow)
end
#=
for (name, rgb_tuple) in Colors.color_names
    println("$name: $rgb_tuple")
end
=#
my_color = colorant"goldenrod1"

refs_coords = first.(refs)
for p in 1:length(refs_coords)
    scatter!(plt1, [refs_coords[p][1]],[refs_coords[p][2]], marker=:circle, markersize=5, color=my_color)
    scatter!(plt2, [refs_coords[p][1]], [refs_coords[p][2]], [Zls[refs_coords[p][1], refs_coords[p][2]]], marker=:circle, markersize=5, color=my_color)
end

t = 1:length(drone_path)# - steps_descend

ceiling = [pract_ceiling for p in t]
plt3 = plot(t, ceiling, lw=2, c=:DeepSkyBlue, legend=true, label="потолок", xticks=1:1:length(path_z), xformatter=(_...) -> "")
plt3 = plot!(t, path_z, lw=2, c=:purple, xlabel="Шаги, 1 шаг", ylabel="Высота (м)", title="Высота дрона и рельефа", legend=true, label="дрон")
plt3 = plot!(t, gnd_height, lw=2, c=:grey, legend=true, label="рельеф", yticks= trunc(Int64, floor(maxdepth)):2:max(pract_ceiling, trunc(Int64, ceil(maxheight))))

l = @layout([a b; c])
plot(plt1, plt2, plt3, layout=l, size=(1200, 1200))

#display(plt3)
#plot(plt3, plt4, layout=(1, 1), size=(1000, 1000))

Start position of drone: [9.0, 104.0, 17.93575600664868]
Terrain height at start = 13.459731537671125
--> Start now flying around single refpoint ((44, 84), 1)
<<< Counterclockwise flight
Landscape hedge blocks a refpt[44,84]

Flown around in 24 steps

Landscape hedge blocks a refpt[44,84]

--> Start now flying around single refpoint ((56, 72), 5)
<<< Counterclockwise flight
Landscape hedge blocks a refpt[44,84]

Landscape hedge blocks a refpt[44,84]

Landscape hedge blocks a refpt[44,84]

Landscape hedge blocks a refpt[44,84]

Landscape hedge blocks a refpt[44,84]

Landscape hedge blocks a refpt[44,84]

Flown around in 24 steps

--> Ignored a single refpoint - too close!
Landscape hedge blocks a refpt[44,84]

Landscape hedge blocks a refpt[44,84]

--> Start now flying around single refpoint ((51, 73), 4)
<<< Counterclockwise flight
Landscape hedge blocks a refpt[44,84]

Landscape hedge blocks a refpt[44,84]

Landscape hedge blocks a refpt[44,84]

Landscape hedge blocks a refpt[44,84]

Landscape hedge blocks a refpt[44,84]

Landscape hedge blocks a refpt[44,84]

Landscape hedge blocks a refpt[44,84]

Landscape hedge blocks a refpt[44,84]

Flown around in 24 steps

Landscape hedge blocks a refpt[44,84]

Landscape hedge blocks a refpt[44,84]

Landscape hedge blocks a refpt[44,84]

--> Start now flying around single refpoint ((41, 71), 3)
>>> Clockwise flight
Landscape hedge blocks a refpt[44,84]

Flown around in 24 steps

Landscape hedge blocks a refpt[44,84]

Landscape hedge blocks a refpt[44,84]

Landscape hedge blocks a refpt[44,84]

--> Start now flying around single refpoint ((52, 59), 6)
>>> Clockwise flight
Flown around in 24 steps

--> Start now flying around single refpoint ((49, 55), 7)
>>> Clockwise flight
Landscape hedge blocks a refpt[53,40]

Landscape hedge blocks a refpt[53,40]

Flown around in 24 steps

Landscape hedge blocks a refpt[52,59]

Landscape hedge blocks a refpt[70,49]

--> Start now flying around single refpoint ((70, 49), 10)
>>> Clockwise flight
Flown around in 24 steps

Время полёта дрона: 344 шагов
FlownGroups: [1, 5, 4, 3, 6, 7, 10]
All visible Ref. points: [((44, 84), 1), ((37, 91), 2), ((41, 71), 3), ((51, 73), 4), ((56, 72), 5), ((52, 59), 6), ((49, 55), 7), ((37, 49), 8), ((34, 66), 9), ((70, 49), 10)]

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)
    for p in 1:length(RefPts)
        scatter!(base_surface, [RefPts[p][1]], [RefPts[p][2]], marker=:circle, markersize=3, color=:yellow)    
    end
    
    scatter!(base_2d, [start[1]], [start[2]], marker=:circle, markercolor=:green)
    scatter!(base_2d, [finish[1]], [finish[2]], marker=:circle, markercolor=:red)
    for p in 1:length(RefPts)
        scatter!(base_2d, [RefPts[p][1]], [RefPts[p][2]], marker=:circle, markersize=3, color=:yellow)    
    end

    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(LScape, drone_path, drone_heights, obstacles, speed, Start, Finish; fps=5)

[ Info: Saved animation to /user/tmp.gif
No description has been provided for this image
Группа видимости
ПубличноПросматривать контент могут все пользователи сообщества

Тип

Проекты

Краткое описание

Моделируется облет дроном ориентиров, обнаруживаемых при помощи бортовой камеры в процессе полета

Связанные материалы

Пусто

Теги

Категории

Моделирование в Engee
МероприятияЛетняя школа Julia 2025

Языки

    Julia

Форматы

    ngscript

Уровень

    Базовый

Статус

Опубликовано

Обновлено

Источник

Сообщество