Конструкторы Mindstorms компании LEGO находят самое широкое применение в образовательном процессе, как в школах, так и в высших учебных заведениях. Распространенность и популярность роботов серии Mindstorms обоснована, в первую очередь, их доступностью широкому кругу пользователей. Многие с детства так или иначе сталкивались с конструкторами компании LEGO. Вместе с тем статьи по программированию данных конструкторов на текстовых языках встречаются редко. Скорее всего это связано со стоимостью наборов (от 20 тыс. рублей), на эту сумму можно купить кучу всего (Arduino, AVR, STM32, различные модули и прочая рассыпуха) и собрать больше интересных устройств. Мне в этом плане повезло, сначала в университете, а теперь и в лицее, где я преподаю, этих конструкторов предостаточно. Об одном реализованном проекте я бы хотел рассказать.
Конструкция робота
Началось все с того, что на Youtube был найден ролик с интересной конструкцией робота.
Конструкция показалась нам интересной и мы с ребятами решили её собрать. Сначала пытались скопировать, но потом удалось найти инструкцию. Конструкция робота разработана Лауренсом Валком (Laurens Valk). Робот представляет собой гусеничную платформу с манипулятором, способную передвигаться в различных направлениях, захватывать, поднимать и перемещать небольшие предметы. Конструкция включает датчики расстояния (ультразвуковой дальномер), касания и цвета. Дальномер позволяет роботу оценивать расстояние до объектов перед ним и избегать столкновений. Датчик касания используется для определения крайней точки подъёма захвата. Датчик цвета предназначен для определения цвета захваченного объекта. Манипулятор робота имеет одну степень свободы – он может только подниматься и опускаться. Управление манипулятором (захват и подъём/опускание) осуществляется с помощью отдельного сервопривода. В общем конструкция представляет собой удобную платформу для экспериментов с автономными роботами и может успешно использоваться в различных соревнованиях.
 |
 |
 |
 |
 |
 |
 |
 |
 |
 |
 |
 |
