Моделирование процесса нанесения краски устройством с применением робота Kawasaki

Введение

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

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

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

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

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

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

Для достижения поставленной цели необходимо решить следующие задачи:

·        Прямую задачу кинематики манипуляционного робота;

·        Обратную задачу кинематики манипуляционного робота;

·        Задачу позиционирования захвата манипуляционного робота;

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

·        Получить ряд траекторий в рабочей зоне установки для нанесения краски;

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

1. Обзор современных робототехнических комплексов

1.1 Определения работа

Рис. 1

Робот (чеш. robot, от robota - подневольный труд, rob - раб), машина с антропоморфным (человекоподобным) поведением, которая частично или полностью выполняет функций человек (иногда животного) при взаимодействии с окружающим мира. Термин "робот" (Р) был впервые введён К. Чапеком <#"655982.files/image002.gif"> <#"655982.files/image003.gif">

Рис. 3

Гонконгская компания WowWee (широко известная по андроиду Robosapien) представила на международной выставке бытовой электроники (2008 International CES) самоходную вебкамеру-робот. Новинка называется Rovio. Она симпатична, "умна" и обладает рядом полезных навыков.

Робот оснащенную WiFi, и может контролироваться удаленно при помощи устройства с доступом в Интернет - мобильными телефоном, ПК или игровой видеоконсолью. Пользователи могут удаленно наблюдать за своим домом или офисом при помощи камеры встроенной в роботе, который передает видео через Интернет.

Кроме просмотра видео микрофон робота позволяет прослушивать помещение, в котором он находится.

Рис.4

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

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

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

Рис. 5

Помимо связи Wi-Fi, робот оснащён USB-портом. Причём настройка этой машинки-охранника облегчается её способностью определять при подключении к компьютеру пользователя настройки PC.

При цене менее 1000$ этот робот становится недорогой альтернативой целой когорте роботов-охранников самых разных размеров и с самыми разными возможностями, которые мы уже видели в действии.

.4 Робот для хирургии

Вероятность трансляции внутреннего изображения тела пациента позволит, по мнению специалистов NASA (Американского космического агентства) оказывать астронавтам медицинскую помощь непосредственно в космосе. Для этого поручили компании Intuitive Surgical разработку такого аппарата, который смог бы заменить манипуляции хирурга, находящегося на Земле. В итоге появился хирургический робот да Винчи - названный в честь великого ученого и гениального художника, который с невероятной точностью первым отобразил структуру тела человека и строение его внутренних органов. И хотя NASA уже больше не запускает космические челноки, но да Винчи робот стал жизненно необходимым инструментом при проведении многих медицинских операций.

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

Рис. 6

В этом же году была совершена первая успешная операция на головном мозге с помощью робота.

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

А уже в 1987 году - проведена успешная лапароскопическая операция с использованием робота - удаление желчного пузыря (холецистэктомия). В следующем году-с той же системой PUMA была проведена трансуретральная резекция.

Итак, использование роботов дало возможность установить 2 исключительных направления в медицине:

)        телехирургия, где хирург при проведении операции сам руководит роботом, не контактируя непосредственно с пациентом, т.е. хирург оперирует руками робота.

)        хирургия с минимальным (малоинвазивным) вмешательством.

Такое применение роботов для различных операций позволяет буквально оперировать«без следа».

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

Такое определение «роботизированная операция» может любых больных ввести в заблуждение.

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

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

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

Изобретение и внедрение роботизированного аппарата да Винчи, позволяющего проводить операции дистанционно - истинный прорыв в современной медицине.

Рис. 7

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

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

Рис. 8

Благодаря аппарату да Винчи лапароскопическая простатэктомия стала наиболее безопасной и простой.

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

Основными преимуществами роботизированной хирургии являются:

наименьший риск осложнений, свойственных классической хирургии;

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

понижение необходимости в переливании крови;

уменьшение риска попадания инфекции в раны;

после операции минимальная болезненность;

усиленный косметический эффект из-за отсутствия

послеоперационных больших шрамов;

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

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

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

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

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

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

Таким образом, при оперировании риск сводится к нулю и у пациента почти не остается никаких послеоперационных рубцов.

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

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

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

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

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

Рис. 9

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

Да Винчи робот-манипулятор обеспечен инструментами, подобные человеческому запястью (EndoWrist®), которые на много превосходят ловкость руки человека.

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

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

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

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

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

Компания Smith & Nephews в тесном сотрудничестве с URS Orthopedic Systems написали компьютерную программу, применяемую системой да Винчи при внедрении имплантов в коленный сустав.

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

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

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

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

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

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

Если консоль управления выучиться воспроизводить чувство «ощущения», характерное при проведении классической открытой операции, врач-хирург будет иметь возможность пользоваться преимуществами обоих операционных способов: "ощущение" и "обзор".

Да Винчи робот одними из первых на континенте и первыми в Израиле начали использовать в Иерусалиме медицинском центре Хадасса. В настоящее время его уже так же применяют в медицинских центрах Шиба и РАМБАМ.

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

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

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

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

.5 Робот space system (Космические)

Космические исследования и размещение лазерного оружия - самое перспективное направление в развитии космической робототехники.

Еще с 50-х годов прошлого века роботы и автоматические станции начали осваивать солнечную систему.

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

Сегодня же - это уже совсем другое их поколение. А завтра они полностью могут исключить человека в наиболее тяжелых исследованиях космического пространства. Вот о них и поговорим.

Одним из самых ярких представителей полных по своей автоматизации и интеллекту космических роботов являлся загубленный из-за перестроечного дуризма корабль-челнок "Буран" советского производства.

Рис. 10

Этот корабль еще в 80-е годы показал, чего можно добиться с помощью роботов.

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

Если бы не бездарная политика руководства страны в тот период, мы могли бы быть лидерами в этой области. «Буран» задумывался как военная система.

Тактико-техническое задание на разработку многоразовой космической системы выдано Главным управлением космических средств Министерства обороны СССР и утверждено Д. Ф. Устиновым 8 ноября 1976 года. «Буран» предназначался для:

комплексного противодействия мероприятиям вероятного противника по расширению использования космического пространства в военных целях;

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

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

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

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

Рис. 11

Что еще важнее, потеря автоматического исследователя гораздо предпочтительнее гибели астронавта, хотя разработка и производство киберов - занятие недешевое.

