Pull to refresh

Шести-степенная жизнь на хвосте питона

Reading time 8 min
Views 11K
Продолжая цикл статей моделирования параллельных механизмов с использованием аппаратной платформы Arduino и различного подручного хлама (Ловим горизонт с Arduino, Трех-степенной манипулятор на Arduino), сегодня стоит уделить внимание управлению шести-степенной платформой с помощью языка Python из операционной системы Linux.

В предыдущих постах я рассказывал о перспективе создания макета шести-степенной платформы Стьюарта. И так все компоненты пришли из далекого Китая, а также собраны на столе, можно приступать!

image


Инструменты и материалы


Разработка предыдущих макетов производилась с использованием ноутбука с операционной системой Windows 7, с установленной Visual Studio 2008. Все программирование осуществлялось на языке C# с библиотекой Microsoft .NET Framework.

Так как основной машиной для работы и выполнения повседневных задач является стационарный компьютер с установленной операционной системой Ubuntu Linux 10.4, то было решено освоить взаимодействие на Python с Arduino. Почему бы не вооружиться опытом работы с «железкой» из различных сред ;)

Для работы нам понадобятся следующие инструменты и материалы:
Найменование Количество
Агрегат с установленным Linux 1 шт.
Шести-степенная платформа 1 шт.
Arduino Mega 1 шт.
Макетная плата 1 шт.
Стяжка 6 шт.
Сервоприводы 6 шт.
Уголки 6 шт.
Шаровые опоры 12 шт.
Стержни от гелиевых ручек 6 шт.
Соединительные провода 20 шт.
Пингвин + скотч 1 шт.
Джойстик 1 шт.

Из программного обеспечения необходимо:
  • Linux
  • Arduino 0022
  • Python
  • IPython
  • PyGame
  • PySerial
  • PyQt4


Собираем платформу


Область применения механизмов с различными степенями свободы, в основе которых лежат параллельные кинематическими связями очень широка:
  • многокоординатные фрезерные и сверлильные станки
  • симуляторы летательных аппаратов
  • регулируемые шарнирные фермы для позиционирования объектов
  • самописцы т.д.

Масштабируемость таких механизмов позволяет легко строить на их основе крупногабаритные агрегаты, обладающие высокой скоростью, а большие ускорения конструкции применяются для создания различных промышленных роботов или платформ, например, для телескопов, лазеров, телекоммуникационных антенн или реактивных двигателей.

Подробное описание работы с сервоприводами описано в моей статье про трех-степенную платформу, в данном случае вместо трех приводов используется шесть, схема расположения соответствует платформе Стьюарта.

Чтобы не поломать платформу в критических положениях или при нагрузке в качестве соединительных частей коллега по работе (огромное спасибо Михаилу) предложил использовать обычные стержни от гелиевых ручек, а в качестве узлов стыковки шаровую опору от вертолета.

Вид на платформу сверху после сборки:
image

Управление сервоприводами осуществляется с помощью ШИМ, соответственно с 2-7 выходов Arduino Mega:
image

А внутри под платформой сервы подключены обычным образом на макетной плате:
image

В общем случае, мы просто удвоили количество компонентов относительно трех-степенной схемы и скомпоновали в одну, довольно устойчивую конструкцию. Важно зафиксировать шаровые опоры на стержнях с помощью клея, потому что во время множества тестов вылетания соединений просто раздражают.

Шесть степеней


Понятие шесть степеней свободы по Википедии имеет следующее определение:

Шесть степеней свободы (часто употребляется аббревиатура 6DoF, от англ. Six degrees of freedom) — указывает на возможность геометрической фигуры совершать геометрические движения в (трёхмерном) пространстве, а именно: двигаться вперёд/назад, вверх/вниз, влево/вправо (в декартовой трёхмерной системе координат), включая повороты вокруг каждой из трёх взаимноперпендикулярных осей (рыскание, тангаж, крен). Известно, что перемещение по любому вектору в пространстве можно представить в виде суммы трёх элементарных перемещений по базовым векторам вдоль каждой из осей, причём каждое такое элементарное перемещение невыводимо из двух других. Произвольный поворот формы в пространстве также может быть задан последовательностью поворотов вокруг каждой из осей (по Эйлеру). Отсюда цифра 6 (шесть).