Контроллер NXT используемый в наборах LEGO Mindstorms позволяет подключать до 3-х сервомоторов (порты A, B, C). Скоростью вращения каждого из сервомоторов можно управлять, задавая значения от -100 до 100. При этом отрицательные значения означают инверсию направления вращения мотора. Сервомоторы имеют встроенные датчики угла поворота (энкодеры), что позволяет опрашивать их с целью вычисления пройденного расстояния. Точность составляет 1 градус.
Моторы, управляющие движением робота, подключены к портам B и C. Они позволяют роботу двигаться поступательно по прямолинейной траектории (двигатели В и С синхронизированы). Поворот робота происходит за счет увеличения мощности (скорости вращения вала двигателя) одного двигателя относительного другого. Конструкция робота примечательна тем, что один мотор, подключенный к оставшемуся порту A, используется как для открытия/закрытия захвата, так и для его поднятия/опускания. Реализовано это за счет сложной механической части. В связи с этим робот имеет определенные ограничения – например, невозможно раскрыть захват в поднятом состоянии. Подключение к порту A сервомотора, управляющего захватом, обусловлено схемотехникой управляющего контроллера: данный порт способен выдавать больший ток на двигатель, что позволяет поднимать предметы с большим весом.
Поскольку контроллеры NXT оснащены 4-мя входными портами, остается незадействованным еще один порт. В ходе работы было предложено добавить датчик-гироскоп, как наиболее перспективный вариант развития возможностей робота. Датчики-гироскопы играют важную роль в современной робототехнике.
Для этого в конструкцию был добавлен датчик-гироскоп компании HiTechnic. Это одноосный гироскоп, позволяющий измерять скорость и направление вращения в горизонтальной плоскости. Датчик позволяет измерять угловые скорости до ±360° в секунду (знак зависит от направления вращения). Частота опроса датчика может достигать до 300 Гц.
Одной из областей применения гироскопов является инерциальная навигация. Инерциальная навигация – метод определения координат и параметров движения различных объектов (судов, самолетов и др.) и управления их движением, основанный на свойствах инерции тел. Принцип инерциальной навигации заключается в измерении движений объекта, характеризуемых изменениями во времени его ускорения, скорости и координат, при помощи датчиков пространственного перемещения. Полученные от датчиков данные используются для решения задач управления и навигации. Исходя из вышесказанного были поставлены две задачи:
- Робот должен уметь находить и захватывать ближайший к нему предмет (как в ролике выше);
- Робот должен уметь ориентироваться на плоскости и, зная свои текущие координаты, самостоятельно возвращаться в точку старта.
Однако для начала необходимо было определиться со средствами программирования.
Средства программирования
Контроллеры LEGO Mindstorms NXT могут программироваться с использованием как графических, так и текстовых сред разработки. В графических средах программирования программы «собираются» из отдельных «кубиков», каждый из которых представляет набор определенных команд. Графические среды программирования наглядны и очень просты в использовании, что позволяет быстро освоить их человеку с любым уровнем подготовки. Самым распространенным примером подобной среды является среда NXT-G. Однако недостатком графических сред является сложность разработки объемных программ (ввиду большого количества блоков), а также ограниченные функциональные возможности.
Другим типом сред программирования являются текстовые среды. В настоящей работе для разработки программ использовалась среда RobotC – текстовая среда программирования, позволяющая разрабатывать программы для управления LEGO Mindstorms, используя популярный язык программирования Си. Среда разработана в Академии Робототехники Карнеги-Меллона и ориентирована в первую очередь на студентов. RobotC обладает простым в использовании интерфейсом, а также имеет мощный интерактивный отладчик, позволяющий производить отладку программы в режиме реального времени. Это значительно сокращает время, необходимое для поиска и исправления ошибок в программе. Для выполнения программ, написанных на RobotC, необходимо предварительно загрузить специальную прошивку. Использование данной прошивки позволяет получить доступ к большим возможностям управляющего контроллера, а также увеличить скорость работы программ.
Разработка программы
Первоначально было поставлено две цели: научить робота хватать ближайший к нему предмет и научить робота ориентироваться на плоскости. Под ориентированием подразумевается определение текущих координат относительно начальных. Поскольку датчик-гироскоп измеряет угловую скорость, то для преобразования показаний, необходимо произвести интегрирование показаний по времени. Но для этого надо постоянно отслеживать показания датчика, а выполнять это в основном цикле программы накладно. Одной из особенностей контроллеров LEGO Mindstorms NXT является поддержка параллельного выполнения нескольких процессов – многозадачности. То есть робота можно запрограммировать на одновременное выполнение нескольких действий. Поэтому код обработки показаний датчика-гироскопа выделяется в отдельную задачу, которая называется драйвером гироскопа. Ниже представлен код такого драйвера.
float gyroAngle = 0.0;
float gyroBias = 0.0;
task GyroDriver()
{
float gyroVelocity;
long samplesSum = 0;
// Суммируем 100 сэмплов гироскопа для определения смещения (bias)
for (int i = 0; i < 100; i++)
{
samplesSum += SensorRaw[gyroPort];
wait1Msec(10);
}
gyroBias = (float)samplesSum / 100;
// Бесконечный цикл для обновления угла по данным с гироскопа
while (true)
{
gyroVelocity = SensorValue[gyroPort] - gyroBias;
gyroAngle += gyroVelocity * 0.01; // Интегрирование скорости для получения угла
}
}
Контроллеры LEGO Mindstorms NXT поддерживают одновременное выполнение до 10 задач. Ресурсы процессора распределяются между задачами, для каждой из которых устанавливается время, в течение которого задача будет активна.
Для поворота робота на заданный угол также написана отдельная подпрограмма, принимающая в качестве входных параметров угол поворота и скорость поворота. Положительному углу поворота соответствует поворот робота вправо, а отрицательному – влево. При вызове функции текущий угол отклонения обнуляется, затем в цикле осуществляется поворот до тех пор, пока текущий угол не равен заданному значению угла. Поворот осуществляется вращением двигателей в противоположные стороны. Код функции поворота представлен ниже.
void Rotate(float angle = 180, int speed = 30)
{
ResetMotor(motorB);
ResetMotor(motorC);
speed = abs(speed);
if (angle > 0)
speed *= -1;
angle = abs(angle);
gyroAngle = 0;
nSyncedMotors = synchBC;
nSyncedTurnRatio = -100;
motor[motorB] = speed;
while (abs(gyroAngle) < angle)
{
// Подождать перед повторной проверкой гироскопа
wait1Msec(10);
}
ResetMotor(motorB);
ResetMotor(motorC);
}
Для движения робота по прямой (вперёд/назад) также написана отдельная функция, входными параметрами которой являются скорость движения и расстояние в сантиметрах, которое необходимо проехать. Код функции приведен ниже.
void Move(int dist, int speed = 40)
{
if (dist * speed < 0)
speed = -abs(speed);
dist = abs(dist);
ResetMotor(motorB);
ResetMotor(motorC);
nSyncedMotors = synchBC;
nSyncedTurnRatio = 100;
motor[motorB] = speed;
while (abs(nMotorEncoder[motorB]) * kMove < dist)
{
// Подождать перед повторной проверкой пути
wait1Msec(10);
}
ResetMotor(motorB);
ResetMotor(motorC);
}
Алгоритм поиска ближайшего объекта
Поиск ближайшего объекта робот начинает с поворота влево на 90°. Далее запускается цикл поиска объекта. Робот производит сканирование окружающего пространства ультразвуковым дальномером, полученное расстояние сравнивается с хранящимся в памяти минимальным расстоянием (на первой итерации оно равно 255). Если измеренное значение расстояния меньше минимального, то значение, соответствующее минимальному измеренному расстоянию обновляется, вместе с ним также обновляется и угол отклонения, при котором были получено данное минимальное значение расстояния. Цикл завершается при граничном угле в 90°. Таким образом робот сканирует переднюю полусферу и получает пару значений угол/расстояние, которые соответствуют ближайшему к роботу объекту. Затем производится поворот на полученный угол, движение до объекта на полученное расстояние (с корректировкой на размеры манипулятора) и последующий захват объекта. Ниже представлено видео работы программы.
Алгоритм навигации на плоскости
На базе алгоритма опроса гироскопа был разработан алгоритм навигации, позволяющий роботу самостоятельно возвращатсья в точку старта. Робот проезжает по нескольким точкам, координаты которых генерируются случайным образом, а затем возвращается в точку старта. Работает это следующим образом. В цикле от 1 до 10 (количество точек траектории задается в коде) с помощью генератора случайных чисел задаются новые значения угла поворота и расстояния до новой точки, затем робот перемещается в новую точку. Одновременно с этим, в цикле производится вычисление суммарного угла отклонения, а также значения проекций по осям OX и OY. Проекции вычисляются на основе суммарного угла поворота. После выполнения цикла прохождения траектории, вычисляется угол, на который необходимо повернуться роботу, чтобы начать движение к исходной точке по кратчайшему маршруту. Затем вычисляется расстояние по формуле модуля длины вектора, и робот движется к исходной точке. После прихода в исходную точку, робот вычисляет итоговое отклонение угла от начального значения при запуске и поворачивает на вычисленное значение угла. Программа завершается. Ниже представлено видео работы программы.
Работа с Bluetooth-модулем
Беспроводной Bluetooth-модуль, встроенный в контроллер NXT, может использоваться для удаленной загрузки и отладки программ, а также управления роботом. Управлять роботом можно как с компьютера (из среды RobotC), так и с помощью мобильного телефона, предварительно загрузив специальное приложение. Bluetooth-модуль контроллера NXT позволяет связывать до 4-х устройств. При подключении контроллеров друг к другу одно из устройств будет ведущим (master), а остальные будут ведомыми (slave). Такое разделение необходимо для контроля передачи данных. К одному ведущему контроллеру может подключаться до 3-х других ведомых (подчиненных) контроллеров.
Обмен информацией разрешен только между ведущим и ведомым устройствами. Контроллер, работающий в режиме ведомого устройства, не может подключаться к другим контроллерам, поэтому ведомые устройства не могут вести обмен без участия ведущего устройства. В каждый момент времени передавать данные в группе может только одно из устройств
Для организации беспроводного взаимодействия между несколькими контроллерами NXT необходимо предварительно их «познакомить» друг с другом – подключить их друг к другу один раз в ручном режиме. Это необходимо для того, чтобы произвести предварительную настройку параметров соединения (контроллеры обмениваются адресами, именами, списком поддерживаемых профилей, также проверяется совпадение паролей). В дальнейшем эту процедуру можно не повторять, за исключением случаев сброса всех настроек и соединений Bluetooth-модуля. Для соединения двух контроллеров NXT необходимо проделать следующее:
- Включить Bluetooth модуль на обоих контроллерах NXT;
- Проверить режим видимости (visibility), должно быть установлено значение «visible». Это необходимо, чтобы контроллеры могли друг друга обнаружить;
- На контроллере, который планируется использовать в качестве ведущего войти в меню поиска других устройств (Bluetooth -> Search) и запустить поиск;
- Выбрать ведомое устройство, затем подключиться, указав один из предложенных каналов подключения (поскольку подключение начинается с ведущего устройства, то будут предложены номера 1-3). Если нужного контроллера NXT нет в списке, то проверить настройки Bluetooth модуля ведомого устройства, а затем повторить поиск;
- После настройки параметров соединения ведущий контроллер издаст короткий звуковой сигнал и предложит ввести пароль для соединения. По умолчанию задан пароль 1234. При необходимости можно его изменить, но это не обязательно;
- Затем необходимо ввести указанный пароль на ведомом устройстве. Если пароли совпадут, то соединение будет успешно создано и в левом верхнем углу экрана контроллера NXT значок Bluetooth соединения изменится, возле него появится закрывающая угловая скобка.
Для управления роботом было решено собрать пульт управления. Конструкция довольно простая – контроллер и 4 датчика касания (кнопки). Пульт подключается по Bluetooth к роботу и позволяет дистанционно упралять роботом. Четыре датчика касания используются для движения, управление клешней производится нажатием на центральную (оранжевую) кнопку контроллера пульта. На рисунке ниже показан общий вид пульта.
Для обмена информацией между контроллерами в RobotC используются функции cCmdMessageRead и cCmdMessageWriteToBluetooth. При передаче указывается номер канал подключенного устройства, указатель на массив данных и его размер. Однако просто управлять роботом не так интересно. Поэтому мы собрали второго такого же робота и решили попробовать управлять с одного пульта двумя роботами. Что из этого вышло смотрите в видео.
При желании можно подключить еще одного робота. В версии конструктора NXT к одному контроллеру подключается до трёх устройств. В новых конструкторах EV3 возможно объединение в сеть уже до 8 устройств, что позволяет собирать более сложных роботов.
Доработка конструкции
В ходе экспериментов с роботом были проанализированы возможности усовершенствования его конструкции. После прочтения данной статьи, была предпринята попытка замены электрического привода захвата на пневматический. Для этого был использован поршень из набора LEGO Pneumatics. Ниже на рисунке показана клешня с пневматическим приводом.
 |
 |
