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


26

Моя команда и я устанавливаем уличного робота, который имеет кодеры, коммерческий 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 чисел в один и тот же фильтр или они обычно хранятся отдельно? Уже есть хорошо разработанный способ решения этой проблемы?

Ответы:


32

Две вещи.

  1. Если вы планируете выполнять сопоставление, вам нужен полноценный алгоритм одновременной локализации и сопоставления (SLAM). См .: Одновременная локализация и картирование (SLAM): Часть I Основные алгоритмы . В SLAM оценка состояния робота - это только половина проблемы. Как это сделать, это больший вопрос, чем здесь можно ответить.

  2. Что касается локализации (оценки состояния робота), это не работа для фильтра Калмана. Переход от к x ( t + 1 )Икс(T)знак равно[Икс,Y,Икс˙,Y˙,θ,θ˙]Икс(T+1)не является линейной функцией из-за угловых ускорений и скоростей. Поэтому вам нужно учитывать нелинейные оценки для этой задачи. Да, есть стандартные способы сделать это. Да, они доступны в литературе. Да, обычно все входы помещаются в один и тот же фильтр. Положение, скорость, ориентация и угловая скорость робота используются в качестве выходных данных. И да, я представлю краткое введение в их общие темы здесь. Основные блюда на вынос

    1. включите смещение гироскопа и IMU в вашем штате, иначе ваши оценки будут расходиться
    2. Расширенный фильтр Калмана (EKF) обычно используется для этой задачи
    3. Реализации могут быть получены с нуля, и обычно их не нужно искать.
    4. Реализации существуют для большинства проблем локализации и SLAM, поэтому не выполняйте больше работы, чем нужно. См .: Робот Операционная Система ROS

Теперь, чтобы объяснить EKF в контексте вашей системы. У нас есть IMU + гироскоп, GPS и одометрия. Робот, о котором идет речь, является дифференциальным приводом, как уже упоминалось. Фильтрации задача состоит в том, чтобы принять текущую оценку позу робота х т , управляющих входов у т и измерений от каждого датчика, г т , и произвести оценку на следующем временном шаге х т + 1 . Мы назовем измерения IMU I t , GPS - это G t , а одометрия - O t .Икс^TUTZTИкс^T+1яTгTОT

Я предполагаю , что мы заинтересованы в оценке робота позу , как . Проблема с IMU и гироскопами - дрейф. Существует нестационарное смещение ускорений, которое вы должны учитывать в EKF. Это делается (обычно) путем перевода смещения в расчетное состояние. Это позволяет напрямую оценивать смещение на каждом временном шаге. x t = x , y , ˙ x , ˙ y , θ , ˙ θ , bИксTзнак равноИкс,Y,Икс˙,Y˙,θ,θ˙ИксTзнак равноИкс,Y,Икс˙,Y˙,θ,θ˙,бдля вектора смещений .б

Я предполагаю, что:

  1. = два измерения расстояния, представляющие расстояние, пройденное протекторами с небольшим приращением времениОT
  2. = три измерения ориентации α , β , θ и три измерения ускорения ¨ x , ¨ y , ¨ z .яTα,β,θИкс¨,Y¨,Z¨
  3. = положение робота вглобальномфрейме, G x t , G y t .гTгИксT,гYT

Как правило, результат управляющих входов (желаемых скоростей для каждого протектора) сложно сопоставить с выходами (изменение позы робота). Вместо , обычно (см Thrun , одометрии вопрос ) использовать одометрии как «результат» контроля. Это предположение хорошо работает, когда вы не находитесь на поверхности, почти свободной от трения. Как мы увидим, IMU и GPS могут помочь исправить проскальзывание.U

Таким образом, первая задача состоит в том, чтобы предсказать следующее состояние из текущего . В случае робота с дифференциальным приводом это предсказание может быть получено непосредственно из литературы (см. «Кинематика колесных мобильных роботов» или более краткая трактовка в любом современном учебнике по робототехнике) или получено с нуля, как показано здесь: Вопрос по одометрии .Икс^T+1знак равное(Икс^T,UT)

Таким образом, теперь мы можем предсказать х т + 1 = F ( х т , О т ) . Это этап распространения или прогнозирования. Вы можете управлять роботом, просто размножаясь. Если значения O т являются абсолютно точными, вы никогда не будете иметь оценку х , не в точности равно ваше истинное состояние. Это никогда не происходит на практике.Икс^T+1знак равное(Икс^T,ОT)ОTИкс^

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

пTеTпTUT2×20,1×я2×2е FИксзнак равноеИксFUзнак равноеU

пT+1знак равноFИкс*пT*FИксT+FU*UT*FUT

яTгT

часг()чася()рргрячас

sZs

vsзнак равноZs-часs(Икс^T+1)
Ssзнак равноЧАСs*пT+1*ЧАСsT+рs
Кзнак равнопT+1*ЧАСsTSs-1
Икс^T+1знак равноИкс^T+1-К*v
пT+1знак равно(я-К*ЧАСs)*пT+1

Zгзнак равночасг()ЧАСгрг

Zязнак равночася()пяряпя, Включение обновления для предвзятости является более сложным, и из моего опыта. Однако, поскольку вы заинтересованы в плоском движении, вы, вероятно, можете упростить задачу. Вам придется искать в литературе для этого.

Некоторые необычные ссылки:

  1. Повышение точности визуально-инерциальной одометрии на основе EKF

  2. Согласованные оценки EKF на основе наблюдаемости для совместной работы нескольких роботов

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

Это поле достаточно развито, чтобы Google (ученый) мог найти вам работающую реализацию. Если вы собираетесь много работать в этой области, я рекомендую вам взять солидный учебник. Может быть, что-то вроде вероятностной робототехники С. Труна из Google Car Fame . (Я нашел это полезным справочником для тех ночных реализаций).

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


(1) Я не вижу «очевидной» связи с фильтрами частиц. (2) Если есть другие вопросы / темы, которые обсуждают что-то похожее на мой вопрос, пожалуйста, покажите ссылку на них. (3) Я понимаю суть EKF и определенно переключусь на использование одного ... Если бы я действительно знал о переходе состояния в первую очередь (это большая часть моего вопроса). (4) Идея улучшения оценки состояния с помощью камер и лидаров крута в абстрактном смысле, но она выходит за рамки того, что мне нужно. Спасибо за ссылки, хотя.
Робз

Фильтр частиц является нелинейной оценкой. Я скоро обновлю ссылки / ссылки. Переходы между состояниями для IMU, гироскопа и одометрии широко освещены в литературе (включая ссылку 1). Опять же, я скоро обновлю несколько ссылок.
Джош Вандер Хук

@Robz Массивно отредактированный ОП. Не уверен в стандартной практике реагирования на комментарии, поэтому я добавил в пост как можно больше информации.
Джош Вандер Хук

7

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

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

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

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


Таким образом, вы включили в свое состояние EKF оценку смещения заголовка? Из любопытства, насколько хорошо это сработало?
Робз
Используя наш сайт, вы подтверждаете, что прочитали и поняли нашу Политику в отношении файлов cookie и Политику конфиденциальности.
Licensed under cc by-sa 3.0 with attribution required.