PID Керування з TurtleSim

Анотація

Move Turtle - це простий скрипт на Python для ROS, який переміщує черепашку в TurtleSim ROS до вказаних координат (точок). Цей проект почався як класний проект для CSCI473 в Colorado School of Mines (CSOM) і згодом перетворився на проект, який допоміг мені зрозуміти, як можна створювати системи з замкнутим контуром, знання з якого було використано для моєї роботи в Лабораторії Людських Центрів Робототехніки (HCR) CSOM.

Контекст

Для деякого фону, ROS означає Robot Operating Systems. ROS - це програмне забезпечення з відкритим кодом для робототехніки, зазвичай поєднується з операційною системою Ubuntu, яке використовується для кращого управління програмними компонентами/кластерами роботизованої системи. Ви можете дізнатися більше про те, як використовувати ROS, через офіційну вікі. Код, який ви пишете в ROS, написаний або на C++, або на Python.

TurtleSim - це простий 2D-симулятор роботів, який використовується як вступ до ROS і пакетів ROS для нових користувачів ROS. Все, що робить TurtleSim, це відкриває простий 2D-дисплей з черепашкою. Ця черепашка діє як “робот”, і ви можете надсилати повідомлення цьому “роботу”, щоб переміщувати і/або повертати його. Під час переміщення TurtleSim генерує деяку випадкову помилку, щоб простим способом змоделювати фізику реального світу.

Знаючи, що таке ROS і TurtleSim, чому я їх використовував? Що ж, навесні 2020 року я проходив курс під назвою “Людська Центрована Робототехніка” в Colorado School of Mines в рамках мого навчального навантаження з комп’ютерних наук. Курс викладав доктор Хао Чжан. Курс складався з трьох проектів, і перший проект полягав у налаштуванні ROS, вивченні того, як використовувати ROS, і реалізації або відкритої, або замкнутої алгоритму, який змусив би черепашку в TurtleSim намалювати букву М. Оригінальний опис проекту можна переглянути ТУТ.

Коли я проходив курс, я завершив цей проект, реалізувавши відкриту алгоритм. Це рішення працювало, але було дійсно поганим і намалювало А, яка була “достатньо хорошою”, але ніяк не “чудовою”. Після подання проекту приблизно в середині лютого 2020 року, я забув про нього до року пізніше, приблизно в лютому 2021 року.

Виклик

У лютому 2021 року я був найнятий як науковий асистент у Лабораторії Людських Центрів Робототехніки (HCR) в Colorado School of Mines, де я працював під керівництвом доктора Хао Чжана. Мета лабораторії полягає в проведенні досліджень з “довгострокової колаборативної автономії, з метою дозволити роботам працювати та адаптуватися протягом тривалого часу”. Станом на 1 вересня 2022 року лабораторія переїхала з Colorado School of Mines до Університету Массачусетс Амхерст. Але, коли я працював там, вони все ще знаходилися в Колорадо.

У лабораторії HCR мені було доручено працювати над проектом Triton. Проект Triton складався з кількох десятків роботів, званих Тритонами. Тритони були трикутними омні-колесами наземними роботами, які виглядали ось так:

Разом з Тритонами, в лабораторії було вісім ІЧ-камер від OptiTrack, розташованих у “октогональному порядку” на кілька футів вище від землі. Використовуючи ці камери, програмне забезпечення Motive від Optitrack, ПК з Windows 10 та ROS; створюється простір розміром 2 метри на 2 метри на 2 метри (x, y, z), де можна визначити точне реальне положення об’єктів з мітками захоплення руху.

Моє перше завдання в проекті Triton полягало в створенні скриптів, які змушували б робота Тритон переміщатися до конкретної реальної позиції. На той момент я не був впевнений, як цього досягти, але я пам’ятав про перший проект з CSCI473, де мені було представлено ідею системи з замкнутим контуром, тому я почав дійсно досліджувати системи з замкнутим контуром і, в свою чергу, системи зворотного зв’язку.

Дослідження

Під час мого дослідження я натрапив на це чудове відео від AerospaceControlsLab:

У цьому відео мені було показано і пояснено ідею системи PID. Система PID - це система з замкнутим контуром, контрольна петля, яка виробляє специфічні виходи на основі даних з реального світу. У базових термінах, це система зворотного зв’язку, яка регулює свій вихід на основі різниці між бажаним значенням і виміряним значенням. Ось формула для PID контролера:

