Як сплавити лінійні та кутові дані від датчиків

Ми з моєю командою встановлюємо робот на відкритому повітрі, який має кодери, IMU комерційного рівня та датчик GPS . Робот має основний привід цистерни, тому кодери достатньо подають кліщі з лівого та правого коліс. IMU дає повороти, крок, похил та лінійні прискорення у x, y та z. Пізніше ми могли б додати інші IMU, які б привели до надмірності, але також могли б також забезпечити кутові швидкості кочення, нахилу та проколювання. GPS публікує глобальні координати x, y та z.

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

Я намагаюся розібратися, як з'єднати всі ці цифри разом таким чином, щоб оптимально скористатися точністю всіх датчиків. Зараз ми використовуємо фільтр Калмана, щоб генерувати оцінку за [x, x-vel, x-accel, y, y-vel, y-accel]допомогою простої матриці переходу:

[[1, dt, .5*dt*dt, 0,  0,        0],
 [0,  1,       dt, 0,  0,        0],
 [0,  0,        1, 0,  0,        0],
 [0,  0,        0, 1, dt, .5*dt*dt],
 [0,  0,        0, 0,  1,       dt],
 [0,  0,        0, 0,  0,        1]]

Фільтр оцінює стан виключно на основі прискорень, наданих ВМУ. (IMU не найкращої якості; приблизно через 30 секунд він покаже робота (в спокої), який дрейфує за 20 метрів від свого початкового місця.) IMU та потенційно швидкість кочення, нахилу та похитування, кодування даних із коліс та даних GPS для покращення оцінки стану.

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

Мені здається, що тут є два окремих набори даних, які важко сплавити:

1. Оцінки x, x-vel, x-accel, y, y-vel, y-accel
2. Оцінки нахилу, нахилу, просіву та швидкості нахилу, нахилу та прокатки

Незважаючи на те, що між цими двома наборами є кросовер, у мене виникають труднощі міркувати про те, як їх скласти. Наприклад, якщо робот рухається з постійною швидкістю, напрямок робота, визначений його x-vel і y-vel, буде таким же, як його позіхання. Хоча, якщо робот знаходиться в стані спокою, похит не може бути точно визначений за швидкостями x та y. Крім того, дані, що надаються кодерами, перекладені на кутову швидкість, можуть бути оновленням швидкості проникнення ... але яким чином оновлення швидкості пониження може закінчитися, щоб забезпечити кращі позиційні оцінки?

Чи є сенс ставити всі 12 номерів в один і той же фільтр, або вони зазвичай тримаються окремо? Чи існує вже добре розроблений спосіб вирішення цього типу проблем?

Дві речі.

1. Якщо ви плануєте робити карти, вам потрібен повноцінний алгоритм одночасної локалізації та картографування (SLAM). Див.: Одночасна локалізація та картографування (SLAM): частина I Основні алгоритми . У SLAM оцінка стану робота - це лише половина проблеми. Як це зробити - питання більші, ніж можна відповісти тут.

