на карте помещения(квартиры).Трудно научить чему нибудь мобильного робота если он не знает
где находится.Задавшись этой целью я и решил создать такую навигационную систему.
Идея и реализация достаточно просты,но чтобы все заработало мне пришлось довольно много экспериментировать и применять в конечном итоге нетривиальные(для меня) решения.
Суть в определении углов пеленгов с робота на световые маяки установленные в комнате под потолком,работающие в инфракрасном диапазоне и имеющие заранее известные постоянные координаты.По этим углам и определяются координаты робота.
ТТХ системы:максимальная дальность до маяка-6м(можно увеличить путем увеличения мощности излучения светодиодов маяков),погрешность определения координат в комнате размером 4 на 4 метров около 10см,погрешность определения курса робота 1-2 градуса,частота обновления информации 2 гц,размеры мобильной части системы:диаметр 10,5 см,высота с световодами 7,5см.
Для определения координат достаточно трех маяков в помещении расположенных например как
показано на рисунке:
A1,A2-искомые углы между маяками M1,M2 и M2,M3 соответственно.
В качестве маяка применен миниатюрный излучающий ИК 940нм светодиод KA-3528F3C имеющий 120 градусов диаграмму направленности и выдерживающий пиковый ток в 1.2 ампера.Светодиод постоянно работает с частотой задающую вот таким генератором: NE555 Duty Cycle and Frequency Adjustable Module которые дешево предлагаются на ebay.
Делая ремонт в квартире я предусмотрительно разместил светодиод одного из маяков в потолочном плинтусе.
В качестве фотоприемника ИК света взят широко применяемый в системах дистанционного управления
бытовой техники TSOP48.Он обладает хорошей чувствительностью и способен принимать сигналы от мощного светодиода аж до 35 метров. Фотоприемники бывают на 30,33,36,37,38,40 и на 56кГц. Я применил два фотоприемника один на 33 другой на 38 кГц. Дело в том что,чтобы определить от какого из маяков (M1,M2 или M3)принимается сигнал один из маяков должен быть настроен на отличную частоту чем другие.Кстати частоты приемников лучше подбирать другие чем те что установлены в телевизорах или бытовых приборах где применяется дистанционное управление или располагать маяки так чтобы они не светили на них для исключения влияния сигналов от маяков.
фотоприемники установлены в световоды цель которых пропускать сигнал от маяка только когда световод направлен точно на маяк.Световоды с приемниками установлены на вращающуюся от электромотора башню угловое положение которой определяется с помощью энкодера.С вращающейся башней мне было просто потому что она по сути уже у меня была.Я интегрировал систему навигации с построенным мной лидаром о котором подробнее можно прочитать в теме "Сканирующий лазерный дальномер"от citizen .
Самый простой световод это две рядом расположенные непрозрачные пластины образующие узкую щель обзора в четверть сферы для фотоприемника как вперед так и вверх .
По началу я так и сделал,
и вроде все работало,но меня не устраивала максимальная дальность (около 2 метров при пиковом токе через светодиод маяка 100 миллиампер) при которой приемник срабатывал.Ведь чтобы получить узкую диаграмму направленности (1.5-3 градуса),а чем уже тем точнее определение угла, пришлось сделать очень узкую щель тем самым снизив чувствительность фотоприемника.Идти же путем повышения тока через излучающий светодиод маяка или наращивать их количество не хотелось. В итоге я применил пластинки из прозрачного оргстекла толщиной 1 мм изготовленные лазерной резкой.
Пластинки с боков окрашены непрозрачной краской и собраны в пакет из четырех штук.
Торцы оставлены прозрачными в результате получились четыре узких световода обеспечивающие всю полезную площадь фотоприемника.
Оргстекло хоть и не 100% прозрачное для ик излучения (а в моем световоде толщина оргстекла
для прохождения света почти 5см) но форма окружности пластинок образуют своеобразную линзу которая фокусирует свет на фотоприемник тем самым компенсируются потери.Таким образом
максимальная дальность от одного светодиода уже получилась 4 метра,желанные 6 метров набрались путем включения последовательно трех светодиодов.
Теперь о том как же определить координаты робота по замеренным углам A1,A2 между маяками.
Я не смог вывести формулу по которой бы рассчитывались координаты X,Y по известным углам A1,A2.
И подозреваю, что если эта формула выводится то она имеет не слабую длину и потребует не
маленькие вычислительные ресурсы компьютера.Но к счастью обратная задача с помощью простой тригонометрии легко решается.
A1=(atan((yi-y2)/(xi-x2))+atan((y1-yi)/(xi-x1)))*180/PI;
A2=(atan((xi-x2)/(yi-y2))+atan((x3-xi)/(yi-y2)))*180/PI;
где x1,y1,x2,y2,x3,y3-значения координат X и Y маяков №1,№2 и №3.
xi,yi-искомые координаты робота.
Каждая точка помещения имеет свои уникальные углы A1,A2.Заранее для каждой точки вычисляются эти углы(для помещения 4 на 4 метра можно применить шаг 10 см между точками)создавая так называемую угловую карту местности.Контроллер (даже маломощный) имея в памяти эту угловую
карту местности путем перебора всех точек и находя максимальное совпадение замеренных углов с
имеющимися без особого труда определяет координаты робота в реальном масштабе времени.
Очень важно отметить что данная навигационная система определяет не только координаты робота но и угловое положение робота в пространстве,другими словами определяет направление куда смотрит робот.Это свойство очень полезно для дальнейшего использования.
Вообще говоря имея скан местности полученные от лидара (а вышеописываемая система у меня установленана на лидар) и имея карту местности можно вычислить координаты робота без дополнительной навигационной системы.
Алгоритм вычисления следующий (такой же используется в системе ROS):полученный скан от лидара
примеряется к каждой точке на карте помещения.Крутится вокруг оси от 0 до 360 градусов и
определяется положение при котором оценка совпадения максимальная .Так для каждой точки в помещении.После полного перебора положение при котором произошло максимальное совпадение и принимается как положение робота.
Этот метод имеет более хорошие точностные характеристики(по сравнению с моей
навигационной системой) но требует "свежие" карты местности.
Также один из минусов этого метода в том что он очень требователен к вычислительной
мощности компьютера.Я написал программу вычисления координат по данным от своего лидара.
Для обсчета трехкомнатной квартиры требуется более секунды времени вычисления на intel core i3
компьютере и это при том что для сокращения времени обсчета применяю поэтапное приближение.
Вначале первый проход всех точек квартиры делается с большим шагом и каждое значение дальности
от лидара (а их 360 за полное сканирование) принимается совпавшим с значением из карты квартиры с большей погрешностью.Второй проход уже выполняется не по всей квартире а только вблизи точки максимального совпадения выявленную на предыдущем проходе с более точной погрешностью.
Фу!Обьяснил в общих чертах .К чему я это? К тому, что имея еще и вышеописанную систему навигации и вместо трех маяков в комнате хотя бы один работающий, опредилив на него пеленг,время вычисления координат при помощи лидара и известной карты местности можно сократить
грубо говоря в 360 раз(не надо крутить при примерке скан от 0 до 360 градусов).Практически
полностью исключаются ложные решения когда обстановка в помещении в данной позиции похожа с другой или под другим углом или когда стоя около лидара затеняеш ему углы обзора или даже когда текущая обстановка поменялась и не полностью сходится с картой.Также дополнительную селекцию комнат позволяет применение в разных комнатах маяки с разными частотами.
При постройке карт с помощью лидара возможность привязки к координатам от навигационной системы также очень полезна.
Вот так в связке с лидаром,на который собственно и установлена, может работать моя навигационная система.
