Применение триангуляции к модели движения ТС по маршруту
Применение концепции RANSAC к задаче триангуляции при квази-дальномерном подходе.
Триангуляционные модели имеют несовершенство (resudial_map,ngscript). Функция невзяок в случае зашумленных данных становится неунимодальной, и метод оптимизации "наискорейшего спуска" может выдать неверное значение о точке минимума.
В прошлом исследовании мы получили, что ошибка квази-дальномерного подхода составляет порядка 12 сантиметров, но не более метра.
Для устранения данного несовершенства воспользуемся концепцией RANSAC, примененняя ее к задаче триангуляции. для этого расположим 10 вышек на полигоне. таким образом задачу триангуляции по 4 вышкам можно решить C_10^4 способов = 210 способов.
RANSAC (аббр. RANdom SAmple Consensus) — стабильный метод оценки параметров модели на основе случайных выборок. Схема RANSAC устойчива к зашумлённости исходных данных. Метод был предложен в 1981 году Фишлером и Боллесом.
Решив задачу триангуляции каждым из способов мы получим 210 точек (рассчетные координат ЦМ в пространстве). эти данные можно статистически проанализировать, и выбрать наиболее правдоподобные значения положения объекта. Алгоритмы сошедшиеся в точках локального минимума, не являющегося глобальным выдадут координаты, значения которых будут выбиваться из остальных данных - являются выбросами на графиках. таким образом, взяв медиану по X по Y и по Z мы можем получить достаточно точное положение объекта на карте, не опасаясь сторонних решений, вызванных неунимодельностью функции невязок.
Рассмотрим модель ransac_block.engee.
В этой модели реализован алгоритм рассчета положения на основе показаний с 10 вышек с использованием квази-дальномерного алгоритма.
данный алгоритм берет медианные значения положений, рассчитанных по измерениям с 10 вышек.
для примера, мощность шума (10м)^2 для каждой вышки.
proj_root_folder = "$(@__DIR__)/veh_pos_test_ransac"
include("$proj_root_folder/map_params.jl")
include("$proj_root_folder/car_params.jl")
include("$proj_root_folder/sensors_params.jl");
sim_model = engee.load("$(proj_root_folder)/ransac_block.engee", force=true)
res = engee.run(sim_model)
engee.close(sim_model, force=true);
true_x = collect(res.dict["true_x"]).value
true_y = collect(res.dict["true_y"]).value
true_z = collect(res.dict["true_z"]).value
mes_x = collect(res.dict["x"]).value
mes_y = collect(res.dict["y"]).value
mes_z = collect(res.dict["z"]).value;
Ошибка измеренного положения от истинных данных получаются следующей:
errors = sqrt.((true_x .- mes_x).^2 .+ (true_y .- mes_y).^2 .+ (true_z .- mes_z).^2)
plot(errors, title="Максимальная ошибка: $(round(max(errors...), digits = 4))", )
plot!(sort(errors), legend = :none)
plot(mes_x, mes_y, mes_z, label = "measured", lw = 2)
plot!(true_x, true_y, true_z, label = "true")
Таким образом получили, что данный подход позволяет получать результаты высокой точности даже в условиях очень большого шума
proj_root_folder = "$(@__DIR__)/veh_pos_test_ransac"
include("$(proj_root_folder)/map_params.jl")
include("$(proj_root_folder)/car_params.jl");
include("$(proj_root_folder)/sensors_params.jl");
sim_model = engee.load("$(proj_root_folder)/simple_veh_model_with_positioning_quazi_ransac.engee", force=true)
engee.run(sim_model)
engee.close(sim_model, force=true);
pos_vect_7 = collect(pos_by_ransac).value
pos_vect_7_calc = collect(pos_calc_by_ransac).value
plot(
map(x->x[1], pos_vect_7),
map(x->x[2], pos_vect_7),
map(x->x[3], pos_vect_7),
lw=2, color=:red, label="real_traj"
)
plot!(
map(x->x[1], pos_vect_7_calc),
map(x->x[2], pos_vect_7_calc),
map(x->x[3], pos_vect_7_calc),
color=:blue, label="calc_traj",
legend=:bottomright,
legendfont = font("Times New Roman", 12),
titlefont = font("Times New Roman", 14),
title = "Траектории ЦМ при движении по RANSAC"
)
pos_vect_7 = collect(pos_by_ransac).value
pos_vect_7_calc = collect(pos_calc_by_ransac).value
plot(
map(x->x[1], pos_vect_7),
map(x->x[2], pos_vect_7),
lw=2, color=:red, label="real_traj"
)
plot!(
map(x->x[1], pos_vect_7_calc),
map(x->x[2], pos_vect_7_calc),
color=:blue, label="calc_traj",
legend=:bottomright,
legendfont = font("Times New Roman", 12),
titlefont = font("Times New Roman", 14),
title = "Траектории ЦМ при движении по RANSAC, 2D"
)
data = map(x-> sqrt(sum(x.^2)), (pos_vect_7[1:100:end] .- pos_vect_7_calc))
plot(data, title="Отклонение измерений от реальных данных по RANSAC, суммарно: $(10*sum(data))", legend = false,
titlefont = font("Times New Roman", 14)
)
plot!(sort(data))
Рассчитать значение шума координат на основании шума датчиков является достаточно проблематичной задачей, а между тем эти данные нужны будут для рассчета фильтра фалмана в следующем пункте, так что рассчитаем среднеквадратичное отклонение измерений от истинных данных для полученных в ходе данной симуляции 100 точек.
sygma_xyz_triangulation = sqrt(sum(map(x-> sum(x.^2), (pos_vect_7[1:100:end] .- pos_vect_7_calc)))/(3 * length(pos_vect_7_calc)))
println("Дисперсия триангуляции: $(sygma_xyz_triangulation)")
Также заметим, что несмотря на позиционирование гораздо более высокой точности продвижение данной модели по диагональному маршруту происходит гораздо медленнее чем при движении по данным БИНС. Это происходит потому что сигнал частотой 1 Гц не позволяет эффективно управлять транспортным средством, что можно увидеть сравнив траектории движения по БИНС и по триангуляции
distance = round(sqrt(sum((pos_vect_5[end].-pos_vect_7[end]).^2)), digits=2)
plot(
map(x->x[1], pos_vect_7),
map(x->x[2], pos_vect_7),
lw=2, color=:red, label="ransac", title="За время симуляции ТС по бинс проезжает на $(distance)м дальше",
)
plot!(
map(x->x[1], pos_vect_5),
map(x->x[2], pos_vect_5),
lw=1, color=:blue, label="bins", legend =:bottomleft
)
Сделать поступление данного сигнала более частым - слишком сильно повысит количество необходимых расчетов в единицу времени, и слишком сильно нагрузит систему. Совместим оба этих метода - RANSAC и БИНС. рассмотрим модель KALMAN