Сообщество Engee
Engee Community
/
Примеры и проекты
/
Применение триангуляции к модели движения ТС по маршруту

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

Автор
avatar-bogoslmbogoslm
Notebook

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

Введение

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

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

    • Реализован в модели test_classic.engee

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

    • Реализован в модели test_diff.engee

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

    • Реализован в модели test_classic_time_corr.engee

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

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

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

  • Псевдо-датчик расстояния от объекта до вышки

  • Блок оценки ошибки

Снимок экрана 2025-11-18 в 19.05.16.png
Рассмотрим эти модели.

test_classic.engee

Снимок экрана 2025-11-18 в 19.00.53.png

test_diff.engee

Снимок экрана 2025-11-18 в 19.06.55.png

test_classic_time_corr.engee

Снимок экрана 2025-11-18 в 19.08.19.png

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

Построим графики моделирования
Суммарно-Дальномерный
In [ ]: 
sim_model = engee.load("$(@__DIR__)/test_classic.engee", force=true)
res = engee.run(sim_model)
engee.close(sim_model)
In [ ]: 
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",
)
Out[0]:
In [ ]: 
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="Ошибки"
)
Out[0]:
Разностно-Дальномерный
In [ ]: 
sim_model = engee.load("$(@__DIR__)/test_diff.engee", force=true)
res = engee.run(sim_model)
engee.close(sim_model)
In [ ]: 
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",
)
Out[0]:
In [ ]: 
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="Ошибки"
)
Out[0]:
Квази-Дальномерный
In [ ]: 
sim_model = engee.load("$(@__DIR__)/test_classic_time_corr.engee", force=true)
res = engee.run(sim_model)
engee.close(sim_model)
In [ ]: 
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",
)
Out[0]:
In [ ]: 
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="Ошибки"
)
Out[0]:

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

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

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

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

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

Снимок экрана 2025-11-18 в 19.43.36.png
Снимок экрана 2025-11-18 в 19.43.48.png
In [ ]: 
sim_model = engee.load("$(@__DIR__)/vehicle_dynamics_test.engee", force=true)
engee.run(sim_model)
engee.close(sim_model)
In [ ]: 
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
)
Out[0]:

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

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

Снимок экрана 2025-11-21 в 17.22.49.png

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

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

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

In [ ]: 
sim_model = engee.load("$(@__DIR__)/simple_veh_model.engee", force=true)
engee.run(sim_model)
engee.close(sim_model);
In [ ]: 
pos_vect_route = collect(pos_by_route).value

plot(
    map(x->x[1], pos_vect_route),
    map(x->x[3], pos_vect_route), 
    map(x->x[2], pos_vect_route),
    lw=2, color=:red
)
plot!(
    route[:, 1],
    route[:, 2],
    [1.31 for _=1:length(route[:, 1])],
    lw=1, color=:blue
)
Out[0]:

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

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

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

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

Снимок экрана 2025-11-23 в 18.45.18.png

Проведем моделирование данной системы с тремя видами датчиков.

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

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

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

In [ ]: 
# pos_vect_1 = collect(pos_by_sensors).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
)
plot!(
    map(x->x[1], pos_vect_route),
    map(x->x[3], pos_vect_route), 
    map(x->x[2], pos_vect_route),
    color=:blue
)
Out[0]:
In [ ]: 
plot(sum.(abs.(pos_vect_1 .- pos_vect_route)), title="Отклонение от траектории")
Out[0]:
Разностно-дальномерный
In [ ]: 
# pos_vect_2 = collect(pos_by_sensors).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
)
plot!(
    map(x->x[1], pos_vect_route),
    map(x->x[3], pos_vect_route), 
    map(x->x[2], pos_vect_route),
    color=:blue
)
Out[0]:
In [ ]: 
plot(sum.(abs.(pos_vect_2 .- pos_vect_route)), title="Отклонение от траектории")
Out[0]:
Квази-дальномерный
In [ ]: 
pos_vect_3 = collect(pos_by_sensors).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
)
plot!(
    map(x->x[1], pos_vect_route),
    map(x->x[3], pos_vect_route), 
    map(x->x[2], pos_vect_route),
    color=:blue
)
Out[0]:
In [ ]: 
plot(sum.(abs.(pos_vect_3 .- pos_vect_route)), title="Отклонение от траектории")
Out[0]:
Итог

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

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

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

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

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

Также стоит заметить, что координаты для вышек переданы в соответствии с внутренней системой координат объекта.

Снимок экрана 2025-11-30 в 02.35.42.png

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

№

X

Y

Z

1

-460

-460

40

2

-460

460

20

3

460

-460

20

4

460

460

60
In [ ]: 
include("$(@__DIR__)/tower_params.jl")
include("$(@__DIR__)/map_params.jl")
include("$(@__DIR__)/car_params.jl");
In [ ]: 
sim_model = engee.load("$(@__DIR__)/simple_veh_model_with_positioning.engee", force=true)
engee.run(sim_model)
engee.close(sim_model)

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

In [ ]: 
pos_vect_4 = collect(pos_by_sensors).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
)
Out[0]:
Группа видимости
ПубличноПросматривать контент могут все пользователи сообщества

Тип

Интерактивные скрипты

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

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

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

Пусто

Теги

Категории

Учебные дисциплиныИнженерное направлениеАвтомобили

Языки

    Julia

Форматы

    ngscript

Уровень

    Средний

Статус

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

Обновлено

Источник

Сообщество