Диссертация (1090939), страница 3

Просмтор этого файла доступен только зарегистрированным пользователям. Но у нас супер быстрая регистрация: достаточно только электронной почты!

Текст из файла (страница 3)

Данные алгоритмы предназначены для рекурсивного дооцениваниявектора состояния априорно известной динамической системы. Такимобразом, для расчёта текущего состояния системы необходимо знать текущееизмерение, а также предыдущее состояние самого фильтра. Фильтр Калмана,подобно другим рекурсивным фильтрам, реализован не в частотном, а вовременном представлении, но также, в отличие от других подобных решений,фильтр Калмана оперирует не только оценками состояния, но еще иоценками плотности распределения вектора состояния, опираясь на формулуБайеса условной вероятности. Состояние фильтра находится в двухпеременных: Xk – оценка вектора состояния динамической системы в моментвремени k; Pk – ковариационная матрица ошибок – мера точности оцениваниявектора состояния в момент времени k.

Расширенный фильтр Калманаобладает схожими с простым фильтром Калмана особенностями, за темисключением, что он может быть использован в нелинейных процессах. Наданный момент это один из наиболее распространенных методов решениязадачи одновременной локализации и построения карты пространства [38].Он позволяет не только уточнять оценку положения робота на карте, но иположение всех обнаруженных ориентиров (ключевых особенностей).Обычно процесс оценки состояния системы, в контексте рассматриваемойзадачи, разбивают на три этапа:1.

обновление оценки состояния системы на основе данных одометрии;2. обновлениеоценкисостояниясистемыобнаруженных ориентиров;3. добавление новых ориентиров в систему.15наосновеповторноОсновным недостатком решения на основе расширенного фильтраКалмана является серьезная зависимость вычислительной сложностиалгоритма от количества рассматриваемых ориентиров. Это вызвано тем, чтоматрица ковариации фильтра  имеет размерность n n , где n – количествоориентиров. На каждом этапе обновления матрицы  , должен быть обновленкаждый ее элемент, в связи с чем сложность алгоритма составляет O(n 2 ) .Таким образом, расширенный фильтр Калмана наиболее применим вситуации, когда среда имеет малое количество устойчиво отслеживаемыхориентиров, которое в большинстве практических случаев составляетнесколько сотен элементов.Состояние мобильного робота в произвольный момент времени можнопредставить с помощью вектора оценки его текущего местоположения Xp иковариационной матрицы Pp.

Эти величины имеют следующий вид: xx2  xy2  x2 X p = [ x p , y p , p ]T , Pp =  yx2  yy2  y2 . 2x  2y  2 Коэффициенты ковариационной матрицы  p характеризуют степеньлинейной зависимости координат положений камеры друг от друга.Диагональные элементы представляют собой среднеквадратическую ошибкуоценкисоответствующейдиагональнымэлементамкоординаты.Приприсваиваютсяинициализациизначенияпосистемыумолчанию,отражающие неопределенность начального положения.

Как бы ни былаточна информационно-измерительная система, имеет смысл умышленнозадать ошибку оценки начального положения отличной от нуля. Иначе, взависимости от реализации фильтра на вычислительной машине, нулевыезначения на диагональных элементах могут привести к ошибке привычислении обратной матрицы.Положения ориентиров можно представить в виде вектора оценки ихкоординат X o и ковариационной матрицы Po :16 x21x1 2X 0 = [ x1 , y1 , x2 , y2 ...xn , yn ]T , Pp =  y1x1 ... x21y1 ... y21y1 ...,......где n – количество ориентиров, обнаруженных роботом, x i , y i – оценкакоординат i -го ориентира.

По аналогии с матрицей  p матрица  o призванаотражать степень зависимости оценки координат ориентиров друг от друга.Общее состояние системы характеризуется вектором X , отражающимоценки положения робота и ориентиров, и ковариационной матрицей  ,имеющими вид: PpX = [ X Tp X 0T ]T , P0 =  T Pp 0Pp 0 .P0 В матрице Рp значения диагональных элементов устанавливаютсяотличнымиотнуля,таккакониотражаютошибкуначальногопозиционирования.Обновление оценки состояния системыНаследующемэтапеработыалгоритма,называемомшагомпредсказания, оценка состояния системы xk обновляется на основе состояниясистемы в предыдущий момент времени xk-1, модели движения иодометрических данных.

Сигнал управления, используемый для расчетаоценки нового положения робота, а также состояние всей системы можнопредставить в виде матриц:u = [[x, y], ]T ,X k = [ f p ( X Pk 1 , u), X Ok 1 ]T ,g X p  PPk 1  g TX pPk = T (g X p  PPOk 1 )17g X p  PPOk 1 .POk 1Здесь k – оценка состояния системы на текущем шаге, X Pk 1 – оценкасостояния робота на предыдущем шаге, X Ok 1 – оценка состояния ориентировна предыдущем шаге, а f p имеет вид: X P  t  v  cos( P  )k 1 k 1f p ( X Pk 1 , u ) =  YPk 1  t  v  sin ( Pk 1   . Pk 1  )Даннаяфункциявпредставленномвидеописываетмодель,включающую в себя сигналы управления.

Это, как правило, характерно дляробототехнических систем, когда задача одновременной локализации икартографирования решается применительно к мобильному роботу. Дляобщности не будем использовать представленную функцию в приведенномвиде, учитывая зависимость между сигналами управления и изменениемположения. Матрица шума процесса Q определяется следующей формулой:Q = gU U  gUT .Якобианы и матрица ковариации U вычисляются следующим образом:g X p v2=, g u =,U = dx pdu p0df pdf p.sigma 02Обработка повторно обнаруженных ориентировОценка состояния системы, которая была получена на предыдущемэтапе, является неточной вследствие ошибок, вносимых одометрическимиданными.Использованиепространственныхориентировпозволяеткомпенсировать эту неточность, благодаря еще одной дополнительнойоценке, полученной иным, нежели с помощью системы одометрии, путем.Рассогласование между оценками, полученными с помощью разных типовизмерений используется для уточнения параметров системы.

