Проектирование манипулятора

манипулятор управление моделирование

Манипулятор — совокупность пространственного рычажного механизма и системы приводов, осуществляющая под управлением программируемого автоматического устройства или человека-оператора действия (манипуляции), аналогичные действиям руки человека.

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

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

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

Компьютерное моделирование является одним из эффективных методов изучения сложных систем. Компьютерные модели проще и удобнее исследовать, в тех случаях когда реальные эксперименты затруднены из-за финансовых или физических препятствий или могут дать непредсказуемый результат. Логичность и формализованность компьютерных моделей позволяет выявить основные факторы, определяющие свойства изучаемого объекта. [2]

Целью данной работы является разработка кинематической схемы трехстепенного манипулятора и построение трёхмерной модели его конструкции, моделирование компьютерной динамической модели манипулятора, выполнить анализ движения модели робота при различных условиях, определить некоторые характеристики робота на основе моделирования, проектирование отдельных деталей и изготовление их при помощи 3D печати, сборка робота, разработка системы управления манипулятором на основе Arduino.

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

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

Несмотря на очевидные преимущества конструкции, управлять таким роботом достаточно сложно. При перемещении каждого звена принцип минимального значения требуемого угла, и манипулятор движется не по прямой линии (как декартов, например), а выполняет довольно сложную траекторию, имитируя движения живой руки. В результате мысленное представление всех движений «руки» сильно затруднено, что создаёт трудности при программировании. [4]

1. Подходы к моделированию и управлению манипуляторами

1.1 Типы манипуляторов

Первый вопрос, с которым сталкивается создатель манипулятора, — выбор его кинематической схемы, структуры его скелета. В процессе выполнения операций с объектами манипулирования в большинстве случаев манипуляторы имитируют движение рук человека. Поэтому структурная схема манипулятора должна обладать кинематическими характеристиками, аналогичными характеристикам руки человека.

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

Существует большое количество схем манипуляторов, различным образом реализующих региональные движения, но наиболее распространенными в промышленности являются пять следующих схем с одноподвижными кинематическими парами:

Декартовый тип. Манипулятор (рис. 1.1), функционирующий в декартовой (прямоугольной) системе координат, прост в управлении и отличается высокой точностью действий. Схват манипулятора поступательно перемещается вдоль трех основных осей: х, у и z (т.е. слева направо, вперед-назад и вверх-вниз).

Рис. 1.1. Манипулятор декартового типа

Цилиндрический тип. Манипулятор (рис. 1.2), работающий в цилиндрической системе координат. Его схват может выдвигаться и втягиваться, а также перемещаться вверх и вниз вдоль стойки. Кроме того, весь узел манипулятора может поворачиваться вокруг оси основания, но не на полный оборот, что позволяет ему выполнять операции в окружающей цилиндрической зоне.

Рис. 1.2. Манипулятор цилиндрического типа

Сферический тип. Манипулятор (рис. 1.3), действующий в сферической (или полярной) системе координат. Его схват может выдвигаться и втягиваться. Вертикальные перемещения манипулятора достигаются путем поворота его в вертикальной плоскости в «плечевом» суставе. Весь узел манипулятора может также поворачиваться вокруг оси основания. Зона действия подобного манипулятора представляет усеченную сферу. Первые модели промышленных роботов были сконструированы именно по этому принципу.

Рис. 1.3. Манипулятор сферического типа

Ангулярный тип. Шарнирный манипулятор (рис. 1.4), действующий в ангулярной системе координат, не имеет поступательных кинематических пар, а имеет только вращательные кинематические пары. Манипулятор такого типа очень напоминает руку человека, поскольку имеет «плечевое» и «локтевое» сочленения, а также «запястье». Его зона обслуживания значительно больше, чем у роботов других типов. Он способен обходить препятствия гораздо более разнообразными путями и даже складываться, но вместе с тем он исключительно сложен в управлении.

Рис. 1.4. Манипулятор ангулярного типа

Тип SCARA. Своеобразную схему имеет манипулятор системы SCARA (рис. 1.5), представляющий собой вариант манипулятора с цилиндрической системой координат. Все кинематические пары этого манипулятора располагаются в горизонтальной плоскости, благодаря чему механизм способен разворачиваться подобно складной ширме. Его зона обслуживания имеет цилиндрическую форму.

Рис. 1.5. Манипулятор SCARA типа Перспективными представляются роботы еще двух типов. Первый из них, «Spine» (рис. 1.6), спроектирован специалистами фирмы «Спайн роботикс». В нем используется длинный хоботоподобный манипулятор, состоящий из множества чечевицеобразных дисков, которые соединены между собой двумя парами тросов, обеспечивающих натяжение. Тросы соединены с поршнями гидравлических цилиндров, которые, создавая натяжение, вызывают перемещение манипулятора. Специальные датчики передают на систему управления информацию о положении манипулятора и его кисти. Такой робот отличается чрезвычайно большой гибкостью, значительным радиусом действия и высокой маневренностью.

Рис. 1.6. Манипулятор Spine типа Другой робот маятникового типа (рис. 1.7), IR B1000, разработан специалистами фирмы ASEA; его манипулятор подвешен подобно маятнику с двойным карданным подвесом и может перемещаться по направляющим относительно продольной и поперечной осей. По утверждению специалистов фирмы ASEA, это устройство движется в 1,5 раза быстрее, чем традиционные манипуляторы, что обеспечивает высокую производительность. [5]

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

1.2 Классификация манипуляторов

Манипуляторы составляют 85−90% всех роботов в мире.

Классификация роботов по назначению в промышленности:

· Керамическая промышленность: выдавливание керамического сырья, загрузка вальцовых (крокетных) машин, извлечение сформованных изделий, складирование, покрытие глазурью путем окунания, нанесение глазури пульверизатором, шлифовка изделия после обжига, загрузка и разгрузка печей;

· Стекольная промышленность: загрузка и разгрузка машин;

· Швейная промышленность: загрузка швейных машин;

· Деревообрабатывающая промышленность: покрытие лаком, сборка изделий, забивка гвоздей, закручивание винтов;

· Производство и обработка кожи: загрузка машин;

· Резинообрабатывающая промышленность: распознавание образов, манипулирование шинами;

· Асбестообрабатывающая промышленность: разрезка, обточка, шлифовка, штукатурка;

· Обработка пластиков: загрузка сырья, разгрузка машин;

· Мясообрабатывающая промышленность: рубка мяса.

По степени универсальности:

· универсальные (для выполнения разных операций совместно с различными видами оборудования);

· специализированные (выполняет одну операцию из нескольких возможных с различным оборудованием);

· специальные (выполняет конкретную операцию с одним типом оборудования).

По виду технологических операций:

· осуществляющие основные технологические операции;

· выполняющие вспомогательные технологические операции по обслуживанию технологического оборудования (средства автоматизации).

По показателям, определяющим их конструкцию:

· тип приводов робота (электрический, гидравлический, пневматический);

· грузоподъемность (сверхлегкие — до 1 кг; легкие — от 1 до 10 кг; средние 10ё200 кг; тяжелые — 200ё1000 кг; сверхтяжелые — свыше 1000 кг);

· количество манипуляторов (от 1 до 4 рук);

· тип и параметры рабочей зоны манипуляторов (прямоугольная, цилиндрическая, сферическая, угловая (ангулярная) и различные их комбинации).);

· подвижность робота определяется наличием или отсутствием у него устройства передвижения (подвижный или стационарный).

· по способу размещения стационарные и подвижные роботы бывают напольными, подвесными (перемещаются по монорельсу), встраиваемые в другое оборудование (в станок или др.);

· по исполнению робота — зависит от назначения (нормальное, пылезащитное, теплозащитное, влагозащитное, взрывобезопасное и т. п.).

По способу управления:

· с программным управлением;

· с адаптивным управлением;

· с интеллектуальным управлением.

По быстродействию движений:

· малое быстродействие — до 0,5 м/с;

· среднее — линейные скорости от 0,5 до 1 м/с (~80% роботов);

· высокое — свыше 1 м/с (~20% роботов).

По точности движений:

· малая точность — при линейной погрешности от 1 мм и выше;

· средняя — от 0,1 до 1 мм (больше всего роботов);

· высокая — менее 0,1 мм.

Параметры, определяющие технический уровень роботов:

· надёжность;

· число одновременно работающих степеней подвижности;

· время программирования;

· удельная грузоподъёмность (отнесённая к массе робота);

· выходная мощность манипулятора (произведение грузоподъёмности на скорость перемещения), отнесённая к мощности его приводов;

· относительные оценки габаритных параметров и т. п.

Эти параметры служат критериями качества, предназначенные для их оптимизации при проектировании и сравнительной оценки роботов. [6]

Разрабатываемый в данной работе трёхстепенной манипулятор будет иметь следующие параметры классификации:

· степень универсальности — специальный, предназначен для переноски небольших лёгких предметов;

· тип приводов — электрический;

· грузоподъемность — сверхлёгкий (до 1 кг);

· тип рабочей зоны — ангулярная;

· подвижность робота — стационарный;

· способ размещения — напольный;

· способ управления — программный и ручной;

· быстродействие движений — малое быстродействие (до 0,5 м/с);

· точность передвижений — малая точность (погрешность боле 1 мм).

1.3 Системы управления манипулятором

Системами управления оснащены все манипуляторы, у которых перемещение подвижных звеньев осуществляются с помощью различных немеханических приводов.

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

Различают три основных вида управления ПР: цикловое, позиционное, контурное. Цикловое управление программирует последовательность выполнения движений и условия начала и окончания движений; положение, до которой идет движение, задаются на самом манипуляторе, а не в программе; скорость перемещения определяется характеристиками привода и также не задается в программе. При позиционном управлении команды подаются так, что перемещение рабочего органа происходит от точки к точке, причем положения точек задаются программой. Скорость перемещения между точками не контролируется и не реализуется. При контурном управлении движение рабочего органа происходит по заданной траектории с задаваемой скоростью. В программе задаются сами траектории и режимы движения. Контурное управление используется значительно в технологических работах.

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

Все основные ПР образуют систему управления. Объектом управления является исполнительное устройство (манипулятор плюс устройство передвижения, если оно есть). В исполнительное устройство также входят приводы. Все остальное оборудование робота предназначено для формирования и выдачи управляющих воздействий исполнительному устройству. Таким образом, устройство управления получает сигналы (от датчиков) и выдает сигналы (на приводы манипулятора).

Для пульта ручного управления основными являются связи с устройством управления. С пульта ручного управления могут осуществляться ввод программ, настройка. На пульт управления поступают сигналы о выполнении различных движений, а также о возможных нарушениях режимов работы и об отказах. Следует иметь в виду, что в устройство управления обычно поступают сигналы от внешних (по отношению к роботу) датчиков и систем (например, от систем управления обслуживаемым оборудованием). Устройство управления роботом также может быть связано с ЭВМ, координирующей работу нескольких единиц оборудования, например всего оборудования технологического участка или линии. В этих случаях эта ЭВМ как бы находится на более высоком этаже, на следующем уровне управления. Такая многоуровневая система управления типична для современных гибких производственных систем. [7]