После "золотой эры" беспилотных исследований, когда зонды из СССР и США бороздили космические просторы Солнечной системы и проводили наблюдения на поверхности Луны, Венеры и Марса, мало уже кто сомневался в том, что автоматические исследовательские аппараты ждет большое будущее.

Рис. 12

Весьма скоро, в конце декабря этого года, посадочный модуль "Гюйгенс" отделится от аппарата "Кассини", чтобы впервые прилуниться на крупнейшем в Солнечной системе планетоиде Титане.

Американские марсоходы Spirit и Opportunity уже доказали, что автоматам по силам исследовательские миссии чрезвычайной сложности, но киберпомощников конструируют не только в NASA.

В научно-техническом центре Европейского космического агентства в Нидерландах (ESTEC) ведется активная работа по созданию автоматических помощников астронавтов, призванных заменить дорогостоящие пилотируемые экспедиции рачительными миссиями роботов.

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

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

Тем не менее, к автоматам для работы в безвоздушном пространстве предъявляются некоторые особые требования. Они должны:

*перенести запуск

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

* весить как можно меньше, так как каждый килограмм, выведенный на орбиту, стоит дорого

* потреблять мало энергии и иметь долгий срок службы

* работать в автоматическом режиме

* обладать чрезвычайной надежностью

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

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

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

Самые распространенные из автоматических аппаратов,

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

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

поверхности другой планеты, неся на борту научные приборы. Как

правило, и сам ровер, и научное оборудование на нем функционируют в автоматическом режиме.

Европейское космическое агентство в сотрудничестве с некоторыми промышленными концернами разработало необычайно мелкий микроровер Nanokhod ("Наноход").

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

Более крупный робот был спроектирован для сбора образцов грунта других планет.

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

Третий минировер ,разрабатывающийся в ЕКА - пятнадцатикилограммовый Solero, все энергопотребности которого обеспечиваются солнечной батареей и миниатюрными подзаряжаемыми аккумуляторами. Данный аппарат имеет принципиально новую конструкцию шасси: шесть колес, расположенных по вершинам шестиугольника, обеспечивают ему отличную проходимость.

Рис. 13

Конструкторы роботов черпают вдохновение в творениях природы. Хорошим примером тому служит автомат Aramies/Scorpion, разработкой которого также заведует Европейское космическое агентство. Восемь ног позволяют киберу передвигаться подобно скорпиону по очень пересеченной местности и песчаным дюнам.

Еще одним примером воплощения в разработках идей, позаимствованных у природы, является EUROBOT.

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

Не обошлось без природы и при создании прыгающего робота.

При размерах даже меньше сорока сантиметров такой автомат способен перепрыгивать препятствия высотой в два метра.

Подобное практически неосуществимо на Земле, с ее силой тяжести, зато вполне возможно на Луне или Марсе.

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

Рис. 14

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

Потому космороботы и создаются такими самостоятельными , способными работать без связи с Землей и справляться , по возможности , с любыми возникающими при выполнении миссии проблемами.

2. Формальная постановка задачи

.1 Описание задачи

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

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

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

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

.2 Описание рабочей системы

В качестве манипулятора используется робот фирмы Kawasaki - FS03N (рисунок 2.1). Управление роботом осуществляется через контроллер. К контроллеру может быть подключён пульт управления и компьютер (рисунок 2.2).

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

Рис. 2.1. Робот-манипулятор Kawasaki FS03N.

Контроллер - это центр управления всей системой, осуществляющий ежедневные операции.

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

·        Ручное управление роботом

·        Обучение данных позиции (координат)

·        Обучение вспомогательных данных (блочное программирование)

Рис. 2.2. Система управления роботом.

·        Установка условий повторения

·        Управление сигналами

·        Выбор программы

·        Отображение программных шагов и имён

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

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

Программа для робота-манипулятора пишется на языке AS. Фактически, эта программа должна осуществить движение манипулятора по определённой траектории. Соответствующая траектория в программе обычно задаётся в виде массива позиций, через которые проходит центр схвата робота (TCP - tool center point). В языке используется термин «позиция» так как этот термин выбран в стандарте ISO, фактически же позицией является совокупность трёх координат конца центра схвата (TCP), а также трёх эйлеровых углов, определяющих векторы ориентации и подхода (ориентацию схвата в пространстве относительно базовой системы координат). В языке AS позиция может быть описана двумя способами: либо как последовательность из трёх декартовых координат и трёх углов, определяющих положение свата в пространстве, т.е. в декартовых координатах, либо при помощи шести углов, возникающих между цепями манипулятора (угловые координаты или обобщённые координаты), которые также определяют положение захвата. Фактически управление роботом осуществляется посредством указания этих углов (рисунок 2.3.). Если для описания позиции используется положение захвата, то эта позиция всё равно неявно преобразуется автоматически к системе из шести углов при помощи решения обратной задачи кинематики, встроенной во внутреннюю систему управления манипулятором.

звеньев с тремя вращательными степенями свободы.

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

Рис. 2.3. Перемещение манипулятора, состоящего из трёх

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

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

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

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

.4 Формулировка требований

В соответствии со сформулированными сценариями управления движением необходимо решить четыре задачи:

)        Прямая задача кинематики

)        Обратная задача кинематики

)        Позиционирование манипулятора

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

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

Осуществить всё это язык AS позволяет, так как в нём предусмотрена возможность выполнения алгоритмов в режиме реального времени во время движения манипулятора (состояние при котором манипулятор остановился, осуществляется при помощи инструкции BREAK или DELAY, которая программой воспринимается как особая форма движения).

3. Прямая задача кинематики

.1 Описание робота-манипулятора. Представление Денавита-Хартенберга

С точки зрения кинематики манипуляционный робот может рассматриваться как незамкнутая цепь твёрдых звеньев, соединённых последовательно посредством либо призматических (поступательных), либо шарнирных (вращательных) сочленений (рисунок 3.1.). Один конец этой цепи соединён с основанием манипулятора, другой её конец, как правило, снабжён захватным устройством. Способность осуществлять какие-либо движения достигается благодаря тому, что манипулятор обладает несколькими степенями свободы. Число степеней свободы это число возможных перемещений системы. Расположение и ориентация захватного устройства зависит от совместного действия вращения и/или переноса каждого сочленения цепи звеньев. В данной задаче используется шестизвенный робот-манипулятор с шестью вращательными степенями свободы, здесь и далее рассматривается манипулятор именно с такой конфигурацией.

Управление манипулятором представляет собой перемещение захвата в определённую точку с определённой ориентацией. Это перемещение осуществляется за счёт указания обобщённых координат.

