УДК 621.791
СИСТЕМА НАВИГАЦИИ ПРОМЫШЛЕННОГО ТРАНСПОРТНОГО РОБОТА Ю.С. Слепокуров, В.В. Пешков, А.В. Кузовкин
Рассматривается возможность построения навигационной системы промышленного транспортного робота, предназначенного для внутрицеховых транспортных операций, на базе электронного магнитометра
Ключевые слова: транспортный робот, система ориентации, магнитометр, акселерометр
В настоящее время развитие мобильной робототехники стимулируется расширением областей ее применения в деятельности человека, сопряженной в общем случае с риском для его здоровья и жизни. Это определяет актуальность развития методов управления движением мобильных роботов (МР), испытания которых в рамках экспериментов и соревнований проводят на специальных полигонах, оснащенных, например, инфракрасными маяками, поверхностью с контрастной полосой.
Не менее важным является и разработка новых систем навигации транспортных манипуляторов, предназначенных для работы в условиях цехов промышленных предприятий.
При реализации систем контроля подвижных объектов (ПО) в настоящее время с успехом применяют спутниковые системы навигации (СНС) GPS и ГЛОНАСС. В дифференциальном режиме СНС позволяют определять текущие координаты ПО с погрешностью порядка нескольких сантиметров. Однако для этого необходимо иметь возможность пользоваться сигналами стационарных станций наземного сегмента СНС, что далеко не всегда возможно. В автономном режиме приемники СНС дают погрешность в определении координат от 3 до 30 м и более, в зависимости от количества видимых спутников и их положения относительно плоскости горизонта. Эти погрешности имеют характер изменения близкий к «белому» шуму и затрудняют реализацию системы управления движением робота без использования дополнительной информации [1].
Фотоэлектрические датчики наиболее разнообразны по своим характеристикам и сфере применения, однако их принцип работы одинаков. Излучаемый датчиком свет рассеивается, отражается или поглощается объектом, и эти изменения воспринимаются фотоприемником. Благодаря тому, что в последних моделях фотоэлектрических датчиков применяется микропроцессорная обработка сигнала, удалось воплотить новые функции приборов, среди которых - автоматическое обучение в
Cлепокуров Юрий Cергеевич - ВГТУ, канд. техн. наук, доцент, e-mail: [email protected]
Пешков Владимир Владимирович - ВГТУ, д-р техн. наук, профессор, e-mail: peshkov@otsp. vorstu.ru Кузовкин Алексей Викторович - ВГТУ, д-р техн. наук, профессор, тел. (473) 252-27-84
процессе работы. Например, для того чтобы перенастроить датчики контрастных меток серии KT5G производства компании Sick нет необходимости останавливать технологическую линию, как это делалось ранее. Перенастройка прибора происходит в процессе работы. С другой стороны, многие функции датчиков, ранее доступные только для дорогих моделей, в настоящее время стали функционировать и в более дешевых изделиях. Примером тому являются датчики контрастных меток, стоимость которых снизилась в 2 - 3 раза.
Наиболее перспективным направлением развития транспортных промышленных роботов следует считать напольные безрельсовые транспортные роботы, которые обеспечивают значительное снижение капиталовложений на монтаж трассы; устранение загромождения производственных площадей стационарными транспортными устройствами; экономию производственных площадей вследствие совмещения трасс роботов с внутрицеховыми проездами и проходами.
Напольные транспортные роботы движутся: 1) вдоль провода, уложенного на глубине 40 - 60 мм от поверхности пола (по проводу пропускают ток силой в несколько сотен миллиампер, с частотой 2 - 20 кГц и напряжением не более 12 В; создается переменное электромагнитное поле, за которым следят датчики транспортного робота); 2) по светоотражающей полосе, прикрепленной к полу (слежение за трассой проводится с помощью фотосчитывающих датчиков, реагирующих на изменение световых потоков и вырабатывающих управляющие сигналы. Оба способа предполагают движение только по заранее проложенным трассам, что не приемлемо для межцеховых перемещений.
Не так давно на рынке появились сравнительно недорогие электронные компоненты для навигационных устройств - микроэлектромехани-ческие системы (МЭМС). Стоимость МЭМС-датчиков движения от STMicroelectronics (акселерометры и гироскопы) составляет от 250 до 540 рублей. Ассортимент МЭМС-датчиков компании STM преимущественно составляют акселерометры и гироскопы, позволяющие контролировать параметры линейных и угловых перемещений, соответственно. Недавно в ассортименте STM появилась новая оригинальная МЭМС-продукция: трехосевые гироскопы L3G4200D/DH с цифровым выходом и модуль цифрового компаса LSM303DLH.
Появление Ь8М303БЬЫ является ответом на возрастающую потребность в портативной электронной технике потребительского назначения (в т.ч. смарт-фоны и КПК) с расширенными функциональными возможностями навигации. Среди этих возможностей - автоматическая ориентация карты, индикация направления (в т.ч. при отсутствии движения), сохранение функций навигации в условиях неустойчивого или полного отсутствия приема сигналов от систем глобального спутникового позиционирования.
Каналы измерения напряженности магнитного поля предусматривают работу в одном из семи программно-выбираемых диапазонов от ±(0,13...0,81) мТл (ведется разработка модуля для диапазона до 2 мТл). В свою очередь каналы измерения линейных ускорений поддерживают работу в одном из трех программно-задаваемых диапазонов ±2/±4/±8д. Вывод результатов измерений организован в цифровом виде (16-битный формат). Для этого в модуль интегрированы АЦП и два независимых последовательных интерфейса 12С (поддерживаются скоростные режимы 100 и 400 кГц). Модуль предусматривает возможность раздельного перевода в экономичный режим работы трактов магнетометра и акселерометра и генерации двух сигналов прерывания с гибкой программируемой настройкой. Данные прерывания позволяют активизировать управляющий процессор в случае обнаружения движения. Функциональные возможности модуля завершают функции тестирования, которые могут быть инициированы по запросу пользователя раздельно для трактов магнетометра и акселерометра. Столь внушительные возможности модуля реализованы в рамках чрезвычайно компактного 28-выводного корпуса ЬОА с размерами 5х5х1 мм. Его рабочие характеристики гарантированы для диапазонов напряжения питания 2,5...3,3 В и температур -30...85°С.
Акселерометр и магнитометр - абсолютно самостоятельные устройства и имеют разные шины выхода, источники питания и адресацию. Однако внутреннее программное обеспечение интерфейса обмена выполнено так, что для обеспечения функции ориентации даже в одной плоскости считывать придется все шесть регистров - три оси магнетометра и три оси акселерометра. Обмен информацией между электронным компасом и системой управления организуется по интерфейсу 12С. . Физически шина 12С представляет собой две сигнальные линии, одна из которых (БСЬ) предназначена для передачи тактового сигнала, а вторая (ББА) для обмена данными. Для управления линиями применяются выходные каскады с открытым коллектором, поэтому линии шины должны быть подтянуты к источнику питания +5 В через резисторы сопротивлением 1...10 кОм, в зависимости от физической длины линий и скорости передачи данных. Длина соединительных линий в стандартном режиме может достигать 2-х метров, скорость передачи данных - 100 кбит/с.
Работать с устройством очень просто. Обращаемся с ним как с обычной БЕРКОМ. Адрес уст-
ройства на 12с шине = 0*3С, адрес ячейки однобайтный.
У магнитометра есть лишь несколько регистров конфигурации. Полная информация о регистрах управления доступна в технической документации
(ЬИр://'№№^з1:.сотЛп1ете1:/сот/ТЕСН№САЬ_КЕ8 ОШСЕ8/ТЕСЫ№САЬ_иТЕКАТиКЕ/ОАТА8ЫЕ ЕТ/СБ00260288.рё1). Самые необходимые из них для работы с магнетометром:
• СКА_ЯЕС_М - выбор частоты замера и смещения (30Гц).
• СКВ_ЯЕС_М - выбор коэффициента усиления магнитометра.
• МК_ЯЕС_М - режим работы. Спящий, одиночный замер, циклический замер.
• ОиТ_Х_Н_М - Ось Х старший байт
• ОиТ_Х_Ь_М - Ось Х младший байт
• ОиТ_У_Н_М - Ось У старший байт
• ОиТ_У_Ь_М - Ось У младший байт
• OUT_Z_H_M - Ось Z старший байт
• OUT_Z_L_M - Ось Z младший байт
• 8К_ЯЕС_М - Статусный регистр. Позволяет включить внутренний стабилизатор напряжения, а также показывает, заблокированы ли выходные регистры.
• Шх_КЕС_М - Байты идентификации. Вроде серийного номера.
Вначале инициализация. Модуль надо включить и настроить частоту обновления: СЯА_ЯЕО_М = 0х14 - выставляем частоту обновления в 30 Гц
МЯ_ЯЕО_М = 0x00 - переключаем в активный режим, непрерывное преобразование.
Дальше надо лишь читать периодически 6 выходных регистров
• ОиТ_Х_Н_М
• ОиТ_Х^_М
• ОиТ_У_Н_М
• ОиТ_У^_М
• Ои^_Н_М
• ОШ^_Ь_М
Причем если мы начали считывать с любого выходного регистра, то остальные регистры блокируются на запись до тех пор, пока мы не считаем все выходные регистры. Так что считывать надо сразу все шесть. Значения в регистрах знаковые, от -2048 до +2048.
Процедура определения позиции транспортного робота упрощается тем, что он перемещается в одной горизонтальной плоскости, поэтому и результирующее ускорение и азимут направления движения можно определить по двум проекциям. Величину скорости, которая необходима для определения положения робота в пространстве, будем рассчитывать и хранить в системе управления верхнего уровня. Математические выражения, необходимые для расчета положения объекта с из-
вестными начальными координатами по значениям скорости, ускорения и азимута движения, достаточно подробно описаны в теории навигации [2]. Для парковки робота в зоне достижения цели могут быть использованы датчики ближнего контроля. Наша задача - оценить возможность использования прибора в условиях промышленного цеха с большим количеством крупных металлических изделий и электромагнитных возмущений, вызванных работой электрооборудования.
Рассмотрим влияние всех факторов на примере изменения показаний магнитометра, как наиболее уязвимого прибора в цеховых условиях.
Для уменьшения влияния шумов и упрощения программного обеспечения выполним следующее:
• показания регистров магнитометра урезаем до восьмибитного уровня, начиная от старшей единицы. Такое ограничение существенно уменьшает точность показаний (255 точек отсчета против 2024), но уменьшает влияние помех. Для эксперимента этого достаточно, а в системе навигации для уменьшения помех можно использовать фильтр Калмана;
• добавляем смещение к считанным значениям, обеспечивающее переход к целым беззнаковым показаниям в диапазоне 0 ... 255.
Рассмотрим показания магнитометра в двух различных по насыщенности оборудованием местах цеха при вращении датчика со скоростью 45 градусов в секунду: в зоне станков токарной группы (рис.1) и в зоне станков фрезерной группы (рис.2).
Рис. 1. Показания регистров магнитометра в зоне группы токарных станков
Рис. 2. Показания регистров магнитометра в зоне группы фрезерных станков
Что мы наблюдаем в экспериментально снятых характеристиках по сравнению с теоретическими:
• амплитуды синусоид незначительно, но, все же, различаются (ось х - 100, ось у - 110;
• присутствует смещение синусоид по вертикальной оси;
• смещение по горизонтальной оси отсутствует.
Из этого можно сделать вывод о том, что влияние цеховых условий и наличие мощных электрических установок в выключенном состоянии не окажут влияния на точность измерения азимута ориентации транспортного робота с датчиком электронного компаса. Однако для получения точных показаний результаты по осям придется нормировать.
Во втором случае, горизонтальное смещение синусоид практически отсутствует. Незначительно изменились и амплитуды синусоид. Исключение составляет вертикальное смещение синусоид относительно друг друга. На этом графике оно значительно выросло.
Дальнейшие измерения в различных точках цеха и при различных режимах работы электрооборудования станков показали аналогичные зависимости двух синусоид и не выявили заметных искажений в форме сигнала. Исключение составляет измерения вблизи включаемого электропривода постоянного тока.
Итак, мы имеем программу и оборудование, которые могут определить направление и мгновенное ускорение движения. Однако, существует одна проблема - смещение синусоид магнитометра относительно нулевой точки, которое меняется в зависимости от положения робота в цехе и состоянии оборудования. Например, по экспериментальным измерениям для угла поворота 85 градусов значение функции азимута для выключенного и включенного оборудования в одной точке составляют:
Х1=210, У1=100 - аг^(х/у)= агс-
1ап(210/100)=64,5366549381
Х2=235, У2=85 - аг^(х/у)= агс-
1ап(235/85)=70,1148348861.
Как видим, ошибка определения угла ориентации весьма велика.
Для коррекции показания магнитометра обычно предлагают использовать характеристику полного круга вращения для заданной точки. Как правило - это эллипс, коэффициент сжатия осей которого можно использовать для определения поправок показаний прибора.
Предположим, что нам известна величина смещения синусоид. Т.е. мы имеем возможность снять синусоиду в пределах полного поворота магнитометра. В этом случае, те же два значения были бы определены следующим образом:
Х1(0)=115, У1(0)=90 - агйап((210-
115)/(100-90))=83,9909940425
Х1(0)=125, У1(0)=70 - агйап((220-
125)/(80-70))=83,9909940425.
Таким образом, имея только уровень нуля и максимальную амплитуду синусоид (т. е. значения показаний магнетометра при ориентации на север и на запад) можно точно определить азимут ориентации платформы, невзирая на смещение показаний.
Экспериментальные показания магнитометра в разных зонах цеха позволили выделить пять основных областей, в которых существенно меняются показания. Для определения корректирующих показаний в каждой из зон на пневматической поворотной оси располагаем стационарные магнитометры, которые обеспечат систему управления информацией о краевых параметрах показаний магнитометра в данной точке. Управление поворотом и считывание показаний производятся системой управления верхнего уровня. Для упрощения программного обеспечения контроллера мобильной платформы заданием на выбор направления движения могут быть реальные показания магнитометра по осям Х и У, принятые в качестве задания от системы управления верхнего уровня и ве-
личина скорости перемещения (уровень напряжения питания электропривода мобильного робота). Cвязь робота с системой управления верхнего уровня может осуществляться через Wi-Fi точки доступа, распределенные по наиболее открытым местам цеха. Разумеется, что количество и место расположения стационарных «постов» магнитометров будет определяться для каждого конкретного роботизируемого участка.
Литература
1. Алёшин Б.С Ориентация и навигация подвижных объектов: современные информационные технологии / Б.С Алёшин, К.К.Веремеенко, А.И.Черноморский. -М.: ФИЗМАТЛИТ, 2006. - 424 с.
2. Бранец В.Н. Введение в теорию бесплатфор-менных инерциальных навигационных систем / В.Н. Бранец, И.П.Шмыглевский. - М.: Наука, 1992. -280 с.
Воронежский государственный технический университет
NAVIGATION SYSTEM OF INDUSTRIAL TRANSPORT ROBOT
Ju.S. Slepokurov, V.V. Peshkov, A.V. Kuzovkin
Discusses the possibility of industrial transport robot navigation systems construction, designed for intershop transport operations on the basis of electronic magnetometer
Key words: transport robot, orientation system, magnetometer, accelerometer