У моего давнего британского партнёра (именно для него два года назад писалось «Распознавание почтовых адресов») появилась новая идея по оптимизации бизнес-процессов: коробки по складу должны возить роботы, а грузчики — только перекладывать товары с робота на полку и обратно. Смысл, естественно, не в том, чтобы за каждым роботом по пятам шёл грузчик, и принимался за погрузку-разгрузку, как только робот остановится — а чтобы роботов было намного больше, чем грузчиков, и чтобы роботы большую часть времени стояли в конечной точке своего маршрута, ожидая погрузки. Тогда грузчик будет лишь переходить от одного робота к следующему, нагружая каждый, и не будет тратить рабочее время на переноску товаров.
В прошлом году мы экспериментировали с платформой самоходного пылесоса Roomba. Новенький пылесос обошёлся нам около £300 (подержанный можно найти за £100 и даже дешевле), и в его состав входят два электропривода на колёса, два датчика касания спереди, инфракрасный датчик снизу (для обнаружения ступенек) и сверху (для поиска зарадной станции). Точный перечень датчиков зависит от модели: в протоколе предусмотрено до четырёх инфракрасных датчиков снизу, каждый из которых возвращает один бит («пол виден/не виден»). В любом случае, никаких дальномеров: все имеющиеся датчики однобитные. Кроме того, никаких «программируемых ардуин» в Roomba нет, и чтобы им управлять, нужно установить сверху лаптоп (или ардуину) и общаться с роботом по RS-232. Поигравшись с пылесосом вдоволь, мы так и оставили его пылиться на одной из полок склада.
В этом году мы решили попробовать Microsoft Robotics Development Studio (MRDS), для продвижения которого Microsoft сформулировала спецификацию «MRDS Reference Platform» — набор оборудования и протокол управления «стандартным» роботом. Эта спецификация позволила бы роботолюбам создавать совместимых роботов и переносить между ними программы. По сравнению с аппаратным оснащением пылесоса, Reference Platform намного сложнее и мощнее: в спецификацию включён Kinect, три ИК-дальномера и два ультразвуковых, а также датчики вращения колёс (encoders). Реализацию MRDS RP пока что предлагает только фирма Parallax под названием Eddie (порядка £1000, не включая Kinect). Необычайное сходство Eddie с фотографиями робота-прототипа в спецификации MRDS RP наводит на мысли, что спецификация создавалась в тесном сотрудничестве с Parallax, иначе говоря — Parallax удалось добиться, что Microsoft приняли их платформу за эталонную.
Кроме разнообразия датчиков, Eddie обладает механически впечатляющей платформой (заявленная грузоподъёмность 20кг, а мощности моторов достаточно, чтобы толкать впереди себя складской погрузчик) и программируемым контроллером Parallax Propeller, т.е. критические куски кода можно зашить непосредственно в робота, а не только командовать им с компа.
Британский Channel 4 включил в один из выпусков программы Gadget Man двухминутный фрагмент с демонстрацией Eddie, интегрированного в магазинную тележку. К сожалению, просмотреть программу на сайте Channel 4 могут только обладатели британских IP-адресов, а я мне не удалось её сграбить и перезалить (может, кому-нибудь из читателей удастся?) — поэтому привожу только стоп-кадры оттуда.
Просто управлять роботом «вперёд-назад-влево-вправо» — ничего хитрого, да и прямо в поставку Eddie включена программа такого «удалённого управления». Но рулить сворой роботов-грузчиков некому: каждый из них должен сам осознавать, в какой точке склада он находится, и прокладывать себе дорогу до следующей точки. Вот с этим определением положения и возникла основная проблема. GPS-подобные технологии отметаются: сквозь крышу склада сигнал от спутников не проходит. Kinect хорош для распознавания жестов, но как распознавать им складские полки? На самих полках уже наклеены уникальные баркоды через каждый метр, но во-первых, они считываются не слишком надёжно «на ходу» робота и на расстоянии 20см от полок (ближе нельзя — велик риск врезаться в опору или в выступающую с полки коробку). Во-вторых, чтобы подъехать к полке на расстояние 20см и прочитать баркод, уже нужно примерно понимать текущее положение — иначе робот, отъехавший от полок в сторону, теряет ориентацию полностью и непоправимо.
Первая попытка навигации основывалась на дальномерах: робот «нащупывает» полку и едет вдоль неё, поддерживая заданное расстояние, подобно тому, как пошатывающийся пьяница идёт, через шаг опираясь о стену. Сходство действительно было огромным: задержка на получение сигнала от дальномеров, обработку их в MRDS, формирование команды для моторов, и преодоление их инерции — составляла десятые доли секунды. За это время робот здорово «уводило» в сторону, так что «коррекция курса» каждый раз составляла десятки градусов, а траектория получалась широким зигзагом. Вдобавок к этому, дальномеры Eddie оказались не слишком точными — погрешность до ±5см — и довольно узконаправленными, т.е. полки распознавались только на одной, наперёд заданной высоте от пола.
Более перспективной оказалась навигация по датчикам колёс, дававшим пробег каждого колеса с точностью около ±7мм. В прошивку Eddie уже включено вычисление «пробега робота» (полусумма пробегов обоих колёс) и «направления робота» (разность пробегов колёс, с нормирующим коэффициентом), но текущие координаты (х, у) из только лишь значений двух счётчиков никак не вычислить. Нужно разбить траекторию робота на маленькие простые участки, и суммировать перемещение на каждом участке. На первый взгляд хочется приблизить путь робота ломаной; но на самом деле, «в покое» — при постоянной скорости моторов — робот будет двигаться по дуге окружности: тем большего радиуса, чем меньше разница между скоростями моторов. Напротив, точек излома, когда робот меняет направление, не сдвигаясь с места, в реальности нет и быть не может. Поэтому будем приближать траекторию робота последовательностью дуг.
Началось всё с чертежа на салфетке глубокой ночью, когда меня вдруг осенило:
В начале дуги робот был в точке A (известной), в конце — оказался в точке B (неизвестной), при этом левое колесо проехало известный путь S1 = φr1, а правое — известный путь S2 = φr2. Расстояние w между колеями постоянно, и задаётся конструкцией робота. Получаем:
r2 / r1 = S2 / S1 и r2 = r1 + w, откуда
(r1 + w) / r1 = S2 / S1
r1S1 + wS1 = r1S2
r1 = wS1 / (S2 — S1)
Значит, угол, на который повернулся робот, φ = S1/r1 = (S2 — S1) / w, а расстояние |AB| = 2|AM| = 2(r1 + w/2)sin(φ/2) = (2wS1 / (S2 — S1) + w)sin(φ/2) = w·sin(φ/2)(S2 + S1) / (S2 — S1)
Вот, по большому счёту, и вся математика для определения положения: каждый раз прибавляем к углу направления робота вычисленное φ, а текущие координаты сдвигаем на расстояние |AB|. Остаётся вопрос практический: как часто выполнять такие обновления позиции? Если слишком часто — упрёмся в неточность колёсных датчиков, отмеряющих пробег дискретными «тиками». Если слишком редко — приближение сегмента пути дугой окружности окажется недостаточно точным.
Я решил, что показание колёсного датчика наиболее точно непосредственно в момент «тика», поэтому я пересчитываю позицию робота по каждому тику любого колеса — но не чаще, чем время опроса робота, зафиксированное где-то около 80мс; и не реже, чем раз в секунду, чтобы остановленное колесо (например, при повороте) тоже регистрировалось. Все зарегистрированные «тики» сохраняются в список, поскольку для пересчёта позиции мне требуется пробег обоих колёс за один и тот же промежуток времени. Промежуток берём от более раннего из предыдущих «тиков» обоими колёсами, и линейно интерполируем по «списку тиков» пробег второго колеса в этот же момент. Для конца промежутка — точно так же, берём считанный датчиком пробег для «тикнувшего» колеса, и линейно экстраполируем пробег второго. Таким образом точность должна получаться максимально достижимой.
Каждый раз, когда позиция робота пересчитывается, нужно по-новой задать скорости колёс, чтобы направить робота к его заданной цели. Направить робота точно на нужный «азимут», вращаясь на месте — нереально: упомянутая погрешность пробега колёс в ±7мм оборачивается погрешностью в несколько градусов при повороте робота, т.е. робот всё равно заведомо поедет «вкось», и его придётся снова поворачивать по дороге. Поэтому, если направление на цель отличается от текущего не слишком сильно, я решил направлять робота к цели по дуге, без предварительной «наводки на азимут». (Если текущее направление отличается от нужного больше, чем на π/2, тогда такая дуга будет больше полуокружности, что явно непрактично. В этом случае роботу ничего не остаётся, кроме как вращаться на месте в нужную сторону.)
Чтобы рассчитать дугу, вернёмся к чертежу выше. Теперь нам известны точки A и B, а стало быть, и угол φ; найти нужно отношение S2 / S1 = r2 / r1: оно задаст отношение скоростей колёс, нужное для нашей дуги. Повторяем сделанный выше расчёт в обратную сторону:
|AB| = 2|AM| = 2(r1 + w/2)sin(φ/2)
r1 = |AB| / (2sin(φ/2)) — w/2
r2 = |AB| / (2sin(φ/2)) + w/2
r2 / r1 = 1 + w / (|AB|/(2sin(φ/2)) — w/2) = 1 + 2 / (|AB|/(w·sin(φ/2)) — 1)
Насколько далеко такая дуга уведёт нас в сторону от прямой AB? Если роботу для выполнения дуги нужно два метра свободного пространства по бокам — тогда на нашем складе просто нет такой возможности: ширина проходов между полками лишь полтора метра.
Вычислим расстояние |MH| между дугой и прямой: |OH|-|OM| = r — r·cos(φ/2) = (r1 + w/2) (1 — cos(φ/2))
Если величина этого «горба» превысит заданное ограничение, то мы отказываемся от движения по дуге, и, вращаясь на месте, попытаемся повернуться ближе в сторону цели.
Как оказалось, в действительности отношение напряжений на моторах ещё не задаёт отношение скоростей колёс. Можно было бы подбирать правильное преобразование между ними, но я решил задачу «грубой силой»: возвёл отношение в 9 степень, т.е. малейшее отклонение в одну из сторон тут же сильно толкает робот в противоположную сторону. На самом деле, именно с этим показателем степени движение робота стало по-настоящему гладким: с меньшими степенями он очень сильно отклонялся вбок, и возвращался к правильному маршруту только у самой цели; с большими — коррекция оказывалась слишком сильной, и робота «мотало» из стороны в сторону узким зигзагом.
Я задумывал добавить новую функциональность в сервис MarkRobot из поставки ReferencePlatform2011: реализовать новый портсет в духе
Для начала нужно что-то для работы с координатами: не в
Метод
(Только сейчас, скопировав сюда код, я заметил, у меня получилась в точности «формула двойного угла», и что вместо сдвига на |AB| под углом φ/2 можно было сдвинуться на r1+w/2 под углом φ. Ну да ладно.)
Дальше, сервис содержит внутри себя экземпляр
Кроме «списков тиков», в
Осталось дополнить класс
Всё, что осталось в коде интересного — обработчик
Этот робот ездил довольно неплохо, но проблема возникала из-за накапливающейся погрешности. Для коррекции координат достаточно было считывать успешно хотя бы 20% баркодов, и «перезапускать»
Намного хуже обстояло дело с ошибкой направления (heading), потому что направление нам скорректировать нечем: баркод может прочитаться под любым углом (и слава богу). Накопившейся ошибки направления в несколько градусов уже достаточно, чтобы робот ехал вкось, и въезжал со всего размаха в полку. Лобовым решением было бы установить гироскоп; но готового гироскопа для Eddie не существует, а таинств с паяльником и написанием прошивки мне хотелось бы избежать.
Коррекции направления без гироскопа и других экзотических датчиков я собираюсь посвятить следующую статью.
Предыстория
В прошлом году мы экспериментировали с платформой самоходного пылесоса Roomba. Новенький пылесос обошёлся нам около £300 (подержанный можно найти за £100 и даже дешевле), и в его состав входят два электропривода на колёса, два датчика касания спереди, инфракрасный датчик снизу (для обнаружения ступенек) и сверху (для поиска зарадной станции). Точный перечень датчиков зависит от модели: в протоколе предусмотрено до четырёх инфракрасных датчиков снизу, каждый из которых возвращает один бит («пол виден/не виден»). В любом случае, никаких дальномеров: все имеющиеся датчики однобитные. Кроме того, никаких «программируемых ардуин» в Roomba нет, и чтобы им управлять, нужно установить сверху лаптоп (или ардуину) и общаться с роботом по RS-232. Поигравшись с пылесосом вдоволь, мы так и оставили его пылиться на одной из полок склада.
В этом году мы решили попробовать Microsoft Robotics Development Studio (MRDS), для продвижения которого Microsoft сформулировала спецификацию «MRDS Reference Platform» — набор оборудования и протокол управления «стандартным» роботом. Эта спецификация позволила бы роботолюбам создавать совместимых роботов и переносить между ними программы. По сравнению с аппаратным оснащением пылесоса, Reference Platform намного сложнее и мощнее: в спецификацию включён Kinect, три ИК-дальномера и два ультразвуковых, а также датчики вращения колёс (encoders). Реализацию MRDS RP пока что предлагает только фирма Parallax под названием Eddie (порядка £1000, не включая Kinect). Необычайное сходство Eddie с фотографиями робота-прототипа в спецификации MRDS RP наводит на мысли, что спецификация создавалась в тесном сотрудничестве с Parallax, иначе говоря — Parallax удалось добиться, что Microsoft приняли их платформу за эталонную. Кроме разнообразия датчиков, Eddie обладает механически впечатляющей платформой (заявленная грузоподъёмность 20кг, а мощности моторов достаточно, чтобы толкать впереди себя складской погрузчик) и программируемым контроллером Parallax Propeller, т.е. критические куски кода можно зашить непосредственно в робота, а не только командовать им с компа.
Британский Channel 4 включил в один из выпусков программы Gadget Man двухминутный фрагмент с демонстрацией Eddie, интегрированного в магазинную тележку. К сожалению, просмотреть программу на сайте Channel 4 могут только обладатели британских IP-адресов, а я мне не удалось её сграбить и перезалить (может, кому-нибудь из читателей удастся?) — поэтому привожу только стоп-кадры оттуда.
Задача
Просто управлять роботом «вперёд-назад-влево-вправо» — ничего хитрого, да и прямо в поставку Eddie включена программа такого «удалённого управления». Но рулить сворой роботов-грузчиков некому: каждый из них должен сам осознавать, в какой точке склада он находится, и прокладывать себе дорогу до следующей точки. Вот с этим определением положения и возникла основная проблема. GPS-подобные технологии отметаются: сквозь крышу склада сигнал от спутников не проходит. Kinect хорош для распознавания жестов, но как распознавать им складские полки? На самих полках уже наклеены уникальные баркоды через каждый метр, но во-первых, они считываются не слишком надёжно «на ходу» робота и на расстоянии 20см от полок (ближе нельзя — велик риск врезаться в опору или в выступающую с полки коробку). Во-вторых, чтобы подъехать к полке на расстояние 20см и прочитать баркод, уже нужно примерно понимать текущее положение — иначе робот, отъехавший от полок в сторону, теряет ориентацию полностью и непоправимо.
Первая попытка навигации основывалась на дальномерах: робот «нащупывает» полку и едет вдоль неё, поддерживая заданное расстояние, подобно тому, как пошатывающийся пьяница идёт, через шаг опираясь о стену. Сходство действительно было огромным: задержка на получение сигнала от дальномеров, обработку их в MRDS, формирование команды для моторов, и преодоление их инерции — составляла десятые доли секунды. За это время робот здорово «уводило» в сторону, так что «коррекция курса» каждый раз составляла десятки градусов, а траектория получалась широким зигзагом. Вдобавок к этому, дальномеры Eddie оказались не слишком точными — погрешность до ±5см — и довольно узконаправленными, т.е. полки распознавались только на одной, наперёд заданной высоте от пола.Более перспективной оказалась навигация по датчикам колёс, дававшим пробег каждого колеса с точностью около ±7мм. В прошивку Eddie уже включено вычисление «пробега робота» (полусумма пробегов обоих колёс) и «направления робота» (разность пробегов колёс, с нормирующим коэффициентом), но текущие координаты (х, у) из только лишь значений двух счётчиков никак не вычислить. Нужно разбить траекторию робота на маленькие простые участки, и суммировать перемещение на каждом участке. На первый взгляд хочется приблизить путь робота ломаной; но на самом деле, «в покое» — при постоянной скорости моторов — робот будет двигаться по дуге окружности: тем большего радиуса, чем меньше разница между скоростями моторов. Напротив, точек излома, когда робот меняет направление, не сдвигаясь с места, в реальности нет и быть не может. Поэтому будем приближать траекторию робота последовательностью дуг.
Расчёты
Началось всё с чертежа на салфетке глубокой ночью, когда меня вдруг осенило:
В начале дуги робот был в точке A (известной), в конце — оказался в точке B (неизвестной), при этом левое колесо проехало известный путь S1 = φr1, а правое — известный путь S2 = φr2. Расстояние w между колеями постоянно, и задаётся конструкцией робота. Получаем:
r2 / r1 = S2 / S1 и r2 = r1 + w, откуда
(r1 + w) / r1 = S2 / S1
r1S1 + wS1 = r1S2
r1 = wS1 / (S2 — S1)
Значит, угол, на который повернулся робот, φ = S1/r1 = (S2 — S1) / w, а расстояние |AB| = 2|AM| = 2(r1 + w/2)sin(φ/2) = (2wS1 / (S2 — S1) + w)sin(φ/2) = w·sin(φ/2)(S2 + S1) / (S2 — S1)
Вот, по большому счёту, и вся математика для определения положения: каждый раз прибавляем к углу направления робота вычисленное φ, а текущие координаты сдвигаем на расстояние |AB|. Остаётся вопрос практический: как часто выполнять такие обновления позиции? Если слишком часто — упрёмся в неточность колёсных датчиков, отмеряющих пробег дискретными «тиками». Если слишком редко — приближение сегмента пути дугой окружности окажется недостаточно точным.
Я решил, что показание колёсного датчика наиболее точно непосредственно в момент «тика», поэтому я пересчитываю позицию робота по каждому тику любого колеса — но не чаще, чем время опроса робота, зафиксированное где-то около 80мс; и не реже, чем раз в секунду, чтобы остановленное колесо (например, при повороте) тоже регистрировалось. Все зарегистрированные «тики» сохраняются в список, поскольку для пересчёта позиции мне требуется пробег обоих колёс за один и тот же промежуток времени. Промежуток берём от более раннего из предыдущих «тиков» обоими колёсами, и линейно интерполируем по «списку тиков» пробег второго колеса в этот же момент. Для конца промежутка — точно так же, берём считанный датчиком пробег для «тикнувшего» колеса, и линейно экстраполируем пробег второго. Таким образом точность должна получаться максимально достижимой.
Каждый раз, когда позиция робота пересчитывается, нужно по-новой задать скорости колёс, чтобы направить робота к его заданной цели. Направить робота точно на нужный «азимут», вращаясь на месте — нереально: упомянутая погрешность пробега колёс в ±7мм оборачивается погрешностью в несколько градусов при повороте робота, т.е. робот всё равно заведомо поедет «вкось», и его придётся снова поворачивать по дороге. Поэтому, если направление на цель отличается от текущего не слишком сильно, я решил направлять робота к цели по дуге, без предварительной «наводки на азимут». (Если текущее направление отличается от нужного больше, чем на π/2, тогда такая дуга будет больше полуокружности, что явно непрактично. В этом случае роботу ничего не остаётся, кроме как вращаться на месте в нужную сторону.)
Чтобы рассчитать дугу, вернёмся к чертежу выше. Теперь нам известны точки A и B, а стало быть, и угол φ; найти нужно отношение S2 / S1 = r2 / r1: оно задаст отношение скоростей колёс, нужное для нашей дуги. Повторяем сделанный выше расчёт в обратную сторону:
|AB| = 2|AM| = 2(r1 + w/2)sin(φ/2)
r1 = |AB| / (2sin(φ/2)) — w/2
r2 = |AB| / (2sin(φ/2)) + w/2
r2 / r1 = 1 + w / (|AB|/(2sin(φ/2)) — w/2) = 1 + 2 / (|AB|/(w·sin(φ/2)) — 1)
Насколько далеко такая дуга уведёт нас в сторону от прямой AB? Если роботу для выполнения дуги нужно два метра свободного пространства по бокам — тогда на нашем складе просто нет такой возможности: ширина проходов между полками лишь полтора метра.
Вычислим расстояние |MH| между дугой и прямой: |OH|-|OM| = r — r·cos(φ/2) = (r1 + w/2) (1 — cos(φ/2))
Если величина этого «горба» превысит заданное ограничение, то мы отказываемся от движения по дуге, и, вращаясь на месте, попытаемся повернуться ближе в сторону цели.
Как оказалось, в действительности отношение напряжений на моторах ещё не задаёт отношение скоростей колёс. Можно было бы подбирать правильное преобразование между ними, но я решил задачу «грубой силой»: возвёл отношение в 9 степень, т.е. малейшее отклонение в одну из сторон тут же сильно толкает робот в противоположную сторону. На самом деле, именно с этим показателем степени движение робота стало по-настоящему гладким: с меньшими степенями он очень сильно отклонялся вбок, и возвращался к правильному маршруту только у самой цели; с большими — коррекция оказывалась слишком сильной, и робота «мотало» из стороны в сторону узким зигзагом.
Реализация
Я задумывал добавить новую функциональность в сервис MarkRobot из поставки ReferencePlatform2011: реализовать новый портсет в духе
PositionOperations : PortSet<GetPosition, SetPosition, SetDestination>, и чтобы сервис сам, прозрачно для пользователя, опрашивал колёсные датчики и выставлял напряжения на моторах. Фактически до этой реализации дело не дошло — готов был только мутный прототип без разделения ответственностей между сервисами. Попробую, тем не менее, показать, что у меня получалось.Для начала нужно что-то для работы с координатами: не в
System.Drawing.PointF же их хранить.public struct Position
{
public readonly double x, y, heading;
public Position(double x, double y, double heading)
{
this.x = x;
this.y = y;
this.heading = heading;
}
private static double Sqr(double d)
{
return d * d;
}
public double DistanceTo(double x, double y)
{
return Math.Sqrt(Sqr(this.y - y) + Sqr(this.x - x));
}
public static double NormalizeHeading(double heading)
{
while (heading < -Math.PI) heading += 2 * Math.PI;
while (heading > Math.PI) heading -= 2 * Math.PI;
return heading;
}
// turn angle/2, go ahead distance, turn angle/2 again
public Position advance(double distance, double angle)
{
double newHeading = heading + angle / 2,
newX = x + distance * Math.Cos(newHeading),
newY = y + distance * Math.Sin(newHeading);
return new Position(newX, newY, NormalizeHeading(heading + angle));
}
}
Метод
advance реализует пересчёт координат по заданному углу поворота φ и пройденному расстоянию |AB|, как на чертеже в середине статьи.(Только сейчас, скопировав сюда код, я заметил, у меня получилась в точности «формула двойного угла», и что вместо сдвига на |AB| под углом φ/2 можно было сдвинуться на r1+w/2 под углом φ. Ну да ладно.)
Дальше, сервис содержит внутри себя экземпляр
PositionKeeping, внутри которого два экземпляра EncoderLog — «списка тиков», отдельный экземпляр для каждого колеса. Список храним в SortedList, потому что (теоретически) данные датчиков могут прибывать не в хронологическом порядке. Единственный нетривиальный метод в EncoderLog — линейная апроксимация пробега для получения его значения в любой момент времени между «тиками» или после последнего «тика».private class PositionKeeping {
private static readonly DateTime initialized = DateTime.Now;
public class EncoderLog {
private SortedList<DateTime, double> log = new SortedList<DateTime, double> { { initialized, 0 } };
public void Register(DateTime at, double reading) { log[at] = reading; }
public void Reset(DateTime at, double reading) { log.Clear(); log.Add(at, reading); }
public DateTime LastTick { get { return log.Last().Key; } }
public double LastReading { get { return log.Last().Value; }}
public double ReadingAt(DateTime at) {
int index = log.Count - 1;
while(index>=0 && log.Keys[index] > at)
index--;
if(index<0) return double.NaN; // before first reading; impossible
// now, log.Keys[index] <= at, and log.Keys[index+1] > at
DateTime preceding = log.Keys[index],
following = index<log.Count-1 ? log.Keys[index+1] : DateTime.MaxValue;
if(following == DateTime.MaxValue) {
// last reading precedes at; extrapolate
if(index==0) // there's only one reading
return log[preceding];
else {
DateTime nextPreceding = log.Keys[index-1];
return log[nextPreceding] + (at-nextPreceding).TotalSeconds * (log[preceding]-log[nextPreceding]) / (preceding-nextPreceding).TotalSeconds;
}
} else // both readings are available; interpolate
return log[preceding] + (at-preceding).TotalSeconds * (log[following]-log[preceding]) / (following - preceding).TotalSeconds;
}
}
public EncoderLog leftEnc = new EncoderLog(),
rightEnc = new EncoderLog();
Кроме «списков тиков», в
PositionKeeping должен храниться список позиций на момент каждого «тика» — от этой сохранённой позиции мы будем отмерять пройденное расстояние, чтобы получить новую позицию. private SortedList<DateTime, Position> position = new SortedList<DateTime, Position> { { initialized, new Position(0, 0, Math.PI / 2) } };
public void Register(DateTime at, Position pos) { position.Add(at, pos); }
public void Reset(DateTime at, Position pos) {
// the position has changed => old ticks logs become obsolete
leftEnc.Reset(at, leftEnc.ReadingAt(at));
rightEnc.Reset(at, rightEnc.ReadingAt(at));
position.Clear();
position.Add(at, pos);
}
public Position Current { get { return position.Last().Value; } }
Осталось дополнить класс
PositionKeeping вычислением новой позиции по данным обоих колёсных датчиков — в соответствии с выведенными формулами. Update может вызываться из цикла проверки датчиков (ServiceHandlerBehavior.Concurrent), и принимает аргументом порт сервиса, куда пересылает новые координаты, если зарегистрирован «тик». Это сообщение изменяет состояние сервиса, поэтому оно должно обрабатываться как ServiceHandlerBehavior.Exclusive. private static readonly TimeSpan RegisterDelay = TimeSpan.FromSeconds(1); // register null-tick if no actual ticks for this long
public void Update(DateTime at, double left, double right, PositionOperations mainPort)
{
DateTime prevRef = Min(leftEnc.LastTick, rightEnc.LastTick);
SetPosition set = new SetPosition
{
Timestamp = at,
LeftEncUpdated = left != leftEnc.LastReading || at > leftEnc.LastTick + RegisterDelay,
LeftEncReading = left,
RightEncUpdated = right != rightEnc.LastReading || at > rightEnc.LastTick + RegisterDelay,
RightEncReading = right
};
if(set.LeftEncUpdated || set.RightEncUpdated)
{
set.Position = Recalculate(prevRef, left, right);
mainPort.Post(set);
}
}
private Position Recalculate(DateTime prevRef, double left, double right)
{
double sLeft = left - leftEnc.ReadingAt(prevRef),
sRight = right - rightEnc.ReadingAt(prevRef);
Position refPos = position[prevRef]; // has to exist if the encoder reference exists
if (Math.Abs(sRight - sLeft) < .5) // less then half-tick difference: go straight
return refPos.advance(Constants.CmPerTick * (sRight + sLeft) / 2, 0);
else
{
double angle = Constants.CmPerTick * (sRight - sLeft) / Constants.WheelsDist,
distance = Constants.WheelsDist * Math.Sin(angle / 2) * (sRight + sLeft) / (sRight - sLeft);
return refPos.advance(distance, angle);
}
}
}
Всё, что осталось в коде интересного — обработчик
SetPosition, задающий напряжения на моторах так, чтобы робот двигался к цели.[ServiceHandler(ServiceHandlerBehavior.Exclusive)]
public void SetPositionHandler(SetPosition set)
{
if (!set.LeftEncUpdated && !set.RightEncUpdated)
{ // position updated by an absolute reference.
positionKeeping.Reset(set.Timestamp, set.Position);
}
else
{
if (set.LeftEncUpdated) positionKeeping.leftEnc.Register(set.Timestamp, set.LeftEncReading);
if (set.RightEncUpdated) positionKeeping.rightEnc.Register(set.Timestamp, set.RightEncReading);
positionKeeping.Register(set.Timestamp, set.Position);
}
// the navigator
Destination dest = state.dest;
double distance = set.Position.DistanceTo(dest.x, dest.y);
if (distance < 5) // reached
{
drivePort.SetDrivePower(0, 0);
SendNotification(submgrPort, new DriveDistance());
return;
}
double heading = Position.NormalizeHeading(Math.Atan2(dest.y - set.Position.y, dest.x - set.Position.x)),
power = (distance < 50) ? .2 : .4; // a few magic numbers
if (Math.Abs(heading) < .05)
{ // straight ahead
drivePort.SetDrivePower(power, power);
return;
}
double r = distance / (2 * Math.Sin(heading / 2)),
hump = r * (1 - Math.Cos(heading / 2));
if (Math.Abs(heading) > Math.PI / 2 || Math.Abs(hump) > Constants.MaxHump)
{
// not reachable by an arc; rotate
if (heading > 0) // rotate left
drivePort.SetDrivePower(-.3, .3);
else // rotate right
drivePort.SetDrivePower(.3, -.3);
}
else
{
// go in arc
double rLeft = Math.Abs(r - Constants.WheelsDist / 2),
rRight = Math.Abs(r + Constants.WheelsDist / 2),
rMax = Math.Max(rLeft, rRight);
// <Patrician|Away> what does your robot do, sam
// <bovril> it collects data about the surrounding environment, then discards it and drives into walls
drivePort.SetDrivePower(power * Math.Pow(rLeft / rMax, 9), power * Math.Pow(rRight / rMax, 9));
}
}
Что дальше?
Этот робот ездил довольно неплохо, но проблема возникала из-за накапливающейся погрешности. Для коррекции координат достаточно было считывать успешно хотя бы 20% баркодов, и «перезапускать»
PositionKeeping вызовом SetPosition с LeftEncUpdated = RightEncUpdated = false. За несколько метров между считанными баркодами ошибка в определении координат не превышала 20см — как раз столько мы и оставляли про запас между роботом и полками.Намного хуже обстояло дело с ошибкой направления (heading), потому что направление нам скорректировать нечем: баркод может прочитаться под любым углом (и слава богу). Накопившейся ошибки направления в несколько градусов уже достаточно, чтобы робот ехал вкось, и въезжал со всего размаха в полку. Лобовым решением было бы установить гироскоп; но готового гироскопа для Eddie не существует, а таинств с паяльником и написанием прошивки мне хотелось бы избежать.
Коррекции направления без гироскопа и других экзотических датчиков я собираюсь посвятить следующую статью.