Рис. 3.1. Шарнирное (вращательное) и призматическое соединения.

Обобщенными координатами являются измеряемые угловые перемещения между двумя соседними звеньями робота. С каждым звеном робота связана локальная декартова система координат. Начало систем координат совмещено с каждой степенью подвижности манипулятора (рисунок 3.2).

Рис. 3.2. Система локальных координат робота-манипулятора.

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

Каждая система координат формируется на основе трёх правил:

.        Ось z_i-1 направлена вдоль оси i-го сочленения.

.        Ось x_i перпендикулярна оси z_i-1 и направлена от неё.

.        Ось y_i дополняет оси x_i, z_i до правой декартовой системы координат.

Локальные системы декартовых координат выберем следующим специальным образом (в соответствии с представлением Денавита-Хартенберга): пронумеруем все звенья манипулятора от стойки (звено 0) до схвата (звена n, которое в рассматриваемом случае равно 6), ось z_i идет по оси кинематической пары (i, i+1); начало координат системы i, жестко связанной со звеном i, лежит на общем перпендикуляре к осям z_i-1 и z_i, либо в точке их пересечения, если таковая имеется, либо в любой точке оси кинематической пары, если ось z_i совпадает с осью z_i-1 или параллельна ей. Ось x_i идет по общему перпендикуляру, проведенному к осям z_i-1 и z_i, и направлена от точки пересечения этого перпендикуляра с осью z_i-1 к точке его пересечения с осью x_i (или в любую сторону по нормали к плоскости, содержащей оси z_i-1 и z_i, если они пересекаются, или произвольным способом, если z_i-1 и z_i идут по одной прямой). Ось y_i выбирается по правилу правой тройки векторов. Начало координат системы 0, т. е. системы, жестко связанной со стойкой, может лежать в любой точке оси пары (0, 1).

Выбор системы n тоже выпадает из общего правила, так как звено n+1 отсутствует. Поэтому предлагается вообразить любого типа пару (n, n+1) и после этого выбрать систему по общему правилу. Начало выбранной таким образом системы называется центром схвата.

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

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

Для разрешения этих вопросов, с целью описания вращательных и поступательных связей между соседними звеньями Денавит и Хартенберг предложили матричный метод построения систем координат, связанных с каждым звеном кинематической цепи. Смысл представления Денавита-Хартенберга состоит в формировании однородной матрицы преобразования, имеющей размерность 4х4 и описывающей положение системы координат каждого звена относительно системы координат предыдущего. Это даёт возможность последовательно преобразовать координаты схвата манипулятора из системы отсчёта, связанной с последним звеном, в базовую систему отсчёта [6].

Так как управление состоит в указании угла между двумя соседними звеньями - изменение этого угла приводит к изменению положения одной локальной системы координат относительно другой, что приводит к необходимости описывать каждую последующую локальную систему относительно предыдущей. Для описания изменения положения и ориентации одной локальной системы координат относительно предыдущей используются однородные матрицы преобразования (А-матрицы, эти матрицы также называют преобразованием Денавита-Хартенберга, рисунок 3.3).

Рисунок 3.3. Однородная матрица преобразования.

Матрица преобразования ставится в соответствие каждой локальной системе координат (кроме самой первой, что у стойки) и несёт двойной смысл: во-первых она описывает положение и ориентацию локальной системы координат (i-й) относительно предыдущей локальной системы координат (i-1, если нумеровать, начиная со стойки), а во-вторых является средством преобразования i-й координатной системы в i-1-ую, то есть координаты вектора, в i-й системе координат могут быть представлены в i-1-й путём умножения координат этого вектора на i-ю матрицу преобразования (А матрицу). Матрица А состоит из четырёх частей: 3х3 матрица, которая определяет с одной стороны ориентацию соответствующей локальной системы координат относительно предыдущей при помощи углов Эйлера (она состоит из трёх единичных векторов направления которых совпадают с направлениями соответствующих осей i-й локальной системы координат, эти векторы рассматриваются относительно i-1-й системы) и с другой стороны является матрицей поворота, 3х1 вектор с одной стороны является координатой i-й локальной системы относительно i-1-й и с другой стороны вектором переноса, последние две части - строка 1х3 определяет перспективу и один элемент, является коэффициентом масштабирования. Эти две части не используются. Вектор положения и коэффициент масштабирования вообще с точки зрения линейной алгебры можно воспринимать как единое целое - вектор, выраженный в однородных координатах. В более общем случае матрица А в линейной алгебре является однородной матрицей преобразования, которая преобразует вектор, выраженный в однородных координатах из одной системы отсчёта в другую.

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

θ_i - присоединённый угол (обобщённая координата) - угол, на который надо повернуть ось x_i-1 вокруг оси z_i-1, чтобы она стала со направлена с осью x_i (знак определяется в соответствии с правилом правой руки).

d_i - расстояние между пересечением оси z_i-1 с осью x_i и началом (i-1-й системы координат, отсчитываемое вдоль оси z_i-1).

a_i - линейное смещение - расстояние между пересечением оси z_i-1 с осью x_i и началом i-й системы координат, отсчитываемое вдоль оси x_i, т.е кратчайшее расстояние между осями z_i-1 и z_i.

α_i - угловое смещение - угол, на который надо повернуть ось z_i-1 вокруг оси x_i, чтобы она стала со направленной с осью z_i (знак определяется в соответствии с правилом правой руки).

Параметры a_i и α_i характеризуют конструкцию звена, их можно рассматривать соответственно как длину и угол скрутка i-го звена. Для вращательных сочленений d_i, a_i и α_i являются постоянными характеристиками сочленения для данного робота. θ_i является переменной величиной, изменяющейся при вращении i-го звена относительно i-1-го.

Как только системы координат сформированы для всех звеньев, можно построить однородные матрицы преобразования, связывающие i-ю и i-1-ю системы координат.

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

Рис. 3.4. Система координат звена.

1.      Поворот системы i-1вокруг оси z_i-1 на угол θ_i до тех пор, пока ось x_i-1 не станет параллельной оси x_i.

2.      Сдвиг повернутой системы вдоль оси z_i-1 на величину d_i до тех пор, пока оси x_i-1 и x_i не окажутся на одной прямой.

.        Сдвиг вдоль оси x_i на величину a_i до тех пор, пока не совпадут начала координат.

.        Поворот вокруг оси x_i на угол α_i до совмещения оси z_i-1 с осью z_i.