Преимуществом пневматического привода являются скорость срабатывания, меньший вес исполнительных устройств (по сравнению с электрическими приводом), простота и надёжность. С конструктивной точки зрения использование пневматического привода позволило упростить конструкцию манипулятора, но потребовало еще одного сервомотора для компрессора, что в свою очередь приведёт к необходимости использования второго контроллера, либо специализированного мультиплексора моторов. Использование вместо компрессора только баллона со сжатым воздухом не представляется возможным из-за утечек воздуха при переключении клапанов. Кроме того, для управления клапанами и контроля давления в воздушном баллоне потребовались сервомоторы и датчики сторонних производителей. Таким образом, без использования сторонних компонентов, которые не всегда возможно приобрести, данная идея была признана несостоятельной.
Другим направлением совершенствования робота стала доработка гусеничного шасси. В оригинальной конструкции робота гусеницы натянуты слабо, что приводит к их проскальзыванию. На сайте Thingiverse.com была найдена 3D-модель рамы, которая позволила увеличить натяжение резиновых гусениц и уменьшить проскальзывание. Размеры модели, а также диаметры отверстий были определены экспериментальным путем. Доработка модели осуществлялась в программе Blender. После этого модель была распечатана из PLA-пластика на 3D-принтере MakerBot 2. Полученная деталь была использована для доработки конструкции робота. Благодаря наличию дополнительных отверстий под оси, удалось увеличить общую жесткость конструкции робота. Как результат натяжение гусениц увеличилось, точность перемешений робота возросла. Внешний вид распечатанной детали, а также доработанной конструкции шасси робота показан ниже.
 |
 |
Кстати существует и вариант для версии Mindstorms EV3. Видео ниже.