Как объединить линейные и угловые данные с датчиков

Моя команда и я устанавливаем уличного робота, который имеет кодеры, коммерческий 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. (IMU не самого лучшего качества; примерно через 30 секунд он покажет, что робот (в состоянии покоя) дрейфует на хорошие 20 метров от своего первоначального местоположения.) Я хочу узнать, как использовать крен, тангаж и рыскание с IMU, и, возможно, скорости крена, тангажа и рыскания, данные кодировщика с колес и данные GPS для улучшения оценки состояния.

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

Мне кажется, что здесь есть два отдельных набора данных, которые трудно объединить:

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. Что касается локализации (оценки состояния робота), это не работа для фильтра Калмана. Переход от к x ( t + 1 $x(t)=[x,y,\dot{x},\dot{y},\theta,\dot{\theta}]$$x(t+1)$не является линейной функцией из-за угловых ускорений и скоростей. Поэтому вам нужно учитывать нелинейные оценки для этой задачи. Да, есть стандартные способы сделать это. Да, они доступны в литературе. Да, обычно все входы помещаются в один и тот же фильтр. Положение, скорость, ориентация и угловая скорость робота используются в качестве выходных данных. И да, я представлю краткое введение в их общие темы здесь. Основные блюда на вынос

   1. включите смещение гироскопа и IMU в вашем штате, иначе ваши оценки будут расходиться
   2. Расширенный фильтр Калмана (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 и гироскопами - дрейф. Существует нестационарное смещение ускорений, которое вы должны учитывать в 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 (ученый) мог найти вам работающую реализацию. Если вы собираетесь много работать в этой области, я рекомендую вам взять солидный учебник. Может быть, что-то вроде вероятностной робототехники С. Труна из Google Car Fame . (Я нашел это полезным справочником для тех ночных реализаций).

* В операционной системе робота (ROS) доступно несколько оценок на основе PF . Тем не менее, они были оптимизированы для использования внутри помещений. Фильтры частиц имеют дело с мультимодальными PDF-файлами, которые могут возникнуть в результате локализации на основе карт (я рядом с этой дверью или той дверью). Я полагаю, что большинство наружных реализаций (особенно те, которые могут использовать GPS, по крайней мере с перерывами) используют Расширенный фильтр Калмана (EKF). Я успешно использовал расширенный фильтр Калмана для наружного наземного вездехода с дифференциальным приводом.

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

• Многие IMus «коммерческого уровня» (например, Xsens) имеют очень шумные акселерометры. Даже не пытайтесь соединить их, чтобы получить скорость, одометрия уже на порядок лучше. Единственные пригодные для использования данные, которые IMU собирается предоставить, - это шаг и крен и, в некоторой степени, заголовок (см. Следующий пункт)
• направление из ИДУ не так уж и заслуживает доверия. Он использует магометры и покажет огромные дрейфы (в нашем случае до 25 градусов более 2 метров) вблизи ферромагнитных масс, таких как тот, который вы можете найти в стенах здания. Чтобы решить эту проблему, мы использовали заголовок IMU, но оценили смещение заголовка.
• Если вы находитесь на улице, не забывайте, что проезд 10 м по наклону 10 градусов не приводит к таким же изменениям по осям X и Y, как перемещение 10 м по ровной местности. Обычно это объясняется оценкой Z, но я думаю, что это можно оценить по-разному.
• GPS также является сукой, как правило, в условиях многолучевого распространения. Плюс низкосортные (и даже в некоторых условиях полноценные) GPS имеют тенденцию сообщать об очень неправильных стандартных отклонениях. Мы использовали несколько простых тестов хи-квадрат, чтобы проверить, нужно ли интегрировать определенное измерение GPS (то есть проверить, соответствует ли оно текущей оценке фильтра до определенной точки), что дало нам приличные результаты.

«Типичным» решением для нас является использование одометрии + IMU для получения оценки эго-движения, а затем использование GPS для коррекции X, Y, Z и смещения курса.

Вот реализация EKF, которую мы широко использовали. Если вам необходимо оценить ориентацию IMU (то есть, если он еще не имеет встроенного фильтра), вы также можете использовать один из этих двух фильтров: UKF и EKF .