Каждому из этих элементарных движений соответствует одна из матриц: либо матрица вращения (В_вр), либо матрица сдвига (В_сд). Результирующая матрица перехода А_i, связывающая системы i-1 и i, является произведением этих матриц:

 (3.1)

После перемножения получаем:

 (3.2)

Таким образом, матрица преобразования зависит от четырёх параметров, один из которых (в данном случае обобщённая координата) меняется.

3.2 Прямая задача кинематики

Прямая задача кинематики манипуляторов формулируется следующим образом: задана кинематическая схема манипулятора (то есть параметры d_i, a_i и α_i для всех звеньев) и в некоторый момент времени известны значения обобщенных координат, определяющие положение всех звеньев манипулятора друг относительно друга. Требуется определить положение и ориентацию последнего звена манипулятора (схвата) в системе отсчета, связанной со стойкой.

Для того, чтобы получить координаты последней, шестой локальной системы координат относительно базовой необходимо провести последовательно цепочку преобразований локальных систем. Таким образом, кинематическое положение схвата может быть получено последовательным преобразованием координат точки Ri (rxi, ryi, rzi) из одной системы координат i в другую систему координат i-1 следующим образом:

=А1*R1·=А2*R2 =А3*R3 =А4*R4 =А5*R5 =А6*R6

и получим =====> R0= А1*А2*А3*А4*А5*А6*R6

Обозначим Т6= А1*А2*А3*А4*А5*А6 (3.3)

Таким образом, R₀= Т_6*R₆ (3.3)

Произведение матриц преобразования также даёт матрицу преобразования поэтому получается, что матрица Т₆ в итоге является матрицей А и имеет полностью такую же структуру, но в данном случае нас интересует рассмотрение её с точки зрения описания позиции схвата относительно базовой системы координат, а не как средство преобразования в базовую систему координат. Матрицу Т называют однородной матрицей композиции преобразования. В матрице Т вектор p является координатой центра схвата - это вектор положения, а единичные векторы, определяющие ориентацию схвата в базовой системе (эти же векторы можно рассматривать как направляющие косинусы) называются соответственно векторами подхода - а, ориентации - о, и нормали - n, все они образуют правостороннюю систему координат (рисунок 3.5). Так как каждый из этих векторов может быть получен в результате векторного произведения двух других, то для однозначного неизбыточного определения ориентации в пространстве достаточно выбрать любые два вектора из этой тройки.

Рисунок 3.5. Структура матрицы Т₆ и единичные векторы, определяющие ориентацию схвати.

Иногда вводят ещё более глобальную систему координат, чем та, что у стойки. Если матрицей преобразования между базовой и глобальной системами является матрица R, то позиция схвата относительно глобальной системы координат (R0) описывается следующим образом:

Т6=А1·А2·А3·А4·А5·А6=R·T6 R6= А0·T6 R6=R·T6 (3.4)

где R-1 - координаты точки в глобальной, удаленной от робота системе координат.

3.3 Обратная задача кинематики робота-манипулятора

Обратная задача кинематики. Способы решения

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

Матрица Т6 выглядит следующим образом:

  (3.5)

Получается, что элементы матрицы Т₆ как произведения шести матриц А также содержат синусы и косинусы углов (неизвестных углов θ_i). Каждому элементу матрицы 4х4, некоторые элементы которой представляют собой сумму произведений синусов и косинусов от неизвестных обобщённых координат соответствует другая матрица 4х4 элементы которой представляют собой числа.

Поэлементное сопоставление двух матриц в результате даёт систему шести нелинейных уравнений, так как независимыми в матрице Т₆ являются шесть элементов (к примеру над диагональных):

 (3.6)

Выбор шести элементов из двенадцати с аналитической точки зрения можно обосновать как выбор независимых уравнений. Если рассматривать матрицу Т₆ не как источник уравнений, а как способ описания положения схвата в пространстве, то этих шести элементов достаточно для описания позиции схвата (а₃, а₅, а₆ - координаты центра схвата; а₁, а₂, а₄ описывают углы Эйлера, то есть ориентацию схвата в пространстве). Поэтому такой способ описания позиции может применяться также как и описание в обобщённых координатах.

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

  (3.7)

Или в матричной форме:

f(θ)=a (3.8)

Существует два типа решений: аналитическое в закрытой форме и численное. Кроме того есть также и геометрический способ для манипуляторов с определённой конструкцией.

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

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

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

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

Рис. 3.6. Два разных решения одной обратной задачи кинематики.

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

*Углы Эйлера

Углы Эйлера определяют три поворота системы, которые позволяют привести любое положение системы к текущему. Обозначим начальную систему координат как (x,y,z), конечную как (x,y,z). Пересечение координатных плоскостей (xy) и (XY) называется линией узлов N.

Рис. Углы Эйлера

·              Угол α между осью (x) и линией узлов.

·              Угол β между осями (z) и (Z).

·              Угол γ между осью (X) и линией узлов.

Повороты системы на эти углы называются прецессия <#"655982.files/image034.gif">  (3.9)

где n - число степеней свободы.

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

Якобиан в целом определяет малое линейное изменение положения схвата в пространстве при малом изменении всех обобщённых координат - в этом его смысл. Другими словами он устанавливает связь дифференциальных приращений в пространстве обобщённых координат и пространстве координат конечного звена (базовой системы координат у стойки).

Рис. 3.7. Линейное изменение позиции схвата в пространстве при изменении одной обобщённой координаты.

При изменении i-й обобщённой координаты на Δθ_i изменение положения схвата можно приблизительно вычислить так:

Δx≈Δθ_i (3.10)

где  - i-й столбец Якобиана. Этот столбец состоит из 12 элементов.

Приближённое изменение положения схвата в пространстве при изменении всех обобщённых координат:

Δx ≈ J Δθ,где Δθ=[Δθ₁, Δθ₂, Δθ₃, Δθ₄, Δθ₅, Δθ₆]^T (3.11)

В более общем случае Якобиан должен иметь размер 12х6 (3.12), но так как независимых уравнений в матрице Т₆ шесть (и их достаточно для определения дифференциального сдвига и поворота), то в данном случае он может иметь меньшую размерность - 6х6.

 (3.12)

Найдём столбец Якобиана:

,гдеi=1..6 (3.13)

Если ось сустава  является вращательной осью, то тогда получаем для матрицы  следующее:

 (3.14)

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

3.4 Решение методом Ньютона

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