$$ u(t) = K_p e(t) + K_i \int e(t) dt + K_d \frac{de(t)}{dt} $$

$u(t)$ - це контрольний сигнал.
$K_p$, $K_i$, $K_d$ - це пропорційні, інтегральні та диференціальні коефіцієнти відповідно.
$e(t)$ - це сигнал помилки (різниця між бажаним і фактичним виходом).
$\int e(t) dt$ - це інтеграл помилки за часом.
$\frac{de(t)}{dt}$ - це похідна помилки.

Щоб дізнатися більше про це, перегляньте відео AerospaceControlsLab або цю чудову статтю у вікіпедії, з якої походить ця формула.

Після проведення дослідження, а також отримання допомоги від моїх розумних друзів і професорів, я визначив, що використання пропорційного контролера, P контролера, буде найкращим для допомоги Тритону досягти конкретної координати.

Оскільки Тритон є наземним роботом, мені потрібно було турбуватися лише про координати X і Y. Знаючи це, я тоді мав визначити, які бажані значення та виміряні значення потрібно вимірювати та обробляти в рамках P контролера(ів). У своєму дослідженні я знайшов цей чудовий пост на вікі ROS під назвою Go to Goal, де вони зосередилися на наступних помилках:

Помилка відстані: різниця в відстані між бажаним місцем (X, Y) та поточним місцем (X, Y).
Помилка Тета: різниця між бажаною орієнтацією та поточною орієнтацією.

Запропоноване рішення

Знаючи все це, рішення для того, щоб Тритон перемістився до конкретної реальної координати, полягає в використанні пропорційного контролера для помилки відстані Тритона та помилки тета відносно цільової координати та поточної координати Тритона. Але перед тим, як реалізувати це в фізичній системі, я хотів протестувати ідею в симуляції, і на той момент я вважав, що використання TurtleSim як симуляції було найкращим вибором.

Тестування в симуляції

З теоретичним рішенням, я вирішив спочатку перевірити його через симуляцію, а не в реальному світі. Реальний світ часто переповнений шумом, який може спотворити ефективність рішення і призвести до вирішення нерелевантних проблем. Визнаючи це, я знову звернувся до свого проекту з CSCI473 і адаптував його як тестову платформу для цього теоретичного рішення.

Після оновлення старого коду з використання ROS Melodic на ROS Noetic і з деякими ітеративними вдосконаленнями, я зміг реалізувати запропоноване рішення в TurtleSim. Демонстрації його функціональності, з деякими класними шляхами цільових координат, можна знайти в кінці цього блогу. Код для всього цього можна переглянути ТУТ.

Висновок

Підсумовуючи, через цей проект черепашка TurtleSim ефективно навігує до встановлених координат, використовуючи систему з замкнутим контуром, значно перевершуючи мою початкову відкриту систему з 2020 року. Успішно протестувавши, я реалізував це рішення з роботами Тритон. Але шум реального світу викликав несподівані проблеми, які вимагали тижнів налагодження та тестування в реальному світі. Після цих тижнів я врешті-решт змусив Тритон досягти конкретних реальних координат, використовуючи рішення, яке я протестував у TurtleSim. Ось демонстрація Тритона, що рухається до конкретних реальних координат, використовуючи метод, протестований у TurtleSim:

Озираючись назад, я б хотів, щоб я використовував симуляцію, яка краще моделює фізику реального світу. TurtleSim чудовий для навчання, але не дуже підходить для тестування ідей реальної робототехніки. На той момент я вважав, що це буде достатньо для мого простого випадку використання, але це пізніше виявилося не так.

Основною метою цього блогу було окреслити створення системи з замкнутим контуром, що дозволяє черепашці в TurtleSim, а згодом і роботам Тритон, точно навігувати до вказаних цільових позицій (X, Y). Я вважаю, що ця мета була досягнута, але це могло викликати певний інтерес до моєї роботи в лабораторії HCR. Знаючи це, я маю плани на створення більше блогів про мій досвід роботи в лабораторії HCR…

Демонстрації Рухомої Черепашки (TurtleSim)

Це відео містить наступні демонстрації:

Cayley Nodal: Простий дизайн Cayley Nodal
Кола: Дизайн, схожий на павутину
Внутрішні Кола: Дизайн “кола всередині кіл”
М: Дуже простий дизайн М