A*搜索算法 双向A*搜索算法 Julia实现

算法概述

抽象的在非负有向边权图 $G=(V,E,W),W:E\to \mathbb{R}+$ 上的 BFS 过程可以看作这样：
(1) 设 $C$ 点集表示已遍历的点，$\forall n\in C,d(n)$ 表示源点 $s$ 到点 $n$ 的最短距离，$O$ 点集表示可遍历的点，$\forall n\in O,g(n)$ 表示源点 $s$ 到点 $n$ 的当前最短距离，初始状态 C = ∅ , O = { s } , g ( s ) = 0 C = \emptyset, O = \{ s \}, g(s) = 0 C=∅,O={s},g(s)=0；
(2) 从 $O$ 中取本轮遍历点 $n_k$ 满足 $n_k={\mathrm{argmin}}_{n\in O}g(n)$，更新已遍历点集 C = C + { n k } , d ( n k ) = g ( n k ) C = C + \{ n_k \}, d(n_k) = g(n_k) C=C+{nk​},d(nk​)=g(nk​)，并更新可遍历点集 O = O − { n k } + { n j ∣ n j n k ∈ E   a n d   n j ∉ C } , ∀ n j ∈ O , g ( n j ) = min ⁡ ∀ n i ∈ C d ( n i ) + W ( n i n j ) O = O - \{ n_k \} + \{ n_j | n_j n_k \in E \ and \ n_j \notin C \}, \forall n_j \in O, g(n_j) = \min_{\forall n_i \in C} d(n_i) + W(n_i n_j) O=O−{nk​}+{nj​∣nj​nk​∈E and nj​∈/C},∀nj​∈O,g(nj​)=min∀ni​∈C​d(ni​)+W(ni​nj​)；
(3) 迭代 (2) 直到目标点 $t\in C$，此时 $d(t)$ 必然为最短路径。

启发式搜索策略会设置一个启发函数 $h(n)$，用来评估点 $n$ 到目标点的距离，每轮选择本轮遍历点 $n_k$ 时，条件改为 $n_k={\mathrm{argmin}}_{n\in O}g(n)+h(n)$，如此一来 BFS 可看作 $h(n)\equiv 0$ 的特殊情形。

设最优启发函数 $h^{\ast }(n)=d(t)-d(n)$，则可证明 $h(n)$ 策略有以下两种情形：
(1) $\forall n,h(n)\le h^{\ast }(n)$，此时得到的结果必然为最短路径，这样的搜索算法称为 A* 搜索；
(2) $\exists n_k,h(n_k)>h^{\ast }(n_k)$，此时得到的结果不一定为最短路径。

问题描述和结果

问题描述：在网格上从源点移动至目标点，有些网格为障碍不可通行，可通行的网格分 0, 2, 4 三种地形代价，移动分为移动到边相邻的网格和对角相邻的网格，移动代价分为 1 和 $\sqrt{2}$，总代价为源点到目标点经过网格的网格代价（不含源点）和移动代价之和。

代码：

using Plots, Printf
import Base: +, -, sign
struct Sqr2
    x::Int32; y::Int32
end
+(l::Sqr2, r::Sqr2) = Sqr2(l.x + r.x, l.y + r.y)
-(l::Sqr2, r::Sqr2) = Sqr2(l.x - r.x, l.y - r.y)
function sign(z::Sqr2)
    if z.x == 0
        return sign(z.y)
    elseif z.x > 0 && z.y >= 0 || z.x < 0 && z.y <= 0
        return sign(z.x)
    else
        return sign(z.x^2 - 2 * z.y^2) * sign(z.x)
    end
end
value(z::Sqr2) = z.x + z.y * sqrt(2)

function Heap(c)
    dat, rev, tim, T = [], Dict(), Dict(), 0
    cmp(x, y) = c(x, y) == 0 ? tim[x] < tim[y] : c(x, y) < 0
    function dec(k)
        i = rev[k]
        while i > 1
            j = i ÷ 2
            if cmp(dat[j], k)
                break
            end
            dat[i], rev[dat[j]] = dat[j], i
            i = j
        end
        dat[i], rev[k] = k, i
    end
    function ins(k)
        tim[k], T = T, T + 1
        if !haskey(rev, k)
            push!(dat, k); rev[k] = length(dat)
        end
        dec(k)
    end
    function ext()
        i, k, m = 1, dat[end], dat[1]
        pop!(dat); delete!(rev, m); delete!(tim, m)
        n = length(dat)
        while i * 2 <= n
            j = i * 2
            if j + 1 <= n && cmp(dat[j + 1], dat[j])
                j += 1
            end
            if cmp(k, dat[j])
                break
            end
            dat[i], rev[dat[j]] = dat[j], i
            i = j
        end
        if n > 0
            dat[i], rev[k] = k, i
        end
        return m
    end
    return (ins = ins, ext = ext, emp = () -> (length(dat) == 0))
end

struct Node
    l::Tuple{UInt32, UInt32}; r::UInt8
end