С общих позиций метод Ньютона можно рассматривать как итерационный метод, использующий специальную линеаризацию задачи и позволяющий свести решение исходного нелинейного уравнения к решению последовательности линейных [8].

При использовании метода Ньютона необходимо найти набор из шести обобщённых координат . Заданными считаем шесть над диагональных элементов матрицы T₆. Значения θ_i определим путём последовательных приближений. Необычность данного решения состоит в том, что метод Ньютона применяется к функции от шести неизвестных, несмотря на это процедура абсолютно та же, как и в случае с одной.

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

Пусть  - номер шага. Приближенное значение обобщенной координаты  обозначим , а уточненное -  (для первого шага приближенное значение - , уточненное - ). Представим зависимость матрицы T₆ от обобщенных координат в виде отрезка степенного ряда Тейлора, ограниченного линейными членами, с центром разложения в точке :

 (3.15)

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

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

 (3.16)

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

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

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

С учётом того, что является столбцом Якобиана, то формулу метода Ньютона можно в матричном виде переписать следующим образом:

, где  =[Δθ₁, Δθ₂, Δθ₃, Δθ₄, Δθ₅, Δθ₆]^T (3.17)

 (3.18)

Получается, что существуют две трактовки формулы, соответственно есть два способа вычисления методом Ньютона, первый при помощи решения системы линейных уравнений, а второй при помощи вычисления обратной матрицы Якоби (jacobian inverse). В первом случае столбцы Якобиана на каждой итерации участвуют в формировании системы линейных уравнений, а во втором Якобиан вычисляется целиком, чтобы потом подсчитать обратную ему матрицу. Эти две точки зрения, по сути, являются одной, так как процесс решения системы линейных уравнений в данном случае как раз и заключается в отыскании обратного Якобиана.

3.5 Позиционирование захвата робота

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

Для математической постановки задачи должны быть определены малые линейные изменения относительного положения  относительно истинного положения при малых изменениях обобщённых координат робота .

 (3.19)

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

Уравнение (3.19) можно представить в матричной форме, и оно будет иметь вид:

 (3.20)

Сначала возьмем вектор , для него имеет место:

 (3.21)

Записав уравнение в матричной форме, получим первые 3 линейные алгебраические уравнения для позиционирования захвата робота:

  (3.22)

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

Для вектора подхода a в общей матрице :

 (3.23)

И при предположении, что ориентация захвата при позиционировании должна оставаться неизменной, то могут быть записаны следующие 3 линейные уравнения:

 (3.24)

Из общей матрицы  возьмем вектор нормали:

 (3.25)

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

 (3.26)

Итоговая система уравнений расширяется следующим образом: 2 уравнения берутся из системы (3.24) и одно из системы (3.26) посредством этого достигается постоянство ориентации захвата робота при позиционировании.

В итоге полученная система будет иметь вид:

  (3.27)

Из этих 6 уравнений могут быть получены приращения координат робота  при позиционировании схвата робота-манипулятора (рисунок 3.8). Расстояние получаем как разность между матрицей точного положения схвата и той, что была вычислена при решении обратной задачи кинематики. Кроме этого при предположении постоянности положения точки захвата робота, ориентация захвата будет изменена, при этом могут быть получены соответствующие значения для  из системы линейных алгебраических уравнений. В зависимости от требуемой точности данная процедура может повторяться, при этом первоначальная позиция, полученная при решении обратной задачи кинематики корректируется на каждом шаге при помощи полученных приращений обобщённых координат.

Рисунок 3.8. Позиционирование схвата робота.

Задача позиционирования по аналогии с обратной задачей кинематики может быть решена с точки зрения Якобиана:

Δx ≈ J Δθ, где Δθ = [Δθ₁, Δθ₂, Δθ₃, Δθ₄, Δθ₅, Δθ₆]^T (3.28)

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

3.6 Построение траектории

Траектория движения манипулятора - последовательность позиций, через которые проходит захват (TCP), эти узловые точки, в принципе, бывают двух типов:

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

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

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

Б) Промежуточные точки, вводимые специально для коррекции движения манипулятора с целью максимального соответствия выбранной траектории движения.

Рисунок 3.9. Точка подхода и ухода.

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

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

Kawasaki может двигаться как прямолинейно, так и с использованием углового интерполированного движения. Прямолинейные траектории, вероятно, следовало бы избегать, так как создают нежелательные нагрузки на двигательные узлы манипулятора, обрывочные прямолинейные движения постепенно изнашивают манипулятор. Однако в модели FS03N инструкциям движения неявно соответствуют разные значения скоростей и ускорений по умолчанию, поэтому даже прямолинейные траектории с остановкой в каждой позиции не создают нагрузок на приводы. Кроме того существует CP режим (режим непрерывного движения), при установке которого робот будет плавно переходить от одного сегмента движения к другому без остановок (сегмент движения - движение, совершённое одной инструкцией).

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

Рисунок 3.11. Траектория движения манипулятора.

Рисунок 3.10. Основные точки движения.

В данном случае описанием траектории движения манипулятора является семь позиций (рисунок 3.10.), направление вектора подхода во всех из них кроме последней должно быть перпендикулярно поверхности. Позиции 3, 4, 5, 6 отличаются только координатой центра схвата, а углы Эйлера у них одинаковые. Аналогично позиции 1 и 2 отличаются только координатами центра. Позиция 7 вообще может иметь любую ориентацию в пространстве, положение схвата выбирается произвольно, главное, чтобы оно выходило за определённую черту, за которую можно выбросить готовый продукт, после чего он упадёт в пул готовых продуктов. Позиции 1, 3 и 5 могут служить точками позиционирования, где манипулятор корректирует положение схвата.

Вместе с роботами фирмы Kawasaki поставляется программа PC-ROSET, позволяющая моделировать движения манипуляторов. С помощью этой программы генерируются реальные позиции, которые могут быть вставлены в управляющую программу на языке AS. Выбрав соответствующие значения декартовых или обобщённых координат, можно добиться нужной позиции, изменение положения захвата при этом визуализируется (рисунок 3.12.):

Рисунок 3.12. Позиция захвата объекта.

В соответствии с построенной траекторией при помощи программы PC-ROSET формируется набор позиций в декартовых координатах (таблица 3.1).

Таблица 3.1. Позиции манипулятора, выраженные через декартовы координаты и углы Эйлера.

4. Разработка программной реализации алгоритмов

.1 Прямая задача кинематики

