Воскресенье, 13 Март 2016 10:42

Программируем робота - машинку на Arduino. Часть 2. Расчет координат и направления робота

Автор
Оцените материал
(2 голосов)

Следующий этап, который я решил предпринять на пути повышения «интеллекта» робота на Arduino – это научить его  ориентироваться в системе координат, и перемещаться в заданную двумя координатами точку.

Эта кажущаяся несложной задача заняла прилично времени и заставила перетряхнуть школьные знания по геометрии. Кто не говорил в школе фразу: «Да на кой черт мне все эти синусы – косинусы в жизни пригодятся?!» Ну, если кто с детства спал и видел себя инженером – то, может, и не говорил. Но я ж не инженер…

Итак, условия задачи. Робот едет вперед, его колеса вращаются с разной скоростью из-за разной мощности двигателей . Скорость эта ещё и не постоянная – я ведь все же пытаюсь её более-менее уравнять с помощью ШИМ.  В итоге едет он по неправильной кривой. Как точно рассчитать его положение  и текущее направление?

Давайте думать. Для начала разобьем кривую траекторию движения робота на небольшие отрезки. Каждый из таких отрезков будет являться дугой.  О, это уже проще! Получается, что центр робота (я его разместил посредине между колесами) и каждое из колес описывает дугу с разными радиусами, но с общим центром О и с одним и тем же центральным углом α:

движение робота машинки на коротком отрезке кривой

 При этом в конце движения вектор движения робота (зеленая стрелка) отклоняется от первоначального направления на тот же угол α.  Обозначим эти дуги буквами (меньшую)и L (большую). Расстояние от условного центра до центра робота обозначим как R, а расстояние от центра робота до колес – r.  Изображение машинки можно теперь убрать и заниматься чистой геометрией:

геометрия движения робота на Arduino

Длину дуг можно узнать, подсчитав количество срабатываний энкодеров колес за какой-то промежуток времени (я взял 200 миллисекунд).  r узнаем с помощью линейки. Это три величины, которые нам даны. А найти нужно ВХ, ВУ и ∠α.

Так сразу это не выйдет, сначала найдем чему равен R. Воспользуемся формулой длины дуги 

for1

и посмотрим, чему равна длина дуг L и l :

for2

Упс, а где π и 180 градусов? А они тут не нужны. Во-первых, все равно потом сократятся. А во-вторых, ардуино считает все углы в радианах, а не в градусах. Так что сразу стараемся избавляться от этих 180, 90 и 360 градусов, так привычных со школьной скамьи.

Теперь выразим центральный угол через длину дуг:

for3

Угол один и тот же, так что приравниваем правые части формул и решаем уравнение:

for4

Ну вот, угол, на который робот повернул на данном отрезке пути, уже найден!

Теперь найдем, на какое расстояние робот сместился по осям х и у. Соединим начальные и конечные точки движения колес и центра робота отрезками. Получим три подобных треугольника: EOF, AOB, COD. Так как по две их стороны являются радиусами – треугольники равнобедренные. А у равнобедренных треугольников углы при основании равны (нас интересуют конкретно ∠ВАО , потому как он входит в треугольник АВУ, а в нем сторона ВУ является одним из искомых значений). Сумма углов любого треугольника = 180® - значит,

∠ВАО=(180-α)/2

АВ является хордой, значит, ее длина

АВ=2R*sin(α /2)

Треугольник АВУ – прямоугольный, значит,

∠АВУ=180-90-∠ВАУ = α /2

АУ = АВ*sin(∠АВУ) = 2R*sin2(α /2) – вот и нашли, насколько робот переместился по оси У !

ВУ = АВ*sin(∠ВАУ) = 2R*sin(α /2)*sin((180- α)/2) = 2R*sin(α /2)*cos(α /2) = 2R*(sin(α)/2) = R*sin(α) – а это смещение робота по оси Х !

Та-дам, первая задача решена!

Теперь можно через каждые, к примеру, 200 миллисекунд оценивать сколько сантиметров робот проехал по иксу и по игреку, и на сколько радиан изменился его угол, добавлять эти данные к предыдущим координатам и получать новые.Следующая задача!

Ок, допустим, теперь мы знаем, где находится робот (точка О) и что вектор его движения (синяя стрелочка) образует с осью иксов угол carAngle (ну, так я уже его назвал в программе). И знаем координаты точки, куда ему нужно двигаться на следующем этапе. Следующая задача: рассчитать угол, на который должен повернуть робот, чтобы вектор его движения стал направлен на целевую точку (угол beta). И после этого – расстояние, которое он должен пройти (dS).

for5

 Все эти рассчеты я использовал в новой версии скетча для Arduino, который можно скачать по этой ссылке, файл LazyCarBot_05.zip

Машинка под управлением этого скетча перемещается к заданным координатам примерно так, как было задумано. Но далеко не идеально, должен сказать. То ли разрешение энкодеров маловато, то ли часть сигналов от энкодеров не проходит... но чем дальше - тем больше накапливается ошибка позиционирования. Но поначалу - едет вполне туда, куда хозяин приказал. Вот пруф:

Прочитано 1786 раз Последнее изменение Суббота, 12 Март 2016 19:28
Авторизуйтесь, чтобы получить возможность оставлять комментарии