В данной работе системой управления манипулятором будет микроконтроллер Arduino, который будет получать сигналы управления с компьютера через COM порт. Все вычисления будут выполняться на компьютере, микроконтроллер будет отвечать за повороты сервоприводов робота. Через программу в компьютере можно будет управлять манипулятором вручную, т. е. поворачивать звенья манипулятора или передвигать рабочую точку робота по декартовым осям координат. Также будет режим программирования, где рабочая точка робота будет перемещаться от положения к положению, т. е. реализуется позиционная система управления.

1.4 Способы моделирования манипуляторов

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

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

Существуют различные подходы к разработке динамической модели робота-манипулятора. Есть математические и компьютерные динамические модели.

Математическая динамическая модель манипулятора может быть построена на основе известных законов ньютоновской или лагранжевой механики. Результатом применения этих законов являются уравнения, связывающие действующие в сочленениях силы и моменты с кинематическими характеристиками и параметрами движения звеньев. Таким образом, уравнения динамики движения реального манипулятора могут быть получены традиционными методами Лагранжа-Эйлера или Ньютона-Эйлера.

Вывод уравнений динамики движения манипулятора методом Лагранжа-Эйлера отличается простотой и единством подхода и основан на следующем:

1. На описании взаимного пространственного расположения систем координат i-го и (i-1) — го звеньев с помощью матрицы преобразования однородных координат.

2. На использовании уравнения Лагранжа-Эйлера С целью получения более эффективных с вычислительной точки зрения алгоритмов расчета обобщенных сил и моментов используются уравнения Ньютона-Эйлера. Вывод уравнений Ньютона-Эйлера прост по содержанию, но весьма трудоёмок. Результатом является система прямых и обратных рекуррентных уравнений, последовательно применяемых к звеньям манипулятора.

Уравнения Лагранжа-Эйлера обладают низкой вычислительной эффективностью, это обусловлено тем, что для описания кинематической цепи используются матрицы преобразования однородных координат. Уравнения Ньютона-Эйлера обладают большей вычислительной эффективностью, что связано с их векториальным представлением и их рекуррентной природой. [9]

Для построения компьютерной динамической модели робота используются программы предназначенные для моделирования динамики и кинематики пространственных механических систем, например: MSC. ADAMS [10]; UM [11]; Unigraphics NX [12]; CATIA [13]; SolidWorks.

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

В данной работе динамическая модель робота-манипулятора будет строиться в программной среде MSC. ADAMS, предназначенной для виртуального моделирования сложных механизмов. Широкие возможности программного пакета, высокая надёжность и малая трудоёмкость его использования позволяют исследовать множество вариантов механизмов, моделируя на компьютере реальные условия их работы, сравнивать и выбирать лучший вариант. Также для сравнения с компьютерной моделью будет выведена математическая динамическая модель с использованием уравнений Лагранжа-Эйлера.

2. Требования к разрабатываемому устройству

1. Разрабатываемое устройство должно обладать следующими свойствами:

· Основание, поворачивающееся вокруг своей оси, способное крепиться к горизонтальной плоской поверхности;

· Два звена, имеющих длину по 20 см с шарнирными соединениями;

· Механический схват, способный схватить лёгкие предметы диаметром до 7 см.

2. Построить кинематическую схему робота.

3. Разработать трёхмерную модель конструкции трехстепенного манипулятора с механическим захватом.

4. В трёхмерной модели определить диапазоны углов поворотов звеньев с учётом ограничений конструкции робота.

5. Построить компьютерную динамическую модель робота, с помощью которой определить:

· Рабочую зону манипулятора;

· Скорости, ускорения, силы и моменты, возникающие в шарнирах звеньев манипулятора при различных условиях;

· Максимальную грузоподъёмность робота.

6. Изготовить детали звеньев и захвата манипулятора при помощи 3D печати.

7. Собрать манипулятор.

8. Разработать систему управления манипулятором на основе Arduino.

9. Разработать компьютерную программу для управления манипулятором, которая позволит:

· Вручную управлять поворотами каждого звена по отдельности, а также схватом;

· Управлять передвижением рабочей точки манипулятора вдоль осей X, Y, Z декартовой системы координат.

· Управлять скоростью движения манипулятора;

· Позволить программировать манипулятор для автоматического передвижения через конкретные точки.

10. Проанализировать работу конструкции манипулятора и программу управления манипулятором и сделать выводы.

3. Разработка конструкции манипулятора

3.1 Построение структурной кинематической схемы манипулятора

Перед разработкой трёхмерной модели конструкции манипулятора необходимо составить его структурную кинематическую схему (Рис. 3.1). Для начала необходимо определить тип манипулятора. Было решено разработать манипулятор ангулярного типа т.к. вращательные кинематические пары наиболее просты в конструкции и легко реализуемы, также такая конструкция имеет довольно обширную рабочую зону и своей гибкостью позволяет обходить некоторые препятствия. Также по заданию манипулятор должен иметь всего 3 степени подвижности: поворот основания вокруг своей оси и повороты 1 го и 2 го звеньев. Чтобы схват захватывал объекты правильно, он должен быть направлен в определённой ориентации, для этого в кинематическую схему добавлен специальный механизм для удержания схвата в неизменной ориентации.

Кинематическая схема разрабатывалась на основе реального промышленного робота Fanuc M-410iB/450 (Рис. 3.1), который имеет 4 степени подвижности и используется для переноски груза.

Рис. 3.1. Промышленный манипулятор Fanuc M-410iB/450 и его рабочая зона Разработанная кинематическая схема трёхстепенного манипулятора указана на рисунке 3.1. На схеме указаны 1 ое и 2 ое звенья, которые поворачиваются на углы q₁ и q₂, основание, вокруг которого манипулятор поворачивается на угол q₀, и схват, а также проставлены длины всех частей манипулятора.

Рис. 3.1. кинематическая схема манипулятора В этой кинематической схеме используются некоторые дополнительные звенья и элементы, которые образуют параллельный механизм, который позволяет схвату манипулятора оставаться всегда параллельно основанию независимо от того, на какие углы q1 и q2 повернуться звенья манипулятора На рисунке 3.2 (а, б, в, г) показан принцип работы параллельного механизма, на котором видно что при изменении углов поворота звеньев, схват робота сохраняет параллельное положение относительно основание. Данное свойство удобно для захвата, перемещения и установления объекта в новое положение, при этом сохраниться его вертикальная ориентация. Так же отпадает необходимость ставить 3е управляемое звено. Несмотря на множество шарнирных соединений в данной схеме для ориентации манипулятора используется всего 3 сервопривода, которые расположены в точках углов поворотов звеньев q₀, q₁, q₂, ещё один сервопривод нужен для сжатия / разжатия схвата.

Рис. 3.2. принцип работы параллельного механизма

3.2 Решение прямой и обратной задачи кинематики

Прямая задача — это определение положений звеньев манипулятора при заданных углах поворота соединений и размеров звеньев.

Обратная задача — это определение углов поворота при заданных координатах положения рабочей точки манипулятора Пред решением задач составим структурную схему (Рис. 3.3) с нужными для решения обозначениями.

Рис. 3.3. Структурная схема с обозначениями В данной схеме:

q₀, q₁, q₂ — углы поворота;

L₀, L₁, L₂, L₃ — длины звеньев;

P₀, P₁, P₂, P₃, P₄ — точки положения звеньев Все вычисления для прямой и обратной задач проводились в среде Mathcad.

Решение прямой задачи:

Задаём углы и расстояния:

Задаём длины звеньев, мм:

Матрицы переноса:

Матрицы поворота:

Вычисление матриц положения точек:

Координаты точек манипулятора:

На рисунке 3.4 изображено решение прямой задачи

Рис. 3.4. Решение прямой задачи Для наглядного представления решения обратной задачи показана геометрическая схема на рисунке 3.5.

Рис. 3.5. Решение обратной задачи

Решение обратной задачи:

Задаём длины звеньев, мм:

Задаём координаты положения точки схвата:

Решение задачи:

Находим проекцию точки P₄ на плоскость OXY:

Находим проекцию точки P₃ на плоскость OXY:

Смещаем вниз координату z точки P₃ на длину L₀:

Находим расстояние между началом координат и точкой P₃:

Находим угол q0:

1 ое и 2 ое звенья и отрезок B образуют треугольник, т.к. мы знаем их длины, то можем определить углы этого треугольника по формулам:

Найдём угол q₀₁:

Найдём угол q₀r:

Найдём угол q₀₂:

Найдя углы треугольника можно определить углы поворотов звеньев:

Найдём значения углов в градусах:

Решение показывает, что подставив значения координат из решения прямой задачи в обратную задачу и решив её, получим значения углов такие же как задавали в прямой задачи, что подтверждает правильность решения этих задач.

3.3 Выбор сервоприводов

Поворот звеньев манипулятора и сжатие / разжатие захвата будут осуществлять цифровые сервоприводы. При длине двух звеньев (по 20 см каждое) и с учетом размера захвата приблизительная максимальная длина манипулятора будет 57 см. Чтобы определить мощность, необходимую сервоприводу для поднятия манипулятора, нужно определить вес манипулятора. Для этого нужно учесть вес сервоприводов и деталей звеньев и захвата. Сервопривод весит в среднем 60 грамм, т.к. их 3, то общий их вес 180 грамм, остальные детали будут сделаны из пластика и металла, в целом вес робота должен быть около 500 грамм без основания. Допустим что масса робота примерно распределена равномерно вдоль всей его длины, следовательно, при закреплённом основании и расположении звеньев робота перпендикулярно силе тяжести, масса в рабочей точке робота будет 250 грамм (Рис. 3.6).

Рис. 3.6. Примерная оценка массы робота При массе 250 грамм в рабочей точки и длине робота в 57 см, момент на валу сервопривода будет 14,25 кг*см при условии, что захват робота будет без груза.

Доступные в данный момент сервоприводы с достаточно большим моментом были сервоприводы DF15RMG (Рис. 3.7) для работы 1 го звена манипулятора и RDS3115 (Рис. 3.8) для работы 2 го звена, эти сервоприводы практически идентичны по характеристикам и одинаковые по размерам.

Рис. 3.7. Внешний вид DF15RMG с дополнительными кронштейнами

Спецификации для сервопривода DF15RMG:

— угол поворота: 170°

— Скорость поворота: 60°/0.16s

— Момент: 19.3 кг•см при 7.4V

— Металлические шестерни

— Размеры: 40×40×20 мм

— Вес: 65 г.

— Рабочее напряжение: 5−7.4v

— Максимальный ток: 3A при 8.5V

— PPM Voltage:3V-5V

— Разрешение: 2 мкс

— Частота: до 4 КГц [17]

Рис. 3.8. Внешний вид RDS3115 с дополнительными кронштейнами Спецификации для сервопривода RDS3115:

— угол поворота: 180°

— Скорость поворота: 60°/0.16s

— Момент: 17 кг•см при 7.2V

— Металлические шестерни

— Размеры: 40×40×20 мм