Решение прямой задачи кинематики является первым этапом работы программы (рисунок 4.1), введя соответствующие конфигурации манипулятора параметры, а также обобщённые координаты получаем матрицу положения Т₆. Здесь же определяется сам манипулятор, сохранив его параметры, мы можем в дальнейшем не вводить их во втором этапе при решении обратной задачи.

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

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

Алгоритм решения состоит в последовательном произведении матриц А_i, которые ставятся в соответствие каждому звену, все эти матрицы, кроме последней могут восприниматься как правила, преобразующие одну локальную систему координат к другой, вплоть до базовой системы координат (рисунок 4.2). Последняя матрица А₆ рассматривается в качестве матрицы, описывающей положение схвата манипулятора с точки зрения пятой локальной системы координат. Так как процесс представляет собой цепочку последовательных преобразований, то порядок умножения матриц имеет значение.

Рисунок 4.2. Алгоритм решения прямой задачи кинематики.

I - единичная матрица

4.2 Обратная задача кинематики

Для решения обратной задачи необходимо либо ввести новую конфигурацию манипулятора либо восстановить ту, что была введена на первом этапе, а также матрицу Т₆, для которой мы хотим найти углы между сочленениями. Кроме того нужно указать точность и начальные обобщённые координаты, которые должны быть выбраны таким образом, чтобы итерационный процесс решения методом Ньютона сходился (рисунок 4.3.).

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

На практике это означает, что точка, описываемая начальными углами должна недалеко отстоять от точной позиции, которая соответствует матрице Т₆.

Алгоритм решения обратной задачи кинематики можно разделить на две части: во-первых, сам алгоритм, реализующий итерационный процесс, а во вторых алгоритм получения столбцов Якобиана, который является составной частью первого. В данном случае каждое новое уточнённое значение обобщённых координат находится путём решения системы линейных уравнений, процесс формирования этой системы требует получения не всего Якобиана целиком (как в случае с вычислением обратной матрицы), а только очередного его столбца. Всего система линейных уравнений состоит из двенадцати элементов, однако независимыми из них являются только шесть, что позволяет сформировать квадратную матрицу. При решении обратной задачи можно использовать и все 12 уравнений, что улучшит точность решения системы на каждом очередном этапе, однако библиотека Iridium позволяет решать системы линейных уравнений, которые можно описать в матричном виде, как A*X=B, только в том случае, если матрица A является квадратной, то есть когда число неизвестных соответствует числу независимых уравнений. Если бы решение рассматривалось не с точки зрения СЛАУ, а Якобиана, то найти обратную матрицу можно было бы только если она является квадратной, однако и здесь можно было бы использовать все 12 уравнений, так как тогда вычислялась бы псевдообратная матрица, которая соответствует не всем свойствам обратной, а только части из них.

При формировании СЛАУ необходимо получать столбцы Якобиана (рисунок 4.4):

Рис. 4.4. Алгоритм получения столбцов Якобиана.

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

В итоге получаем матрицу 4х4 из которой потом выбираем шесть элементов, соответствующих столбцу Якобиана:

 (4.1)

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

Итерационный алгоритм методом Ньютона можно описать следующим образом (рисунок 4.5):

Рис. 4.5. Итерационный численный алгоритм методом Ньютона.

Критерием окончания служит соответствие матрицы Т_n, полученной на определённом шаге, точной матрице Т₆. Для проверки вычисляется модуль разности матриц, а затем первые 12 элементов суммируются, эта сумма (sum) и сравнивается с числом ε:

T6minusTn = T6-T6n; = 0.0;i = 1:3j = 1:4= sum + abs(T6minusTn(i,j));

end

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

.9956

.9987

.0008

.9999

.0034

.9987

Это решение совпадает с точными углами. А график на рисунке 4.8 соответствует решению:

.2213

.0899

.1496

.6052

.0615

.4732

При этом матрица Т_n в данном случае совпадает с матрицей Т₆.

Рисунок 4.6. График зависимости точности (sum) от итераций.

Кроме того при решении системы линейных уравнений получаются большие углы в радианах:

.677282242454829

.709237201657174

.063980202317705

.151914198811085

.154640688892563

.248819247347353

Их приходится уменьшать до тех пор, пока они не станут находиться в диапазоне от - до . Если это не делать на каждой итерации - точность решения упадёт, а в определённых случаях решение даже перестанет сходиться. Сходимость без уменьшения углов (рисунок 4.7) хуже, чем с их корректировкой (рисунок 4.8).

Рисунок 4.8. График зависимости точности (sum) от итераций с уменьшением углов.

Рисунок 4.7. График зависимости точности (sum) от итераций без уменьшения α углов.

Вероятно, это связано с тем, что хотя угол α+2πn и эквивалентен углу γ, тем не менее, их синусы и косинусы могут незначительно отличаться при вычислении на компьютере, а так как при решении прямой задачи кинематики и Якобиана соответствующие математические функции используются интенсивно, то в сумме получается заметная погрешность.

4.3 Позиционирование

Позиционирование - третий, заключительный этап работы программы, который напрямую зависит от результатов решения обратной задачи кинематики, так как, по сути, уточняет это решение (рисунок 4.9). Единственный параметр, который необходимо ввести - точность, после чего можно запустить позиционирование. Конечным результатом является вычисление приращений обобщённых координат, которыми необходимо скорректировать решение обратной задачи кинематики, конечная цель - ещё большее соответствие реального положения схвата тому, что задано матрицей Т₆.

Рисунок 4.9. Реализация решения задачи позиционирования.

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

Как и решение обратной задачи, позиционирование можно рассматривать либо с точки зрения решения СЛАУ либо вычисления обратной матрицы, что в данном случае одно и то же.

Алгоритм решения задачи позиционирования такой (рисунок 4.10):

Рисунок 4.10. Алгоритм решения задачи позиционирования.

Для более точного решения обратной задачи кинематики на рисунке 4.3. результат позиционирования следующий (рисунок 4.11):

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

При уменьшении точности вычисления обратной задачи кинематики (рисунок 4.12) робот достигает позиции, которая расположена дальше от целевой, поэтому позиционирование при той же точности осуществляется за большее число шагов (рисунок 4.13).

Рисунок 4.12. Реализация менее точного решения обратной задачи кинематики.

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

Заключение

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

