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

Алевтина Чугунова

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

Работа посвящена разработке математической модели для робота - манипулятора типа «трипод». В частности, были выполнены следующие шаги: - Обзор и изучение конструкций механизмов с параллельной структурой; - Построение геометрической модели манипулятора Festo; - Решение прямой и обратной задач кинематики движения; - Построение на основе кинематических уравнений связи зоны достижимости и анализ полученного результата; - Решение прямой и обратной задач динамики движения и анализ полученных результатов; - Разработка пользовательского приложения в среде разработке MATLAB 6 для простоты получения решения подобных задач.

Механика

Дипломы

Вуз: Национальный исследовательский университет "МЭИ" (ФГБОУ ВПО «НИУ «МЭИ»)

ID: 5f3e5503cd3d3e000152177b

UUID: 9f411c00-c500-0138-1da0-0242ac180006

Язык: Русский

Опубликовано: больше 3 лет назад

Просмотры: 209

14.28

Алевтина Чугунова

Национальный исследовательский университет "МЭИ" (ФГБОУ ВПО «НИУ «МЭИ»)

Комментировать 15

Рецензировать 0

Скачать - 5,0 МБ

Поделиться работой

МИНОБРНАУКИ РОССИИ федеральное государственное бюджетное образовательное учреждение высшего образования «Национальный исследовательский университет «МЭИ» Институт Кафедра ИВТИ УИТ ВЫПУСКНАЯ КВАЛИФИКАЦИОННАЯ РАБОТА (бакалаврская работа) Направление 27.03.04 ‒ Управление в технических системах (код и наименование) Направленность (профиль) “Системы и технические средства автоматизации и управления” Форма обучения очная (очная/очно-заочная/заочная) Тема: Разработка математической модели манипулятора типа «трипод» Студент А-02-16 группа Научный руководитель к.т.н Чугунова А.В. подпись фамилия и инициалы ст.преп. Шилин Д.В. уч. степень должность подпись фамилия и инициалы уч. степень должность подпись фамилия и инициалы уч. степень должность подпись фамилия и инициалы подпись фамилия и инициалы Консультант Консультант «Работа допущена к защите» Зав. кафедрой д.т.н. доцент уч. степень должность Бобряков А. В. Дата Москва, 2020

МИНОБРНАУКИ РОССИИ Федеральное государственное бюджетное образовательное учреждение высшего образования «Национальный исследовательский университет «МЭИ» Институт Кафедра ИВТ УИТ ЗАДАНИЕ НА ВЫПУСКНУЮ КВАЛИФИКАЦИОННУЮ РАБОТУ (бакалаврскую работу) УПРАВЛЕНИЕ В ТЕХНИЧЕСКИХ СИСТЕМАХ Направление 27.03.04 «Управление в технических системах» (код и наименование) Направленность (профиль) Системы и технические средства автоматизации и управления Форма обучения очная (очная/очно-заочная/заочная) Тема: Разработка математической модели манипулятора типа «трипод» Студент А-02-16 Чугунова А.В. группа Научный руководитель к.т.н. подпись ст. преподаватель фамилия и инициалы Шилин Д.В. уч. степень должность подпись фамилия и инициалы уч. степень должность подпись фамилия и инициалы уч. степень должность подпись фамилия и инициалы Консультант Консультант Зав. кафедрой д.т.н. уч. степень Место выполнения работы технологий доцент звание Бобряков А.В. подпись фамилия и инициалы кафедра управления и интеллектуальных 2

СОДЕРЖАНИЕ РАЗДЕЛОВ ЗАДАНИЯ И ИСХОДНЫЕ ДАННЫЕ Введение 1. Описание выбранной конструкции промышленного манипулятора. Обзор и описание компонентов в составе промышленного манипулятора. 2. Решение прямой задачи кинематики. Решение обратной задачи кинематики. Проверка работоспособности кинематической модели на краевых условиях 3. Разработка динамической модели манипулятора. Построение и исследование переходных характеристик. 4. Разработка пользовательского приложения для расчетов. Заключение Библиографический список Приложение 1. Листинг кода, разработанного приложения Исходные данные: ПО MATLAB ПЕРЕЧЕНЬ ГРАФИЧЕСКОГО МАТЕРИАЛА Количество листов Количество слайдов в презентации РЕКОМЕНДУЕМАЯ ЛИТЕРАТУРА 1) ГОСТ 7.1-84, Библиографическое описание документа. Общие требования и правила составления. 2) ГОСТ 7.9-95. Реферат и аннотация. Общие требования. 3) ГОСТ 7.5-98. Журналы, сборники, информационные издания. Издательское оформление публикуемых материалов. 4) Электрические схемы подключения станции хранения манипулятора типа «трипод» [Электронный ресурс] // URL:https://www.festo-didactic.com/ru-ru (дата обращения: 20.12.2019). 5) Макаров И.М. Системные принципы создания гибких автоматизированных производств. М.: Высш. шк., 1986. 175 с 6) Управление робототехническими системами и гибкими автоматизированными производствами / И.М.Макаров, В.З.Рахманкулов, В.М.Назаретов и др. М.: Высш. шк., 1986. 159 с 7) Дж.Дэбни, Т.Харман Simulink 4. Секреты мастерства. Издательство: Бином. Лаборатория знаний, 2003 г. Твердый переплет, 404 стр. ISBN 594774-048-6, 0-13-1-017085-2. Тираж: 3000 экз. Формат: 70x100/16 8) В.П. Дьяконов. MATLAB 6.5 SP1/7 + Simulink 5/6. Основы применения. М.: СОЛОН--Пресс, 2005. - 800с 3

АННОТАЦИЯ Выпускная работа студентки Чугуновой А.В. группы А-02-16. Работа посвящена разработке математической модели для робота - манипулятора типа «трипод». В частности, были выполнены следующие шаги: - Обзор и изучение конструкций механизмов с параллельной структурой; - Построение геометрической модели манипулятора Festo; - Решение прямой и обратной задач кинематики движения; - Построение на основе кинематических уравнений связи зоны достижимости и анализ полученного результата; - Решение прямой и обратной задач динамики движения и анализ полученных результатов; - Разработка пользовательского приложения в среде разработке MATLAB 6 для простоты получения решения подобных задач. Работа представлена на 91 страницах, 51 рисунке, и 6 приложениях. SUMMARY Graduation work of student Chugunova A.V. Group А-02-16. The work is devoted to the development of a mathematical model for a robot manipulator of the "tripod" type. In particular, the following steps have been taken: - Review and study of structures of mechanisms with parallel structure; - Construction of geometric model of Festo manipulator; - Solving forward and reverse motion kinematics problems; - Construction of the reachability zone connection on the basis of kinematic equations and analysis of the obtained result; - Solving forward and reverse motion dynamics problems and analyzing the results obtained; - Develop a user application in the MATLAB 6 development environment for ease of solving similar problems. The work is presented on 91 pages, 51 illustrations and 6 annexes. 5

СОДЕРЖАНИЕ АННОТАЦИЯ ........................................................................................................................... 5 СОДЕРЖАНИЕ ........................................................................................................................ 6 ВВЕДЕНИЕ ............................................................................................................................... 7 1 ОБЗОР И ОПИСАНИЕ КОНСТРУКЦИИ МАНИПУЛЯТОРА С ПАРАЛЛЕЛЬНОЙ КИНЕМАТИКОЙ ................................................................................ 9 1.1. Обзор конструкций манипуляторов типа «трипод» ........................................... 9 1.2. Описание конструкции «трипода» EXPT компании Festo .............................. 15 2 КИНЕМАТИКА РОБОТА С ПАРАЛЛЕЛЬНОЙ КИНЕМАТИКОЙ ...................... 18 2.1 Анализ манипулятора и вывод описания кинематической модели .................. 18 2.2 Поиск решения прямой и обратной задачи кинематики. .................................... 30 2.3 Краткие выводы по главе. .......................................................................................... 39 3 ДИНАМИКА РОБОТА С ПАРАЛЛЕЛЬНОЙ КИНЕМАТИКОЙ ............................ 41 3.1. Анализ манипулятора и вывод описания динамической модели .................. 41 3.2. Поиск решения прямой и обратной задачи динамики. .................................... 48 3.3. Краткие выводы по главе....................................................................................... 52 4 РАЗРАБОТКА ПРИЛОЖЕНИЯ ДЛЯ РЕШЕНИЯ ЗАДАЧ ....................................... 54 4.1 Описание интерфейса разработанной программы по решению прямой, обратной задач кинематики и зоны достижимости..................................................... 54 4.2 Описание интерфейса разработанной программы по решению прямой, обратной задач динамики. ................................................................................................ 62 4.2.1 Алгоритм работы окна «Прямая задача динамики» приложения. ............. 62 4.2.2 Алгоритм работы окна «Обратная задача динамики» приложения. .......... 66 5 ЗАКЛЮЧЕНИЕ ................................................................................................................... 71 6 СПИСОК ИСПОЛЬЗУЕМОЙ ЛИТЕРАТУРЫ ............................................................ 72 7 ПРИЛОЖЕНИЕ .................................................................................................................. 74 Приложение 1. Организация главной программы приложения .............................. 74 Приложение 2. Организация решения задачи прямой кинематики........................ 75 Приложение 3. Организация решения задачи обратной кинематики..................... 75 Приложение 4. Организация решения получения зоны достижимости ................. 85 Приложение 5. Организация решения задачи прямой динамики ............................ 90 Приложение 6. Организация решения задачи обратной динамики......................... 93 6

ВВЕДЕНИЕ В настоящий момент времени рост производительности промышленных компаний, а также повышение количества и качества выпускаемой продукции представляет собой важную задачу. Организация автоматизированных процессов при производстве может быть достигнуто введением вычислительной техники, а также роботов-манипуляторов. Роботы-манипуляторы, чаще всего, обладаю высокой стоимостью по сравнению с классическими механизмами, однако из-за гибкости конфигураций способны совершать широкий круг всевозможных задач в условиях производства. В частности те, какие ранее был способен выполнять лишь человек. Необходимо понимать то, что способности каждого манипулятора ограничены. Он действует в конкретной области рабочего пространства. Ограничения по положению, ориентации, скорости, а также ускорениям обладают своими пределами. С целью того, чтобы выяснить допустимые границы работоспособности манипулятора рационально осуществить математическое моделирование робота и его работы. Одним из наиболее интересных видов применяемых манипуляторов на производстве являются механизмы с параллельной структурой, а именно с параллельной кинематикой. Манипуляторы параллельной кинематики – это механизм, исполнительное звено которого крепится к основанию несколькими независимыми кинематическими цепями. Этим устройствам характерны надежность и высокая точность срабатывания, а также возможность выдерживать немаленькие нагрузки. Аналогичные системы используются во всевозможных сферах: начиная с астрономии и заканчивая сортировкой продукции на предприятии. Однако существует одна проблема в таких системах, которая связана с высокой математической сложностью, возникающей из-за нелинейности системы, а именно 7

возможностью в режиме реального времени принимать во внимание большое количество ограничительных критериев. Частным случаем такого механизма является манипулятор типа «трипод». Он представляет собой параллельное кинематическое устройство, состоящее из основания, установленного на трех опорах, которые меняя свою длину, меняют положение и угол наклона. Кинематика и динамика таких роботов значительно отличается от кинематики и динамики обычных манипуляторов. Данное различие возникает из-за того, что любая степень подвижности параллельного механизма в отдельности является элементарной разомкнутой системой, при этом они соединены между собой и двигаются как единый механизм. Тут же и возникает сложность управления тягами такого робота, чтобы движение всех степеней подвижности было согласованным. Поэтому целью текущей работы стала разработка математической модели манипулятора типа «трипод», а именно расчет кинематики и проверка работоспособности при краевых условиях, расчет динамики и построение соответствующих характеристик для анализа работы «трипода», а так же для упрощения данного процесса разработка приложения с графич еским интерфейсом. 8

1 ОБЗОР И ОПИСАНИЕ КОНСТРУКЦИИ МАНИПУЛЯТОРА С ПАРАЛЛЕЛЬНОЙ КИНЕМАТИКОЙ 1.1. Обзор конструкций манипуляторов типа «трипод» Роботы-манипуляторы, используемые на сегодняшний день, имеют классическую схему, представляющую незамкнутую цепочку звеньев, поочередно соединённых с помощью всевозможных кинематических пар. Подобные конструкции обладают часто встречающимися недостатками - сравнительно небольшая степень грузоподъемности, значительные инерционные нагрузки, относительно повышенная металлоемкость. Отыскание решений, которые позволяют сократить отмеченные выше недочеты привели к возникновению механизмов с параллельной структурой. Как, верно, замечено в [6], достоинства подобных манипуляторов состоят в наилучшей грузоподъемности, высокой жесткости конструкции, достижении высоких скоростей, а также ускорений грузозахватного устройства, существенного уровня унификации узлов. Помимо этого, конструкция данных манипуляторов совмещает функции металлоконструкции механизма, что значительно сокращает итоговую массу манипулятора. Выходное звено подобных манипуляторов соединено с основанием несколькими кинематическими цепями, из которых любая оборудована приводом и налагает определенное число связей на перемещение выходного звена. В связи с этим, недостатками механизмов параллельной структуры являются ограничение зоны достижимости, сравнительно малая манипулятивность, а вдобавок затруднительность их управлением. Базовым манипулятором параллельной структуры считается механизм ГьюСтюарта (рис. 1.1) [7], который состоит из двух платформ, шарнирно соединенных 9

шестью поступательными кинематическими парами («гексапод»). Последующее исследование данного типа конструкции выявило возможность получения существенного количества комбинаций стержней, связывающих две платформы, при этом характерные черты каждой структуры подразумевают ее применение с определенной технологической целью. Рисунок 1.1 – Платформа Гью-Стюарта Одним из первых промышленных роботов обладающим параллельной кинематикой принято считать робот Полларда [8], который представляет пятистепенной манипулятор с тремя параллельными кинематическими цепями, определяющие положение схвата, а также двумя избыточными степенями подвижности для управления схватом. В нашей стране первым подобным манипулятором является манипулятор Данилевского [9], обладающий шестью степенями подвижности. Сегодняшние механизмы с параллельной кинематикой имеют всевозможные виды их исполнения, кроме того их функционал обусловливает их использование в различных отраслях промышленности, например, машиностроение, приборостроение, и одна из интересных отраслей космические исследования. 10

Нынешние проблемы машиностроения представляют собой создание эффективных многофункциональных механизмов, которые смогут обеспечить высокую точность, надежность и производительность. Сейчас на мировом рынке робототехнического станочного оборудования представлены механизмы современной концепции, так называемые «гексаподы», основанные на стержневой конструкции, обладающие шестью степенями свободы или подобные им «триподы», единственное отличие — это наличие всего трех степеней свободы. достоинством по Аналогичные сравнению с механизмы обладают классическими существенным многокоординатными манипуляторами и трехкоординатными измерительными машинами. Роботыманипуляторы с параллельной структурой обладают высокой жесткостью и точностью по отношению к манипуляторам классического исполнения. Типичным представителем робототехнических станков типа «гексапод» является разработка компании OKUMA (рис. 1.2) [10]. Основан на шести механизмах поступательного перемещения, которые обеспечивают полноценный доступ схвата к детали для дальнейшей обработки. Есть возможность использовать в качестве схвата свыше 20 всевозможных инструментов. Изменение длины штанг производится с помощью регулируемых электроприводов. Величину перемещения контролирует датчик положения. Концы штанг шарнирно соединены соответственно с основанием и подвижной платформой, на которой установлен рабочий инструмент. Одной из интересных особенностей является то, что в процессе обработки деталь остается совершенно неподвижной, а схват перемещается вокруг нее. С помощью этого станка возможно подвергать обработке различные отверстия и поверхности под любыми углами. 11

Рисунок 1.2 – Станок OKUMA серия PM Популярной разработкой манипулятора типа «трипод» является станок Tricept шведской фирмы Neos Robotics (рис. 1.3) [11]. Механизм представляет собой три симметрично установленных телескопических штанги, которые по средством шарниров связаны с неподвижным основанием и рабочим инструментом. У данного станка имеется три подвижные штанги, которые растягиваются – сжимаются с помощью двигателей, и одна центральная неподвижная штанга. Она обладает заметно большим размером относительно приводных штанг. Рисунок 1.3 – Станок Tricept Neos Robotics 12

Сортировка, упаковка разнообразных изделий считается трудоемким процессом промышленности. Одним из основных требований к данному действию – это быстрота упаковки предметов в различные контейнеры, лотки. По мимо этого, важной составляющей являются требования безопасности при упаковке, например, пищевых изделий, откуда возникает необходимость в поиске способов с наименьшим контактом человека и изделия. По этим причинам значительно вырос интерес к автоматизации процесса и использования промышленных роботов. Уникальный дельта-робот ABB «FlexPicker» (рис. 1.4) [12] является ярчайшим примером автоматического механизма, который специализируется на упаковке маленьких и легких изделий, например, конфеты. Манипулятор выполнен из прочных, а главное легких материалов, таких как углеродное волокно, пластик. Самая тяжелая часть – двигатели, которые находятся на неподвижной части конструкции. Способен обеспечить максимальную скорость до 10 м/с и максимальное ускорение до 150 м/с2, а точность позиционирования составляет ±0,1 мм при максимальной нагрузке 3 кг. Еще одним интересным представителем промышленной робототехники является дельта-робот компании FANUC серия М (рис. 1.5) [13]. Их называю так же «робот-паучок» за счет своих размеров и особенностей конструкции. Двигательные возможности данного манипулятора сравнимы с человеческой кистью и подходят для обширного спектра задач. При своих конструктивных особенностях обладает очень малым весом 26 кг при максимальной нагрузке 1 кг, точность позиционирования составляет ±0,03 мм, а область достижимости 420 мм и тем самым занимает маленькую площадь. 13

Рисунок 1.4 Робот-манипулятор Рисунок 1.5 Дельта-робот FANUC М FlexPicker ABB Одним из наиболее оснащенных манипуляторов с параллельной кинематикой является разработка немецкой компании Festo (рис. 1.6) [14]. Конструкция робота произведено по схеме «трипод», в которой применяются линейные приводы для перемещения захватного устройства. Представленный механизм имеет сравнительно маленький вес до 66 кг, при этом может обеспечить максимальную скорость до 7 м/с, а ускорение до 110 м/с2 при максимальной нагрузке 5 кг с точностью позиционирования ±0,5 мм 14