— Вес: 60 г.

— Рабочее напряжение: 4.8−8.4v [18]

Для работы захвата и поворота основания манипулятора будут использованы сервоприводы SR430 (Рис. 3.8). Они развивают меньший момент чем DF15RMG, т.к. для работы захвата и поворота основания не требуется высокий момент.

Рис. 3.8. Внешний вид SR430

Спецификации для сервопривода SR430:

— Рабочее напряжение: 6,0 — 7,4 В

— Скорость: 0,18 сек/60° при 6 В

— Усилие на валу: 4,2 кг/см при 6 В

— Скорость: 0,16 сек/60° при 7,4 В

— Усилие на валу: 5,3 кг/см при 7,4 В

— Угол вращения: 180°

— Размеры: 42×39,5×20,5 мм

— Вес: 44 гр.

— Шестерни редуктора: пластиковые [19]

3.4 Проектирование деталей

Выбор САПР

Для проектирования 3D моделей деталей робота была выбрана программа Google SketchUp, т.к. она является одной из простейших программ для 3D моделирования и является бесплатной, а также имеет интерфейс на русском языке. Программа оснащена упрощенным набором инструментов, с которым пользователю работать максимально удобно — это позволит всего лишь за несколько часов освоить работу в программе и начать проектировать самые разные 3D-модели, от простейших геометрических фигур до полномасштабных красивых моделей.

Проектирование 1-го и 2-го звеньев

Сервоприводы DF15RMG уже обладают готовыми креплениями (Рис. 3.9) к ним, поэтому звено нужно спроектировать таким образом, чтобы оно могло крепиться к ним. Значит необходимо в детали сделать такие же отверстия как и в кронштейнах, чтобы их можно было стянуть болтами. Диаметр отверстий 3 мм, расстояние между ним 10 мм. Деталь будет сделана из алюминиевой рейки т.к. металл выдержит более высокие нагрузки и он менее деформируется по сравнению с пластиком.

Рис. 3.9. 3D модель сервопривода DF15RMG и его кронштейнов Стоит учесть, что сервоприводы, соединённые с рейкой образуют единое звено манипулятора, которое должно иметь длину 20 см (Рис. 3.10), следовательно длину рейки нужно подобрать таким образом, чтобы выполнялось это условие.

Рис. 3.10. Модель 1-го звена В результате получится рейка длиной 12,8 см (Рис. 3.11), которая образует с двумя сервоприводами 1 ое звено длиной 20 см.

Рис. 3.11. Трёхмерный вид детали 1 го звена с указанными размерами Рейка 2 го звена будет такая же как и для 1 го звена, но только другой длинны. 2 ое звено образуют сервопривод, металлическая рейка и специальное крепление для схвата. Расстояние между осью вала сервопривода и осью вращения крепления для схвата составляет 20 см, что является длиной 2 ого звена (Рис. 3.12).

Рис. 3.12. Модель 2-го звена В результате получится рейка 2 го звена длиной 14,9 см (Рис. 3.13).

Рис. 3.13. Трёхмерный вид детали 2 го звена с указанными размерами

Проектирование схвата

Модель схвата будет строится на основе готового чертежа (Рис. 3.14). Такой схват имеет 6 подвижных деталей, которые крепятся на основание (Рис. 3.15).

Рис. 3.14. Чертёж для построения модели схвата Рис. 3.15. Модель основания схвата с разных сторон Расположение крепления для сервопривода было изменено, по сравнению с готовым чертежом, для того, чтобы схват мог шире раскрыться. Также на креплении добавлены специальные опоры для сервопривода SR430. В основании имеется специальное отверстие, чтобы вал сервопривода соединить с шестернёй, которая приводит механизм схвата в движение.

Расположение подвижных деталей и их форма остались как на чертеже. Все детали схвата (Рис. 3.17) должны крепится между собой болтами диаметром 3 мм и длиной желательно 15 мм.

Рис. 3.17. Детали и основание схвата и сервопривод SR430

На рисунке 3.19 изображены 3D модели собранного схвата с разных ракурсов вместе с сервоприводом SR430.

Рис. 3.19. Модель собранного схвата с разных ракурсов Из рисунка 3.20 видно, что длина схвата меняется в зависимости от степени его раскрытия.

Рис. 3.20. Схват при различных положениях и проставленные размеры

Проектирование основания

На основание манипулятора будут идти самые большие нагрузки, также оно примет на себя весь его вес. К тому же к основанию предъявляется ряд требований: оно должно иметь возможность крепиться к плоской поверхности; поворачиваться вокруг своей оси; за вращение должен отвечать сервопривод SR430; основание должно быть достаточно широким, чтобы не опрокинуться при нагрузках и прочным, чтобы не сломаться от высоких нагрузок.

При проектировании основания было решено следующее: т.к. основание вращается оно должно быть цилиндрической формы, в нём должен поместиться сервопривод, оно должно иметь крепления для 1 го звена манипулятора. Креплением 1 го звена является кронштейн его сервопривода, сделаем в основании отверстия для крепления кронштейна (Рис. 3.21).

Рис. 3.21. Крепление 1 го звена к основанию манипулятора Внутри основания предусмотрены крепления для сервопривода SR430, они достаточно прочны чтобы выдержать нагрузки при вращении основания. Сервопривод крепится так, чтобы центр его вала находился по центру основания (Рис. 3.22).

Рис. 3.22. Крепление сервопривода в основании манипулятора Чтобы основание вращалось, у него есть отдельная нижняя часть, относительно которой оно вращается (Рис. 3.23). Нижняя часть имеет по контуру 8 отверстий для крепления к плоской поверхности. Чтобы основание не вывалилось из своей нижней части, нижняя часть крепится болтами к валу сервопривода, который крепится к основанию (Рис. 3.24). При вращении вала сервопривода основание начнёт вращаться относительно нижней части.

Рис. 3.23. Соединение основания с нижней частью Рис. 3.24. Структура основания На рисунках 3.25 и 3.26 указаны основные размеры деталей основания: верхней поворачивающейся части и нижней части, крепящейся к поверхности.

Рис. 3.25. Верхняя часть основания с указанными размерами Рис. 3.26. Нижняя часть основания с указанными размерами

Проектирование всей конструкции манипулятора

Используя разработанные модели основания, звеньев и схвата манипулятора необходимо разработать модель всей конструкции манипулятора на основе его кинематической схемы (Рис. 3.1). Для этого нужно разработать модели деталей, которые должны образовать параллельный механизм согласно кинематической схеме. На рисунке 3.27 обозначены числами 1 — 5 те шарниры и звенья, которые нужно спроектировать для параллельного механизма.

Рис. 3.27. Обозначение элементов параллельного механизма на кинематической схеме Звенья под номерами 2 и 4 (Рис. 3.27) имеют одинаковую длину по 20 см. Следовательно детали этих звеньев можно спроектировать одинаковыми. На рисунке 3.28 изображена модель этих звеньев, они будут выполняться из алюминиевых пластин, чтобы иметь достаточную прочность при их длине.

Рис. 3.28. Модель дополнительных звеньев для параллельного механизма Модель детали, которая образует шарнирное соединение в точке 1 (Рис. 3.27) изображена на рисунке 3.29. Эта деталь будет изготавливаться при помощи 3D печати и будет крепиться к поворачивающемуся основанию манипулятора.

Рис. 3.29. Модель детали для шарнирного соединения Модель детали, которая образует 3 шарнирных соединения в точке 3 (Рис. 3.27) изображена на рисунке 3.30. Эта деталь будет изготавливаться при помощи 3D печати и будет крепиться к валу сервопривода 2 го звена манипулятора, а также будет соединять дополнительные звенья.

Рис. 3.30. Модель центральной детали параллельного механизма Модель шарнирного механизм, а под номером 5 (Рис. 3.27) объединяет 2 ое звено, схват и дополнительное звено манипулятора, она состоит из двух отдельных деталей, которые печатаются на 3D принтере и соединяются между собой длинным болтом. На рисунках 3.31 и 3.32 изображены эти детали по отдельности и объединённые вместе.

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

Рис. 3.33. Модель всей конструкции манипулятора

На рисунке 3.34 показаны различные положения модели манипулятора и их кинематические схемы, тут же демонстрируется принцип работы параллельного механизма.

Рис. 3.34. Манипулятор в различных положениях

3.5 Определение диапазонов углов поворотов звеньев

Построив 3D модель манипулятора (Рис. 3.33) с учётом всех длин звеньев и размеров деталей, можно определить диапазоны углов поворотов звеньев робота относительно начального положения. Эти диапазоны будут учитываться при определении рабочей зоны динамической модели манипулятора.

Угол поворота основания q₀:

За поворот основания отвечает сервопривод SR430, диапазон его поворота составляет 180⁰. В конструкции робота нет препятствий, ограничивающих этот диапазон, следовательно робот будет поворачивать на ±90⁰ относительно начального положения (Рис. 3.35).

Рис. 3.35. Угол поворота основания q₀

Угол поворота 1 го звена q₁:

За поворот 1 го звена отвечает сервопривод DF15RMG с диапазоном поворота 170⁰. Конструкция робота позволяет максимально наклонить звено в одну сторону на 102⁰, а на поворот в другую сторону 38⁰ относительно начального положения (Рис. 3.36).

Рис. 3.36. Угол поворота 1 го звена q₁

Угол поворота 2 го звена q₂:

За поворот 2 го звена отвечает сервопривод RDS3115 с диапазоном поворота 180⁰. Конструкция робота позволяет максимально наклонить звено в одну сторону на 48⁰, поворот в другую возможен только на 90⁰ (Рис. 3.37).

Рис. 3.37. Угол поворота 2 го звена q₂

Диапазоны поворотов углов были сделаны максимально возможными в одну сторону для того, чтобы сделать рабочую зону перед роботом как можно больше.

4. Разработка динамической модели манипулятора

4.1 Система виртуального моделирования Adams

Динамическая модель робота-манипулятора (Рис. 4.1) строилась в программе Adams View. При построении были учтены все размеры из кинематической схемы (Рис. 3.1) и 3D модели робота (Рис. 3.33).

Рис. 4.1. Трёхмерная модель робота для моделирования динамики

ADAMS/View предназначен для создания, тестирования и оптимизации работы моделей механизмов и конструкций, состоящих из абсолютно твердых тел и их соединений (шарниров, нитей, пружин и т. д.).

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

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

Построенная модель робота в Adams при входных данных: углы поворота, угловая скорость и масса звеньев, позволяет определить выходные данные: координаты положения звеньев в пространстве, силы, моменты, угловые ускорения действующие в сочленениях звеньев (Рис. 4.2).

Рис. 4.2. Входные и выходные данные в модели манипулятора Также возможны и другие варианты входных и выходных данных, например при заданном моменте найти угловые скорость и ускорение.

Основой для системы уравнений, описывающих динамику модели робота в программе Adams, послужили уравнения в форме Эйлера-Лагранжа с множителями