Графически данное определение можно отобразить следующим образом (источник)
image

Исходя из этого получаем что шесть степеней свободы это:
  • Линейное смещение по оси Х
  • Линейное смещение по оси Y
  • Линейное смещение по оси Z
  • Поворот вокруг оси Х (roll — крен)
  • Поворот вокруг оси Y (pitch — тангаж)
  • Поворот вокруг оси Y(yaw — рысканье)


Пример поворота вокруг оси Х — крена судна (источник):
image

Пример поворота вокруг оси Y — тангажа судна (источник):
image

Пример поворота вокруг оси Z — рысканья судна (источник):
image

В интернете имеется много примеров описания степеней поворота вокруг осей, вот один из них.

Программирование


После сборки макета, а также модификации программы управления тремя сервоприводами формат команды управления получается следующий:
image

Команда управления состоит из 1 байта префикса команды, в нашем случае будет использоваться символ 'S', и шести положений приводов по два байта. Как сформировать такую команду с помощью языка C понятно сразу: объявить структуру соответствующего формата. В языке Python в «комплекте батареек» имеется замечательный модуль struct для формирования строки из набора полей.

Представленная выше структура в терминах модуля struct имеет следующий вид:
import struct
 
struct.pack('<chhhhhh''S', pos1, pos2, pos3, pos4, pos5, pos6 )


По документации модуля:
  • < — указывает на порядок байт от младшего к старшему после преобразования переменных
  • c — сопоставляет строку Python длиной в 1 символ переменной типа char (1 байт) языка Си
  • h — сопоставляет целочисленное значение Python в переменную типа short (2 байта) языка Си


Все первичные тесты можно выполнять в консоли языка Python, в нашем случае используется мощное средство IPython, а для подключения к «железу» нам понадобится библиотека Serial.

Подключение к платформе осуществляется следующим образом:
import serial
 
serialPort = serial.Serial('/dev/ttyUSB0'115200)
serialPort.open()


Отправка данных после успешного подключения и открытия порта выполняется так:
serialPort.write(struct.pack('<chhhhhh''S',x,y,z,roll,pitch,yaw))


Данный код ляжет в основу процедур тестирования платформы, а также для программы управления платформы с помощью джойстика.

Для автоматического тестирования всех степеней свободы можно использовать следующие простые процедуры на Python:
  1. import struct
  2. import time
  3. import math
  4.  
  5. def send_data(serial, x, y, z, roll, pitch, yaw):
  6.     serial.write(struct.pack('<chhhhhh''S',x, y, z, roll, pitch, yaw))
  7.  
  8. def complex_test(serial, lenght=200, times=1000, delta=0.05, pause=0.7):
  9.     test_linear(serial, lenght, times, delta)
  10.     time.sleep(pause)
  11.     test_yaw(serial, lenght, times, delta)
  12.     time.sleep(pause)
  13.     test_angles(serial, lenght, times, delta)
  14.     time.sleep(pause)
  15.  
  16. def test_linear(serial, lenght=200, times=1000, delta=0.05):
  17.     for angle in xrange(1, times, 5):
  18.         a = angle * math.pi / 180
  19.         send_data(serial, int(lenght * math.cos(a))int(lenght * math.sin(a)),0,0,0,0)
  20.         time.sleep(delta)
  21.     send_data(serial, 0,0,0,0,0,0)
  22.  
  23. def test_angles(serial, lenght=200, times=1000, delta=0.05):
  24.     for angle in xrange(1, times, 5):
  25.         a = angle * math.pi / 180
  26.         send_data(00,0,0,int(30 * math.cos(a)),int(30 * math.sin(-a)))
  27.         time.sleep(delta)
  28.     send_data(serial, 0,0,0,0,0,0)
  29.  
  30. def test_yaw(serial, lenght=200, times=1000, delta=0.05):
  31.     for angle in xrange(1, times, 5):
  32.         a = angle * math.pi / 180
  33.         send_data(serial, int(lenght * math.cos(a)),0,0,int(30 * math.sin(a)),0,0)
  34.         time.sleep(delta)
  35.     send_data(serial, 0,0,0,0,0,0)
  36.  