В данном дипломе c учётом реальных возможностей модели Kawasaki FS03N построена траектория движения манипулятора, которую он должен воспроизводить при покраске объектов, также разработан набор алгоритмов и их программная реализация, позволяющая моделировать конкретный шестизвенный манипулятор с шестью вращательными степенями свободы. С помощью этих алгоритмов, во-первых по конкретным координатам точек траектории может быть сгенерирован набор позиций, который должен быть вставлен в программу управления на языке AS, а во-вторых их можно использовать в самой системе управления роботом с целью позиционирования. Все эти средства позволяют организовать процесс покраски объектов при помощи робота-манипулятора.

Библиографический список

1.      Пол Р. Моделирование, планирование траекторий и управление движением робота-манипулятора. - М., 1976, C.16 -26.

.        М. Шахинпур. Курс робототехники. Под редакцией С.Л. Зинкевича. - Москва “Мир”,1990. - 380с.

3.      Bill Goodwine. Inverse Kinematics. University of Notre Dame, 2005. - 30c.

.        Niels Joubert. Numerical Methods for Inverse Kinematics. UC Berkeley, 2008. - 8c.

5.      AS язык программирования. Руководство по программированию. Kawasaki Heavy Industries, Ltd, 2002. - 335с.

6.      К. Фу, Р. Гонсалес, К. Ли. Робототехника. Под редакцией В.Г. Градецкого. - Москва “Мир”,1989. - 620с.

7.      Bill Baxter. Презентация «Fast Numerical Methods for Inverse Kinematics». University of North Carolina at Chapel Hill, 2000.

8.      Амосов А.А, Дубинский Ю.А, Копченова Н.В. Вычислительные методы для инженеров: Учеб. пособие. - М.: Высш. Шк., 1994, С.105-113, 201-207.

9.      Jorge Angeles. Fundamentals of robotic mechanical systems: theory, methods, and algorithms. Springer-Velag New York, 2003. - 545с.

.        Craig, J.J. Introduction to Robotics Mechanics and Control. Addison-Wesley, Reading, MA, 1989. - 463c.

.        Marcello Restelli. Презентация «Robot Kinematics». Politecnico di Milano, 2008.

.        Sami Haddadin. Презентация «Robotics: Inverse kinematics». Institute of Robotics and Mechatronics, German Aerospace Center (DLR), Germany, 2009.

13.    Мэтью Мак-Дональд, Марио Шпушта. Microsoft ASP.NET 3.5 с примерами на C# 2008 для профессионалов. -М., «И. Д. Вильямс», 2008. - 1424 с.

14.    Vâclav Hlavâĉ. Презентация «Robot Kinematics». Czech Technical University, Faculty of Electrical Engineering, Department of Cybernetics.

15.    Гильман А.М., Первин Ю.А. Dynamic Organisation of Files in Management Control Systems (Динамическая организация файлов в АСУП). - В кн.: Материалы Международного конгресса ИФИП, Любляна, Югославия, 1971.

.        Myke Predko, ‘123 Robotics experiments’.

.        Первин Ю.А. О простых и иерархических системах корректировки информационных массивов в АСУП. - В кн.: Математические модели в автоматизированных системах управления производством, М., 1971.

.        Летюшова О.И., Первин Ю.А., Шевякова Т.К. Параллельная корректировка семейства структуры изделия. - В кн.: Труды Всесоюзной конференции по математическому обеспечению АСУП, Новосибирск, 1972.

.        Первин Ю.А. Списковые структуры в информационных подсистемах АСУП с последовательным доступом. - В кн.: Материалы 4-го Всесоюзного семинара по экспериментальным задачам управления, Ленинград-Бологое, 1972.

.        Матэр Е.А., Первин Ю.А. Динамическая структура информационных массивов в АСУП. - В кн.: Прикладная математика и кибернетика. Наука, Москва-Горький, 1973.

.        Гильман А.М., Корионова Г.П., Матэр Е.А., Первин Ю.А. Математические вопросы организации и построения на ЭВМ базисной подсистемы АСУП. - В сб: Информационные материалы Научного Совета по комплексной проблеме «Оптимальное планирование и управление народным хозяйством», М., 1969.

.        Искусственный интеллект. Современный подход, С. Рассел, П. Норвиг, 2-е изд, ИД Вильямс, 2007 г. 1408 стр., с ил.; ISBN 978-5-8459-0887

.        Интеллектуальные роботы, И.А. Каляев, В.М. Лохин, И.М. Макаров, под общ. ред. Е.И. Юревича, Машиностроение, 2007 г., 360 стр.,

.        Компьютерное зрение. Современный подход, Д. Форсайт, Д. Понс , ИД Вильямс, 2004 г., 928 стр.,

.        Основы управления манипуляционными роботами, С.Л. Зенкевич, А.С. Ющенко, МГТУ им. Н.Э. Баумана, 2005 г., 480 стр.,

.        Основы робототехники, Е.И. Юревич, БХВ-Петербург, 2005 г., 408 стр.

кинематика манипуляционный робот алгоритм

Приложение

Реализация алгоритмов прямой, обратной задач кинематики, а также позиционирования в среде MATLAB.

function Main();= [2.0; 2.0; 2.0; 2.0; 2.0; 2.0];= [-90; 0; 0; 90; -90; 0];= [0; 30; 40; 50; 0; 0];= [250; 0; 0; 0; 0; 80];= SolveForvardKinematics(Theta, Alpha, Apar, Dpar);= [1.5; 1.5; 1.6; 1.6; 1.7; 1.7];(Theta); ('----------------------- Positioning ---------------------------');= SolveReverseKinematics(Theta0, Alpha, Apar, Dpar, T6, 0.0000001);(Angles);= SolveForvardKinematics(Angles, Alpha, Apar, Dpar);(T6); display(Tpos);= [T6(1,4)-Tpos(1,4);T6(2,4)-Tpos(2,4);T6(3,4)-Tpos(3,4)];= GetPositioningAngles(dT6, T6, Angles, Alpha, Apar, Dpar, 0.0001);(SolveForvardKinematics(Angles+dTheta, Alpha, Apar, Dpar));(Angles+dTheta);

%----------------------------------------------------------------Res = SolveForvardKinematics(Theta, Alpha, Apar, Dpar)= eye(4,4);i = 1:6= Theta(i); Al = Alpha(i); A = Apar(i); D = Dpar(i);= [cos(Th), -cos(Al)*sin(Th), sin(Al)*sin(Th), A*cos(Th);(Th), cos(Al)*cos(Th), -sin(Al)*cos(Th), A*sin(Th);

.0, sin(Al), cos(Al), D;

.0, 0.0, 0.0, 1.0];= Res * Buf;