Для задания движения твердого тела используются инерциальные глобальные координаты его центра масс и углы Эйлера. По умолчанию ориентация определяется последовательными поворотами вокруг главных центральных осей тела. Выбор одной из 24-х систем углов Эйлера должен быть сделан в процессе сборки модели, перед началом симуляции. Необходимость смены системы эйлеровых обобщенных координат, связана с вырождением матрицы связи проекций вектора угловой скорости на ортогональные оси и обобщенных скоростей.

4.2 Рабочая зона манипулятора

Была задана функция, с помощью которой модель робота повернула свои звенья по всему диапазону допустимых углов. На рисунке 4.3 показан скриншот движения модели манипулятора. При этом схват робота прошёлся по контуру своей рабочей зоны. Координаты точек контура были сохранены, по которым потом построилась рабочая зона на координатной плоскости. Контур рабочей зоны показан на плоскости по с осями OXZ на рисунке 4.4 и на плоскости с осями OXY на рисунке 4.5.

Рис. 4.3. Движение модели манипулятора для определения рабочей зоны Для большей наглядности была добавлена модель манипулятора на координатную плоскость с рабочей зоной.

Рис. 4.4. Контур рабочей зоны с моделью манипулятора (вид сбоку)

Рис. 4.5. Контур рабочей зоны с моделью манипулятора (вид сверху)

4.3 Динамические характеристики манипулятора

Нужно определить динамические характеристики в точках поворота звеньев манипулятора q0, q1, q2 (Рис. 4.6) при различных его положениях и передвижениях т.к. в этих точках расположены сервоприводы и важно понять какая будет действовать на них нагрузка.

Рис. 4.6. точки в которых будут определены динамические характеристики Динамические характеристики будут следующими: сила F, момент M, угловая скорость щ, угловое ускорение а. Чтобы их определить нужно задать движение звеньев модели манипулятора, а именно повернуть на определённые углы с конкретной скоростью. Далее программа рассчитает массив данных, на основе которых построятся графики, по которым можно определить значения динамических характеристик при определённом угле поворота звена и в конкретный момент времени.

Определение динамических характеристик в точках q1 b q2

Было задано движение звеньев модели манипулятора таким образом, что он занимает 5 различных положений (Рис. 4.7), при этом средняя угловая скорость поворота каждого звена составляла 1.57 рад/с, достигнув каждого положения манипулятор останавливался на 0.5 сек.

Рис. 4.7. Движение модели манипулятора для определения динамических характеристик На рисунке 4.8 показаны графики: угол поворота q1 1 ого звена, сила F, момент M, угловая скорость щ и угловое ускорение а, зависящие от времени. В начальный момент времени, когда манипулятор в положении «1» угол поворота 1 ого звена считается нулевым.

Рис. 4.8. Динамические характеристики манипулятора в точке q1

Стоит заметить что в динамической модели угловая скорость изменяется плавно, а вращающий момент в среднем не превышает 1 Н*м, что является допустимой нагрузкой реальный сервопривод.

Аналогично строятся графики динамических характеристик для точки q2 (Рис. 4.9)

Рис. 4.9. Динамические характеристики манипулятора в точке q2

Здесь угол q2 поворота 2 ого звена в начальный момент времени (положение «1») отклонён на 90 градусов.

Определение динамических характеристик в точке q0

Точка q0 — это точка поворота основания манипулятора вокруг своей оси. Движение модели манипулятора пройдёт через 4 разных положения (Рис. 4.10). Сначала манипулятор поворачивает на 90 градусов с максимально вытянутой вперёд рукой, затем отклоняет руку назад и обратно поворачивается на 90 градусов.

Рис. 4.10. Движение модели манипулятора для определения динамических характеристик Графики динамических характеристик точки q0 показаны на рисунке 4.11

Рис. 4.11. Динамические характеристики манипулятора в точке q0

4.4 Грузоподъёмность манипулятора

Грузоподъёмность манипулятора зависит от мощности его сервоприводов. Самая большая нагрузка идёт на сервопривод DF15RMG, отвечающий за поворот 1 го звена на угол q1. Он создаёт момент на валу 19 кг*см? 1.86 Н*м. Максимальная грузоподъёмность — это тот максимальный вес, который манипулятор сможет поднять при максимально вытянутой вперёд руки.

Модель манимулятора ставится в положение с максимально вытянутой рукой вперёд, на ось 1 го звена задаётся вращающий момент М=1.86 Н*м, который сможет развить сервопривод, схват манипулятора держит груз, масса которого m постепенно увеличивается до тех пор, пока силы момента не хватит чтобы его поднять (Рис. 4.12). Максимальная масса которую смог поднять манипулятор = 150 грамм (при 160 грамм он уже опускается вниз).

Рис. 4.12. Определение грузоподъёмности манипулятора Таким же способом можно определить сколько поднимет сервопривод RDS3115, поворачивающий 2 ое звено, с моментом M = 17 кг*см? 1.67 Н*м (Рис. 4.13).

Рис. 4.13. Определение грузоподъёмности 2 го звена Максимальный вес, который смогло поднять 2 ое звено составил 330 грамм.

Также с помощью динамической модели можно определить с какой скоростью манипулятор сможет повернуть свои звенья при определённом вращающем моменте и массе груза. Ниже показаны зависимости угловой скорости 1 ого звена от времени и угла поворота q1 при вращающем моменте М=1.86 Н*м, без груза (Рис. 4.14), с грузом массой 150 грамм (Рис. 4.15).

Рис. 4.14. Зависимость угловой 1 ого звена скорости от времени (без груза) Рис. 4.15. Зависимость угловой 1 ого звена скорости от времени (с грузом) Такие же зависимости были определены для 2 го звена с вращающим моментом М=1.67 Н*м без груза (Рис. 4.16) и с грузом массой 150 грамм (Рис. 4.17).

Рис. 4.16. Зависимость угловой 1 ого звена скорости от времени (без груза) Рис. 4.17. Зависимость угловой 1 ого звена скорости от времени (с грузом) Стоит заметить что в реальности сервоприводы обоих звеньев не смогут вращаться быстрее 400 град/с (по техническим характеристикам). Так же в динамической модели не учитываются различные сопротивления и силы трения, поэтому реальные скорости окажутся куда ниже.

4.5 Математическое описание динамической модели манипулятора

Для упрощения математического описания динамики манипулятора сделаем его поворотное основание неподвижным, тогда получим двухзвенный манипулятор (Рис. 4.18).

Рис. 4.18. Двухзвенный манипулятор Для описания динамики движения манипулятора выполним применение уравнений Лагранжа-Эйлера.

присоединенными переменными являются ;

— первое и второе звенья имеют массы и ;

— параметры звеньев имеют значения;; .

Тогда для матрицы имеем:

, ,

где

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

получаем:

аналогично для и получаем:

.

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

Определения слагаемых, описывающих центробежное и кориолисово ускорение:

.

Слагаемые, определяющие влияние гравитационных сил :

Таким образом, вектор, определяющий влияние силы тяжести:

.

Окончательно имеем уравнения описывающие динамику движения манипулятора, где сила, которую должен развить силовой привод i-го сочленения, чтобы реализовать данное движение:

Для сравнения работы математической и компьютерной динамических моделях манипулятора был вычислен график зависимости момента от времени в сочленении 2-го и 1-го звеньев. График компьютерной модели был показан в главе 4.3 на рисунке 4.9. Для расчёта момента в упрощённой математической модели были использованы данные из расчётов в главе 4.3, а именно функции угловых скоростей и ускорений 1-го и 2-го звеньев, а так же их массы и длины. Это выполнялось для того, чтобы расчёт момента в математической модели выполнялся при тех же условиях, которые были заданы в компьютерной модели. На рисунке 4.19 представлено сравнение графиков момента, возникающего в сочленении 2-го и 1-го звена при движении манипулятора. Как видно форма графиков похожа, но в некоторой степени отличаются значения моментов на разных графиках. Такая разница происходит из-за того, что в математической модели рассчитывалась упрощённая схема двухзвенного манипулятора, а также не учитывалось влияние параллельного механизма манипулятора.

Рис. 4.19. Сравнение графиков динамической характеристики манипулятора, рассчитанных на компьютерной и математической динамических моделях

5. Разработка аппаратного комплекса манипулятора

5.1 Печать деталей на 3D-принтере

Детали схвата манипулятора детали изготавливались при помощи 3D печати на 3D принтере Makerbot Replicator. Процесс печати показан на рисунке 5.1. Детали печатались пустотелыми, для экономии материала, и получились лёгкими по весу, что хорошо скажется на грузоподъёмности манипулятора, при этом они обладают достаточной прочностью.

Рис. 5.1. Печать деталей для схвата манипулятора Так же на принтере Makerbot Replicator печатались детали для шарнира, который соединяет схват и 2 ое звено манипулятора, эти детали также являются пустотелыми для уменьшения веса. Все детали печатались по 3D моделям, разработанным в главе 3.4.

Всё остальное печаталось на 3D принтере 3DTouch. Это были детали основания и детали параллельного механизма. Они печатались с более грубой точностью чем детали схвата и не являются пустотелыми, при этом обладают значительной массой, но являются очень прочными. Прочность основания играет большую роль, т.к. на него действуют самые большие нагрузки. При этом большой вес основания не влияет на грузоподъёмность манипулятора. На рисунке 5.2 показаны напечатанные деталь основания и кронштейн для крепления звена параллельного механизма к основанию.

Рис. 5.2. Напечатанные деталь для основания и кронштейн

5.2 Ручное изготовление деталей

Детали 1-го и 2-го звена и дополнительных звеньев параллельного механизма манипулятора изготавливаются вручную из алюминиевых реек. Т.к. эти детали достаточно длинные, они должны обладать хорошей прочностью и жёсткостью. Алюминий обладает этими качествами в отличии от пластика, который более гибкий, и использование пластика привело бы к сильным колебаниям манипулятора во время движения.

Детали изготавливались согласно размерам 3D моделей из главы 3.4. На рисунке 5.3 изображена деталь 1 го звена с прикреплёнными к ней кронштейнами сервоприводов.

Рис. 5.3. Деталь 1 го звена На деталь 2 го звена кронштейн сервопривода крепится только с одной стороны (Рис. 5.4), к другой стороне будет крепится шарнирное соединение схвата.

Рис. 5.4. Деталь 2 го звена Чтобы манипулятор стоял устойчиво и не опрокинулся при работе, было решено сделать для него специальную платформу, к которой будет крепиться основание манипулятора. Размеры платформы будут 50×50 см. Т.к. значительная часть рабочей зоны манипулятора находится ниже его основания (Рис. 4.4), было решено поднять основание над платформой на 10 см. На рисунке 5.5 изображена модель платформы с указанными размерами.

Рис. 5.5. Схема платформы для манипулятора Платформа изготавливалась целиком из фанеры толщиной 10 мм, все элементы платформы скреплены саморезами (Рис. 5.6).

Рис. 5.6. Платформа для манипулятора

5.3 Сборка манипулятора

Все детали схвата скреплены болтами диаметром 4 мм, они не слижком затянуты, чтобы схват работал без затруднений. На рисунке 5.7 изображен собранный схват (вид сверху и снизу).

