Реализация алгоритмов треангуляционного позиционирования объекта

Введение

В рамках данного раздела реализовано три вида алгоритмов треангуляции - три подхода к решению задачи треангуляции.

Суммарно-дальномерный подход
- Реализован в модели test_classic.engee
Разностно-дальномерный подход
- Реализован в модели test_diff.engee
Квази-дальномерный подход
- Реализован в модели test_classic_time_corr.engee

Все блоки, реализованные для данных моделей добавлены в nglib - пользовательскую библиотеку, так что при желании их можно удобно использовать в других моделях.

Реализованы блоки:

Для каждого из трех подходов расчет положения объекта на основании входных данных - расстояния до вышек в метрах (при желании легко использовать для расстояний в секундах на C - скорость света)
Псевдо-датчик расстояния от объекта до вышки
Блок оценки ошибки

Рассмотрим эти модели.

test_classic.engee

test_diff.engee

test_classic_time_corr.engee

Блоки выполнены при помощи маскированных блоков engee а также engee function, внутри которых реализован алгоритм на языке julia

Построим графики моделирования

Суммарно-Дальномерный

sim_model = engee.load("$(@__DIR__)/positioning_tests/test_classic.engee", force=true)
res = engee.run(sim_model)
engee.close(sim_model)

plot(
    collect(res.dict["calc_x"]).value,
    collect(res.dict["calc_y"]).value,
    collect(res.dict["calc_z"]).value,
    lw = 2, color=:red, label="calc_traj"
)
plot!(
    collect(res.dict["x_true.1"]).value,
    collect(res.dict["y_true.1"]).value,
    collect(res.dict["z_true.1"]).value,
    lw = 1, color=:blue, label="real_traj",
    legendfont = font("Times New Roman", 12),
    titlefont = font("Times New Roman", 14),
    title = "Визуализация траектории движения"
)

plot(
    collect(res.dict["abs_x"]).value, label="abs_x"
)
plot!(
    collect(res.dict["abs_y"]).value, label="abs_y"
)
plot!(
    collect(res.dict["abs_z"]).value, label="abs_z"
)
plot!(
    collect(res.dict["rel_x"]).value, label="rel_x"
)
plot!(
    collect(res.dict["rel_y"]).value, label="rel_y"
)
plot!(
    collect(res.dict["rel_z"]).value, label="rel_z",
    title="Ошибки при суммарно-дальномерном подходе",
    legendfont = font("Times New Roman", 12),
    titlefont = font("Times New Roman", 14),
)

Разностно-Дальномерный

sim_model = engee.load("$(@__DIR__)/positioning_tests/test_diff.engee", force=true)
res = engee.run(sim_model)
engee.close(sim_model)

plot(
    collect(res.dict["x_calc"]).value,
    collect(res.dict["y_calc"]).value,
    collect(res.dict["z_calc"]).value,
    lw = 2, color=:red, label="calc_traj"
)
plot!(
    collect(res.dict["x"]).value,
    collect(res.dict["y"]).value,
    collect(res.dict["z"]).value,
    lw = 1, color=:blue, label="real_traj",
    legendfont = font("Times New Roman", 12),
    titlefont = font("Times New Roman", 14),
    title = "Визуализация траектории движения"
)

plot(
    collect(res.dict["abs_x"]).value, label="abs_x"
)
plot!(
    collect(res.dict["abs_y"]).value, label="abs_y"
)
plot!(
    collect(res.dict["abs_z"]).value, label="abs_z"
)
plot!(
    collect(res.dict["rel_x"]).value, label="rel_x"
)
plot!(
    collect(res.dict["rel_y"]).value, label="rel_y"
)
plot!(
    collect(res.dict["rel_z"]).value, label="rel_z",
    title="Ошибки при разностно-дальномерном подходе",
    legendfont = font("Times New Roman", 12),
    titlefont = font("Times New Roman", 14),
)

Квази-Дальномерный

sim_model = engee.load("$(@__DIR__)/positioning_tests/test_classic_time_corr.engee", force=true)
res = engee.run(sim_model)
engee.close(sim_model)