%----------------------------------------------------------------Res = SolveReverseKinematics(Theta0, Alpha, Apar, Dpar, T6, eps)= Theta0;= 0.0;counter = 0:50000= zeros(6);= zeros(6, 1);n = SolveForvardKinematics(ThetaN, Alpha, Apar, Dpar);= zeros(6, 1);(1,1) = T6(1, 2) - T6n(1, 2);(2,1) = T6(1, 3) - T6n(1, 3);(3,1) = T6(1, 4) - T6n(1, 4);(4,1) = T6(2, 3) - T6n(2, 3);(5,1) = T6(2, 4) - T6n(2, 4);(6,1) = T6(3, 4) - T6n(3, 4);(1,1) = TminusT(1,1);(2,1) = TminusT(2,1);(3,1) = TminusT(3,1);(4,1) = TminusT(4,1);(5,1) = TminusT(5,1);(6,1) = TminusT(6,1);i = 1:6= GetDerive(i, ThetaN, Alpha, Apar, Dpar);(1, i) = delta(1, 2);(2, i) = delta(1, 3);(3, i) = delta(1, 4);(4, i) = delta(2, 3);(5, i) = delta(2, 4);(6, i) = delta(3, 4); (1,1) = B(1,1) + delta(1, 2)*ThetaN(i);(2,1) = B(2,1) + delta(1, 3)*ThetaN(i);(3,1) = B(3,1) + delta(1, 4)*ThetaN(i);(4,1) = B(4,1) + delta(2, 3)*ThetaN(i);(5,1) = B(5,1) + delta(2, 4)*ThetaN(i);(6,1) = B(6,1) + delta(3, 4)*ThetaN(i);%i= linsolve(A, B);i = 1:6(abs(ThetaNew(i))>(pi))(i) = ThetaNew(i) - (abs(ThetaNew(i))/ThetaNew(i))*2*pi;n = SolveForvardKinematics(ThetaNew, Alpha, Apar, Dpar);minusTn = T6-T6n; = 0.0;i = 1:3j = 1:4= sum + T6minusTn(i,j)*T6minusTn(i,j);

%Graph(counter+1)=sum;(sum < eps)

%plot(Graph);= ThetaN;;= ThetaNew;%counter= ThetaN;

%----------------------------------------------------------------Res = GetDerive(ThetaIndex, Theta, Alpha, Apar, Dpar)= eye(4,4);i = 1:6= Theta(i); Al = Alpha(i); A = Apar(i); D = Dpar(i);i == ThetaIndex= [-sin(Th), -cos(Al)*cos(Th), sin(Al)*cos(Th), -A*sin(Th); (Th), -cos(Al)*sin(Th), sin(Al)*sin(Th), A*cos(Th);

.0, 0.0, 0.0, 0.0;

.0, 0.0, 0.0, 0.0];= [cos(Th), -cos(Al)*sin(Th), sin(Al)*sin(Th), A*cos(Th);(Th), cos(Al)*cos(Th), -sin(Al)*cos(Th), A*sin(Th);

.0, sin(Al), cos(Al), D;

.0, 0.0, 0.0, 1.0];;

= Res * Buf;

%----------------------------------------------------------------Res = GetPositioningAngles(dT6, T6, Angles, Alpha, Apar, Dpar, eps)= 0.0;counter = 0:50000= zeros(6); B = zeros(6, 1);(1, 1) = dT6(1);(2, 1) = dT6(2);(3, 1) = dT6(3);i=1:6= GetDerive(i, Angles, Alpha, Apar, Dpar);(1, i) = delta(1, 4);(2, i) = delta(2, 4);(3, i) = delta(3, 4);(4, i) = delta(1, 3);(5, i) = delta(2, 3);(6, i) = delta(1, 2);= linsolve(A, B);= Angles+dTheta;= SolveForvardKinematics(Angles, Alpha, Apar, Dpar);= [T6(1,4)-Tpos(1,4);T6(2,4)-Tpos(2,4);T6(3,4)-Tpos(3,4)];= 0.0;minusTn = T6 - Tpos;i = 1:3= sum + T6minusTn(i,4)*T6minusTn(i,4);(counter+1)=sum;(sum < eps)= dTheta;(Graph);(counter);;= dTheta;

>point st [mm] Y[mm] Z[mm] O[deg] A[deg] T[deg]

.189 500.493 -163.541 16.458 175.912 -172.144 ? (If not, Press RETURN only.)

>point a [mm] Y[mm] Z[mm] O[deg] A[deg] T[deg]

.324 500.170 -312.694 -62.625 178.689 89.959 ? (If not, Press RETURN only.)

>point b [mm] Y[mm] Z[mm] O[deg] A[deg] T[deg]

.307 505.258 -135.254 76.795 178.645 -120.480 ? (If not, Press RETURN only.)

>point c [mm] Y[mm] Z[mm] O[deg] A[deg] T[deg]

.108 531.104 -6.018 89.992 89.843 -115.431 ? (If not, Press RETURN only.)

>point d [mm] Y[mm] Z[mm] O[deg] A[deg] T[deg]

.140 657.350 -6.038 89.995 89.846 -115.428 ? (If not, Press RETURN only.)

>point f [mm] Y[mm] Z[mm] O[deg] A[deg] T[deg]

.820 533.110 -6.042 89.996 89.847 -115.434 ? (If not, Press RETURN only.)

>point e [mm] Y[mm] Z[mm] O[deg] A[deg] T[deg]

.332 652.843 -6.057 89.994 89.849 -115.432 ? (If not, Press RETURN only.)

>point g [mm] Y[mm] Z[mm] O[deg] A[deg] T[deg]

.269 540.402 -6.043 90.003 89.849 -115.429 ? (If not, Press RETURN only.)

>point h [mm] Y[mm] Z[mm] O[deg] A[deg] T[deg]

.224 498.795 -304.789 80.486 179.687 54.000 ? (If not, Press RETURN only.)

>edit printer

.Program printer()

?s

LMOVE st

?p

LMOVE st

LAPPRO a,100

LMOVE a,1

LMOVE b

LMOVE c

LMOVE d

TWAIT 5

LMOVE f

LMOVE e

TWAIT 5

LMOVE g

DRIVE 4,190

ALIGN

DRIVE 6,-15

LAPPRO h,100

LMOVE h,-1

LMOVE st

?e