Engee 文档
Notebook

使用 GNN 跟踪物体

在当今世界，物体跟踪（追踪）任务有着广泛的应用，从自动驾驶汽车和视频监控系统到雷达和传感器监测，不一而足。 这一领域的主要挑战之一是将测量结果与轨迹联系起来，尤其是在不确定、噪声和可能的轨迹交叉等条件下。

我们考虑采用经典的 GNN 跟踪算法。

所介绍的代码包括以下步骤。 1.建立物体沿双曲线轨迹运动的模型。 2.执行卡尔曼滤波器以预测和更新轨迹状态。 3.以动画图表的形式将结果可视化。

主要目的是展示该算法在处理噪声测量和构建运动物体轨迹（本例中为沿双曲线轨迹运动的物体）方面的有效性。

接下来，让我们进入算法，首先声明数据结构和物体跟踪系统的常数。

1.Track - 一个变量结构，用于存储物体状态（坐标、速度）、协方差矩阵（不确定性）、轨迹年龄、错过的测量次数以及可视化状态历史。 2.2. Detection - 用于存储测量数据（坐标和时间）的不可变结构。 H 大小为 2×4 的观测矩阵，用于从状态向量中提取位置分量（x,y）。

这些结构构成了算法实现的基础。

In [ ]: 
using LinearAlgebra

# Изменяемая структура трека
mutable struct Track
    state::Vector{Float64}     # Вектор состояния
    covariance::Matrix{Float64} # Матрица ковариации
    age::Int                   # Возраст трека (число обновлений)
    missed::Int                # Число пропущенных измерений
    history::Vector{Vector{Float64}} # История состояний для отрисовки
end

# Добавляем history в конструктор Track
Track(state, covariance, age, missed) = Track(state, covariance, age, missed, [copy(state)])

# Измерение: [x, y, timestamp]
struct Detection
    position::Vector{Float64}
    time::Float64
end

const H = [1 0 0 0; 0 1 0 0];  # Матрица наблюдений

接下来，让我们考虑如何实现卡尔曼滤波器的跟踪功能。

1.预测！根据恒定速度（CV）模型更新轨迹状态，预测新位置并增加不确定性（协方差）。 2.update! 使用卡尔曼增益矩阵 (K) 通过测量修正轨迹状态，减少不确定性并保留历史变化。

In [ ]: 
# Функция предсказания состояния трека
function predict!(track::Track, dt::Float64)
    F = [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]  # Матрица перехода (CV)
    track.state = F * track.state
    track.covariance = F * track.covariance * F' + diagm([0.1, 0.1, 0.01, 0.01])
    return track
end

# Функция обновления трека на основе измерения
function update!(track::Track, detection::Detection)
    y = detection.position - H * track.state
    S = H * track.covariance * H' + diagm([0.5, 0.5])
    K = track.covariance * H' / S
    track.state += K * y
    track.covariance = (I - K * H) * track.covariance
    track.age += 1
    track.missed = 0
    push!(track.history, copy(track.state)) # Сохраняем историю
    return track
end

[ Info: Starting sender/receiver loops
Out[0]:
update! (generic function with 1 method)

下面我们来看看跟踪算法的功能。全局最近邻（GNN）是一种基于贪婪地将测量结果与最近轨迹相匹配的简单方法。在我们的代码中，trackerGNN 函数实现了匹配轨迹和测量值的 GNN 算法。算法本身由以下部分组成。 1.预测：复制并预测所有轨迹的新状态（predict!） 2.距离计算：根据 Mahalanobis 统计距离建立轨迹和测量值之间的距离矩阵。 3. 贪婪匹配：通过检查阈值（门）找到距离最小的轨迹-测量值对，并排除已匹配的轨迹/测量值。 4. 更新和过滤：更新匹配测量值上的轨迹（更新！），并删除缺少 >3 个测量值的轨迹。

In [ ]: 
function trackerGNN(detections::Vector{Detection}, existing_tracks::Vector{Track}, gate::Float64)
    predicted = [predict!(deepcopy(t), 1.0) for t in existing_tracks] # Предсказание состояний

    # Матрица расстояний
    dist = zeros(length(predicted), length(detections))
    for (i,t) in enumerate(predicted), (j,d) in enumerate(detections)
        y = d.position - H*t.state
        dist[i,j] = y' / (H*t.covariance*H') * y
    end

    dets_assigned = tracks_assigned = Int[] # Cопоставление
    for _ in 1:min(length(predicted), length(detections))
        i,j = argmin(dist).I
        dist[i,j] > gate && break
        push!(tracks_assigned, i)
        push!(dets_assigned, j)
        dist[i,:] .= dist[:,j] .= Inf
    end
        
    updated = deepcopy(predicted) # Обновление и фильтрация
    foreach(((i,j),) -> update!(updated[i], detections[j]), zip(tracks_assigned, dets_assigned))
    filter!(t -> t.missed < 3, updated)
end
Out[0]:
trackerGNN (generic function with 1 method)

plot_tracks 函数将跟踪过程可视化。该函数创建了一个可视化界面，可将算法性能与动态中的实际值进行比较，其中包括以下逻辑。 1.蓝点（散点图）--当前测量值（检测值）。 2.蓝色虚线--物体（obj1、obj2）的真实轨迹。 3.红色方框--GNN 历史轨迹（实线）。

