Всем привет! Продолжаем наш путь в захватывающий мир беспилотных технологий. В первой части статьи мы познакомились с симулятором Carla, создали собственный беспилотный автомобиль и научили его ехать прямо.
Во второй части мы займемся улучшением плавности хода при помощи PID-контроллера, освоим алгоритм Stanley для точного управления рулём и научим машину реагировать на внезапные препятствия. Готовы погрузиться глубже и сделать ваш виртуальный беспилотный автомобиль ещё умнее и безопаснее? Тогда пристёгивайтесь и поехали!
PID регулятор скорости
Код на Github
Команда запуска: python -m sd_5
В первой части статьи мы научили наш автомобиль поддерживать скорость путем постоянного ускорения и торможения. Чтобы улучшить плавность движения, давайте реализуем PID-регулятор скорости.
PID-регулятор является одним из наиболее распространенных автоматических регуляторов. Он нужен, чтобы автоматически и плавно удерживать нужную величину (например, скорость автомобиля) без резких колебаний. Состоит из трёх составляющих:
Пропорциональная (P) - Влияет на текущую «ошибку» — разницу между фактической и целевой скоростью. Чем больше ошибка, тем сильнее «газ» или «тормоз».
Интегральная (I) - Учитывает накопленную ошибку за всё время работы. Если постоянно не хватает скорости, интегральная часть постепенно увеличивает нажатие педали газа, чтобы устранить постоянное отставание.
Дифференциальная (D) - Отслеживает, как быстро меняется ошибка. Помогает сгладить резкие скачки управления и предотвратить «перерегулирование». Благодаря ей управление становится более плавным: вместо рывка «газ–тормоз–газ» машина аккуратно подстраивается под изменение ошибки.
Каждая из этих трёх составляющих умножается на свой коэффициент (Kp, Ki, Kd), а их сумма дает итоговый управляющий сигнал. Таким образом PID‑регулятор автоматически добавляет или убавляет «газ» и «тормоз» так, чтобы машина точно держала заданную скорость и при этом не «рывком» переходила от одного значения к другому.
Реализуем это в коде.
Пропорциональная часть на основе разницы текущей и целевой скорости:
# Рассчитываем ошибку
error = v_desired - v_current
# Пропорциональная часть
proportional = self.kp * errorИнтегральная часть накапливает ошибку за промежуток времени. Она суммирует предыдущие накопленные ошибки и текущую ошибку, умноженную на время, прошедшее с предыдущего измерения (delta_t). Эта составляющая помогает устранить постоянные отклонения от заданной скорости:
# Интегральная часть
self._integral = self._integral + error * delta_t
integral = self.ki * self._integralДифференциальная часть рассчитывается как разность текущей и предыдущей ошибки, деленная на время между измерениями (delta_t). Эта составляющая реагирует на резкие изменения состояния системы и позволяет стабилизировать процесс управления:
# Дифференциальная часть
derivative = self.kd * (error - self._previous_error) / delta_tСуммируем полученные значения и преобразуем их в управляющие сигналы:
u = proportional + integral + derivative
if u >= 0:
control.throttle = min(u, 1.0)
control.brake = 0.0
else:
control.throttle = 0.0
control.brake = min(abs(u), 1.0)Запустим и проверим результат:
python -m sd_5
Waypoint Following with PID Speed Control
Движение автомобиля стало гораздо более плавным и стабильным. Перейдем к следующей части — управлению рулем.
Контроль поворота руля (Stanley)
Код на Github
Команда запуска: python -m sd_6
После того как мы научились двигаться прямо и поддерживать заданную скорость, перейдем к следующему важному шагу — повороту руля. Для начала обновим наши путевые точки (waypoints). Теперь наша задача состоит в том, чтобы проехать траекторию, напоминающую по форме букву "П".
Для вычисления угла поворота руля нам понадобится латеральный контроллер (он же поперечный контроллер). Это алгоритм, который автоматически компенсирует отклонение автомобиля от заданной траектории. Для это используются две точки:
Целевая путевая точка — это следующая точка маршрута, к которой автомобиль стремится.
Предыдущая путевая точка — это точка, через которую автомобиль уже проехал непосредственно перед целевой.
По этим двум точкам строится воображаемая прямая (идеальная траектория).
Метод Stanley
Мы применяем алгоритм Stanley, разработанный командой DARPA Grand Challenge (Стэнфордский университет). Он рассчитывает два компонента рулевого управления:
Угол ошибки по направлению (heading error) — разница между курсом идеальной траектории и текущим курсом автомобиля.
Поперечная ошибка (cross-track error) — боковое смещение автомобиля от идеальной линии.
Сумма этих двух углов даёт итоговый угол поворота руля, который минимизирует отклонение и выводит машину обратно на маршрут.
Сначала вычислим разницу угла между идеальной траекторией и текущим углом автомобиля. Угол траектории рассчитывается по координатам текущей целевой и предыдущей точек:
# Угол сегмента пути
yaw_path = np.arctan2(wp_target[1] - wp_prev[1], wp_target[0] - wp_prev[0])
# Ошибка по курсу
yaw_diff = yaw_path - vehicle_yaw_radЗатем определим расстояние от текущего положения автомобиля до траектории движения. Для этого воспользуемся уравнением прямой, проходящей через две точки (целевую и предыдущую).
diff_x = target_x - previous_x
diff_y = target_y - previous_y
if diff_x == 0:
diff_x = 0.1
slope = diff_y / diff_x
correction = previous_y - slope * previous_x
# Переводим уравнение в форму ax + by + c = 0
a = -slope
b = 1.0
c = correctionТеперь вычислим поперечную ошибку (Cross-Track Error или CTE), которая показывает расстояние от текущего положения до заданной траектории:
# Расчет поперечной ошибки
if abs(diff_x) < 1e-6:
# Вертикальная линия: ошибка по X
crosstrack_error = vehicle_location.x - wp_prev[0]
else:
slope = diff_y / diff_x
a = -slope
b = 1.0
c = (slope * wp_prev[0]) - wp_prev[1]
crosstrack_error = (a * vehicle_location.x + b * vehicle_location.y + c) / np.sqrt(a ** 2 + b ** 2)Затем мы скорректируем угол поворота руля с учетом поперечного отклонения и текущей скорости автомобиля по формуле Stanley:
yaw_diff_crosstrack = np.arctan(k * crosstrack_error / v)
# Итоговый угол поворота руля
steer_expect = yaw_diff + yaw_diff_crosstrackСоздадим более сложный маршрут с поворотами и разворотами. На графике сверху отобразим запланированную траекторию (синим цветом) и фактический маршрут автомобиля (красным). Запустим и проверим результат:
Waypoint Following with PID Speed, Stanley Steering
Отлично! Наш беспилотник научился ездить четко "по рельсам", придерживаясь заданной скорости и траектории.
Кратко: модуль Control отвечает за реализацию движения по заданным waypoints. Он получает текущее состояние машины (Localization) и путевые точки (Planning), а на выходе формирует управляющие команды для автомобиля (steer, throttle, brake).
В следующей части мы разработаем систему автоматического торможения перед препятствиями.
Perception — распознавание объектов вокруг
Код на Github
Команда запуска: python -m sd_7
Наш беспилотник пока движется «вслепую»: он не знает о карте, расположении домов, дорог и препятствий. Единственное, что ему известно, — это текущие координаты и запланированная траектория. Если на его пути вдруг появится автомобиль или пешеход, произойдет ДТП.
Для предотвращения таких ситуаций используется perception — система восприятия окружающей среды. Обычно беспилотник оснащается тремя основными типами сенсоров:
Лидар — основной сенсор беспилотника. Это лазерный дальномер, создающий подробную 3D-карту окружающего пространства в радиусе до 100 метров. Лидар распознает объекты, их форму, размер и расстояние до них. Если вам интересно подробнее узнать, как запустить лидар в Carla и визуализировать полученные данные, рекомендую почитать здесь: https://earnold.me/post/mayavilidar/
Радар — использует радиоволны для определения расстояния до объектов, их скорости и траектории. Радар постоянно посылает импульсы, которые отражаются от препятствий и позволяют машине оперативно реагировать на изменения на дороге.
Камеры — необходимы для распознавания сигналов светофоров, дорожных знаков и разметки.
Интеграция этих датчиков и обучение моделей распознавания объектов — сложный и длительный процесс. Однако симулятор Carla предоставляет удобный виртуальный датчик препятствий, объединяющий функционал всех сенсоров.
obstacle_detected_state = False
obs_bp_library = world.get_blueprint_library()
obs_bp = obs_bp_library.find('sensor.other.obstacle')
obs_bp.set_attribute("distance", "15") # расстояние обнаружения (метры)
obs_bp.set_attribute("sensor_tick", "0.1") # частота проверки (секунды)
obstacle_sensor = world.spawn_actor(obs_bp, sensor_transform, attach_to=vehicle)
# Настраиваем обработку событий:
obstacle_sensor.listen(obstacle_callback):
obstacle_detected_state = True
# В случае обнаружения препятствия максимально быстро тормозим:
if obstacle_detected_state:
print(" -> Obstacle detected! OVERRIDING control: EMERGENCY BRAKE")
control.throttle = 0.0
control.brake = 1.0 # Полное торможение
#Добавим машину-препятствие на дорогу:
obstacle_bp = world.get_blueprint_library().find('vehicle.audi.tt')
obstacle_bp.set_attribute('role_name', 'obstacle')
obstacle_location = carla.Location(x=10.0, y=24.5, z=0.1)
obstacle_transform = carla.Transform(obstacle_location, carla.Rotation(yaw=0))
obstacle_vehicle = world.try_spawn_actor(obstacle_bp, obstacle_transform)Проверим работу:
python -m sd_7
Waypoint Following with Obstacle Avoidance
Теперь наш беспилотник успешно останавливается перед препятствиями!
Однако что произойдет, если пешеход неожиданно выйдет перед машиной? Чтобы избежать таких ситуаций, в реальные беспилотники устанавливают дополнительный модуль Prediction.
Кратко: модуль Perception отвечает за распознавание объектов вокруг автомобиля. На вход поступают данные от сенсоров и текущее состояние машины (Localization). На выходе формируется информация о распознанных объектах (тип, размеры, скорость, ускорение и направление движения).
Prediction – предсказываем движение других объектов
В нашем виртуальном мире пока нет движущихся объектов, кроме самого автомобиля. Однако в реальной жизни ситуация гораздо сложнее — вокруг машины постоянно движутся другие автомобили, пешеходы и различные объекты. Чтобы успешно избегать столкновений, беспилотнику необходимо уметь предсказывать, как будут вести себя окружающие объекты. Для этого используются два основных подхода: алгоритмический и машинное обучение.
Алгоритмический подход основан на текущем состоянии объектов. Если мы знаем скорость и направление движения другой машины или пешехода, то можем вычислить, пересекутся ли наши траектории, и заранее принять меры для предотвращения столкновения.
Модели машинного обучения, напротив, обучаются на огромных наборах данных из миллионов реальных ситуаций на дорогах. Такие модели способны предсказывать неожиданные маневры других участников дорожного движения — например, резкие торможения или неожиданные повороты. Успех этого метода зависит от объема данных и качества применяемых алгоритмов.
В рамках статьи мы не будем реализовать свой модуль Prediction, так как это потребовало бы еще 2-3 статьи. Если вам интересно, вы можете ознакомиться с реализацией модуля prediction в проекте pylot (раздел prediction) или попробовать построить модель самому
Кратко: Prediction отвечает за прогнозирование будущего поведения окружающих объектов. На входе он получает данные о распознанных объектах (Perception) и текущее состояние автомобиля (Localization). На выходе формируется информация о возможных маршрутах и поведении объектов.
Общая схема работы модулей
Еще несколько схем работы беспилотного автомобиля
Заключение
На этом наша статья подходит к концу. Если вы хотите продолжить погружение в мир беспилотных автомобилей и автономных технологий, рекомендую пройти соответствующие курсы на платформах Coursera или Udacity.
А если однажды вам на дороге встретится беспилотный автомобиль — помните, что он внимательно следит за вами с помощью своих сенсоров (Perception) и пытается предугадать ваши действия (Prediction). Он всегда знает, где находится (Localization), заранее планирует безопасный путь (Planning) и движется по маршруту (Control). Так что не стесняйтесь, улыбнитесь и помашите рукой — камеры и лидары обязательно это заметят!