Управление джойстиком


Самый простой способ получения значений с «палки удовольствия», это воспользоваться модулем PyGame. Данный модуль базируется на SDL и имеет множество удобных средств для разработки игрушек на Python, а также он включает в себя средства для работы с игровыми контроллерами.
image

Простой пример инициализации джойстика и получения координат:
import pygame
 
# Шкала значений
SCALE = 1000
 
# Инициализация pygame, функция инициализации джойстика осуществляется автоматически
pygame.init()
 
# Получение доступных устройств
for jid in range(pygame.joystick.get_count()):
    self.joysticksComboBox.addItem(u'%d %s' % (jid, pygame.joystick.Joystick(jid).get_name()))
 
# Инициализировать объект джойстика
self._joystick = pygame.joystick.Joystick(self.joysticksComboBox.currentIndex())
 
# Обновить события в pygame
pygame.event.pump()
 
# Получить положение от джойстика по заданной оси (в нашем случае ось X = 0)
# значение возвращается от -1 до 1 в виде float, поэтому его шкалируем
self._joystick.get_axis(0) * SCALE
 


Программа управления платформой с помощью джойстика имеет графический интерфейс благодаря библиотеке PyQt4, хотя сначала было горячее желание сделать её на curses (что почти было реализовано). Когда подключен джойстик и открыт порт работы с платой, то по таймеру данные через интервал в 33 мс получаются с игрового контроллера и передаются на плату.

Интерфейс программы управления:
image

Примечание: Сразу хочу оговориться, что джойстик дешевый, поэтому у него много мертвых зон и отвратительная чувствительность, а платформа на «быстрых» сервах, поэтому косяки джойстика сразу видны.

В результате тестирования программы получилось следующее видео:


Выводы


В результате проделанной работы мы достигли следующих результатов.
  • Освоили работу с Arduino из операционной системы Linux, что особенно понравилось, так это отсутствие проблем разворачивания среды программирования, а также плата сразу подхватилась ядром (в Windows необходим FTDI драйвер).
  • Научились работать с RS232 средствами языка Python, а также управлять «железом». Данный опыт позволяет без особых сложностей тестировать протокол обмена с платой просто из консоли без разработки лишнего кода, после чего можно данный протокол закладывать в любые уже готовые решения по работе с RS232.
  • Научились работать с контроллером с помощью PyGame, это не сложно, но может новичкам и пригодится.


Основные проблемы на будущее, в решение которых часто могут помочь ваши комментарии:
  • Хочется уйти от использования сервоприводов, потому не возможно рассчитать точно угол поворота для уменьшения линейных искажений движений платформы, в любом случае нужны линейные приводы (актуаторы). Из существующих я нашел только Frigelli L12, но он очень медленные, другой вариант использование ШВП, но их доступных не нашел вообще.
  • В случае изменения силовой части придется искать новые шаровые опоры, используемые сейчас будут слишком малы для решения задачи в другом масштабе.
  • Способы обратной связи с актуаторов, в промышленной автоматике это решается датчиками с ПИД регуляторами, но в меньших масшатабах оказался проблем.


P.S.: В качестве новатора в данной области не выступаю, приветствую любые пожелания и критику.

Спасибо за внимание!

Tags:
Hubs:
+66
Comments 34
Comments Comments 34

Articles