In [ ]: 
function plot_tracks(tracks::Vector{Track}, detections::Vector{Detection}, obj1, obj2, step)
    p = scatter(getindex.(d.position[1] for d in detections), getindex.(d.position[2] for d in detections), label="Detections", color=:blue, markersize=6)
    # Реальные траектории
    plot!(getindex.(p[1] for p in obj1[1:step]), getindex.(p[2] for p in obj1[1:step]), label="", color=:blue, linestyle=:dash)
    plot!(getindex.(p[1] for p in obj2[1:step]), getindex.(p[2] for p in obj2[1:step]), label="", color=:blue, linestyle=:dash)

    # Вспомогательная функция для отрисовки треков
    function plot_tracks!(tracks, label, color, marker)
        for (i, t) in enumerate(tracks)
            scatter!([t.state[1]], [t.state[2]], label=i==1 ? label : "", color=color, markersize=8, marker=marker)
            # Отрисовка истории трека
            if length(t.history) > 1
                plot!(getindex.(h[1] for h in t.history), getindex.(h[2] for h in t.history), 
                      label="", color=color, linestyle=:solid, linewidth=2)
            end
        end
    end
    
    # Треки разных типов с историей
    plot_tracks!(tracks, "GNN Track", :red, :square)
    plot!(legend=:topleft, title="Шаг: $step", xlims=(-5, 5), ylims=(-5, 5))
end
Out[0]:
plot_tracks (generic function with 1 method)

generate_hyperbola_trajectories 函数生成两个对称双曲线轨迹，即创建一个均匀分布的-3 至 3 的参数 t 范围，然后计算两个双曲线的点坐标。

  • 第一个：acosh(t), y = bsinh(t)
  • 第二个：-acosh(t), y = bsinh(t)

这个函数非常适合在相交轨迹上测试跟踪算法。它可以创建逼真的测试轨迹，模拟物体沿双曲路径运动。

In [ ]: 
# Функция для генерации траекторий гипербол
function generate_hyperbola_trajectories(steps, a=1.0, b=1.0)
    t = range(-3, 3, length=steps)
    [(a*cosh(ti), b*sinh(ti)) for ti in t], [(-a*cosh(ti), b*sinh(ti)) for ti in t]
end
Out[0]:
generate_hyperbola_trajectories (generic function with 3 methods)

现在，让我们对双曲线轨迹跟踪进行模拟和可视化。

模拟周期包括以下步骤 1 算法每一步都会在实际位置上添加高斯噪声。 2 更新 GNN 跟踪器，并通过 plot_tracks 可视化当前状态。 3.3. 将最后一帧保存为 PNG 图像，并创建整个跟踪过程的 GIF 动画（5fps）。

In [ ]: 
steps = 50
obj1, obj2 = generate_hyperbola_trajectories(steps)

# Инициализация треков с историей
initial_state1 = [obj1[1][1], obj1[1][2], 0.5, 0.5]
initial_state2 = [obj2[1][1], obj2[1][2], -0.5, 0.5]
tracks_gnn = [Track(initial_state1, Matrix(1.0I, 4, 4), 1, 0),
              Track(initial_state2, Matrix(1.0I, 4, 4), 2, 0)]

# Создаем GIF
anim = @animate for step in 1:steps
    # Генерируем измерения для текущего шага
    noise = 0.3(randn(4))
    detections = [Detection([obj1[step][1] + noise[1], obj1[step][2] + noise[2]], step), 
                  Detection([obj2[step][1] + noise[3], obj2[step][2] + noise[4]], step)]
    # Обновляем трек
    updated_tracks_gnn = trackerGNN(detections, deepcopy(tracks_gnn), 50.0)
    # Обновляем исходные треки для следующего шага
    global tracks_gnn = updated_tracks_gnn
    # Визуализация
    plot_tracks(tracks_gnn, detections, obj1, obj2, step)
    if step == steps
        savefig("last_frame.png")
    end
end 
# Сохраняем GIF
gif(anim, "tracking_simulation.gif", fps=5)
Out[0]:
No description has been provided for this image

输出

结果：
1.最后一个模拟步骤显示，尽管在测量中增加了噪音，算法仍成功地跟踪了两个物体。 2. 跟踪轨迹（红线）接近于物体的真实轨迹（蓝色虚线），这证明了算法的准确性。同时，我们可以看到最大的误差恰恰出现在轨迹接触的时刻。 3.LIIar: 50 - Mahalonobis 距离的临界值表示算法对误报的稳定性。

In [ ]: 
using Images
println("Последний шаг моделирования:")
img = load("last_frame.png")

Последний шаг моделирования:
Out[0]:
No description has been provided for this image

所提出的算法对于非线性轨迹的目标跟踪问题具有很高的效率。其主要优点如下

  • 由于采用了卡尔曼滤波器，因此对噪声具有鲁棒性；
  • 易于使用 GNN 方法对测量结果进行比较；
  • 清晰的可视化便于解释结果。

该算法成功地解决了跟踪问题，可应用于自主系统、视频监控和机器人等领域。为了进一步改进该算法，可以考虑使用更复杂的运动模型（如恒定加速度模型）或 JPDA 或 MHT 等跟踪方法来处理高物体密度问题。