plot(
    collect(res.dict["calc_x"]).value,
    collect(res.dict["calc_y"]).value,
    collect(res.dict["calc_z"]).value,
    lw = 2, color=:red, label="calc_traj"
)
plot!(
    collect(res.dict["x_true.1"]).value,
    collect(res.dict["y_true.1"]).value,
    collect(res.dict["z_true.1"]).value,
    lw = 1, color=:blue, label="real_traj",
    legendfont = font("Times New Roman", 12),
    titlefont = font("Times New Roman", 14),
    title = "Визуализация траектории движения"
)

plot(
    collect(res.dict["abs_x"]).value, label="abs_x"
)
plot!(
    collect(res.dict["abs_y"]).value, label="abs_y"
)
plot!(
    collect(res.dict["abs_z"]).value, label="abs_z"
)
plot!(
    collect(res.dict["rel_x"]).value, label="rel_x"
)
plot!(
    collect(res.dict["rel_y"]).value, label="rel_y"
)
plot!(
    collect(res.dict["rel_z"]).value, label="rel_z",
    title="Ошибки при квази-дальномерном подходе",
    legendfont = font("Times New Roman", 12),
    titlefont = font("Times New Roman", 14)
)

Мы наблюдаем ограниченные скачки относительных (rel) ошибок в областях пересечения координатой нуля. Это ожидаемое поведение, данный выброс учитывать не нужно.

Таким образом алгоритмы прошли первые тесты, и их можно опробовать на реальной модели

Сборка модели беспилотного карьерного ТС

Соберем модель карьерного ТС. Рассмотрим файл vehicle_dynamics_test.engee Входами данной модели являются сигналы воздействия на рулевое колесо и на педаль газа/тормоза (в данной модели это один сигнал). Проведем моделирование на базовых ступенчатых воздействиях.

Поведение модели зависит от сохраненных данных карты. для данного примера возьмем карту - ровную поверхность 100м x 100м Добавим параметры при помощи обратных вызовов модели.

sim_model = engee.load("$(@__DIR__)/vehicle_test/vehicle_dynamics_test.engee", force=true)
engee.run(sim_model)
engee.close(sim_model)

pos_vect = collect(pos).value

plot(
    map(x->x[1], pos_vect),
    map(x->x[3], pos_vect), 
    map(x->x[2], pos_vect),
    lw=2, color=:red, label="traj",
    legendfont = font("Times New Roman", 12),
    titlefont = font("Times New Roman", 14),
    title = "Траектории ЦМ при ступенчатых воздействиях"
)

Сборка модели системы управления для беспилотного карьерного ТС

Соберем систему управления для карьерного беспилотного ТС. Модель управления вместе с моделью ТС реализована в файле simple_veh_model.engee. Рассмотрим ее.

Модель состоит из нескольких частей. Выдача маршрута - на основании данных карты, заданного маршрута и текущего положения ТС. Управление рулевым колесом, на основании выдаваемого маршрута и управление скоростью (разгон и торможение) на основании выдаваемого маршрута и сигнала рулевого управления.

Смоделируем движение транспортного средства по маршруту, представляющему собой диагональ на плоскости. В начальный момент времени ТС располагается в левом верхнем углу и стоит по направлению влево.

Проведем моделирование данной системы. Увидим, что реальная траектория движения ТС сходится к желаемой. При этом системе также присуща и колебательность.

sim_model = engee.load("$(@__DIR__)/vehicle_test/simple_veh_model.engee", force=true)
engee.run(sim_model)
engee.close(sim_model);

pos_vect_route = collect(pos_by_route).value

plot(
    map(x->x[1], pos_vect_route),
    map(x->-1*x[3], pos_vect_route), 
    map(x->x[2], pos_vect_route),
    lw=2, color=:red, label="traj"
)
plot!(
    route[:, 1],
    route[:, 2],
    [1.31 for _=1:length(route[:, 1])],
    lw=1, color=:blue,
    label="route",
    legendfont = font("Times New Roman", 12),
    titlefont = font("Times New Roman", 14),
    title = "Траектории ЦМ при движении по диагональному маршруту"
)

