В нашем проекте NAVIDOZ3R, о котором сегодня пойдет речь, мы немного отойдем от хоббийных роботов и построим более привычного всем мобильного EV3-робота, способного решать довольно интересную задачу.
Представьте себе робота на поверхности планеты и спутник на ее орбите. Спутник видит общую карту местности и помогает запланировать маршрут робота, позволяя оператору расставить путевые точки и последовательность их прохождения, однако не обладает детальной информацией о нюансах, которые могут встретиться на пути робота (мелкие препятствия).
Мобильный робот, приняв со спутника маршрут, должен преодолеть его, самостоятельно объезжая возникающие небольшие препятствия, обеспечив прохождение всех путевых точек в заданной последовательности.
Данная задача - на инерциальную навигацию. Что это такое? Как гласит Википедия, инерциальная навигация — метод навигации (определения координат и параметров движения различных объектов — судов, самолётов, ракет и др.) и управления их движением, основанный на свойствах инерции тел, являющийся автономным, то есть не требующим наличия внешних ориентиров или поступающих извне сигналов. Неавтономные методы решения задач навигации основываются на использовании внешних ориентиров или сигналов (например, звёзд, маяков, радиосигналов и т. п.). Эти методы в принципе достаточно просты, но в ряде случаев не могут быть осуществлены из-за отсутствия видимости или наличия помех для радиосигналов и т.п. Необходимость создания автономных навигационных систем явилась причиной возникновения инерциальной навигации.
Сущность инерциальной навигации состоит в определении ускорения объекта и его угловых скоростей с помощью установленных на движущемся объекте приборов и устройств, а по этим данным — местоположения (координат) этого объекта, его курса, скорости, пройденного пути и др., а также в определении параметров, необходимых для стабилизации объекта и автоматического управления его движением.
В этот раз мы не будем выдумывать нового робота, а совместим "приятное с полезным" - соберем одну из ранее не собиравшихся нами бонусных моделей, инструкция по сборке к которой свободно доступна всем желающим в составе домашней версии ПО LEGO.
Кроме этого, в задней части робота вместо датчика-кнопки установим гироскоп, который понадобится чтобы робот мог удерживаться на заданном курсе.
С конструкцией разобрались, теперь давайте поставим роботу задачу, которую ему предстоит решить. Вернемся к нашему роботу на планете и спутнику. Спутник в нашем проекте - виртуальный, в его роли выступает ПК. На экране компьютера оператор видит заранее нарисованную "карту местности", на которой предстоит работать роботу. Мышкой по карте устанавливаются стартовая позиция робота и последовательность путевых точек, которые роботу предстоит пройти. Далее по нажатию кнопки "Сохранить маршрут" эти данные сохраняются в файл на работе.
Программировать мы снова будем на EV3 Basic. Задумав этот проект, мы хотели в том числе пощупать его возможности по гибридному режиму работы "робот-ПК", когда часть кода исполняется на ПК, часть кода на роботе. При этом робот должен быть соединен с компьютером по USB, Bluetooth или Wi-Fi.
Давайте посмотрим на первую, гибридную программу, которая выполняется и на ПК и на роботе:
' Массивы с путевыми точками
PTx[0] = 0
PTy[0] = 0
' Показываем графическое окно
GraphicsWindow.Show()
' Очистить окно
GraphicsWindow.Clear()
' Заголовок окна
GraphicsWindow.Title = "NAVIDOZ3R"
' запрещаем изменять размеры окна
GraphicsWindow.CanResize = "False"
' Размеры окна
GraphicsWindow.Width=825
GraphicsWindow.Height=640
' Окно по центру экрана
GraphicsWindow.Left = (Desktop.Width - GraphicsWindow.Width) / 2
GraphicsWindow.Top = (Desktop.Height - GraphicsWindow.Height) / 2
' Цвет фона
GraphicsWindow.BackgroundColor = "Black"
GraphicsWindow.PenColor = "Red"
' Шрифт
GraphicsWindow.FontName = "Arial"
GraphicsWindow.FontSize = 20
GraphicsWindow.PenColor = "Black"
' фоновая картинка:
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 экраном вниз, в самом начале программы мы ждем некоторое время, за которое нужно успеть неподвижно установить робота на месте старта.
Program.Delay(2000)
Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/load")
Speaker.Wait()
Program.Delay(1000)
После этого делаем аппаратный сброс гироскопа, переключив его несколько раз из режима в режим.
' Предварительная попытка устранения дрифта
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 с одновременным выводом на экран для контроля (робота можно взять в руки в этот момент для отладки и посмотреть что считывается из файла).
LCD.Clear()
LCD.Text(1,0,0,2,Line)
Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/plan")
For i = 0 To PT-1
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 Gyro
While"True"
Gyro = -1*(Sensor.ReadRawValue(3,0) - Drift10 * (Time/10))
EndWhile
EndSub
Создадим процедуру, которая выводит на экран текущие координаты робота, предыдущую путевую точку и текущий азимут (направление движения). Эта информация крайне полезна при отладке и анализе текущей ситуации.
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
e = Azimut-(-1)*Gyro
u=Pk*e+Dk*(e-e_old)
Motor.Start("B",-1*speed+u)
Motor.Start("C",-1*speed-u)
e_old = e
AzimutTMP = Math.Remainder(Azimut+3600,360)
if AzimutTMP < 90 Then
X_tmp = Math.Abs(Motor.GetCount("B")+Motor.GetCount("C"))/2*(diam*Math.Pi/360)*Math.Sin(Math.GetRadians(Math.Remainder(AzimutTMP+3600,90)))
Y_tmp = Math.Abs(Motor.GetCount("B")+Motor.GetCount("C"))/2*(diam*Math.Pi/360)*Math.Cos(Math.GetRadians(Math.Remainder(AzimutTMP+3600,90)))
X = X_old+X_tmp
Y = Y_old+Y_tmp
ElseIf 90 <= AzimutTMP and AzimutTMP < 180 Then
X_tmp = Math.Abs(Motor.GetCount("B")+Motor.GetCount("C"))/2*(diam*Math.Pi/360)*Math.Cos(Math.GetRadians(Math.Remainder(AzimutTMP+3600,90)))
Y_tmp = Math.Abs(Motor.GetCount("B")+Motor.GetCount("C"))/2*(diam*Math.Pi/360)*Math.Sin(Math.GetRadians(Math.Remainder(AzimutTMP+3600,90)))
X = X_old+X_tmp
Y = Y_old-Y_tmp
ElseIf 180 <= AzimutTMP and AzimutTMP < 270 Then
X_tmp = Math.Abs(Motor.GetCount("B")+Motor.GetCount("C"))/2*(diam*Math.Pi/360)*Math.Sin(Math.GetRadians(Math.Remainder(AzimutTMP+3600,90)))
Y_tmp = Math.Abs(Motor.GetCount("B")+Motor.GetCount("C"))/2*(diam*Math.Pi/360)*Math.Cos(Math.GetRadians(Math.Remainder(AzimutTMP+3600,90)))
X = X_old-X_tmp
Y = Y_old-Y_tmp
ElseIf 270 <= AzimutTMP and AzimutTMP < 360 Then
X_tmp = Math.Abs(Motor.GetCount("B")+Motor.GetCount("C"))/2*(diam*Math.Pi/360)*Math.Cos(Math.GetRadians(Math.Remainder(AzimutTMP+3600,90)))
Y_tmp = Math.Abs(Motor.GetCount("B")+Motor.GetCount("C"))/2*(diam*Math.Pi/360)*Math.Sin(Math.GetRadians(Math.Remainder(AzimutTMP+3600,90)))
X = X_old-X_tmp
Y = Y_old+Y_tmp
EndIf
If Sensor.ReadPercent(2)<62 Then
Finish = "False"
Motor.Stop("BC","True")
Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/break")
Speaker.Wait()
Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/objezd")
Speaker.Wait()
Display()
'Program.Delay(8000)
X_old = X
Y_old = Y
Detour3()
X_old = X
Y_old = Y
Display()
'Program.Delay(8000)
GotoXY()
EndIf
Program.Delay(10)
EndWhile
Motor.Stop("BC","True")
Program.Delay(100)
EndSub
Далее опишем три процедуры объезда препятствий. Первая из них выполняет простой объезд "по половинке ромба":
Sub Detour1
'R = Sensor.ReadPercent(2)
Azimut_old = Azimut
Azimut = Azimut+45
MinimumTurn()
Turn()
Dist = Math.SquareRoot(22*22+22*22)*10
Go_Forward()
Display()
'Program.Delay(8000)
X_old = X
Y_old = Y
Azimut_old = Azimut
Azimut = Azimut-90
MinimumTurn()
Turn()
Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/vozvrat")
Speaker.Wait()
Dist = Math.SquareRoot(22*22+22*22)*10
Go_Forward()
Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/continue")
Speaker.Wait()
EndSub
Второй вариант объезда - "по половинке квадрата" является потенциально рекурсивным, если препятствие объехать не получается, процедура объезда будет вызываться вновь и вновь до тех пор пока объезд не будет выполнен.
Sub Detour2
'R = Sensor.ReadPercent(2)
Azimut_old = Azimut
Azimut = Azimut-90
MinimumTurn()
Turn()
Dist = 180
Go_Forward()
Display()
'Program.Delay(8000)
X_old = X
Y_old = Y
Azimut_old = Azimut
Azimut = Azimut+90
MinimumTurn()
Turn()
Dist = 360
Go_Forward()
Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/vozvrat")
Speaker.Wait()
X_old = X
Y_old = Y
Azimut_old = Azimut
Azimut = Azimut+90
MinimumTurn()
Turn()
Dist = 180
Go_Forward()
Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/continue")
Speaker.Wait()
EndSub
Третий вариант объезда - "по половинке правильного многоугольника" также является рекурсивным. При большом количестве сторон получаем объезд по полукругу.
Например процедура для 12-угольника:
Sub Detour3
For h=1 To 6
'Display()
'Program.Delay(8000)
X_old = X
Y_old = Y
Azimut_old = Azimut
If h = 1 Then
Azimut = Azimut-75
Else
Azimut = Azimut+30
EndIf
MinimumTurn()
Turn()
Dist = 103.52
Go_Forward()
If h=5 Then
Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/vozvrat")
Speaker.Wait()
EndIf
EndFor
Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/continue")
Speaker.Wait()
EndSub
Чтобы минимизировать углы разворота при следовании по маршруту потребуется процедура 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() выполняет все цепочку действий, необходимых для достижения следующей путевой точки маршрута:
- рассчитывает новый Azimut
- минимизирует угол разворота (выбирая кратчайший маневр) вызывая MinimumTurn()
- разворачивает робота в заданном направлении вызывая Turn()
- рассчитывает расстояние до следующей путевой точки Dist
- перемещает робота, сохраняя направление, вызовом Go_forward() и выполняя объезды препятствий
Sub GotoXY
Azimut_old = Azimut
If X_old <> X_new And Y_old <> Y_new Then
If X_old>X_new And Y_old>Y_new Or X_old<X_new And Y_old<Y_new Then
Azimut = Math.GetDegrees(Math.ArcTan(Math.Abs(X_old-X_new)/Math.Abs(Y_old-Y_new)))
Else
Azimut = Math.GetDegrees(Math.ArcTan(Math.Abs(Y_old-Y_new)/Math.Abs(X_old-X_new)))
EndIf
If X_old>X_new And Y_old>Y_new Then
Azimut = Azimut+180
ElseIf X_old<X_new And Y_old>Y_new Then
Azimut = Azimut+90
ElseIf X_old>X_new And Y_old<Y_new Then
Azimut = Azimut+270
EndIf
Else
If X_old = X_new And Y_old < Y_new Then
Azimut = 0
ElseIf X_old = X_new And Y_old > Y_new Then
Azimut = 180
ElseIf X_old < X_new And Y_old = Y_new Then
Azimut = 90
ElseIf X_old > X_new And Y_old = Y_new Then
Azimut = -90
EndIf
EndIf
'Azimut = Math.Remainder(Azimut + 540, 360) - 180
MinimumTurn()
Turn()
Dist = Math.SquareRoot((X_old-X_new)*(X_old-X_new) + (Y_old-Y_new)*(Y_old-Y_new))
Go_Forward()
EndSub
Все нужные процедуры созданы, далее - основное тело программы. Первым делом запустим в параллельном процессе процедуру Gyro, чтобы иметь в памяти актуализированную информацию о показаниях гироскопа с компенсированным дрифтом. Затем в цикле пробегаем все путевые точки, формируя маршрут и вызывая соответствующие процедуры для выполнения маневров.
' Запуск задачи Gyro
Thread.Run = Gyro
' Перед запуском блока следует передать ему значения
' через соответствующие переменные
For i = 1 To PT-1
X_old = PTx[i-1]
Y_old = PTy[i-1]
X_new = PTx[i]
Y_new = PTy[i]
GotoXY()
If Finish = "False" Then
X_old = X
Y_old = Y
X_new = PTx[0]
Y_new = PTy[0]
Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/back_to_start")
GotoXY()
i = 1000
'Program.End()
Else
If i<>PT-1 Then
Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/poi")
Speaker.Wait()
EndIf
EndIf
EndFor
После того как робот достиг последней путевой точки он развернется в тоже положение, в котором был на старте - для контроля величины дрифта. Кроме этого оценим дрифт еще раз в выедем на экран его величину в момент старта и в момент завершения задания.
Azimut_old = Azimut
Azimut = 0
MinimumTurn()
Turn()
Speaker.Play(100,"/home/root/lms2012/prjs/NAVIDOZ3R/poi_finish")
Speaker.Wait()
LCD.Clear()
LCD.Text(1,0,0,2,Drift10 * (Time/10))
Gyro1 = Sensor.ReadRawValue(3,0)
Program.Delay(5000)
Gyro2 = Sensor.ReadRawValue(3,0)
Drift10 = (Gyro1 - Gyro2) / 5
LCD.Text(1,0,20,2,Drift10 * (Time/10))
Speaker.Note(100,"C4",1000)
Program.Delay(10000)
Во время прохождения роботом маршрута, в том числе при выполнении объездов текущие координаты робота периодически записываются в файл. Эти данные можно использовать в дальнейшем в программе на ПК для отображения на карте фактически пройденного пути. В тексте программ выше эта функция отсутствует, однако в полной версии программы, которую вы можете скачать, она реализована. Вот так вот выглядит фактически пройденный трек, загруженный из файла по нажатию кнопки "Действительный маршрут".
Скачать исходный код к нашего проекта можно в GitHub, а на видео ниже - посмотреть, как работает наш NAVIDOZ3R.