Рис. 5.7. Собранный схват манипулятора На рисунке 5.8 изображено скрепленные 1 ое и 2 ое звено, это основная часть конструкции манипулятора, её вес составляет 260 грамм.

Рис. 5.8. 1-ое и 2-ое звенья манипулятора

Схват крепится к 2 му звену через специальное шарнирное соединение (Рис. 5.9).

Рис. 5.9. Шарнирное соединение Нижняя неподвижная часть основания манипулятора крепится болтами к платформе (Рис. 5.10), затем к ней крепится верхняя подвижная часть основания.

Рис. 5.10. Нижняя часть основания Полностью собранный робот, установленный на платформу и подключенный к плате Arduino и блоку питания показан на рисунке 5.11

Рис. 5.11. Готовый манипулятор

6. Разработка системы управления манипулятором

6.1 Разработка системы управления манипулятором на основе Arduino

Система управления манипулятором разработана на основе Arduino Uno которая подключена к компьютеру через COM порт, по которому поступает на Arduino питание и команды для управления сервоприводами манипулятора. Четы сервопривода подключены к Arduino к цифровым портам под номерами 6, 8, 10, 12. Сервоприводы подключены параллельно к отдельному источнику питания, номинальное напряжение питания 7 В. Также земля (GND) сервоприводов подключена к Arduino к контакту GND.

Сервопривод «servo 0», подключенный к pin 6 (Рис. 6.1), управляет поворотом основания манипулятора, модель сервопривода SR430. «servo 1», подключенный к pin 8, управляет поворотом 1 го звена манипулятора, модель сервопривода DF15RMG. «servo 2», подключенный к pin 10, управляет поворотом 2 го звена манипулятора, модель сервопривода RDS3115. «servo 3», подключенный к pin 12, управляет сжатием / разжатием механического схвата манипулятора, модель сервопривода SR430.

При подключении Arduino к питанию, в ней тут же запускается программа и посылает сигналы на сервоприводы, чтобы они повернулись в начальное положение. Далее программа начинает ожидать команды, поступающие в COM порт с компьютера. Команды являются строками, содержащими значения углов поворотов сервоприводов манипулятора. Как только поступает команда (строка) в COM порт, программа считывает данные углов поворота из этой строки и отправляет сигналы управления с новыми значениями на сервоприводы.

6.2 Разработка копмьютерной программы для управления манипулятором

Компьютерная программа выполняет все вычисления для управления манипулятором. Она позволяет выполнить следующее: вручную управлять поворотами каждого звена по отдельности, а также схватом; управлять передвижением рабочей точки манипулятора вдоль осей X, Y, Z декартовой системы координат; управлять скоростью движения манипулятора; позволить программировать манипулятор для автоматического передвижения через конкретные точки.

Код программы написан на языке C# в среде Visual Studio. Внешне программа выглядит в виде формы, на которой расположены отдельные панели, выполняющие свои функции, например: панель для ручного управления роботом, которое выполняется с помощью кнопок; панель настройки скорости движения манипулятора; панель программирования манипулятора, которая позволяет задать и запустить программу автоматического управления роботом; панель информации, где указываются данные о состоянии манипулятора. На рисунке 6.3 показана копия экрана программы в которой стоят обозначения отдельных её функций, которые далее будут более подробно описаны.

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

7. Тестирование работы манипулятора

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

Сначала подключаются сервоприводы к Arduino согласно схеме на рисунке 6.1, далее Arduino подключается по COM порту к компьютеру. При подачи питания на сервоприводы манипулятор становится в начальное положение.

При запуске компьютерной программы управления манипулятором, она сразу определяет подключенный COM порт: выводится сообщение «COM порт подключен!». Для проверки правильности подключения сервоприводов к Arduino в ручном режиме управления проверяется каждое звено по отдельности, т. е. при нажатии кнопки должен отреагировать соответствующий сервопривод. Все сервоприводы подключены верно. Для проверки стабильности программы внезапно отключаем COM порт Arduino от компьютера, программа вывела сообщение «COM порт не подключен!», робот остался в том же положении в каком и был и никаких действий самостоятельно не производит. После обратного подключения COM порта нажимаю кнопку «Поиск COM портов» и программа тут же находит этот COM порт и подключается к нему, теперь возможно дальнейшее управление манипулятором. При ручном управлении манипулятором по осям координат, рабочая точка передвигается строго вдоль оси, по которой её передвигаю. Все звенья работают синхронно, т.к. для каждого нового движения просчитывается обратная задача кинематики.

При движении манипулятора заметны некоторые небольшие резкие рывки сервоприводов что приводит к колебанию всей конструкции манипулятора. Это происходит из-за того, что сервоприводы работают не с достаточно высокой точностью и на малые углы поворота (например в 1 градус) могут не отреагировать. При этом программа Arduino поворачивает сервоприводы с шагом в 0.01 градуса, т. е. с программной точки зрения движение должно происходить плавно.

Далее выполняется проверка программного управления. Подвожу в ручную положение рабочей точки манипулятора в нужную позицию и нажимаю кнопку «Добавить положение», данная позиция сохраняется. Таким образом создаётся вся программа целиком и добавляю команды задержки в некоторых местах. Нажимаю кнопку «Запуск», манипулятор начинает движение проходя через все запрограммированные точки и ожидает заданное количество времени на командах задержки, при движении манипулятора есть возможность регулировать скорость его движения в реальном времени.

Заключение

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

Также была построена 3D модель манипулятора, с учётом всех длин и размеров деталей, для определения её конструктивных ограничений, а именно диапазонов углов поворотов звеньев. Эти углы определили форму рабочей зоны, максимальный радиус которой получился 570 мм от центра основания робота. Рабочая зона строилась при помощи динамической модели манипулятора, в которой манипулятор прошёлся через все крайние положения рабочей зоны.

В динамической модели были определены зависимости различных велечин таких как: сила, момент, угловая скорость и угловое ускорение, которые действовали на оси поворотов звеньев и основания. Также было смоделировано как вращающий момент, который развивают реальные сервоприводы, действовал на движение звеньев манипулятора. Этот же момент использовался для определения грузоподъёмности манипулятора, которая составила 150 грамм при максимально вытянутой руке манипулятора.

На основе спроектированной трёхмерной модели были изготовлены детали для схвата, основания и ещё некоторые элементы манипулятора при помощи 3D печати из пластика и металлические детали звеньев, которые создавались вручную.

Была разработана система управления манипулятором на основе платы Arduino Uno, которая получает сигналы управления с компьютера. Компьютерная программа позволяет управлять манипулятором в различных режимах как вручную — управление каждым звеном манипулятора по отдельности или передвижением рабочей точки по осям координат, так и управление в автоматическом режиме, когда программируются различные позиции, по которым должен пройти манипулятор.

Проверка работоспособности манипулятора показала, что робот отрабатывает точно все движения, которые ему задаются в программе. При этом были заметны некоторые небольшие рывки при движении конструкции манипулятора, это происходит из-за особенностей сервоприводов. Тестирование работы манипулятора позволило определить его следующие параметры: повторяемость, которая в среднем составляла 2 мм; реальная грузоподъёмность, равная от 50 до 100 грамм в зависимости от длины плеча, что является меньше чем при моделировании, т.к. реальная масса робота оказалась немного больше ожидаемой; рабочая зона, которая получилась такая же как и при моделировании.

Так же были получены новые знания и навыки построения 3D моделей деталей, работы с 3D принтером, сборки манипулятора, разработки и тестировании его компьютерной динамической модели, разработки системы управления и программировании на языке C#.

Разработанный в данной работе, манипулятор может эффективно использоваться в качестве лабораторного оборудования для изучения задач кинематики и динамики роботов.

1. Промышленные роботы и манипуляторы [электронный ресурс]. URL: http://cncnc.ru/documentation/theory_of_mechanismus_and_machines/lect₁₉.htm (дата обращения: 09.06.2015).

2. Клюев С. А. Компьютерное моделирование: Учебно-методическое пособие. — М.: Волжский политехнический институт, 2009. — 89 с.

3. Arduino [электронный ресурс]. URL: https://www.arduino.cc/ (дата обращения: 09.06.2015).

4. Промышленные роботы манипуляторы [электронный ресурс]. URL: http://robo24.ru/promyshlennye-roboty (дата обращения: 09.06.2015).

5. Манипуляционные роботы [электронный ресурс]. URL: http://refleader.ru/jgeyfsrnarna.html (дата обращения: 10.06.2015).

6. Лекции по УРиРТС [электронный ресурс]. URL: http://www.studfiles.ru/dir/cat39/subj1292/file13007/view135824/page2.html (дата обращения: 10.06.2015).

7. Схиртладзе А. Г., Выходец В. И. Оборудование машиностроительных предприятий. — М.: РПК «Политехник», 2005. — 92 с.

8. Компьютерное моделирование [электронный ресурс]. URL: http://www.inf1.info/book/export/html/215 (дата обращения: 8.03.2015).

9. Гонсалес Р., Фу К., Ли К. Робототехника. — М.: Москва «Мир», 1989. — 620 с.

10. Adams — MSC Software [электронный ресурс]. URL: http://www.mscsoftware.com/product/adams (дата обращения: 10.06.2015).

11. Universal Mechanism — моделирование динамики механических систем [электронный ресурс]. URL: http://www.umlab.ru/pages/index.php? id=1 (дата обращения: 10.06.2015).

12. NX: Siemens PLM Software [электронный ресурс]. URL: http://www.plm.automation.siemens.com/ru_ru/products/nx/ (дата обращения: 10.06.2015).

13. CATIA — Dassault Systиmes [электронный ресурс]. URL: http://www.3ds.com/ru/produkty-i-uslugi/catia/ (дата обращения: 10.06.2015).

14. SolidWorks-Russia [электронный ресурс]. URL: http://www.solidworks.ru/ (дата обращения: 10.06.2015).

15. Пособие по ADAMS [электронный ресурс]. URL: http://mmm_samgu.ssu.samara.ru/polyakov/adams/Adams_pos_new.pdf (дата обращения: 8.03.2015).

16. Fanuc M-410iB Series [электронный ресурс]. URL: http://www.fanucrobotics.com/cmsmedia/datasheets/M-410iB % 20Series₁₅.pdf (дата обращения: 11.06.2015).

17. Описание сервопривода DF15RMG [электронный ресурс]. Режим доступа: http://www.electronshik.ru/item/df15rmg-tilt-kit-20kg-1 114 314 (дата обращения: 12.06.2015).

18. Описание сервопривода RDS3115 [электронный ресурс]. Режим доступа: http://www.aliexpress.com/snapshot/6 494 586 405.html? orderId=65 771 461 539 714 (дата обращения: 12.06.2015).

19. Описание сервопривода SR430 [электронный ресурс]. Режим доступа: http://robot-kit.ru/print_product_info.php/products_id/438 (дата обращения: 12.06.2015).

20. Описание Google SketchUp 8 [электронный ресурс]. Режим доступа: http://reviewsoft.ru/Windows/Audio-Video/Audio-Recorders/google-sketchup/ (дата обращения: 12.06.2015).