Для движения модели по более сложному маршруту нужно усовершенствовать логику управления и эмуляцию шумов.

Каждый ПИ регулятор снабдим логикой сброса чтобы избежать накопления ошибки от предыдущих звеньев маршрута
Передадим карту в нужные блоки через импортирование файла во внутрь с использованием пользовательского написание структуры engee function.

рассмотрим модель simple_veh_model_level_up.engee

sim_model = engee.load("$(@__DIR__)/vehicle_test/simple_veh_model_level_up.engee", force=true)
engee.run(sim_model)
engee.close(sim_model);

pos_vect_route_lu = collect(pos_by_route_level_up).value

plot(
    map(x->x[1], pos_vect_route_lu),
    map(x->-1*x[3], pos_vect_route_lu), 
    map(x->x[2], pos_vect_route_lu),
    lw=2, color=:red, label="traj"
)
plot!(
    route[:, 1],
    route[:, 2],
    [1.31 for _=1:length(route[:, 1])],
    lw=1, color=:blue,
    label="route",
    legendfont = font("Times New Roman", 12),
    titlefont = font("Times New Roman", 14),
    title = "Траектории ЦМ при движении по диагональному маршруту"
)

Моделирование движения ТС по данным от триангуляционных датчиков

Интегрируем систему расчета положения объекта в модель ТС и его САУ.

Расположим вышки с четырех сторон плоскости. Передадим так называемой подсистеме, отвечающей за выдачу маршрута не истинное значение положения объекта, получаемое от модели ТС, а значение, рассчитанное на основании данных с псевдо-датчиков расстояний до вышек и алгоритмов триангуляции.

Рассмотрим модель simple_veh_model_with_positioning_sum.engee

Проведем моделирование данной системы с тремя видами датчиков. Здесь же приведем только графики, наложив также график движения, не использующий данные алгоритмы позиционирования. Также предположим, что данные с вышек зашумлены из-за разного рода технических ограничений

include("$(@__DIR__)/veh_pos_test/tower_params.jl");

Суммарно-дальномерный подход

Будем выбирать высокую точность поиска для визуализации возможных систематических ошибок алгоритмов позиционирования.

sim_model = engee.load("$(@__DIR__)/veh_pos_test/simple_veh_model_with_positioning_sum.engee", force=true)
engee.run(sim_model)
engee.close(sim_model);

pos_vect_1 = collect(pos_by_sensors_sum).value

plot(
    map(x->x[1], pos_vect_1),
    map(x->-x[3], pos_vect_1), 
    map(x->x[2], pos_vect_1),
    lw=2, color=:red, label = "detectors_traj"
)
plot!(
    map(x->x[1], pos_vect_route_lu),
    map(x->-x[3], pos_vect_route_lu), 
    map(x->x[2], pos_vect_route_lu),
    color=:blue, label = "traj",
    legendfont = font("Times New Roman", 12),
    titlefont = font("Times New Roman", 14),
    legend=:bottomright,
    title = "Траектории ЦМ при движении по диагональнмоу маршруту по суммарно-дальномерным датчикам"
)

plot(sum.(abs.(pos_vect_1 .- pos_vect_route_lu)), title="Отклонение от траектории при суммарно-дальномерном подходе", legend = false,
    titlefont = font("Times New Roman", 14))

Разностно-дальномерный

sim_model = engee.load("$(@__DIR__)/veh_pos_test/simple_veh_model_with_positioning_diff.engee", force=true)
engee.run(sim_model)
engee.close(sim_model);

pos_vect_2 = collect(pos_by_sensors_diff).value

plot(
    map(x->x[1], pos_vect_2),
    map(x->x[3], pos_vect_2), 
    map(x->x[2], pos_vect_2),
    lw=2, color=:red, label = "detectors_traj"
)
plot!(
    map(x->x[1], pos_vect_route_lu),
    map(x->x[3], pos_vect_route_lu), 
    map(x->x[2], pos_vect_route_lu),
    color=:blue, label = "traj",
    legend=:bottomright,
    legendfont = font("Times New Roman", 12),
    titlefont = font("Times New Roman", 14),
    title = "Траектории ЦМ при движении по диагональнмоу маршруту по разностно-дальномерным датчикам"
)