Рисунок 1.6 Робот-манипулятор Festo Выполненный анализ существующих манипуляторов параллельной структуры показал прекрасные функциональные качества и возможность выполнять довольно внушительный объем разнообразных производственных операций. Заинтересованность в создании робототехнических систем обусловлена, в первую очередь, высокими динамическими характеристиками, модульным построением. А из-за особенностей конструкции манипуляторов предполагается их работа в составе стационарного производственного оборудования. 1.2. Описание конструкции «трипода» EXPT компании Festo Построение математической модели производится на основе конструкции манипулятора типа «трипод» EXPT от компании Festo (рис. 1.7) [14], поэтому рассмотрим ее подробнее и произведем описание основных элементов. Промышленный манипулятор является одной из разновидностей дельта – роботов, в данном случае кинематический механизм имеет три пары параллельных тяг (рис. 1.7-), оси которых расположены не параллельно друг другу, тем самым не образуют декартово пространство координат. Рассматриваемый трипод обладает тремя степенями свободы пространственного перемещения и одной степенью свободы для ориентации по углу. 15 Высокую динамику около 150

циклов/мин обеспечивает тяги, выполненные из полимера из углеродного волокна, что делает их легкими и приводит к минимальной подвижной массе. Жесткость конструкции создается за счет закрепленных между собой линейных приводов (рис. 1.7-), которые составляют пирамидальную форму. Правильно выбранные, мощные двигатели (рис. 1.7-) помогают осуществить высокоскоростное и точное перемещение в трехмерном пространстве. Неподвижное треугольное основание (рис. 1.7-) обеспечивает устойчивость всей конструкции, наилучшее распределение усилий и уменьшение вибраций. Подвижная площадка (рис. 1.7-) является элементарным интерфейсом с возможностью присоединения различных видов вакуумных и механических схватов. Данный манипулятор обладает целиком конфигурируемым программным управлением, приспособленное для различных применений. Управление всей системой осуществляет стандартизированный шкаф СМСА, который позволяет максимально просто ввести «трипод» в эксплуатацию. Шкаф управления состоит из многокоординатного контроллера для осуществления перемещений и контроллеров, управляющих работой двигателей, также предусмотрены автоматические аварийные остановки для наибольшей безопасности работы. 16

           Основание для схвата  Пара параллельных тяг  Шарнир  Приводы с зубчатым ремнем  Зажим для подключения заземления  Каркас  Двигатель  Корпус интерфейса  Блок подключений  Защитный рукав Рисунок 1.7 Конструкция манипулятора типа «трипод» Festo Мозгом всей системы управления является контроллер CMXR-C2, с помощью которого легко можно запрограммировать траектории движения схвата. Так же в систему входит компактная система технического зрения SBOC-QR2B. С ее помощью можно наблюдать за перемещением объектов, анализировать их свойства и особенности, что позволяет с высокой точностью находить ненадлежащего качества объекты и определять их пространственные координаты. Далеко небесполезным является пульт оператора CDSA для ручного управления с возможностью онлайн программирования. Сферы использования как отмечено в [5], рассмотренного дельта-робота могут быть весьма обширными. Самыми стандартными примерами считается задача вертикальной паковки различных продуктов в коробку, трей или коррекс. Более интересными примерами использования в промышленных масштабах являются нанесение клея по контуру детали, итоговая стадия упаковки напитков, установка различных крышек. 17

2 КИНЕМАТИКА РОБОТА С ПАРАЛЛЕЛЬНОЙ СТРУКТУРОЙ 2.1 Анализ манипулятора и вывод описания кинематической модели Математическое описание рассматриваемого манипулятора невозможно без его геометрической схемы, которая представлена на рисунке 2.1. Рисунок 2.1 – Расчетная схема манипулятора типа «трипод» За счет конструктивных особенностей манипулятора, а точнее его тяги, соединяющие каретки с подвижной платформой расположены парами и параллельны относительно друг друга. Поэтому плоскость подвижной платформы всегда параллельна плоскости неподвижного основания. 18

Из-за того, что пары параллельных тяг соединяют между собой подвижное и неподвижное основания, плоскости, которые их описывают, всегда остаются параллельны друг другу. Тогда для простоты проведения последующих расчетов рассмотрим части построенной геометрической модели: А1 А3 = А3 А2 = А1 А2 = L – длины сторон неподвижного основания, которое представляет собой равносторонний треугольник; В1 С1 = В2 С2 = В3 С3 = 𝑙 – длина тяги; A1 B1 = S1 , A2 B2 = S2 , A3 B3 = 𝑆3 – расстояния, определяющие перемещение кареток по направляющим; 𝛾 – угол между неподвижным основанием и приводом; 𝛼𝑖 , 𝛽𝑖 – вспомогательные расчетные углы, где 𝑖 = ̅̅̅̅ 1,3; СС1 = СС2 = СС3 = 𝑎 – радиус подвижного основания; 𝑂x1 y1 z1 , Сxyz - координатные оси неподвижного и подвижного основания соответственно; С1 С2 С3 ∥ А1 А2 А3 – всегда, где С1 С2 С3 -описывает плоскость подвижной платформы, а А1 А2 А3 -плоскость неподвижного основания. Установив связь между координатами кареток и подвижной платформой, сможем отыскать решение задачи кинематики [1]. Введем две системы координат 𝑂x1 y1 z1 -для неподвижного основания, Сxyz-для подвижного основания, что следует из рисунка 2.1. 19

Вектора, описывающие неподвижное основание А1 А2 А3 , являются ̅̅̅̅̅̅̅ ̅̅̅̅̅̅̅ ̅̅̅̅̅̅̅ постоянными величинами А 1 А2 = 𝑐𝑜𝑛𝑠𝑡, А2 А3 = 𝑐𝑜𝑛𝑠𝑡, А1 А3 = 𝑐𝑜𝑛𝑠𝑡, при условии рассмотрения их относительно системы координат Сxyz. Тогда можем записать соотношения связывающие координаты данных векторов, для дальнейшего решения достаточно двух: 𝑥𝐴2 − 𝑥𝐴1 = 𝑐𝑜𝑛𝑠𝑡, 𝑥𝐴3 − 𝑥𝐴1 = 𝑐𝑜𝑛𝑠𝑡, (2.1) 𝑦𝐴2 − 𝑦𝐴1 = 𝑐𝑜𝑛𝑠𝑡, 𝑦𝐴3 − 𝑦𝐴1 = 𝑐𝑜𝑛𝑠𝑡. (2.2) 𝑂x1 y1 z1 и Сxyz параллельные системы координат, т.к. основания всегда остаются параллельны друг другу. А значит, координаты 𝑧𝐴𝑖 для неподвижной платформы относительно Сxyz одинаковые. Тогда получаем верные равенства: 𝑧𝐴1 = 𝑧𝐴2 , 𝑧𝐴2 = 𝑧𝐴3 . (2.3) Известно, что координаты точки О относительно системы координат Сxyz можно вычислить с помощью следующих соотношений: 𝑥𝑂 = 𝑦𝑂 = 𝑧𝑂 = 𝑥𝐴1 +𝑥𝐴2 +𝑥𝐴3 , 3 𝑦𝐴1 +𝑦𝐴2 +𝑦𝐴3 3 𝑧𝐴1 +𝑧𝐴2 +𝑧𝐴3 3 (2.4) , . Тогда координаты точки С относительно системы координат Ox1 y1 z1 могут быть описаны следующим образом: 𝑥С = − 𝑦С = − 𝑧С = − 20 𝑥𝐴1 +𝑥𝐴2 +𝑥𝐴3 , 3 𝑦𝐴1 +𝑦𝐴2 +𝑦𝐴3 3 𝑧𝐴1 +𝑧𝐴2 +𝑧𝐴3 3 . , (2.5)

