Читаю одну книжку, Enhancing the Pololu 3Pi with RobotBASIC. Там очень хорошо описана реализация нахождения своих координат в комнате с помощью двух маяков. Погрешность определения координат около 1см.
Заинтересованны?
Тогда начну по порядку.
Маяки:
Обычные ИК мояки, посылают пачку сигналов. Расположены в углах комнаты. У которых (смомощью рулетки) известно расстояние между ними.
Робот:
а) два ИК приемника расположенные по бокам робота, которые смогут определить направление к маяку. (повернутся так сказать лицом к маяку, чем точнее повернется, тем меньше погрешность).
б) компас - датчик который определяет насколько развернут робат в градусах относительно "севера".
К теме приложена картинка, где схематично обозначены углы.
Где:
B1 - ИК маяк 1
B2 - ИК маяк 2
rx,ry - искомое координаты
BA1, BA2 - углы ИК мояков (взятые из компаса)
A1, A2 - относительные углы
Алгоритм следующий.
Позицируемся на мояк 1
С компаса узнаем угол BA1 на который мы развернулись.
Позицируемся на мояк 2
Опять же с компаса узнаем угол BA2 поворота.
Считаем относительные углы (вообще они зависят от калибровки компаса, распложения вашей комнаты относительно севера итд.)
A1 = 180 - BA1
A2 = 360 - BA2
Высчитываем тангенс углов (Тангенс - это тригонометрическая функция, представляющая собой отношение синуса угла к косинусу этого же угла (с)КЭП) (еще тангенс обычно считают в радианах, так что нужно соотвественно перевести градусы в радианы)
M1 = tan(A1)
M2 = tan(A2)
Далее можно высчитать уже свои координаты.
Для этого возьмем еще одну переменную RW - это расстояние между маяками (можно взять в сантиметрах)
Тогда
rx = RW / ( M1- M2)
ry = M1 * rx
Вуаля!
Мы имеем координаты своего местоположения rx, ry
Какие могут быть проблемы?
Это мояки, чтобы сигналы мояков друг друга неперекрывали, их нужно пускать с разной частатой, либо включать встрого определеннмо времении. Или включать выклюяать дистационно через вайффайл или блютузом.
Также, былобы неплохо, чтобы мояки высылали не просто пачку сигналов, но свой индификатор + расстояние между маяками RW.