roboforum.ru

Система навигации для домашнего робота должна определять текущие координаты робота
на карте помещения(квартиры).Трудно научить чему нибудь мобильного робота если он не знает
где находится.Задавшись этой целью я и решил создать такую навигационную систему.
Идея и реализация достаточно просты,но чтобы все заработало мне пришлось довольно много экспериментировать и применять в конечном итоге нетривиальные(для меня) решения.
Суть в определении углов пеленгов с робота на световые маяки установленные в комнате под потолком,работающие в инфракрасном диапазоне и имеющие заранее известные постоянные координаты.По этим углам и определяются координаты робота.



ТТХ системы:максимальная дальность до маяка-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 градусов).Практически
полностью исключаются ложные решения когда обстановка в помещении в данной позиции похожа с другой или под другим углом или когда стоя около лидара затеняеш ему углы обзора или даже когда текущая обстановка поменялась и не полностью сходится с картой.Также дополнительную селекцию комнат позволяет применение в разных комнатах маяки с разными частотами.
При постройке карт с помощью лидара возможность привязки к координатам от навигационной системы также очень полезна.
Вот так в связке с лидаром,на который собственно и установлена, может работать моя навигационная система.

Отличная работа

Вот молодец! Даже просто МОЛОДЕЦ! Я подобное думал с года полтора назад - но сделал тестовый стенд и забросил.

_________________
Дым, идущий из всех устройств в помещении предвещает появление электрика.

А если вместо ИК маяков использовать катафоты? Тогда не надо питать ИК излучатели по углам, не нужны генераторы частот, не нужны световоды для приемника и т.д.
"Стрелять" ИК надо будет с робота и сканировать пространство в поисках катафот по 2м координатам или стрелять вертикальной линией и ловить отраженный сигнал.

вероятно катафот будет путаться с любой глянцевой поверхностью расположенной внезапно под прямым углом.

_________________
<telepathmode>На вопросы отвечает Бригадир Телепатов!</telepathmode>
Всё уже придумано до нас!

Глянец внезапно под прямым углом вряд ли - робот внизу, а катафоты на потолке.

Супер! Главное, что работает. И описание подробное. Навигационную систему в каждый дом! Маяки на батарейках? Сигналы с маяков могут и уникальный код передавать, чтобы различаться. В описании маяков светодиод несколько раз назван фотодиодом. И в описании говорится о 2-х фотоприемниках, а на рисунке только 1.

Да,фотодиод принимает,светодиод излучает.Маяки работают от розетки через блоки питания.
Батареек хватило бы только на несколько часов.Фотоприемника в системе два, работают на разных частотах соответственно и два световода(расположенные на одной линии но смотрят в разные стороны).На рисунке показана пластинка одного световода формы четверти круга пропускает сигнал как сверху так и частично спереди по окружности .У меня пока светодиоды работают непрерывно но модуляция их возможна и даже желательна.TSOP фотоприемник имеет отрицательную обратную связь по усилению и если сигналы от маяков идут без достаточного промежутка между ними происходит автоматическое понижение чувствительности.
Насчет катафот на потолке:Идея интересная но в данном случае практической силы не имеет.Чтобы облучать катафоту нужно видимо применять узкий луч лазера причем не "точечный" а "плоскостной" формы направленности.Плоскостной потому, что когда робот подъезжает близко к маяку он у него в зените а когда отъезжает он уходит в сторону горизонта.Но в таком случае будет облучаться не только потолок но и стены.Другими словами если катафота далеко она отражает такой же силы сигнал как допустим часть белой стены расположенной близко.Да и создать катафоту возвращающую свет обратно одинаково с разных сторон проблематично.

Вообще-то катафот уже заточен под то, чтобы возвращать свет обратно в сторону источника не зависимо от направления. Он состоит из кучи маленьких призмочек. Уголковый отражатель.

можно, надо на приёмник поставить направленную антенну.
тока в реале антенну с узким углом сложновато сделать, робот будет определять угол с точностью под 90 градусов)))
а вот узкий световод сделать это не сложно.

_________________
<telepathmode>На вопросы отвечает Бригадир Телепатов!</telepathmode>
Всё уже придумано до нас!

Все стесняюсь спросить - а за глазки не боязно?

ну там вроде написано что используется ток 100 мА, так что ничего страшного.

вобще странноватая конструкция, если никакой информации не передаётся то почему бы не взять просто несколько фототранзисторов веером между двух чёрных пластинок.
ширина диода 3 мм, тоесть в самом простом случае щель тоже будет 3 мм и диаграмма получится достаточно узкая.

ЗЫ
у тсопа очень хорошая настройка на частоту, малейшее отклонение и дистанция срабатывания падает в разы.
может плохо ловит из за неточно выставленной частоты/скважности генератора?

_________________
<telepathmode>На вопросы отвечает Бригадир Телепатов!</telepathmode>
Всё уже придумано до нас!

TSOP изобретен специально для того чтобы исключить ложные срабатывания от всевозможных засветок (от ламп ,солнечного света и т.д.)и он я считаю как нельзя лучше подходит для данной системы. Если бы система работала в полной темноте то да можно применить обычные чувствительные фотодиоды или фототранзисторы без всяких специальных схем фильтрации. В TSOP же уже все это есть.
Применение 3мм щели при глубине установки приемника 50мм дает диаграмму направленности в 7 градусов.Что очень много так как невозможно добиться всегда стабильного срабатывания в одинаковые моменты времени при входе и выходе из поля зрения приемника света от маяка.В моем случае применение пластинок 1мм толщиной позволяет сузить диаграмму направленности до 2,2 градусов и далее вычисляется среднее значение угла приема.Сборка пластинок в пакет позволяет
обеспечить всю полезную площадь фотоприемника(диаметр линзы TSOP около 5мм).Вроде есть оргстекло толщиной в 0.5 миллиметра но я его нигде не смог достать,его применение сузило бы диаграмму направленности в 2 раза и соответственно повысило бы точностные характеристики.

Примененный ИК 940нм светодиод KA-3528F3C при пиковом токе через него 100 ма обеспечивает
дальность 6 метров без световода перед TSOP.Со световодом 4 м, что тоже неплохо я считаю.
Выбор светодиода KA-3528F3C обусловлен его большой диаграммой излучения в 120 градусов.
Например в пультах дистанционного управления используются светодиоды с более узкой диаграммой 40 градусов и пиковые токи там достигают 1 ампера для обеспечения большой дальнобойности.

Очень интересно. Правда не понял почему не получилось вывести формулу определения координат по углам. Попробую.
Вместо описанной конструкции можно использовать видеокамеру с OpenСV для определения маяков. На первый взгляд это более громоздко, но на перспективу может оказаться более универсальным решением, т.к. камера более универсальный сенсор. И в качестве маяков можно будет использовать QR или цветные метки или просто характерные предметы интерьера. А если это будет ещё и стерео-камера, то помимо углов она будет давать ещё и расстояние до маяков. Кстати, на вашем принципе тоже можно попытаться реализовать стерео"зрение". Не думали?
Ещё одно соображение в пользу камеры. При известной вертикальной координате маяка расстояние до него легко определяется даже моно-камерой по углу места.
Но это всё теоретизмы, приятно видеть работающее решение.

Добавлено спустя 2 минуты 30 секунд:

Легко. Любой ёлочный шарик вернёт блик обратно. Но с катафотом тема всё равно интересная.