2. Що стосується локалізації (оцінка стану робота), це не фільтр для Kalman Filter. Перехід від до x ( t + 1 $x(t)=[x,y,\dot{x},\dot{y},\theta,\dot{\theta}]$$x(t+1)$не є лінійною функцією внаслідок кутових прискорень та швидкостей. Тому для цього завдання потрібно розглянути нелінійні оцінки. Так, є стандартні способи зробити це. Так, вони доступні в літературі. Так, зазвичай всі входи складаються в один і той же фільтр. Положення, швидкість, орієнтація та кутова швидкість роботи використовуються як вихідні. І так, я наведу тут короткий вступ до їх загальних тем. Основні прийоми є

   1. включіть упередження щодо гіроскопа та ВНУ у вашій державі, або ваші оцінки розходяться
   2. Для цієї проблеми зазвичай використовується розширений фільтр Kalman (EKF)
   3. Реалізація може бути отримана з нуля, і їх взагалі не потрібно "шукати".
   4. Реалізатори існують для більшості проблем локалізації та SLAM, тому не робіть більше роботи, ніж вам доведеться. Див.: Робоча операційна система ROS

Тепер, щоб пояснити EKF в контексті вашої системи. У нас є IMU + гіроскоп, GPS та одометрія. Робот, про який йде мова, є диференційованим приводом, як згадувалося. Фільтрації завдання полягає в тому, щоб прийняти поточну оцінку позу робота х т , керуючих входів у т і вимірювань від кожного датчика, г т , і зробити оцінку на наступному часовому кроці х т + 1 . Ми будемо називати вимірювання IMU I t , GPS - G t , а одометрія - O t .${\hat{x}_t}$$u_t$$z_t$$\hat{x}_{t+1}$$I_t$$G_t$$O_t$

Я припускаю, що ми зацікавлені в оцінці пози робота як . Проблема з IMU та Gyros - це дрейф. Існує нестаціонарний ухил у прискореннях, які необхідно враховувати в EKF. Це робиться (як правило) шляхом встановлення зміщення в оціночний стан. Це дозволяє безпосередньо оцінювати зміщення на кожному етапі часу. x t = x , y , ˙ x , ˙ y , θ , ˙ θ , $x_t={x,y,\dot{x},\dot{y},\theta,\dot{\theta}}$$x_t={x,y,\dot{x},\dot{y},\theta,\dot{\theta},b}$, для вектора зміщення .$b$

Я припускаю:

1. = два вимірювання відстані, що представляють відстань, яку пройдено протекторами за невеликий приріст часу$O_t$
2. = три вимірювання орієнтації α , β , θ і три вимірювання акселерації ¨ x , ¨ y , ¨ z .$I_t$${\alpha, \beta, \theta}$${\ddot{x},\ddot{y},\ddot{z}}$
3. = положення робота вглобальномукадрі, G x t , G y t .$G_t$$^Gx_t,^Gy_t$

Як правило, результат керуючих входів (бажані швидкості для кожного протектора) важко відобразити на виходи (зміна пози робота). Замість прийнято використовувати одометрію як "результат" контролю (див. Питання Thrun , Одометрія ). Це припущення добре працює, коли ви не знаходитесь на поверхні, що майже не має тертя. IMU та GPS можуть допомогти виправити ковзання, як ми побачимо.$u$

Таким чином, перше завдання полягає в тому, щоб передбачити наступний стан з поточного . Що стосується робота з диференційованим приводом, це передбачення можна отримати безпосередньо з літератури (див. «Кінематика мобільних роботів на колесах» або більш стисле звернення в будь-якому сучасному навчальному посібнику з робототехніки), або вивести з нуля, як показано тут: Питання з одометрії .$\hat{x}_{t+1} = f(\hat{x}_t,u_t)$

Таким чином, тепер ми можемо передбачити х т + 1 = F ( х т , Про т ) . Це етап поширення чи прогнозування. Ви можете управляти роботом, просто розповсюджується. Якщо значення O т є абсолютно точними, ви ніколи не будете мати оцінку х , що не в точності так само ваше справжнє стан. На практиці цього ніколи не відбувається.$\hat{x}_{t+1} = f(\hat{x}_t, O_t)$$O_t$$\hat{x}$

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

$P_t$$f$$t$$P_t$$U_t$$2\times2$$.1\times I_{2\times 2}$$f$ $F_x=\frac{\partial f}{\partial x}$$F_u=\frac{\partial f}{\partial u}$

$I_t$$G_t$

$h_g()$$h_i()$$R$$R_g$$R_i$$h$

$s$$z_s$

$z_g=h_g()$$H_g$$R_g$

$z_i=h_i()$$p_i$$R_i$$p_i$. Включити оновлення для зміщення складніше, і я не знаю. Однак, оскільки вас цікавить планарний рух, ви, ймовірно, можете спростити проблему. Для цього вам доведеться шукати літературу.

Деякі посилання на голову:

1. Підвищення точності візуально-інерціальної одометрії на основі EKF

2. Послідовні оцінники EKF на основі спостереження для локалізації спільної роботи з кількома роботами

3. Адаптивний двоступінчастий EKF для системи INS-GPS із слабким поєднанням із невідомими зміщеннями несправності

Це поле є достатньо зрілим, що Google (вчений), ймовірно, може знайти вам робочу реалізацію. Якщо ви збираєтеся зробити багато роботи в цій галузі, рекомендую вам підібрати ґрунтовний підручник. Можливо, щось на кшталт ймовірнісної робототехніки С. Thrun із Google Car Fame. (Я вважаю це корисним посиланням для тих впроваджень пізньої ночі).

* У операційній системі роботів (ROS) доступні кілька оцінок на основі ПФ . Однак вони були оптимізовані для використання в приміщенні. Фільтри частинок мають справу з мультимодальними PDF-файлами, які можуть бути результатом локалізації на основі карт (я біля цієї двері чи тієї двері). Я вважаю, що більшість зовнішніх реалізацій (особливо тих, які можуть використовувати GPS принаймні з перервами) використовують розширений фільтр Kalman (EKF). Я успішно використовував розширений фільтр Kalman для зовнішнього наземного мотора з диференціальним приводом.

Ви можете значно спростити проблему в більшості випадків:

• Багато IMus комерційного рівня (наприклад, Xsens) мають дуже шумні акселерометри. Навіть не турбуйтеся, збиваючи їх, щоб отримати швидкість, одометрія вже на порядок краще. Єдині корисні дані, які збирається надати IMU, - це крок та прокрутка, а певною мірою заголовок (див. Наступний пункт)
• Заголовок від IMU не є таким надійним. Він використовує магетометри, і буде показувати величезні заноси (до 25 градусів понад 2 м у нашому випадку) поблизу феромагнітних мас, таких як той, який ви можете знайти в будівництві стін. Що ми зробили для вирішення цього питання - використовувати заголовок IMU, але оцінити зміщення заголовка.
• Якщо ви перебуваєте на відкритому повітрі, не забувайте, що подорож на 10 м на нахилі 10 градусів не призводить до такої ж зміни в X та Y, як подорож на 10 м по рівній місцевості. Зазвичай це враховується шляхом оцінки Z, але я думаю, що його можна оцінити інакше.
• GPS також є брехливою сукою, як правило, у середовищі з високим рівнем руху. Плюс низькосортні (і навіть в деяких умовах повноцінні) GPS мають тенденцію повідомляти про дуже неправильні стандартні відхилення. Ми використовували кілька простих тестів на квадратний чи, щоб перевірити, чи слід інтегрувати конкретний вимірювання GPS (тобто перевірити, чи відповідає він поточній оцінці фільтра до певного моменту), що дало нам гідні результати.

"Типовим" рішенням для нас є використання одометрії + IMU, щоб отримати оцінку его-руху, а потім використовувати GPS для корекції зміщення X, Y, Z та заголовків.

Ось реалізація EKF, яку ми широко використовували. Якщо вам потрібно оцінити орієнтацію IMU (тобто, якщо він ще не має вбудованого фільтра), ви також можете скористатися цими двома фільтрами: UKF та EKF .