ВВЕДЕНИЕ Важной задачей в современной робототехнике является разработка способов определения положения робота в окружающем пространстве. Не зная положения робота в пространстве, не зная как выглядит окружающее пространство невозможно решить даже простейшую задачу движения робота из одной точки в другую. Наиболее часто используемые способы определения положения – интегрирование перемещений робота (с помощью одометров) или применение маяков, установленных в определенных местах. Использование маяков не универсально и требует предварительного оборудования рабочих помещений, при этом маяки постоянно должны быть в зоне видимости роботом. Интегрирование показаний одометров не обеспечивает точности позиционирования из-за накопления ошибки по всем отслеживаемым координатам. Актуальность выбранной темы обусловлена значимостью развития методов локализации робота и одновременного построения карты местности в современной робототехнике. Целью данной работы – является разработка алгоритма SLAM на основе фильтра частиц для наземного мобильного робота, использующего простейшие ультразвуковые или инфракрасные датчики дистанции, и моделирование его работы в среде моделирования V-REP. В соответствии с поставленной целью необходимо решить следующие задачи: а) Изучить теоретические основы и ознакомиться с основными методами и подходами к решению задачи SLAM; б) На основе проведенного анализа различных подходов разработать алгоритм, который позволяет определить положения робота в пространстве, и построить карту окружения робота. 6
1 ОБЗОР СУЩЕСТВУЮЩИХ МЕТОДОВ 1.1 Актуальность и новизна Мобильные автономные робототехнические системы в настоящее время, являются весьма обширной областью робототехники. Основным направлением исследований при разработке таких систем является разработка алгоритмов определения положения и ориентации робота в пространстве. Данную задачу можно разделить на две составляющих: построение карт (отображение) и локализация робота на местности (локализация).
Для решения первой задачи, необходимо знать максимально точное положение мобильного робота, чтобы основываясь на данных получаемых от датчиков производить построение карты. Однако тогда задача локализации становится приоритетной, и возникает вопрос, как определять положение робота? Использование одних одометров, хотя и является относительно простым способом определения позиции робота, этот метод не отличается высокой точностью. Использование GPS не всегда представляется возможным. Тогда разумным решением будет использование карты для определения положения робота, но откуда мы её можем получить, если окружающее робота В идеальном случае имеется возможность загрузить рабочую карту окружающего пространства, однако на практике такая возможность имеется не всегда, поэтому встает естественная задача: научить робота строить карту местности и одновременно определять положение в этой местности и траекторию движения. Область знаний, описывающая методы решения данной задачи, получила название SLAM (одновременная локализация и сопоставление).
Поняття переведення на іншу роботу
... пропонувалося ввести в законодавство різні визначення, а саме: переведення на іншу роботу, переведення на роботу з іншими істотними умовами праці, переведення на роботу в інший колектив, переведення на роботу в іншу місцевість [8,с.74]. Починаючи з 90-х років ...
На данный момент существует довольно большое количество 8
Первым шагом итерации локализации фильтра частиц является генерация нового частичного распределения для данной модели движения и примененного управления. Следующим шагом, рассчитывается вес каждого значения. Частицам, для которых прогнозируемые значения соответствуют измеренным, даются большие веса. И последний этап — это повторная выборка частиц. Она основывается на весах частиц из предыдущего распределения, которые берутся случайным образом, создавая новое распределения. Главной проблемой является логарифмическая сложность фильтра, которая сильно зависит от числа частиц. При их малом количестве, в течение длительного времени работы алгоритма могут возникать серьезные ошибки, тем самым искажая полученный образ пространства. Однако, чем большим числом частиц мы будем располагать, тем больше времени потребуется для вычислений. К самым распространённым алгоритмам, базирующимся на фильтре частиц можно отнести: а) Алгоритм FastSLAM. FastSLAM – один из подходов к решению задачи SLAM. В основе алгоритма лежит фильтр частиц и применение Байесовской сети. В первые такой подход для решения задачи SLAM предложили в своей статье Monterlo, Trum, Kollerm, Wedbreit [12], а также его последующее улучшение в статье о FastSLAM 2.0 [13].
FastSLAM разделяет задачу локализации и картографии на множество подзадач, используя независимость состояния отдельных элементов модели SLAM. На рисунке 2 представлена диаграмма процесса SLAM, где V – текущее положение робота, Z – показания датчиков и U – сигнал управления. 17
Стоит обратить внимание, что, несмотря на большую общность в отношении среды, этот метод рассчитан на определенный тип сенсоров точные лазерные дальномеры и может потребовать отдельные алгоритма HectorSlam усовершенствований для других типов. в) Алгортим HectorSlam. Описание представлено в работе [13].
Данный алгоритм использует в качестве модели рабочей среды всю накопленную карту. Карта представлена в виде сетки занятости, где значения функции M(PW ) = 1, если соответствующая ячейка занята, и M(PW ) = 0, если ячейка свободна. Чтобы смоделировать вероятностное распределение точек, относящихся к объектам на карте, авторы используют билинейную интерполяцию. При этом функции карты становятся непрерывными, и легко вычислить её градиент в любой точке. В качестве меры совпадения скана и карты используется среднеквадратичная ошибка всех точек скана. Тогда для оценки положения ξ∗ мобильного робота необходимо воспользоваться методом наименьших квадратов: i ξ∗ = arg min e[1 − M(SS (ξ))]h , Sj+ где SS (ξ) – координаты i-й точки скана, преобразованной в систему координат помещения, если считать, что скан получен из положения ξ. Учитывая простоту вычислений градиента, минимизация этой функции выполняется методом Гаусса-Ньютона. Однако оптимизируемая функция имеет локальные минимумы, поэтому авторы предложили искать решение в нескольких этапов, каждый из которых имеет дело с картой различного разрешения. Начиная с карты с большим разрешением, алгоритм находит неточную оценку положения, близкую к истинному. Затем выбирая все меньшие разрешения, эта оценка улучшается до тех пор, пока не достигнет необходимой точности, либо поиск останавливается на карте с наилучшим 21
Інтернет (електронна пошта, робота користувача)
... дослідження є електронна пошта як широковідома послуга мережі «Інтернет». Предметом дослідження є робота користувача з електронною поштою. Ключові терміни, що використовуються у роботі: Інтернет, електронна пошта, e-mail, ... повноцінного доступу (on-line) до Інтернету. Через електронну пошту можна отримати послуги інших сервісів мережі. Електронна пошта - типовий сервіс відкладеного зчитування (off- ...
разрешением. Наконец, применительно к задаче SLAM, оценка положения мобильного робота обновляется с использованием ФК. Рассмотренный алгоритм позволяет получить точную оценку положения робота, причем, накапливаемая ошибка мала для использования всей карты при сопоставлении скана. В качестве недостатка можно выделить необходимость в построении и обновлении нескольких карт с помощью различных разрешений, что обеспечивает эффективность алгоритма. г) Алгоритм Gmapping. Данный алгоритм является одним из основных алгоритмов SLAM в операционной системе ROS, что обусловлено его относительной эффективностью и простотой подхода. В основе данного алгоритма лежит метод рекурсивной фильтрации частиц Рао-Блэквелла на сетчатой карте [19, 20]. Каждая частица является потенциальной траекторией робота и содержит в себе, как потенциальную траекторию робота, так и информацию о состоянии карты рабочей среды в текущий момент времени. При наивном подходе на каждой итерации алгоритма генерируются несколько новых частиц из каждой предыдущей, при этом их число растет экспоненциально, что приводит к невозможности реализации данного алгоритма. Авторы предложили два подхода, которые позволяют увеличить производительность фильтра для возможности решения задачи SLAM в реальном времени, это: а) вспомогательная функция распределения, которая учитывает точность сенсоров, таким образом, позволяя создавать частицы весьма точным способом; б) техника адаптивного ресемплинга, которая поддерживает разумное число частиц для достаточной точности локализации и, в то же время, снижения риска опустошения множества частиц. Вспомогательная функция распределения вычисляется путем оценки степени похожести скана и карты при помощи процедуры регистрации и одометрической информации. 22
Ключевой идеей данного фильтра является оценка условной вероятности каждой частицы, которую можно разложить на множители, согласно теореме Рао-Блэквелла-Колмогорова: p(x+:l , m|z+:l , u+:l)+) = p(m|x+:l , z+:l) ∙ p(x+:l|z+:l , u+:l)+), где x+:l – траектория робота, из последовательных положений, m – карта помещений, z+:l – последовательность вектора наблюдения, u+:l)+ – измерения одометрии. Работу фильтра можно описать следующим алгоритмом а) Семплирование – формирование выборки частиц следующего (S) поколения {xl } из вспомогательной функции распределения π на основе (S) предыдущей выборки {xl)+} и вероятностной модели одометрии. (S) б) Взвешивание степени важности каждой частицы wl , основываясь на её апостериорной вероятности и значении вспомогательной функции распределения. в) Ресемплин – частицы выбираются с заменой, пропорционально их весу, вследствие чего веса оставшихся частиц выравниваются. г) Для каждой частицы вычисляется соответствующая оценка карты m на основе её траектории x+:l и истории наблюдений z+:l . Реализация рассмотренного алгоритма описана в [21].
По сути, рассмотренный алгоритм предназначен для фильтрации оценки положения робота, которая вычисляется некоторым методом регистрации сканов, и служит для повышения точности локализации. Отсюда видно, что достоинством данного алгоритма является высокая точность построения карты, однако при использовании метода регистрации сканов с низкой погрешностью целесообразность данного алгоритма ставится под сомнение, учитывая его достаточно высокую вычислительную сложность. 23
Системы адаптивного управления роботами
... деталей. Структура адаптивных систем управления Анализируя функции программной и адаптивной систем управления роботом, решающим рассматриваемую задачу, ... роботом и другие подобные факторы, требующие адаптации управляющей системы, т.е. самонастройки и приспособления к реальным условиям эксплуатации. Реакция системы управления проявляется в изменении структуры, параметров, а иногда и алгоритма ...
2.2 Датчики робота В данной работе в качестве дальномеров мы будем использовать ультразвуковые или инфракрасные датчики. Стоит отметить, что никакой разницы какие датчики использовать для большинства алгоритмов SLAM нет, однако стоить учитывать, что при использовании ультразвуковых датчиков получаемые значения могут быть серьезно искажены [26].
Это может вносить определенные проблемы в работу алгоритма SLAM. Также стоит отметить, что в большинстве алгоритмов SLAM в качестве датчиков определения расстояния, используются датчики, позволяющие за одну итерацию цикла алгоритма сформировать двумерную карту окружающего пространства, такие как лидар, кинект и прочие. Эти датчики отличаются высокой эффективности для задачи SLAM так, как они позволяют отслеживать положения множества ориентиров, но минус этих датчиков заключается в высокой стоимости из-за чего их применение ограничено. Из этого следует, что при использовании обычных датчиков мы столкнемся с несколькими проблемами: а) При использовании обычных жестко закрепленных датчиков мы будем получать не более одного значения с одного датчика, а возможно и вообще ни одного (если, например препятствия будут находиться вне диапазона датчиков), из-за чего нам придётся полагаться только на данные одометров и как следствие это будет приводить к накоплению ошибок в определении положения робота. б) Большинство алгоритмов SLAM в качестве ориентиров используют так называемые особые точки: углы, выступы, некоторые характерны объекты на местности и др. При использовании обычных датчиков такой подход невозможен, что приводит к необходимости использовать другие подходы, например сетчатую карту. 29
СПИСОК ИСПОЛЬЗОВАННЫХ ИСТОЧНИКОВ 1. A new approach to linear filtering and prediction problems / R. E. Kalman // Journal of Basic Engineering. – 1960. – №2. – C. 35 – 45. 2. Welch, G. An Introduction to the Kalman Filter : учеб. пособие / G. Welch, G. Bishop // Department of Computer Science University of North Carolina. – M. : 2006. – 35. 3. Enhanced SLAM for a Mobile Robot using Extended Kalman Filter / K.S. Choi, and S.J. Lee // International Journal of Precision Engineering and Manufacturing. – 2010. – №2. – C. 255–264. 4. О некоторых особенностях применения не доопределённых моделей в робототехнике / В.Э. Карпов // V Международная научно-практическая конференция «Интегрированные модели и мягкие вычисления в искусственном интеллекте». – М., – 2009. – Физматлит. – C. 520-532. 5. An EKF SLAM Algorithm for Mobile Robot with Sensor Bias Estimation / X. Xie, Y. Yu, X. Lin, C. Sun // 2016 IEEE International Conference. – M., – 2016. – C. 281 – 285. 6. Адаптация фильтра Калмана для использования с локальной и глобальной системами навигации / А.Н. Забегаев, В.Е. Павловский // XII национальная конференция по искусственному интеллекту с международным участием. – М., – 2010. – Физматлит. – C. 399-404. 7. Zunino, G. Simultaneous Localization and Mapping for Navigation in Realistic Environments : учеб. пособие / G. Zunino – M. : KTH, 2002. – 187 c. 8. Riisgaard, S. SLAM for Dummies : учеб. пособие / S. Riisgaard, M.R. Blas. – M. : 2002. – 127 p. 9. Mobile Robot: SLAM Implementation for Unknown Indoor Environment Exploration, / M. Emharraf, M. Bourhaleb, M. Saber, M. Rahmoun // Journal of Computer Science. – 2016. – № 2. – C. 106 –112. 46
Рекламна робота страхової компанії
... для страхової компанії. Об'єктом дослідження проекту є страхової компанії, діюча у сфері уже виповнилося 15 років. Дипломна робота складається ... Факти"), іноземні корпорації Chupa Chups holding (Іспанія), CORIS International (Франція) та інших. РЕСО-Гарантия - член Всеросійського ... оцінка рекламну діяльність даної компанії. У третій главі розробляється безпосередньо сама рекламна програму і оцінюється ...
10. FastSLAM: A factored solution to the simultaneous localization and mapping problem with unknown data association / M. Montemerlo, S. Thrun, D. Koller, B. Wegbreit. // Proceedings of the AAAI National Conference on Artificial Intelligence. – M., – 2002. – С. 593-598. 11. FastSLAM 2.0: An Improved Particle Filtering Algorithm for Simultaneous Localization and Mapping that Provably Converges / M. Montemerlo, S. Thrun // 12. Simultaneous localization and mapping with unknown data association using FastSLAM / M. Montemerlo, S. Thrun. // In Robotics and Automation (ICRA), 2003 IEEE International Conference on. IEEE. – М., – 2003. – C. 234 – 238. 13. DP-SLAM: Fast, Robust Simultaneous Localization and Mapping Without Predetermined Landmark / A. Eliazar, R. Parr // Proceedings of the Eighteenth International Joint Conference on Artificial Intelligence. – М., – 2003. – C. 345– 353. 14. DP-SLAM 2.0 / A. Eliazar, R. Parr // 2004 IEEE International Conference. – M., – 2004. – C. 1314–1320. 15. A Flexible and Scalable SLAM System with Full 3D Motion Estimation / S. Kohlbrecher, O. von Stryk, J. Meyer, U. Klingauf. // Safety, Security, and RescueRobotics (SSRR), 2011 IEEE International Symposium. – М., – 2011. – P. 155-160. 16. 2D SLAM Quality Evaluation Methods / A. Filatov, A. Filatov, K. Krinkin // Proceeding of the 21st conference of fruct association. –2017. – С. 120-125. 17. Построение карты мобильным роботом, оснащенным лазерным дальномером, методом рекуррентной фильтрации / С.Л. Зенкевич, А.А. Минин // Мехатроника, автоматизация, управление. – М., – 2007. – № 8. – С. 5-12. 18. A comparison of slam algorithms based on a graph of relations. / W. Burgard, C. Stachniss, G. Grisetti, B. Steder, R. Kmmerle, C. Dornhege, M. Ruhnke, A. Kleiner, and J. D. Tards // 2009 IEEE International Conference on Intelligent Robots and Systems. – М., – 2009. – C. 2089–2095. 47
19. Tinyslam improvements for indoor navigation. In Multisensor Fusion and Integration for Intelligent Systems (MFI) / A. Huletski, D. Kartashov, K. Krinkin // 2016 IEEE International Conference. – М., – 2016. – C. 493–498. 20. Comparison of methods to efficient graph slam under general optimization framework. In Automation (YAC) / H. Li, Q. Zhang, and D. Zhao // 2017 32nd Youth Academic Annual Conference of Chinese Association. –2017. – C. 321–326. 21. An object-based semantic world model for long-term change detection and semantic querying. In Intelligent Robots and Systems (IROS) / J. Mason and B. Marthi // 2012 IEEE/RSJ International Conference on. – М., – 2012. – C. 3851–3858. 22. An evaluation of 2d slam techniques available in robot operating system. / J. M. Santos, D. Portugal, and R. P. Rocha // In 2013 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR).
– М., – 2013. – C. 1-6. 23. Real-Time Loop Closure in 2D LIDAR SLAM, in Robotics and Automation (ICRA) / W. Hess, D. Kohler, H. Rapp, D. Andor // 2016 IEEE International Conference. – M., – 2016. – С. 1271–1278. 24. A Visual Landmark Recognition System for Topological Navigation of Mobile Robot / M. Mata, J.M. Armingol, A. Escalera, and M.A. Salics // International Conference on Robotics and Automation. – М., – 2001. –C. 1124-1129. 25. Natural Corners Extraction Algorithm in 2D Unknown Indoor Environment with Laser Sensor / R.J. Yan, J. Wu, W.J. Wang // International Conference on Control, Automation and Systems. – М., – 2012. – C. 983-987. 26. Natural Corners-based SLAM in Unknown Indoor Environment / R.J. Yan, J. Wu, S.J. Lim, J.Y. Lee, and C.S. Han // International Conference on Ubiquitous Robots and Ambient Intelligence. – М., – 2012. – C. 259-261. 27. Алгоритм локальной навигации и картографии для бортовой системы управления автоматического мобильного робота / Р.В. Кучерский, C.В. Манько // In 2013 IEEE International Symposium on Safety, Security, and Rescue Robotics (SSRR).
Информатика программирование : Разработка автоматизированной ...
... ООО «Анелик», являющегося объектом исследования в данной работе. Задачи, которые стояли при изложении данного дипломного проекта: Определить понятие корпоративной информационной системы; Определить ... и методологии проектирования автоматизированных систем управления документооборотом. В процессе подготовки дипломного проекта был изучен широкий спектр специальной литературы, посвященной вопросам ...
– М., –2013 – C. 13-22. 48
СОДЕРЖАНИЕ АННОТАЦИЯ ……………………………………………………………………………………….. 3 ОБОЗНАЧЕНИЯ И СОКРАЩЕНИЯ …………………………………………………….. 5 ВВЕДЕНИЕ …………………………………………………………………………………………… 6 1 ОБЗОР СУЩЕСТВУЮЩИХ МЕТОДОВ …………………………………………… 8 1.1 Актуальность и новизна ……………………………………………………………….. 8 1.2 Классификация алгоритмов SLAM ……………………………………………….
9 1.2.1 Фильтр Калмана ……………………………………………………………………… 9 1.2.2 Расширенный фильтр Калмана …………………………………………….. 14 1.2.3 Фильтр частиц ………………………………………………………………………. 16 1.2.4 Методы основанные на графах ………………………………………………. 24 1.3 Выводы по первой главе ……………………………………………………………… 26 2 ОПИСАНИЕ РАЗРАБОТАННОГО АЛГОРИТМА SLAM ………………… 27 2.1 Модель объекта управления ………………………………………………………..
27 2.2 Датчики робота……………………………………………………………………………. 29 2.3 Карта пространства …………………………………………………………………….. 30 2.4 Фильтр частиц…………………………………………………………………………….. 32 2.4.1 Движение ……………………………………………………………………………….. 35 2.4.2 Измерение ……………………………………………………………………………… 35 2.4.3 Отсев ……………………………………………………………………………………… 36 2.5 Выводы по второй главе ……………………………………………………………… 36 3 МОДЕЛИРОВАНИЕ РАБОТЫ АЛГОРИТМА …………………………………. 38 3.1 Требования к моделированию …………………………………………………….. 38 3.1 Результаты моделирования …………………………………………………………. 40 3.2 Выводы по третей главе ………………………………………………………………. 44 ЗАКЛЮЧЕНИЕ …………………………………………………………………………………… 45 СПИСОК ИСПОЛЬЗОВАННЫХ ИСТОЧНИКОВ ……………………………… 46 ПРИЛОЖЕНИЕ А ……………………………………………………………………………….. 51 50
Фролова Ксения. по теоретической механике
... math]M = gamma*trianglevarphi[/math] [math]T*h = M = gamma*trianglevarphi[/math] [math]T = gamma*frac{trianglevarphi}{h}[/math] [math]F = 2*T*cosbeta = 2*gamma*frac{trianglevarphi}{h}*cosbeta[/math] Геометрия Найдем [math]anglebeta[/math] , а точнее, [math]cosbeta[/math] : По ... за растяжимую нить [math]anglebeta[/math] [math]T = c*triangle p = c*(p - p_0)[/math] Для вычисления длины нити в ...
Листинг класса SLAM import Odometry import Mapping import Pioneer import ParticleFilter import math import numpy as np class SLAM: # Инициализация объекта класса def __init__(self, numParticle=15, width=600, height=600, coordinatesRobotX=0, coordinatesRobotY=0, orientationRobot=0): # Параметры карты self.widthMap = width # Ширина карты self.heightMap = height # Высота карты self.Map = np.zeros(self.widthMap * self.heightMap).reshape(self.widthMap, self.heightMap) # инициализация Массива (Нулями) # Количество частицы self.numParticle = numParticle # Кол.частиц # Инициализация объектов self.odometer = Odometry.Odometry(0.095, 0.1655, 0, 0, 0) # Инициализация Одометра self.mapping = Mapping.Mapping(self.widthMap, self.heightMap) # Инициализация Карты self.robot = Pioneer.Pioneer() # Инициализация Робота self.particleFilter = ParticleFilter.ParticleFilter() # Инициализация Фильтра # Координаты робота self.coordinatesRobotX = coordinatesRobotX # Начальная координата X self.coordinatesRobotY = coordinatesRobotY # Начальная координата Y self.orientationRobot = orientationRobot # Начальный угол робота # Массив препятсвий self.massObstacle = [] # Массив препятствий self.massCoordinate = [] # Maссив координат # Создание файла для записи self.fileResults = open(‘Results.csv’, ‘w’) # Создание файла self.fileResults.close() # Закрытие файла # Счетчик self.j = 0 # Обновление карты def update(self, N): # Считывание данных с робота (Положение колес, координаты и ореинтация робота и т.д.) coorX, coorY, orientation, distance1, sensorOrientation1, distance2, sensorOrientation2, orientation1, orientation2 = self.robot.motion() coorRobotX, coorRobotY, orientationRobot = self.odometer.calculationOdometers(orientation1, orientation2) orientationRobot = orientationRobot % (2 * math.pi) orientation = orientation % (2 * math.pi) # Обновление показаний датчика if self.j == 5: self.j = 0 self.robot.stop() # Остановка робота self.scanning(N) # Сканирование карты self.coordinatesRobotX, self.coordinatesRobotY, self.orientationRobot = self.particleFilter.localization( coorRobotX, coorRobotY, orientation, self.coordinatesRobotX, self.coordinatesRobotY, self.orientationRobot, self.massCoordinate) 52
По информатике «Роботы, проводящие операции»
... в каких областях медицины и с какими целями применяются роботы; рассмотреть медицинские модели роботов. Первый хирургический робот Unimate Puma 560 был создан в конце 1980-х в ... предотвращают их. Современные военные учения максимально приближены к условиям реальности, благодаря роботам, имитирующим противника. Роботы для военных учений не отличаются стильным дизайном, но достаточно хорошо имитируют ...
self.coordinatesRobotX = self.rsch(coorX, self.coordinatesRobotX) self.coordinatesRobotY = self.rsch(coorY, self.coordinatesRobotY) self.mapping.rendering(coorX, coorY, orientation, self.coordinatesRobotX, self.coordinatesRobotY, self.orientationRobot, self.massObstacle) self.odometer.updateOfCoordinates(self.coordinatesRobotX, self.coordinatesRobotY, self.orientationRobot) # Работа с файлом self.fileResults = open(‘Results.csv’, ‘a’) # открытие файла для записи результатов self.fileResults.write(str(coorX) + «;» + str(coorY) + «;» + str(orientation) + «;;» + str( self.coordinatesRobotX) + «;» + str(self.coordinatesRobotY) + «;» + str(self.orientationRobot) + «\n») self.fileResults.close() self.j = self.j + 1 # Сканирование пространства def scanning(self, N): self.massObstacle = [] self.massCoordinate = [] for _ in range(N): dist1, orien1, dist2, orien2 = self.robot.scanning() if dist1 != None: self.massObstacle.append(True) self.massObstacle.append(dist1) self.massObstacle.append(orien1) # Обнуление массива a, b = self.calculation(True, dist1, orien1) self.massCoordinate.append(0) self.massCoordinate.append(0) self.massCoordinate.append(a) self.massCoordinate.append(b) if dist2 != None: self.massObstacle.append(False) self.massObstacle.append(dist2) self.massObstacle.append(orien2) a, b = self.calculation(False, dist2, orien2) self.massCoordinate.append(0) self.massCoordinate.append(0) self.massCoordinate.append(a) self.massCoordinate.append(b) # Отрисовка в начальный момент if self.triger == True: self.triger = False self.mapping.rendering(0, 0, 0, self.massCoordinate) # Расчет данных def calculation(self, bol, dist, orien): if bol: return self.coordinatesRobotX + 9.9 * math.cos( math.pi / 4 + self.orientationRobot) + dist * math.cos( self.orientationRobot + orien), self.coordinatesRobotY + 9.9 * math.sin( -math.pi / 4 — self.orientationRobot) + dist * math.sin( -self.orientationRobot — orien) else: return self.coordinatesRobotX + 9.9 * math.cos( -math.pi / 4 + self.orientationRobot) + dist * math.cos( self.orientationRobot — orien), self.coordinatesRobotY — 9.9 * math.sin( -math.pi / 4 + self.orientationRobot) — dist * math.sin( self.orientationRobot — orien) 53
«Основы программирования в JS»
... кнопки Отмена – значение false. JavaScript Рассмотрим математические константы, которые являются свойствами объекта Math: Math. e – основание натурального логарифма. Примерно равно 2.72. Math. pi – значение числа " ... аргумента. Значение вычисленного угла в радианах. Math .atan2 – арктангенс вектора, заданного двумя аргументами x и y . JavaScript Рассмотрим синтаксис данных функций и результат ...
Листинг класса Pioneer: import math import vrep import sys import Mapping import Odometry class Pioneer: def __init__(self, ID=True): # Задание основных параметров и переменных роботов self.ID = ID self.noDetectionDist = 0.25 self.maxDetectionDist = 0.1 vrep.simxFinish(-1) self.clientID = vrep.simxStart(‘127.0.0.1’, 19999, True, True, 5000, 5) self.pioneerUltrasonicSensor = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] self.usensors = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] self.braitenbergL = [-0.2, -0.4, -0.6, -0.8, -1, -1.2, -1.4, -1.6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] self.braitenbergR = [-1.6, -1.4, -1.2, -1, -0.8, -0.6, -0.4, -0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] self.v0 = 2 self.orientationSensor1 = 0 self.orientationSensor2 = 0 self.leftWheelOrientation = 0 self.rightWheelOrientation = 0 self.angleOfRotation = 5 # угол поворота датчика self.angleValue = 0 # текущий угол датчика self.lazerSensor = 0 self.odometer = Odometry.Odometry(0.095, 0.1655, 0, 0, 0) if self.clientID != -1: print(«Connected to remote server») else: # self.mapp.plottingPlane() print(‘Connection not successful’) sys.exit(‘Could not connect’) # Задание дискрипторов errorCode, self.pioneer_p3dx = vrep.simxGetObjectHandle(self.clientID, ‘Pioneer_p3dx’, vrep.simx_opmode_oneshot_wait) errorOrientation, self.orientationPieneer = vrep.simxGetObjectOrientation(self.clientID, self.pioneer_p3dx, -1, vrep.simx_opmode_streaming) errorCode, self.left_motor_handle = vrep.simxGetObjectHandle(self.clientID, ‘Pioneer_p3dx_leftMotor’, vrep.simx_opmode_oneshot_wait) errorCode, self.right_motor_handle = vrep.simxGetObjectHandle(self.clientID, ‘Pioneer_p3dx_rightMotor’, vrep.simx_opmode_oneshot_wait) errorCode, self.motorSensor1 = vrep.simxGetObjectHandle(self.clientID, ‘Pioneer_p3dx_leftMotor0’, vrep.simx_opmode_oneshot_wait) errorCode, self.motorSensor2 = vrep.simxGetObjectHandle(self.clientID, ‘Pioneer_p3dx_rightMotor0’, vrep.simx_opmode_oneshot_wait) for _ in range(16): errorCode, self.pioneerUltrasonicSensor[_] = vrep.simxGetObjectHandle(self.clientID, ‘Pioneer_p3dx_ultrasonicSensor’ + str( _ + 1), vrep.simx_opmode_oneshot_wait) errorCode, self.Sensor1 = vrep.simxGetObjectHandle(self.clientID, ‘Proximity_sensor’, vrep.simx_opmode_oneshot_wait) errorCode, self.Sensor2 = vrep.simxGetObjectHandle(self.clientID, ‘Proximity_sensor0’, vrep.simx_opmode_oneshot_wait) # Ининциализация ультрозвуковых датчиков for _ in range(16): self.usensors[_] = vrep.simxReadProximitySensor(self.clientID, self.pioneerUltrasonicSensor[_], vrep.simx_opmode_blocking) 54
self.distance1 = vrep.simxReadProximitySensor(self.clientID, self.Sensor1, vrep.simx_opmode_blocking) self.distance2 = vrep.simxReadProximitySensor(self.clientID, self.Sensor2, vrep.simx_opmode_blocking) # Вывод ошибки if errorCode == -1: print(‘Can not find Pioneer’) sys.exit() def motion(self): # Определение позиции робота returnCode, self.positionPioneer = vrep.simxGetObjectPosition(self.clientID, self.pioneer_p3dx, -1, vrep.simx_opmode_oneshot_wait) self.detect = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] # Задание движения self.ultrasonicSensor() for _ in range(16): if self.usensors[_][0] == 0 and self.usensors[_][1] != False: dist = self.distanceRobot(self.usensors[_][2][0], self.usensors[_][2][1]) if (dist < self.noDetectionDist): if dist < self.maxDetectionDist: dist = self.maxDetectionDist self.detect[_] = 1 — ( (dist — self.maxDetectionDist) / (self.noDetectionDist — self.maxDetectionDist)) else: self.detect[_] = 0 b, self.orientationPieneer = vrep.simxGetObjectOrientation(self.clientID, self.pioneer_p3dx, -1, vrep.simx_opmode_buffer) self.vLeft = self.v0 self.vRight = self.v0 for _ in range(16): self.vLeft = self.vLeft + self.braitenbergL[_] * self.detect[_] self.vRight = self.vRight + self.braitenbergR[_] * self.detect[_] if self.angleValue <= -45: self.angleOfRotation = 1 elif self.angleValue >= 135: self.angleOfRotation = -1 self.angleValue = self.angleValue + self.angleOfRotation error = vrep.simxSetJointTargetPosition(self.clientID, self.motorSensor1, self.angleValue * math.pi / 180, vrep.simx_opmode_streaming) error = vrep.simxSetJointTargetPosition(self.clientID, self.motorSensor2, -self.angleValue * math.pi / 180, vrep.simx_opmode_streaming) b, self.orientationSensor1 = vrep.simxGetJointPosition(self.clientID, self.motorSensor1, vrep.simx_opmode_streaming) b, self.orientationSensor2 = vrep.simxGetJointPosition(self.clientID, self.motorSensor1, vrep.simx_opmode_streaming) error, self.leftWheelOrientation = vrep.simxGetJointPosition(self.clientID, self.left_motor_handle, vrep.simx_opmode_streaming) error, self.rightWheelOrientation = vrep.simxGetJointPosition(self.clientID, self.right_motor_handle, vrep.simx_opmode_streaming) errorCode = vrep.simxSetJointTargetVelocity(self.clientID, self.left_motor_handle, self.vLeft, vrep.simx_opmode_streaming) errorCode = vrep.simxSetJointTargetVelocity(self.clientID, self.right_motor_handle, self.vRight, vrep.simx_opmode_streaming) if error == -1 or returnCode != 0: print(‘Can not find left or right motor’) 55
sys.exit() self.distance1 = vrep.simxReadProximitySensor(self.clientID, self.Sensor1, vrep.simx_opmode_blocking) self.distance2 = vrep.simxReadProximitySensor(self.clientID, self.Sensor2, vrep.simx_opmode_blocking) return [self.positionPioneer[0] * 50, self.positionPioneer[1] * 50, self.orientationPieneer[2], self.distance(self.distance1[2][2] * 50, self.distance1[2][0] * 50, self.distance1[1]), self.orientationSensor1, self.distance(self.distance2[2][2] * 50, self.distance2[2][0] * 50, self.distance2[1]), self.orientationSensor2, self.leftWheelOrientation, self.rightWheelOrientation] def ultrasonicSensor(self): for _ in range(16): self.usensors[_] = vrep.simxReadProximitySensor(self.clientID, self.pioneerUltrasonicSensor[_], vrep.simx_opmode_streaming) def stop(self): errorCode = vrep.simxSetJointTargetVelocity(self.clientID, self.left_motor_handle, 0, vrep.simx_opmode_streaming) errorCode = vrep.simxSetJointTargetVelocity(self.clientID, self.right_motor_handle, 0, vrep.simx_opmode_streaming) def scanning(self): if self.angleValue <= -45: self.angleOfRotation = 1 elif self.angleValue >= 135: self.angleOfRotation = -1 self.angleValue = self.angleValue + self.angleOfRotation error = vrep.simxSetJointTargetPosition(self.clientID, self.motorSensor1, self.angleValue * math.pi / 180, vrep.simx_opmode_streaming) error = vrep.simxSetJointTargetPosition(self.clientID, self.motorSensor2, -self.angleValue * math.pi / 180, vrep.simx_opmode_streaming) b, self.orientationSensor1 = vrep.simxGetJointPosition(self.clientID, self.motorSensor1, vrep.simx_opmode_streaming) b, self.orientationSensor2 = vrep.simxGetJointPosition(self.clientID, self.motorSensor1, vrep.simx_opmode_streaming) self.distance1 = vrep.simxReadProximitySensor(self.clientID, self.Sensor1, vrep.simx_opmode_blocking) self.distance2 = vrep.simxReadProximitySensor(self.clientID, self.Sensor2, vrep.simx_opmode_blocking) return [self.distance(self.distance1[2][2] * 50, self.distance1[2][0] * 50, self.distance1[1]), self.orientationSensor1, self.distance(self.distance2[2][2] * 50, self.distance2[2][0] * 50, self.distance2[1]), self.orientationSensor2] def distance(self, a, b, bo): if bo == True: c = (a ** 2 + b ** 2) ** 0.5 if c > 1000: return None elif c <= 30: return None else: return c else: return None def distanceRobot(self, a, b): c = (a ** 2 + b ** 2) ** 0.5 if c >= 1000: return 0 elif c <= 30: return 0 56
if (belief > -666666 and belief < 0): red = 255 blue = 255 green = 255 if (belief == 0.0): red = 170 blue = 170 green = 170 if (belief > 30.0): red = 0 blue = 0 green = 0 self.img[k][l] = [red, blue, green, 255] pilImage = Image.fromarray(self.img, ‘RGBA’) pilImage.save(‘mapping’ + ‘.png’) return (x, y, theta) # def computeWeight(self, particlesObj, k, sensorReadings): c_x, c_y, theta = particlesObj.X[k], particlesObj.Y[k], particlesObj.Theta[k] weight = 0.0 for k in range(0, len(sensorReadings), 2): x1 = float(sensorReadings[k]) y1 = float(sensorReadings[k+1]) x0, y0 = 0.0, 0.0 d = sqrt(pow((x0 — x1), 2) + pow((y0 — y1), 2)) alpha1 = atan2(y1, x1) G_x1 = d * cos(theta + alpha1) + c_x G_y1 = d * sin(theta + alpha1) + c_y x0 = c_x y0 = c_y d0 = sqrt(pow((x0 — c_x), 2) + pow((y0 — c_y), 2)) alpha0 = atan2(y0, x0) G_x0 = d0 * cos(theta + alpha0) + c_x G_y0 = d0 * sin(theta + alpha0) + c_y t = atan2((G_y1 — G_y0), (G_x1 — G_x0)) d = sqrt(pow(G_x1 — G_x0, 2) + pow(G_y1 — G_y0, 2)) x = int(round(G_x1 / 1)) y = int(round(G_y1 / 1)) x += int(self.sizeMap / (self.sizeCell * 6)) y += int(self.sizeMap / (self.sizeCell * 2)) ON = 0 if (x >= 0 and x < int(self.sizeMap / self.sizeCell) and y >= 0 and y < int(self.sizeMap / self.sizeCell)): for q in [-1, 0, 1]: for r in [-1, 0, 1]: if (x + r > 0 and x + r < len(self.Grid) and y + q > 0 and y + q < len(self.Grid)): if (self.Grid[x + r][y + q] > 600.0): weight += (12.0) ON = 1 OUTSIDE = 0 if (ON == 0): for K in range(int(d * 1.0)): k = K / 1.0 x = int((G_x0 + ((G_x1 — G_x0) / fabs(G_x1 — G_x0)) * fabs(k * cos(t))) / self.sizeCell) 58
y = int((G_y0 + ((G_y1 — G_y0) / fabs(G_y1 — G_y0)) * fabs(k * sin(t))) / self.sizeCell) x += int(self.sizeMap / (self.sizeCell * 6)) y += int(self.sizeMap / (self.sizeCell * 2)) if (x >= 0 and x < int(self.sizeMap / self.sizeCell) and y >= 0 and y < int(self.sizeMap / self.sizeCell) and k > 0): if (self.Grid[x][y] > 600.0): weight += (4.0) OUTSIDE = 1 break if (OUTSIDE == 0): weight += 8.0 return weight def boundAngle(self, phi): from math import fmod, pi # Bound angle to [-pi,pi] if (phi >= 0): phi = fmod(phi, 2 * pi) else: phi = fmod(phi, -2 * pi) if (phi > pi): phi -= 2 * pi if (phi <
- pi): phi += 2 * pi return phi Листинг класса Particle: from random import gauss, random from math import * class Particle: # Инициализация частицы def __init__(self, numParticles=50, x=0, y=0, theta=0): self.random = random self.X = [x for k in range(numParticles)] self.Y = [y for k in range(numParticles)] self.Theta = [theta for k in range(numParticles)] self.Weight = [(1.0 / numParticles) for k in range(numParticles)] self.numParticles = numParticles self.gauss = gauss self.random = random self.cos = cos self.sin = sin # Координаты частиц X # Координаты частиц Y # Ориентиры частиц # Веса частиц # Количество частиц # Функция Гаусса # образец def resampleParticles(self): weightArr = [0 for k in range(self.numParticles)] # Создание массива весов for k in range(self.numParticles): weightArr[k] = sum(self.Weight[:(k + 1)]) tempX = [0 for k in range(self.numParticles)] tempY = [0 for k in range(self.numParticles)] tempTheta = [0 for k in range(self.numParticles)] tempWeight = [1.0 / self.numParticles for k in range(self.numParticles)] for j in range(self.numParticles): sample = self.random() for k in range(self.numParticles): if (sample <
- weightArr[k]): tempX[j], tempY[j], tempTheta[j], tempWeight[j] = self.X[k], self.Y[k], self.Theta[k], self.Weight[ 59
self.widthMap = width self.heightMap = height self.size = (width, height) # Ширина Карты # Высота Карты # Параметры Карты self.Map = np.zeros(self.widthMap * self.heightMap).reshape(self.widthMap, self.heightMap) # инициализация Массива (Нулями) self.img = np.zeros((self.widthMap, self.heightMap, 4), np.uint8) self.white = 255, 255, 255 self.dlinX = int(width / 2) self.dlinY = int(height / 2) # Определяемое положение робота self.startPositionX = int(width / 2) # Начальное положение X self.startPositionY = int(height / 2) # Начальное положение Y self.robotOrientation = 0 # Начальное направение self.realPositionX = int(width / 2) # Координата робота X (реальная) self.realPositionY = int(height / 2) # Координата робота Y (реальная) self.robotRealOrientation = 0 # Ориентир робота (реальный) self.odometerPositionX = int(width / 2) # Координата робота X (расчитанная) self.odometerPositionY = int(height / 2) # Координата робота Y (расчитанная) self.robotOdometerOrientation = 0 # Ориентир робота (расчитанный) # Инициализация дисплея в PyGame pygame.init() pygame.display.set_caption(«Результаты моделирования») self.screen = pygame.display.set_mode(self.size) self.image = pygame.image.load(«mapp.png») self.screen.fill(self.white) # ————————————————————-# Отрисовка препятствий pygame.draw.line(self.screen, (0, 255, 0), (50, 50), (550, 50)) pygame.draw.line(self.screen, (0, 255, 0), (50, 50), (50, 550)) pygame.draw.line(self.screen, (0, 255, 0), (50, 550), (550, 550)) pygame.draw.line(self.screen, (0, 255, 0), (550, 50), (550, 550)) pygame.draw.line(self.screen, (0, 255, 0), (350, 50), (350, 250)) pygame.draw.line(self.screen, (0, 255, 0), (350, 250), (550, 250)) pygame.draw.line(self.screen, (0, 255, 0), (250, 550), (250, 450)) pygame.draw.line(self.screen, (0, 255, 0), (100, 150), (100, 550)) pygame.draw.line(self.screen, (0, 255, 0), (50, 150), (150, 150)) pygame.draw.line(self.screen, (0, 255, 0), (150, 50), (150, 150)) pygame.draw.line(self.screen, (0, 255, 0), (480, 550), (550, 480)) pygame.image.save(self.screen, «mapp.png») # Инициализация массивов self.positionsRobot= [] # кодировка цветов на карте self.white = [255, 255, 255, 255] self.black = [0, 0, 0, 255] self.red = [255, 0, 0, 255] self.green = [0, 255, 0, 255] self.blue = [0, 0, 255, 255] self.x = int(width / 2) self.y = int(height / 2) self.x1 = int(width / 2) self.y1 = int(height / 2) self.pdatchik = 19.8/2 # Позиция робота # Кодировка отсутствие препятствия # Кодировка препятсвий # Кодировка траектории по SLAM # Кодировка реальных объектов # Кодировка реальной траектории # 61
# Обновление карты def update(self, coorRX, coorRY, orienR, coorOX, coorOY, orienO): # Положение робота (реальное) self.realPositionX = coorRX + self.dlinX self.realPositionY = -coorRY + self.dlinY self.robotRealOrientation = orienR # реальныое значение X # реальные значения Y # направление движения робота # Положение робота (по одометру (или одометр + SLAM) ) self.odometerPositionX = coorOX + self.dlinX self.odometerPositionY = -coorOY + self.dlinY self.robotOdometerOrientation = orienO % (math.pi * 2) # Координата робота X (рассчитанная) # Координата робота Y (рассчитанная) # Ориентация робота (рассчитанная) # Сканирование пространства def scanning(self, distanse1, sensorOrientation1, distanse2, sensorOrientation2): # Показание первого датчика self.distanse1 = distanse1 self.sensorOrientation1 = sensorOrientation1 # Показания второго датчика self.distanse2 =distanse2 self.sensorOrientation2 = sensorOrientation2 # Отрисовка def rendering(self, x, y, phi, x1, y1, phi1, mass): massObstracle =[] x = x + 300 y = -y + 300 x1 = x1 + 300 y1 = -y1 + 300 self.image = pygame.image.load(«mapp.png») for event in pygame.event.get(): if event.type == pygame.QUIT: run = False self.screen.blit(self.image, (600, 600)) pygame.draw.line(self.screen, (0, 0, 255), (self.x, self.y), (x, y)) pygame.draw.line(self.screen, (255, 0, 0), (self.x1, self.y1), (x1, y1)) for i in range(0, len(mass), 3): if mass[i] == True: pygame.draw.rect(self.screen, (0, 0, 0), (int(x1 + self.pdatchik * math.cos(math.pi / 4 + phi1) + mass[i + 1] * math.cos( phi1 + mass[i + 2])), int(y1 + self.pdatchik * math.sin(-math.pi / 4 — phi1) + mass[i + 1] * math.sin( -phi1 — mass[i + 2])), 1, 1)) if mass[i] == False: pygame.draw.rect(self.screen, (0, 0, 0), (int(x1 + self.pdatchik * math.cos(-math.pi / 4 + phi1) + mass[i + 1] * math.cos( phi1 — mass[i + 2])), int( y1 — self.pdatchik * math.sin(-math.pi / 4 + phi1) — mass[i + 1] * math.sin( phi1 — mass[i + 2])), 1, 1)) self.x = x self.y = y self.x1 = x1 self.y1 = y1 pygame.image.save(self.screen, «mapp.png») pygame.display.flip() Листинг класса Odometry import math class Odometry: «»» Класс отвечающий за реализацию расчета координат на основе одометров «»» 62
def __init__(self, r=0.095,l=0.1655, startOdometerLeft=0, startOdometerRight=0, starOrientationRobotsPhi=0): self.radius = r self.length = l self.diameter = r + r self.width = l + l # радиус колес # длина половины расстояния между колесами # диаметр колеса робота # ширина робота self.coordinatesRobotX = 0 self.coordinatesRobotY = 0 self.orientationRobotPhi = 0 # обновленная координата робота X # обновленная координата робота Y # обновленный угол self.oldOdometerLeft = startOdometerLeft self.oldOdometerRight = startOdometerRight # начальные показания левого одометра # начальные показания правого одометра # Определение координат робота на основе данных с одометров def calculationOdometers(self, odometerLeft, odometerRight): # расчет левого колеса if ((0 <= odometerLeft) and (0 <= self.oldOdometerLeft)) or ((0 >= odometerLeft) and (0 >= self.oldOdometerLeft)): self.odometerLeft = odometerLeft — self.oldOdometerLeft elif (math.pi/2 <= odometerLeft) and (-math.pi/2 >= self.oldOdometerLeft): self.odometerLeft = -(math.pi — odometerLeft) + (math.pi + self.oldOdometerLeft) elif (-math.pi/2 >= odometerLeft) and (math.pi/2 <= self.oldOdometerLeft): self.odometerLeft = (math.pi+odometerLeft)+(math.pi-self.oldOdometerLeft) else: self.odometerLeft = odometerLeft — self.oldOdometerLeft # расчет правого колеса if ((0 <= odometerRight) and (0 <= self.oldOdometerRight)) or ((0 >= odometerRight) and (0 >= self.oldOdometerRight)): self.odometerRight = odometerRight — self.oldOdometerRight elif (math.pi/2 <= odometerRight) and (-math.pi/2 >= self.oldOdometerRight): self.odometerRight = -(math.pi — odometerRight) + (math.pi + self.oldOdometerRight) elif (-math.pi/2 >= odometerRight) and (math.pi/2 <= self.oldOdometerRight): self.odometerRight = (math.pi+odometerRight)+(math.pi-self.oldOdometerRight) else: self.odometerRight = odometerRight — self.oldOdometerRight Dr = math.pi * self.diameter * self.odometerRight/(math.pi*2) Dl = math.pi * self.diameter * self.odometerLeft/(math.pi*2) D = (Dr+Dl)/2 # пройденный путь правого колеса # пройденный путь левого колеса # прйденный путь self.coordinatesRobotX = self.coordinatesRobotX + D * math.cos(self.orientationRobotPhi) # обновленная координата робота X self.coordinatesRobotY = self.coordinatesRobotY + D * math.sin(self.orientationRobotPhi) # обновленная координата робота Y self.orientationRobotPhi = self.orientationRobotPhi + (Dr — Dl)/self.width # обновленная ориентация робота phi self.oldOdometerLeft = odometerLeft self.oldOdometerRight = odometerRight coordinatesRobot = [self.coordinatesRobotX*50, self.coordinatesRobotY*50, self.orientationRobotPhi] return coordinatesRobot # Возврат изменения координат и ориентации def updateOfCoordinates(self, X, Y, Phi): self.coordinatesRobotX = X / 50 self.coordinatesRobotY = Y / 50 self.orientationRobotPhi = Phi 63