На данномэтапе работы алгоритма этот процесс выполняется итеративно для каждогоориентира.18Наблюдениеi –гоориентира,полученноеотинформационно-измерительной системы, может быть представлено в виде столбца:zi = [r, ]T ,где r – расстояние от камеры до ориентира, а  – угол, под которым виденориентир, отсчитывается от вектора движения против часовой стрелки.Теперь необходимо предсказать вектор наблюдения, основываясь на оценкеположения камеры и координатах i-го ориентира, которые находятся ввекторе состояния системы на текущем шаге: ( X i  X P ) 2  (Yi  YP ) 2 kkkkYik  YPkhi ( X k ) = .arctan()PkX ik  X PkПосле этого можно посчитать рассогласование между наблюдением hiи данными информационно – измерительной системы z i по следующейформуле:vi = z i  hi ( X k ).Величина vi называется инновацией и обладает следующей матрицейковариации:Si = hx  Pk  hxT  R,где R – матрица, характеризующая точность информационно измерительнойсистемы.КоэффициентусиленияКалмана,показывающий,влияниерассогласования между ожидаемым и измеренным наблюдением i –гоориентира на изменение оценки текущего состояния системы, может бытьопределен следующим образом:Wi = Pk  hxT  Si1  R.На основании приведенных заключений можно обновить оценкусостояния системы и ковариационную матрицу состояния по следующимформулам:19X k = X k  Wi  vi ,Pk = Pk  Wi  Si Wi T .Все действия, описанные на данном этапе, производятся для каждогоориентира, обнаруженного повторно.

Таким образом, каждое наблюдениевносит свой вклад в эволюцию состояния системы.По мере движения камеры в пространстве и получения новых данных оместности могут быть обнаружены новые ориентиры, которые необходимодобавить в систему. Прежде всего необходимо добавить в вектор оценкисостояния системы новые элементы – оценку координат обнаруженногоориентира.

Делается это следующим образом:XkXk = ,f(X,z)iPikгде функция f i служит для перевода вектора наблюдения z i в декартовусистему координат. Далее необходимо обновить ковариационную матрицувектора состояния системы. При этом в матрицуPkдобавляютсяподматрицы, отражающие корреляция координат нового ориентира скоординатами робота и всех ориентиров, отслеживаемых в данный момент: PPkTPk =  PPOkg  P X p PkPPOkPOkPPk  g TX pTPPO g TX pkg X p  PPOkg X p  PPk  g TX p. g z  R  g zT Таким образом, процесс одновременной локализации как камеры, так иориентировпространствасостоитвциклическомвоспроизведениирассмотренных этапов обработки ориентиров и обновления состояниясистемы.1.2.2. Решения на основе фильтра частицВ предыдущем разделе, речь в котором шла о применениирасширенного фильтра Калмана для решения задачи локализации робота20картографииместности,былаобозначенаосновнаяпроблема,ограничивающая область применимости данного подхода, а именноневозможность его использования на картах, насчитывающих значительноечисло ориентиров, превышающее несколько сотен.

Для решения этойпроблемы в 2002 году был разработан новый подход к решению задачиодновременной локализации и картографии [50]. Метод FastSLAM разделяетзадачу на множество равнозначных подзадач, используя независимостьсостояния отдельных элементов модели SLAM.Постановка задачиB основе алгоритма FastSLAM лежит применение вероятностноймодели байесовской сети.

Диаграмма данного процесса схематичнопредставлена на рисунке 1.2. Каждое из измерений z1 ..z n является функциейкоординат  i ориентира, а также положения робота в момент произведенияизмерений. Как можно увидеть в представленной диаграмме, задача SLAMподразумевает важное условие независимости измерений друг от друга.Таким образом, данная проблема может быть рассмотрена как n независимыхвычислительных задач по оценке пути движения каждого из ориентиров. Этозамечание подробно рассмотрено в работе [62] для разработки эффективногофильтра частиц.Рис. 1.2. Диаграмма процесса SLAM: движение происходит от положенияx t 1 под управляющим воздействием u t 1 ..u t 1 . Наблюдения ориентира mобозначены как z t 1 , z t t 1 .21Основываясь на приведенном заключении, можно рассматриватьпроблему одновременной локализации мобильного робота и построениякарты пространства как задачу, состоящую из двух частей: оценкитраектории движения робота и оценки положений ориентиров, которые всвою очередь зависят от координат робота на момент каждого измерения.

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

Каждая из частиц характеризуется определенным весом,определяемым состояниями n фильтров Калмана, используемыми для оценкиположений ориентиров. Данный алгоритм является Rao-Blackwellizedфильтром частиц [35, 63]. Наивная реализация рассмотренных идей обладаеталгоритмической сложностью, равной O(MK ) , где M – это количествочастиц, которыми оперирует фильтр, а K– количество ориентиров,используемых в процессе навигации. Использование древовидных структурхранения данных снижает сложность до значения O(MlogK ) , что приводит кзначительному выигрышу в производительности по сравнению с решениямиSLAM, основанными на расширенном фильтре Калмана.Вероятностные свойства задачи SLAMОбратимсяквероятностнойпостановкезадачиодновременнойлокализации и картографии.

Характеристики

Список файлов диссертации