21. Visual Studio — Microsoft Developer Tools [электронный ресурс]. Режим доступа: https://www.visualstudio.com/ru-ru/visual-studio-homepage-vs.aspx (дата обращения: 13.06.2015).

Приложения

Приложение 1

Код Matlab для вычисления математической динамической модели манипулятора

L1 = 0.2;

L2 = 0.2;

Q1 = pi/4; dQ1 = 0; ddQ1 = 0;

Q2 = pi/4; dQ2 = pi/8; ddQ2 = pi/6;

m1 = 0.21;

m2 = 0.18;

g = 9.80 665;

C1 = cos (Q1); S1 = sin (Q1);

C2 = cos (Q1); S2 = sin (Q1);

C12 = cos (Q1+Q2); S12 = sin (Q1+Q2);

A1₀ = [C1 S1 0 L1*C1

S1 C1 0 L1*S1

0 0 1 0

0 0 0 1];

A2₁ = [C2 — S2 0 L2*C2

S2 C2 0 L2*S2

0 0 1 0

0 0 0 1];

A2₀ = A1₀ * A2₁;

MQ = [0 -1 0 0

1 0 0 0

0 0 0 0

0 0 0 0];

U11 = MQ * A1₀;

U21 = MQ * A2₀;

U22 = A1₀ * MQ * A2₁;

J1 = [1/3*m1*L1² 0 0 -½*m1*L1

0 0 0 0

0 0 0 0

— ½*m1*L1 0 0 m1];

J2 = [1/3*m2*L2² 0 0 -½*m2*L2

0 0 0 0

0 0 0 0

— ½*m2*L2 0 0 m2];

D11 = 1/3*m1*L1² + 4/3*m2*L2² + m2*C2*L2²;

D12 = 1/3*m2*L2² + ½*m2*L2²*C2;

D22 = 1/3*m2*L2²;

h1 = -½*m2*S2*L2²*dQ2² — m2*S2*L2²*dQ1*dQ2;

h2 = ½*m2*S2*L2²*dQ1²;

c1 = ½*m1*g*L1*C1 + ½*m2*g*L2*C12 + m2*g*L1*C1;

c2 = ½*m2*g*L2*C12;

D = [D11 D12

D12 D22];

ddQ = [ddQ1

ddQ2];

h = [h1

h2];

c = [c1

c2];

T = D*ddQ + h + c;

Приложение 2

Код программы Arduino для управления сервоприводами манипулятора

#include «Servo.h»

// Обьекты Servo

Servo servo0;

Servo servo1;

Servo servo2;

Servo servo3;

String s="";

int i;

int q0, nq0 = 94; // начальное положене угла серво0

int q1, nq1 = 144; // начальное положене угла серво1

int q2, nq2 = 180; // начальное положене угла серво2

int q3, nq3 = 50; // начальное положене угла серво3

float p0, p1, p2, p3;

void setup ()

{

Serial.begin (9600);

servo0.attach (6); // серво1 подключен к пин 6

servo1.attach (8); // серво1 подключен к пин 8

servo2.attach (10); // серво2 подключен к пин 10

servo3.attach (12); // серво3 подключен к пин 12

q0 = nq0;

q1 = nq1;

q2 = nq2;

q3 = nq3;

servo0.write (q0); // Повернуть серво0 на q0 градусов

servo1.write (q1); // Повернуть серво1 на q1 градусов

servo2.write (q2); // Повернуть серво2 на q2 градусов

servo3.write (q3); // Повернуть серво3 на q3 градусов

p0=q0;

p1=q1;

p2=q2;

p3=q3;

}

void loop ()