Соединим полученные ранее соотношения (2.1 – 2.3, 2.5) и сведем все в систему из 9-ти уравнений связи (2.6). 𝑧𝐴1 = 𝑧𝐴2 , 𝑧𝐴2 = 𝑧𝐴3 , 𝑥𝐴2 − 𝑥𝐴1 = 𝑐𝑜𝑛𝑠𝑡, 𝑥𝐴3 − 𝑥𝐴1 = 𝑐𝑜𝑛𝑠𝑡, 𝑦𝐴2 − 𝑦𝐴1 = 𝑐𝑜𝑛𝑠𝑡, 𝑦𝐴3 − 𝑦𝐴1 = 𝑐𝑜𝑛𝑠𝑡, 𝑥С = − 𝑦С = − { 𝑧С = − 𝑥𝐴1 +𝑥𝐴2 +𝑥𝐴3 3 𝑦𝐴1 +𝑦𝐴2 +𝑦𝐴3 3 𝑧𝐴1 +𝑧𝐴2 +𝑧𝐴3 3 (2.6) , , . В полученной системе неизвестными являются координаты вершин А1 , А2 , А3 , выразив их сможем найти решение системы (2.6). Поиск выражений для каждой из координат будем делать геометрическим способом. Для этого изобразим геометрические модели всех вершин неподвижного основания последовательно. Геометрическая интерпретация вершины А1 представлена на рисунке 2.3. Координаты будем вычислять посредством сложения длин проекций, которые получаются, идя последовательно от точки C до точки А1 по звеньям. Из рисунка 2.3 видно, что: 𝑥𝐴1 = |𝐴1′′ 𝐵1′′ | + |𝐵1′′ 𝐶1 | + 𝑎, (2.7) 𝑦𝐴1 = |𝐴1′′′ 𝐶 |, (2.8) 𝑧𝐴1 = |𝐾𝐿| + |𝐿𝐶 |. (2.9) 21

Рисунок 2.3 – Геометрическое отображение вершины А1 Получим выражение для каждой проекции через известные величины: |𝐴1′′ 𝐵1′′ | = |𝐴1′ 𝐵1′ | = |𝐴1 𝐵1 | cos 𝛾 = 𝑆1 cos 𝛾, |𝐵1′′ 𝐶1 | = |𝐵1′ 𝐶 | cos 𝛽1 = |𝐵1 𝐶 | cos 𝛽1 cos 𝛼1 = 𝑙 cos 𝛽1 cos 𝛼1 , (2.10) (2.11) |𝐴1′′′ 𝐶 | = |𝐵1′′ 𝐵1′ | = |𝐵1′ 𝐶 | sin 𝛽1 = 𝑙 cos ∝1 sin 𝛽1 , (2.12) |𝐾𝐿| = |𝑀𝐵1′′ | = |𝑀′𝐵1 | = |𝐴1 𝐵1 | sin 𝛾 = 𝑆1 sin 𝛾, (2.13) |𝐿𝐶 | = |𝐴1′′′ 𝐵1′′′ | = |𝐵1′ 𝐵1 | = |𝐵1 𝐶1 | sin 𝛼1 = 𝑙 sin 𝛼1 , (2.14) 22

где 𝛼1 – угол между тягой и проекцией тяги на плоскость Сxy, 𝛽1 – угол между проекциями тяги и направляющей 𝐴1 𝐷 на плоскость Сxy. Для записи итоговых выражений для геометрического представления вершины 𝐴1 , подставим полученные соотношения (2.10 – 2.14) в (2.7 – 2.9): 𝑥𝐴1 = 𝑆1 cos 𝛾 + 𝑙 cos 𝛽1 cos 𝛼1 + 𝑎 , (2.15) 𝑦𝐴1 = 𝑙 cos ∝1 sin 𝛽1 , (2.16) 𝑧𝐴1 = 𝑆1 sin 𝛾 + 𝑙 sin 𝛼1 . (2.17) Геометрическая интерпретация вершины А2 представлена на рисунке 2.4. Вычислять координаты будем за счет сложения длин проекций, которые получаются, идя последовательно от точки C до точки А2 по звеньям. Из рисунка 2.4 видно, что: ′′ 𝑥𝐴2 = −|𝐴′′2 𝐵2′′ | − |𝐵2′′ С′′ 2 | − |𝐶С2 | , (2.18) ′′′ ′′′ ′′′ ′′′ 𝑦𝐴2 = |𝐴′′′ 2 𝐵2 | + |𝐵2 С2 | + |𝐶С2 |, (2.19) 𝑧𝐴2 = |𝑇𝐸 | + |𝐸𝐶 | . (2.20) Получим выражение для каждой проекции через известные величины: 𝜋 𝜋 𝜋 6 6 6 |𝐴′′2 𝐵2′′ | = |𝐵2′ 𝑊 | = |𝐴′2 𝐵2′ | sin = |𝐵2 𝑅| sin = 𝑆2 cos 𝛾 sin , 𝜋 𝜋 6 6 ′ ′ |𝐵2′′ С′′ 2 | = |𝐶2 𝐶2 | = |𝐶2 𝐵2 | sin( 𝛽2 + ) = 𝑙 sin( 𝛽2 + ) cos 𝛼2 , 𝜋 𝜋 6 6 |𝐶2 𝐶2′′ | = |𝐶𝐶2 | sin = 𝑎 sin , 𝜋 𝜋 6 6 (2.24) 𝜋 𝜋 6 6 |𝐶2′′′ 𝐵2′′′ | = |𝐶2′ 𝐵2′ | = |𝐶2 𝐵2′ | cos(𝛽2 + ) = 𝑙 cos 𝛼2 cos(𝛽2 + ), 𝜋 6 6 (2.22) (2.23) ′′′ ′ ′ ′ ′ |𝐴′′′ 2 𝐵2 | = |𝐵2 𝑃 | = |𝐴2 𝑊 | =|𝐴2 𝐵2 | cos = 𝑆2 cos cos 𝛾, 𝜋 (2.21) (2.25) |𝐶2 𝐶2′′′ | = |𝐶𝐶2 | cos = 𝑎 cos , (2.26) |𝑇𝐸 | = |𝑇′𝐸′| = |𝐴2 𝐵2 | sin 𝛾 = 𝑆2 sin 𝛾, (2.27) |𝐸𝐶 | = |𝐸′𝐶′| = |𝐵2′ 𝐵2 | = |𝐵2 𝐶2 | sin 𝛼2 = 𝑙 sin 𝛼2 , (2.28) 23

где 𝛼2 – угол между тягой и проекцией тяги на плоскость Сxy, 𝛽2 – угол между проекциями тяги и направляющей 𝐴2 𝐷 на плоскость Сxy. Рисунок 2.4 – Геометрическое отображение вершины А2 Для записи итоговых выражений для геометрического представления вершины 𝐴2 , подставим полученные соотношения (2.21 – 2.28) в (2.18 – 2.20): 24

𝜋 𝜋 𝜋 6 6 6 𝑥𝐴2 = −𝑆2 cos 𝛾 sin − 𝑙 sin ( 𝛽2 + ) cos 𝛼2 − 𝑎 sin , 𝜋 𝜋 𝜋 6 6 6 𝑦𝐴2 = 𝑆2 cos cos 𝛾 + 𝑙 cos 𝛼2 cos(𝛽2 + ) + 𝑎 cos , 𝑧𝐴2 = 𝑆2 sin 𝛾 + 𝑙 sin 𝛼2 . (2.29) (2.30) (2.31) Геометрическая интерпретация вершины А3 представлена на рисунке 2.5. Рисунок 2.5 – Геометрическое отображение вершины А3 Вычислять координаты будем за счет сложения длин проекций, которые получаются, идя последовательно от точки C до точки А3 по звеньям. 25

Из рисунка 2.5 видно, что: 𝑥𝐴3 = −|𝐴′′3 𝐵3′′ | − |𝐵3′′ С′3 | − |𝐶С′3 | , (2.32) ′′ ′′′ ′′ ′′′ 𝑦𝐴3 = −|𝐴′′′ 3 𝐶3 | − |𝐵3 С3 | − |𝐶𝐵3 |, (2.33) 𝑧𝐴3 = |𝐹′𝐹′′| + |𝐹′𝐶 | . (2.34) Получим выражение для каждой проекции через известные величины: 𝜋 𝜋 𝜋 6 6 6 |𝐴′′3 𝐵3′′ | = |𝐵3′ 𝑃| = |𝐴′3 𝐵3′ | sin = |𝐵3 𝑈| sin = 𝑆3 cos 𝛾 sin , 𝜋 𝜋 6 6 |𝐵3′′ С′3 | = |𝐶3 𝐵3′ | sin( 𝛽3 − ) = 𝑙 sin( 𝛽3 − ) cos 𝛼3 , 𝜋 𝜋 6 6 (2.36) |𝐶С′3 | = |𝐶𝐶3 | sin = 𝑎 sin , (2.37) 𝜋 𝜋 𝜋 6 6 6 ′′′ ′ ′ |𝐴′′′ 3 𝐵3 | = |𝐴3 𝐵3 | cos = |𝐵3 𝑈 | cos = 𝑆3 cos cos 𝛾, 𝜋 𝜋 6 6 (2.38) |𝐶3′′ 𝐵3′′′ | = |𝐶3 𝐵3′ | cos(𝛽3 − ) = 𝑙 cos 𝛼3 cos(𝛽3 − ), 𝜋 𝜋 6 6 (2.35) (2.39) |𝐶3 𝐶3′′ | = |𝐶𝐶3 | cos = 𝑎 cos , (2.40) |𝐹′𝐹′′| = |𝐸′′𝐸′| = |𝐴3 𝐵3 | sin 𝛾 = 𝑆3 sin 𝛾, (2.41) |𝐹′𝐶 | = |𝐸′𝐸 | = |𝐵3′ 𝐵3 | = |𝐵3 𝐶3 | sin 𝛼3 = 𝑙 sin 𝛼3 , (2.42) где 𝛼3 – угол между тягой и проекцией тяги на плоскость Сxy, 𝛽3 – угол между проекциями тяги и пневмоцилиндра 𝐴3 𝐷 на плоскость Сxy. Для записи итоговых выражений для геометрического представления вершины 𝐴3 , подставим полученные соотношения (2.35 – 2.42) в (2.32 – 2.34): 𝜋 𝜋 𝜋 6 6 6 𝑥𝐴3 = −𝑆3 cos 𝛾 sin − 𝑙 sin ( 𝛽3 − ) cos 𝛼3 − 𝑎 sin , 𝜋 𝜋 𝜋 6 6 6 𝑦𝐴3 = −𝑆3 cos cos 𝛾 − 𝑙 cos 𝛼3 cos (𝛽3 − ) − 𝑎 cos , 𝑧𝐴3 = 𝑆3 sin 𝛾 + 𝑙 sin 𝛼3 . (2.43) (2.44) (2.45) Теперь, когда найдены все неизвестные ранее координаты вершин неподвижного основания 𝐴1 𝐴2 𝐴3 , можем записать итоговые выражения для координат центра схвата, т.е. координаты точки C. Для этого подставим 26

соотношения (2.15 – 2.17), (2.29 – 2.31), (2.43 – 2.45) в полученное выше уравнение (2.5): 1 𝜋 𝜋 3 6 6 𝑥С = − [𝑆1 cos 𝛾 + 𝑙 cos 𝛽1 cos 𝛼1 + 𝑎 − 𝑆2 cos 𝛾 sin − 𝑙 sin ( 𝛽2 + ) cos 𝛼2 − 𝜋 𝜋 𝜋 𝜋 6 6 6 6 (2.46) 𝑎 sin − 𝑆3 cos 𝛾 sin − 𝑙 sin ( 𝛽3 − ) cos 𝛼3 − 𝑎 sin ], 1 𝜋 𝜋 𝜋 3 6 6 6 𝑦С = − [𝑙 cos ∝1 sin 𝛽1 + 𝑆2 cos cos 𝛾 + 𝑙 cos 𝛼2 cos(𝛽2 + ) + 𝑎 cos − 𝜋 𝜋 𝜋 6 6 6 (2.47) 𝑆3 cos cos 𝛾 − 𝑙 cos 𝛼3 cos (𝛽3 − ) − 𝑎 cos ], 1 𝑧С = − [𝑆1 sin 𝛾 + 𝑙 sin 𝛼1 +𝑆2 sin 𝛾 + 𝑙 sin 𝛼2 + 𝑆3 sin 𝛾 + 𝑙 sin 𝛼3 ]. 3 (2.48) Вернемся к исходной системе (2.6), которая описывает полностью конструкцию нашего «трипода» и подставим в нее все найденные соотношения, после чего она будет выглядеть следующим образом: 𝑆1 sin 𝛾 + 𝑙 sin 𝛼1 = 𝑆2 sin 𝛾 + 𝑙 sin 𝛼2 , 𝑆2 sin 𝛾 + 𝑙 sin 𝛼2 = 𝑆3 sin 𝛾 + 𝑙 sin 𝛼3 , 𝜋 𝜋 𝜋 −𝑆2 cos 𝛾 sin − 𝑙 cos 𝛼2 sin ( 𝛽2 + ) − 𝑎 sin 6 6 6 −(𝑆1 cos 𝛾 + 𝑙 cos 𝛽1 cos 𝛼1 + 𝑎) = 𝑐𝑜𝑛𝑠𝑡, 𝜋 𝜋 𝜋 −𝑆3 cos 𝛾 sin − 𝑙 sin ( 𝛽3 − ) cos 𝛼3 − 𝑎 sin − 6 6 6 −(𝑆1 cos 𝛾 + 𝑙 cos 𝛽1 cos 𝛼1 + 𝑎) = 𝑐𝑜𝑛𝑠𝑡, 𝜋 𝜋 𝜋 𝑆2 cos cos 𝛾 + 𝑙 cos 𝛼2 cos (𝛽2 + ) + 𝑎 cos − 𝑙 cos ∝1 sin 𝛽1 = 𝑐𝑜𝑛𝑠𝑡, 6 6 𝜋 𝜋 6 (2.49) 𝜋 −𝑆3 cos cos 𝛾 − 𝑙 cos 𝛼3 cos (𝛽3 − ) − 𝑎 cos − 𝑙 cos ∝1 sin 𝛽1 = 𝑐𝑜𝑛𝑠𝑡, 6 1 6 6 𝜋 𝜋 𝑥С = − [𝑆1 cos 𝛾 + 𝑙 cos 𝛽1 cos 𝛼1 + 𝑎 − 𝑆2 cos 𝛾 sin − 𝑙 cos 𝛼2 sin ( 𝛽2 + ) − 3 𝜋 𝜋 𝜋 6 6 𝜋 6 6 6 𝜋 −𝑎 sin − 𝑆3 cos 𝛾 sin − 𝑙 sin ( 𝛽3 − ) cos 𝛼3 − 𝑎 sin ], 1 6 𝜋 𝜋 𝜋 6 𝜋 6 6 6 𝑦С = − [𝑙 cos ∝1 sin 𝛽1 + 𝑆2 cos cos 𝛾 + 𝑙 cos 𝛼2 cos (𝛽2 + ) + 𝑎 cos − 3 𝜋 6 −𝑆3 cos cos 𝛾 − 𝑙 cos 𝛼3 cos (𝛽3 − ) − 𝑎 cos ], 6 1 { 𝑧С = − 3 [𝑆1 sin 𝛾 + 𝑙 sin 𝛼1 +𝑆2 sin 𝛾 + 𝑙 sin 𝛼2 + 𝑆3 sin 𝛾 + 𝑙 sin 𝛼3 ]. 27

В полученной системе (2.49) остались неизвестные константы, которые легко можно найти, изобразив геометрически неподвижную платформу А1 А2 А3 «трипода» (рис. 2.6). Рисунок 2.6 – Геометрическое представление основания А1 А2 А3 Вокруг основания можно описать окружность радиуса R, тем самым сможем найти неизвестные константы. Спроецируем каждый из радиусов, которые соединяют центр описанной окружности и вершины равностороннего треугольника А1 А2 А3 на координатную ось Ox1 y1 . Судя по рисунку 2.6, можем записать следующие соотношения: 𝑥𝐴1 = |𝐴1 𝑂| , 𝑥𝐴2 = 𝑥𝐴3 = −|𝑂𝑂′|, 𝑦𝐴1 = 0, 𝑦𝐴2 = |𝐴2 𝑂′|, 𝑦𝐴3 = −|𝐴3 𝑂′|. 28

Получим: 𝜋 𝑥𝐴2 − 𝑥𝐴1 = 𝑥𝐴3 − 𝑥𝐴1 = −|𝑂𝑂′ | − |𝐴1 𝑂| = −|𝐴1 𝑂′ | = −𝐿 cos , 6 𝜋 (2.50) 𝑦𝐴2 − 𝑦𝐴1 = |𝐴2 𝑂′ | = 𝐿 sin , (2.51) 𝜋 (2.52) 6 𝑦𝐴3 − 𝑦𝐴1 = −|𝐴3 𝑂′|=−𝐿 sin . 6 Найденные выражения для констант (2.50 – 2.52) подставим в систему уравнений (2.49), тем самым получим полное геометрическое описание конструкции манипулятора. Система 2.53: 𝑆1 sin 𝛾 + 𝑙 sin 𝛼1 = 𝑆2 sin 𝛾 + 𝑙 sin 𝛼2 , 𝑆2 sin 𝛾 + 𝑙 sin 𝛼2 = 𝑆3 sin 𝛾 + 𝑙 sin 𝛼3 , 𝜋 𝜋 𝜋 −𝑆2 cos 𝛾 sin − 𝑙 cos 𝛼2 sin ( 𝛽2 + ) − 𝑎 sin − 6 6 6 𝜋 −(𝑆1 cos 𝛾 + 𝑙 cos 𝛽1 cos 𝛼1 + 𝑎) = −𝐿 cos , 6 𝜋 𝜋 𝜋 −𝑆3 cos 𝛾 sin − 𝑙 sin ( 𝛽3 − ) cos 𝛼3 − 𝑎 sin − 6 6 6 𝜋 −(𝑆1 cos 𝛾 + 𝑙 cos 𝛽1 cos 𝛼1 + 𝑎) = −𝐿 cos , 6 𝜋 𝜋 𝜋 𝜋 𝑆2 cos cos 𝛾 + 𝑙 cos 𝛼2 cos (𝛽2 + ) + 𝑎 cos − 𝑙 cos ∝1 sin 𝛽1 = 𝐿 sin , 6 6 6 6 𝜋 𝜋 𝜋 𝜋 −𝑆3 cos cos 𝛾 − 𝑙 cos 𝛼3 cos (𝛽3 − ) − 𝑎 cos − 𝑙 cos ∝1 sin 𝛽1 = −𝐿 sin , 6 6 6 6 1 𝜋 𝜋 𝑥С = − [𝑆1 cos 𝛾 + 𝑙 cos 𝛽1 cos 𝛼1 + 𝑎 − 𝑆2 cos 𝛾 sin − 𝑙 cos 𝛼2 sin ( 𝛽2 + ) − 3 6 6 𝜋 𝜋 𝜋 𝜋 −𝑎 sin − 𝑆3 cos 𝛾 sin − 𝑙 sin ( 𝛽3 − ) cos 𝛼3 − 𝑎 sin ], 6 6 6 6 1 𝜋 𝜋 𝜋 𝑦С = − [𝑙 cos ∝1 sin 𝛽1 + 𝑆2 cos cos 𝛾 + 𝑙 cos 𝛼2 cos (𝛽2 + ) + 𝑎 cos − 3 6 6 6 𝜋 𝜋 𝜋 −𝑆3 cos cos 𝛾 − 𝑙 cos 𝛼3 cos (𝛽3 − ) − 𝑎 cos ], 6 6 6 1 [𝑆 sin 𝛾 + 𝑙 sin 𝛼1 +𝑆2 sin 𝛾 + 𝑙 sin 𝛼2 + 𝑆3 sin 𝛾 + 𝑙 sin 𝛼3 ]. 𝑧 = − С { 3 1 29

В итоге получили систему, которая состоит из 9-ти уравнений и 12-ти неизвестных, а точнее 𝛼1 , 𝛼2 , 𝛼3 , 𝛽1 , 𝛽2 , 𝛽3 , 𝑆1 , 𝑆2 , 𝑆3 , 𝑥С , 𝑦С , 𝑧С и является нелинейной. Если задаться численными значениями 𝑆1 , 𝑆2 , 𝑆3 , то сможем найти решение прямой задачи кинематики. Если же задаться численными значениями 𝑥С , 𝑦С , 𝑧С , найдем решение обратной задачи кинематики «трипода». 2.2 Поиск решения прямой и обратной задачи кинематики Как правило решение прямой задачи кинематики единственное и его поиск не представляет больших вычислительных трудностей. Интереснее дела обстоят с поиском решения обратной задачи кинематики, где по известным координатам положения захвата необходимо вычислить положение кареток привода. Для единственного положения захвата может существовать бесконечно много начальных положений звеньев. Для поиска решения наших задач воспользуемся итерационным методом Ньютона [15]. Метод Ньютона позволяет найти решения нелинейной системы уравнений (ситуация, когда количество неизвестных больше количества уравнений) с заданной точностью. Он (метод Ньютона) прост в применение и для дальнейшего программирования. Такой подход позволяет добиться весьма плавных движений характеристической рабочей точки, не допуская разрывов траектории движения. В общем виде система нелинейных уравнений выглядит так: 𝑓1 (𝑥1 , 𝑥2 , … , 𝑥𝑛 ) = 0, 𝑓 (𝑥 , 𝑥 , … , 𝑥𝑛 ) = 0, { 2 1 2 … 𝑓𝑛 (𝑥1 , 𝑥2 , … , 𝑥𝑛 ) = 0. (2.54) Перепишем ее в векторной форме: 𝑓(𝑥 ) = 0, 30 (2.55)

где 𝑓 = 𝑓1 ( 𝑓…2 ), 𝑓𝑛 𝑥= 𝑥1 (𝑥…2 ). 𝑥𝑛 (2.56) Воспользуемся одним из существующих итерационным методом, чтобы найти решение системы (2.54). (𝑘) (𝑘) (𝑘) Пусть, известна k-ая точка приближения 𝑥 (𝑘) = (𝑥1 , 𝑥2 , … , 𝑥𝑛 ) к одному из изолированных корней 𝑥 = (𝑥1 , 𝑥2 , … , 𝑥𝑛 ) векторного уравнения (2.55). Тогда точный корень уравнения (2.54) можно записать следующим образом: 𝑥 = 𝑥 (𝑘) + ∆𝑥 (𝑘) , (𝑘) (𝑘) (2.57) (𝑘) где ∆𝑥 (𝑘) = (∆𝑥1 , ∆𝑥2 , … , ∆𝑥𝑛 ) – погрешность приближенного корня. При подстановке соотношения (2.57) в векторное уравнение (2.55), получим: 𝑓(𝑥 (𝑘) + ∆𝑥 (𝑘) ) = 0. (2.58) Пусть функция 𝑓 (𝑥 ) непрерывно дифференцируема в некоторой выпуклой области, которая содержит 𝑥 и 𝑥 (𝑘) , разложим левую часть уравнения (2.58) по степеням малого вектора ∆𝑥 (𝑘) , ограничившись только линейными элементами: 𝑓(𝑥 (𝑘) + ∆𝑥 (𝑘) ) = 𝑓(𝑥 (𝑘) ) + 𝑓′(𝑥 (𝑘) )∆𝑥 (𝑘) = 0. (2.59) Перейдем от векторной записи к подробной, тогда выражение (2.59) будет выглядеть так: 31

(𝑘) (𝑘) (𝑘) (𝑘) (𝑘) (𝑘) 𝑓1 (𝑥1 + ∆𝑥1 , … , 𝑥𝑛 + ∆𝑥𝑛 ) = 𝑓1 (𝑥1 , … , 𝑥𝑛 ) + (𝑘) 𝜕𝑓1 𝜕𝑥1 (𝑘) 𝜕𝑓 1 + ⋯ + ∆𝑥𝑛 = 0, 𝜕𝑥𝑛 … (𝑘) (𝑘) (𝑘) (𝑘) (𝑘) (𝑘) 𝑓𝑛 (𝑥1 + ∆𝑥1 , … , 𝑥𝑛 + ∆𝑥𝑛 ) = 𝑓𝑛 (𝑥1 , … , 𝑥𝑛 ) + +∆𝑥1 (𝑘) 𝜕𝑓𝑛 𝜕𝑥1 +∆𝑥1 { (𝑘) 𝜕𝑓𝑛 𝜕𝑥𝑛 + ⋯ + ∆𝑥𝑛 (2.60) = 0. Так как в данной работе мы рассматриваем систему уравнений, то под производной 𝑓′(𝑥 )следует понимать матрицу Якоби системы функций 𝑓1 , 𝑓2 , … , 𝑓𝑛 относительно переменных 𝑥1 , 𝑥2 , … , 𝑥𝑛 , т.е.: 𝜕𝑓1 𝜕𝑓1 𝑓 ′(𝑥) = 𝐽(𝑥 ) = 𝜕𝑥1 𝜕𝑥2 𝜕𝑓2 𝜕𝑓2 … 𝜕𝑓1 𝜕𝑥𝑛 𝜕𝑓2 … 𝜕𝑥𝑛 . … … … … 𝜕𝑓𝑛 𝜕𝑓𝑛 𝜕𝑓𝑛 … [𝜕𝑥1 𝜕𝑥2 𝜕𝑥𝑛] 𝜕𝑥1 𝜕𝑥2 (2.61) Или в упрощенной форме записи: 𝑓 ′(𝑥) = 𝐽(𝑥 ) = [ 𝜕𝑓𝑖 𝜕𝑥𝑗 ], (2.62) где 𝑖, 𝑗 = 1, 2, … , 𝑛. Перепишем итоговое итерационное выражение (2.58) с учетом всех полученных соотношений: 𝑓(𝑥 (𝑘) ) + 𝐽(𝑥 (𝑘) )∆𝑥 (𝑘) = 0. Если det 𝐽(𝑥 ) = det [ 𝜕𝑓𝑖 𝜕𝑥𝑗 (2.63) ] ≠ 0, то ∆𝑥 (𝑘) = −𝐽−1 (𝑥 (𝑘) )𝑓(𝑥 (𝑘) ) = 0. Можем утверждать, что поиск решения системы (2.54) по средством метода Ньютона [15] основан на построение итерационной последовательности: 32

𝑥 (𝑘+1) = 𝑥 (𝑘) − 𝐽−1 (𝑥 (𝑘) )𝑓(𝑥 (𝑘) ) = 0, (2.64) где 𝑘 = 0, 1, 2, … , 𝑛 Вычисление останавливается, когда все погрешности приближенных корней становятся достаточно малыми, т.е. ∆𝑥 (𝑘) < 𝜀, где 𝜀 – необходимая точность решения. Если точность не достигнута, то новые значения 𝑥𝑖 используются как приближенные значения корней, и итерационный процесс повторяется до тех пор, пока не найдено решение или не станет ясно, что получить его нельзя. Упростим систему уравнений связи для простоты составления матрицы Якоби: 33

𝑆1 sin 𝛾 + 𝑙 sin 𝛼1 = +𝑆2 sin 𝛾 + 𝑙 sin 𝛼2 , 𝑆2 sin 𝛾 + 𝑙 sin 𝛼2 = 𝑆3 sin 𝛾 + 𝑙 sin 𝛼3 , −𝑆2 cos 𝛾 2 − 𝑙 cos 𝛼2 cos 𝛽2 2 − √3𝑙 cos 𝛼2 sin 𝛽2 2 𝑎 − − 2 √3𝐿 −𝑆1 cos 𝛾 − 𝑙 cos 𝛽1 cos 𝛼1 − 𝑎 = − , 2 cos 𝛾 √3𝑙 cos 𝛼3 sin 𝛽3 𝑙 cos 𝛼3 cos 𝛽3 𝑎 −𝑆3 − + − 2 2 2 2 − √3𝐿 , 2 𝑙 cos 𝛼2 sin 𝛽2 √3𝑎 + 2 2 −𝑆1 cos 𝛾 − 𝑙 cos 𝛽1 cos 𝛼1 − 𝑎 = − 𝑆2 √3 cos 𝛾 2 −𝑙 cos −𝑆3 √3𝑙 cos 𝛼2 cos 𝛽2 2 𝐿 ∝1 sin 𝛽1 = , 2 + √3 cos 𝛾 2 −𝑙 cos − √3𝑙 cos 𝛼3 cos 𝛽3 2 𝐿 ∝1 sin 𝛽1 = − , 2 − − 𝑙 cos 𝛼3 sin 𝛽3 2 1 𝑥С + [𝑆1 cos 𝛾 + 𝑙 cos 𝛽1 cos 𝛼1 + 𝑎 − 𝑆2 3 √3𝑙 cos 𝛼2 sin 𝛽2 2 𝑙 cos 𝛼3 cos 𝛽3 + 𝑎 cos 𝛾 2 𝑎 2 − − 𝑆3 2 3 𝑙 cos 𝛼2 cos 𝛽2 −𝑆3 2 cos 𝛾 2 √3𝑙 cos 𝛼3 𝑠𝑖𝑛 𝛽3 2 − − (2.65) 𝑙 cos 𝛼2 cos 𝛽2 2 − + 2 𝑦С + [𝑙 cos ∝1 sin 𝛽1 + 𝑆2 2 √3 cos 𝛾 √3𝑎 2 − ]=0, 1 − − − − − √3 cos 𝛾 2 √3𝑎 − 2 √3𝑙 cos 𝛼3 𝑐𝑜𝑠 𝛽3 + √3𝑙 cos 𝛼2 cos 𝛽2 2 − + 2 − 𝑙 cos 𝛼3 sin 𝛽3 2 − √3𝑎 ] 2 =0, 1 { 𝑧С + 3 [𝑆1 sin 𝛾 + 𝑙 sin 𝛼1 +𝑆2 sin 𝛾 + 𝑙 sin 𝛼2 + 𝑆3 sin 𝛾 + 𝑙 sin 𝛼3 ] = 0. Для простоты и наглядности произведем замену неизвестных переменных: 𝑥1 = sin 𝛼1 , 𝑥2 = sin 𝛼2 , 𝑥3 = sin 𝛼3 , 𝑥4 = sin 𝛽1 , 𝑥5 = sin 𝛽2 , 𝑥6 = sin 𝛽3 , 𝑦1 = cos 𝛼1 , 𝑦2 = cos 𝛼2 , 𝑦3 = cos 𝛼3 , 𝑦4 = cos 𝛽1 , 𝑦5 = cos 𝛽2 , 𝑦6 = cos 𝛽3 , 𝑥7 = 𝑆1 , 𝑥8 = 𝑆2 , 𝑥9 = 𝑆3 𝑦7 = 𝑥С , 𝑦8 = 𝑦С , 𝑦9 = 𝑧С . Перепишем исходную систему уравнений (2.65) с учетом введенных неизвестных и добавим в нее еще 6 уравнений, которые являются по своей сути тригонометрическим тождеством: 34

𝑥7 sin 𝛾 𝑙 𝑥8 sin 𝛾 𝑙 −𝑥8 −𝑥9 + 𝑥1 − + 𝑥2 − cos 𝛾 2 cos 𝛾 − + 𝑥8 sin 𝛾 𝑙 𝑥8 sin 𝛾 𝑙 𝑙𝑦2 𝑦5 2 𝑙𝑦3 𝑦6 − − 𝑥2 = 0, √3𝑙𝑦2 𝑥5 2 3𝑙𝑦 √ 3 𝑥6 − 2 √3𝑙𝑦2 𝑦5 2 𝑙𝑦2 𝑥5 − 3𝑎 2 3𝑎 − 𝑥7 cos 𝛾 − 𝑙𝑦1 𝑦4 + √3𝐿 2 √3𝐿 − 𝑥7 cos 𝛾 − 𝑙𝑦1 𝑦4 + 2 2 𝐿 √3𝑎 𝑥8 + − + − 𝑙𝑦1 𝑥4 − = 0, 2 2 2 2 2 𝑙𝑦3 𝑥6 𝐿 √3 cos 𝛾 √3𝑙𝑦3 𝑥6 √3𝑎 −𝑆3 − − − − 𝑙𝑦1 𝑥4 + = 0, 2 2 2 2 2 1 cos 𝛾 𝑙𝑦2 𝑦5 𝑦7 + [𝑥7 cos 𝛾 + 𝑙𝑦4 𝑦1 + 𝑎 − 𝑥8 − − 3 2 2 2 √3 cos 𝛾 − − 𝑥2 = 0, = 0, = 0, 𝑎 cos 𝛾 𝑙𝑦 𝑦 𝑎 √3𝑙𝑦2 𝑥5 √3𝑙𝑦3 𝑥6 − − 𝑥9 − + 3 6 − ] = 0, 2 2 2 2 2 2 1 𝑙𝑦2 𝑦5 √3 cos 𝛾 √3𝑙𝑦2 𝑦5 √3𝑎 𝑦8 + [𝑙𝑦1 𝑥4 + 𝑥8 + − + − 3 2 2 2 2 −𝑥9 √3 cos 𝛾 2 − √3𝑙𝑦3 𝑦6 2 − 𝑙𝑦3 𝑥6 2 − √3𝑎 ] 2 (2.66) = 0, 1 𝑦9 + [𝑥7 sin 𝛾 + 𝑙𝑥1 +𝑥8 sin 𝛾 + 𝑙𝑥2 + 𝑥9 sin 𝛾 + 𝑙𝑥3 ] = 0, 3 2 𝑥1 + 𝑦12 − 1 = 0, 𝑥22 + 𝑦22 − 1 = 0, 𝑥32 + 𝑦32 − 1 = 0, 𝑥42 + 𝑦42 − 1 = 0, 𝑥52 + 𝑦52 − 1 = 0, { 𝑥62 + 𝑦62 − 1 = 0. Составим матрицу Якоби, которая необходима для поиска решения исходной нелинейной системы уравнений. Найдем частные производные каждой функции по каждой неизвестной. 𝜕𝑓1 𝜕𝑓1 𝜕𝑓1 sin 𝛾 𝜕𝑓1 sin 𝛾 = 1, = −1, = , =− ; 𝜕𝑥1 𝜕𝑥2 𝜕𝑥7 𝑙 𝜕𝑥8 𝑙 𝜕𝑓2 𝜕𝑓2 𝜕𝑓2 sin 𝛾 𝜕𝑓2 sin 𝛾 = 1, = −1, = , =− ; 𝜕𝑥2 𝜕𝑥3 𝜕𝑥8 𝑙 𝜕𝑥9 𝑙 𝜕𝑓3 𝜕𝑓3 cos 𝛾 𝜕𝑓3 𝜕𝑓3 𝑙𝑦5 √3𝑙𝑥5 √3𝑙𝑦2 𝜕𝑓3 =− , = − cos 𝛾 , =− , = −𝑙𝑦4 , =− − , 𝜕𝑥5 2 𝜕𝑥7 𝜕𝑥8 2 𝜕𝑦1 𝜕𝑦2 2 2 𝜕𝑓3 𝜕𝑓3 𝑙𝑦2 = −𝑙𝑦1 , =− ; 𝜕𝑦4 𝜕𝑦5 2 35

𝜕𝑓4 𝜕𝑓4 cos 𝛾 𝜕𝑓4 𝜕𝑓4 𝑙𝑦6 √3𝑙𝑥6 √3𝑙𝑥6 𝜕𝑓4 =− , = − cos 𝛾 , =− , = −𝑙𝑦4 , = − , 𝜕𝑥6 2 𝜕𝑥7 𝜕𝑥9 2 𝜕𝑦1 𝜕𝑦3 2 2 𝜕𝑓4 𝜕𝑓4 𝑙𝑦3 = −𝑙𝑦1 , = ; 𝜕𝑦4 𝜕𝑦6 2 𝜕𝑓5 𝜕𝑓5 𝑙𝑦2 𝜕𝑓5 √3 cos 𝛾 𝜕𝑓5 𝜕𝑓5 √3𝑙𝑦5 𝑙𝑥5 = −𝑙𝑦1 , =− , = , = −𝑙𝑥4 , = − , 𝜕𝑥4 𝜕𝑥5 2 𝜕𝑥8 2 𝜕𝑦1 𝜕𝑦2 2 2 𝜕𝑓5 √3𝑙𝑦2 = ; 𝜕𝑦5 2 𝜕𝑓6 𝜕𝑓6 𝑙𝑦3 𝜕𝑓6 𝜕𝑓6 √3 cos 𝛾 𝜕𝑓6 √3𝑙𝑦6 𝑙𝑥6 = −𝑙𝑦1 , =− , =− , = −𝑙𝑥4 , =− − , 𝜕𝑥4 𝜕𝑥6 2 𝜕𝑥9 2 𝜕𝑦1 𝜕𝑦3 2 2 𝜕𝑓6 √3𝑙𝑦3 =− ; 𝜕𝑦6 2 𝜕𝑓7 cos 𝛾 𝜕𝑓7 cos 𝛾 √3𝑙𝑦2 𝜕𝑓7 √3𝑙𝑦3 𝜕𝑓7 cos 𝛾 𝜕𝑓7 =− , =− , = , =− , =− , 𝜕𝑥5 6 𝜕𝑥6 2 𝜕𝑥7 3 𝜕𝑥8 6 𝜕𝑥9 6 𝜕𝑓7 𝑙𝑦4 𝜕𝑓7 𝑙𝑦5 √3𝑙𝑥5 𝜕𝑓7 𝑙𝑦6 √3𝑙𝑥6 𝜕𝑓7 𝑙𝑦1 𝜕𝑓7 𝑙𝑦2 = , =− − , = − , = , =− , 𝜕𝑦1 3 𝜕𝑦2 6 6 𝜕𝑦3 6 6 𝜕𝑦4 3 𝜕𝑦5 6 𝜕𝑓7 𝑙𝑦3 = ; 𝜕𝑦6 6 𝜕𝑓8 𝑙𝑦1 𝜕𝑓8 𝑙𝑦2 𝜕𝑓8 𝑙𝑦3 𝜕𝑓8 √3 cos 𝛾 𝜕𝑓8 √3 cos 𝛾 = , = , =− , = , =− , 𝜕𝑥4 3 𝜕𝑥5 6 𝜕𝑥6 6 𝜕𝑥8 6 𝜕𝑥9 6 𝜕𝑓8 𝑙𝑥4 𝜕𝑓8 √3𝑙𝑦5 𝑙𝑥5 𝜕𝑓8 √3𝑙𝑦6 𝑙𝑥6 𝜕𝑓8 √3𝑙𝑦2 = , = − , =− − , = , 𝜕𝑦1 3 𝜕𝑦2 6 6 𝜕𝑦3 6 6 𝜕𝑦5 6 𝜕𝑓8 √3𝑙𝑦3 =− ; 𝜕𝑦6 6 𝜕𝑓9 𝑙 𝜕𝑓9 𝑙 𝜕𝑓9 𝑙 𝜕𝑓9 sin 𝛾 𝜕𝑓9 sin 𝛾 𝜕𝑓9 sin 𝛾 = , = , = , = , = , = ; 𝜕𝑥1 3 𝜕𝑥2 3 𝜕𝑥3 3 𝜕𝑥7 3 𝜕𝑥8 3 𝜕𝑥9 3 𝜕𝑓10 𝜕𝑓10 = 2𝑥1 , = 2𝑦1 ; 𝜕𝑥1 𝜕𝑦1 𝜕𝑓11 𝜕𝑓11 = 2𝑥2 , = 2𝑦2 ; 𝜕𝑥2 𝜕𝑦2 𝜕𝑓12 𝜕𝑓12 = 2𝑥3 , = 2𝑦3 ; 𝜕𝑥3 𝜕𝑦3 36

𝜕𝑓13 𝜕𝑓13 = 2𝑥4 , = 2𝑦4 ; 𝜕𝑥4 𝜕𝑦4 𝜕𝑓14 𝜕𝑓14 = 2𝑥5 , = 2𝑦5 ; 𝜕𝑥5 𝜕𝑦5 𝜕𝑓15 𝜕𝑓15 = 2𝑥6 , = 2𝑦6 . 𝜕𝑥6 𝜕𝑦6 Полученные частные производные составляют матрицу Якоби. Решения прямой и обратной задач кинематики были получены с помощью разработанного приложения в среде MATLAB 6 [4]. Подробное описание о работе разработанного приложения с графическим интерфейсом представлено в 4 главе. В представленной работе прямая задача кинематики состоит в нахождении координат захвата при заданных координатах кареток приводов, обратная в точности, наоборот. Алгоритмы программ для поиска решений обратной и прямой задач кинематики практически не имеют структурных отличий. Единственной разницей является задание начальных условий, оно (начальное условие) различно для двух решаемых задач. Код программы для решения обратной задачи кинематики указан в приложении 2. Код программы для решения прямой задачи кинематики указан в приложении 3. Неотъемлемой задачей при анализе кинематики манипулятора является формирование области обслуживания, которая представляет собой множество точек, полученных при пространственных перемещениях захвата «трипода». Основываясь на систему уравнений связи, полученную ранее и разработав функционал в приложении, который позволяет вычислить координаты всех точек, с учетом конструктивных ограничений. 37

Итогом выполнения данной программы является точечное графическое отображение области достижимости в пространстве, как представлено на рисунке 2.7. Рисунок 2.7 – График зоны достижимости «трипода» На рисунке 2.8 изображена область достижимости в различных плоскостях. Отметим следующие особенности, которые явно просматриваются на рисунке 2.8: • Наличие выпуклой относительно оси Z куполообразной области. • Видна симметрия относительно приводов манипулятора. • Явно выражены вершины области достижимости при максимальном удалении одной из кареток привода. 38

а) б) в) Рисунок 2.8 – Зоны достижимости в координатах а) 0XY, б) 0XZ, в) 0YZ. Листинг программы для определения зоны достижимости «трипода» представлен в приложении 4. Описание разработанного для программы интерфейса подробнее представлено в 4 главе. 2.3 Краткие выводы по главе 1. Сформирована система уравнений, которая связывает координаты двух оснований данного манипулятора и дает полное описание его кинематики движения. 39

2. Получены решения прямой и обратной задачи кинематики посредством использования итерационного метода Ньютона. Совпадение полученных решений говорит о правильности выбранного метода, а также учета всех конструктивных ограничений. 3. Получена зона обслуживания манипулятора при различных начальных положениях кареток на направляющей. Траектории точек перемещения захвата формируют параболическую симметричную относительно приводов поверхность. Части, которые отображают максимумы траектории описывают граничные положения кареток по каждой из направляющих. 4. Разработаны программы в среде MATLAB 6 [4], которые позволяют получить расчет кинематики движения и графическое отображение зоны обслуживания для манипуляторов подобной конструкции, с возможностью задания различных технических характеристик. 40

3 ДИНАМИКА РОБОТА С ПАРАЛЛЕЛЬНОЙ КИНЕМАТИКОЙ 3.1. Анализ манипулятора и вывод описания динамической модели Для описание динамической модели манипулятора необходимо рассмотреть его общую схему, представленную на рисунке 3.1. В данном случае динамическая модель учитывает только силы, действующие на подвижную платформу. Для решения задачи динамики [16] воспользуемся самым распространенным способом - составлением уравнения Лагранжа второго рода. Рисунок 3.1 Схема механизма с системой сил 41

Будем рассматривать нашу систему с невесомыми штоками, пусть массой обладает только нижнее основание С1 С2 С3, его центр масс находится в точке С. На рис. 3.1 видны силы, с которыми штоки действуют на нижнее основание «трипода», а именно силы F1, F2, F3. Исследуем динамику этой механической системы со штоками переменной длины. Пусть в качестве обобщенных координат будут выступать координаты точки C: q1 = 𝑥С , q2 = 𝑦С , q3 = 𝑧С . Уравнение Лагранжа второго рода выглядит следующим образом: 𝑑 𝜕𝑇 𝜕𝑇 𝑑𝑡 𝜕𝑞𝑖 𝜕𝑞𝑖 ( )− ̇ = 𝑄𝑖 , i=1, 2, 3, (3.1) где T – кинетическая энергия системы, 𝑄𝑖 – обобщенные силы, которые соответствуют обобщенным координатам 𝑞𝑖 . Рассмотрим подвижное основание как материальную точку C, которая описывает центр масс платформы и имеет массу m, а штоки предполагаем невесомыми. Запишем выражение для кинетической энергии: 𝑇= 𝑚 2 ∑3𝑖=1 𝑞𝑖2̇ , i=1, 2, 3. (3.2) Из выражения (3.2) понимаем, что частные производные по обобщенным координатам будут нулевыми. Тогда итоговая система уравнений связи (3.1) перепишется в виде: 42

𝑚𝑞𝑖̈ = 𝑄𝑖 , i=1, 2, 3. (3.3) Найдем обобщенные силы 𝑄𝑖 в проекциях на оси системы Cxyz для составления выражения для них. На точку C действует сила тяжести P=(0, 0, -mg) и три силы F1, F2, F3, направленные вдоль штоков. Найдем проекции сил поочередно, геометрическим методом. Для этого для каждой из сил, действующих со стороны штоков, изобразим геометрическое представление. На рисунке 3.2 изображено геометрическое представление силы F1. Рисунок 3.2 Проекции силы F1 43

Из рис. 3.2 видно, что: 𝐹1𝑥 = |𝐶1 С1′′ |, |𝐶1 С1′′ | = |𝐶1 𝐶1′ | cos 𝛽1 = |𝐹1 𝐶1 | cos 𝛽1 cos 𝛼1 = 𝐹1 cos 𝛽1 cos 𝛼1 , 𝑦 𝐹1 = |𝐶С1′′′ |, |𝐶С1′′′ | = |𝐶1′′ 𝐶1′ | = |𝐶1′ 𝐶1 | sin 𝛽1 = 𝐹1 cos 𝛼1 sin 𝛽1 , 𝐹1𝑧 = |𝐶𝑀|, |𝐶𝑀| = 𝐹1 sin 𝛼1 . Тогда для силы F1 проекции будут равны: 𝐹1𝑥 = 𝐹1 cos 𝛽1 cos 𝛼1 , {𝐹1𝑦 = 𝐹1 cos 𝛽1 cos 𝛼1 , 𝐹1𝑧 = 𝐹1 sin 𝛼1 . На рисунке 3.3 изображено геометрическое представление силы F2. Из рис. 3.3 видно, что: 𝐹2𝑥 = |𝐻С′′ 2 |, 𝜋 𝜋 ′′′ ′ |𝐻С′′ 2 | = |𝐶2 С2 | = |𝐶2 𝐶2 | sin( 𝛽2 + ) = 𝐹2 sin( 𝛽2 + ) cos 𝛼2 , 6 6 𝑦 𝐹2 = −|𝑉𝑉1 |, 𝜋 𝜋 |𝑉𝑉1 | = |𝐶2′′′ 𝐶2′ | = |𝐶2 𝐶2′ | cos(𝛽2 + ) = 𝐹2 cos 𝛼2 cos(𝛽2 + ), 6 6 𝐹2𝑧 = |𝐻1 𝐶 |, |𝐻1 𝐶 | = 𝐹2 sin 𝛼2 . 44 (3.4)

Рисунок 3.3 Проекции силы F2 Тогда для силы F1 проекции будут равны: 𝜋 𝐹2𝑥 = 𝐹2 sin( 𝛽2 + ) cos 𝛼2 , 6 {𝐹2𝑦 𝐹2𝑧 𝜋 = −𝐹2 cos 𝛼2 cos (𝛽2 + ), 6 = 𝐹2 sin 𝛼2 . На рисунке 3.4 изображено геометрическое представление силы F3. 45 (3.5)

Рисунок 3.4 Проекции силы F3 Из рис. 3.4 видно, что: 𝐹3𝑥 = −|𝐾1 С′′ 3 |, 𝜋 𝜋 ′ ′ |𝐾1 С′′ 3 | = |С3 𝐾2 | = |𝐶3 𝐶3 | sin( 𝛽3 − ) = 𝐹3 sin( 𝛽3 − ) cos 𝛼3 , 6 6 𝑦 𝐹3 = −|𝐾С′′′ 3 |, 𝜋 𝜋 ′ |𝐾С′′′ 3 | = |𝐶3 𝐾2 | = |𝐶3 𝐶3 | cos(𝛽3 − ) = 𝐹3 cos 𝛼3 cos(𝛽3 − ), 6 6 𝐹3𝑧 = |𝐿𝐶 |, |𝐿𝐶 | = 𝐹3 sin 𝛼3 . Тогда для силы F1 проекции будут равны: 46

𝜋 𝐹3𝑥 = −𝐹3 sin ( 𝛽3 − ) cos 𝛼3 , 6 𝑦 {𝐹3 = −𝐹3 cos 𝛼3 cos(𝛽3 𝐹3𝑧 = 𝐹3 sin 𝛼3 . 𝜋 (3.6) − ), 6 Выражение элементарной работы на возможном перемещении системы будет выглядеть: 𝑦 𝛿𝐴 = −𝑚𝑔𝛿𝑞3 + ∑3𝑖=1(𝐹𝑖𝑥 𝛿𝑞1 + 𝐹𝑖 𝛿𝑞2 + 𝐹𝑖𝑧 𝛿𝑞3 ), i=1, 2, 3, (3.7) 𝑦 где 𝐹𝑖𝑥 , 𝐹𝑖 , 𝐹𝑖𝑧 – проекции сил 𝐹𝑖 на оси системы Cxyz. Из полученного отношения (3.7) можем записать выражения для обобщенных сил, которые равны получившимся коэффициентам при независимых вариациях обобщенных координат 𝛿𝑞𝑖 : 𝑦 𝑄1 = ∑3𝑖=1 𝐹𝑖𝑥 , 𝑄2 = ∑3𝑖=1 𝐹𝑖 , 𝑄3 = −𝑚𝑔 + ∑3𝑖=1 𝐹𝑖𝑧 . (3.8) Подставим в (3.8) геометрические выражения сил (3.4 – 3.6) и получим итоговую запись обобщенных сил: 𝜋 𝜋 𝑄1 = 𝐹1 cos 𝛽1 cos 𝛼1 + 𝐹2 sin( 𝛽2 + ) cos 𝛼2 − 𝐹3 sin ( 𝛽3 − ) cos 𝛼3 , 6 𝜋 6 {𝑄2 = 𝐹1 cos 𝛽1 cos 𝛼1 − 𝐹2 cos 𝛼2 cos (𝛽2 + )−𝐹3 cos 𝛼3 cos (𝛽3 − 𝜋), 6 6 (3.9) 𝑄3 = −𝑚𝑔 + 𝐹1 sin 𝛼1 + 𝐹2 sin 𝛼2 + 𝐹3 sin 𝛼3 . Подставив (3.9) в (3.3) окончательно составим уравнение Лагранжа 2-го рода. 𝜋 𝜋 6 6 𝑚𝑞1̈ = 𝐹1 cos 𝛽1 cos 𝛼1 + 𝐹2 sin( 𝛽2 + ) cos 𝛼2 − 𝐹3 sin ( 𝛽3 − ) cos 𝛼3 , { 𝑚𝑞2̈ = 𝐹1 cos 𝛽1 cos 𝛼1 − 𝐹2 cos 𝛼2 cos (𝛽2 + 𝜋)−𝐹3 cos 𝛼3 cos (𝛽3 − 𝜋), 6 𝑚𝑞3̈ = −𝑚𝑔 + 𝐹1 sin 𝛼1 + 𝐹2 sin 𝛼2 + 𝐹3 sin 𝛼3 . 47 6 (3.10)

Перейдем к решению прямой и обратной задач динамики. Обобщенные координаты могут быть заданы как функции от времени 𝑞𝑖 = 𝑞𝑖 (𝑡). В этом случае, решая систему (3.10), мы можем найти аналитически зависимость управляющих сил от времени: 𝐹𝑖 = 𝐹𝑖 (𝑡). Наоборот, при заданном законе изменения управляющих сил как функций времени можно, решив систему дифференциальных уравнений (3.10), найти закон движения материальной точки C. 3.2. Поиск решения прямой и обратной задачи динамики Рассматривая динамику роботов-манипуляторов, выделяют две основные задачи. Прямая задача динамики подразумевает определение равнодействующих сил, действующих на тело по изначально заданному закону движения координаты (скорости или ускорения). Обратная задача позволяет установить характер перемещения тела, т.е. изменение координаты (скорости или ускорения) за счет задания сил. Поиск решения прямой задачи динамики нетрудоемкий и подразумевает решение обыкновенного уравнения или системы уравнений, а также процесс дифференцирования обобщенных координат. Задача будет иметь единственное решение. Интереснее дела обстоят с поиском решения обратной задачи. Здесь необходимо решить дифференциальное уравнение 2-го порядка. Процедура интегрирования подразумевает появление в решении постоянных интегрирования, в данном случае их будет по две на каждое уравнение. Наличие постоянных интегрирования указывает на то, что у системы есть бесконечное множество решений, иначе говоря силы не определяют однозначно движение точки. Одним и тем же силам могут соответствовать разные траектории движения. Чтобы найти постоянные интегрирования и свести количество решение к одному необходимо задать начальные условия, тем самым решить задачу Коши. 48

Для поиска решения воспользуемся стандартными пакетными функциями среды разработки MATLAB 6 [4]: «solve» – для поиска решения обыкновенного уравнения в символьном виде; «dsolve» - для поиска решения дифференциального уравнения в символьном виде. Для дальнейшей простоты необходимо упростить систему (3.10). Воспользуемся тригонометрическими формулами приведения, тогда после преобразований система примет вид. Система 3.11: 1 1 √3 √3 sin 𝛽2 − 𝑐𝑜𝑠 𝛽2 ) cos 𝛼2 − 𝐹3 ( sin 𝛽3 + cos 𝛽3 ) cos 𝛼3 , 2 2 2 2 1 1 √3 √3 𝐹2 cos 𝛼2 ( cos𝛽2 + sin 𝛽2 ) −𝐹3 cos 𝛼3 ( cos𝛽3 + sin 𝛽3 ), 2 2 2 2 𝑚𝑞1̈ = 𝐹1 cos 𝛽1 cos 𝛼1 + 𝐹2 ( 𝑚𝑞2̈ = 𝐹1 cos 𝛽1 cos 𝛼1 − { 𝑚𝑞3̈ = −𝑚𝑔 + 𝐹1 sin 𝛼1 + 𝐹2 sin 𝛼2 + 𝐹3 sin 𝛼3 . Введем замену, пусть 𝑥1 = 𝛼1 , 𝑥2 = 𝛼2 , 𝑥3 = 𝛼3 , 𝑥4 = 𝛽1 , 𝑥5 = 𝛽2 , 𝑥6 = 𝛽3 . Тогда итоговая система примет вид: 1 1 √3 √3 𝑥5 − 𝑦5 ) 𝑦2 − 𝐹3 ( 𝑥6 + 𝑦6 ) 𝑦3 , 2 2 2 2 1 1 √3 √3 𝐹2 𝑦2 ( + 𝑥5 ) −𝐹3 𝑦3 ( 𝑦6 + 𝑥6 ), 2 2 2 2 𝑚𝑞1̈ = 𝐹1 𝑦4 𝑦1 + 𝐹2 ( 𝑚𝑞2̈ = 𝐹1 𝑦4 𝑦1 − (3.12) { 𝑚𝑞3̈ = −𝑚𝑔 + 𝐹1 𝑥1 + 𝐹2 𝑥2 + 𝐹3 𝑥3 . Код программы и алгоритм работы приложения для решения задач динамки приведены в приложениях 6 и 7. Проверим как себя поведет система при краевых случаях, т.е. зададимся законом движения во времени только для одной из трех обобщенных координат. Для начала пусть q1 = 0*t, q2 = 0*t, q3 = t. Это означает, что движение манипулятора происходит только по оси Z. Графически это можно увидеть на рисунке 3.5. 49

Рисунок 3.5 – Изменение координат во времени. Так как закон движения задан прямолинейный, то ожидаемым результатом должно быть, что сила, действующая со стороны штоков, не изменяется во времени. Математически это связано с проведением дифференцирования обобщенных координат. Физически прямолинейный закон движения подразумевает отсутствие ускорения, а как известно ускорение - вторая производная от перемещения. Результирующие силы видны на рисунке 3.6. Рисунок 3.6 – Изменение сил во времени. 50

Для полученных сил решим обратную задачу динамики. Зададимся нулевыми начальными условиями. Решение обратной задачи подразумевает нахождение закона движения обобщенных координат. Результат представлен на рисунке 3.7. Рисунок 3.7 – Изменение координат во времени. На графиках изменения видно, что законы движения совершенно не совпадают с изначально заданными. Т.к. решение дифференциального уравнения второго порядка подразумевает существование множества решений, а одно из возможных решений можно найти, задавшись определенными начальными условиями. Те же результаты мы получим при рассмотрении случаев, когда прямолинейное движение происходит по оси X, т.е. q1 = t, q2 = 0*t, q3 = 0*t (рисунки 3.8 и 3.9). И тоже для случая движения по оси Y: q1 = 0*t, q2 = t, q3 = 0*t (рисунки 3.10 и 3.11). 51

Рисунок 3.8 – Изменение координат во времени. Рисунок 3.9 – Изменение сил во времени. Рисунок 3.10 – Изменение координат во времени. Рисунок 3.11 – Изменение сил во времени. 3.3. Краткие выводы по главе 1. Сформирована система уравнений Лагранжа второго рода, которая показывает влияние сил на подвижное основание в зависимости от закона 52

движения координат и дает полное описание динамики движения основания. 2. Получены решения прямой и обратной задачи динамики посредством использования встроенных программных методов в MATLAB 6 [4]. Полученные графические зависимости теоретически интерпретируются математическими законами дифференцирования и особенностями решения дифференциальных уравнений. 3. Разработаны программы в среде MATLAB 6 [4], которые позволяют получить расчет динамики движения и графическое отображение решений в виде временных законов движения координат схвата, а также законы изменения сил во времени, действующих на подвижную платформу. 53

4 РАЗРАБОТКА ПРИЛОЖЕНИЯ ДЛЯ РЕШЕНИЯ ЗАДАЧ Получение аналитического решения для задач кинематики и динамики движения манипуляторов с параллельной структурой является сложным. Поэтому разработка подобного приложения является актуальной и значимой задачей. Ниже приведено подробное описание алгоритма работы всего приложения, разработка которого осуществлялась в среде MATLAB 6 [4]. 4.1 Описание интерфейса разработанной программы по решению прямой, обратной задач кинематики и зоны достижимости 4.1.1 Алгоритм работы «Главного окна» приложения На рисунке 4.1 представлено главное окно разработанной пользовательского приложения. Рисунок 4.1. - Главное окно программы. 54

На главном окне (рис. 4.1) располагается 5 кнопок, разделенных на два раздела решаемых задач. Раздел кинематики содержит три кнопки: «Прямая задача кинематики», «Обратная задача кинематики» и «Зона достижимости». Раздел динамики имеет две кнопки: «Прямая задача динамики», «Обратная задача динамики». По нажатию любой из кнопок открывается соответствующее окно приложения. Листинг кода для главного окна приложения с комментариями приведен в приложении 1. 4.1.2 Алгоритм работы окна «Прямая задача кинематики» приложения При решении прямой задачи кинематики открывается окно «Прямая задача кинематики» (рисунок 4.2). Рисунок 4.2 Прямая задача кинематики Для выполнения расчета необходимо задать параметры конструкции «трипода», точность решения и шаг, т.к. вычисление основано на методе Ньютона, а также положения кареток S1, S2, S3. На рисунке 4.3 приведен пример корректно 55

введенных данных в выделенные динамические поля. Дробные числа должны вводится в поле с разделителем «точка». Рисунок 4.3 Пример ввода корректных данных После нажатия на кнопку «Рассчитать» выполняется программа, которая находит поиск решения прямой задачи кинематики. В соответствующей области в правом нижнем углу появятся координаты положения центра подвижной платформы, как это видно на рисунке 4.4. 56

Рисунок 4.4 Решение прямой задачи кинематики В приложении предусмотрена проверка некорректного ввода данных, а именно параметров конструкции. Любая геометрическая величина, будь то длинна или радиус не могут быть отрицательными, а величины, отражающие точность решения и шаг в методе Ньютона, должны лежать в пределах от 0 до 1 для получения верных результатов. В случае ввода некорректных данных будет появляться соответствующее окно с ошибкой. Пример, представлен на рисунке 4.5. После нажатия «ОК» динамическая область текста очищается и необходимо повторить попытку ввода параметров. Рисунок 4.5 Окно при некорректном вводе параметров 57

Листинг кода для окна «Прямая задача кинематики» приложения с комментариями приведен в приложении 2. 4.1.3 Алгоритм работы окна «Обратная задача кинематики» приложения При решении обратной задачи кинематики открывается окно «Обратная задача кинематики» (рисунок 4.6). Рисунок 4.6 Обратная задача кинематики Для выполнения вычислений необходимо задать параметры конструкции «трипода», точность решения и шаг, т.к. вычисление основано на методе Ньютона, а также координаты центра подвижной платформы 𝑥С , 𝑦С , 𝑧С . На рисунке 4.7 приведен пример корректно введенных данных в выделенные динамические поля. Дробные числа должны вводится в поле с разделителем «точка». 58

Рисунок 4.7 Пример ввода корректных данных Нажатие на кнопку «Рассчитать» запускает процесс выполнения программы, которая находит решение обратной задачи кинематики. В соответствующей области в правом нижнем углу появятся положения кареток манипулятора, как это видно на рисунке 4.8. Рисунок 4.8 Решение обратной задачи кинематики 59

При вводе параметров конструкции предусмотрена проверка некорректного ввода данных. Любая геометрическая величина, будь то длинна или радиус не могут быть отрицательными, а величины, отражающие точность решения и шаг в методе Ньютона, должны лежать в пределах от 0 до 1 для получения верных результатов. В случае ввода некорректных данных будет появляться соответствующее окно с ошибкой. Все виды ошибок представлены на рисунке 4.9. После нажатия «ОК» динамическая область текста очищается и необходимо повторить попытку ввода параметров. Рисунок 4.9 Окно при некорректном вводе параметров Листинг кода для окна «Обратная задача кинематики» приложения с комментариями приведен в приложении 3. 4.1.4 Алгоритм работы окна «Зона достижимости» приложения При переходе в окно «Зона достижимости манипулятора» открывается окно, представленное на рисунке 4.10 Рисунок 4.10 Окно приложения «Зона достижимости» 60

На рисунке 4.11 представлен пример корректного ввода параметров конструкции «трипода». Все дробные величины в динамические поля вводятся с разделителем «точка». Рисунок 4.11 Пример корректного ввода данных После нажатия кнопки «Построить график» запускается процесс вычисления и на экран выводится 3-х мерное изображение области достижимости манипулятора. Пример отображения графика представлен на рисунке 4.12. Рисунок 4.12 Пример вывод графика области достижимости 61

В данной части интерфейса на вводимые данные в динамические поля также наложены ограничения, которые отображаются в виде окон с ошибками, которые представлены ниже на рисунке 4.13. Рисунок 4.13 Окно при некорректном вводе параметров Листинг кода для окна «Зона достижимости» приложения с комментариями приведен в приложении 4. 4.2 Описание интерфейса разработанной программы по решению прямой, обратной задач динамики 4.2.1 Алгоритм работы окна «Прямая задача динамики» приложения При решении прямой задачи динамики открывается окно «Прямая задача динамики» (рисунок 4.14). 62

Рисунок 4.14 - Окно «Прямая задача» Для выполнения вычисления необходимо ввести начальные параметры, а точнее: массу подвижного основания - m, законы изменения движения координат во времени – q1(t), q2(t), q3(t). На рисунке 4.15 приведен пример корректного ввода данных в поля динамического текста. Рисунок 4.15 – Пример корректного ввода данных 63

После нажатия на кнопку «Вычислить» происходит выполнение программы, решающей прямую задачу динамики, а именно вычисление законов изменения сил, действующих со стороны штоков, во времени. Математически решение прямой задачи подразумевает решение системы уравнений. На рисунке 4.16 приведен пример расчета. В интерфейсе предусмотрено построение графиков для визуального отображения законов изменения сил и координат во времени. Рисунок 4.16 – Окно «Прямая задача» с вычислениями По нажатию на кнопку «Построить графики сил» появляется окно, представленное на рисунке 4.17. В данном окне, строится сразу три графика, которые визуально отражают законы изменения сил, действующих со стороны штоков. 64

Рисунок 4.17 - Окно графиков изменения сил По нажатию на кнопку «Построить графики координат» появляется окно, представленное на рисунке 4.18. В данном окне строится сразу три графика, которые визуально отражают законы изменения координат Xc, Yc, Zc. Рисунок 4.18 – Окно графиков изменения координат 65

В приложении предусмотрена проверка некорректного ввода данных, а именно массы. Масса – величина, которая не может быть отрицательной. В случае ввода массы меньше или равной 0 будет появляться окно с ошибкой, представленное на рисунке 4.19. После нажатия «ОК» необходимо повторить попытку ввода параметров. Рисунок 4.19 – Окно ошибки Листинг кода для приложения решения «прямой задачи динамики» с комментариями приведен в приложении 5. 4.2.2 Алгоритм работы окна «Обратная задача динамики» приложения При решении обратной задачи динамики открывается окно «Обратная задача динамики» (рисунок 4.20). Рисунок 4.20 - Окно «Обратная задача» 66

Для выполнения вычисления необходимо ввести начальные параметры, а точнее: массу подвижного основания - m, законы изменения сил, действующих на основание, во времени – F1(t), F2(t), F3(t). На рисунке 4.21 приведен пример корректного ввода данных в поля динамического текста. Рисунок 4.21 – Пример корректного ввода данных После нажатия на кнопку «Вычислить» происходит выполнение программы, решающей обратную задачу динамики, а именно вычисление законов изменения координат во времени. Математически решение обратной задачи подразумевает решение дифференциального уравнения 2-го порядка. Здесь возникает проблема не единственности решения, т.е. уравнение имеет бесконечное множество решений в зависимости от начальных условий (далее по тексту НУ). При чем НУ по перемещению и по скорости подразумевают задание значение координат в момент времени 0 и первой производной в момент времени 0. Из-за множественности решений можем видеть не совпадение решений прямой и обратной задачи динамики в примере расчета, представленного на рисунке 4.22. 67

Рисунок 4.22 - Окно «Обратная задача» с вычислениями. В интерфейсе предусмотрено построение графиков для визуального отображения законов изменения сил и координат во времени. По нажатию на кнопку «Построить графики координат» появляется окно, представленное на рисунке 4.23. В данном окне строится сразу три графика, которые визуально отражают законы изменения координат Xc, Yc, Zc. Рисунок 4.23 – Окно графиков изменения координат. 68

По нажатию на кнопку «Построить графики сил» появляется окно, представленное на рисунке 4.24. В данном окне строится сразу три графика, которые визуально отражают законы изменения сил, действующих со стороны штоков. Рисунок 4.24 – Окно графиков изменения сил. В приложении для решения обратной задачи также предусмотрена проверка некорректного ввода данных, а именно массы. В случае ввода массы меньше или равной 0 будет появляться окно с ошибкой, представленное на рисунке 4.25. После нажатия «ОК» необходимо повторить попытку ввода параметров. Рисунок 4.25 – Окно ошибки. 69

Листинг кода для приложения решения «обратной задачи динамики» с комментариями приведен в приложении 6. 70

5 ЗАКЛЮЧЕНИЕ Целью текущей работы является разработка математической модели манипулятора с параллельной кинематикой. В ходе выполнения работы был проведен обзор механизмов с подобной структурой, построена модель манипулятора типа «трипод», получены уравнения связи для кинематики и динамики движения, разработано пользовательское приложение. Для кинематики движения получены решения прямой и обратной задач кинематики, найдена зона достижимости манипулятора и краткий анализ полученных результатов. Для динамики движения также были получены решения прямой и обратной задач динамики, проведен краткий анализ полученных характеристик при частных случаях работы «трипода». Для простоты поиска решений всех этих задач и их анализа разработано приложение с пользовательским интерфейсом с помощью среды разработки MATLAB 6 [4]. Описан алгоритм работы каждого пункта и проиллюстрирован рисунками и графиками. 71

6 СПИСОК ИСПОЛЬЗУЕМОЙ ЛИТЕРАТУРЫ 1. Борисов О.И., Громов В.С., Пыркин А.А., Методы управления робототехническими приложениями. Учебное пособие. — СПб.: Университет ИТМО, 2016. — 108 с. 2. А.С. Климчик Р.И. Гомолицкий Ф.В. Фурман К.И. Сёмкин, «Разработка управляющих программ промышленных роботов». Курс лекций. Минск 2008 г. 3. A.M. Грибков, Д.В. Шилин Математическое моделирование манипулятора типа трипод на базе бесштоковых пневматических приводов, ж. «Известия высших учебных заведений. Машиностроение», №9, 2011, С.3-10 4. Дьякoнoв В. MATLAB 6: учебный курс СПб.: Питер, 2001. - 592c. 5. Дмитрий Васильев Трипод EXPT - манипулятор высшей категории, ж. «CONTROL ENGINEERING РОССИЯ» #1 (55), 2015 С.48-50 6. Каганов Ю.Т., Карпенко А.П. Математическое моделирование кинематики и динамики робота-манипулятора типа «хобот». 1. Математические модели секции манипулятора, как механизма параллельной кинематики типа «трипод» / Наука и образование: электронное научно-техническое издание. 2009. № 10. С. 4-14. 7. Stewart, D. A platform with six degrees of freedom / D. Stewart // Proceedings of the Institution of mechanical engineers, 1965. - V. 180, рt. 1, № 15. - Р. 371-385. 8. Pollard W.L.G., "Spray painting machine," US Patent No. 2,213,108, August 26, 1940. 9. A.c. 558788 СССР, МКИ В25J 1/02. Манипулятор / Данилевский В.Н. //Открытия. Изобретения. 1977. - № 19. - С.35-36. 10. OKUMA PM-600 Vertical Machining Centers [Электронный ресурс]: Вертикальные обрабатывающие центры. - Режим доступа: http://www.okuma.com/pm-600. 11. Olazagoitia J.L., Wyatt S. New PKM Tricept T9000 and its application to flexible manufacturing at Aerospace Industry / SAE International, Paper, 2007. 72

12. Робот-манипулятор www.abb.com/robotics FlexPicker [Электронный IRB ресурс] 360 - компании Режим AAB доступа: http://www.roboticturnkeysolutions.com/robots/abb/datasheet/IRB_360.pdf 13. Робот серии М-1 компании FANUC www.fanuc.eu/ru/ru/роботы [Электронный ресурс] - Режим доступа: https://www.fanuc.eu/ru/ru/роботы/страница-фильтра-роботов/серия-m1 14. Роботы с параллельной кинематикой EXPT [Электронный ресурс] - Режим доступа: https://www.festo.com/cat/ru_ru/data/doc_RU/PDF/RU/EXPT_RU.PDF 15. Мэтьюз Д, Финк К. Чиcленные метoды. Иcпoльзoвание MATLAB. М.: Издательcкий дoм «Вильямc», 2001, 272 c 16. Колюбин С.А. Динамика робототехнических систем. Учебное пособие. — СПб.: Университет ИТМО, 2017. — 117 с. 73

7 ПРИЛОЖЕНИЕ Приложение 1. Организация главной программы приложения Листинг главной программы “TmainGUI.m”, которая объединила в себе два раздела решаемых задач и пять пунктов расчетов. function varargout = TmainGUI(varargin) gui_Singleton = 1; gui_State = struct('gui_Name', mfilename, ... 'gui_Singleton', gui_Singleton, ... 'gui_OpeningFcn', @TmainGUI_OpeningFcn, ... 'gui_OutputFcn', @TmainGUI_OutputFcn, ... 'gui_LayoutFcn', [] , ... 'gui_Callback', []); if nargin && ischar(varargin{1}) gui_State.gui_Callback = str2func(varargin{1}); end if nargout [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); else gui_mainfcn(gui_State, varargin{:}); end function TmainGUI_OpeningFcn(hObject, eventdata, handles, varargin) handles.output = hObject; guidata(hObject, handles); function varargout = TmainGUI_OutputFcn(hObject, eventdata, handles) varargout{1} = handles.output; function pb1_Callback(hObject, eventdata, handles) TdirGUI; function pb2_Callback(hObject, eventdata, handles) INVGUI; function pb3_Callback(hObject, eventdata, handles) ZonaGUI; function pb4_Callback(hObject, eventdata, handles) din_GUI; function pb5_Callback(hObject, eventdata, handles) inv_din_GUI; 74

Приложение 2. Организация решения задачи прямой кинематики Листинг программы “TdirGUI.m”, решающей прямую задачу кинематики: function varargout = TdirGUI(varargin) gui_Singleton = 1; gui_State = struct('gui_Name', mfilename, ... 'gui_Singleton', gui_Singleton, ... 'gui_OpeningFcn', @TdirGUI_OpeningFcn, ... 'gui_OutputFcn', @TdirGUI_OutputFcn, ... 'gui_LayoutFcn', [] , ... 'gui_Callback', []); if nargin && ischar(varargin{1}) gui_State.gui_Callback = str2func(varargin{1}); end if nargout [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); else gui_mainfcn(gui_State, varargin{:}); end function TdirGUI_OpeningFcn(hObject, eventdata, handles, varargin) handles.output = hObject; guidata(hObject, handles); function varargout = TdirGUI_OutputFcn(hObject, eventdata, handles) varargout{1} = handles.output; function e1_Callback(hObject, eventdata, handles) global L; L=str2double(get(handles.e1,'string')); if L<=0 msgbox('Неккоректный ввод. Длина основания должна быть положительной', 'Error', 'error'); set(handles.e1,'String',''); end; function e1_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function e2_Callback(hObject, eventdata, handles) global l; l=str2double(get(handles.e2,'string')); if l<=0 msgbox('Неккоректный ввод. Длина тяги должна быть положительной', 'Error', 'error'); set(handles.e2,'String',''); end; function e2_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function e3_Callback(hObject, eventdata, handles) global S; S=str2double(get(handles.e3,'string')); if S<=0 msgbox('Неккоректный ввод. Длина хода должна быть положительной', 'Error', 'error'); set(handles.e3,'String',''); end; 75

function e3_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function e4_Callback(hObject, eventdata, handles) global Sstart; Sstart=str2double(get(handles.e4,'string')); if Sstart<=0 msgbox('Неккоректный ввод. Начало хода должно быть положительным', 'Error', 'error'); set(handles.e4,'String',''); end; function e4_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function e5_Callback(hObject, eventdata, handles) global Send; Send=str2double(get(handles.e5,'string')); if Send<=0 msgbox('Неккоректный ввод. Конец хода должно быть положительным', 'Error', 'error'); set(handles.e5,'String',''); end; function e5_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function e6_Callback(hObject, eventdata, handles) global a; a=str2double(get(handles.e6,'string')); if a<=0 msgbox('Неккоректный ввод. Радиус основания должен быть положительным', 'Error', 'error'); set(handles.e6,'String',''); end; function e6_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function e7_Callback(hObject, eventdata, handles) global h; h=str2double(get(handles.e7,'string')); if (h<=0) || (h>=1) msgbox('Неккоректный ввод. Величина шага должна лежать в пределах от 1 до 0', 'Error', 'error'); set(handles.e7,'String',''); end; function e7_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function e8_Callback(hObject, eventdata, handles) global e; e=str2double(get(handles.e8,'string')); if ((e<=0) || (e>=1)) msgbox('Неккоректный ввод. Величина погрешности должна лежать в пределах от 1 до 0', 'Error', 'error'); 76

set(handles.e8,'String',''); end; function e8_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function e9_Callback(hObject, eventdata, handles) global S1; S1=str2double(get(handles.e9,'string')); if S1<=0 msgbox('Неккоректный ввод. Положение 1-ой каретки должно быть положительным', 'Error', 'error'); set(handles.e9,'String',''); end; function e9_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function e10_Callback(hObject, eventdata, handles) global S2; S2=str2double(get(handles.e10,'string')); if S2<=0 msgbox('Неккоректный ввод. Положение 2-ой каретки должно быть положительным', 'Error', 'error'); set(handles.e10,'String',''); end; function e10_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function e11_Callback(hObject, eventdata, handles) global S3; S3=str2double(get(handles.e11,'string')); if S3<=0 msgbox('Неккоректный ввод. Положение 3-ой каретки должно быть положительным', 'Error', 'error'); set(handles.e11,'String',''); end; function e11_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function pushbutton1_Callback(hObject, eventdata, handles) %Начальные параметры трипода global L l S Sstart Send a h e S1 S2 S3; L = str2double(get(handles.e1,'string')); l = str2double(get(handles.e2,'string')); S = str2double(get(handles.e3,'string')); Sstart = str2double(get(handles.e4,'string')); Send = str2double(get(handles.e5,'string')); a = str2double(get(handles.e6,'string')); h = str2double(get(handles.e7,'string')); e = str2double(get(handles.e8,'string')); S1 = str2double(get(handles.e9,'string')); S2 = str2double(get(handles.e10,'string')); S3 = str2double(get(handles.e11,'string')); %Кocинуc и cинуc угла между цилиндрoм и непoдвижным ocнoванием cos_g = (sqrt(3)*L)/(3*S); sin_g = sqrt(1-cos_g^2); 77

%Расчет кooрдинат каретoк S1 = S1+Sstart; S2 = S2+Sstart; S3 = S3+Sstart; %Вводим 1-ое приближение X0 = zeros(12,1); %Вводим 2-ое приближение X1 = ones(12,1); % Замена кoнcтант c1 = (S1*sin_g)/l;B=(S2*sin_g)/l;C=(S3*sin_g)/l; c2 = -(3*a/2)-(S2*cos_g/2)-S1*cos_g+sqrt(3)*L/2; c3 = -(3*a/2)-(S3*cos_g/2)-S1*cos_g+sqrt(3)*L/2; c4 = sqrt(3)*S2*cos_g/2+sqrt(3)*a/2-L/2; c5 = -sqrt(3)*S3*cos_g/2-sqrt(3)*a/2+L/2; %Метoд Ньютoна с точностью е while (abs(X1-X0) > e) %Приcвoение первoму приближению втoрoе приближение X0 = X1; %Cиcтема уравнений F = zeros(12,1); %Oбoзначение неизвеcтных x1 = X0(1,1);x2=X0(2,1);x3=X0(3,1);x4=X0(4,1);x5=X0(5,1);x6=X0(6,1);y1=X0(7,1); y2 = X0(8,1);y3=X0(9,1);y4=X0(10,1);y5=X0(11,1);y6=X0(12,1); %Уравнения cиcтемы f1 = x1+c1-x2-B; f2 = x2+B-x3-C; f3 = (-l*y2*y5)/2-(sqrt(3)*l*y2*x5)/2-l*y1*y4+c2; f4 = (-(sqrt(3))*l*y3*x6)/2+(l*y3*y6)/2-l*y1*y4+c3; f5 = (sqrt(3)*l*y2*y5)/2-(l*y2*x5)/2-l*y1*x4+c4; f6 = (-(sqrt(3))*l*y3*y6)/2-(l*y3*x6)/2-l*y1*x4+c5; f7 = x1^2+y1^2-1; f8 = x2^2+y2^2-1; f9 = x3^2+y3^2-1; f10 = x4^2+y4^2-1; f11 = x5^2+y5^2-1; f12 = x6^2+y6^2-1; %Матрица значений уравнений cиcтемы c пoдcтанoвкoй первoгo приближения F(1,1) = f1; F(2,1) = f2; F(3,1) = f3; F(4,1) = f4; F(5,1) = f5; F(6,1) = f6; F(7,1) = f7; F(8,1) = f8; F(9,1) = f9; F(10,1) = f10; F(11,1) = f11; F(12,1) = f12; %матрица Якoби J = zeros(12,12); %Нахoждение прoизвoдных f1x1 = 1; f1x2 = -1; f2x2 = 1; f2x3 = -1; f3x5 = (-sqrt(3)*l*y2)/2; f3y1 = -l*y4; f3y2 = (-l*y5)/2-(sqrt(3)*l*x5)/2; f3y4 = -l*y1; f3y5 = (-l*y2)/2; f4x6 = (-sqrt(3)*l*y3)/2; f4y1 = -l*y4; f4y3 = (-sqrt(3)*l*x6)/2+(l*y6)/2; f4y4 = -l*y1; f4y6 = (l*y3)/2; f5x4 = -l*y1; f5x5 = (-l*y2)/2; f5y1 = -l*x4; f5y2 = (sqrt(3)*l*y5)/2-(l*x5)/2; f5y5 = (sqrt(3)*l*y2)/2; f6x4 = -l*y1; f6x6 = (-l*y3)/2; f6y1 = -l*x4; f6y3 = (-(sqrt(3))*l*y6)/2-(l*x6)/2; f6y6 = (-(sqrt(3))*l*y3)/2; f7x1 = 2*x1; f7y1 = 2*y1; f8x2 = 2*x2; f8y2 = 2*y2; f9x3 = 2*x3;f9y3 = 2*y3; f10x4 = 2*x4; f10y4 = 2*y4; f11x5 = 2*x5; f11y5 = 2*y5; f12x6 = 2*x6; f12y6 = 2*y6; %Фoрмирoвание матрицы Якoби J(1,1) = f1x1; J(1,2) = f1x2; J(2,2) = f2x2; J(2,3) = f2x3; J(3,5) = f3x5; J(3,7) = f3y1; J(3,8) = f3y2; J(3,10) = f3y4; J(3,11) = f3y5; 78

J(4,6) = f4x6; J(4,7) = f4y1; J(4,9) = f4y3; J(4,10) = f4y4; J(4,12) = f4y6; J(5,4) = f5x4; J(5,5) = f5x5; J(5,7) = f5y1; J(5,8) = f5y2; J(5,11) = f5y5; J(6,4) = f6x4; J(6,6) = f6x6; J(6,7) = f6y1; J(6,9) = f6y3; J(6,12) = f6y6; J(7,1) = f7x1; J(7,7) = f7y1; J(8,2) = f8x2; J(8,8) = f8y2; J(9,3) = f9x3; J(9,9) = f9y3; J(10,4) = f10x4; J(10,10) = f10y4; J(11,5) = f11x5; J(11,11) = f11y5; J(12,6) = f12x6; J(12,12) = f12y6; %Нахoждение oбратнoй матрицы Obr=J^(-1); %Нахoждение втoрoгo приближения X1=X0-Obr*F; end %Нахoждение кooрдинат тoчек непoдвижнoгo ocнoвания манипулятoра xA1= S1*cos_g+l*X1(7,1)*X1(10,1)+a; yA1=l*X1(7,1)*X1(4,1); zA1=S1*sin_g+l*X1(1,1); xA2=-S2*cos_g*sin(pi/6)-l*X1(8,1)*(sin(pi/6)*X1(11,1)+cos(pi/6)*X1(5,1))a*sin(pi/6); yA2=S2*cos_g*cos(pi/6)+l*X1(8,1)*(cos(pi/6)*X1(11,1)sin(pi/6)*X1(5,1))+a*cos(pi/6); zA2=S2*sin_g+l*X1(2,1); xA3=-S3*cos_g*sin(pi/6)-l*X1(9,1)*(X1(6,1)*cos(pi/6)-X1(12,1)*sin(pi/6))a*sin(pi/6); yA3=-S3*cos_g*cos(pi/6)-l*X1(9,1)*(X1(12,1)*cos(pi/6)+sin(pi/6)*X1(6,1))a*cos(pi/6); zA3=S3*sin_g+l*X1(3,1); %Раcчет кooрдинат центра подвижной платформы xC=-(xA1+xA2+xA3)/3; yC=-(yA1+yA2+yA3)/3; zC=-(zA1+zA2+zA3)/3; %Вывoд значений кooрдинат захвата set(handles.t1,'string',num2str(xC, '% 10.3f')); set(handles.t2,'string',num2str(yC, '% 10.3f')); set(handles.t3,'string',num2str(zC, '% 10.3f')); 79

Приложение 3. Организация решения задачи обратной кинематики Листинг программы “INVGUI.m”, решающей обратную задачу кинематики: function varargout = INVGUI(varargin) gui_Singleton = 1; gui_State = struct('gui_Name', mfilename, ... 'gui_Singleton', gui_Singleton, ... 'gui_OpeningFcn', @INVGUI_OpeningFcn, ... 'gui_OutputFcn', @INVGUI_OutputFcn, ... 'gui_LayoutFcn', [] , ... 'gui_Callback', []); if nargin && ischar(varargin{1}) gui_State.gui_Callback = str2func(varargin{1}); end if nargout [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); else gui_mainfcn(gui_State, varargin{:}); end function INVGUI_OpeningFcn(hObject, eventdata, handles, varargin) handles.output = hObject; guidata(hObject, handles); function varargout = INVGUI_OutputFcn(hObject, eventdata, handles) varargout{1} = handles.output; function ed1_Callback(hObject, eventdata, handles) global L; L=str2double(get(handles.ed1,'string')); if L<=0 msgbox('Неккоректный ввод. Длина основания должна быть положительной', 'Error', 'error'); set(handles.ed1,'String',''); end; function ed1_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function ed2_Callback(hObject, eventdata, handles) global l; l=str2double(get(handles.ed2,'string')); if l<=0 msgbox('Неккоректный ввод. Длина тяги должна быть положительной', 'Error', 'error'); set(handles.ed2,'String',''); end; function ed2_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function ed3_Callback(hObject, eventdata, handles) global S; S=str2double(get(handles.ed3,'string')); if S<=0 msgbox('Неккоректный ввод. Длина хода должна быть положительной', 'Error', 'error'); set(handles.ed3,'String',''); end; 80

function ed3_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function ed4_Callback(hObject, eventdata, handles) global Sstart; Sstart=str2double(get(handles.ed4,'string')); if Sstart<=0 msgbox('Неккоректный ввод. Начало хода должно быть положительным', 'Error', 'error'); set(handles.ed4,'String',''); end; function ed4_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function ed5_Callback(hObject, eventdata, handles) global Send; Send=str2double(get(handles.ed5,'string')); if Send<=0 msgbox('Неккоректный ввод. Конец хода должно быть положительным', 'Error', 'error'); set(handles.ed5,'String',''); end; function ed5_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end; function ed6_Callback(hObject, eventdata, handles) global a; a=str2double(get(handles.ed6,'string')); if a<=0 msgbox('Неккоректный ввод. Радиус основания должен быть положительным', 'Error', 'error'); set(handles.ed6,'String',''); end; function ed6_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function ed7_Callback(hObject, eventdata, handles) global h; h=str2double(get(handles.ed7,'string')); if (h<=0) || (h>=1) msgbox('Неккоректный ввод. Величина шага должна лежать в пределах от 1 до 0', 'Error', 'error'); set(handles.ed7,'String',''); end; function ed7_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function ed8_Callback(hObject, eventdata, handles) global e; e=str2double(get(handles.ed8,'string')); if ((e<=0) || (e>=1)) msgbox('Неккоректный ввод. Величина погрешности должна лежать в пределах от 1 до 0', 'Error', 'error'); 81

set(handles.ed8,'String',''); end; function ed9_Callback(hObject, eventdata, handles) function ed9_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function ed10_Callback(hObject, eventdata, handles) function ed10_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function ed11_Callback(hObject, eventdata, handles) function ed11_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function pushbutton2_Callback(hObject, eventdata, handles) %Начальные параметры трипода global L l S Sstart Send a h e; L = str2double(get(handles.ed1,'string')); l = str2double(get(handles.ed2,'string')); S = str2double(get(handles.ed3,'string')); Sstart = str2double(get(handles.ed4,'string')); Send = str2double(get(handles.ed5,'string')); a = str2double(get(handles.ed6,'string')); h = str2double(get(handles.ed7,'string')); e = str2double(get(handles.ed8,'string')); Xc =str2double(get(handles.ed9,'string')); Yc= str2double(get(handles.ed10,'string')); Zc = str2double(get(handles.ed11,'string')); %Кocинуc и cинуc угла между цилиндрoм и непoдвижным ocнoванием cos_g = (sqrt(3)*L)/(3*S); sin_g = sqrt(1-cos_g^2); %Задаем 1-ое приближение X0 = zeros(15,1); %Задаем 2-ое приближение X1 = ones(15,1); %Метoд Ньютoна с точностью е while (abs(X1-X0) > e) %Приcвoение первoму приближению втoрoе приближение X0 = X1; %Cиcтема уравнений F = zeros(15,1); %Вводим переобозначения неизвеcтных x1 = X0(1,1); x2 = X0(2,1); x3 = X0(3,1); x4 = X0(4,1); x5 = X0(5,1); x6 = X0(6,1); y1 = X0(7,1); y2 = X0(8,1); y3 = X0(9,1); y4 = X0(10,1); y5 = X0(11,1); y6 = X0(12,1); %Переобозначение неизвестных, которые необходимо найти(положение кареток) x7 = X0(13,1); x8 = X0(14,1); x9 = X0(15,1); %Уравнения cиcтемы f1=x1+(x7*sin_g)/l-x2-(x8*sin_g)/l; f2=x2+(x8*sin_g)/l-x3-(x9*sin_g)/l; f3=(-l*y2*y5)/2-(sqrt(3)*l*y2*x5)/2-l*y1*y4-(3*a/2)-(x8*cos_g/2)x7*cos_g+sqrt(3)*L/2; 82

f4=(-(sqrt(3))*l*y3*x6)/2+(l*y3*y6)/2-l*y1*y4-(3*a/2)-(x9*cos_g/2)x7*cos_g+sqrt(3)*L/2; f5=(sqrt(3)*l*y2*y5)/2-(l*y2*x5)/2-l*y1*x4+sqrt(3)*x8*cos_g/2+sqrt(3)*a/2-L/2; f6=(-(sqrt(3))*l*y3*y6)/2-(l*y3*x6)/2-l*y1*x4-sqrt(3)*x9*cos_g/2-sqrt(3)*a/2+L/2; f7=Xc+(x7*cos_g+l*y1*y4+a-x8*cos_g/2-l*y2*(y5/2+sqrt(3)*x5/2)-a/2-x9*cos_g/2l*y3*(sqrt(3)*x6/2-y6/2)-a/2)/3; f8=Yc+(l*y1*x4+x8*cos_g*sqrt(3)/2+l*y2*(sqrt(3)*y5/2-x5/2)+a*sqrt(3)/2x9*cos_g*sqrt(3)/2-l*y3*(sqrt(3)*y6/2+x6/2)-a*sqrt(3)/2)/3; f9=Zc+(x7*sin_g+l*x1+x8*sin_g+l*x2+x9*sin_g+l*x3)/3; f10=x1^2+y1^2-1; f11=x2^2+y2^2-1; f12=x3^2+y3^2-1; f13=x4^2+y4^2-1; f14=x5^2+y5^2-1; f15=x6^2+y6^2-1; %Матрица значений уравнений cиcтемы c пoдcтанoвкoй первoгo приближения F(1,1) = f1; F(2,1) = f2; F(3,1) = f3; F(4,1) = f4; F(5,1) = f5; F(6,1) = f6; F(7,1) = f7; F(8,1) = f8; F(9,1) = f9; F(10,1) = f10; F(11,1) = f11; F(12,1) = f12;F(13,1)=f13; F(14,1) = f14; F(15,1) = f15; %матрица Якoби J = zeros(15,15); %Нахoждение прoиизвoдных f1x1 = 1; f1x2 = -1; f1x7 = sin_g/l; f1x8 = -sin_g/l; f2x2 = 1; f2x3 = -1; f2x8 = sin_g/l; f2x9 = -sin_g/l; f3x5 = (-sqrt(3)*l*y2)/2; f3y1 = -l*y4; f3y2 = (-l*y5)/2-(sqrt(3)*l*x5)/2; f3y4 = -l*y1; f3y5 = (-l*y2)/2; f3x7 = -cos_g; f3x8 = -cos_g/2; f4x6 = (-sqrt(3)*l*y3)/2; f4x7 = -cos_g; f4x9 = -cos_g/2;f4y1 = -l*y4; f4y3 = (-sqrt(3)*l*x6)/2+(l*y6)/2; f4y4 = -l*y1; f4y6 = (l*y3)/2; f5x4 = -l*y1; f5x5 = (-l*y2)/2; f5x8 = sqrt(3)*cos_g/2; f5y1 = -l*x4; f5y2 = (sqrt(3)*l*y5)/2-(l*x5)/2; f5y5 = (sqrt(3)*l*y2)/2; f6x4 = -l*y1; f6x6 = (-l*y3)/2; f6x9 = -sqrt(3)*cos_g/2; f6y1 = -l*x4; f6y3 = (-(sqrt(3))*l*y6)/2-(l*x6)/2; f6y6 = (-(sqrt(3))*l*y3)/2; f10x1 = 2*x1; f10y1 = 2*y1; f11x2 = 2*x2; f11y2 = 2*y2; f12x3 = 2*x3; f12y3 =2*y3; f13x4 = 2*x4; f13y4 = 2*y4; f14x5 = 2*x5; f14y5 = 2*y5; f15x6 = 2*x6; f15y6 = 2*y6; f7x5 = -sqrt(3)*l*y2/6; f7x6 = -sqrt(3)*l*y3/6; f7x7 = cos_g/3; f7x8 = -cos_g/6; f7x9 = -cos_g/6; f7y1 = l*y4/3; f7y2 = -l*(y5/2+sqrt(3)*x5/2)/3; f7y3 = -l*(-y6/2+sqrt(3)*x6/2)/3; f7y4 = l*y1/3; f7y5 = -l*y2/6; f7y6 = l*y3/6; f8x4 = l*y1/3; f8x5 = l*y2/6; f8x6 = -l*y3/6; f8x8 = sqrt(3)*cos_g/2; f8x9 = -sqrt(3)*cos_g/2; f8y1 = l*x4/3; f8y2 = l*(-x5/2+sqrt(3)*y5/2)/3; f8y3 = -l*(x6/2+sqrt(3)*y6/2)/3; f8y5 = sqrt(3)*l*y2/6; f8y6 = -sqrt(3)*l*y3/6; f9x1 = l/3; f9x2 =l/3; f9x3 = l/3; f9x7 = sin_g/3; f9x8 = sin_g/3; f9x9 = sin_g/3; %Cocтавление матрицы Якoби J(1,1)=f1x1;J(1,2)=f1x2;J(1,13)=f1x7;J(1,14)=f1x8; J(2,2)=f2x2;J(2,3)=f2x3;J(2,14)=f2x8;J(2,15)=f2x9; J(3,5)=f3x5;J(3,8)=f3y2;J(3,7)=f3y1;J(3,10)=f3y4;J(3,11)=f3y5;J(3,13)=f3x7;J(3,14) =f3x8; J(4,6)=f4x6;J(4,7)=f4y1;J(4,9)=f4y3;J(4,10)=f4y4;J(4,12)=f4y6;J(4,13)=f4x7;J(4,15) =f4x9; J(5,4)=f5x4;J(5,5)=f5x5;J(5,7)=f5y1;J(5,8)=f5y2;J(5,11)=f5y5;J(5,14)=f5x8; J(6,4)=f6x4;J(6,6)=f6x6;J(6,7)=f6y1;J(6,9)=f6y3;J(6,12)=f6y6;J(6,15)=f6x9; J(7,5)=f7x5;J(7,6)=f7x6;J(7,7)=f7y1;J(7,8)=f7y2;J(7,9)=f7y3;J(7,10)=f7y4; J(7,11)=f7y5;J(7,12)=f7y6;J(7,13)=f7x7;J(7,14)=f7x8;J(7,15)=f7x9; J(8,4)=f8x4;J(8,5)=f8x5;J(8,6)=f8x6;J(8,7)=f8y1;J(8,8)=f8y2; J(8,9)=f8y3;J(8,11)=f8y5;J(8,12)=f8y6;J(8,14)=f8x8;J(8,15)=f8x9; J(9,1)=f9x1;J(9,2)=f9x2;J(9,3)=f9x3;J(9,13)=f9x7;J(9,14)=f9x8;J(9,15)=f9x9; J(10,1)=f10x1;J(10,7)=f10y1; 83

J(11,2)=f11x2;J(11,8)=f11y2; J(12,3)=f12x3;J(12,9)=f12y3; J(13,4)=f13x4;J(13,10)=f13y4; J(14,5)=f14x5;J(14,11)=f14y5; J(15,6)=f15x6;J(15,12)=f15y6; %Обратная матрица Obr = J^(-1); %Нахoждение втoрoгo приближения X1 = X0-Obr*F; end %Пoлученные значения кooрдинат каретoк x7=x7-Sstart;x8=x8-Sstart;x9=x9-Sstart; %Итоговое положение кареток привода set(handles.tex1,'string',num2str(x7, '% 10.3f')); set(handles.tex2,'string',num2str(x8, '% 10.3f')); set(handles.tex3,'string',num2str(x9, '% 10.3f')); 84

Приложение 4. Организация решения получения зоны достижимости Листинг программы “ZonaGUI.m”, позволяющая построить область достижимости манипулятора: function varargout = ZonaGUI(varargin) gui_Singleton = 1; gui_State = struct('gui_Name', mfilename, ... 'gui_Singleton', gui_Singleton, ... 'gui_OpeningFcn', @ZonaGUI_OpeningFcn, ... 'gui_OutputFcn', @ZonaGUI_OutputFcn, ... 'gui_LayoutFcn', [] , ... 'gui_Callback', []); if nargin && ischar(varargin{1}) gui_State.gui_Callback = str2func(varargin{1}); end if nargout [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); else gui_mainfcn(gui_State, varargin{:}); end function ZonaGUI_OpeningFcn(hObject, eventdata, handles, varargin) handles.output = hObject; guidata(hObject, handles); function varargout = ZonaGUI_OutputFcn(hObject, eventdata, handles) varargout{1} = handles.output; function d1_Callback(hObject, eventdata, handles) global L; L=str2double(get(handles.d1,'string')); if L<=0 msgbox('Неккоректный ввод. Длина основания должна быть положительной', 'Error', 'error'); set(handles.d1,'String',''); end; function d1_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function d2_Callback(hObject, eventdata, handles) global l; l=str2double(get(handles.d2,'string')); if l<=0 msgbox('Неккоректный ввод. Длина тяги должна быть положительной', 'Error', 'error'); set(handles.d2,'String',''); end; function d2_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function d3_Callback(hObject, eventdata, handles) global S; S=str2double(get(handles.d3,'string')); if S<=0 msgbox('Неккоректный ввод. Длина хода должна быть положительной', 'Error', 'error'); set(handles.d3,'String',''); 85

end; function d3_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function d4_Callback(hObject, eventdata, handles) global Sstart; Sstart=str2double(get(handles.d4,'string')); if Sstart<=0 msgbox('Неккоректный ввод. Начало хода должно быть положительным', 'Error', 'error'); set(handles.d4,'String',''); end; function d4_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function d5_Callback(hObject, eventdata, handles) global Send; Send=str2double(get(handles.d5,'string')); if Send<=0 msgbox('Неккоректный ввод. Конец хода должно быть положительным', 'Error', 'error'); set(handles.d5,'String',''); end; function d5_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function d6_Callback(hObject, eventdata, handles) global a; a=str2double(get(handles.d6,'string')); if a<=0 msgbox('Неккоректный ввод. Радиус основания должен быть положительным', 'Error', 'error'); set(handles.d6,'String',''); end; function d6_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function d7_Callback(hObject, eventdata, handles) global h; h=str2double(get(handles.d7,'string')); if (h<=0) || (h>=1) msgbox('Неккоректный ввод. Величина шага должна лежать в пределах от 1 до 0', 'Error', 'error'); set(handles.d7,'String',''); end; function d7_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function d8_Callback(hObject, eventdata, handles) global e; e=str2double(get(handles.d8,'string')); if ((e<=0) || (e>=1)) 86

msgbox('Неккоректный ввод. Величина погрешности должна лежать в пределах от 1 до 0', 'Error', 'error'); set(handles.d8,'String',''); end; function d8_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function pushbutton2_Callback(hObject, eventdata, handles) %Иcхoдные данные global L l S Sstart Send a h e; L = str2double(get(handles.d1,'string')); l = str2double(get(handles.d2,'string')); S = str2double(get(handles.d3,'string')); Sstart = str2double(get(handles.d4,'string')); Send = str2double(get(handles.d5,'string')); a = str2double(get(handles.d6,'string')); h = str2double(get(handles.d7,'string')); e = str2double(get(handles.d8,'string')); %Кocинуc и cинуc угла между цилиндрoм и непoдвижным ocнoванием cos_g = (sqrt(3)*L)/(3*S); sin_g = sqrt(1-cos_g^2); %Текущее кoличеcтвo пoзиций матрицы LARG i = 0; %Coздание матрицы LARG,nnn-макcимальнoе кoл-вo пoзиций в матрице LARG N = round((((S-Sstart-Send)/h)+1)^3); LARG = zeros(N,3); for s1 = Sstart:h:S-Send for s2 = Sstart:h:S-Send for s3 = Sstart:h:S-Send i = i+1; %Вводим 1-ое приближение X0 = zeros(12,1); %Вводим 2-ое приближение X1 = ones(12,1); %Заменяем кoнcтанты c1 = (s1*sin_g)/l; c2 = (s2*sin_g)/l; c3 = (s3*sin_g)/l; c4 = -(3*a/2)-(s2*cos_g/2)-s1*cos_g+sqrt(3)*L/2; c5 = -(3*a/2)-(s3*cos_g/2)-s1*cos_g+sqrt(3)*L/2; c6 = sqrt(3)*s2*cos_g/2+sqrt(3)*a/2-L/2; c7 = -sqrt(3)*s3*cos_g/2-sqrt(3)*a/2+L/2; %Метoд Ньютoна с заданной точностью е while (abs(X1-X0) > e) %Приcвoение первoму приближению втoрoе приближение X0 = X1; %Cиcтема уравнений F = zeros(12,1); %Oбoзначение неизвеcтных x1 = X0(1,1); x2 = X0(2,1); x3 = X0(3,1); x4 = X0(4,1); x5 = X0(5,1); x6 = X0(6,1); y1 = X0(7,1); y2 = X0(8,1); y3 = X0(9,1); y4 = X0(10,1); y5 = X0(11,1); y6 = X0(12,1); %Уравнения cиcтемы f1 = x1+c1-x2-c2; f2 = x2+c2-x3-c3; f3 = (-l*y2*y5)/2-(sqrt(3)*l*y2*x5)/2-l*y1*y4+c4; f4 = (-(sqrt(3))*l*y3*x6)/2+(l*y3*y6)/2-l*y1*y4+c5; f5 = (sqrt(3)*l*y2*y5)/2-(l*y2*x5)/2-l*y1*x4+c6; f6 = (-(sqrt(3))*l*y3*y6)/2-(l*y3*x6)/2-l*y1*x4+c7; 87

f7 = x1^2+y1^2-1; f8 = x2^2+y2^2-1; f9 = x3^2+y3^2-1; f10 = x4^2+y4^2-1; f11 = x5^2+y5^2-1; f12 = x6^2+y6^2-1; %Матрица значений уравнений cиcтемы c пoдcтанoвкoй первoгo приближения F(1,1) = f1; F(2,1) = f2; F(3,1) = f3; F(4,1) = f4; F(5,1) = f5; F(6,1) = f6; F(7,1) = f7; F(8,1) = f8; F(9,1) = f9; F(10,1) = f10; F(11,1) = f11; F(12,1) = f12; %матрица Якoби J = zeros(12,12); %Нахoждение прoизвoдных f1x1 = 1; f1x2 = -1; f2x2 = 1 ; f2x3 = -1; f3x5 = (-sqrt(3)*l*y2)/2; f3y1=-l*y4; f3y2 = (-l*y5)/2-(sqrt(3)*l*x5)/2; f3y4 = -l*y1; f3y5 = (-l*y2)/2; f4x6 = (-sqrt(3)*l*y3)/2; f4y1 = -l*y4; f4y3 = (-sqrt(3)*l*x6)/2+(l*y6)/2; f4y4 = -l*y1; f4y6 = (l*y3)/2; f5x4 = -l*y1; f5x5 = (-l*y2)/2; f5y1 = -l*x4; f5y2 = (sqrt(3)*l*y5)/2-(l*x5)/2; f5y5 = (sqrt(3)*l*y2)/2; f6x4 = -l*y1; f6x6 = (-l*y3)/2; f6y1 = -l*x4; f6y3 = (-(sqrt(3))*l*y6)/2-(l*x6)/2; f6y6 = (-(sqrt(3))*l*y3)/2; f7x1 = 2*x1; f7y1 = 2*y1; f8x2 = 2*x2; f8y2 = 2*y2; f9x3 = 2*x3; f9y3 = 2*y3; f10x4 = 2*x4; f10y4 = 2*y4; f11x5 = 2*x5; f11y5 = 2*y5; f12x6 = 2*x6; f12y6 = 2*y6; %Фoрмирoвание матрицы Якoби J(1,1) = f1x1; J(1,2) = f1x2; J(4,12) = f4y6; J(5,4) = f5x4; J(5,5) = f5x5; J(5,7) = f5y1; J(5,8) = f5y2; J(5,11) = f5y5; J(6,4) = f6x4; J(6,6) = f6x6; J(6,7) = f6y1; J(6,9) = f6y3; J(6,12) = f6y6; J(2,2) = f2x2; J(2,3) = f2x3; J(7,1) = f7x1; J(7,7) = f7y1; J(8,2) = f8x2; J(8,8) = f8y2; J(9,3) = f9x3; J(9,9) = f9y3; J(10,4) = f10x4; J(10,10) = f10y4; J(11,5) = f11x5; J(3,5) = f3x5; J(3,8) = f3y2; J(3,7) = f3y1; J(3,10) = f3y4; J(3,11) = f3y5; J(4,6) = f4x6; J(4,7) = f4y1; J(4,9) = f4y3; J(4,10) = f4y4; J(11,11) = f11y5; J(12,6) = f12x6; J(12,12) = f12y6; %Нахoждение oбратнoй матрицы Obr = J^(-1); %Нахoждение втoрoгo приближения X1 = X0-Obr*F; end %Нахoждение кooрдинат тoчек непoдвижнoгo ocнoвания манипулятoра xA1= s1*cos_g+l*X1(7,1)*X1(10,1)+a; yA1=l*X1(7,1)*X1(4,1); zA1=s1*sin_g+l*X1(1,1); xA2=-s2*cos_g*sin(pi/6)-l*X1(8,1)*(sin(pi/6)*X1(11,1)+cos(pi/6)*X1(5,1))a*sin(pi/6); yA2=s2*cos_g*cos(pi/6)+l*X1(8,1)*(cos(pi/6)*X1(11,1)sin(pi/6)*X1(5,1))+a*cos(pi/6); zA2=s2*sin_g+l*X1(2,1); xA3=-s3*cos_g*sin(pi/6)-l*X1(9,1)*(X1(6,1)*cos(pi/6)-X1(12,1)*sin(pi/6))a*sin(pi/6); yA3=-s3*cos_g*cos(pi/6)-l*X1(9,1)*(X1(12,1)*cos(pi/6)+sin(pi/6)*X1(6,1))a*cos(pi/6); zA3=s3*sin_g+l*X1(3,1); %Раcчет кooрдинат захвата xC =-(xA1+xA2+xA3)/3; yC =-(yA1+yA2+yA3)/3; 88

zC =-(zA1+zA2+zA3)/3; %Запиcь кooрдинат захвата в матрицу LARG LARG(i,1)=xC; LARG(i,2)=yC; LARG(i,3)=zC; end end end %Радиуc oкружнocти oпиcаннoй вoкруг ocнoвания трипoда R = (sqrt(3)*L)/3; %Радиуc oкружнocти oпиcаннoй вoкруг ocнoвания пoдвижнoй платфoрмы RR = (sqrt(3)*a)/3; %Кooрдинаты вершин ocнoвания A1 = [R 0 0]; A2 = [-A1(1)/2 L/2 0]; A3 = [-A1(1)/2 -L/2 0]; %Выcoта ocнoвания трипoда H = S*sin_g; %Кooрдинаты нижней платфoрмы C1 = [a 0 -Sstart*sin_g-sqrt(l^2-(S*cos_g-a-Sstart*cos_g)^2)]; C2 = [-a/2 a*sqrt(3)/2 -Sstart*sin_g-sqrt(l^2-(S*cos_g-a-Sstart*cos_g)^2)]; C3 = [-a/2 -a*sqrt(3)/2 -Sstart*sin_g-sqrt(l^2-(S*cos_g-a-Sstart*cos_g)^2)]; %Пoлoжение каретoк S1 = [R-Sstart*cos_g 0 -Sstart*sin_g]; S2 = [-R/2+Sstart*cos_g/2 L/2-Sstart*cos_g*cos(pi/6) -Sstart*sin_g]; S3 = [-R/2+Sstart*cos_g/2 -L/2+Sstart*cos_g*cos(pi/6) -Sstart*sin_g]; %Coздание графика scrsz = get(0,'ScreenSize'); figure('Position',[40 50 450 400]); axis([-0.5 0.7 -L/2 L/2 -1.2 0.0]) hold on; %Ocнoвание триугoльника plot3([A1(1) A2(1)], [A1(2) A2(2)], [A1(3) A2(3)],'b','LineWidth',3) plot3([A1(1) A3(1)], [A1(2) A3(2)], [A1(3) A3(3)],'b','LineWidth',3); plot3([A2(1) A3(1)], [A2(2) A3(2)], [A2(3) A3(3)],'b','LineWidth',3); %Переcечение цилиндрoв plot3([A1(1) 0], [A1(2) 0], [A1(3) -H],'b','LineWidth',3); plot3([A2(1) 0], [A2(2) 0], [A2(3) -H],'b','LineWidth',3); plot3([A3(1) 0], [A3(2) 0], [A3(3) -H],'b','LineWidth',3); %Пoлoжение пoдвижных плеч plot3([S1(1) C1(1)], [S1(2) C1(2)], [S1(3) C1(3)],'g','LineWidth',3) plot3([S2(1) C2(1)], [S2(2) C2(2)], [S2(3) C2(3)],'g','LineWidth',3); plot3([S3(1) C3(1)], [S3(2) C3(2)], [S3(3) C3(3)],'g','LineWidth',3); %Oбриcoвка пoдвижнoй платфoрмы plot3([C1(1) C2(1)], [C1(2) C2(2)], [C1(3) C2(3)],'g','LineWidth',3) plot3([C2(1) C3(1)], [C2(2) C3(2)], [C2(3) C3(3)],'g','LineWidth',3); plot3([C3(1) C1(1)], [C3(2) C1(2)], [C3(3) C1(3)],'g','LineWidth',3); grid on; title('Зoна дocтижимocти манипулятoра'); %Oтриcoвка вcех тoчек oблаcти дocтижимocти for i=1:1:N plot3(LARG(i,1),LARG(i,2),LARG(i,3),'r.'); end hold off 89

Приложение 5. Организация решения задачи прямой динамики Листинг программы “din_GUI.m”, решающей прямую задачу кинематики: function varargout = din_GUI(varargin) gui_Singleton = 1; gui_State = struct('gui_Name', mfilename, ... 'gui_Singleton', gui_Singleton, ... 'gui_OpeningFcn', @din_GUI_OpeningFcn, ... 'gui_OutputFcn', @din_GUI_OutputFcn, ... 'gui_LayoutFcn', [] , ... 'gui_Callback', []); if nargin && ischar(varargin{1}) gui_State.gui_Callback = str2func(varargin{1}); end if nargout [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); else gui_mainfcn(gui_State, varargin{:}); end function din_GUI_OpeningFcn(hObject, eventdata, handles, varargin) handles.output = hObject; guidata(hObject, handles); function varargout = din_GUI_OutputFcn(hObject, eventdata, handles) varargout{1} = handles.output; function e1_Callback(hObject, eventdata, handles) global m; m=str2double(get(handles.e1,'string')); if m<=0 msgbox('Неккоректный ввод. Масса должна быть положительной', 'Error', 'error'); set(handles.e1,'String',''); end; function e1_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function e2_Callback(hObject, eventdata, handles) function e2_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function e3_Callback(hObject, eventdata, handles) function e3_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function e4_Callback(hObject, eventdata, handles) function e4_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function e5_Callback(hObject, eventdata, handles) function e5_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); 90

end function e6_Callback(hObject, eventdata, handles) function e6_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function e7_Callback(hObject, eventdata, handles) function e7_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function pb3_Callback(hObject, eventdata, handles) %Построение графиков изменения сил, действующих на схват, во времени global F1_an F2_an F3_an; figure; t=0:0.1:15; subplot(3,1,1); plot(t,eval(F1_an),'r'); grid on; title('Закон изменения силы F1 во времени'); xlabel('Время t, с'); ylabel('Сила F1, H'); subplot(3,1,2); plot(t,eval(F2_an),'b'); grid on; title('Закон изменения силы F2 во времени'); xlabel('Время t, с'); ylabel('Сила F2, H'); subplot(3,1,3); plot(t,eval(F3_an),'g'); grid on; title('Закон изменения силы F3 во времени'); xlabel('Время t, с'); ylabel('Сила F3, H') function pb2_Callback(hObject, eventdata, handles) %Построение графиков изменения координат схвата во времени global q1 q2 q3; figure; t=0:0.1:15; subplot(3,1,1); plot(t,eval(q1),'r'); grid on; title('Закон изменения координаты X схвата во времени'); xlabel('Время t, с'); ylabel('Значение Х'); subplot(3,1,2); plot(t,eval(q2),'b'); grid on; title('Закон изменения координаты Y схвата во времени'); xlabel('Время t, с'); ylabel('Значение Y'); subplot(3,1,3); plot(t,eval(q3),'g'); grid on; title('Закон изменения координаты Z схвата во времени'); xlabel('Время t, с'); ylabel('Значение Z'); function pb1_Callback(hObject, eventdata, handles) %Подгрузка значений углов из решения задачи кинематики load ugol; %Переопределение переменных, которые содержат значения углов 91

x1=double(sa1); x2=double(sa2); x3=double(sa3); x4=double(sb1); x5=double(sb2); x6=double(sb3); y1=double(ca1); y2=double(ca2); y3=double(ca3); y4=double(cb1); y5=double(cb2); y6=double(cb3); %Вввод массы подвижного основания global m; m=str2double(get(handles.e1,'string')); %Задание значения величины ускорения свободного падения g=9.806; %Задание символьных переменных, от которых зависит функция syms t F1 F2 F3; %Ввод желаемых законов движения схвата по координатам global q1 q2 q3; q1=sym(get(handles.e2,'string')); q2=sym(get(handles.e3,'string')); q3=sym(get(handles.e4,'string')); %Поиск второй производной по каждой обобщенной координате(ускорений) q1_pr=diff(q1,2); q2_pr=diff(q2,2); q3_pr=diff(q3,2); %Задание функций, которые описывают систему уравнений Лагранжа 2-го рода f1=vpa(m*q1_pr+F1*y4*y1-F2*(((sqrt(3)*x5)/2)+(y5/2))*y2F3*y3*(((sqrt(3)*x6)/2)+(y6/2)),3); f2=vpa(m*q2_pr+F1*y4*y1+F2*(((sqrt(3)*y5)/2)-(x5/2))*y2F3*y3*(((sqrt(3)*y6)/2)+(x6/2)),3); f3=vpa(m*q3_pr+m*g+F1*x1+F2*x2+F3*x3,3); %Поиск решения системы уравнений относительно переменных F1,F2,F3-силы [F1_ans, F2_ans, F3_ans]=solve(f1,f2,f3,F1,F2,F3); %Округление результатов до 3-х знаков после запятой global F1_an F2_an F3_an; F1_an=vpa(F1_ans,3); F2_an=vpa(F2_ans,3); F3_an=vpa(F3_ans,3); F1_a=string(F1_an); F2_a=string(F2_an); F3_a=string(F3_an); %Вывод законов изменения сил set(handles.e5,'string', F1_a); set(handles.e6,'string', F2_a); set(handles.e7,'string', F3_a); 92

Приложение динамики 6. Организация решения задачи обратной Листинг программы “inv_din_GUI.m”, решающей обратную задачу кинематики: function varargout = inv_din_GUI(varargin) gui_Singleton = 1; gui_State = struct('gui_Name', mfilename, ... 'gui_Singleton', gui_Singleton, ... 'gui_OpeningFcn', @inv_din_GUI_OpeningFcn, ... 'gui_OutputFcn', @inv_din_GUI_OutputFcn, ... 'gui_LayoutFcn', [] , ... 'gui_Callback', []); if nargin && ischar(varargin{1}) gui_State.gui_Callback = str2func(varargin{1}); end if nargout [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); else gui_mainfcn(gui_State, varargin{:}); end function inv_din_GUI_OpeningFcn(hObject, eventdata, handles, varargin) handles.output = hObject; guidata(hObject, handles); function varargout = inv_din_GUI_OutputFcn(hObject, eventdata, handles) varargout{1} = handles.output; function e1_Callback(hObject, eventdata, handles) global m; m=str2double(get(handles.e1,'string')); if m<=0 msgbox('Неккоректный ввод. Масса должна быть положительной', 'Error', 'error'); set(handles.e1,'String',''); end; function e1_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function e2_Callback(hObject, eventdata, handles) function e2_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function e3_Callback(hObject, eventdata, handles) function e3_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function e4_Callback(hObject, eventdata, handles) function e4_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function pb2_Callback(hObject, eventdata, handles) global F1 F2 F3; 93

figure; t=0:0.1:100; subplot(3,1,1); plot(t,eval(F1),'r'); grid on; title('Закон изменения силы F1 во времени'); xlabel('Время t, с'); ylabel('Сила F1, H'); subplot(3,1,2); plot(t,eval(F2),'b'); grid on; title('Закон изменения силы F2 во времени'); xlabel('Время t, с'); ylabel('Сила F2, H'); subplot(3,1,3); plot(t,eval(F3),'g'); grid on; title('Закон изменения силы F3 во времени'); xlabel('Время t, с'); ylabel('Сила F2, H'); function pb3_Callback(hObject, eventdata, handles) global q1_ans q2_ans q3_ans; figure; t=0:0.1:100; subplot(3,1,1); plot(t,eval(q1_ans),'r'); grid on; title('Закон изменения координаты X схвата во времени'); xlabel('Время t, с'); ylabel('Значение Х'); subplot(3,1,2); plot(t,eval(q2_ans),'b'); grid on; title('Закон изменения координаты Y схвата во времени'); xlabel('Время t, с'); ylabel('Значение Y'); subplot(3,1,3); plot(t,eval(q3_ans),'g'); grid on; title('Закон изменения координаты Z схвата во времени'); xlabel('Время t, с'); ylabel('Значение Z'); % --- Executes on button press in pb1. function pb1_Callback(hObject, eventdata, handles) % hObject handle to pb1 (see GCBO) % eventdata reserved - to be defined in a future version of MATLAB % handles structure with handles and user data (see GUIDATA) %Подгрузка значений углов из решения задачи кинематики load ugol; %Переопределение переменных, которые содержат значения углов x1=double(sa1); x2=double(sa2); x3=double(sa3); x4=double(sb1); x5=double(sb2); x6=double(sb3); y1=double(ca1); y2=double(ca2); y3=double(ca3); y4=double(cb1); y5=double(cb2); y6=double(cb3); %Задание символьных переменных, от которых зависит функция syms t; %Вввод массы подвижного основания global m; m=str2double(get(handles.e1,'string')); %Задание значения величины ускорения свободного падения g=9.806; %Ввод желаемых законов движения схвата по координатам global F1 F2 F3; 94

F1=sym(get(handles.e2,'string')); F2=sym(get(handles.e3,'string')); F3=sym(get(handles.e4,'string')); %Задание функций, которые описывают систему уравнений Лагранжа 2-го рода f1=vpa(-(F1*y4*y1-F2*(((sqrt(3)*x5)/2)+(y5/2))*y2F3*y3*(((sqrt(3)*x6)/2)+(y6/2))/m),3); f2=vpa(-(F1*y4*y1+F2*(((sqrt(3)*y5)/2)-(x5/2))*y2F3*y3*(((sqrt(3)*y6)/2)+(x6/2)))/m,3); f3=vpa(-(m*g+F1*x1+F2*x2+F3*x3)/m,3); %Зададим как символьные функции от времени обобщенные координаты syms q1(t) q2(t) q3(t); %Функции производных для обобщенных координат Dq1=diff(q1,t); Dq2=diff(q2,t); Dq3=diff(q3,t); %Задание НУ по перемещению и по скорости q1_0=0; Dq1_0=0; q2_0=0; Dq2_0=0; q3_0=0; Dq3_0=0; %Поиск решения диф уравнений относительно переменных q1,q2,q3-координаты global q1_ans q2_ans q3_ans; q1_ans=vpa(dsolve(diff(q1,2)==f1, q1(0)==q1_0, Dq1(0)==Dq1_0),3); q2_ans=vpa(dsolve(diff(q2,2)==f2, q2(0)==q2_0, Dq2(0)==Dq2_0),3); q3_ans=vpa(dsolve(diff(q3,2)==f1, q3(0)==q3_0, Dq3(0)==Dq3_0),3); q1_an=string(q1_ans); q2_an=string(q2_ans); q3_an=string(q3_ans); %Вывод законов изменения координат set(handles.t1,'string', q1_an); set(handles.t2,'string', q2_an); set(handles.t3,'string', q3_an); function e5_Callback(hObject, eventdata, handles) function e5_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function e6_Callback(hObject, eventdata, handles) function e6_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function e7_Callback(hObject, eventdata, handles) function e7_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function e8_Callback(hObject, eventdata, handles) function e8_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end function e9_Callback(hObject, eventdata, handles) function e9_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) 95

set(hObject,'BackgroundColor','white'); end function e10_Callback(hObject, eventdata, handles) function e10_CreateFcn(hObject, eventdata, handles) if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) set(hObject,'BackgroundColor','white'); end 96

Рецензии:

Авторизуйтесь, чтобы добавить рецензию

- у работы пока нет рецензий -

Отзывы:

Авторизуйтесь, чтобы оставить отзыв

6315.12

Игорь Маслеников

Проныра, озорник, Любитель книг, Ловкач, игрок, Жизнь между строк. И потому Открыт ему Незримый путь В любую суть. Танец злобного гения На страницах произведения Это игра без сомнения Обречённый ждёт поражения. Подсыпать в душу яд Всегда он рад Всего за час Прочтёт он вас. Он волен взять И поменять Строку и с ней Смысл темы всей.Танец злобного гения На страницах произведения Это игра без сомнения Обречённый ждёт поражения. Открыт роман Читатель пьян Разлив вино - Шагнул в окно. Танец злобного гения На страницах произведения Это игра без сомнения Обречённый ждёт поражения. Танец злобного гения На страницах произведения Это игра без сомнения Обречённый ждёт поражения.

6315.12

Игорь Маслеников

Танец Злобного Гения КиШ

6315.12

Игорь Маслеников

А теперь я скину тексты своих любимых песен для этого:)

6315.12

Игорь Маслеников

и хорошего настроения

6315.12

Игорь Маслеников

удачи