Привет, Хабр! Недавно прошел ROS Russian Meetup, посвященный робототехнике. На митапе наша команда Bauman Racing Team из МГТУ им. Н.Э. Баумана представила собственный симулятор для беспилотного гоночного автомобиля, о котором пойдет речь в данной статье.

Наша команда работает над постройкой электрического и беспилотного гоночного болида Формула Студент.

Формула Студент – это студенческие инженерные соревнования, зародившиеся в США и получившие большую популярность в Европе. Главной целью соревнований является подготовка высококвалифицированных инженерных кадров для динамично развивающейся автомобильной индустрии.

Соревнования беспилотников состоят из нескольких гонок. На видео представлен заезд болида формула студент в наиболее сложной дисциплине Track Drive.

По условиям соревнований автомобиль должен преодолеть 10 кругов по заранее неизвестной трассе при длине одного круга около полукилометра.

Для решения этой задачи автомобилю необходимо на первом круге построить карту, двигаясь с относительно невысокой скоростью, и затем преодолеть 9 кругов на максимально высокой скорости. Результатом заезда является общее время, за которое автомобиль преодолевает 10 кругов гонки.

Для участия в соревнованиях автомобиль должен быть оборудован блоками управления, датчиками и приводами. Блоками управления нашего болида являются контроллеры верхнего и нижнего уровней. Датчиками являются камеры, лидар, датчики одометрии, IMU и GNSS. Приводами являются электрическая силовая установка, рулевой электропривод и тормозной пневмопривод.

Помимо железной составляющей беспилотному болиду необходимы алгоритмы управления. Алгоритмы управления нашего автомобиля состоят из модулей Perception, Velocity estimator, SLAM, Path planner и Control.

Модуль Perception принимает на вход данные с камер и лидара и выдает положения и цвета увиденных конусов.

Модуль Velocity estimator принимает на вход данные с IMU, GNSS и датчиков колесной одометрии. После комплексирования принятых данных Velocity estimator выдает три скорости автомобиля: продольную, поперечную и угловую.

Данные от модулей Perception и Velocity estimator поступают в модуль SLAM, который занимается построением карты и определением положения автомобиля.

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

Построенная траектория движения поступает в блок Control, задача которого - генерировать управляющее воздействие на исполнительные приводы.

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

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

Рассмотрим сценарии тестирования, для которых был разработан наш симулятор.

Для тестирования модуля Perception симулятор должен обладать реалистичными моделями камер и лидара. При этом модуль Velocity estimator вместе с его датчиками может быть заменен псевдо датчиком скорости автомобиля, а модель движения автомобиля может быть упрощена до минимума и в качестве нее может быть использована кинематическая модель.

Для тестирования модуля Velocity estimator ситуация обратная. Модулю необходимы реалистичные датчики IMU, GNSS и датчики колесной одометрии. Для получения наиболее реалистичных данных с колесной одометрии в симуляторе необходимо использовать сложную динамическую модель движения автомобиля. В качестве замены блока Perception и его датчиков может быть использован псевдо сенсор положения конусов, который будет выдавать положения и цвета конусов в заданной области пространства вокруг болида.

Для тестирования модулей SLAM и Path planner необходимы псевдо сенсоры положения конусов, псевдо датчик скорости автомобиля и кинематическая модель движения.

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

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

Рассмотрев сценарии, мы сформировали требования к математической модели автомобиля и виртуальным датчикам нашего симулятора.

В дополнении к перечисленным требованиям были сформированы требования к среде симуляции.

1. Доступность. Среда должна быть доступна для ее легального использования в некоммерческом проекте;

2. Интеграция с ROS2. В ней должна быть реализована поддержка ROS2, так как эта версия фреймворка используется в нашем беспилотнике;

3. Хорошая документация к API, чтобы при необходимости можно было дописывать свои модули;

4. Относительно невысокие требования к вычислительной мощности компьютера, так как будет запускаться на средних ноутбуках разработчиков;

5. Наличие возможности проводить симуляцию в реальном времени для того, чтобы создать условия для тестирования алгоритмов и блоков управления, максимально приближенные к реальным;

6. Должна быть относительно популярной, чтобы было большое комьюнити, у которого можно было бы искать ответы на возникающие вопросы.

При выборе среды были рассмотрены CARLA, которая является очень популярной среди команд разработчиков, занимающихся постройкой беспилотных автомобилей и Gazebo, наверное, самая популярная среда симуляции для всевозможной робототехники.

Выбор производился по оговоренным параметрам.

CARLA проиграла Gazebo по требованиям к железу, потому что ей требуются относительно мощный процессор и видеокарта.

