Я, Большой железный робот и мои создатели, команда робототехников Карандаш и Самоделкин, поздравляем всех с наступающим Новым годом!
Хочется всем пожелать, чтобы в наступающем году с нами произошло то самое чудо, о котором мы все так мечтаем. Хотя у каждого оно свое, но оно обязательно самое необходимое и самое важное. Желаю никому не ржаветь, иметь под рукой свежую батарейку, на которую можно всегда рассчитывать. Желаю Вам в грядущем году быть в окружении исключительно положительных роботов и доброжелательных людей, переживать только приятные эмоции без багов, радоваться каждому прожитому циклу, дарить радость и энергию окружающим. Пусть этот Новый год станет для Вас особенным. С Новым Годом!
Во второй задаче у нас будет целых два робота. Первый будет двигаться по линии постоянно изменяя скорость своего движения. Второй робот будет его догонять, стараясь двигаться с одинаковой с ним скоростью. Два раза в минуту роботы меняются ролями.
В нашем проекте NAVIDOZ3R, о котором сегодня пойдет речь, мы немного отойдем от хоббийных роботов и построим более привычного всем мобильного EV3-робота, способного решать довольно интересную задачу.
Представьте себе робота на поверхности планеты и спутник на ее орбите. Спутник видит общую карту местности и помогает запланировать маршрут робота, позволяя оператору расставить путевые точки и последовательность их прохождения, однако не обладает детальной информацией о нюансах, которые могут встретиться на пути робота (мелкие препятствия).
Мобильный робот, приняв со спутника маршрут, должен преодолеть его, самостоятельно объезжая возникающие небольшие препятствия, обеспечив прохождение всех путевых точек в заданной последовательности.
Данная задача - на инерциальную навигацию. Что это такое? Как гласит Википедия, инерциальная навигация — метод навигации (определения координат и параметров движения различных объектов — судов, самолётов, ракет и др.) и управления их движением, основанный на свойствах инерции тел, являющийся автономным, то есть не требующим наличия внешних ориентиров или поступающих извне сигналов. Неавтономные методы решения задач навигации основываются на использовании внешних ориентиров или сигналов (например, звёзд, маяков, радиосигналов и т. п.). Эти методы в принципе достаточно просты, но в ряде случаев не могут быть осуществлены из-за отсутствия видимости или наличия помех для радиосигналов и т.п. Необходимость создания автономных навигационных систем явилась причиной возникновения инерциальной навигации.
Сущность инерциальной навигации состоит в определении ускорения объекта и его угловых скоростей с помощью установленных на движущемся объекте приборов и устройств, а по этим данным — местоположения (координат) этого объекта, его курса, скорости, пройденного пути и др., а также в определении параметров, необходимых для стабилизации объекта и автоматического управления его движением.
В этот раз мы не будем выдумывать нового робота, а совместим "приятное с полезным" - соберем одну из ранее не собиравшихся нами бонусных моделей, инструкция по сборке к которой свободно доступна всем желающим в составе домашней версии ПО LEGO.
Немного погоняв собранного ROBODOZ3R, мы приняли решение устранить недостатки в его шасси. Как показали испытания, шестеренки, установленные в качестве верхних колес, недостаточно уверенно натягивают гусеницы. Вместо них мы рекомендуем использовать третью пару колесных дисков. Эта модификация заметно сказывается на точности движений робота, гусеницы перестают проскальзывать.
Кроме этого, в задней части робота вместо датчика-кнопки установим гироскоп, который понадобится чтобы робот мог удерживаться на заданном курсе.
С конструкцией разобрались, теперь давайте поставим роботу задачу, которую ему предстоит решить. Вернемся к нашему роботу на планете и спутнику. Спутник в нашем проекте - виртуальный, в его роли выступает ПК. На экране компьютера оператор видит заранее нарисованную "карту местности", на которой предстоит работать роботу. Мышкой по карте устанавливаются стартовая позиция робота и последовательность путевых точек, которые роботу предстоит пройти. Далее по нажатию кнопки "Сохранить маршрут" эти данные сохраняются в файл на работе.
Программировать мы снова будем на EV3 Basic. Задумав этот проект, мы хотели в том числе пощупать его возможности по гибридному режиму работы "робот-ПК", когда часть кода исполняется на ПК, часть кода на роботе. При этом робот должен быть соединен с компьютером по USB, Bluetooth или Wi-Fi.
Давайте посмотрим на первую, гибридную программу, которая выполняется и на ПК и на роботе:
' фоновая картинка: background = ImageList.LoadImage(Program.Directory + "/map1.jpg") GraphicsWindow.DrawImage(background, 0, 0) ' Разрешение карты в пикселях ImageX_pix = 480 ImageY_piy = 480 ' Размеры поля в миллиметрах ImageX_mm = 1117 ImageY_mm = 1116
' Добавим кнопки btnPOI = Controls.AddButton("Сохранить маршрут", 20,GraphicsWindow.Height-100) btnStart = Controls.AddButton("Старт", 520,GraphicsWindow.Height-100) ' Событие кнопки Controls.ButtonClicked = OnClick ' количество путевых точек POI = 0 Sub OnClick Sound.PlayClick() btn=Controls.LastClickedButton If btn="Button1" then GraphicsWindow.ShowMessage("Маршрут записан в робота","EV3") F = EV3File.OpenWrite("/home/root/lms2012/prjs/NAVIDOZ3R/NAVIDOZ3R_PT.txt") EV3File.WriteLine(F,POI) For i = 0 To POI-1 EV3File.WriteLine(F,Math.Round(PTx[i])) EV3File.WriteLine(F,Math.Round(ImageY_mm - PTy[i])) EndFor EV3File.Close(F) Else ' выход из программы Program.End() EndIf EndSub ' Отслеживание событий нажатия кнопки мыши GraphicsWindow.MouseDown = OnMouseDown Sub OnMouseDown Sound.PlayClick() If POI = 0 Then GraphicsWindow.BrushColor = "Blue" Else GraphicsWindow.BrushColor = "Red" EndIf GraphicsWindow.FillEllipse(x-5,y-5,10,10) Program.Delay(500) PTx[POI] = x*ImageX_mm/ImageX_pix PTy[POI] = y*ImageY_mm/ImageY_piy If POI >= 1 Then GraphicsWindow.DrawLine(PTx[POI-1]/ImageX_mm*ImageX_pix, PTy[POI-1]/ImageY_mm*ImageY_piy, PTx[POI]/ImageX_mm*ImageX_pix, PTy[POI]/ImageY_mm*ImageY_piy) EndIf POI = POI + 1 EndSub While "True" x = GraphicsWindow.MouseX y = GraphicsWindow.MouseY EndWhile
Теперь запрограммируем робота, написав вторую программу, предназначенную для навигации робота. В этом режиме робот полностью автономен, ПК не задействуется. В файловой системе робота первой программой, предварительно запуленной на ПК, сформирован файл, содержащий количество путевых точек и их координаты (X,Y) в миллиметрах. Первая путевая точка - это точка старта робота. На старте робот ориентирован в направлении верхней части карты.
Так как в данной конструкции используется крепление блока EV3 экраном вниз, в самом начале программы мы ждем некоторое время, за которое нужно успеть неподвижно установить робота на месте старта.
После этого делаем аппаратный сброс гироскопа, переключив его несколько раз из режима в режим.
' Предварительная попытка устранения дрифта
Sensor.SetMode(2,0)
Sensor.SetMode(3,0)
Sensor.SetMode(3,1)
Sensor.SetMode(3,0)
Sensor.SetMode(3,1)
Sensor.SetMode(3,0)
Гироскоп EV3 подвержен неприятному явлению - дрейфу показаний (дрифту), поэтому оцениваем его в течении 5 секунд, для дальнейшей компенсации.
Time = EV3.Time
' Взятие дрифта за 5 секунд
Gyro1 = Sensor.ReadRawValue(3,0)
Program.Delay(5000)
Gyro2 = Sensor.ReadRawValue(3,0)
Drift10 = (Gyro1 - Gyro2) / 500
Получили в переменной Drift10 величину дрифта за 10 мс. Теперь открываем файл с путевыми точками и определим их количество.
F = EV3File.OpenRead("/home/root/lms2012/prjs/NAVIDOZ3R/NAVIDOZ3R_PT.txt")
Line = EV3File.ConvertToNumber(EV3File.ReadLine(F))
PT = Line
Очищаем экран и начинаем чтение файла, одновременно сохраняя путевые точки в массивы PTx и PTy с одновременным выводом на экран для контроля (робота можно взять в руки в этот момент для отладки и посмотреть что считывается из файла).
Line = EV3File.ConvertToNumber(EV3File.ReadLine(F))
PTx[i] = Line
LCD.Text(1,0,i*10+20,1,Line)
Program.Delay(50)
Line = EV3File.ConvertToNumber(EV3File.ReadLine(F))
PTy[i] = Line
LCD.Text(1,40,i*10+20,1,Line)
Program.Delay(50)
EndFor
EV3File.Close(F)
Program.Delay(3000)
Создаем кучу переменных, они нам все обязательно понадобятся
' Объявление переменных
u = 0
e = 0
e_old = 0
Pk = 4
Dk = 8
X = 0
Y = 0
X_old = 0
Y_old = 0
X_new = 0
Y_new = 0
X_tmp = 0
Y_tmp = 0
speed = 25
diam = 32.75125
Dist = 0
Azimut = 0
Azimut_old = 0
Gyro = 0
Finish = "False"
TimeTurn = 0
Далее опишем параллельную задачу (подпроцесс) в котором считываются показания гироскопа и компенсируется дрифт. Итоговые показания публикуем в переменную Gyro. Именно из нее, а не с гироскопа мы будем брать показания.
' Подпроцесс передаёт показания гироскопа с устранением дрифта
Создадим процедуру, которая выводит на экран текущие координаты робота, предыдущую путевую точку и текущий азимут (направление движения). Эта информация крайне полезна при отладке и анализе текущей ситуации.
Sub Display
Speaker.Note(100,"C5",500)
LCD.Clear()
LCD.Text(1,0,0,2,X)
LCD.Text(1,0,20,2,Y)
LCD.Text(1,0,40,1,X_old)
LCD.Text(1,0,50,1,Y_old)
LCD.Text(1,0,70,2,Azimut)
LCD.Text(1,0,90,1,Gyro)
Program.Delay(3000)
EndSub
Прежде чем изучать нашу программу дальше, давайте посмотрим каким образом робот, находясь в произвольном месте на плоской карте, может достичь целевой точки по кратчайшему пути.
Зная свои координаты (X,Y) и координаты целевой точки (X_new,Y_new) робот должен рассчитать две величины - расстояние, которое от должен проехать Dist и направление, в котором он должен это сделать Azimut. Расстояние Dist рассчитывается просто, по теореме Пифагора:
А направлением Azimut сложнее, в зависимости от направления движения оно рассчитывается по разному:
Следует обратить внимание на тот факт, что в разных четвертях системы координат при расчете величины угла меняются местами прилежащий и противолежаний к углу катеты треугольников (см.рис):
Следующая процедура, которая нам понадобится - Go_Forward - ведет робота вперед ан заданное расстояние Dist, удерживая при этом направление Azimut. Если в процессе движения на пути возникает препятствие, она вызывает одну из процедур объезда Detour1() .. Detour3().
Сбрасываем энкодеры моторов на шасси и используя ПИД-регулятор движемся вперед компенсируя отклонения показаний регулируемой величины Gyro от целевой Azimut. Следует учесть, что Azimut у нас увеличивается по часовой стрелке, а Gyro - против.
Во время движения обновляем показания текущих координат робота (X,Y).
Sub Go_Forward
Motor.ResetCount("BC")
Finish = "True"
While Math.Abs((Motor.GetCount("B")+Motor.GetCount("C"))/2) < Dist/(diam*Math.Pi)*360
Второй вариант объезда - "по половинке квадрата" является потенциально рекурсивным, если препятствие объехать не получается, процедура объезда будет вызываться вновь и вновь до тех пор пока объезд не будет выполнен.
Третий вариант объезда - "по половинке правильного многоугольника" также является рекурсивным. При большом количестве сторон получаем объезд по полукругу.
Чтобы минимизировать углы разворота при следовании по маршруту потребуется процедура MinimumTurn(). Необходимость ее использования связана с тем, что показания гироскопа могут выходить за пределы 0..360. Она переносит текущий расчетный азимут в тот "круг", в пределах которого находятся сейчас показания гироскопа. Таким образом любой разворот не сможет превысить 180 градусов.
Sub MinimumTurn
For j=-10 To 10
If Math.Abs(Azimut+j*360-Azimut_old)<180 Then
Azimut = Azimut+j*360
EndIf
EndFor
EndSub
Процедура Turn() выполняет поворот, ориентируя робота в нужном направлении, причем ей не важно как робот ориентирован в данный момент. Она использует ПИД-регулирование, подобно процедуре Go_forward(), однако рассчитанное результирующее воздействие не суммируется с постоянной величиной скорости, тем самым робот разворачивается на месте.
Sub Turn
TimeTurn = EV3.Time
While Math.Abs(Azimut-(-1)*Gyro)>=3 And EV3.Time-TimeTurn<5000
e = Azimut-(-1)*Gyro
u=Pk*e+Dk*(e-e_old)
Motor.Start("B",u)
Motor.Start("C",-1*u)
e_old = e
Program.Delay(10)
EndWhile
Motor.Stop("BC","True")
Program.Delay(100)
EndSub
Процедура GotoXY() выполняет все цепочку действий, необходимых для достижения следующей путевой точки маршрута:
Все нужные процедуры созданы, далее - основное тело программы. Первым делом запустим в параллельном процессе процедуру Gyro, чтобы иметь в памяти актуализированную информацию о показаниях гироскопа с компенсированным дрифтом. Затем в цикле пробегаем все путевые точки, формируя маршрут и вызывая соответствующие процедуры для выполнения маневров.
' Запуск задачи Gyro
Thread.Run = Gyro
' Перед запуском блока следует передать ему значения
После того как робот достиг последней путевой точки он развернется в тоже положение, в котором был на старте - для контроля величины дрифта. Кроме этого оценим дрифт еще раз в выедем на экран его величину в момент старта и в момент завершения задания.
Во время прохождения роботом маршрута, в том числе при выполнении объездов текущие координаты робота периодически записываются в файл. Эти данные можно использовать в дальнейшем в программе на ПК для отображения на карте фактически пройденного пути. В тексте программ выше эта функция отсутствует, однако в полной версии программы, которую вы можете скачать, она реализована. Вот так вот выглядит фактически пройденный трек, загруженный из файла по нажатию кнопки "Действительный маршрут".
Скачать исходный код к нашего проекта можно в GitHub, а на видео ниже - посмотреть, как работает наш NAVIDOZ3R.
