Содержание к диссертации
Введение
Глава 1. Методы и алгоритмы навигации сервисных мобильных роботов 10
1.1. Обзор существующих решений 10
1.2. Структура навигационной системы 14
1.3. Методы локализации мобильного робота в пространстве
1.3.1. Последовательная локализация робота на плоскости 17
1.3.2. Глобальная локализация робота на плоскости 25
1.3.3. Локализация робота в трехмерном пространстве 26
1.4. Методы управления движением мобильного робота 29
1.4.1. Общие принципы управления на траекторном уровне 31
1.4.2. Методы управления в динамической среде
1.5. Постановка задачи диссертации 36
1.6. Выводы по первой главе 36
Глава 2. Локализация мобильного робота 38
2.1. Фильтрация скана 39
2.2. Метод преобразования нормальных распределений
2.2.1. Представление пространства в виде карты нормальных распределений 43
2.2.2. Поиск соответствия скана и карты нормальных распределений 47
2.2.3. Минимизация кросс-корреляционной функции 50
2.2.4. Способ обновления карты нормальных распределений 53
2.3. Анализ точности и сходимости решения задачи локализации методом NDT 55
2.3.1. Анализ алгоритма NDT с минимизацией методом Ньютона 57
2.3.2. Анализ алгоритма NDT с минимизацией методом интегрирования 59
2.4. Метод сеточных функций 62
2.4.1. Представление окружающего пространства мобильного робота 63
2.4.2. Использование карты-сетки в задаче локализации 64
2.4.3. Решение задачи SLAM с использованием кросс-корреляционных функций 66
2.4.4. Поиск соответствия скана и карты сеточных функций 68
2.4.5. Обновление карты помещения 2.5. Анализ точности и сходимости решения задачи локализации методом сеточных функций 76
2.6. Определение положения мобильного робота на карте 81 Стр.
2.7. Сравнение методов решения задачи SLAM 83
2.8. Выводы по второй главе 84
Глава 3. Управление мобильным роботом в динамической среде 86
3.1. Планирование траектории на карте сеточных функций 88
3.2. Трассировка подвижных препятствий
3.2.1. Построение списка препятствий 98
3.2.2. Определение габаритной окружности препятствия 100
3.2.3. Оценка вектора состояния препятствия 103
3.2.4. Анализ результатов трассировки подвижных препятствий 105
3.3. Управление мобильным роботом для движения вдоль заданной траектории 107
3.3.1. Метод эффективного пути 108
3.3.2. Система управления движением мобильного робота к опорной точке 113
3.3.3. Анализ эффективности системы управления мобильным роботом 116
3.4. Обход подвижных препятствий 120
3.4.1. Прогнозирование столкновений 121
3.4.2. Метод динамического окна
3.5. Сравнительный анализ методов управления мобильным роботом в динамической среде 131
3.6. Выводы по третьей главе 134
Глава 4. Разработка программного обеспечения, моделирование и экспериментальные исследования 136
4.1. Разработка ПО навигационной системы 136
4.1.1. Алгоритм планирования траектории А 137
4.1.2. Алгоритмы трассировки и обхода подвижных препятствий 138
4.1.3. Пакет программ навигационной системы 139
4.2. Моделирование навигационной системы сервисного робота 144
4.2.1. Динамическая модель трехколесного мобильного робота 145
4.2.2. Оценка качества построения карты помещения 155
4.2.3. Среда моделирования 156
4.2.4. Проверка работоспособности навигационной системы 157
4.2.5. Определение точности локализации мобильного робота 159
4.2.6. Оценка качества управления движением мобильного робота 163
4.2.7. Результаты моделирования 165
4.3. Экспериментальные исследования 166 Стр.
4.3.1. Исследование навигационной системы 168
4.3.2. Определение точности локализации мобильного робота 170
4.3.3. Оценка качества движения по траектории 175
4.3.4. Результаты экспериментальных исследований
4.4. Внедрение навигационной системы 178
4.5. Выводы по четвертой главе 180
Основные результаты диссертации 181
- Локализация робота в трехмерном пространстве
- Анализ алгоритма NDT с минимизацией методом интегрирования
- Анализ результатов трассировки подвижных препятствий
- Оценка качества управления движением мобильного робота
Введение к работе
Актуальность проблемы
Сегодня робототехника находит свое применение в сфере услуг. По всему миру сервисные роботы стали использоваться в музеях, аэропортах, магазинах, больницах и в других учреждениях, где требуются операции обслуживания, такие как: перемещение предметов (товаров, лекарств, и т.п.), патрулирование или уборка помещений, предоставление информационных ресурсов, и т.д. В сегменте роботов для населения становятся все более популярными машины, оказывающие непосредственную поддержку человеку. Например, роботы для телеприсутствия, позволяющие удаленно находиться в помещении и двигаться по нему, наблюдая происходящее с помощью бортовой видеокамеры. Подобные роботы могут использоваться в образовательных целях: в школах и ВУЗах, на различных выставках и форумах. В век информационных технологий сервисные роботы просто незаменимы, когда аудио- и видеосвязи недостаточно, поскольку робот позволяет не только перемещаться в пространстве, но и взаимодействовать с объектами.
Существующие сервисные роботы, в основном, функционируют в режиме
телеуправления, который мало автоматизирован. Датчики на борту робота –
инфракрасные и ультразвуковые дальномеры – позволяют избежать
столкновений со статическими препятствиями, однако в динамической среде данный подход не обеспечивает безопасность движения. Для полноценного выполнения своих задач робот должен построить карту помещения, определять собственные координаты на ней, двигаться по заданному маршруту и при этом соблюдать меры безопасности. Данную проблему решает автоматическое движение робота с использованием навигационной системы, оставляя оператору только функцию целеуказания.
Во всем мире ведутся интенсивные исследования по технологиям навигации роботов с использованием сканирующего лазерного дальномера, с помощью беспроводных сетей, ориентиров, карты местности, в т.ч. трехмерной. Использование лазерного дальномера позволяет достичь наиболее высокой точности локализации.
Существующие системы навигации достаточно хорошо проработаны для применения в индустриальной сфере, которая полностью детерминирована и статична. В сфере услуг обстановка в корне отличается: помещение, в котором находится робот, наполнено подвижными препятствиями (людьми, другие роботами, животными). Поэтому автоматическое управление сервисным мобильным роботом в среде, содержащей подвижные препятствия, является актуальной научно-технической задачей.
В диссертации рассмотрены вопросы, связанные с разработкой
навигационной системы сервисного мобильного робота на основе сканирующего лазерного дальномера, в состав которой входят алгоритмы одновременной локализации и построения карты (SLAM), планирования маршрута и система управления движением робота. При разработке навигационной системы были использованы методы преобразования нормальных распределений (NDT), сеточных функций, А* (А-звезда), эффективного пути и динамического окна.
К достоинствам системы можно отнести высокую точность локализации мобильного робота в помещении при эффективном использовании бортовых вычислительных ресурсов, а также оптимальное по времени движение к цели в условиях динамической среды.
Разработки навигационных систем ведутся в различных организациях как в нашей стране, так и за рубежом. Ключевыми в данном вопросе являются: университет Карнеги-Меллон (США), Стэнфордский университет (США), Боннский университет (Германия). В нашей стране это: НИИ СМ МГТУ им. Н.Э. Баумана, НУЦ «Робототехника», ЦНИИ РТК, ИПМ им. М.В. Келдыша РАН, МГТУ «Станкин», МИРЭА.
Цели и задачи
Целью диссертационной работы является разработка и исследование навигационной системы мобильного робота на основе сканирующего лазерного дальномера для управления движением в среде с динамическими препятствиями. Для достижения цели в работе были поставлены и решены следующие задачи.
-
Разработать метод одновременной локализации и построения карты помещения (SLAM) по данным от сканирующего лазерного дальномера.
-
Предложить способ управления движением мобильного робота в среде с динамическими препятствиями.
-
Разработать программное обеспечение навигационной системы.
-
Провести математическое моделирование системы управления мобильного робота, оснащенного навигационной системой.
-
Провести экспериментальные исследования движения мобильного робота в среде с динамическими препятствиями.
Методы исследования
Для решения поставленных задач были привлечены классические методы
теории автоматического управления, оптимального управления, теории
вероятности, методы глобальной оптимизации, теории графов, современные способы фильтрации и математический аппарат цифровой обработки сигналов. Проверка работоспособности алгоритмов и оценка точности осуществлялась путем математического моделирования, проведением экспериментальных исследований в лабораторных условиях и испытаний системы на мобильном роботе.
Научная новизна
-
Разработан новый метод сеточных функций для одновременной локализации мобильного робота и построения карты помещения.
-
Предложен эффективный способ минимизации кросс-корреляционной функции для метода преобразования нормальных распределений.
-
Разработан новый способ управления движением сервисного робота в среде с динамическими препятствиями с использованием нового метода сеточных функций.
-
Предложен критерий оптимизации метода динамического окна для управления роботом при наличии динамических препятствий.
Практическая ценность
Разработан пакет программ для бортового компьютера, реализующий алгоритмы навигации мобильного робота, оснащенного сканирующим лазерным дальномером, для автономной работы в условиях среды с динамическими препятствиями. Предложенный алгоритм навигации позволяет эффективно решать задачи доставки грузов, патрулирования помещения и т. д. без участия человека в процессе управления.
Защищаемые положения
-
Новый метод одновременной локализации и построения карты помещения (SLAM) по данным сканирующего лазерного дальномера, позволяющий увеличить скорость движения робота.
-
Метод скользящей точки для минимизации кросс-корреляционной функции метода преобразования нормальных распределений, который улучшает его сходимость.
-
Способ управления движением мобильного робота с использованием разработанной навигационной системы, обеспечивающий движение в среде с динамическими препятствиями.
-
Способ определения параметров (трассировки) подвижных препятствий на основе данных от лазерного дальномера.
-
Модифицированный метод динамического окна (DW) для управления движением робота в среде с динамическими препятствиями, учитывающий вероятностный характер их движения.
-
Математическая модель навигационной системы сервисного робота, которая решает задачи обхода динамических препятствий.
-
Программная среда моделирования, позволяющая определить параметры навигационной системы и визуализировать процесс управления роботом.
Апробация работы.
Основные результаты работы докладывались и получили положительную оценку на:
Международной научно-технической конференции «Экстремальная робототехника» (Санкт-Петербург, 2011);
Всероссийской научно-технической конференции «Экстремальная робототехника» (Санкт-Петербург, 2012);
4-й научно-технической конференции «Техническое зрение в системах управления» (Москва, 2013);
7th International Workshop on Robotics for Risky Environment - Extreme Robotics «7th IARP RISE-ER’2013» (Saint-Petersburg, 2013);
5-й научно-технической конференции «Техническое зрение в системах управления» (Москва, 2014);
25-й, юбилейной научно-технической конференции «Экстремальная робототехника (ЭР-2014)» (Санкт-Петербург, 2014);
Научных семинарах кафедры «Роботы и робототехнические системы» МГТУ им. Н.Э. Баумана в 2011-2015 гг.
Публикации.
Результаты диссертационной работы отражены в 8 научных трудах, среди которых 2 публикации в изданиях из перечня ВАК РФ.
Структура и объем работы.
Диссертационная работа состоит из введения, 4-х глав, заключения и списка литературы. Объем работы составляет 186 печатных страниц, 119 рисунков, 11 таблиц. Библиография содержит 59 наименований, из них 43 иностранных источника.
Локализация робота в трехмерном пространстве
Интерфейс верхнего уровня СУ получает целевую точку, в которую необходимо вывести робот. Навигационная система возвращает интерфейсу информацию о текущем положении робота, что является обратной связью.
Планировщик пути использует текущее положение и карту занятости, чтобы проложить оптимальный по заданному критерию маршрут к целевой точке (в качестве такого критерия может выступать кратчайшая длина пути). Также, планировщик может принимать во внимание подвижные препятствия.
Трассировщик подвижных препятствий, имея в своем распоряжении текущий скан и зная собственное положение робота, строит список подвижных препятствий и определяет их параметры (положение, скорость, размеры).
Алгоритм обхода препятствий выдает задание на исполнительный модуль робота, при этом выполняя сразу две функции: во-первых, следует спланированной траектории, обходя статические препятствия, а, во-вторых, прогнозирует движение подвижных препятствий и выполняет соответствующие маневры, чтобы избежать столкновения.
Интерфейс СУ робота получает задание в виде линейной и угловой скорости и передает управляющий сигнал непосредственно на СУ исполнительного модуля робота.
Таким образом, предложенная структура позволяет решить задачу навигации мобильного робота в помещении для общего случая. Особенностью структуры является независимость от типа шасси мобильного робота, а также наличия и типа датчиков одометрии, что позволяет использовать разрабатываемую навигационную систему на всех типах мобильных роботов в помещении. Однако это выдвигает высокие требования к алгоритму SLAM по точности, устойчивости и быстродействию.
На данный момент существует множество различных алгоритмов SLAM, отличающихся как по типу входной информации, представлению окружающего пространства в виде карты, так и по методам обработки этой информации. Во-первых, введем классификацию алгоритмов локализации по размерности картографируемого пространства: 1) двумерная локализация на плоскости (2D-SLAM); 2) трехмерная локализация в пространстве (3D-SLAM); 3) цветовая локализация по R, G, B компонентам изображения (Colour-SLAM); 4) цветовая трехмерная локализация в пространстве (6D-SLAM). Эти характеристики зависят напрямую от используемого сенсора. Если робот оснащен сканирующим лазерным дальномером, например, SICK LMS200, Hokuyo URG04LX, входной информацией для алгоритма является двумерное горизонтальное сечение рельефа окружающих объектов, соответственно для обработки применяется 2D-SLAM. При наличии дополнительной оси сканирования можно получить трехмерное облако точек, дающее представление объектов помещения с учетом их взаимного расположения в пространстве, поэтому здесь применим 3D-SLAM. Существуют алгоритмы, например, описанные в работе [1], оценивающие положение робота по изображению с установленной на нем цветной видеокамеры, – это алгоритмы цветовой локализации. Однако в наше время набирают популярность сенсоры, позволяющие получить трехмерное цветное изображение объектов, к примеру, TOF-камеры, Kinect и им подобные; для обработки таких изображений с целью локализации и построения карты применяются алгоритмы 6D-SLAM. Следует отметить, что подавляющее большинство алгоритмов локализации на плоскости могут быть расширены на трехмерное пространство.
Во-вторых, следует разделить глобальную локализацию и последовательную. Глобальная локализация позволяет определить положение робота на карте без начального приближения. Еще одна особенность алгоритмов SLAM данного типа – это возможность замыкания циклов, т.е. распознавание уже пройденного участка карты с последующей релаксацией всей карты вдоль траектории робота. Последовательная (относительная) локализация определяет изменение положения робота между двумя последовательными сканами. Алгоритмы этого типа, как правило, дают более точный результат определения положения мобильного робота, по сравнению с глобальными алгоритмами SLAM, но должны выполняться в реальном времени. Однако, если данный алгоритм не сошелся хотя бы один раз, дальнейшее его использование невозможно без дополнительных поправок. Для достижения наилучшего результата по точности и надежности локализации необходимо использовать оба алгоритма совместно.
В данной работе в качестве сенсора используется сканирующий лазерный дальномер SICK LMS200, поэтому сначала рассмотрим подробнее алгоритмы 2D-SLAM, а затем, ввиду популярности трехмерных систем технического зрения, проведем обзор алгоритмов локализации в трехмерном пространстве.
В данной диссертационной работе основной упор сделан на разработку и реализацию алгоритма SLAM, обладающего высокой точностью локализации и достаточным быстродействием для работы в реальном времени. Здесь и далее под реальным временем будем понимать ситуацию, когда время, затраченное на обработку одного скана, не превышает период поступления входной информации. Исходя из этого, обзор алгоритмов SLAM начнем с существующих алгоритмов последовательной локализации. Процесс последовательной локализации имеет несколько вариантов названий: сопоставление сканов (scan matching) или регистрация сканов (scan registration), т.к. алгоритм изначально был предназначен для последовательного сканирования некоторого объекта с целью получения его трехмерной геометрической модели. И самым первым успешным алгоритмом стал ICP (Iterative Closest Point – итеративный алгоритм ближайших точек), который имеет свое применение до сих пор.
Алгоритм ICP. В основе данного алгоритма лежит метод последовательных приближений для минимизации суммы квадратов расстояний между соответствующими точками двух сканов. Первый шаг алгоритма – поиск соответствия пар точек между текущим сканом и опорным. Для каждой точки текущего скана выбирается ближайшая по Евклидовому расстоянию из опорного скана. Основным ограничением ICP является факт, что ближайшая точка в общем случае не соответствует той же самой точке сканируемой поверхности, особенно если два скана выполнены с отдаленных друг от друга позиций. Тем не менее, в успешных приложениях алгоритма ICP, он сходится к некоторому полезному решению. Самой трудоемкой задачей является поиск точек на двух сканах, соответствующих одной точке на поверхности реального объекта.
Одним из вариантов решения этой задачи является использование взвешенных пар точек, при этом наибольший вес имеют пары, наиболее вероятно соответствующие одной точке поверхности. Простейший способ – это принимать в качестве веса величину, обратно пропорциональную расстоянию между точками, таким образом, отдаленные друг от друга точки имеют меньший вес, чем находящиеся рядом. Однако, такое решение не подходит для сопоставления сканов, полученных в коридорах и тоннелях, т.к. протяженные объекты при небольшом отклонении курса будут давать слишком маленький вес пар точек, удаленных от центра вращения робота, что приведет к снижению производительности алгоритма.
Еще одной проблемой является необходимость исключения изолированных и посторонних точек из скана. При использовании лазерного дальномера, такие точки могут появляться на границах объектов и в случаях, когда луч лазера «скользит» по поверхности объекта. Обычный критерий поиска таких точек – это расстояние между ними, превышающее некоторый порог. Кроме того, необходимо исключать пары точек, соответствующие границе объекта, в противном случае непересекающиеся области сканов внесут в результат систематическое смещение (Рис. 1.5) [48]. Однако, определение граничных точек является довольно сложной задачей. После установки надежного соответствия пар точек, измеренные расстояния между этими точками минимизируются, и затем процесс повторяется опять с новой выборкой, пока алгоритм не сойдется.
Анализ алгоритма NDT с минимизацией методом интегрирования
В этом разделе описан новый метод сеточных функций для решения задачи одновременной локализации и построения карты. Как и многие другие методы SLAM, он основан на сопоставлении скана и карты при помощи кросс-корреляционной функции. Однако для описания модели рабочей среды робота был предложен новый подход: преобразование скана в сеточную функцию. При этом, каждая точка скана преобразуется в некоторую непрерывную функцию, затем они объединяются при помощи выбранного принципа суперпозиции и накладываются на карту-сетку, тем самым образуя сеточную функцию.
Основной проблемой был выбор функции, в которую преобразуется каждая точка. К ней предъявляются следующие требования: - непрерывность и гладкость (существование первых двух производных); - функция должна иметь единственный экстремум в точке скана; - область определения функциидолжна быть конечной; - функция должна быть нормирована, т.е. область ее значений должна принадлежать отрезку [0; 1];Относительно используемого принципа суперпозиции выдвинем условие, что он должен быть симметричным и не изменять значения других функций вблизи их экстремумов.
Изначально скан, полученный от лазерного дальномера, обработанный описанными выше методами, представляет собой массив точек в полярных координатах, которые лежат на поверхности объектов пространства вокруг робота. Такое описание пригодно для области пространства непосредственно вокруг робота. Однако использование полярных координат для описания всего помещения усложняет преобразование из системы координат, связанной с роботом, в систему, связанную с помещением.
Зная положение мобильного робота в текущий момент времени, можно перенести точки скана в систему координат помещения для дальнейшего хранения и использования, что относится к подзадаче обновления карты, а на данном этапе положим, что оценка положения мобильного робота p нам известна и скан преобразован в систему координат, связанную с помещением. Введем обозначение текущего к-го скана как массив точек в декартовых координатах: sk = {xj}k .
Для восстановления карты помещения достаточно хранить список точек всех сканов в системе координат помещения. Однако чтобы получить выборку накопленных точек в окрестности некоторой, интересующей нас, необходимо из всего списка выбрать те, которые удовлетворяют заданному условию, например, максимальному евклидовому расстоянию. Это, в свою очередь, приводит к постоянному росту потребляемых ресурсов вычислителя и, в конечном итоге, к остановке работы алгоритма.
Отметим, что для решения данной задачи активно применяются алгоритмы систем технического зрения, такие как распознавание образов, кластеризация, векторизация и т.п. Применение этих алгоритмов позволяет получить карту помещения в виде списка объектов, относящихся к конкретному классу, либо форме, либо кривой п-го порядка, с параметрами, однозначно определяющими границы объекта. Данный подход существенно сокращает объем необходимой памяти для хранения информации об окружающем пространстве, но требует больших вычислительных ресурсов, что не позволяет работать в режиме реального времени. Проанализировав имеющиеся способы хранения карты помещения, которые дают возможность быстрого нахождения объекта в заданной точке и упрощают локализацию мобильного робота, была использована карта-сетка.
Большинство алгоритмов SLAM, накапливающих данные об окружающем пространстве, используют карту-сетку (в англоязычной литературе - grid map), представляющую собой матрицу, каждый элемент которой описывает прямоугольный участок пространства Мi j, где 0 i NY и 0 j NX.
Самое простое описание заключается в следующем: если элемент матрицы равен 1, то в соответствующем участке находится объект, в противном случае, если элемент равен 0, он свободен. Данное представление можно сравнить с бинарным растровым изображением плана помещения. Применительно к задаче локализации необходимо отметить, что сравнение скана с накопленной картой помещения выполняется тривиально: подсчетом количества ячеек карты, содержащих объект в точках скана. Чем больше таких ячеек, тем скан точнее совпадает с картой.
В данной диссертационной работе эти и последующие выкладки, касательно сравнения скана с картой, будут формализованы в следующем разделе с целью дальнейшего анализа.
Более сложная модель пространства заключает в себе информацию о вероятности нахождения какого-либо объекта в каждой ячейке карты, в этом случае каждому элементу карты-сетки присваивается значение от 0 до 1. При этом 1 означает достоверное нахождение объекта в участке помещения, соответствующем ячейке карты, а 0 - отсутствие каких-либо объектов. Такой подход позволяет учесть статистические характеристики измерений, что, в свою очередь, повышает детализацию карты. При этом сравнение скана с картой проводится путем сложения плотностей вероятности, содержащихся в ячейках карты, в которые попали точки текущего скана. В отличие от предыдущего варианта, здесь можно более точно оценить степень совпадения, а, следовательно, определить преобразование координат с меньшей погрешностью.
Заметим, что решение задачи локализации заключается в поиске максимального совпадения скана с картой. Таким образом, можно сформулировать задачу локализации мобильного робота следующим образом: локализация робота - это задача оптимизации, в которой критерием является степень соответствия скана и карты, а аргументом -преобразование координат точки сканирования.
Анализ результатов трассировки подвижных препятствий
После того, как найден эффективный путь, необходимо выдавать сигнал управления на робот, чтобы он двигался к опорной точке. Существует ряд способов управления движением мобильного робота. В главе 1 были подробно рассмотрены некоторые из них. Как было сказано, для нашего случая целесообразно использовать управление с линейными регуляторами, т.к. оно хорошо изучено и является самым простым в реализации. На основе классической схемы управления трехколесным мобильным роботом была спроектирована модель системы управления для выхода в заданную точку на плоскости, структурная схема которой представлена на Рис. 3.24. Основная идея такого управления - это минимизировать ошибки системы как по углу между направлением робота и направлением на точку, так и по расстоянию до этой точки.
Рассмотрим состав системы. Объектом управления является робот, управляемый по скорости. Блок «SLAM» оценивает положение робота по скану, полученному от лазерного дальномера. Координаты целевой точки задаются входными параметрами «target X» и «target Y». Для получения ошибки используются сумматоры и блок, преобразующий разность координат в угол и расстояние. Блок «Angle Corrector» корректирует разность углов, учитывая цикличность, и переводит ее в промежуток (–л; л]. Для вычисления управляющих сигналов применяется П-регулятор. Коэффициенты усилителей «Kro» и «Kfi» задают добротность контура управления по расстоянию до цели Kр и углу K , соответственно.
Усилитель «Kangle» и следующие за ним блоки «Saturation» и «cos» подстраивают коэффициент регулятора расстояния K посредством блока умножения. Блок «Relay» разрывает контур управления по углу, когда целевая точка достигнута с заданной погрешностью в расстоянии. Блоки «V saturation» и «w saturation» ограничивают управляющие сигналы скорости поступательного движения и скорости поворота, соответственно. Блоки «V limit» и «w limit» ограничивают первые производные управляющих сигналов, т.е. линейное ускорение и угловое, соответственно. Блоки «Trajectory», «Position» и «Control» используются для вывода графиков траектории, положения робота и управляющих сигналов.
Рассмотрим принцип работы системы. На вход подаются координаты заданной точки (x\y). Координаты заданной точки сравниваются с координатами текущей (x, y) и на выходе сумматоров появляются сигнал ошибки расстояния e (на схеме - er) и угол на целевую точку. Этот угол затем сравнивается с текущим курсом робота, и сумматор выдает разницу углов efl. Как уже было сказано, блок «Angle Corrector» преобразует efi в интервал (–я; тг], и это значение рассматривается как ошибка по углу е.
Предположим, что е = 0. Тогда систему можно рассматривать как следящую по положению. Примем допущение, что астатизм системы обеспечивается контуром скорости, реализованным в приводах робота. Тогда для реализации слежения достаточно использовать П-регулятор. Точно такие же рассуждения можно провести и для управления по углу.
Теперь рассмотрим управление при ненулевых ошибках расстояния и угла. Для того, чтобы система была устойчивой, поступательное движение робота должно сокращать расстояние до целевой точки. Это справедливо, когда направление движения робота примерно совпадает с направлением на точку. Если это не учитывать, регулятор расстояния будет всегда выдавать управляющее воздействие линейной скорости, и робот станет в некоторых случаях удаляться от цели. Решение этой проблемы было реализовано с помощью переменного коэффициента П-регулятора. Была задана нелинейная зависимость этого коэффициента от ошибки по углу поворота е так, что при нулевой ошибке Кр = х, а при ошибке больше определенного порога К = 0. В качестве такой зависимости была взята функция
График данной функции показан на Рис. 3.25. Как видно из графика, Kangie задает диапазон ошибки угла, при котором робот может перемещаться, а Кх - добротность контура положения при движении робота в направлении на опорную точку.
При достижении роботом опорной точки возникают круговые колебания из-за погрешностей, вызванных, к примеру, проскальзыванием. Поэтому в контур углового положения был введен релейный блок, который размыкает контур на расстоянии от опорной точки, меньшем некоторого предела, и тем самым останавливает движение робота.
Управляющие сигналы с регуляторов ограничены по максимальному значению и по скорости нарастания, что определяется максимальными ускорениями робота. С увеличением расстояния до целевой точки управляющий сигнал задания скорости v растет пропорционально ему, поэтому ограничение необходимо из очевидных соображений. Кроме того, привода мобильного робота не смогут отработать произвольный управляющий сигнал. Ограничение по ускорению введено для обеспечения плавного старта и останова. Параметры этих ограничений определяются динамикой приводов мобильного робота и могут быть заданы произвольно.
Оценка качества управления движением мобильного робота
Моделирование показало, что расчет регулятора для данного привода проведен верно. Однако, следует обратить внимание, что ток во время переходного процесса увеличивается до 125 А, а это трудно реализовать. Кроме того, управляющее напряжение выходит за границы номинального диапазона (12 В). Чтобы приблизить ситуацию к действительности, рассчитанный регулятор был проверен на модели реального привода (Рис. 4.11). Модель реального привода отличается от классической наличием нелинейных элементов. В данной модели были учтены следующие ограничения: - квантование сигнала измеренной скорости, вызванное использованием фотоимпульсного датчика (ФИД); - дискретизация управляющего сигнала цифровым контроллером; - ограничение управляющего напряжения.
Для реализации этих ограничений в исходную модель были введены блоки «Encoder», «Delay» и «Saturation», соответственно. Блок «Encoder» осуществляет квантование сигнала скорости с дискретом 512 имп/об. «Delay» реализует 0-интерполяцию кода управляющего сигнала, т.е. задержку на период времени 0,001 с, затрачиваемого на вычисления в контроллере. Блок «Saturation» моделирует насыщение управляющего сигнала, если он превышает допустимый предел; в данном случае если код ШИМ по модулю превосходит значение 32768, то на двигатель подается ±12 В, в зависимости от знака кода ШИМ.
Результат моделирования работы реального привода показан на Рис. 4.13. Как видно, система стала неустойчивой, а точнее, возникли автоколебания, что является недопустимым.
Для выяснения причины автоколебаний был проведен анализ модели. Выяснилось, что основная причина заключается в приблизительном совпадении частоты дискретизации и частоты среза контура скорости. Менее сильное влияние на качество переходного процесса оказывает блок насыщения: при большой амплитуде входного сигнала полоса пропускания контура сдвигается в сторону низких частот, что, наоборот, уменьшает автоколебания. Однако, при этом увеличивается время переходного процесса. Для разгона от нуля до номинальной скорости двигателя нужно 0,04 с.
Таким образом, для восстановления работоспособности привода необходимо уменьшить частоту среза, т.е. его быстродействие. Это можно сделать, корректируя добротность привода, варьируя коэффициент Kрс регулятора скорости. Выбрав значение Kрс = 8667, получили удовлетворительный переходный процесс, вид которого показан на Рис. 4.14.
Из графиков видно, что быстродействие привода составило 0,007 с, при заданном значении скорости 100 рад/с, что на порядок больше, чем в идеальном случае. В реакцию на ступенчатое управляющее воздействие теперь входит латентный период, равный периоду дискретизации системы управления. Реакция на возмущающее воздействие замедлилась примерно в два раза по сравнению с классической моделью, что вызвано, в основном, ограничением напряжения, подаваемого на обмотки двигателя.
Таким образом, была разработана модель электропривода с цифровым управлением, выбран регулятор и найдены его коэффициенты. Результаты показали, что модель адекватно отражает реальные процессы, протекающие в приводе, а это позволило использовать ее для описания динамики мобильного робота.
Реализация динамической модели робота. Для описании динамики мобильного робота была предложена структурная схема (Рис. 4.15). В основе данной структуры лежат блоки: «Right Drive» и «Left Drive», реализующих рассмотренную выше модель правого и левого привода, а также «Robot dynamics», в котором описана динамика робота. Также в модель добавлены нелинейные элементы – «Friction» и «Dead Zone», которые нужны для учета сухого трения, возникающего в приводах.
Как видно из рисунка, на вход модели робота подаются значения заданных линейной и угловой скоростей мобильного робота. Эти скорости преобразуются блоком «Decoupling» в скорости вращения двигателей. Привода отрабатывают задание и создают вращающий момент, преобразуемый блоком динамики в скорости робота. На выходе блока динамики получаем отработанные скорости вращения приводов, которые используются в качестве обратной связи и учета сухого трения, а также, после преобразования, линейную и угловую скорости робота, которые являются выходами модели робота. Эти скорости интегрируются в блоке «Kinematic model» в соответствии с уравнениями кинематики (4.1). В модели данный блок выдает интегрированное положение и угол поворота робота. В реальном роботе положение робота определяется алгоритмом SLAM, поэтому на Рис. 4.15 показано, что выходом модели робота является скан, полученный от дальномера из текущего положения мобильного робота.
Рассмотрим подробнее динамику шасси трехколесного мобильного робота, которая описывается 3 и 4 уравнениями (4.1). Структурная схема модели, реализованная на основе этих уравнений, выглядит, как показано на Рис. 4.16.
Здесь сумма моментов, развиваемых двигателями, идет на линейное ускорение робота, а разница – на угловое. Так как моменты связаны, обязательно будет присутствовать взаимовлияние приводов. По верхней линии на схеме идет динамическая составляющая линейного движения, а по нижней – составляющая вращения робота. В середине схемы блоки умножения и сумматоры описывают возникающие при движении центробежную и кориолисову силы. Блок «Decoupling», как уже говорилось, преобразует скорости робота в скорости вращения приводов.
Для проверки модели сначала на вход были поданы такие сигналы управления, чтобы на выходе блока «Decoupling» (Рис. 4.16) на правый двигатель выдавалась скорость 500 рад/с, а на левый – 0. Затем через некоторое время (0,1 с) линейная скорость была увеличена в 2 раза, а угловая сброшена в 0, что не изменяет задание на правый привод, а на левый задает 500 рад/с. После моделирования динамики робота получились следующие результаты (Рис. 4.17).
Во-первых, необходимо заметить, что имеет место взаимовлияние приводов. Например, при разгоне правого двигателя, левый также изменил свою скорость, и приводу левого двигателя пришлось увеличить момент для поддержания заданной нулевой скорости. Во-вторых, усилилось перерегулирование. Это вызвано переменной нагрузкой из-за взаимовлияния приводов, и в меньшей степени инерциальными силами, возникающими в процессе движения.
Для построения карты помещения была использована модель лазерного дальномера и оценка положения робота, полученная с помощью алгоритма SLAM (п. 2.4). На Рис. 4.18 представлена разностная карта помещения, где черным цветом показано наличие объектов на исходной карте, а белым – на построенной. При этом те участки, где карты совпадают, остаются серыми. Из рисунка видно, что построенная карта отличается от исходной не более чем на 5 см, что говорит о работоспособности алгоритма, а также об адекватности модели лазерного дальномера.