Также CARLA проиграла в популярности.

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

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

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

Для программной реализации сенсора положения конусов сенсор был создан класс ConeSensor, который представляет собой наследника класса gazebo::ModelPlugin. Его задача публиковать положения и цвета конусов, расположенных вблизи болида, в соответствующий топик, зная расположение всех конусов на трассе и текущее положение автомобиля.

В зависимости от значения параметров, переданных при инициализации объекта, ConeSensor может имитировать работу камеры или работу лидара.

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

Обе модели представляют собой так называемые bicycle model, у которых вместо четырех колес есть только два, первое стоит в центре передней оси, а второе стоит в центре задней оси.

Из названия ясно, что кинематическая модель учитывает только геометрические характеристики автомобиля, а динамическая помимо геометрических характеристик учитывает еще и силы, возникающие в модели.

Сразу перейдем к рассмотрению реализации динамической модели, поскольку именно она является наиболее сложной и поэтому представляет особый интерес.

Справа на изображении представлен вектор изменения состояния системы.

Первые две координаты вектора – скорости центра автомобиля в глобальной системе координат вдоль осей X и Y.

Третья координата вектора -  угловая скорость автомобиля.

Последние три координаты – продольное, поперечное и угловое ускорения автомобиля.

При расчете сил, возникающих в пятне контакта шин с дорогой, необходимо использовать уравнения, учитывающие нелинейную зависимость данных сил от угла скольжения шины (slip angle) и величины проскальзывания (slip ratio).

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

На слайде приведен пример уравнений для расчета сил в пятне контакта заднего колеса. Из уравнений видно, что они оба зависят от величины угла скольжения и величины проскальзывания. Это так называемые  Magic Formula (Pacejka) для сложного скольжения (combined slip), состоящего одновременно из продольного и поперечного соскальзываний. Эти уравнения позволяют учесть взаимное влияние поперечной и продольной сил, то есть при увеличении продольной силы, будет уменьшаться максимальная поперечная сила и наоборот.

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

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

Благодаря двум недавним статьям (статья №1), (статья №2) данную проблему удалось решить, используя уравнения для вычисления угла скольжения и величины проскальзывания, представленные на изображении справа.

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

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

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

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

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

Для программной реализации двух моделей было решено создать три класса: BaseModel, Axis и Wheel. Класс BaseModel объединил в себе общий интерфейс для общения с моделями. Через него можно отправлять в модель управляющие воздействия, забирать текущее состояние автомобиля и обновлять состояние модели.

Класс Axis объединил в себе общий интерфейс для осей автомобиля, с помощью него клиентский код может получать значение сил, возникающих в оси.

Класс Wheel объединил в себе общий интерфейс для колес, с помощью которого клиентский код может получать значения сил, возникающих в пятне контакта.

Имея общие интерфейсные классы были созданы два типа колес, два типа осей и два типа самих моделей. В последствии количество классов осей и моделей расширится, потому что сейчас идет работа над созданием четырехколесной модели автомобиля.

Объект класса BaseModel является частью класса RaceCarModel. который в свою очередь является наследником класса gazebo::ModelPlugin. Через класс RaceCarModel объект класса BaseModel получает управляющее воздействие из топика control_command.

Объекты классов CarSpeedSensor и WheelsSpeedSensor также являются частью класса RaceCarModel. Их задача забирать вектор состояния системы от BaseModel и, накладывая шум, публиковать данные о скорости автомобиля и угловых скоростях колес в соответствующие топики через класс RaceCarModel.

В результате был получен симулятор, способный тестировать все модули в отдельности и одновременно. На слайде продемонстрирован процесс тестирования модулей Perception, SLAM и Path planner в нашем симуляторе.

Слева сверху приведено изображение, на котором происходит детектирование конусов с помощью виртуальной камеры и нейросети YOLO v5.

Слева снизу приведено изображение, на котором продемонстрирована работа виртуального лидара.

Справа приведено изображение, на котором представлена работа сенсора положения конусов, который детектирует конусы вблизи автомобиля.

Ниже представлены две видеозаписи того, как движется один и тот же автомобиль по одной и той же трассе с двумя разными математическими моделями. Автомобиль слева имеет кинематическую модель, справа – динамическую модель.

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

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

В заключение хочется сказать, что симулятор все еще находится в активной стадии разработки и как только качество кода и документации дойдет до приемлемого уровня, симулятор будет опубликован в открытый доступ. Работе с ним будут посвящены отдельные статьи, поэтому следите за обновлениями и всем удачи в ваших проектах!