Применение триангуляции к модели движения ТС по маршруту
Доработка динамической модели для расчета линейных ускорений и реализация моделибесплатформенной инерционной навигационной системы (БИНС).
В текущей реализации ошибка позиционирования при квази-дальномерном подходе может достигать нескольких метров при незначительных шумах. на данном же этапе моделирования мы будем учитывать куда большую амплитуду шумов, что позволит получать больше точные данные в условиях существующих ограничений - рельеф местности, радио-помехи и т.п. поэтому необходим второй способ позиционирования, который позволит двигаться по карьеру более точно. Реализуем БИНС.
Усовершенствуем динамическую модель, для получения ускорений, исходя из следующих геометрических соображений
Тогда
тогда подсистема vehicle_dynamics/Kinematics должна работать так:
Создадим простейшую модель с БИНС.
Для моделирования сигналов с датчиков добавим шум к истинным значениям параметров динамической модели.
параметры шума - файл noises.jl:
accelarations_noise_power = (10*10^-5)^2 # 1 мм /c^2
abs_speed_noise_power = 0.05^2 # σ = 5 см /c
euler_speed_noise_power = (2pi/360/60 * 6) ^ 2 # 6 с^-2
axis_angles_noise = (2pi/360/60 * 20) ^ 2 # 27'
slip_angles_noise = (2pi/360/60 * 20) ^ 2 # 27'
dkt_noise_power = (2pi/360)^2; # 1 градус
magnetometer_noise_power = (2pi/360)^2; # 1 градус
proj_root_folder = "$(@__DIR__)/bins_positioning_test"
include("$(proj_root_folder)/map_params.jl")
include("$(proj_root_folder)/car_params.jl");
include("$(proj_root_folder)/noises.jl");
sim_model = engee.load("$(proj_root_folder)/simple_veh_model_with_positioning_bins.engee", force=true)
engee.run(sim_model)
engee.close(sim_model, force = true);
pos_vect_5 = collect(pos_by_bins_res).value
pos_vect_5_calc = collect(pos_calc_by_bins_res).value
plot(
map(x->x[1], pos_vect_5),
map(x->x[2], pos_vect_5),
map(x->x[3], pos_vect_5),
lw=2, color=:red, label="real_traj"
)
plot!(
map(x->x[1], pos_vect_5_calc),
map(x->x[2], pos_vect_5_calc),
map(x->x[3], pos_vect_5_calc),
color=:blue, label="calc_traj",
legend=:bottomright,
legendfont = font("Times New Roman", 12),
titlefont = font("Times New Roman", 14),
title = "Траектории ЦМ при движении по БИНС"
)
pos_vect_5 = collect(pos_by_bins_res).value
pos_vect_5_calc = collect(pos_calc_by_bins_res).value
plot(
map(x->x[1], pos_vect_5),
map(x->x[2], pos_vect_5),
lw=2, color=:red, label="real_traj"
)
plot!(
map(x->x[1], pos_vect_5_calc),
map(x->x[2], pos_vect_5_calc),
color=:blue, label="calc_traj",
legend=:bottomright,
legendfont = font("Times New Roman", 12),
titlefont = font("Times New Roman", 14),
title = "Траектории ЦМ при движении по БИНС, 2D"
)
plot(collect(pos_by_bins_res).time, map(x-> sqrt(sum(x.^2)), (pos_vect_5 .- pos_vect_5_calc)), title="Отклонение от истинного положения при движении по БИНС e(t) по X, Y, Z", legend = false,
titlefont = font("Times New Roman", 14))
plot(collect(pos_by_bins_res).time, map(x-> sqrt(x[1]^2 + x[2]^2), (pos_vect_5 .- pos_vect_5_calc)), title="Отклонение от истинного положения при движении по БИНС e(t), по X, Y", legend = false,
titlefont = font("Times New Roman", 14))
На данном простом примере маршрута мы видим, что данная система позиционирования достигает ошибки в 1 метр только за 96 секунд. За 30 секунд ошибка достигнет 10 см, что куда точнее текущей реализации квази-дальномерного подхода, который может дать ошибку до 15 сантиметров в текущих условиях в любой момент времени
Однако, как видно из первого графика, БИНС накапливает ошибку позиционирования, а значит, со временем значительно отклоняется от реальных данных. значит движение по маршруту только лишь по бинс не будет успешным.
Учитывая данный недостаток БИНС попробуем совместить использование двух подходов позиционирования - триангуляцию и БИНС.