Використання Vision або Motion Capture систем для Position Estimation

Системи візуальної інерційної одометрії (VIO) та захоплення руху (MoCap) дозволяють транспортним засобам здійснювати навігацію, коли джерело глобального позиціонування недоступне або ненадійне (наприклад, в приміщенні або під час прольоту під мостом. тощо).

І VIO, і MoCap визначають положення (положення і позицію) транспортного засобу на основі "візуальної" інформації. Основна відмінність між ними - перспектива кадру:

• VIO використовує бортові датчики для отримання даних про позу з точки зору транспортного засобу (див. egomotion).
• MoCap використовує систему зовнішніх камер для отримання даних про положення транспортного засобу в 3D-просторі (тобто це зовнішня система, яка повідомляє транспортному засобу його положення).

Дані про положення, отримані від обох типів систем, можуть бути використані для оновлення оцінки локального положення автопілота на базі PX4 (відносно локальної точки відліку), а також, за бажанням, можуть бути інтегровані в оцінку положення транспортного засобу. Крім того, якщо зовнішня система позиціонування також забезпечує вимірювання лінійної швидкості, її можна використовувати для покращення оцінки стану (об'єднання вимірювань лінійної швидкості підтримується лише EKF2).

У цій темі пояснюється, як налаштувати систему на базі PX4 для отримання даних від систем MoCap/VIO (або через ROS, або через іншу систему MAVLink) і, зокрема, як налаштувати системи MoCap, такі як VICON і Optitrack, і системи оцінки на основі комп'ютерного зору, такі як ROVIO, SVO та PTAM).

INFO

Інструкції відрізняються залежно від того, чи використовуєте ви оцінювач EKF2 або LPE.

Інтеграція PX4 з MAVLink

PX4 використовує наступні повідомлення MAVLink для отримання інформації про зовнішню позицію і зіставляє їх з темами uORB:

MAVLink	uORB
VISION_POSITION_ESTIMATE	vehicle_visual_odometry
ODOMETRY (frame_id = MAV_FRAME_LOCAL_FRD)	vehicle_visual_odometry
ATT_POS_MOCAP	vehicle_mocap_odometry
ODOMETRY (frame_id = MAV_FRAME_MOCAP_NED)	vehicle_mocap_odometry

EKF2 підписується лише на теми vehicle_visual_odometry і тому може обробляти лише перші дві повідомлення (система MoCap повинна генерувати ці повідомлення для роботи з EKF2). Повідомлення odometry є єдиним повідомленням, яке може також відправляти лінійні швидкості в PX4. Оцінювач LPE підписується на обидві теми, тому може обробляти всі вищезазначені повідомлення.

TIP

EKF2 - це типовий оцінювач, який використовується PX4. Він краще протестований і підтримується, ніж LPE, і його слід використовувати за умовчанням.

Повідомлення повинні транслюватися з частотою між 30 Гц (якщо містять коваріанти) і 50 Гц. Якщо частота повідомлень занадто низька, EKF2 не буде обробляти повідомлення з зовнішнього візуального спостереження.

Наступні повідомлення "візій" MAVLink наразі не підтримуються PX4: GLOBAL_VISION_POSITION_ESTIMATE, VISION_SPEED_ESTIMATE, VICON_POSITION_ESTIMATE

Основні відмінності

PX4 використовує FRD (X вперед, Y праворуч і Z донизу) для локального тілесного каркасу, а також для опорного каркасу. Коли використовується напрямок магнітометра, вісь x опорного каркасу PX4 буде вирівнена з північним напрямком, тому вона називається NED (X північ, Y схід, Z донизу). Напрямок опорного каркасу оцінювача PX4 та напрямок зовнішньої оцінки положення в більшості випадків не збігаються. Тому напрямок опорного каркасу зовнішньої оцінки положення має різну назву - він називається MAV_FRAME_LOCAL_FRD.

Залежно від джерела вашого опорного каркасу, вам потрібно буде застосувати власне перетворення до оцінки положення перед надсиланням повідомлення MAVLink Vision/MoCap. Це необхідно для зміни орієнтації батьківського та дочірнього каркасу оцінки положення так, щоб вона відповідала конвенції PX4. Подивіться на плагін оцінювання MAVROS odomдля необхідних перетворень.

TIP

Користувачі ROS можуть знайти більш детальні інструкції нижче в розділі Основні відмінності та ROS.

Наприклад, якщо ви використовуєте фреймворк Optitrack, локальний каркас має координати $x$ та $z$ на горизонтальній площині (x вперед і z праворуч), в той час як вісь y є вертикальною і спрямована вверх. Простий трюк полягає в тому, що обмінюються вісіми, щоб отримати конвенцію NED.

Якщо x_{mav}, y_{mav} і z_{mav} - це координати, які надсилаються через MAVLink як зворотний зв'язок з позицією, тоді ми отримуємо:

x_{mav} = x_{mocap}
y_{mav} = z_{mocap}
z_{mav} = - y_{mocap}

Щодо орієнтації, залишайте частину скаляра w кватерніону такою самою і обмінюйте частину вектора x, y та z так само. Ви можете застосувати цей трюк у будь-якій системі - якщо вам потрібно отримати рамку NED, подивіться на вивід вашого MoCap та обміняйте вісі відповідно.

Конфігурація та налаштування EKF2

Примітка: це короткий огляд. Для отримання більш детальної інформації перегляньте посібник з налаштування EKF2

Наступні параметри повинні бути встановлені для використання зовнішньої інформації про положення з EKF2 (їх можна встановити в QGroundControl > Налаштування транспортного засобу > Параметри > EKF2).

Параметр	Налаштування для Зовнішньої Оцінки Положення
EKF2_EV_CTRL	Встановіть злиття горизонтального положення, вертикального положення за допомогою візій, злиття швидкості та злиття курсу відповідно до вашої бажаної моделі злиття.
EKF2_HGT_REF	Встановіть на Vision для використання візії як джерела посилання для оцінки висоти.
EKF2_EV_DELAY	Встановіть різницю між міткою часу вимірювання та "фактичним" часом захоплення. Для отримання більш детальної інформації див. нижче.
EKF2_EV_POS_X, EKF2_EV_POS_Y, EKF2_EV_POS_Z	Встановіть положення візійного датчика (або маркерів MoCap) відносно тіла робота.

Ви також можете вимкнути GNSS, baro та range finder fusion за допомогою EKF2_GPS_CTRL, EKF2_BARO_CTRL та EKF2_RNG_CTRL, відповідно.

TIP

Перезавантажте контролер польоту, щоб зміни параметрів набули чинності.

Налаштування EKF2_EV_DELAY

EKF2_EV_DELAY - це затримка Vision Position Estimator відносно вимірювань IMU.

Іншими словами, це різниця між міткою часу системи комп'ютерного зору та "фактичним" часом захоплення, який був би зафіксований годинником IMU ("базовим годинником" для EKF2).

Технічно цей параметр можна встановити на 0, якщо між комп'ютерами MoCap і (наприклад) ROS є правильне маркування часу (не тільки час прибуття) і синхронізація часу (наприклад, NTP). Насправді це потребує деякого емпіричного налаштування, оскільки затримки в усьому ланцюжку MoCap->PX4 дуже залежать від налаштувань. Рідко коли система налаштована з повністю синхронізованим ланцюжком!

Приблизну оцінку затримки можна отримати з логів, перевіривши зсув між частотами IMU та EV. Щоб увімкнути реєстрацію швидкості EV, встановіть біт 7 (Комп'ютерний зір та уникнення) у SDLOG_PROFILE.

Графік зовнішніх даних проти вбудованої оцінки (як вище) може бути створений за допомогою FlightPlot або подібних засобів аналізу польоту. На момент написання статті (липень 2021 року) ні Flight Review, ні MAVGCL не підтримують цю функцію.

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

LPE Конфігурація/Налаштування

Спочатку вам потрібно перейти до оцінювача LPE, встановивши наступні параметри: LPE_EN (1), EKF2_EN (0), ATT_EN (0).

Якщо ви використовуєте обладнання px4_fmu-v2, вам також потрібно використовувати версію прошивки, яка містить модуль LPE (прошивка для іншого обладнання серії FMU містить як LPE, так і EKF). Версію LPE можна знайти у zip-файлі для кожного випуску PX4 або зібрати з вихідного коду за допомогою команди збірки make px4_fmu-v2_lpe. Дивіться Створення коду для більш детальної інформації.

Увімкнення зовнішнього введення позиції

Для використання зовнішньої інформації про місцезнаходження з LPE потрібно встановити такі параметри (їх можна встановити у QGroundControl > Vehicle Setup > Parameters > Local Position Estimator).

Parameter	Налаштування для Зовнішньої Оцінки Положення
LPE_FUSION	Інтеграція зору увімкнена, якщо встановлено прапорець fuse vision position (за замовчуванням увімкнено).
ATT_EXT_HDG_M	Встановіть значення 1 або 2, щоб увімкнути інтеграцію зовнішніх заголовків. Встановлення значення 1 призведе до використання зору, тоді як 2 увімкне використання заголовків MoCap.

Вимкнення Barometer Fusion

Якщо високоточна висота вже доступна з інформації VIO або MoCap, може бути корисно вимкнути корекцію баро в LPE, щоб зменшити дрейф по осі Z.

Це можна зробити у QGroundControl, знявши позначку з опції fuse baro у параметрі LPE_FUSION.

Параметри налаштування шуму

Якщо дані вашого комп'ютерного зору або MoCap є дуже точними, і ви просто хочете, щоб оцінювач чітко відстежував їх, вам слід зменшити параметри стандартного відхилення: LPE_VIS_XY і LPE_VIS_Z (для VIO) або LPE_VIC_P (для MoCap). Зменшення їх призведе до того, що оцінювач буде більше довіряти вхідній оцінці положення. Можливо, вам доведеться встановити їх нижче допустимого мінімуму та ввімкнути примусове збереження.

TIP

Якщо продуктивність все ще низька, спробуйте збільшити параметр LPE_PN_V. Це змусить оцінювача більше довіряти вимірюванням під час оцінювання швидкості.

Увімкнення автоматичних режимів з локальним розташуванням

Всі автоматичні режими польоту PX4 (такі як Mission, Return, Land, Hold, Orbit)) вимагають global оцінки положення, яка зазвичай надходить від системи GPS/GNSS.

Системи, які мають лише local оцінку положення (від MOCAP, VIO або подібних), можуть використовувати повідомлення SET_GPS_GLOBAL_ORIGIN MAVLink, щоб встановити початок координат EKF на певне глобальне місцезнаходження. Після цього EKF надасть оцінку глобального положення на основі походження та локального положення у просторі.

Це може бути використано при плануванні та виконанні місій у приміщенні, для встановлення місцевої точки повернення тощо.

Робота з ROS

ROS не є обов'язковим для надання зовнішньої інформації про позицію, але настійно рекомендується, оскільки він вже має хорошу інтеграцію з системами VIO та MoCap. PX4 вже мають бути налаштовані як вище.

Отримання даних про позицію в ROS

Системи VIO та MoCap мають різні способи отримання даних про положення, а також власні налаштування та теми.

Налаштування для конкретних систем висвітлені нижче. Для інших систем зверніться до документації з налаштування виробника.

Передача даних про позицію до PX4

MAVROS має плагіни для передачі візуальної оцінки з системи VIO або MoCap за допомогою наступних пайплайнів:

ROS	MAVLink	uORB
/mavros/vision_pose/pose	VISION_POSITION_ESTIMATE	vehicle_visual_odometry
/mavros/odometry/out (frame_id = odom, child_frame_id = base_link)	ODOMETRY (frame_id = MAV_FRAME_LOCAL_FRD)	vehicle_visual_odometry
/mavros/mocap/pose	ATT_POS_MOCAP	vehicle_mocap_odometry
/mavros/odometry/out (frame_id = odom, child_frame_id = base_link)	ODOMETRY (frame_id = MAV_FRAME_LOCAL_FRD)	vehicle_mocap_odometry

Ви можете використовувати будь-який з наведених вище пайплайнів за допомогою LPE.

Якщо ви працюєте з EKF2, підтримуються лише "vision" пайплайни. Щоб використовувати дані MoCap з EKF2, вам потрібно remap позицію теми, яку ви отримали з MoCap:

• Теми MoCap ROS типу geometry_msgs/PoseStamped або geometry_msgs/PoseWithCovarianceStamped має бути змінено на /mavros/vision_pose/pose. Тема geometry_msgs/PoseStamped є найпоширенішою, оскільки MoCap зазвичай не має пов'язаних з даними коваріацій.
• Якщо ви отримуєте дані через ROS-повідомлення nav_msgs/Odometry, вам потрібно перевести його на /mavros/odometry/out, переконавшись, що ви оновили frame_id та child_frame_id відповідним чином.
• Фрейми одометрії frame_id = odom, child_frame_id = base_link можуть бути змінені шляхом оновлення файлу у mavros/launch/px4_config.yaml. Однак поточна версія mavros (1.3.0) повинна мати можливість використовувати дерево tf для пошуку трансформації з frame_id до жорстко заданого кадру odom_ned. Те ж саме стосується child_frame_id, яке потрібно підключити в дереві tf до жорстко заданої рамки base_link_frd. Якщо ви використовуєте mavros 1.2.0 і не оновили файл mavros/launch/px4_config.yaml, то ви можете безпечно використовувати кадри одометрії frame_id = odom, child_frame_id = base_link без особливих турбот.
• Зверніть увагу, що якщо ви надсилаєте дані одометрії до px4, використовуючи child_frame_id = base_link, то вам потрібно переконатися, що частина twist повідомлення nav_msgs/Odometry виражена в тілі, а не в інерційній системі координат!!!!!.

Референсні системи координат та ROS

Локальна/світова та світова системи координат, що використовуються в ROS та PX4, відрізняються.

Frame	PX4	ROS
Body	FRD (X Forward, Y Right, Z Down)	FLU (X Forward, Y Left, Z Up), зазвичай називається base_link
World	FRD або NED (X North, Y East, Z Down)	FLU або ENU (X East, Y North, Z Up), з іменуванням odom або map

TIP

Дивіться REP105: Системи координат для мобільних платформ для отримання додаткової інформації про системи координат ROS.

Обидві системи координат показані на зображенні нижче (FRD зліва / FLU справа).

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

При створенні жорсткого тіла в програмному забезпеченні MoCap не забудьте спочатку вирівняти локальну вісь робота x зі світовою віссю x, інакше оцінка розвороту матиме зміщення. Це може призупинити правильну роботу злиття зовнішньої оцінки позиції. Кут крену повинен дорівнювати нулю, коли тіло та опорна система вирівнюються.

Використовуючи MAVROS, ця операція є простою. ROS використовує фрейми ENU як конвенцію, тому зворотний зв'язок щодо позиції повинен бути наданий в ENU. Якщо у вас є система Optitrack, ви можете використати вузол mocap_optitrack, який транслює позицію об'єкта на тему ROS, що вже є у ENU. За допомогою переналаштування ви можете безпосередньо опублікувати його на mocap_pose_estimate таким, як він є, без будь-яких перетворень, і MAVROS позбудеться перетворень NED.

Плагін MAVROS відомий тим, що спрощує роботу з координатними рамками. Він використовує пакет tf ROS. Ваш зовнішній система позиціонування може мати зовсім іншу конвенцію рамки, яка не відповідає конвенції PX4. Корпусна рама зовнішньої оцінки позиції може залежати від того, як ви встановите корпусну раму в програмному забезпеченні MOCAP або від того, як ви встановите сенсор VIO на дрона. Плагін відомостей MAVROS потребує знання, як дитяча рамка зовнішньої позиції орієнтована відносно рамки тіла FRD або FLU повітряного судна, відомої за допомогою MAVROS. Отже, вам потрібно додати зовнішню позу тіла до дерева tf. Це можна зробити, включивши адаптовану версію наступного рядка до вашого ROS-файлу запуску.

Переконайтеся, що ви змінили значення крену, тангажу та кочення так, що вони належним чином приєднують корпус зовнішньої позиції до base_link або base_link_frd. Подивіться на пакет tf package для отримання додаткової допомоги з приводу того, як вказати трансформацію між кадрами. Ви можете використовувати rviz, щоб перевірити, чи ви правильно прикріпили рамку. Назва external_pose_child_frame повинна збігатися з child_frame_id вашого повідомлення nav_msgs/Odometry. Те ж саме стосується і для опорної рамки зовнішньої позиції. Вам потрібно прикріпити опорний каркас зовнішньої позиції як дитину до рамки odom або odom_frd. Адаптуйте тому відповідно кодовий рядок.

  <node pkg="tf" type="static_transform_publisher" name="tf_odom_externalPoseParentFrame"
        args="0 0 0 <yaw> <pitch> <roll> odom <external_pose_parent_frame> 1000"/>

Якщо опорна рама має вісь z, що вказує вгору, ви можете прикріпити її без будь-якого обертання (yaw=0, pitch=0, roll=0) до рами odom. Назва external_pose_parent_frame повинна збігатися з frame_id повідомлення про відомість.

При використанні плагіну MAVROS odom важливо, щоб жоден інший вузол не публікував трансформацію між зовнішнім посиланням позиції та дочірнім кадром. Це може зламати дерево tf.

Конкретні налаштування системи

OptiTrack MoCap

Наступні кроки пояснюють, як подавати оцінки позиції з системи OptiTrack в PX4. Припускається, що система MoCap налаштована. Дивіться це відео для навчання процесу калібрування.

Кроки у програмному забезпеченні Motive MoCap

• Вирівняйте напрямок вашого робота з system +x-axis
• Визначте жорстке тіло в програмному забезпеченні Motive. Вкажіть роботу ім'я, яке не містить пробілів, наприклад, robot1 замість Rigidbody 1
• Увімкніть трансляцію кадру та потокове відтворення VRPN
• Встановіть вісь Up на ось Z (за замовчуванням - Y)

Отримання даних про позицію в ROS

• Встановіть пакет vrpn_client_ros
• Ви можете отримати позу кожного жорсткого тіла на окрему тему, запустивши

  roslaunch vrpn_client_ros sample.launch server:=<mocap machine ip>

Якщо ви назвали rigidbody як robot1, ви отримаєте тему, схожу на /vrpn_client_node/robot1/pose

Передача / перенаправлення даних про позу

MAVROS надає плагін для передачі даних позиції, опублікованих на /mavros/vision_pose/pose, до PX4. Припускаючи, що MAVROS працює, вам просто потрібно переналаштувати тему позиції, яку ви отримуєте від MoCap /vrpn_client_node/<rigid_body_name>/pose безпосередньо на /mavros/vision_pose/pose. Зверніть увагу, що також є тема mocap, яку надає MAVROS для подачі ATT_POS_MOCAP в PX4, але вона не застосовується до EKF2. Однак, це застосовується з LPE.

Переналаштування тем постави описано вище Передача даних постави в PX4 (/vrpn_client_node/<rigid_body_name>/pose має тип geometry_msgs/PoseStamped).

Припускаючи, що ви налаштували параметри EKF2, як описано вище, PX4 тепер встановлений і об'єднує дані MoCap.

Ви тепер готові перейти до першого політ.

Перший політ

Після налаштування однієї з (специфічних) систем, описаних вище, ви повинні бути готові до тесту. Інструкції нижче показують, як це зробити для систем MoCap та VIO

Перевірте зовнішню оцінку

Перед першим польотом обов'язково виконайте наступні перевірки:

• Встановіть параметр PX4 MAV_ODOM_LP на 1. Після цього PX4 передасть отриману зовнішню позицію назад у вигляді повідомлень MAVLink ODOMETRY.
• Ви можете перевірити ці повідомлення MAVLink за допомогою QGroundControl Інспектора MAVLink Для цього поверніть транспортний засіб, поки кватерніон повідомлення ODOMETRY дуже близький до одиничного кватерніону. (w=1, x=y=z=0)
• На цьому етапі корпус виробу зорієнтований у відповідності з ориєнтацією відносно зовнішньої системи координат. Якщо вам не вдається отримати кватерніон, близький до одиничного, без обертання або нахилу вашого літака, це, ймовірно, означає, що ваша рама все ще має зміщення нахилу або кочування. У цьому випадку не продовжуйте і перевірте знову свої координатні рамки.
• Після зорієнтування ви можете підняти літак з землі, і ви маєте бачити, як координата z позиції зменшується. Переміщення засобу в напрямку вперед повинно збільшити координату X. Під час руху транспортного засобу вправо слід збільшувати координату y. У разі, якщо ви також надсилаєте лінійні швидкості зовнішньої системи позиціонування, вам також слід перевірити лінійні швидкості. Перевірте, що лінійні швидкості виражені в описаній відносно корпусу FRD відліковій системі.
• Встановіть параметр PX4 MAV_ODOM_LP на 0. PX4 припинить передавати це повідомлення назад.

Якщо ці кроки є послідовними, ви можете спробувати свій перший польот.

Покладіть робота на землю і почніть передавати зворотний зв'язок MoCap. Потягніть палицю газу вниз і зберметизуйте двигуни.

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

Помістіть лівий джойстик в середину, це зона мертвої зони. З цим значенням палиці робот підтримує свою висоту; підняття палиці збільшить висоту посилки, тоді як зниження значення зменшить її. Те ж саме для правої палиці по x та y.

Збільште значення лівої палиці, і робот злетить, поверніть його назад у середину праворуч після цього. Перевірте, чи він може утримати своє положення.

Якщо це працює, ви можете налаштувати експеримент поза бортом, відправивши позиційний вказівник з віддаленої земної станції.