plot(sum.(abs.(pos_vect_2 .- pos_vect_route_lu)), title="Отклонение от траектории при разностно-дальномерном подходе", legend = false,
    titlefont = font("Times New Roman", 14))

Квази-дальномерный

sim_model = engee.load("$(@__DIR__)/veh_pos_test/simple_veh_model_with_positioning_quazi.engee", force=true)
engee.run(sim_model)
engee.close(sim_model);

pos_vect_3 = collect(pos_by_sensors_quazi).value

plot(
    map(x->x[1], pos_vect_3),
    map(x->x[3], pos_vect_3), 
    map(x->x[2], pos_vect_3),
    lw=2, color=:red, label="detectors_traj"
)
plot!(
    map(x->x[1], pos_vect_route_lu),
    map(x->x[3], pos_vect_route_lu), 
    map(x->x[2], pos_vect_route_lu),
    color=:blue, label="traj",
    legend=:bottomright,
    legendfont = font("Times New Roman", 12),
    titlefont = font("Times New Roman", 14),
    title = "Траектории ЦМ при движении по диагональнмоу маршруту по квази-дальномерным датчикам"
)

plot(sum.(abs.(pos_vect_3 .- pos_vect_route_lu)), title="Отклонение от траектории при квази-дальномерном подходе", legend = false,
    titlefont = font("Times New Roman", 14))

Итог

Проведя моделирование получили что наименьшее отклонение от траектории т.н. теоретического управления получается при квази-дальномерном подходе. Выберем его для моделирования в реальном карьере

Моделирование по реальному маршруту реальной карты карьера

Проведем аналогичное моделирование для модели карьерного ТС воспользовавшись картой реального карьера. Расположим вышки по углам карьера.

Выберем следующее расположение вышек для суммарно-дальномерного подхода к задаче триангуляции

№	X	Y	Z
1	-460	-460	40
2	-460	460	20
3	460	-460	20
4	460	460	60

include("$(@__DIR__)/tower_params.jl")
include("$(@__DIR__)/map_params.jl")
include("$(@__DIR__)/car_params.jl");

sim_model = engee.load("$(@__DIR__)/simple_veh_model_with_positioning_quazi.engee", force=true)
engee.run(sim_model)
engee.close(sim_model)

Получим график измерений с датчиков положения объекта при движении объекта (карьерного ТС) по маршруту, ориентируясь на показания датчиков

pos_vect_4 = collect(pos_by_sensors_res).value[1:10:end]

plot(
    map(x->x[1], pos_vect_4),
    map(x->-x[3], pos_vect_4), 
    map(x->x[2], pos_vect_4),
    lw=4, color=:red, label="traj",
    legendfont = font("Times New Roman", 12),
    titlefont = font("Times New Roman", 14),
    title = "Траектории ЦМ при движении по маршруту по квази-дальномерным датчикам"
)
cicle_route =  [route ; route[1, :]']
plot!(
    cicle_route[:, 1],
    cicle_route[:, 2], 
    zeros,
    lw=4, color=:red, label="route",
    legendfont = font("Times New Roman", 12),
)

Применение триангуляции к модели движения ТС по маршруту

Реализация алгоритмов треангуляционного позиционирования объекта

Введение

Рассмотрим эти модели.

Построим графики моделирования

Суммарно-Дальномерный

Разностно-Дальномерный

Квази-Дальномерный

Сборка модели беспилотного карьерного ТС

Сборка модели системы управления для беспилотного карьерного ТС

Моделирование движения ТС по данным от триангуляционных датчиков

Суммарно-дальномерный подход

Разностно-дальномерный

Квази-дальномерный

Итог

Моделирование по реальному маршруту реальной карты карьера

Тип

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

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

Теги

Категории

Языки

Форматы

Уровень

Статус

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

Обновлено

Источник