CC BY
61
Рудь М.Н.
Национальный исследовательский Томский политехнический университет, г. Томск, магистрант кафедры интегрированных компьютерных систем управления,
rudmax 13@ gmail . com
Система локализации для автономного мобильного робота с использованием технологии CUDA
КЛЮЧЕВЫЕ СЛОВА
Мобильный робот, фильтр частиц, локализация Монте - Карло, NVIDIA CUDA, GPGPU.
АННОТАЦИЯ
В статье описывается система локализации для автономного мобильного робота, основанная на фильтре частиц (метод Монте-Карло). Выполнение алгоритма было ускорено на графическом процессоре с использованием технологии NVIDIA CUDA. Проведены оптимизация алгоритма для исполнения на GPU и испытания на реальном оборудовании. В качестве бортового вычислительного блока робота используется одноплатный компьютер NVIDIA Jetson TK1, построенный на гибридном процессоре Tegra K1, поддерживающем технологию CUDA.
Введение
Одной из важнейших задач, решаемых автономным мобильным роботом, действующим в заранее известной среде, является определение месторасположения себя в пространстве (локализация). Для решения этой задачи существуют множество алгоритмов, основанных на получении информации с датчиков различного типа. В данной работе используется алгоритм, основанный на фильтре частиц (метод Монте - Карло) [1]. Текущее положение робота в алгоритме определяется исходя из его предыдущего положения, данных сенсоров и команд, поданных на моторы. Этот метод доказал свою эффективность и применимость к нелинейным и негауссовым системам.
Метод Монте-Карло основан на представлении состояния системы в виде системы частиц. Каждая частица - объект, который содержит информацию о вероятном местоположении и направлении движения робота, а также дополнительный параметр, именуемый весом частицы. Частицам предоставляется карта местности в виде сетки (occupancy grid). Каждая ячейка сетки содержит одно из двух значений - 0 или 1, что означает, занята ли ячейка препятствием или она свободна для движения. Каждый цикл получения данных с сенсоров вес частицы вычисляется в зависимости от того, насколько точно показания «виртуальных» сенсоров
частицы совпадают с реальными показаниями сенсоров робота. После этого происходит отбор наиболее сильных частиц. В результате, через определенное количество циклов движения и получения информации с сенсоров, в системе останутся частицы, показавшие наилучшие результаты. Они будут сконцентрированы вокруг реального положения робота и смогут достоверно его определить.
Чтобы получить необходимую точность локализации, необходимо охватить как можно больше вероятных состояний робота, что ведет к увеличению числа частиц в системе. Это делает применение фильтра частиц в реальном времени очень затратным с точки зрения скорости вычислений. Необходимо отметить, что большинство шагов фильтра выполняются независимо для каждой частицы, что позволяет осуществлять параллельную обработку всех частиц.
CUDA (Compute Unified Device Architecture) - программно-аппаратная архитектура параллельных вычислений на графических процессорах (GPU) компании NVIDIA [2]. Это одна из наиболее распространенных технологий для осуществления вычислений общего назначения на GPU.
В данной работе на архитектуру CUDA был перенесен наиболее ресурсоемкий шаг фильтра частиц - вычисление весов, который выполняется независимо для каждой частицы и пригоден для параллельного исполнения. Для повышения наглядности полученных результатов алгоритм был реализован как на многоядерном CPU, так и на устройстве с поддержкой CUDA - мобильной видеокарте GeForce GT 640M. К моменту создания данной статьи алгоритм еще не был протестирован на Jetson TK1.
Статья организована следующим образом. В разделе «Аппаратная часть» приведена краткая информация о конструкции используемого робота, системе сенсоров и бортовом компьютере. В разделе «Алгоритм» подробно описываются все шаги алгоритма локализации для конкретного робота. Раздел «Реализация на CUDA» посвящен реализации алгоритма Монте - Карло на GPU, а также проведенным оптимизациям. Раздел «Результаты» посвящен анализу полученных результатов и дальнейшим направлениям работы.
Аппаратная часть
Конструкция используемого мобильного робота представлена на рисунке 1. Робот имеет два ведущих колеса и одно колесо без привода, установленное в передней части. В качестве основного сенсора используется устройство Microsoft Kinect [3], установленное по направлению движения робота. Kinect предоставляет видеоизображение, комбинированное с соответствующей ему картой глубины, содержащей расстояния до каждой точки обозреваемого пространства. Сенсор имеет разрешение 640 х 480 точек и угол обзора приблизительно 60 градусов. Также для определения направления движения робота используется электронный компас.
Рис. 1. Колесный робот с установленным сенсором Kinect
Все необходимые вычисления на борту выполняет одноплатный компьютер NVIDIA Jetson TK1. Процессор Tegra K1 обладает 192 - ядерным GPU, построенном на архитектуре Kepler, а также 4 - ядерным CPU ARM Cortex A15. Jetson TK1 имеет все необходимые интерфейсы коммуникации, 16 Гб хранилище данных, а также предустановленную операционную систему Linux for Tegra, что дает возможность взаимодействия с сенсором Kinect посредством библиотеки OpenNI [4].
Алгоритм
Алгоритм, используемый в работе, основан на использовании фильтра частиц. Каждая частица в системе представляет собой одно из возможных состояний мобильного робота. Мобильный робот действует в закрытом помещении лаборатории робототехники, что позволяет использовать сенсор Microsoft Kinect, непригодный для открытых пространств. Состояние робота включает три переменных: координаты x, y центра масс робота, а также угол рысканья в. В начальный момент времени значения переменных равны нулю. Для каждого цикла движения и считывания информации с сенсоров состояния обновляются с учетом поданных на моторы команд, показаний электронного компаса и сенсора Kinect. Далее будут описаны основные шаги алгоритма Монте - Карло для осуществления локализации робота.
• Инициализация начальных состояний частиц.
Изначально придаем переменным x, y и в для всех частиц случайные значения.
• Предсказание следующего положения робота.
На данном шаге выполняется чтение показаний электронного компаса и энкодеров двигателей. Показания энкодеров приходят в виде количества градусов, на которые повернулся двигатель. Для того, чтобы предсказать следующее положение робота, необходимо из текущего показания энкодеров вычесть показания энкодеров на предыдущем цикле считывания. Это число градусов должно быть переведено в расстояние, пройденное роботом за это время, с учетом диаметра колес. Наконец, необходимо прибавить полученное число к предыдущей позиции робота. Такой же порядок действий необходимо выполнить и для показаний
электронного компаса. Учитывая наличие шумов в энкодерах и компасе, в модель движения должен быть также добавлен случайный шум. Псевдокод данного шага представлен в алгоритме 1:
Алгоритм 1. Предсказание следующей позиции робота
для всех i от 0 до Ы_частиц:
частица^].х = частица^].х + одометрия.х + шум.х;
частица^].у = частица^].у + одометрия.у + шум.у;
частица^]. = частица^]. д + одометрия. 9 + шум. д ;
конец
• Вычисление весов частиц
Наиболее трудоемкий шаг алгоритма. Как было отмечено ранее, вес частицы зависит от того, насколько точно показания «виртуальных» датчиков частицы совпадают с реальными показаниями сенсора ЮпеЛ. Исходя из этого, необходимо создать некую аппроксимирующую модель сенсора Юпес^ которой могли бы пользоваться частицы для получения своих показаний.
Карта глубины, получаемая сенсором, размера 640 х 480 точек, содержит значения от 0 до 255. 255 - наиболее удаленный объект. Для создания модели сенсора карта глубины разбивается на 36 регионов (рис. 2):
Рис. 2. Карта глубины Шт^, разбитая на 36 регионов
Для каждой ячейки размера 107 х 80 находится среднее значение глубины путем сложения значений всех пикселей и деления полученной суммы на общее число пикселей. Полученная матрица размера 6 х 6 разбивается на столбцы. Для каждого из 6 столбцов находится минимальное значение.
Таким образом, мы получаем своеобразный набор из 6 дальномеров, лучи которых расположены по дуге с промежутком 10 градусов. Подобная модель будет использована частицами для получения расстояний до объектов на карте.
Перейдем непосредственно к вычислению весов частиц. В начале, инициализируем веса частиц равными единицам. Затем, будем последовательно умножать этот вес на следующее выражение:
Мг] + Мр}
где - показание /-го датчика робота, MPi - показание /-го датчика частицы, / = {0,1,2,3,4,5} - количество датчиков, в данном случае, 6.
Частицы, наиболее близкие к реальному положению робота, будут иметь наибольший вес, наиболее удаленные частицы - наименьший. • Отбор частиц
На этом шаге необходимо провести отбор наиболее сильных частиц. Общее число частиц при этом должно сохраняться неизменным, поэтому некоторые частицы попадут в новый набор несколько раз. Основные шаги отбора представлены в алгоритме 2: Алгоритм 2. Отбор частиц
1. Нормализация весов. Веса всех частиц складываются, вес каждой частицы делится на полученную сумму.
2. Построение нового набора частиц осуществляется путем использования колеса отбора (рис.3):
Рис. 3. Колесо отбора частиц Частицы с большим весом занимают соответственно большую долю колеса, и наоборот.
Создадим переменную index и придадим ей случайное значение в интервале от 0 до N, где N - количество частиц. Создадим также переменную в, инициализировав её нулем. Теперь, в цикле от 0 до N выполним следующие действия: для всех i от 0 до N
в = в+ rand(0...2*wтах) // wmax - максимальный вес частицы в наборе
если (частица[^ех]^ < в ) {
в = в -частица[^ех]^ index = index + 1;
}
иначе
{ }
конец
Поместить частицу в новый набор
3. Выполнив этот цикл для всех частиц, мы получим новый набор, в который попадут только наиболее приспособленные частицы.
• Определение местоположения робота
После получения нового набора частиц, вычислим предполагаемые координаты x,y,в робота путем вычисления средних значений данных переменных между всеми частицами.
• Обратно на шаг 2.
После определенного количества подобных циклов, в наборе останутся наиболее приспособленные частицы, сконцентрированные вокруг реального положения робота.
Реализация с помощью CUDA 1. Модель вычислений CUDA
Технология CUDA реализует модель вычислений типа «сетка». Низшими звеньями в этой модели выступают так называемые «нити», которые выполняют элементарные операции, например, сложение элементов двух векторов. Нити объединяются в блоки, причем мы можем пользоваться тремя измерениями. На практике обычно используются одномерный или двухмерный случаи. Блоки, в свою очередь, объединяются в сетку (grid), которая имеет два измерения (рис. 4):
Блок
Нить
Рис.4. Модель вычислений CUDA
2. Предсказание следующего положения робота
Для реализации данного шага на GPU мы должны запустить ядро (специальное название для функции, выполняемой на GPU), с количеством нитей равным количеству частиц. Аргументами ядра выступают предыдущая позиция робота, показания энкодеров и компаса. Случайные числа для реализации шума генерируются непосредственно на GPU с помощью библиотеки cuRAND, поставляемой вместе с CUDA Toolkit.
Каждая нить ядра выполняет все необходимые вычисления для соответствующей ей частицы.
3. Моделирование сенсора Kinect
Карта глубины с Kinect поступает на вход ядра CUDA, запущенного для тридцати шести двухмерных блоков размера 107 х 80 нитей. Каждому блоку соответствует свой регион карты глубины. Для каждого блока обрабатываемый участок изображения копируется в разделяемую память
для более быстрого доступа к данным. Далее, с помощью параллельной редукции вычисляется сумма всех элементов в блоке и делится на количество пикселей. Полученное значение записывается в 36-мерный вектор в глобальной памяти GPU. После того, как все блоки завершили работу, полученный вектор с найденными средними значениями секторов подается на вход второго ядра, запущенного для 1 блока из 36 нитей. Данное ядро, также с помощью параллельной редукции, вычисляет максимумы среди шести элементов каждого из столбцов. Полученный в результате вектор из шести элементов поступает на вход ядра, вычисляющего веса частиц.
4. Вычисление весов частиц
Данный шаг реализуется на GPU аналогично предыдущему. Мы вновь запускаем ядро, где каждая нить отвечает за вычисление веса для соответствующей частицы в наборе. На вход ядра поступает вектор данных с сенсора Kinect, а также карта помещения, размещенная в текстурной памяти GPU. Так как карта не меняется со временем, она копируется в память GPU лишь единожды. После проведения вычислений, массив с весами частиц передается в оперативную память для проведения отбора, который на данной стадии работы выполняется полностью на CPU. Это объясняется тем, что наиболее сложная часть алгоритма (непосредственно отбор) не обладает параллелизмом. Проведенные тесты показали, что прирост производительности, полученный при выполнении отдельных шагов отбора, таких как нормализация весов и вычисление средних значений переменных, практически полностью нивелируется временем копирования данных между CPU и GPU. Стоит отметить, что в Jetson TK1 существует унифицированная физическая память для CPU и GPU. Это позволяет устранить задержки, связанные с копированием данных.
Результаты
В качестве тестового стенда был использован ноутбук Sony VAIO с процессором Intel Core i5 3210M и мобильной видеокартой NVIDIA GeForce GT 640M. GeForce GT 640M построен на архитектуре Kepler и содержит два потоковых мультипроцессора со 192 вычислительными ядрами в каждом (всего 384 ядра на чипе). Intel Core i5 3210M имеет архитектуру последнего поколения Ivy Bridge. Два вычислительных ядра работают на частоте 2,5 Ггц.
Для наглядности алгоритм был протестирован на нескольких значениях количества частиц в системе. Результаты, полученные при реализации алгоритма на CPU представлены на рис. 5; результаты, полученные с ускорением на GPU, представлены на рис. 6.
N частиц Предсказание (мс) Вычисление весов (мс) Отбор (мс) Нахождение среднего (мс) В общем (мс)
8000 2,62 8,58 0,08 0,64 11,92
16000 5,33 17,41 0,18 1,3 24,22
32000 10,6 34,54 0,38 2,75 48,27
64000 20,75 71,34 0,89 5,12 98,1
128000 41,76 137,46 1,83 10,39 191,44
256000 82,56 271,75 6,98 20,49 381,78
Рис.5. Результаты производительности при реализации на CPU
N частиц Предсказание (мс) Вычисление весов (мс) Отбор (мс) Нахождение среднего (мс) В общем (мс)
8000 0,57 0,69 0,08 0,64 1,98
16000 1,12 1,25 0,18 1,3 3,85
32000 2,22 2,34 0,38 2,75 7,69
64000 4,42 4,31 0,89 5,12 14,74
128000 8,91 8,22 1,83 10,39 29,35
256000 17,7 16,14 6,98 20,49 69,31
Рис.6. Результаты производительности при реализации на GPU
450
400
350
300
J. 250 а:
К 200
о.
CD
150 100 50
о
^^CPU ^^GPU Рис. 7. График производительности алгоритма Как можно увидеть из рисунков, представленных выше, при ускорении алгоритма на GPU нами было получено повышение производительности более чем в 15 раз по сравнению с реализацией на CPU. Такой значительный прирост связан в первую очередь с переносом на GPU вычисления весов частиц, которое обладает высокой степенью параллелизма. Преимущество параллельного исполнения особенно заметно при большом количестве используемых частиц.
В дальнейшем планируется протестировать алгоритм на одноплатном компьютере Jetson TK1, провести дальнейшие оптимизации
sooo
32000 64000
N частиц
128000
256000
выполнения алгоритма на GPU, а также интегрировать систему локализации в единую систему управления автономным роботом.
Благодарности
Авторы выражают свою признательность заведующему кафедрой интегрированных компьютерных систем управления Института кибернетики ТПУ Лиепиньшу А.В. за возможность получения одноплатного компьютера Jetson TK1, а также Александровой Т.В. за предоставление доступа в лабораторию робототехники бизнес - инкубатора ТПУ для проведения экспериментов.
Литература
1. S. Thrun «Probabilistic robotics» // Wolfram Burgard, 2000
2. N. Wilt «CUDA Handbook: A comprehensive guide to GPU programming» // Addison-Wesley, 2013
3. S. Falahati «OpenNI Cookbook» // PACKT Publishing, 2011
4. NVIDIA Jetson TK1. URL: http://www.nvidia.ru/object/jetson-tk1-embedded-dev-kit-ru.html