{

while (Serial.available ())

{ // считывание данных с COM порта

char c = Serial. read (); // читаем символ

s += c; // добавляем к строке

if (c == '!') // если считали символ, строка закончилась

{

i=0;

q0=0;

q1=0;

q2=0;

q3=0;

while (s[i]≠ ' ')

{

q0=q0*10;

q0=q0+(int (s[i]) — 48);

i++;

}

i++;

while (s[i]≠ ' ')

{

q1=q1*10;

q1=q1+(int (s[i]) — 48);

i++;

}

i++;

while (s[i]≠ ' ')

{

q2=q2*10;

q2=q2+(int (s[i]) — 48);

i++;

}

i++;

while (s[i]≠ '!')

{

q3=q3*10;

q3=q3+(int (s[i]) — 48);

i++;

}

s = ««;

}

}

// повороты сервоприводов

if (p0

{

p0=p0+0.01;

servo0.write (p0); // Повернуть серво0 на p0 градусов

}

if (p0>q0)

{

p0=p0−0.01;

servo0.write (p0); // Повернуть серво0 на p0 градусов

}

if (p1

{

p1=p1+0.01;

servo1.write (p1); // Повернуть серво1 на p1 градусов

}

if (p1>q1)

{

p1=p1−0.01;

servo1.write (p1); // Повернуть серво1 на p1 градусов

}

if (p2

{

p2=p2+0.01;

servo2.write (p2); // Повернуть серво2 на p2 градусов

}

if (p2>q2)

{

p2=p2−0.01;

servo2.write (p2); // Повернуть серво2 на p2 градусов

}

if (p3

{

p3=p3+0.01;

servo3.write (p3); // Повернуть серво3 на p3 градусов

}

if (p3>q3)

{

p3=p3−0.01;

servo3.write (p3); // Повернуть серво2 на p2 градусов

}

delay (0.01);

}

Приложение 3

Код компьютерной программы на языке C# для управления манипулятором

using System;

using System. Collections. Generic;

using System. ComponentModel;

using System. Data;

using System. Drawing;

using System. Linq;

using System. Text;

using System. Threading. Tasks;

using System. Windows. Forms;

using System.IO. Ports; // подключение пространства имен с классом SerialPort

using System. IO;

namespace RobotProgram

{

public partial class Form1: Form

{

double q0 = 0, q1 = 69, q2 = 38, q3 = 50; // значения начальных углов поворотов звеньев

int s0 = 1, s1 = 1, s2 = 1, s3 = 1; // скорости поворотов звеньев (изменять не нужно)

// длины звеньев, мм

int L0 = 90; // основание

int L1 = 200; // 1 ое звено

int L2 = 200; // 2 ое звено

int L3 = 170; // схват

// переменные координат и углов звеньев

double X, Y, Z, Xm, Ym, Zm, qq0, qq1, qq2;

// флаги наджатия кнопок управления

bool f0U = false, f0D = false,

f1U = false, f1D = false,

f2U = false, f2D = false,

f3U = false, f3D = false;

bool flag = true;

bool flag1 = false;

bool start_flag = false;

bool gotovo = true;

bool f_time = false;

int time1 = 0;

string str_pos = ««;

string str_pos₁ = ««;

string str1 = ««;

string str_x = ««;

string str_y = ««;

string str_z = ««;

string str_s = ««;

string str_t = ««;

int xk, yk, zk, sk, tk;

double k = 57.295 779 513;

double q3r;

string stroka = ««;

SerialPort Port1;

public Form1 ()

{

InitializeComponent ();

}

private void Form1_Load (object sender, EventArgs e)

{

string[] myPort; // создаем массив строк

myPort = System.IO. Ports. SerialPort. GetPortNames (); // в массив помещаем доступные порты

comboBox1. Items. AddRange (myPort); // теперь этот массив заносим в список (comboBox)

if (comboBox1. Items. Count≠ 0) comboBox1. SelectedIndex = 0; // если есть порты выбираем 1 ый

}

private void Form1_FormClosing (object sender, FormClosingEventArgs e)

{

if (comboBox1. Items. Count≠ 0) // если есть доступный COM порт

{

if (Port1. IsOpen≠ false)

{

Port1. Close (); // при закрытии программы закрываем порт (если он открыт)

}

}

}

private void button1_Click (object sender, EventArgs e) {}

private void button2_Click (object sender, EventArgs e) {}

//// управление роботом ////

//// ручное управление ////

private void timer1_Tick (object sender, EventArgs e)

{

if (angles. Checked) // если выбрано управление углами

{

label1. Text = «Q0»;

label2. Text = «Q1»;

label3. Text = «Q2»;

// определение значений скоростей регулировки углов

//s0 = Q0TB. Value; // скорость движения манипулятора

s0 = 1;

s1 = s0;

s2 = s0;

s3 = 1;

// настройка скорости

if ((q3r * k + s1) > 0)

{

s1 = (int) Math. Abs (Math. Round (q3r * k));

s2 = s1;

if (s1 == 0)

{

s1 = 1;

s2 = 1;

}

}

// управление углами

if (f0U)

{

q0 = q0 + s0; // увеличение q0

if (q0 > 85) q0 = 85;

}

if (f0D)

{

q0 = q0 — s0; // уменьшение q0

if (q0 < -85) q0 = -85;

}

Q0T. Text = q0. ToString (); // вывод значения угла q0

if (f1U)

{

q1 = q1 + s1; // увеличение q1

if (q1 > 102) q1 = 102;

}

if (f1D && ((q3r * k) < 0))

{

q1 = q1 — s1; // уменьшение q1

if (q1 < -42) q1 = -42;

}

Q1T. Text = q1. ToString (); // вывод значения угла q1

if (f2U)

{

q2 = q2 + s2; // увеличение q2

if (q2 > 48) q2 = 48;

}

if (f2D && ((q3r * k) < 0))

{

q2 = q2 — s2; // уменьшение q2

if (q2 < -90) q2 = -90;

}

Q2T. Text = q2. ToString (); // вывод значения угла q2

if (f3U)

{

q3 = q3 + s3; // увеличение q3

if (q3 > 90) q3 = 90;

}

if (f3D)

{

q3 = q3 — s3; // уменьшение q3

if (q3 < 0) q3 = 0;

}

Q3T. Text = q3. ToString (); // вывод значения угла q3

// корректировка q1 и q2

if ((q3r * k) > 0)

{

if (q1 < 0) q1 = q1 + s0;

if (q2 < 0) q2 = q2 + s0;

}

pryamaya_zadacha (); // решение прямой задачи

// присвоение координат

X = Xm;

Y = Ym;

Z = Zm;

}

if (coordinates. Checked) // если выбрано управление координатами

{

label1. Text = «X»;

label2. Text = «Y»;

label3. Text = «Z»;

// скорость движения манипулятора

s0 = 1;

s3 = 1;

// настройка скорости

if ((q3r * k + s1) > 0)

{

s0 = (int) Math. Abs (Math. Round (q3r * k));

if (s0 == 0) s0 = 1;

}

// управление координатами

if (f0U &&! ((flag == false) && (X > 0)))

{

X = X + s0; // увеличение X

}

if (f0D)

{

X = X — s0; // уменьшение X

}

Q0T. Text = X. ToString (); // вывод значения X

if (f1U &&! ((flag == false) && (Y > 0)))

{

Y = Y + s0; // увеличение Y

}

if (f1D &&! ((flag == false) && (Y < 0)))

{

Y = Y — s0; // уменьшение Y

}

Q1T. Text = Y. ToString (); // вывод значения Y

if (f2U && ((q3r * k) < 0) &&! ((flag == false) && (Z > 0)))

{

Z = Z + s0; // увеличение Z

}

if (f2D &&! ((flag == false) && (Z < 0)))

{

Z = Z — s0; // уменьшение Z

}

Q2T. Text = Z. ToString (); // вывод значения угла Z

// управление схватом

if (f3U)

{

q3 = q3 + s3; // разжимать схват

if (q3 > 90) q3 = 90;

}

if (f3D)

{

q3 = q3 — s3; // сжимать схват

if (q3 < 0) q3 = 0;

}

Q3T. Text = q3. ToString (); // вывод значения угла q3

obratnaya_zadacha (); // решение обратной задачи

if (double. IsNaN (qq0) || double. IsNaN (qq1) || double. IsNaN (qq2))

{ // если при решении обратной задачи значение угла будет NaN, значит это уже недосягаемая зона

label9. ForeColor = Color. Red;

label9. Text = «Достигнут край рабочей зоны!»;

flag = false; // ставим флаг, чтобы робот дальше не двигался

}

else

{

q0 = qq0;

q1 = qq1;

q2 = qq2;

flag = true;

}

korrektirovka (); // корректировка углов поворотов звеньев

}

// вывод различных значений на экран

label10. Text = «X =» + Math. Round (X).ToString ();

label11. Text = «Y =» + Math. Round (Y).ToString ();

label12. Text = «Z =» + Math. Round (Z).ToString ();

label13. Text = «Q0 =» + q0. ToString ();

label14. Text = «Q1 =» + q1. ToString ();

label15. Text = «Q2 =» + q2. ToString ();

label19. Text = «Схват =» + q3. ToString ();

if (start_flag) Start. Text = «Стоп»;

else Start. Text = «Запуск»;

if ((gotovo == false) && (start_flag == true)) action ();

if (start_flag)

{

groupBox1. Enabled = false;

angles. Checked = true;

}

else groupBox1. Enabled = true;

//// порграммное управление ////

if (start_flag && radioButton1. Checked) // одноразовое выполнение программы

{

if (listBox1. Items. Count > 0) // если есть строки

{

try

{

string st = listBox1. Items [listBox1. SelectedIndex]. ToString (); // если нет выделенных строк

}

catch

{

listBox1. SetSelected (0, true); // выделяем 1 ую строку

}

if ((listBox1. Items [listBox1. SelectedIndex]. ToString ().Length > 0) && gotovo) // если есть строка

{

str_pos = listBox1. Items [listBox1. SelectedIndex]. ToString (); // считывание строки-команды

if (String. Equals (str_pos, str_pos₁) == false) gotovo = false; // если поступила новая команда, ставим флаг на выполнение команды

}

if (gotovo) // если команда выполнена

{

if (listBox1. Items. Count > (listBox1. SelectedIndex + 1)) listBox1. SetSelected (listBox1. SelectedIndex + 1, true); // переключаемся на следующую строку-команду

else start_flag = false; // если строк-команд нет, программа завершена

}

}

else start_flag = false; // если строк-команд нет, программа завершена

}

if (start_flag && radioButton2. Checked) // выполнение в цикле

{

if (listBox1. Items. Count > 0) // если есть строки

{

try

{

string st = listBox1. Items [listBox1. SelectedIndex]. ToString (); // если нет выделенных строк

}

catch

{

listBox1. SetSelected (0, true); // выделяем 1 ую строку

}

if ((listBox1. Items [listBox1. SelectedIndex]. ToString ().Length > 0) && gotovo) // если есть строка

{

str_pos = listBox1. Items [listBox1. SelectedIndex]. ToString (); // считывание строки-команды

if (String. Equals (str_pos, str_pos₁) == false) gotovo = false; // если поступила новая команда, ставим флаг на выполнение команды

}

if (gotovo) // если команда выполнена

{

if (listBox1. Items. Count > (listBox1. SelectedIndex + 1)) listBox1. SetSelected (listBox1. SelectedIndex + 1, true); // переключаемся на следующую строку-команду

else listBox1. SetSelected (0, true); // если строк-команд нет, переходим в начало

}

}

else start_flag = false; // если строк-команд нет, программа завершена

}

if (start_flag && radioButton3. Checked) // пошаговое выполнение

{

if (listBox1. Items. Count > 0) // если есть строки

{

try

{

string st = listBox1. Items [listBox1. SelectedIndex]. ToString (); // если нет выделенных строк

}

catch

{

listBox1. SetSelected (0, true); // выделяем 1 ую строку

}

if ((listBox1. Items. Count > (listBox1. SelectedIndex + 1))) // если есть следующая строка

{

if ((listBox1. Items [listBox1. SelectedIndex + 1]. ToString ().Length > 0) && gotovo && (flag1==false)) // если есть строка

{

if (flag1 == false) listBox1. SetSelected (listBox1. SelectedIndex + 1, true); // переключаемся на следующую строку-команду

str_pos = listBox1. Items [listBox1. SelectedIndex]. ToString (); // считывание строки-команды

gotovo = false; // ставим флаг на выполнение команды

flag1 = true; // ставим флаг, что начали выполнение программы

}

if (gotovo && flag1) // если команда выполнена

{

flag1 = false;

start_flag = false; // программа завершена

}

}

else

{

start_flag = false; // если строк-команд нет, программа завершена

}

}

else start_flag = false; // если строк-команд нет, программа завершена

}

//// конец программного управления ////

// вывод информации о состоянии программы

if (start_flag)

{

label20. Text = «Выполнение программы»;

if (String. Equals (str1, «P»)) // если это команда позиции

{

label21. Text = «Движение к позиции»;

label22. Text = «X=» + str_x +" Y=" + str_y +" Z=" + str_z;

label23. Text = «Схват=» + str_s;

}

if (String. Equals (str1, «D»)) // если это команда задержки

{

label21. Text = «Ожидание»;

label22. Text = «time="+time1. ToString ();

label23. Text = ««;

}

}

else

{

label20. Text = ««;

label21. Text = ««;

label22. Text = ««;

label23. Text = ««;

}

}

///// конец управления роботом //////

private void korrektirovka () // корректировка положения углов

{

bool f1 = false;

if (q0 > 85)

{

q0 = 85;

f1 = true;

}

if (q0 < -85)

{

q0 = -85;

f1 = true;

}

if (q1 > 102)

{

q1 = 102;

f1 = true;

}

if (q1 < -42)

{

q1 = -42;

f1 = true;

}

if (q2 > 48)

{

q2 = 48;

f1 = true;

}

if (q2 < -90)

{

q2 = -90;

f1 = true;

}

if ((q3r * k) > 0)

{

if (q1 < 0) q1 = q1 + s0;

if (q2 < 0) q2 = q2 + s0;

f1 = true;

}

pryamaya_zadacha ();

if (f1)

{

X = Xm;

Y = Ym;

Z = Zm;

f1 = false;

}

}

private void pryamaya_zadacha ()

{

// -Прямая_задача- //

k = 57.295 779 513;

double q0r = q0 / k;

double q1r = q1 / k;

double q2r = q2 / k;

q3r = - (q1r + q2r);

double sin_q0 = Math. Sin (q0r);

double sin_q1 = Math. Sin (q1r);

double sin_q2 = Math. Sin (q2r);

double sin_q3 = Math. Sin (q3r);

double cos_q0 = Math. Cos (q0r);

double cos_q1 = Math. Cos (q1r);

double cos_q2 = Math. Cos (q2r);

double cos_q3 = Math. Cos (q3r);

Xm = L2 * (cos_q0 * cos_q1 * cos_q2 — cos_q0 * sin_q1 * sin_q2) + L3 * (cos_q3 * (cos_q0 * cos_q1 * cos_q2 — cos_q0 * sin_q1 * sin_q2) — sin_q3 * (cos_q0 * cos_q1 * sin_q2 + cos_q0 * cos_q2 * sin_q1)) + L1 * cos_q0 * sin_q1;

Ym = L2 * (cos_q1 * cos_q2 * sin_q0 — sin_q0 * sin_q1 * sin_q2) + L3 * (cos_q3 * (cos_q1 * cos_q2 * sin_q0 — sin_q0 * sin_q1 * sin_q2) — sin_q3 * (cos_q1 * sin_q0 * sin_q2 + cos_q2 * sin_q0 * sin_q1)) + L1 * sin_q0 * sin_q1;

Zm = L0 — L2 * (cos_q1 * sin_q2 + cos_q2 * sin_q1) — L3 * (cos_q3 * (cos_q1 * sin_q2 + cos_q2 * sin_q1) + sin_q3 * (cos_q1 * cos_q2 — sin_q1 * sin_q2)) + L1 * cos_q1;

Xm = Math. Round (Xm);

Ym = Math. Round (Ym);

Zm = Math. Round (Zm);

}

private void obratnaya_zadacha ()

{

// -Обратная_задача- //

double xx = Math. Sqrt (X * X + Y * Y);

double x1 = xx — L3;

double z1 = Z — L0;

double B = Math. Sqrt (x1 * x1 + z1 * z1);

qq0 = Math. Asin (Y / xx);

qq1 = 1.570 796 327 — Math. Acos ((L1 * L1 + B * B — L2 * L2) / (2 * B * L2)) — Math. Asin (z1 / B);

qq2 = 1.570 796 327 — Math. Acos ((L1 * L1 + L2 * L2 — B * B) / (2 * L1 * L2));

qq0 = Math. Round (qq0 * k);

qq1 = Math. Round (qq1 * k);

qq2 = Math. Round (qq2 * k);

}

// обработчики нажатия кнопок управления

private void Q0U_MouseDown (object sender, MouseEventArgs e)

{

f0U = true;

}

private void Q0U_MouseUp (object sender, MouseEventArgs e)

{

f0U = false;

}

private void Q0D_MouseDown (object sender, MouseEventArgs e)

{

f0D = true;

}

private void Q0D_MouseUp (object sender, MouseEventArgs e)

{

f0D = false;

}

private void Q1U_MouseDown (object sender, MouseEventArgs e)

{

f1U = true;

}

private void Q1U_MouseUp (object sender, MouseEventArgs e)

{

f1U = false;

}

private void Q1D_MouseDown (object sender, MouseEventArgs e)

{

f1D = true;

}

private void Q1D_MouseUp (object sender, MouseEventArgs e)

{

f1D = false;

}

private void Q2U_MouseDown (object sender, MouseEventArgs e)

{

f2U = true;

}

private void Q2U_MouseUp (object sender, MouseEventArgs e)

{

f2U = false;

}

private void Q2D_MouseDown (object sender, MouseEventArgs e)

{

f2D = true;

}

private void Q2D_MouseUp (object sender, MouseEventArgs e)

{

f2D = false;

}

private void Q3U_MouseDown (object sender, MouseEventArgs e)

{

f3U = true;

}

private void Q3U_MouseUp (object sender, MouseEventArgs e)

{

f3U = false;

}

private void Q3D_MouseDown (object sender, MouseEventArgs e)

{

f3D = true;

}

private void Q3D_MouseUp (object sender, MouseEventArgs e)

{

f3D = false;

}

private void trackBar1_Scroll (object sender, EventArgs e)

{

// регулировка задержки таймера1

// отвечает за скорость скорость перемещения робота

int period1 = trackBar1. Value;

period1 = (101 — period1);

timer1. Interval = period1; // уровень задержки таймера

labelTimer. Text = (101 — period1).ToString ()+ «%»;

}

private void trackBar2_Scroll (object sender, EventArgs e)

{

// регулировка задержки таймера2

// отвечает за частоту отправки команд управления

int period2 = trackBar2. Value;

double period21 = 1000 / period2;

period21 = Math. Round (period21);

timer2. Interval = (int) period21; // уровень задержки таймера

label8. Text = period2. ToString () + «команд/сек»;

}

private void comboBox1_SelectedIndexChanged (object sender, EventArgs e)

{ // выбирает доступный COM порт, если он есть

if (comboBox1. SelectedItem. ToString ()≠ ««) Port1 = new SerialPort (comboBox1. SelectedItem. ToString (), 9600);

}

private void Poisk_Click (object sender, EventArgs e)

{

// поиск доступных COM портов

string[] myPort; // создаем массив строк

myPort = System.IO. Ports. SerialPort. GetPortNames (); // в массив помещаем доступные порты

comboBox1. Items. Clear (); // очистка comboBox

comboBox1. Items. AddRange (myPort); // теперь этот массив заносим в список (comboBox)

if (comboBox1. Items. Count≠ 0) comboBox1. SelectedIndex = 0;

}

private void timer2_Tick (object sender, EventArgs e)

{

// -Отправка_команды_на_Arduino- //

// преобразование углов для сервоприводов

double q0s = Math. Round (94 — q0);

double q1s = Math. Round (102 — q1);

double q2s = Math. Round (132 + q2);

string komanda = ««; // очистка строки

if (System.IO. Ports. SerialPort. GetPortNames ().Length == 0) comboBox1. Items. Clear (); // если нет портов очистка comboBox

if (double. IsNaN (q0) || double. IsNaN (q1) || double. IsNaN (q2) || double. IsNaN (X) || double. IsNaN (Y) || double. IsNaN (Z))

{

label9. ForeColor = Color. Red;

label9. Text = «Выход из рабочей зоны!»;

}

else

{

komanda = q0s. ToString () + «» + q1s. ToString () +" «+ q2s. ToString () +» «+ q3. ToString () +»!"; // создание строки значений углов поворотов звеньев

textBox1. Text = komanda;

if (komanda≠ stroka)

{

if (comboBox1. Items. Count≠ 0) // если есть доступный COM порт

{

label9. ForeColor = Color. LimeGreen;

if (flag) label9. Text = «COM порт подключен!»;

try

{

Port1 = new SerialPort (comboBox1. SelectedItem. ToString (), 9600);

Port1. Open (); // открываем порт

Port1. Write (komanda); // отправка строки значений углов

Port1. Close (); // закрываем порт

}

catch (Exception)

{

label9. ForeColor = Color. Red;

if (flag) label9. Text = «Ошибка подключения по COM порту!»;

}

}

else

{

label9. ForeColor = Color. Red;

if (flag) label9. Text = «COM порт не подключен!»;

}

}

}

stroka = komanda;

// -Конец_отправки- //

}

private void button1_Click₁ (object sender, EventArgs e)

{

string position = «P» + X + «» + Y +" «+ Z +» «+ q3;

// добавление в список позицию робота

if ((listBox1. Items. Count == 0) || (listBox1. SelectedIndex == (listBox1. Items. Count — 1)))

{

listBox1. Items. Add (position);

listBox1. SetSelected (listBox1. Items. Count — 1, true);

}

else

{

listBox1. Items. Insert (listBox1. SelectedIndex + 1, position);

}

}

private void button2_Click₁ (object sender, EventArgs e)

{

string delay = «D» + textBox2. Text;

// добавление в список позицию робота

if ((listBox1. Items. Count == 0) || (listBox1. SelectedIndex == (listBox1. Items. Count — 1)))

{

listBox1. Items. Add (delay);

listBox1. SetSelected (listBox1. Items. Count — 1, true);

}

else

{

listBox1. Items. Insert (listBox1. SelectedIndex + 1, delay);

}

}

private void button3_Click (object sender, EventArgs e)

{

// удаление выбранной строки

int p = listBox1. SelectedIndex;

if (p >= 0) listBox1. Items. RemoveAt (p);

if (p — 1 >= 0) listBox1. SetSelected (p — 1, true);

if ((p == 0) && (listBox1. Items. Count >= 1)) listBox1. SetSelected (p, true);

}

private void button4_Click (object sender, EventArgs e)

{

listBox1. Items. Clear (); // очистить поле

}

private void button6_Click (object sender, EventArgs e)

{

// сохранение текста программы двмжения робота

Stream myStream;

SaveFileDialog saveFileDialog1 = new SaveFileDialog ();

saveFileDialog1. Filter = «txt files (*.txt)|*.txt»;

saveFileDialog1. FilterIndex = 2;

saveFileDialog1. RestoreDirectory = true;

if (saveFileDialog1. ShowDialog () == DialogResult. OK)

{

if ((myStream = saveFileDialog1. OpenFile ())≠ null)

{

StreamWriter sw = new StreamWriter (myStream);

for (int i = 0; i < listBox1. Items. Count; i++)

{

listBox1. SelectedIndex = i;

sw. WriteLine (listBox1. SelectedItem. ToString ());

}

sw. Close ();

myStream. Close ();

}

}

}

private void button7_Click (object sender, EventArgs e)

{

// загрузка текста программы движения робота

OpenFileDialog openFileDial = new OpenFileDialog ();

string line;

if (openFileDial. ShowDialog () == DialogResult. OK)

{

listBox1. Items. Clear (); // очистить поле

string fileName = openFileDial. FileName;

FileStream sss = File. Open (fileName, FileMode. Open, FileAccess. Read);

if (sss≠ null)

{

StreamReader reader = new StreamReader (sss);

while ((line = reader. ReadLine ())≠ null)

listBox1. Items. Add (line);

sss. Close ();

}

}

}

private void action ()

{

// выполнение команды программы

if (String. Equals (str_pos, str_pos₁)==false) // если поступила новая команда

{

str_pos₁ = str_pos;

str1 = ««;

str_x = ««;

str_y = ««;

str_z = ««;

str_s = ««;

str_t = ««;

int i = 0;

// считывание команды

if (str_pos. Length > 0)

{

while ((i < str_pos. Length) && (Char. Equals (str_pos[i], ' ') == false))

{

str1 = str1 + str_pos[i];

i = i + 1;

}

if (String. Equals (str1, «P») == true) // если это команда позиции

{

i = i + 1;

while ((i < str_pos. Length) && (Char. Equals (str_pos[i], ' ') == false))

{

str_x = str_x + str_pos[i];

i = i + 1;

}

i = i + 1;

while ((i < str_pos. Length) && (Char. Equals (str_pos[i], ' ') == false))

{

str_y = str_y + str_pos[i];

i = i + 1;

}

i = i + 1;

while ((i < str_pos. Length) && (Char. Equals (str_pos[i], ' ') == false))

{

str_z = str_z + str_pos[i];

i = i + 1;

}

i = i + 1;

while ((i < str_pos. Length) && (Char. Equals (str_pos[i], ' ') == false))

{

str_s = str_s + str_pos[i];

i = i + 1;

}

xk = int. Parse (str_x);

yk = int. Parse (str_y);

zk = int. Parse (str_z);

sk = int. Parse (str_s);

// -Обратная_задача- //

double xx = Math. Sqrt (xk * xk + yk * yk);

double x1 = xx — L3;

double z1 = zk — L0;

double B = Math. Sqrt (x1 * x1 + z1 * z1);

qq0 = Math. Asin (yk / xx);

qq1 = 1.570 796 327 — Math. Acos ((L1 * L1 + B * B — L2 * L2) / (2 * B * L2)) — Math. Asin (z1 / B);

qq2 = 1.570 796 327 — Math. Acos ((L1 * L1 + L2 * L2 — B * B) / (2 * L1 * L2));

qq0 = Math. Round (qq0 * k);

qq1 = Math. Round (qq1 * k);

qq2 = Math. Round (qq2 * k);

}

if (String. Equals (str1, «D»)) // если это команда задержки

{

i = i + 1;

while ((i < str_pos. Length) && (Char. Equals (str_pos[i], ' ') == false))

{

str_t = str_t + str_pos[i];

i = i + 1;

}

tk = int. Parse (str_t);

}

}

}

// выполнение команды

if (String. Equals (str1, «P»)) // если это команда позиции

{

if (q0 < qq0)

{

q0 = q0 + 1;

if (q0 > 85) q0 = 85;

}

if (q0 > qq0)

{

q0 = q0 — 1;

if (q0 < -85) q0 = -85;

}

//Q0T. Text = q0. ToString (); // вывод значения угла q0

if (q1 < qq1)

{

q1 = q1 + 1;

if (q1 > 102) q1 = 102;

}

if ((q1 > qq1) && ((q3r * k) < 0))

{

q1 = q1 — 1;

if (q1 < -42) q1 = -42;

}

//Q1T. Text = q1. ToString (); // вывод значения угла q1

if (q2 < qq2)

{

q2 = q2 + 1;

if (q2 > 48) q2 = 48;

}

if ((q2 > qq2) && ((q3r * k) < 0))

{

q2 = q2 — 1;

if (q2 < -90) q2 = -90;

}

//Q2T. Text = q2. ToString (); // вывод значения угла q2

if (q3 < sk)

{

q3 = q3 + 1;

if (q3 > 90) q3 = 90;

}

if (q3 > sk)

{

q3 = q3 — 1;

if (q3 < 0) q3 = 0;

}

//Q3T. Text = q3. ToString (); // вывод значения угла q3

if ((q3r * k) > 0)

{

if (q1 < 0) q1 = q1 + 1;

if (q2 < 0) q2 = q2 + 1;

}

pryamaya_zadacha ();

X = Xm;

Y = Ym;

Z = Zm;

// если все углы достигнуты, команда выполнена

if ((q0 == qq0) && (q1 == qq1) && (q2 == qq2) && (q3 == sk)) gotovo = true;

}

if (String. Equals (str1, «D»)) // если это команда задержки

{

if (time1 == 0) time1 = tk*10;

f_time = true;

}

}

private void Start_Click (object sender, EventArgs e)

{

if (start_flag == false) start_flag = true;

else start_flag = false;

}

private void timer3_Tick (object sender, EventArgs e)

{

if (f_time && start_flag)

{

if (time1 == 1)

{

f_time = false;

gotovo = true;

}

if (time1 > 0) time1 = time1 — 1;

}

}

private void textBox2_KeyPress (object sender, KeyPressEventArgs e)

{

// ограничение наввод только цифр

if (Char. IsDigit (e. KeyChar) || (e. KeyChar == 8))

{

}

else

{

e. Handled = true;

}

}

}

}