function visualize(L, S, T, M)
    ind = 0
    function visbg()
        K = ['#', ' ', '.', ':']
        C = [:grey, :white, "#00B0F0", "#FFC000"]
        C = [c for (k, c) in zip(K, C) if k in M]
        F = Dict('#' => -1, ' ' => 0, '.' => 1, ':' => 2)
        z = [F[i] for i in M]
        heatmap(1 : L[2], 1 : L[1], z, aspect_ratio = 1;
            size = (L[2] * 40, L[1] * 40 + 100), framestyle = :none,
            xlims = (.5, L[2] + .5), ylims = (.5, L[1] + .5), yflip = true,
            colorbar = false, color = C)
        hline!(.5 : (L[1] + .5), color = :black, legend = false)
        vline!(.5 : (L[2] + .5), color = :black, legend = false)
        annotate!([(0, i, text(string(i), 12)) for i = 0 : 5 : L[1]])
        annotate!([(i, 0, text(string(i), 12)) for i = 5 : 5 : L[2]])
        annotate!(S[2], S[1], text("S", 12))
        annotate!(T[2], T[1], text("T", 12))
    end
    function viscene(B, P, f, save::Bool = true)
        visbg()
        C = (:green, :blue, :lightgreen, :lightblue)
        for (n::Node, p::Node) in P
            if B[n.l...] != n.r
                c, l, q = [C[n.r], C[n.r + 2], :pink], (:top, :bottom)[n.r], Node(p.l, 3 - p.r)
                plot!([n.l[2], p.l[2]], [n.l[1], p.l[1]]; color = haskey(P, q) && P[q] == Node(n.l, q.r) ? c[end] : c[2], w = 5)
                annotate!(n.l[2], n.l[1], text((@sprintf "%.2f" value(f(n))), 8, l, c[1]))
            end
        end
        for (n::Node, p::Node) in P
            if B[n.l...] == n.r
                c = C[n.r]
                plot!([n.l[2], p.l[2]], [n.l[1], p.l[1]]; color = c, w = 5)
            end
        end
        if save
            savefig(@sprintf "output\\%03d.png" ind)
            ind += 1
        end
    end
end

function AStar(L, S, T, M, bidirect::Bool = false)
    B::Matrix{UInt8}, D::Dict{Node, Sqr2}, P = fill(0, L), Dict(), Dict()
    function h(n::Node)
        d = abs.([n.l...] - [(T, S)[n.r]...])
        return Sqr2(abs(d[1] - d[2]), minimum(d))
    end
    f(n::Node) = D[n] + h(n)
    H = Heap((x::Node, y::Node) -> sign(f(x) - f(y)) != 0 ? sign(f(x) - f(y)) : sign(D[P[y]] - D[P[x]]))
    s::Node, t::Node = Node(S, 1), Node(T, 2)
    D[s], P[s] = Sqr2(0, 0), s; H.ins(s)
    if bidirect
        D[t], P[t] = Sqr2(0, 0), t; H.ins(t)
    end
    C, V = Dict(' ' => Sqr2(0, 0), '.' => Sqr2(2, 0), ':' => Sqr2(4, 0)), visualize(L, S, T, M)
    path(n::Node) = n.l == P[n].l ? [n.l] : n.r == 1 ? vcat(path(P[n]), n.l) : vcat(n.l, path(P[n]))
    while !H.emp()
        #V(B, P, f)
        i::Node = H.ext()
        if i == Node(T, 1) || B[i.l...] != 0
            s, t = Node(i.l, 1), Node(i.l, 2)
            println(value(D[s] + (bidirect ? D[t] : Sqr2(0, 0))))
            p = bidirect ? vcat(path(s), path(t)) : path(s)
            V(B, P, f, false)
            plot!([y for (x, y) in p], [x for (x, y) in p]; color = :red, w = 5)
            savefig("output.png")
            return true
        end
        B[i.l...] = i.r
        for (dx, dy) in ((0, -1), (1, 0), (0, 1), (-1, 0), (1, -1), (1, 1), (-1, 1), (-1, -1))
            j::Node = Node((i.l[1] + dx, i.l[2] + dy), i.r)
            if j.l[1] in 1 : L[1] && j.l[2] in 1 : L[2] && M[j.l...] != '#'
                d = D[i] + (dx * dy == 0 ? Sqr2(1, 0) : Sqr2(0, 1)) + C[M[(j.l, i.l)[i.r]...]]
                if !haskey(D, j) || sign(D[j] - d) > 0
                    D[j], P[j] = d, i; H.ins(j)
                end
            end
        end
    end
    return false
end

function input(filename)
    open(filename) do file
        n = map(s -> parse(Int, s), split(readline(file)))
        lines = readlines(file)
        L, S, T = (n[1], n[2]), (n[3], n[4]), (n[5], n[6])
        M = fill(' ', L)
        for i = 1 : L[1]
            M[i, :] = collect(lines[i])
        end
        return L, S, T, M
    end
end

@time f = AStar(input("sample1.txt")...)
@time f = AStar(input("sample1.txt")..., true)
@time f = AStar(input("sample2.txt")...)
@time f = AStar(input("sample2.txt")..., true)

地图以文件形式给出。

Sample 1

sample1.txt

14 17 9 4 10 15
                 
                 
                 
                 
                 
      #          
      #          
       #         
       #         
       ##        
        #        
        #        
                 
                 

Sample 2

sample2.txt

20 40 11 5 1 36
   #   #    #           ::::::::::::::::
       #    #            :::::::::.:::::
###### #### #             :::::::.::::::
        #   #             ::::::.::::   
            #             :::::::.::    
       ##   #              ::::::..     
  ######    #              ::::::..     
  #  # #    #                ::::...#   
     #                          ....    
     # #                        ... #   
  #  # ##          ###      #   .. ..   
  ####  #          ###         #. ..    
   #    #   #      ###           ..     
   #    ## ##                  #...     
   #    #   #                   ...     
   ######   #           ##     ...      
   #        #           ##     ...      
       #    #                 ...       
   #   #    #                ...        
   #   #    #               ...