Расширенный фильтр Калмана с лазерным сканированием + известная карта

10

В настоящее время я работаю над проектом для школы, где мне нужно реализовать расширенный фильтр Калмана для точечного робота с лазерным сканером. Робот может вращаться с радиусом поворота 0 градусов и двигаться вперед. Все движения кусочно-линейные (движение, вращение, движение).

Симулятор, который мы используем, не поддерживает ускорение, все движения происходят мгновенно.

У нас также есть известная карта (изображение в формате PNG), которую нам нужно локализовать. Мы можем проследить лучи на изображении для имитации лазерного сканирования.

Мы с моим партнером немного смущены тем, какие модели датчиков движения и нам нужно использовать.

До сих пор мы моделируем состояние как вектор .(x,y,θ)

Мы используем уравнения обновления следующим образом

void kalman::predict(const nav_msgs::Odometry msg){
    this->X[0] += linear * dt * cos( X[2] ); //x
    this->X[1] += linear * dt * sin( X[2] ); //y
    this->X[2] += angular * dt; //theta

    this->F(0,2) = -linear * dt * sin( X[2] ); //t+1 ?
    this->F(1,2) =  linear * dt * cos( X[2] ); //t+1 ?

    P = F * P * F.t() + Q;

    this->linear = msg.twist.twist.linear.x;
    this->angular = msg.twist.twist.angular.z;
    return;
}

Мы думали, что у нас все работает, пока не заметили, что мы забыли инициализировать Pи что он равен нулю, то есть исправление не происходит. По-видимому, наше распространение было очень точным, так как мы еще не вносили шум в систему.

Для модели движения мы используем следующую матрицу для F:

F=[10−v∗Δt∗sin(θ)01v∗Δt∗cos(θ)001]

Как якобиан наших обновлений формул. Это правильно?

xyθ

Другая проблема, связанная с инициализацией P. Мы попробовали 1,10,100, и все они размещают робота за пределами карты (-90, -70), когда карта только 50x50.

Код для нашего проекта можно найти здесь: https://github.com/en4bz/kalman/blob/master/src/kalman.cpp

Любые советы высоко ценится.

РЕДАКТИРОВАТЬ:

В этот момент я получил фильтр для стабилизации с базовым шумом движения, но без фактического движения. Как только робот начинает двигаться, фильтр довольно быстро расходится и выходит из карты.

mobile-robot ros kalman-filter ekf en4bz
источник

Ответы:

3
  • Использование EKF для локализации, основанной на лазерном сканировании и известной карте, является плохой идеей и не будет работать, потому что одно из основных предположений EKF недопустимо: модель измерения не дифференцируема.

Я бы посоветовал изучить локализацию Монте-Карло (Фильтры частиц). Это не только решит проблему с вашей моделью измерения, но также позволит глобальную локализацию (начальная позиция на карте совершенно неизвестна).

Изменить: извините, я забыл упомянуть еще один важный момент:

  • zt=h(xt)+N(0,Qt)h(xt) часть и добавление гауссовского шума.
sebsch
источник
^^ «Использование EKF для локализации на основе лазерного сканирования и известной карты - плохая идея», кто это сказал? Пожалуйста, предоставьте ссылки. EKF является наиболее успешным оценщиком, и многие статьи предлагают использовать EKF для решения проблемы локализации. На самом деле, я удивлен, что вы говорите. Основными предположениями EKF являются модели движения и наблюдения, линейные, а шум гауссовский. Я не знаю, что вы имеете в виду под "моделью измерения не дифференцируемо".
CroCo
В оригинальном плакате упоминается трассировка лучей, что означает, что он намеревается использовать модель измерения, аналогичную «лучевой модели дальномеров» в вероятностной робототехнике. Эта измерительная модель демонстрирует скачки (представьте, что лазерный луч едва попадает в препятствие и пропускает его: требуется всего лишь небольшое вращение для прыжка, который не
различим
0

Прежде всего, вы ничего не упомянули о том, какую локализацию вы используете. Это с известными или неизвестными соответствиями?

Теперь, чтобы приобрести якобиана в Matlab, вы можете сделать следующее 

>> syms x y a Vtran Vrotat t
>> f1 = x + Vtran*t*cos(a);
>> f2 = y + Vtran*t*sin(a);
>> f3 = a + Vrotat*t;
>> input  = [x y a];
>> output = [f1 f2 f3];
>> F = jacobian(output, input)

Результат 

F =
[ 1, 0, -Vtran*t*sin(a)]
[ 0, 1,  Vtran*t*cos(a)]
[ 0, 0,               1]

Расширенный фильтр Калмана требует, чтобы система была линейной, а шум - гауссовским. Система здесь означает, что модели движения и наблюдения должны быть линейными. Лазерные датчики дают диапазон и угол к ориентиру от рамы робота, поэтому модель измерения не является линейной. О P, если вы предполагаете, что он большой, тогда ваш EKF-оценщик будет плохим в начале, и он будет опираться на измерения, потому что предыдущий вектор состояния недоступен. Однако, как только ваш робот начнет двигаться и чувствовать, EKF станет лучше, потому что он использует модели движения и измерения для оценки позы робота. Если ваш робот не обнаруживает какие-либо ориентиры, неопределенность резко возрастает. Кроме того, вы должны быть осторожны с углом. В C ++cos and sinугол должен быть в радианах. В вашем коде нет ничего об этой проблеме. Если при расчете в радианах вы принимаете шум в градусах в градусах, вы можете получить странные результаты, поскольку один градус в виде шума в то время как вычисления в радианах считаются огромными.

крокодиловый
источник