Автор: Попов С.А. Воробьев Е.И. Шевелева Г.И.
Теги: регулирование и управление машинами, процессами кибернетика механика динамика кинематика роботы промышленные
ISBN: 5-06-001201-8
Год: 1988
Текст
ПРОМЫШЛЕННЫХ
В трех книгах
Под редакцией К.В.Фролова, Е.И.Воробьева
Кинематика
и динамика
Допущено Министерством высшего и среднего
специального образования СССР
в качестве учебного пособия для студентов
высших технических учебных заведений
Москва « Высшая школа »1988
1
ББК 32.816
М 55
УДК 62-52
Авторы:
Е. И. ВОРОБЬЕВ, С. А. ПОПОВ, Г. И. ШЕВЕЛЕВА
Рецензенты: кафедра «Промышленные роботы и ро-
бототехнические системы» -Московского станкоинструмен-
тального института (зав. кафедрой — д-р техн, наук, проф*.
В. С. Кулешов); кафедра «Робототехнические системы»
Белорусского политехнического института (зав. кафедрой —
д-р техн, наук, проф. Г. И. Хутский)
Механика промышленных роботов: Учеб, по-
М55 собие для втузов: В 3 кн./Под ред. К. В. Фро-
лова, Е. И. Воробьева. Кн. 1: Кинематика и
динамика/Е. И. Воробьев, С. А. Попов, Г. И. Ше-
велева. — М.: Высш, шк., 1988. — 304 с.: ил.
ISBN 5-06-001201-8
В книге рассмотрены основные методы анализа кинематики
и динамики промышленных роботов как пространственных систем
твердых тел с несколькими степенями свободы. Кинематический
анализ исполнительных механизмов роботов излагается методами:
векторным, матриц и винтов. Методы динамического анализа
основаны на уравнениях Лагранжа, принципе
.ала.мбера
прин-
ципе Гаусса и ориентированы на применение ЭВМ. Приведены
примеры анализа кинематики и динамики конкретных манипуля-
торов.
м 2702000000(4309000000) -332
001 (01)-88
159-88
ББК 32.816
6Ф0.1
© Издательство
ISBN 5 - 06 - 001201-8
«Высшая школа», 1988
1
Предисловие
Настоящая книга открывает серию «Механика промышлен-
ных роботов», которая состоит из трех книг: «Кинематика
и динамика», «Расчет и проектирование механизмов», «Осно-
вы конструирования». В серии последовательно изложены
основные вопросы, возникающие при конструировании испол-
нительных механизмов промышленных роботов. Важность
и актуальность этих вопросов связаны с необходимостью
повышения качества подготовки специалистов в области
робототехники.
Вопросы механики и конструирования манипуляторов
изучаются в различных курсах робототехники, и в настоя-
щее , время ощущается острая потребность в учебной
литературе. Издаваемая серия предназначена прежде всего
для подготовки конструкторов промышленных роботов,
хотя может быть использована и для подготовки других
специалистов.
В первой книге изложены основные методы анализа
кинематики и динамики манипуляторов. В разделе кине-
матики рассмотрены методы: векторный, матриц и винтов;
в разделе динамики — методы механики, позволяющие
построить эффективные алгоритмы расчета на ЭВМ:
метод кинетостатики, метод, построенный на уравнениях
Лагранжа, и метод наименьшего принуждения Гаусса.
Изложен метод анализа динамики манипуляторов с учетом
кулоновского трения в кинематических парах.
В книге рассмотрено большое число примеров анализа
конкретных механизмов манипуляторов, разработаны приме-
ры заданий для курсовых работ.
Главы 1, 2, 4, 5, 8, § 3.9, 3.10, Введение, Приложения I,
III написаны Е. И. Воробьевым; § 5.1 —С. А. Поповым;
§ 3.1—3.8, главы 6, 7, Приложение II
Авторы выражают благодарность рецензентам за крити-
ческие замечания и полезные советы, а также сотрудникам
кафедр «Теория механизмов и робототехника» Москов-
ского института химического машиностроения и «Теорети-
ческая механика» Московского станкоинструментального
института за помощь при подготовке книги.
Замечания и пожелания по улучшению содержания
книги просим направлять по адресу: 101430, Москва,
ГСП-4, Неглинная ул., д. 29/14, издательство «Высшая школа».
Г. И. Шевелевой.
I
I
)
f
I
г
1
1
Авпюры
1
Введение
Робототехника — новое, быстроразвивающееся направление
науки и техники, связанное с созданием и применением
роботов и робототехнических систем. Робототехника
возникла как самостоятельное научное направление на
основе механики и кибернетики. В то же время развитие
робототехники поставило в ряде смежных наук новые
проблемы и задачи и стимулировало их развитие.
Робот, являющийся одним из основных объектов изу-
чения в этой науке, представляет собой универсальный
или инои степени воспроизводящих
автомат для воспроизведения двигательных и интеллектуаль-
ных функций человека. Существуют различные классы
роботов, в той
двигательные или интеллектуальные функции человека. Их
классификация дана в работе [39]. Среди этих роботов
важным классом являются манипуляционные роботы. Спе-
цифика манипуляционных роботов, частным видом которых
являются промышленные роботы, связана с наличием
манипулятора — его исполнительного органа.
Практической целью создания роботов явилась передача
им тех видов деятельности, которые для человека являют-
ся трудоемкими, тяжелыми, монотонными, вредными для
здоровья и жизни. Это прежде всего вспомогательные
производственные операции: загрузка и разгрузка устано-
вок, станков и автоматов и основные производственные
операции: сварка, окраска, резка, сборка и т. д., а также
работы в так называемых экстремальных условиях:
под водой, в космосе, в радиоактивных и ядовитых средах.
Роботы, предназначенные для выполнения двигательных
и управляющих функций в производственном процессе,
называются промышленными. Именно необходимость реше-
ния производственных задач обусловила бурное развитие
в последние два десятилетия исследований в области
робототехники и производства роботов. С помощью про-
мышленных роботов удается решать задачи комплексной
механизации и автоматизации мелкосерийного производ-
ства в машиностроении.
Промышленные роботы являются одним из важных
средств решения задачи комплексной автоматизации про-
изводства, роста производительности труда, улучшения ка-
чества продукции. От традиционных средств автоматиза-
ции промышленные роботы отличаются универсальностью,
возможностью их быстрой переналадки, что позволяет
1
создавать на базе универсального оборудования роботизи-
рованные технологические комплексы, быстропереналаживае-
мые гибкие производственные системы (ГПС), гибкие
автоматизированные производства (ГАП). Создание ГАП
в настоящее время рассматривается как одна из основных
тенденций развития современного машиностроения.
Следует отметить, что значение роботов нельзя сводить
лишь к решению на их основе задач’ комплексной
автоматизации промышленного производства, роботы начи-
нают применяться и в других областях народного
хозяйства: на транспорте, в сельском хозяйстве, здраво-
охранении, сфере обслуживания.
Применение роботов является одним из путей решения
острой проблемы трудовых ресурсов и имеет большое
социальное значение.
В результате развития робототехники человечество полу-
чит возможность решать принципиально новые научные и
производственные задачи, например создание искусственного
интеллекта, и принципиально новых технологий на земле
и в космосе.
Созданию промышленных роботов предшествовали боль-
шие научные исследования, в которые внесли значительный
вклад академик И. И. Артоболевский, члены-корреспонденты
АН СССР И. М. Макаров, Д. Е. Охоцимский,
Е. П. Попов и другие ученые.
Первые образцы промышленных роботов в нашей
стране появились в 1971 г. и были созданы под руко-
водством члена-корреспондента АН СССР П. Н. Белянина и
Б. Н. Сурнина. В настоящее время в нашей стране
создано свыше 250 моделей промышленных роботов,
более 50 моделей изготовляется серийно [4].
Различным аспектам робототехники посвящено значитель-
ное число работ. В книгах М. Б. Игнатьева, Ф. М. Кула-
кова, А. М. Покровского [16], М. Вукобратовича [И],
Е. П. Попова, А. Ф. Верещагина, С. Л. Зенкевича [39],
А. В. Тимофеева [47] изложены вопросы кинематики,
динамики и управления манипуляционных роботов. В рабо-
тах В. С. Кулешова, Н. А. Лакоты [22], В. С. Медведева,
А. Г. Лескова, А. С. Ющенко [26], А. В. Тимофеева
[47], Е. И. Юревича и др. [51] рассматриваются системы
управления и алгоритмы управления роботов. Устройство
промышленных роботов и их применение в современном
производстве описаны в книгах под редакцией Я. А. Шифрина,
Е. И. Юревича [41, 42] (см. также [4, 20, 43]) и в серии книг
под редакцией члена-корреспондента АН СССР И. М. Мака-
рова [44]. Теория шагающих робототехнических систем
разрабатывается под руководством члена-корреспондента
АН СССР Д. Е. Охоцимского.
Манипуляционный робот содержит две органически
связанные части: устройство управления и манипулятор.
Устройство управления включает в себя чувствительные
(сенсорные) устройства, устройства обработки и хранения
информации (вычислительное устройство, накопители инфор-
мации), устройство управления приводами. Манипулятор
с точки зрения механики и теории механизмов — это
сложный пространственный управляемый механизм с несколь-
кими степенями свободы, содержащий жесткие и упругие
звенья, передачи и приводы. Манипуляционный робот пред-
ставляет единую динамическую систему. Ввиду ее сложности
при исследовании приходится выделять и рассматривать
отдельно механизмы, приводы и систему управления.
Движение манипулятора осуществляется от приводов,
которые могут располагаться на подвижных звеньях или
на неподвижном основании. Число приводных двигателей
обычно равно числу степеней свободы манипулятора, хотя
во время выполнения технологических операций на систему
могут накладываться дополнительные связи.
Передача движения от двигателей к звеньям механизма
осуществляется с помощью передаточных механизмов
различного вида. Система передаточных механизмов в случае
расположения приводов на основании может быть достаточ-
но сложной.
На рис. В.1,а,б показана конструкция промышленного
робота портального типа СМ40 грузоподъемностью 40 кг,
предназначенного для обслуживания металлорежущих стан-
ков. Манипулятор робота имеет четыре степени свободы
и содержит три вращательные и одну поступательную
кинематические пары. Движение звеньев в кинематических
парах осуществляется гидроприводами.
На рис. В.2,«,б показана одна из компоновок модуль-
ного промышленного робота РПМ-25 грузоподъемностью
25 кг. Особенностью робота РПМ-25 является возможность
изменять свою структуру и число степеней свободы в
зависимости от выполняемой операции. Показанная- на
рисунке компоновка робота имеет семь степеней свободы:
три переносных для перенесения объекта в заданную
точку, три ориентирующих для ориентации детали и одну —
сдвига вдоль линии станков. Движение по каждой
обобщенной координате осуществляется от электромехани-
ческого привода.
1
Если не учитывать переда-
точные механизмы, то манипу-
лятор представляет собой не-
замкнутую кинематическую
цепь. В книге рассматриваются
кинематика и динамика мани-
пуляторов как незамкнутых ки-
нематических цепей.
Рис* В.1
Следует отметить, что динамика твердых и упругих
тел уже давно изучается в механике и является до
настоящего времени одним из основных объектов исследо-
вания ввиду значительной сложности и общности.
Специфика задач механики роботов состоит в том, что
роботы являются управляем ими системами, к ним предъяв-
ляются высокие требования по точности и быстродействию
при реализации исполнительным механизмом самых различ
ных условий движения объекта (отработка заданных поло
Рис* В.2
1
жений, скоростей, ориентации, ускорений, усилий), причем
структура кинематической цепи может изменяться в движе-
нии. Все это требует при проектировании исполнитель-
ных механизмов промышленных роботов использования
всего арсенала средств и методов механики, теории
механизмов, теории автоматического управления, теории
упругости и колебаний, теории привода.
Высокие требования по быстродействию и точности
приводят к необходимости учитывать упругость звеньев
основного и передаточных механизмов промышленного
робота. Для современных роботов отношение массы перено-
симого груза к массе всей конструкции составляет
1 — 10%. Это обусловлено тем, что с уменьшением массы
конструкции робота снижаются его жесткость и точность
позиционирования. Алгоритмы моделирования динамики
управляемых манипуляторов с учетом упругости звеньев
построены в работах Ф. Л. Черноусько [49], В. Г. Гра-
децкого, Н. А. Лакоты, В. И. Шведова, Е. В. Рахманова
[23], М. Вукобратовича, П. Б. Слиеде [45].
Следует иметь в виду, что при анализе динамики
манипуляторов недостаточно использовать хорошо развитые
методы колебаний упругих систем с постоянными пара-
метрами, а необходимо рассматривать задачу движения
упругих тел, сопровождающегося колебаниями.
Сложность расчета манипуляторов обусловила развитие
методов, ориентированных на применение ЭВМ. Весьма удоб-
ным с этой точки зрения является метод матриц.
Следует отметить, что метод матриц в кинематику
механизмов был впервые введен советским ученым
Ю. Ф. Морошкиным [28, 29], а затем уже получил развитие
за рубежом. Применение метода матриц к кинематике
манипуляторов впервые дано в работе [8]. Позднее
появился ряд работ по применению метода матриц в
кинематике и динамике манипуляторов.
Опыт показал, что широко применяемый в настоящее
время метод матриц 4-го порядка по сравнению с мето-
дом матриц 3-го порядка ведет к значительному увели-
чению числа операций, хотя и является более удобным
для программирования при решении прямой задачи кине-
матики, когда при известных обобщенных координатах
находится положение схвата. При решении обратной задачи
о положениях манипулятора, под которой понимается опре-
деление обобщенных координат манипулятора при заданном
положении и ориентации схвата, по-видимому, самым эф-
фективным является векторный метод, широко использо-
1
ванный ранее в теории механизмов [25] и получивший
применение и в кинематике манипуляторов [47, 68].
Метод винтов, известный в механике и теории ме-
ханизмов [21], является наиболее компактным и ждет еще
своего широкого применения в теории манипуляторов,
хотя ряд работ уже имеется [72, 73].
Особенностью задач динамического анализа манипуля-
торов является значительная сложность и громоздкость
их уравнений движения. Это приводит к необходимости
развивать методы автоматизированного построения урав-
нений динамики манипуляторов на ЭВМ. Для построения
систем дифференциальных уравнений используются самые
различные методы аналитической механики. Большое число
методов автоматизированного формирования уравнений дви-
жения манипуляторов основано на уравнениях Лагранжа J и
II рода. Применение уравнений Лагранжа связано с фор-
мированием и дифференцированием выражений для кине-
матической и потенциальной энергий по заданной структуре
кинематической цепи. Ф. М. Кулаков показал, что для
простых незамкнутых кинематических цепей можно избежать
численного дифференцирования [21]. Уравнения Лагранжа II
рода в работах американских авторов использовались в
совокупности с методом матриц 4-го порядка [48]. Этот
метод получил применение также для автоматизированного
анализа пространственных механизмов и манипуляторов.
В ряде работ используются уравнения Лагранжа I рода,
позволяющие учесть дополнительные связи.
Принципы Даламбера и Даламбера — Лагранжа для авто-
матизированного формирования уравнений манипуляторов
использовались в- работах А. Г. Овакимова [34], Е. И. Во-
робьева [9], И. С, Виттенбурга [7], Л. Лилова [54] и других
авторов. В этом случае легко учитывать инерционность
звеньев как основного, так и передаточного механизма.
А. Г. Овакимовым в работе [34] показано весьма
значительное влияние на динамику манипуляторов инер-
ционности вращающихся звеньев приводов и предложен
метод учета их инерции. Достоинством этого метода яв-
ляется простота формирования уравнений движения манипу-
лятора, которое сводится по существу к скалярному пере-
множению векторов моментов и сил инерции и векторов
возможных перемещений точек и тел и не требует
составления и дифференцирования выражения кинетической
энергии механизма.
В работах американских ученых значительное распро-
странение получил метод формирования динамической
1
модели манипулятора, построенный на основе уравнений
Ньютона — Эйлера [56]. В этом методе для каждого
звена используются два уравнения движения твердого тела
с учетом реакций связей: уравнение движения центра масс
как материальной точки и динамическое уравнение Эйлера
вращения вокруг центра масс. При этом трудоемкость
решения прямой задачи динамики (определение сил по
заданному движению) для манипулятора пропорциональна
числу звеньев.
Алгоритмы анализа динамики на основе принципа Гаусса
и уравнений Аппеля приведены в работах А. Ф. Верещагина
[6, 39], Л. Лилова [55], В. Шилена [57]. В них для
расчета обобщенных ускорений используется прямая миними-
зация функции Гиббса на основе принципа наименьшего
принуждения Гаусса. Для незамкнутой кинематической
цепи А. Ф. Верещагину удалось построить алгоритм с линей-
ной зависимостью числа операций от числа звеньев п.
К недостаткам алгоритмов, построенных на основе уравнений
Аппеля, следует отнести необходимость вычисления функ-
ции энергии ускорений и ее дифференцирования.
Общие теоремы динамики системы — теоремы о движении
центра масс и о кинетическом моменте — для моделиро-
вания динамики использовали М. Вукобратович [11],
А. Г. Лесков, В. С. Медведев [26]. Эти методы при-
менялись в основном для простых незамкнутых кинемати-
ческих цепей.
Исполнительные механизмы роботов, как правило, содер-
жат замкнутые контуры. В этом случае для автоматизиро-
ванного топологического анализа механизмов используются
методы теории графов [7].
Кинематические пары манипуляторов работают в усло-
виях действия пространственной системы сил. В этом
случае нормальные реакции в точках контакта звеньев
достигают больших значений, при этом возникают значи-
тельные силы трения. Учет кулоновского трения значительно
усложняет динамический анализ манипуляторов, однако он
необходим для рационального конструирования шарниров.
В последнее время в динамике манипуляторов появился
новый класс задач — обратные задачи динамики. Под
обратными задачами динамики манипуляторов понимаются
задачи определения сил и параметров движения по задан-
ным условиям движения [18, 19]. Решение этих задач
для манипуляторов позволяет решать вопросы построения
алгоритмов управления приводов и систем управления
[3, 21, 38].
Раздел
Кинематика
манипуляторов
Глава 1
Основные понятия и определения
-Манипулятором называется техническое устройство,
предназначенное для воспроизведения некоторых рабочих
функций рук человека. Манипулятором называют также
исполнительный механизм промышленного робота, оснащен-
ный приводами и рабочим органом, с помощью которого
осуществляется выполнение рабочих функций. Способность
воспроизводить движения, подобные движениям рук человека,
достигается приданием манипулятору нескольких степеней
свободы, по которым осуществляется управляемое движение
с целью получения заданного движения рабочего органа —
схвата.
Числом степеней свободы механической системы
называется число возможных перемещений системы.
С точки зрения механики манипулятор представляет
систему твердых и упругих тел, связанных между собой
посредством соединений с различными видами связей.
С точки зрения теории механизмов манипулятор являет-
ся системой тел, предназначенных для преобразования
движений нескольких тел в требуемые движения других тел,
т. е. с этой точки зрения манипулятор представляет
собой пространственный механизм с несколькими степеня-
ми свободы. Трудность анализа и особенно синтеза
таких систем общеизвестны. До последнего времени в
теории механизмов обычно рассматривались системы с одной
степенью свободы.
Твердые тела, входящие в механическую систему мани-
пулятора, называются звеньями. В механике различают
1
входные и выходные звенья. Входным называется звено,
которому сообщается движение, преобразуемое механиз-
мом. Выходным называется звено, совершающее рабо-
чее движение.
Таким образом, в манипуляторе число входных звеньев
равно числу приводов, а выходное звено, как правило,
одно — схват, или рабочий орган.
Подвижное соединение двух соприкасающихся звеньев
называется кинематической парой.
Кинематические пары могут быть классифицированы
как по числу степеней свободы звеньев в их относи-
тельном движении, так и по числу связей, налагаемых
парой на относительное движение звеньев. По первому
признаку различают одно-,
двух-,
трех-, четырех-, пяти-
подвижные кинематические пары. По второму признаку —
кинематические пары пятого, четвертого, третьего, второго
и первого класса (классификация И. И. Артоболевского).
В манипуляторах в основном получили распространение
одноподвижные кинематические пары, т. е. пары пятого
класса, допускающие относительное вращательное, поступа-
тельное или винтовое движение.
Совокупность звеньев, образующих между собой кине-
матические пары, называется кинематической цепью.
Кинематические цепи подразделяются на плоские и
пространственные в зависимости от вида движения
звеньев: в одной или нескольких параллельных плоскостях
и в пространстве.
Кинематические цепи могут быть замкнутыми или
незамкнутыми. Незамкнутой кинематической цепью
называют такую цепь, в которой есть звенья, входящие
в одну кинематическую пару.
Часто говорят, что манипуляторы имеют незамкнутую
кинематическую цепь, однако это не всегда так. Для
приведения в движение манипулятор по каждой степени
свободы обычно снабжается отдельным приводом и механиз-
мом, имеющим замкнутую кинематическую цепь. Поэтому
весь манипулятор представляет собой наслоение замкнутых
механизмов с одной степенью свободы.
Однако в манипуляторе часто можно выделить
основной механизме несколькими степенями свободы
и, пренебрегая звеньями приводных механизмов, говорить
о наличии незамкнутой кинематической цепи. Этот основной
механизм отражает основные кинематические и динами-
ческие свойства манипулятора как системы с несколькими
степенями свободы.
12
1
Число степеней свободы манипулятора равно числу
обобщенных координат, под которыми понимают*
независимые переменные, однозначно определяющие положе-
ние манипулятора в пространстве.
Кинематические пары накладывают ограничения как на
относительное, так и на абсолютное движение звеньев.
Связи могут накладываться на манипулятор также в про-
цессе выполнения рабочих операций.
Связи могут быть выражены уравнениями или неравен-
ствами, связывающими координаты и их производные по
времени. Связи называются кинематическими или
дифференциальными, если они устанавливают связь
между координатами и их производными по времени. Если
уравнение дифференциальной связи может быть про-
интегрировано, то оно сводится к зависимости только
между координатами, такая связь называется геометри-
ческой или голономной.
Голономные связи выражаются уравнениями вида
f 0 =
где — радиус-вектор произвольной j-й точки системы;
t — время.
Если уравнение дифференциальной связи не может быть
проинтегрировано, то такая связь называется неголоном-
ной.
Неголономные связи выражаются дифференциальными
уравнениями вида
f • • • • » 0 0,
где число точек над вектором означает порядок
производной по времени.
Для манипуляторов (так как они являются управляемы-
ми системами) следует выделить программные связи,
которые выражают условия, накладываемые на движения
манипуляторов в процессе выполнения ими рабочих опе-
раций. Будем считать,
что условия движения манипуля-
тора могут быть выражены уравнениями и неравенствами,
V
так же как и связи.
Принципиальное отличие программных связей от обычных
связей механических неуправляемых систем состоит в том,
что первые реализуются с помощью управляющих сил,
а вторые реализуются, как правило, геометрически и
имеют место во все время движения.
Следует отметить, что для реализации программных
связей манипулятором в его приводах должны быть
13
1
приложены дополнительные силы, которые можно назвать
реакциями программных связей.
Реакциями связей называют силы, приложенные
к механической системе со стороны других тел, а также
силы действия звеньев друг на друга в кинематических
парах манипулятора. Число составляющих реакций в кине-
матических парах равно классу кинематической пары.
1.1. Символическое представление
структуры манипуляторов
Для автоматизированного кинематического и динамического
анализа механизмов на ЭВМ необходимо кодирование
параметров, описывающих структуру, геометрию и распреде-
ление масс системы.
Для описания структуры манипуляторов весьма удобным
и универсальным является метод графов, основанный на
понятии структуры дерева и использованный в [11] для
описания структуры систем твердых тел. При этом важное
значение имеет понятие пути между двумя телами. Под
путем между двумя телами понимается совокупность
векторов, проходящих через шарниры кинематической цепи,
соединяющей тела, при котором ни один шарнир не
проходится дважды.
Тогда структуру типа дерева можно определить
как структуру, для которой между всеми парами тел
путь определяется единственным образом. Очевидно, что
кинематические цепи со структурой типа дерева являются
незамкнутыми кинематическими цепями. Такие структуры
могут быть приведены к замкнутым путем добавления
уравнений связей. Если существуют два различных пути
между двумя телами, то система содержит замкнутую
кинематическую цепь.
П. Б. Слиеде [45] был предложен алгоритм кодиро-
вания механических систем со структурой дерева методом
графов. Этот метод удобен тем, что любой механизм
с замкнутыми и разомкнутыми кинематическими цепями
можно свести к структуре типа дерева с дополнитель-
ными геометрическими связями. Для
просто выбираются обобщенные координаты и легко запи-
сываются уравнения связей. Поэтому такие структуры по-
лучили широкое распространение в динамике систем твер-
дых тел.
Пусть манипулятор содержит п звеньев и имеет структуру
дерева, тогда число кинематических пар также равно и.
1
таких структур
14
1
Звеньям и шарнирам присвоим номера от 1 до N,
начиная от неподвижного звена, номер которого 0. Можно
считать без потери общности, что в системе всегда
имеется одно неподвижное звено с номером 0 — осно-
вание (стойка). Если его нет, то можно ввести фиктивное
звено массой, равной нулю.
Все звенья на пути от i-ro звена к основанию будем
называть звеньями, лежащими ниже i-го звена, а все
звенья, путь из которых к основанию включает i-e звено,—
звеньями, лежащими выше i-го звена.
Для каждого звена i задается номер того звена,
с которым оно соединено при движении по цепи в сторону
основания. При этом звенья следует нумеровать так, чтобы
выполнялось условие < i. Например, для структуры
(рис. 1.1, а) имеем — 0, m2 = l, m3—2, m4 = 3, m5 = 2,
m6 = 5. Эти шесть чисел полностью задают структуру при-
веденной системы.
Таким образом, структура кинематической цепи типа дере-
ва может быть представлена N-мерной матрицей. Так, для
системы, представленной на рис. 1.1, эта матрица имеет вид
т —
[0 1 2 3 2 5].
Для механизмов со структурой типа дерева число тпел
системы равно числу кинематических пар.
Для описания структуры механизма необходимо указать,
какие тела с какими связаны кинематическими парами.
Для этого удобно использовать граф системы. Этот
граф состоит из точек, называемых вершинами, соот-
ветствующих звеньям механизма, и линий, соединяющих
вершины и называемых ребрами. Ребра соответствуют
кинематическим парам механизма.
Рис. 1.1
15
1
1.1, а) граф имеет вид, по-
Так, для системы (рис.
казанный на рис. 1.1, б. Вершина, соответствующая осно-
ванию, обозначается буквой $0, вершины, соответствующие
остальным телам
обозначены кинематические пары, образуемые телами между
собой. Граф, соответствующий механизму со структурой
дерева, также имеет структуру дерева.
s6. Буквами Uj (i
1.2. Задачи кинематики и динамики
манипуляторов
Задачи кинематики и динамики манипуляторов приходится
решать на различных этапах проектирования и при управ-
лении роботами. Рассмотрим основные задачи кинематики
•^-динамики манипуляторов, которые обычно решаются при
проектировании промышленных роботов.
Проектирование промышленного робота обычно начи-
нается с обследования операций и условий производства,
в которых предполагается использовать робот. Изучаются
перемещения, скорости, ускорения объекта: эти условия
движения объекта определяют требования к исполнитель-
ному механизму робота — манипулятору.
Для формулировки требований к приводам манипуля-
тора кинематические и геометрические требования к движе-
нию объекта в абсолютной системе координат должны
быть преобразованы в требования к перемещениям,
скоростям и ускорениям в кинематических парах. С этой
целью решаются так называемые обратные задачи о поло-
жениях, скоростях и ускорениях.
Обратная задача о положениях состоит в определении
относительных координат звеньев манипулятора по задан-
ным положениям объекта или жестко связанного с ним
захватывающего звена.
Обратная задача о скоростях состоит в определении
требуемых обобщенных скоростей в кинематических парах
по заданной скорости выходного звена.
Если требуется реализовать заданные векторы линейной
и угловой скоростей схвата, то очевидно, что для
однозначного решения задачи необходимо, чтобы манипуля-
тор имел не менее шести степеней свободы. Решение
этой задачи позволяет сформулировать требования к
приводам по скорости и передаточному отношению.
Требуемые ускорения в кинематических парах по задан-
ному ускорению схвата обычно не определяются, а
решается задача определения потребных усилий приводов,
16
1
(
I
1
1
1
необходимых для реализации заданного движения манипуля-
юра или заданных усилий на выходном звене. Эту
ища чу называют первой или обратной задачей динамики [12].
Термин «обратная задача динамики» шире общепринято-
ю термина «первая задача динамики» [5] и включает в
себя вопросы определения структуры и параметров систе-
мы управления и алгоритмов управления.
После определения усилий в приводах, обобщенных
скоростей и ускорений можно определить абсолютные
скорости и ускорения центров масс звеньев и угловые
скорости и ускорения звеньев, главные векторы и главные
моменты сил инерции звеньев в их центрах масс. Последние
могут быть использованы для прочностных расчетов.
Очевидно, что все эти расчеты могут быть проведены
лишь при наличии геометрических и инерционных пара-
метров манипуляторов, которые в начальной стадии
проектирования неизвестны и выбираются на основе опыта
инженера и опыта проектирования аналогичных узлов
машин и систем.
Таким образом, процесс конструирования носит итера-
ционный характер: вначале конструктор на основе техни-
ческих требований выбирает кинематическую схему и некото-
рые геометрические параметры манипулятора, далее, решая
обратные задачи, определяет требования к приводам и
системе управления, выбирает или конструирует приводы и
весь исполнительный механизм.
Затем, решая задачу о движении манипулятора (прямую
задачу) под действием заданных сил, конструктор убеждает-
ся в выполнении технических требований или принимает
решение об изменении конструкции манипулятора или систе-
мы управления.
Следует указать, что результаты решения обратных
задач о положениях и скоростях составляют основу
построения кинематических алгоритмов управления: по-
зиционных и алгоритмов управления по вектору скорости.
Позиционные алгоритмы управления строятся с целью
приведения схвата робота в заданное положение на основе
его кинематической схемы. Алгоритмы управления по вектору
скорости строятся с целью сообщения схвату робота за-
данных векторов линейной и угловой скоростей также на
основе его кинематической схемы.
Динамические алгоритмы управления учитывают динами-
ческие свойства систем и строятся на основе решения
обратных задач динамики манипуляционных роботов с учетом
устойчивости системы [3, 19, 38].
17
Глава 2
Векторный метод кинематического
анализа манипуляторов
При решении задач проектирования и управления промыш-
ленными роботами приходится определять как положения
его звеньев относительно неподвижной системы координат—
абсолютные положения звеньев, так и их относительные
положения — обобщенные координаты. Напомним, что пер-
вая задача называется прямой, а вторая — обратной задачей
о положениях манипулятора.
При известных обобщенных координатах решение прямой
задачи о положениях манипулятора сводится к перемно-
жению матриц, определяющих относительные положения
звеньев, или выполняется с применением формул конечного
поворота вектора.
Определение обобщенных координат при заданном поло-
жении выходного звена манипулятора является более слож-
ной задачей, так как это связано с решением нелинейных
систем алгебраических уравнений. Эффективным методом
решения обратной задачи о положениях манипулятора
является векторно-матричный метод [31, 32]. В этом методе
основные условия связей между заданными и неизвестными
величинами используются в векторной форме, а преобра-
зование проекций векторов осуществляется в матричной
форме.
В теории механизмов векторные методы использовались
в работах Н. Г. Бруевича, В. А. Зиновьева, А. Г. Оваки-
мова, П. А. Лебедева и др. Эффективным является подход,
когда задача о положениях пространственных механизмов
решается до конца в векторной форме без перехода к
проекциям векторов [24].
1
(
2.1. Прямая задача о положениях
Задача определения абсолютных положений звеньев при их
заданных относительных положениях решается различными
методами.
Метод, основанный на использовании формул конечного по-
ворота твердого тела. Метод позволяет определить новое
положение вектора, зная его старое положение, ось пово~ /
рота и угол поворота.
Рассмотрим различные формулы конечного поворота
твердого тела. Известная формула Родриго имеет вид [16]
18
1
r cos (p 4- (1
— cos <p) (e • f) • e 4- e x f sin ф,
где
fi — векторы, связанные с телом до
поворота; е — орт оси поворота; ф — угол поворота.
Этой формуле можно придать другой вид, сделав замену
COS СО 1
втф »
20
где 0 = tg (ф/2).
В этом случае * получим
Если угол между осью и вектором г равен я/2, то
формула упрощается и принимает вид
= f cos ф + ё х f sin ф. (2.1)
При совершении двух конечных поворотов тела вокруг
рот находится по формуле
X _ §1 + §2 “ X 02
l-0i62 ’
где б = е tg (ф/2); 6j = tg (Ф1/2) б2 = ё2 tg (ф2/2); е — орт ре-
зультирующего поворота.
Последняя формула показывает, что результирующий
поворот двух конечных поворотов твердого тела вокруг
неподвижных осей зависит от порядка выполнения этих
поворотов.
Матричный метод преобразования координат вектора.
Рассмотрим три декартовы системы координат: Oxyz,
OiX^iZi и O2x2y2z2> Пусть в системе O2x2y2Z2 заданы
координаты вектора f(x2,y2iz2). Тогда в системе О^у^
координаты этого же вектора могут быть определены
в матричной форме следующим образом:
(2.2)
где [*i] = [хь У1, Zi]T — матрица-столбец координат вектора
г в системе OtXiyiZi; [х2] = [x2t y2t z2]T - матрица-столбец
координат вектора г в системе O2x2y2z2, где индекс «т»
означает операцию транспонирования матрицы;
[311 Р12 Р13
р21 Р22 р23
Рз1 Рз2 РзЗ
— матрица перехода от системы O2x2y2z2 к системе
O1X1J1Z2- Элементы pl7 (i,J= 1,2,3) матрицы L2 есть
направляющие косинусы осей системы O2x2y2z2 относитель-
но осей системы О^у^.
Аналогично, для преобразования координат из системы
01Х1У121 к системе Oxyz можно записать
И — М [xj,
(2.3)
где [х] = [х, у, z]T
— матрица-столбец координат вектора г
в системе Oxyz;
«и
«21
«12 «13
«22 «23
«32 «33 —
— матрица перехода от системы O1x1y1zl к системе Oxyz.
Элементы ау (i,j = 1, 2, 3) матрицы Lj есть направляющие
косинусы осей системы О^у^ относительно осей системы
Oxyz.
Заменяя в выражениях (2.3) [xt] из выражения (2.2),
получим
и = l1l2 [х2] .
Для п систем координат можно записать
[х]=
1=1
где [х„] = [хи, уп, z„]T — матрица-столбец координат вектора
г в системе Onxnynzn;
Пример: определение абсолютных положений манипулятора
векторным методом. Рассмотрим манипулятор с пятью вра-
щательными кинематическими парами (рис. 2.1).
Заданными считаем обобщенные координаты механизма,
за которые приняты углы относительного поворота звеньев
в кинематических парах. Для решения задачи мысленно
установим манипулятор в некоторое «нулевое» положение,
в котором все обобщенные координаты равны нулю.
Положение манипулятора в любой момент времени опре-
деляется ориентацией его осей звеньев и кинематических
пар и их положением. В «нулевом» положении векторы,
1
направленные по осям звеньев и кинематических пар, счи-
таем известными. Чтобы перевести манипулятор из «нуле-
вого» положения в положение, задаваемое обобщенными
координатами <pf (i: = 1,..., 5), совершим последовательные
повороты в шарнирах на углы ср,, начиная от неподвиж-
ного звена.
Первый поворот совершим в шарнире А на угол <рх
вокруг вектора к. При этом векторы i2, i3j i4,~ i5 изменят
свое положение и перейдут в векторы i$\
Эти векторы могут быть найдены по формуле Родриго
= iscos ф1 + (к х is)sin <рх + (1 —cos cpj (к• is)k (s=2,3,4,5),
где i, J, к — единичные векторы (орты) декартовой системы
координат.
Ввиду того что векторы is коллинеарны между собой и
ортогональны вектору к, член, содержащий три вектора
в последней формуле, равен нулю; с учетом этого получим
*11) = is cos (Pi 4- (£ х is) sin <Pi (s = 2, 3, 4).
Векторы j2, Л, Л остаются без изменения.
1
Второй поворот совершим в шарнире В на угол ф2
вокруг вектора При этом изменят свое направление
векторы /2, 7з, j4, которые перейдут в векторы Д2), 7з2), Д2),
причем
7i2) = 7s cos ф2 + х 7s) sin ф2
(s = 2, 3, 4).
Подставляя в эту формулу выражение для получим
712) = Js COS ф2 + [4 COS ф! + (к X О sin ф1] js sin ф2 »
s=7e cos ф2 + i, х 7, cos ф1 sin ф2 + i,sin фх sinф2 (s = 2, 3, 4).
Положение осей пар С и D при этом движении не
изменится.
Третий поворот осуществим в шарнире С на угол фз
вокруг вектора коллинеарного вектору i^y; при этом
изменяется направление векторов, связанных со звеньями
4, 5. Следует иметь в виду, что два поворота на углы
ф2 и фз вокруг параллельных осей эквивалентны одному
повороту на угол (ф2 + Фз), поэтому можно записать
Jf1 = J. cos (<р2 + Фз) + I. X A COS ф! sin (ф2 + фз) +
+ i. sin ф1 sin (ф2 + фз) (s = 3, 4).
Четвертый поворот осуществляется вокруг оси пары D,
параллельной осям пар В и С; при этом можно определить
новое положение оси пары Е, задаваемое вектором j43):
Л4) =7< cos (ф2 + фз) +£4 cos фх sin (ф2 + фз 4- ф4) +
+ ившф! вш(ф2 + Фз + ф4).
Пятый поворот осуществляется в паре Е на угол ф5 вокруг
вектора при этом определяется положение вектора i5,
связанного с этим звеном,
cos ф5 +7?} х i5 sin ф5.
После определения векторов, задающих положения осей
пар и звеньев, легко находятся абсолютные положения
точек механизма, например:
Ре
Цк +
где lt — длины звеньев.
Найденные векторы полностью определяют абсолютное
положение манипулятора в пространстве.
1
2.2. Обратная задача о положениях
1
элементов
конструкции, конструирования
сложной задачей в теории механизмов счи
()оратная задача о положениях состоит в определении
переменных параметров манипулятора, его обобщенных
координат при заданном положении выходного звена —
схвата.
Так же как задача о положениях в механизмах замк-
нутого типа, обратная задача о положениях является одной
из основных задач кинематического анализа и синтеза ма-
нипуляторов.
Результаты решения этой задачи используются как при
управлении манипулятором, так и при его проектировании
для определения требуемых характеристик приводов
перемещений в кинематических парах, необходимых для
размещения
звеньев.
Наиболее
гается задача об определении положений пространствен-
ного семизвенного механизма с вращательными парами.
Так как пространственный манипулятор с шестью враща-
тельными шарнирами при заданном положении схвата
превращается в замкнутую систему, то задача определения
относительных положений звеньев оказывается эквивалент-
ной задаче о положениях пространственного семизвенника.
До сих пор не получено аналитическое решение этой
задачи в явном виде, хотя в отдельных частных случаях
задача решена.
Можно показать, что для манипулятора с шестью вра-
щательными парами обратная задача о положениях может
быть сведена к решению уравнения не выше 4-й степени
лишь при условии пересечения трех последовательных вра-
щательных пар в одной точке. В этом случае три вращатель-
ные пары оказываются эквивалентными одной сферической
кинематической паре. Такие структуры кинематических схем
манипуляторов будем называть особыми.
Ниже рассмотрим примеры и методы решения обратных
задач манипуляторов, имеющих особые структуры или число
вращательных пар, меньшее шести.
Обратная задача о положениях манипулятора с пятью
вращательными парами (рис. 2.1). Оси кинематических пар
В, С, D параллельны между собой.
Манипуляторы с пятью степенями свободы применя-
ются довольно широко в промышленности, так как для
осуществления произвольного перемещения точечного
объекта и придания заданного направления рабочему
(
I
!
1
23
1
инструменту типа цилиндра достаточно пяти степеней
свободы.
Для решения этой задачи со звеньями механизма свя-
жем системы координат так, как показано на рис. 2.1. Со
стойкой свяжем неподвижную систему координат Axoyozo.
Со звеном 1 свяжем систему координат Ax^Zi, направив
ось Zi по оси вращательной пары Л, а ось Xi — парал-
лельно оси вращательной пары В. Со звеном 2 свяжем
систему координат Bx2y2z2, направив ось х2 по оси
вращательной пары В, а ось у2 — вдоль звена 2. Анало-
гично свяжем системы координат Cx3y3z3 и Dx4y4z4 со
звеньями 3 и 4 соответственно. Со звеном 5 свяжем систему
координат Ex3yszs, направив ось у5 вдоль оси вращательной
пары Е.
Заданными при решении этой задачи считаем следую-
щие величины:
координаты точки М, принадлежащей переносимому
объекту и одновременно схвату манипулятора (т. е. звену 5), —
Уzm>
проекции ортов неподвижной системы координат Ex5y5z5i
определяющих ориентацию объекта и схвата:
длины звеньев: 1Ъ 1Ъ 13, 14, 15.
Определению подлежат обобщенные координаты Ф1, Ф2,
Фз, Фд, Ф5» характеризующие относительное положение,
звеньев манипулятора.
Задачу решаем векторным методом. Для рассматри-
ваемой схемы можно записать:
Рм — К + К + ?з + U + h • (2.4)
0
0
Векторы /ь 12 и 13 имеют следующие проекции на оси
неподвижной системы координат:
*
—
к
L- /3 cos (<р2 + Фз) sin (pt I
Z3cos((p2 4-фз)СО8<Р1 I.
l3 sin (ф2 + Фз) J
—12 cos ф2 sin ф10
l2 cos ф2 cos ф10
sin ф?
1
Векторы Ц и ls совпадают по направлению с осью у3;
1ик как орт j5 оси у5 известен (известны его проекции),
то они могут быть определены следующим образом:
Ц ®= k/sJ h —
Управление углами фь ф2, ф3 может быть использовано
для перемещения объекта, зафиксированного в схвате, или
свободного схвата (точка М) в рабочем пространстве
манипулятора. Определим эти углы. Запишем:
$D — — ^4 — ^5 > PD = + »2 + »3« (2.5)
Умножив уравнение (2.5) скалярно на соответствующие
орты неподвижной системы координат i0, Jo, получим:
xD = - [/2 cos ф2 + h cos (ф2 + Фз)] sin ф!;
Уп = [h cos ф2 4- l3 cos (ф2 + Фз)] cos Ф1;
zD - It 4- l2 sin ф2 + l3 sin (ф2 4- фз).
Из уравнений (2.4) и (2.5) аналогично получим:
Xd = — (Ц + h)jsxi
Уи = Ум (^4 4" /5) J5yj
2d = ZM — (J4 +
(2.7)
Используя выражение (2.7), можно определить координа-
ты точки D в неподвижной системе координат. Тогда
из выражений (2.7) могут быть определены <рь ф2, ф3.
Возводя в квадрат и складывая второе и третье уравнения
(2.6), получим
(yp/cos Ф1)2 4- (zD - IJ2 = I2 4- /3
2/2/3 cos фз,
(2.8)
откуда
COS ф3 —
(yD/cos фО2 4- (zD - /J2 -
I —ЧИП1 I—И —НИ ,» I ! I. I I Р
2/2/3
(2.9)
Из первых двух уравнений (2.6) получим уравнение
tg Ф1 = ~xD/yD,
(2.Ю)
решение которого
Ф! = arctg (- xD/у d)
не однозначно [так как одному значению отношения
(—xd/уо) соответствуют два значения угла ф i в пределах 2л]
без учета значений xD и yD. Предположим, что угол
может изменяться в пределах —л < фх л. В этом случае
25
1
уравнение (2.10) имеет следующие однозначные решения
в зависимости от значений хв и ув:
<р! = arctg (- хв/ув),
Ф1 ~ — тг/2,
Ф1 = arctg (- хв/ув) - я,
Ф! = arctg (- хв/ув),
Ф1 = 4-я/2,
Ф1 = л + arctg (~xD/yD),
если xD >0, yD > 0;
если xD >0, yD — 0;
если xD >0, yD< 0;
если xD <0, yD > 0;
если xD
если xD <0, yD< 0.
Уравнение (2.9) дает
m 4- о (yD/COS ф 10)2
Фз + arccos--------------
два решения:
Оба эти решения могут быть реализованы (рис. 2.2, а, б),
причем каждому из двух значений угла ф3 соответствует
определенный угол ср2, что также видно из рисунка.
Для определения угла ф2 из первого уравнения (2.6)
найдем cosq>2*
COS ф2 =
h sin фз
12 4- h cos фз
sin ф2 —
xD
(2.12)
Из третьего уравнения (2.6) найдем зтф2:
sin ф2 —
l2 + h cos фз
h sin фз
——------cos ф2.
4- /3 cos фз
(2.13)
Подставляя (2.13) в (2.12), после преобразований получим
Рис. 2.2
26
Дробь xp/sin ф1 обозначим через к. При фх = 0 к = — 1,
при ф1 = 0 и Фз=0 fc=—2. Используя (2.12) и (2.13),
можно однозначно определить значение угла ф2 при уже
известных фь ф2. Для этого можно, используя выражение
(2.14), рассчитать два значения угла ф2 и, используя выра-
жение (2.12) в качестве условия, проверить, какое из этих
двух значений является правильным. Кроме того, на значе-
ние обобщенной координаты ф2 нужно наложить ограни-
чение
—*/2 < ф2 < Л/з,
(2.15)
так как при ф2 == “/2 величины хм и ум станут равными
нулю и определение обобщенной координаты Ф1 по фор-
мулам (2.10) станет невозможным. В этом случае величину
Фх можно определить лишь на основе анализа направлений
ортов i5 и js.
Управление углами ф4 и ф5 позволяет придать объекту,
зафиксированному в схвате, или свободному схвату задан-
ную ориентацию. Эти углы определим, зная проекции
ортов системы координат Ex5y5z5.
Проекции орта j3, соответствующего оси у3,
Г j3x I -sin фх cos(ф2 + фз)
/з =| J*3y I" COS фх COS (ф2 + фз) I.
LbJ L вт(ф2 + ф3) J
(2.16)
Проекции орта i4 оси х4
(2.17)
Так как проекции ортов всех осей координат Ex5y5z5
известны, то можно записать:
[з h = cos ф4; I j3 X js I = sin <р4;
U • 1’5 = cos ф5; | i4 х is | = sin ф5.
Выражения (2.18) представляют собой скалярные и век-
торные произведения соответствующих ортов. Используя
свойства векторного и скалярного произведений, запишем:
COS ф4 =
УЗх/бх J*3jj5y 4“ jzzJSzt
(2.19)
sin ф4 ==
[OWs
Из J5
JsxJStf
0*3x75
; i \2Tj/2.
J3yJ5x) J »
(2.20)
COS ф5
(2.21)
8И1ф5
i i Й
“ Ызу)
• •
^4y^5y
(2.22)
j3 x j5. Этот вектор й согласно
В выражениях (2.20) и (2.22) знак перед радикалом одно-
значно не определен. Для определения знака перед радика-
лом в выражении (2.20) необходимо принять во внимание
направление вектора п
определению векторного произведения, перпендикулярен
плоскости, задаваемой векторами j3n Js, т. е. параллелен
вектору z4. Если векторы п = j3 х и i совпадают по
направлению (т. е. имеют проекции на неподвижные оси
координат, одинаковые по знаку), то знак перед радикалом
в выражении (2.20) будет положительным, а если векторы
п и 14 противоположны по направлению (т. е. знаки проек-
ций этих векторов хотя бы на одну из осей неподвиж-
ной системы координат не совпадают), то знак перед ради-
калом в выражении (2.20) будет отрицательным. Знак перед
радикалом в выражении (2.42) определяется аналогично —
сравнением направлений векторов т = i4 х is и ]3: при
совпадении направлений (проекции с одинаковыми знаками)
будет знак плюс, при несовпадении (знаки проекции
личны) — знак минус.
Векторное произведение векторов n—j3 х ]5 имеет
екции
раз-
про-
Jiyjsz
jszjix
— J 3x7 5 у
“ J3j5y I
— hxjs* I-
~ fajsx J
Векторное произведение векторов m = i4 x i5 имеет про-
екции
J
1
По значению косинуса (2.19) определяем два возможных
значения угла ф4 и отбираем правильное из них, используя
выражение (2.20). Аналогично поступаем при определении
угла <р5, используя выражения (2.21) и (2.22).
Таким образом, все обобщенные координаты определены.
Следует иметь в виду, что с помощью манипулятора с
пятью степенями свободы нельзя осуществить заданное
произвольное положение и ориентацию объекта. Для этого
необходимо иметь число степеней свободы манипулятора,
равное шести. Следовательно, в данном случае на возмож-
ности ориентации объекта наложены некоторые ограничения.
Обобщенные координаты фь ф2, Фз обычно не используются
для ориентации объекта. Используя матрицы перехода,
определим проекции ортов у4 = j5 осей у4 и у5 (из рис. 2.3
видно, что эти оси совпадают между собой):
cos фг
sin ф!
_ 0
— sin ф! 0
СОБф1 0
0 1.
1 о
0 cos а
_0 sin а
0
— sin а
COSOL-
COS ф!
8Шф]
_ о
— sin фх 0
СО8ф1 0
0 1_
0
cos а
_sin а
— sin фю cos а
со5фю cos а
_ sin а
где а = ф2 + Фз + ф4 •
(2.23)
Из (2.23) видно, что положение осей у4 и у5 определя-
ется лишь одной из ориентирующих обобщенных коорди-
нат ф4 и действительно не может быть произвольным.
Для рассматриваемой схемы манипулятора расположение
оси у5 должно удовлетворять условию: проекция вектора й,
являющегося векторным произведением ортов у5 и у3, на
ось z0 должна быть равна нулю. Запишем выражение для п
в развернутом виде
й=у‘з х у5 = {j3yjSs +
+ (hzisx ~J3xjsz)jo +
+ Озх/ 5y J3yj5x) ^0 • (2.24)
Выражение
J3xjsy “ J3yjsx — 0 (2.25)
является условием правильной
ориентации оси у5 объекта и
Рис. 2.3
схвата. Из выражения (2.25) получаем
hx/hy Xhx/hy
Подставляя (2.17) в (2.26), получаем
hx — sin <рх cos (ф2 + Фз) sin ф!
j5, “ cos ф1 cos (<р2 + Фз) cos ф1
Согласно (2.18), имеем
(2.26)
(2.27)
jsxUsy ^d/Ув* (2.28)
Итак, при задании проекций орта J5 необходимо следить,
чтобы j5x и jSy находились между собой в соотношении
(2.28). Этому условию, например, удовлетворяет операция по
установке объекта на горизонтальную плоскость (плоскость,
перпендикулярную оси z0) так, чтобы его ось у5 была
перпендикулярна этой горизонтальной плоскости (рис. 2.3).
В этом случае проекции орта на оси неподвижной
системы координат будут равны
(2.29)
Таким образом, для определения обобщенных координат
манипулятора, схема которого изображена на рис. 2.1, может
быть использован следующий алгоритм:
по формулам (2.7) определяем координаты точки £>;
по формулам (2.11) определяем значение обобщенной
координаты ф!;
по формуле (2.9) определяем два значения обобщенной
координаты ф3;
по формулам (2.13) и (2.14) определяем два значения
обобщенной координаты ф2 (для двух значений ф3);
по формулам (2.19) — (2.22) по заданным проекциям
ортов системы координат £x5y5z5 [с учетом условия (2.25)]
определяются значения обобщенных координат ф4 и ф3.
Очевидно, что предложенный алгоритм связан с большим
объемом вычислений. Поэтому для подобных расчетов целе-
сообразно использовать ЭВМ.
В качестве примера определены с помощью программы,
реализованной на ЭВМ, обобщенные координаты манипуля-
тора при следующих исходных данных:
координаты объекта: хм = 100 мм, ум = 150 мм, zM =
= 250 мм;
проекции орта i5: iSx = 1, iSy — 0, i5z = 0;
проекции орта j5: j5x - 0, j5y = 0, j5z = -1;
длины звеньев манипулятора: li — 200 мм, l2 = 200 мм,
/3 = 200 мм, = 50 мм, /5 = 50 мм.
30
1
В результате расчета получаем следующие значения обоб-
щенных координат (град):
Вариант (pi Ф2 Фз Ф4 Ф5
1 -33,690 — 14,342 108,210 176,133 - 33,690
2 - 33,690 93,867 -108,210 - 75,657 - 33,690
Обратная задача о положениях манипулятора с шестью
вращательными парами. Манипулятор (рис. 2.4) от ранее рас-
смотренного отличается наличием еще одной кинематической
пары для ориентирующего движения, при этом число
степеней свободы рассматриваемого манипулятора равно
шести. Обратная задача о положениях для манипулятора
такой структуры была решена впервые в работе [33];
использовался векторный метод с матричными формулами
для преобразования координат векторов.
Известны следующие величины:
координаты точки М, принадлежащей объекту или
схвату: ум, zM;
1
проекции ортов системы координат Fx6y6z6l определяю-
щих ориентацию объектов или схвата:
Следует найти обобщенные координаты <pt, <р2» Фз, Фд,
Фз, Фе, определяющие относительные положения звеньев.
Используя векторный метод, можно записать:
Ре
Рм
Ре = Рм
(2.30)
(231)
(2.32)
(2.33)
(2.34)
(2.35)
Из (2.35) следует:
%Е ХМ (^5 "1” ^б) Убх »
УЕ ~ УМ “ (!s +
ZE — ZM — (Js + Uj6z-
(2.36)
Проекции на оси неподвижной системы координат век-
торов /ь /2, h, U следующие:
[ — l2 cos ф2 sin Ф1
l2 cos <р2 cos фх
l2 sin ф2
— /3СО8(ф2 + фз)8Шфх I
/3СО8(ф2 + фз)СО8ф1 I;
L /3 sin (ф2 + Фз) J
>1 1Ш —
-/4СО5(ф2 + фз)^Пф1
/4СО8(ф2 + ф3)СО8ф!
/4 sin (ф2 + Фз)
(2.37)
Используя (2.37) и (2.32), найдем координаты точки Е:
хЕ = 0 — l2 cos ф2 sin фх - /3 cos (ф2 + Фз) sin фх -
- /4 cos (ф2 + Фз) sin фх;
уЕ = о + l2 COS ф2 COS фх + I3 COS (ф2 + ф3) COS фх +
+ Z4 cos (ф2 + Фз) cos фх;
zE = /х + l2 sin Ф2 + /3 sin (ф2 + Фз) + h sin (ф2 + Фз) •
1
После преобразований получим:
хЕ == - [l2 cos <р2 + (/3 + U) cos (<р2 + Фз)] sin фх;
Уе = Dz cos ф2 + (/3 + /4) cos (ф2 + фз)] cos ф1;
(2.38)
zE = + h sin ф2 + (/3 + /4) sin (ф2 + ф3).
Используя (2.36), (2.38) и методику, изложенную выше,
можно найти обобщенные координаты фь ф2, ф3.
Для нахождения обобщенных координат ф4, ф5, ф6
определяющих ориентацию схвата, запишем:
Уз ‘Уб — cos Ф5.
(2.39)
Проекции орта /б известны (они заданы в исходных
данных), проекции орта j3 следующие:
—sin фх cos (ф2 + фз)
cos фх cos (ф2 + фз)
♦
sin (<р2 + <р3)
(2.40)
Из (2.39) и (2.40) следует:
COS ф5 = -j6x Sin фх COS (ф2 + фз) +Убу cos cos (ф2 + фз) +
+ ;б28ш(ф2 +фз); (2.41)
ф5 = ±arccos [(/6v cos фх - j6x sin фх) cos (ф2 + ф3) +
+ Уб2 sin (ф2 + фз)]. (2.42)
Из (2.42) следует, что обобщенная координата фз может
принимать два значения при одном и том же положении
схвата (звена 6).
Используя векторное произведение ортов j3 и ;6, опре-
делим проекции орта i5:
*5 = 0*з х yeJ/sm Фз. (2.43)
Используя формулы
sin фз = ±]/1 - cos2 ф5
и (2.43), получаем проекции орта i5
(/3)Убг y'3z/6y)/i|/^
15 =» ОзЛх - У ЗлУбг)/± j/1 “ COS2 ф5
~0злУб
COS ф 5
Л^)/± j/i
COS ф5 J
(2.44)
(2.45)
Орт is может иметь два взаимно противоположных
направления при одном и том же положений схвата в
соответствии с двумя значениями угла Фз-
2 -Механика промышл. роботов, кн. 1
33
1
Для определения угла ф4 используем выражения
i3 • is ® cos <р4; (2.46)
Пз * h I = sin <Р4 • (2.47)
Проекции орта i3
Г *3x1 Г COS фЛ
’зу 1= «тф1 I, (2.48)
L /32 J L О J
т. е. •
>
?з ~ *о cos Ф1 4- Jo sin ф1. (2.49)
Проекции i5x, iSyi i5g орта i5 можно записать, используя
(2.40) и (2.45), в следующем виде:
[/6l cos <р! cos (<р2 + Фз) - jiy sin (<ра + фз)] |
j/1 - COS2 ф5
[/бх sin (Ф2 4- Фз) + Л, sin ф10 cos (ф2 4- ф3)] к
j/1—СО82ф5
[-/бу sin ф! cos (ф2 4- Фз) ~ kx COS Ф1 cos (ф2 4- ф3)].
j/1 - COS2 ф5 J
(2.50)
где к = ± 1.
Из (2.46) следует
cos Ф4 — iixisx
• •
Из (2.47) следует
sin Ф4 i [(ijylSz ^3z^5y)
+ (Ws? — hyisx)2] ,2 •
03zGx ^3x^5z)
(2.51)
Знак перед радикалом в (2.51) будет положительным, если
знаки проекций векторного произведения i3 х i5 и орта у3
будут совпадать, в противном случае знак будет отрица-
тельным. Так как вектор i5 может иметь два направления,
то обобщенная координата ф4 будет принимать два значе-
ния (в 'зависимости от направления i5). Однако анализ
показывает, что эти два значения обобщенной координаты
Ф4 будут отличаться друг от друга на величину я. •
Наложив ограничения, например —л < ф4 4-л, можно,
проанализировав полученные значения, выбрать такое значе-
1
ние ф4, которое удовлетворяло бы принятым ограниче-
ниям. При этом однозначно определятся обобщенная коор-
дината ф5 и направление орта i5.
В случае, если обобщенная координата <р5 принимает
значение <р5 =0; <р5 = ±я, т. е. когда sin <р5 =0, возникает
неопределенность при отыскании значений обобщенных
координат <р4 и ф6, так как в этом случае орты j3 и j5
параллельны и управление обобщенными координатами ф4
и ф6 будет дублировать друг друга. В этом случае будем
считать, что обобщенная координата ф4 = 0, орт i5 парал-
лелен орту i3 и ориентация объекта осуществляется
обобщенной координатой ф6.
Для определения угла ф6 также используем выражения
для векторного и скалярного произведений ортов i5 и i6:
is • i6 = cos ф6;
I i5 x i6 | = sin ф6.
(2.52)
(2.53)
Проекции орта i6 известны. Проекции орта i5 определены
ранее [см. (2.50)]. Из (2.52) следует
COS фб = i$x^6,x "i" ^5у^6у 4“ ^5z^6z • (2.54)
Из (2.53) следует
sin фб =
V*5z^6x
^*5x^6z)
(2.55)
Знак перед радикалом в (2.55) будет положительным,
если знаки проекций векторного произведения i5 х i6 и орта
/б будут совпадать, в противном случае знак будет отри-
цательным. Так как вектор i$ может иметь два направления,
то обобщенная координата фб может принимать два зна-
чения (в зависимости от направления i5).
Таким образом, все обобщенные координаты определены.
Рассматриваемый манипулятор обладает шестью степенями
свободы и может ориентировать объект нужным образом
в пространстве. Поэтому ограничений на задание положе-
ния объекта или схвата в данном случае нет.
Алгоритм определения обобщенных координат манипуля-
тора, схема которого изображена на рис. 2.4, следующий:
по формулам (2.36) определяем координаты точки Е;
по формулам, аналогичным (2.11), определяем значение
обобщенной координаты ф i;
по формуле, аналогичной (2.9), определяем два значе-
ния обобщенной координаты ф3;
1
по формулам, аналогичным (2.13), (2.14), определяем два
значения обобщенной координаты <р2;
по формулам (2.42)—(2.51) определяем возможные значе-
ния обобщенных координат <р4 и ф5 и, используя наложенные
на <р4 ограничения, выбираем необходимые значения ф4 и <р5;
используя формулы (2.54), (2.55), определяем значения
обобщенной координаты <р6.
Для расчета обобщенных координат рассматриваемого
манипулятора может быть использована программа на
алгоритмическом языке БЕЙСИК, реализующая предло-
женный алгоритм.
В' качестве примера определим обобщенные координаты
манипулятора при следующих исходных данных:
координаты объекта: хм = 100 мм, ум = 150 мм, zM =
= 250 мм;
проекции орта i6: i6x = 1, i6y = 0, i6z — 0;
проекции орта j6: j6x = 0, j6y = 1, j6s = 0;
длины звеньев Манипулятора: Ц = 200 мм, 12 — 200 мм,
13 = 150 мм, 14 = 150 мм, 15 = 50 мм, 16 = 50 мм.
В результате расчета получаем
обобщенных координат (град):
следующие значения
Вариант <pj ф2 <рз qu Фз Фе
1 -63,435 -111,502 163,402 111,478 73,981 144,957
2 -63,435 159,692 -163,402 -88,146 63,495 85,853
Формулу (2.1) конечного поворота вектора можно исполь-
зовать при решении обратных задач для установления связей
между переменными и постоянными параметрами меха-
низма.
Такая методика применена в работе [53], где разработан
эффективный алгоритм решения обратной задачи о положе-
ниях манипулятора с шестью вращательными парами,
имеющего такую же схему, как на рис. 2.4. Особенностью
алгоритма является последовательное использование фор-
мулы конечного поворота вектора.
Известными считаются обобщенные координаты схвата:
углы Эйлера ф, ср, 0 и координаты некоторой точки С:
Хо> Уо, zo- Определению подлежат углы относительного
поворота звеньев <pf (i = 1, ..., 6).
Со звеньями манипулятора связываются правые системы
координат: So, St, ..., S6, оси которых Zf (i = 0, 1, ..., 6)
направлены по осям вращательных пар (рис. 2.5). Характер-
ные точки, точки углов ломаной, обозначены О, А, В, С.
1
Для решения задачи используются следующие орты:
i’o = *о, jo — 1 — орты системы So;
♦
Ц = 4, Уб = ^9» ^6 = е7 ~ орты системы 5б;
г —орт проекции оси Z6 на плоскость Ох0у0;
w = v х Ио — орт, компланарный плоскости Ох0Уо и пер-
пендикулярный вектору /с0»’
8, у, Р — углы между ортами
i0 и v, v и £6,w и i6 — отсчи-
тываются от первого орта ко
второму, если смотреть с кон-
цов ортов к0, vv0, ‘ £б соответ-
ственно.
Углы 8, у, р определяют
ориентацию системы S6 относи-
тельно системы So.
Уравнения связи между па-
раметрами получаются из усло-
вия приравнивания выражений
для Го, е7 и е8, записанных
через Уо> zo, S, у, Р, <рь ...
..., <рб.
Для составления выражений
fo, е7 и е8 в функции углов
<Pi ... ф6 используется соотно-
шение (2.1).
Алгоритм решения данной
задачи имеет вид [54]:
Рис. 2.5
1) ё7 = f0 cos 8 cos у + jo sin 8 cos у + k0 sin Y J
2) e8 » i0 (sin 8 cos p + cos 8 sin у sin p) +
+ Jo (—cos 8 cos p + sin у sin 8 sin p) — k0 cosy sin P;
3) Го = x&o 4- ytfo + toko - h = BC;
’ъ.
4) a = ]/(yo)2 + (zo)2; D = a2 + (*o)2:
5) проверка условия |G — GI < + /2J
6) sinipi =
Hi
cos ф1 = |11
jD . /--------2
7) cos фз =----------------» 3 4 sin Ф1 = P2 у 1 ~ cos Фз J
Zll<2
37
8) COS ф2 = - № sin Фз + Ц1О (I, + l2 COS Фз)] ;
sin ф2 = 4 W/2 Sin Фз +...];
9) ^4 =Jo cos <Pi + k0 sin <px;
e5 = i0 sin (<p2 + Фз) - Jo sin cos (ф2 + ф3) + k0 cos^ + ф2);
Es = i0 cos (ф2 + Фз) + Jo sin (ф2 + ф3) - £0 cos ф! sin (ф2 + ф3);
10) cos ф5 = e5e7; sin ф5 = ц3 |/1 — cos2 ф3;
11) cos фб = e4e8 cos ф4 — E5es sin ф4;
в
sin ф6 = e5e8 sin ф5 — e4e8 sin ф4 cos ф5 — E5e8 cos ф4 cos ф5,
где р. — принимает значения ± 1.
Анализ полученных выражений показывает, что сущест-
венно различными являются лишь два решения.
Обратная задача о положениях манипулятора с пятью
вращательными и одной поступательной парами (рис. 2.6).
Со звеньями манипулятора свяжем системы координат, как
показано на рисунке. Положение звена 6 задано. В этом
случае координаты точки F и орт оси пары к6 известны.
Определим координаты точки Е.
Можно записать векторную сумму
ЗВ
1
где pF и р£
Ре = Pf
радиусы-векторы точек F и Е, откуда
(2.56)
Проекции вектора р£, или координаты точки Е, в не-
подвижной системе координат равны
Орты осей В, D и Е (векторы i2, &4 и i5) определим,
используя формулу трех ортов (см. Приложение I).
Определим орт /с4. Имеем р£ = АВ
BE, откуда
(2.57)
Расстояние BE легко определить, зная координаты точек
В и Е:_____________________
BE = |/х| + уе + (ze — zb)2 » хв = Ув “ 0. (2.58)
Зная BE, к4 найдем из (2.57)
£4 = ~ВЕ/ВЕ. (2-59)
Векторное равенство (2.59) означает, что проекции орта
находятся через проекции вектора BE * следующим
образом:
(2.60)
0, где орты к0 и к
Определим орт i2 оси пары В. Запишем два скалярных
произведения: i2 ko=sfy <2’^4 =
известны.
По формуле трех ортов (см. Приложение I)
1*2 = ±(&0 Х ^4)|/1 ~~ Y04/U “ Y04),
где Yo4 = ^0' ^4 •
Орт i5 оси пары Е также определим по формуле трех
ортов. Запишем два скалярных произведениях этого орта
(2.61)
I5 • ко — 0, I5 • к4 — 0.
С учетом (2.1) и (2.61) запишем
I5 = i (fc4 X /с§) 1 74б/(1 У4б)>
где у4б = к4 •
(2.62)
1
Проекции вектора i5 согласно формуле (2.62)
4уЛ6з
4zrv6x
4xn6z
бу
4уЛ6х —
При известных ортах осей вращательных пар углы отно-
сительных поворотов звеньев определяются однозначно из
выражений
cos ф! = io • ii; sin ф! = (i0 x r i) k0;
cos <p2 = £1 sin ф2 = (Ht X 64);
cos фз = i2 • i5; sin фз = (i2 x i5) i2;
cosф5 = k4 • к6; sin ф5 — (к* x к6).
Так как положение звена 6 считаем заданным, то задан-
ными можно считать и все орты (i0, к6, j6). Тогда угол
поворота звена 6 в шарнире F определяется аналогично
(2.63) по формулам
cos ф6 ® i5 • ?б; «п ф6 = к5 (i5 х ?6).
Обратная задача о положениях манипулятора с шестью
вращательными парами, у которого оси трех последовательно
расположенных пар в середине кинематической цепи парал-
лельны (рис. 2.7). Заданными, как и выше, считаем положе-
ние выходного звена 6, т. е. положение точки F [pF(AF,yF,zf)],
и орты |6> /б, системы
со схватом.
координат, связанной
1
Определим положение точки Е — центра шарнира Е.
Запишем pF = р£ + EF = р£ + 15к6, откуда
(2.64)
Определим орт i4 оси вращательной пары D. Орты осей
пар В, С и D (соответственно i2, 1’3 и и) коллинеарны
между собой и перпендикулярны базовой плоскости ABCDE.
Вектор р£ принадлежит этой плоскости: таким образом,
орты i2> hi h ортогональны вектору р£, т. е. можно записать
(Ев х Ре)/(Ев * Ре) = U, (2.65)
где RB = /1 = АВ; р£ = х£ + уЕ + z2E.
Определим орт /5 оси пары Е. Орт j5 ортогонален ортам
Г4 и к6, поэтому можно записать
(2.66)
Имея выражения (2.66), для определения орта /5 доста-
точно применить формулу трех ортов
6
Y46 = U * ^6 -
(2.67)
Орты 1*2, h можно считать известными, так как они кол-
линеарны орту .
При известных ортах осей кинематических пар углы
относительного поворота звеньев определяются по следую-
щим формулам:
cos фх = Го • и ; sin (pi = (Го х Г1) к0;
COS Ф2 = • ^2, sin Ф2 — (&1 Х ^2) hi
cos фз = к2 ’ I sin фз — (к2 х &з) hi
cos ф4 = к3 • к4; sin ф4 — (к3 х к4) i3;
cos ф5 = к4 • ко; sin ф5 = (к4 х к6) i4;
cos ф6 = j5 -j6; sin ф6 = (/5 x j6) k5.
(2.68)
Приведенные выше методы решения обратных задач о
положениях в явном виде позволяют строить эффективные
алгоритмы управления роботами, а также являются основой
для последующих кинематических, и динамических расчетов
манипуляторов.
1
2.3. Прямая задача о скоростях
Прямая задача о скоростях состоит в определении абсолют-
ных линейных скоростей точек звеньев манипулятора и
абсолютных угловых скоростей звеньев при заданных зако-
нах изменения обобщенных координат (t) (i = 1, 2, ...» и),
где и — число степеней свободы манипулятора.
Так как радиус-вектор прризвольной точки звена манипу-
лятора ft (qk) — вектор-функция обобщенных координат, то
можно записать выражение для линейной скорости точки
звена
drt .
(2.69)
где qk — dqk/dt — обобщенные скорости. Выражения dftldqk
называются аналогами линейных скоростей.
Вопрос об угловых скоростях звеньев манипулятора рас-
смотрим ниже.
Метод приведения скоростей. Считаем, что в каждой кине-
матической паре манипулятора совершается одно движение:
вращательное или поступательное. Считаем также, что отно-
сительные движения в каждой кинематической паре заданы,
а требуется изучить движение звеньев манипулятора в не-
подвижной системе координат. Очевидно, что при одновре-
менном движении во всех кинематических парах происходит
сложение нескольких движений твердого тела. Рассмотрим
вначале сложение двух движений.
Пусть два звена входят в одну кинематическую пару.
С каждым из звеньев свяжем системы координат, соот-
ветственно и O2x2y2z2. Пусть первое звено и свя-
занная с ним система координат O^y^i совершает пере-
носное движение по отношению к системе Oxyz, а система
совершает относительное движение по отношению
к системе OpCijiZi. Тогда для произвольной точки М
второго звена скорость может быть найдена по формуле
сложения скоростей
VM = V1 + V2 ,
(2.70)
где Vi — скорость точки первого звена, с которой в данный
момент времени совпадает точка М второго звена; г2 —
скорость точки второго звена относительно первого.
Так как от перестановки слагаемых в формуле (2.70)
суммарный вектор не изменяется, то относительное и пере-
носное движения можно поменять местами.
1
кинематических пар увеличить на еди-
ницу, то найденную скорость точки
М звена 2 (рис. 2.8) можно принять
за переносную при определении ско-
рости точки М звена 3, т. е. скорость
точки конечного звена равна сумме
скоростей этой точки при движении
в каждой кинематической паре мани-
пулятора в отдельности:
VM =
(2.71)
Если все кинематические пары
4
Рис. 2.8
поступательные, то скорости
всех
точек конечного звена равны сумме линейных относи
тельных скоростей.
Если какая-либо из п кинематических пар вращательная,
то соответствующая ей скорость точки М находится
по формуле
(2.72)
где — вектор относительной угловой скорости во враща-
тельной паре г; fin — радиус-вектор, задающий положение
точки М звена п относительно центра пары i.
Рассмотрим вопрос о кинематике движения произволь-
ного звена манипулятора при известных законах относи-
тельного движения в кинематических парах.4 Во вращатель-
ных парах относительное вращательное движение звена i
по отношению к звену i — 1 характеризуется вектором
относительной угловой скорости со,, направленным по оси
вращения. Как известно, этот вектор является скользящим,
т. е. точка его приложения может переноситься лишь по
оси вращения.
Вектор относительной угловой скорости
где Qi — производная по времени от обобщенной коорди-
наты; (р, — обобщенная скорость (<pf — угол поворота в паре i).
В поступательной паре движение звена j по отношению
к звену j — 1 характеризуется вектором щ линейной ско-
рости, причем
— перемещение в поступательной паре j.
1
Рис. 2.9
Следует иметь в виду, что
вектор линейной скорости
является свободным, т. е. мо-
жет переноситься в произ-
вольную точку пространства
с сохранением величины и
направления.
Вектор угловой скорости
как скользящий вектор. Рас-
смотрим некоторые свой-
ства вектора угловой ско-
рости.
Сложение. Пусть звено манипулятора соединено с
неподвижным звеном кинематической цепью с двумя вра-
щательными парами (рис. 2.9), оси которых пересекаются
в одной точке. Векторы относительных угловых скоростей
в кинематических парах соответственно d>i и о>2. Сложим
эти векторы по правилу параллелограмма и запишем выра-
жение для скорости точки М (конца диагонали паралле-
лограмма) в сложном движении:
vM = ®i х ОМ + ш2 х ОМ — (а>1 + со2) х ОМ. (2.73)
Слагаемые <x>i х ОМ и о>2 х ОМ равны по величине и
противоположны по направлению, поэтому vM = 0. Равна
нулю также и скорость точки О. Следовательно, прямая
ОМ — мгновенная ось вращения, по которой направлен
вектор абсолютной угловой скорости звена 2, тогда
v>m ~ П2 х ОМ, (2*74)
где П2 — абсолютная угловая скорость звена 2.
Сравнивая (2.73) и (2.74), получим
П2 = 4- ю2
(2.75)
— векторы угловых скоростей складываются по правилу па-
раллелограмма.
Очевидно, при одновременном вращении вокруг п пересе-
кающихся осей угловая скорость звена п определяется по
формуле
(2.76)
Пара вращений. Если угловые скорости о»! и со2
равны по модулю и направлены в противоположные
стороны, то возникает пара вращений.
1
Покажем, что пара вращений дает поступательные ре-
зультирующие движения. Обозначим через А и В две точки,
взятые на мгновенных осях вращения, и запишем выраже-
ние для скорости произвольной точки М тела в сложном
движении (рис. 2.10)
— <»i х AM + d)2 х ВМ = ©t х AM - ©i х ВМ =
= d)i (AM — ВМ) = d>i х АВ.
Следовательно, скорости всех точек тела равны друг
другу, т. е. оно совершает поступательное движение.
Скорость поступательного движения перпендикулярна
плоскости пары вращения и равна v = odt где d — плечо
пары.
Перенос вектора со. Рассмотрим вопрос о переносе
вектора со в произвольную точку, не лежащую на оси
вращения.
Пусть звено совершает вращательное движение вокруг
мгновенной оси вращения, проходящей через точку А
(рис. 2.11). Возьмем произвольную точку Вив ней прило-
жим два вектора 6' и —со', равные по модулю и кол-
линеарные вектору со. Система векторов со и — со' обра-
зует пару вращения, эквивалентную свободному вектору v,
который можно приложить в любой точке, в том числе
в точке В:
v — & х АВ = тв (со).
Таким образом, вектор со можно переносить в произ-
вольную точку, добавляя соответствующий вектор линейной
скорости, равный моменту вектора со относительно точки.
Приведение векторов скоростей. Если звено манипулятора
соединено с неподвижным звеном кинематической цепью,
содержащей вращательные и поступательные пары, и в неко-
Рис.2.10
Рис. 2.11
1
торыи момент времени в них происходит движение, то это
звено совершает движение, являющееся суммой последних.
Для определения движений звена манипулятора в слож-
ном движении удобно воспользоваться методом приведения
скользящих и свободных векторов угловых и линейных
скоростей к некоторой точке звена. Как было показано
выше, любой скользящий вектор, в частности вектор, при-
ложенный в некоторой точке А на оси вращательной пары,
можно перенести в точку приведения О, прибавив при
этом присоединенную пару вращений.
Момент присоединенной пары равен моменту вектора со,
приложенного в точке А (центре вращательной пары) отно-
сительно центра приведения О, т. е.
т0 (б\) = f Oi х d)f,
(2.77)
где г Qi — радиус-вектор, определяющий положение центра
вращательной пары i относительно центра приведения.
Таким образом, после перенесения в центр О всех
векторов б,- в этой точке получим три системы векторов
со- = G>i и = fOi х ц (i = 1, ..., и).
Кроме того, при наличии поступательных пар в эту же
точку следует перенести свободные векторы Vj относитель-
ных поступательных скоростей звеньев в j поступательных
парах.
Линейную скорость точки О при движении только в
шарнире i можно записать так: г,- = со, х ri0, где гю — радиус-
вектор, определяющий положение точки О относительно
центра вращательной пары i.
Имея в виду, что fi0 = —гОь а также свойство вектор-
ного произведения, последнему выражению можно придать
вид
vt- = fOi х со,.
Из сравнения (2.77) и (2.78) следует, что
Vi = foi х &i.
(2.78)
(2.79)
Следовательно, при приведении всех векторов относи-
тельных угловых и линейных скоростей в кинематических
парах в центр приведения (полюс) О, там образуются три
системы векторов: со^ — со,, Vj и ц- = roi х •
Складывая в центре приведения все угловые со/ и линей-
ные Vj и vt скорости, получим главный момент всех
векторов в полюсе О, равный линейной скорости полюса:
* i
J
1
и главный вектор скользящих векто-
ров g)j в полюсе О, равный абсо-
лютной угловой скорости звена
COj.
Таким образом, при произвольном
наборе пар кинематической цепи,
соединяющей звено манипулятора со
стойкой, движение звена в любой
момент времени можно характери-
зовать вектором линейной скорости
Рис. 2.12
Vo полюса О и
скорости Йо.
вектором угловой
Покажем, что совокупность векторов v0 и Йо можно при-
вести к одному кинематическому винту (рис. 2.12). Разложим
вектор поступательной скорости v0 на составляющие vx и
v2 по направлениям вектора Йо и перпендикулярному ему,
тогда
Vj = v0 cos a; v2 = v0 sin а.
Систему перпендикулярных векторов Йо и v2 можно
преобразовать следующим образом (рис. 2.12). Заменим
мгновенную поступательную скорость v2 парой угловых
скоростей (Й'о, — По), принимая Йо = Йо. Это можно всегда
сделать, выбирая плечо пары.
Плоскость пары перпендикулярна вектору v2 и включает
вектор Йо. Перенося и поворачивая пару в ее плос-
кости, можно добиться расположения, показанного на
рис. 2.12.
Тогда векторы угловых скоростей Йо
и Йо
взаимно
уничтожаются и останется одно мгновенное вращение тела
вокруг мгновенной оси, проходящей через точку О, с угло-
вой скоростью Й'о = Йо, которое вместе с вектором v19 кол-
линеарным Йо, определяет винтовое движение тела вокруг
новой оси.
Пример: решение прямой задачи о скоростях методом
приведения. Найдем абсолютную угловую скорость точки
схвата манипулятора с двумя поступательными и четырьмя
вращательными парами (рис. 2.13). Относительные скорости
звеньев в кинематических парах считаем заданными (фь
s2, s3, q>4, фз, ф6). Эти относительные скорости являются
обобщенными, * если за обобщенные координаты принять
угловые и линейные параметры, определяющие относитель-
ные положения соседних звеньев. Введем векторы линейных
1
и угловых скоростей <в2, v2, и3, d>4, d>5, d)6 в кинематиче-
ских парах А, В, С, D, Е, F соответственно.
Определим абсолютную и линейную скорости полюса
звена 6. Приведем все векторы линейных и угловых ско-
ростей в полюс звена 6 (точку F). Для этого линейные
скорости в поступательных парах v2, v3 переносим в точку F,
а угловые скорости cb4, сЬ5, переносим в точку F,
обавляя соответствующие векторы v4, г5, v6 линейных
скоростей.
В данном случае абсолютная угловая скорость звена 6
0>4 + G)5 + G)6,
а линейная скорость полюса F находится по формуле
6
j= 1
где = cbx х AF — x (CE + EF); v2 = S2-£3; v3 = s3-i3;
v4 = d)4 x EF; v5 = ю5 x EF = 0; i3, k3 — орты системы
Cx3y3z3, связанной co звеном 3. Эти орты известны, так как
положение манипулятора в пространстве считаем заданным.
Зная векторы абсолютной угловой скорости полюса,
можно легко определить скорость любой точки М по
формуле
vM = vF + vMF = vF + Q6 x FM.
1
доен угловыхскоростей. Аналоги угловых скоростей
для пространственных механизмов с несколькими степеня-
ми свободы были введены и изучены в работах [47, 48].
Рассмотрим общий случай движения звена манипулятора
с п степенями свободы.
Получим проекции вектора угловой скорости звена на
подвижные оси, связанные со звеном. Для этого исполь-
зуем известные формулы для производных орт подвижной
системы координат
di . » di
. dfc
Эти
векторы представим в виде составляющих на подвиж-
ные оси
di
dt
— (oz *j — oy ’к’,
Cf)x • /с,
= Шу • i — ox ‘j.
Умножая приведенные выражения соответственно на /,
к, i, получим проекции угловой скорости звена на связан-
ные с ним оси [47]:
(2.80)
Считая, что орты осей i, j, к являются функциями обоб-
щенных координат механизма, можно записать
(2.81)
Подставляя последние выражения в формулы (2.80), по-
лучим проекции угловой скорости в виде
1
Последние выражения можно представить в векторном
виде
(2.82)
где — аналоги угловых скоростей.
Аналог угловой скорости есть вектор, совпадающий
по направлению и величине с вектором угловой скорости,
тогда qk = 1, а все другие обобщенные скорости равны нулю.
Так как для незамкнутой кинематической цепи манипу-
лятора абсолютная угловая скорость
к
то, сравнивая (2.82) и (2.83), получим
Qfc = ек.
(2.83)
(2.84)
Следовательно, для незамкнутой кинематической цепи
аналоги угловых скоростей равны ортам осей вращатель-
ных пар.
2.4. Обратная* задача о скоростях
Обратная задача о скоростях манипулятора состоит в опре-
делении обобщенных скоростей по заданным векторам
абсолютных угловой Q и линейной v скоростей точки
схвата в каждый момент времени. Для решения этой
задачи используется связь между векторами абсолютной
угловой скорости схвата (звена к) и линейной скорости
точки схвата, с одной стороны, и векторами относитель-
ных угловых и линейных скоростей в кинематических парах,
установленная выше, с другой стороны:
4 = Е + Е * г*); о» = Е ®.- (2.85)
j ♦ * * . * . *
причем со£ = e^cpf, Vj = положение манипулятора счита-
ется заданным, т. е. орты eh ej известны.
Выражения (2.85) можно* представить в виде
(2.86)
1
• (2.87)
i
Проецируя (2.86) и (2.87) на неподвижные оси, получим
систему шести линейных уравнений вида:
* к
Vkx ~ = У*,
i = 1 »= 1
к . к
vky ~ X a2iQb ^ку — ]£ a5iQb
i = 1 i = 1
к к
vkz — Zj a3iQii ^kz = a6i4i>
1=1 1 = 1
где 4i — обобщенные скорости.
В матричном виде эту систему уравнений можно запи-
сать так:
Ы = J [4],
(2.88)
где [г] — матрица-столбец проекций векторов абсолютной
угловой и линейной скоростей схвата; J — матрица разме-
ром 6 х и; [4] — матрица-столбец обобщенных координат.
Обратная задача о скоростях состоит в определении
обобщенных скоростей из системы линейных уравнений
(2.88). Формально задача сводится к определению матрицы
[4] из уравнения (2.88). Умножая левую и правую части
уравнения (2.88) на обратную матрицу J"1, получим
[«] = J 1 [»] •
Для решения задачи необходимо, чтобы ранг матрицы был
равен шести. В некоторых положениях манипулятора этот
ранг может стать меньше шести, в этих положениях точное
решение задачи невозможно. Такие положения будем назы-
вать особенными.
Вычисление обратной матрицы J~l является довольно
громоздкой операцией и требует значительного машинного
времени. Упрощения решения можно достичь, проецируя
векторные равенства (2.86) и (2.87) на оси системы коор-
динат, в которой проекции векторов eb Cj являются наиболее
простыми. Такими осями являются обычно оси, связанные
со звеньями манипулятора в середине кинематической цепи
или с точкой пересечения осей вращательных кинематических
пар. Применяя метод статики для определения реакций в
пространственных системах, составляем уравнения сил и мо-
ментов относительно тех осей, в которых эти уравнения
наиболее просты.
Рис. 2.14
Поясним этот метод на примерах.
Обратная задача о скоростях манипулятора с одной вра-
щательной н двумя поступательными парами (рис. 2.14). Рас-
положение системы координат показано на рисунке. Пусть
задана линейная скорость точки схвата v в виде проекций
на неподвижные оси.
Введем систему координат Dx3y3z3, связанную со звеном
3 и началом в точке D. Ось z3 параллельна оси пары Л,
ось х3 направлена вдоль звена 3.
Тогда проекции вектора v в этой системе
Мз = Ь3о [г]0,
где [г3] — матрица-столбец координат проекций вектора v
в системе Dx3y3z3; [г]0 — матрица-столбец координат векто-
ра г в неподвижной системе координат Axyz;
зо —
— sin ф х
COS ф!
О
— матрица преобразования координат при переходе от
системы Axyz к системе Dx3y3z3, тогда
v3x = vx cos <Pi — vy sin ф1; v3y = vx sin ф! + vy cos ф!; v3z = vs.
1
Проекции векторов vl9 v2, v3 скорости точки D от
обобщенных скоростей фх, з2, з3 соответственно равны
Ф1*з3, $2, s3. Приравнивая их проекциям скорости точки D
на оси, получим
vx sin q>i
fyCOSCPi
S2 = Vx COS Ф1 4- Vy sin Ф1.
Обратная задача о скоростях манипулятора с шестью
вращательными кинематическими парами (рис. 2.15). Пусть
заданы проекции векторов абсолютной угловой и линейной
скоростей, выходного звена 6 в неподвижной системе
координат
[vgj — [гх, Vy, tz]Tj [Og] — [Ох, fly, £1S]T
и конфигурация манипулятора, т. е. значения углов ф4 (i =
= 1,...» 6). Требуется определить обобщенные скорости <pfc
обеспечивающие заданные значения v6 и Й6.
9 ГИС. A.
1
Для проецирования векторов скоростей выберем систему
координат, связанную со звеном 3. Ось х3 этой системы
направлена по оси вращательной пары С, а ось z3 — по
оси звена 3 и совпадает с осью пары. Умножим матрицу-
столбец проекций этих векторов в системе Axyz на мат-
рицу L30 перехода от системы Axyz к системе Cx3y3z3.
Имеем
Мз = ^01^12^23 ЕГб]о — А)3 ЕГб]о »
Е^б]з — ^'01^'12-^13 Е^бJo —
(2.89)
^03 Е^б]о«
Матрица ЬОз
Ьоь Мг, -^23
[cos (pt
— sin (pi
0
[1 О
О cos ф3
О — sin ф3
находится как произведение трех матриц
sin ф10 II
cos Ф1 0 10
0 1 JLo
0
COS ф2
— sin ф2
sin ф2 х
я
COS ф2—1
0
sin ф3
cos ф3 _
(2.90)
Для сокращения выкладок удобнее находить проекции
векторов гб и Qg последовательным умножением матриц
справа в выражениях (2.89) с учетом (2.90).
Тогда проекции вектора v6 на оси системы Cx3y3z3
равны:
vx3 = vx cos ф1 4- [vy cos (ф2 4- ф3) 4- vz sin (ф2 4- ф3)] sin фх;
vy3 = ~ vx sin фх 4- Егу cos (Фг + Фз) 4- vz sin (ф2 4- фз)] cos фх;
vi3 = - vy sin (ф2 4- фз) 4- vz cos (ф2 + ф3). (2.91)
Аналогично находим проекции вектора О6.
Найдем моменты векторов угловых скоростей в парах •
относительно осей системы Ex4y4z4. В этих осях уравнения
моментов векторов скоростей имеют вид
X (<»<)=(ЛЕ х e2)i4(pi + (BE х е2)Г4ф2 4- (СЕ х е3)Фзй = vx4;
^wiy(iOi) = (BE x в2)у4ф2 4" (СЕ х Сз)фз/4 = vy4 j (2.92)
X ~ (&E X Cz) ^4<p2 = vz4.
В последнее уравнение входит лищь одна обобщенная
скорость <р2. Моменты всех векторов угловых скоростей
относительно оси Z4, кроме момента вектора <в2, равны 0.
1
Из третьего уравнения системы (2.92) находим
Из второго уравнения системы (2.92) находим обобщен-
ную скорость ф3:
. / (BE х е2) ]л \ г —►
<Рз = (t>,4 - )/[(С£ X гз)Л] •
Из первого уравнения системы (2.92) получим <рх:
. (BE х ё2) i^vz4. \ •
Ф1 “ Vx4 ~ —=5—-—-—(СЕ х e3)i4(p3.
(АЕ х е2)/с4
Для определения <р4, ф5, ф6 спроецируем векторы угло-
вых скоростей d>f (i — 1, ..., 6) на оси системы Ex4y4z4. Так
как оси кинематических пар В и С перпендикулярны оси z4,
а ось пары Е — оси х4, то проекции соответствующих
векторов на эти оси будут равны нулю, а другие проек-
ции находим, используя матрицы перехода. Имеем:
[(01}4 — -Е4зЕ32Е21 [(02]4 — ^43^32
[С0з]4 = ^43 [<°з]з> [«б]4 = ^45 [®бЪ •
(2.93)
Матрицы, входящие
в эти уравнения, выражаются так:
cos ф4 sin ф4 О I
Ь43 == — sin ф4 cos ф4 О I;
_ О 0 1J
1 о о
О cos (ф2 4- Фз) sin (ф2 4- Фз)
О -sin (ф2 4- фз) cos (ф2 4- фз)-
Г1 О О
Ь45 = I ° COS ф5 sin ф5 .
Lo — sin ф5 созф5_
(2.94)
С учетом (2.93), (2.94)
Qx = <Pi sin (ф2 4- фз)зш ф4 4- ф2 cos ф4 4- фз cos ф4 4- ф5 ;
= Ф18Й1(ф2 4- ф3)со8 ф4 4- ф2 sin ф4 4- ф3 sin ф4 + ф6 sin ф6;
О2 = <Pi cos (ф2 + фз) + ф4 4- фб cos ф6. (2.95)
1
Изл второго уравнения системы (2,95) найдем
Оу т- Ф1 sin (ф2 + фз)СОБ ф4 + (ф2 + фз) sin ф4
sin Фе
Из третьего уравнения системы (2.95) найдем
ф4 = Д: - Ф1 COS (ф2 + Фз) “ Фб COS ф6 .
*
Из первого уравнения системы (2.95) найдем
ф5 = Qx - ф1 sin (ф2 + фз)вш ф4 + (Ф2 + Фз)со8 ф4.
Таким образом, в явном виде получено решение обрат-
ной задачи о скоростях.
Определение обобщенных скоростей манипулятора, реали-
зующего движение по заданной траектории с заданной
ориентацией (рис. 2.16). Траекторию в пространстве зададим
как линию пересечения двух поверхностей
fi (х, У, z) = 0; /2 (х, у, z) = 0.
(2.96)
Ориентацию схвата в любой момент времени зададим
направляющими косинусами в виде
fi (х, у, z) = 0 (/ = 4, 5).
(2.97)
Кинематические уравнения движения инструмента в де-
картовой системе координат будем строить в виде [6]
х ® pi (х, у, z); у = р2 (х, у, z); z = р3 (х, у, z). (2.98)
1
4
Используя (2i96), получим
(2.99)
Считая рз произвольной функцией и решая систему
(2.99) относительно pi и р2> получим
• _ Рз ( dfi dfi
• Рз
У = Р2~Ч-
dj±df±
dz bx
dft ЛЛ).
dz by )'
dfi Э/Л
bz bx г
(2.100)
dfddx
df2/dx
dfi/dy
df2/dy_
Закон изменения направляющих косинусов может быть
получен непосредственным дифференцированием по времени
равенств (2.97):
где fj — функции, определяемые равенствами (2.97).
Наличие произвольных функций в выражениях для
Pj (*, У, z) позволяет наложить дополнительные условия на
движение по траектории. Можно потребовать, чтобы движе-
ние по траектории осуществлялось со скоростью v — v (t),
тогда
£ Pi = х2 + у2 + z? = v2 (t). (2.101)
i= 1
Пусть Qj — dfj/dz (/=1,2). Система (2.100) запишется
в виде
Рз /_ df2 df\\
Pi = = Рзйг; (2.102)
\ С/Л С/Л /
Рз ~ Сз,
где
Подставляя значения ръ р2, Рз из (2.102) в (2.101), полу-
чаем:
Рз = ± » (t)V 1 + Qi + Qi- (2.103)
Пусть теперь требуется, чтобы скорость увеличилась от
нуля до значения на некотором участке разгона или
уменьшилась от Vi до нуля на участке торможения, т.е.
v (t0) s 0, v (tj) — v — at, где a — касательное ускорение.
Используя (2.103), получаем
Рз = ± ot//1 + + Qi-
Итак, если накладывается на движение инструмента одно
из условий: движение с заданным законом изменения
скорости или движение с постоянным касательным ускоре-
нием, — то система уравнений (2.99) имеет единственное
решение (рь р2, Рз)-
Для определения обобщенных скоростей требуется знание
зависимостей
q„); j = l,...,m;
(2.104)
где qi9..., qn — обобщенные координаты.
Число уравнений т в системе определяется программой,
которую должен выполнять механизм. Для программы,
обеспечивающей движение по траектории, заданной уравне-
ниями поверхностей А = 0 и /2 = 0, и при нормально рас-
положенном к обрабатывающей поверхности /t инструменте
из (2.104), продифференцировав по t, получаем
(2.105)
Система (2.105) является линейной относительно и состоит
из шести уравнений с и > 6 неизвестными функциями qt.
Для манипулятора с шестью степенями свободы система
(2.105) будет иметь единственное решение, если положения
манипулятора неособенные.
58
1
Например, для манипулятора «Универсал-15» координаты
схвата выражаются формулами
= х = г cos 6 cos <р; х2 = у = г cos 0 sin ф; х3
= г sin 0 + h 9
где qt = г - s3; q2 = 0 = ф2; q3 = ф = Ф1-
Рассмотрим движение инструмента с постоянной ско-
ростью v. Пусть поверхности (хь х2, х3) = 0 и f2 —
=/2 (xi, х2, хз) = о представляют собой плоскости: уравне-
ние обрабатываемой плоскости зададим в виде
а уравнение направляющей плоскости — в виде
(2.106)
/2 (ХЬ х2, х3) = >12X1 + В2х2 + С2х3 + D2 = 0.
(2.107)
Тогда, согласно (2.100),
Vi/dxi
dfifixi
^fil^X2
dfi/()x2
Из тех же формул (2.100) находим Xj и х2:
Xi — Pi — ~^(QiB2 — Q2B1)', х2 — р2 — •д-((22>11 “ 61^2)?
61 = ~"с1Рз = ~~с1*з ~ Рз61> 61 — ~~ci 9 (2.108)
62= -С2Рз= -С2Х3 = P3Q29 62= ~f2-
Учитывая, что схват с инструментом движется с постоян-
ной скоростью, из формул (2.106), (2.107) находим
откуда
Рз (f2^1 “ ^1В2)
—* X1 “
_ • _ Рз(^1А2 — c2Ai)
Р2~Х2 AiB2 - BiA2 *
Рз = ± г/|/1 + (ciА2 - c2Ai)2
+ (Bic2 - ciB2)2.
Обобщенные скорости qt находим из уравнения
з
(2.109)
1
Итак, получили систему трех уравнений с тремя неиз-
вестными. Найдем коэффициенты: с
Vi
-—г = cos q2 cos q3;
OQl
dg2
ДА
qi sin q2 cos q3;
qi cos g2sing3;
д/2
-т— — cos q2 sin q3;
№
i>f2
df2
sin usings;
qt cos q2 cos q3;
dq3
df3
<>Qt
♦
r
ч
*
Следовательно, система вида (2.109) для данного случая
запишется так:
Кз n
~— = и.
Xi = cos q2 cos g34i - sin q2 cos q3q2 — qx cos q2 sin q3q3;
x2 = cos q2 sin q3q3 - qt sin q2 sin g342 + Qi cos q2 cos q3q39
x3 « sin g24i + Qi cos q2q2.
Решим систему относительно 41» 4г» 4з- Главный опре-
делитель системы
Д «= — qj cos^2;
дополнительные определители:
Д1 == -ql cos q2 [р3 sin q2 + cos q2 (pi cos q3 + p2 sin g3)];
' Д2 = Q1 cos q2 (pi sin q2 cos q3 + p2 sin q2 sin q3 - p3 cos g2);
A3 = g1cosg2cos^3(-p3sin^2sinQ3 — p2cosg2)~ gjcos^sin^ x
x (p3 sin q2 cos q3 - pt cos q2) + qt sin2 q2 (pt sin q3 - p2 cos q3).
Значения определителей найдены, тогда выражения для
обобщенных скоростей приводов манипуляторов, при кото-
рых реализуется движение захвата по прямолинейной траек-
тории с заданной скоростью v9 запишутся так:
41 == Д1/Д = р3 sin q2 + cos q2 (pt cos q3 + p2 sin g3);
4г — Д2/Д = -(l/^i)(pising2cos^3 + p2sing2sin^3 - p8cos#2);
4з = Дз/А*
Найденные значения обобщенных скоростей в кинемати-
ческих парах манипулятора, будучи реализованными с по-
мощью приводов, обеспечат движение схвата по заданной
траектории с заданной постоянной скоростью.
60
1
2.5. Угловые ускорения звеньев
и их аналоги
Угловые ускорения звеньев пространственных механизмов
с несколькими степенями свободы и их аналоги изучены
в работах [47, 48].
Угловые ускорения звеньев манипулятора можно рас-
сматривать в относительном и абсолютном движении.
В относительном движении угловое ускорение звена п
определяется в предположении постоянства орта еь т. е.
оно равно
(2.110)
угловое
ускорение звена п находится как
от вектора угловой скорости, т. е.
Абсолютное
производная по времени
(2.1И)
При дифференцировании выражения (2.111) следует учи-
тывать, что величины qk и ек в общем случае являются
функциями времени, а орты ёк определены в подвижных
системах координат. С учетом этого получим
(2.112)
Первый член этой формулы определяет угловое ускоре-
ние звена в начальном движении, т. е. в предположении,
что равны нулю все обобщенные скорости.
Второй член в формуле (2.112) выражает составляющую
углового ускорения в основном движении, т. е. когда равны
нулю все обобщенные ускорения.
Представим угловое ускорение звена в основном дви-
жении в виде
к—1s=1
(2.113)
= dCi.k/dqa —
аналог углового ускорения.
Основные свойства аналогов угловых ускорений. Так как
аналог угловой скорости 6* определен его проекциями
на оси координат звена, то можно записать следующее
выражение:
f
_ dCl*
(2.И4)
1
где первое слагаемое выражает относительную, или ло-
кальную, производную, а второе слагаемое учитывает
поворот системы координат звена, роль которого при
вариации обобщенной координаты qa выполняет ее аналог
Свойства аналогов углового ускорения могут быть пред-
ставлены следующими формулами [47]:
(2.115)
(2.116)
Формула (2.116) устанавливает правила замены индексов
при определении аналогов ускорений.
Свойства (2.115) аналогов ускорений могут быть доказаны
следующим образом [31]. Имея в виду соотношение
с учетом (2.80) получим
По формуле Эйлера имеем
(2.117)
(2.118)
Применяя правило преобразования
(а х 6) (с х J) = (ас) (&?) — (act) (be),
подставляем (2.118) в (2.117) и приходим к (2.115).
Исходя из (2.115), проекции аналога на оси звена
находим как производные от проекций по qa
= d2j - dj dk
dqs ~ dqkdqs J dqs dqk ’
d£lsy d2k * dk di
d<lk dqkdqs 1 + dqs dqk ’
dQ.sz d2i -? di dj
dqk ~ dqkdqk J dqs dqk '
(2.119)
При последовательном соединении звеньев получим
при s > к;
1
Для открытых кинематических цепей манипуляторов
аналоги угловой скорости Пк представляют собой единич-
ные векторы, коллинеарные осям вращательных пар части
кинематической цепи, соединяющей рассматриваемое звено
с неподвижным.
Для открытой кинематической цепи положение осей
номера к не зависит от положения осей номера s при
s > к, поэтому
а формула для углового ускорения звена получает вид
[47]
п к — 1 и
Ел ~ EksQkQs У ^kQk* (2.120)
k=2s=i k=l
Формулы для углового ускорения звеньев и их аналогов
ниже будут использованы для определения моментов сил
инерции звеньев.
2.6. Линейные ускорения
У
Линейные ускорения точек звеньев манипулятора могут
определяться различными способами.
Первый способ — нахождение ускорения точки звена
манипулятора как ускорения точки твердого тела при
известных ускорении полюса, абсолютной угловой скорости
и углового ускорения звена. В этом случае имеем
@Сп @Оп “Ь Ид X (Од X Уи) Ч* £д X Ув, (2.121)
где don — ускорение полюса звена и; Q„, 8И — соответствен-
но векторы абсолютной угловой скорости и углового ускоре-
ния звена и; у„ — радиус-вектор, определяющий положение
точки С относительно полюса О.
Второй способ — применение формулы производной
от функции нескольких переменных. По определению
ускорение точки равно второй производной по времени от
г с — ее радиуса-вектора. Радиус-вектор точки звена манипу-
лятора является векторной функцией обобщенных коор-
динат, т. е. гс = rc(qi) (i = 1,...,и). Тогда линейная скорость
точки звена манипулятора
63
1
ii ускорение
п п п
(2.122)
Здесь первый член формулы выражает ускорение точки
звена манипулятора в начальном, а второй — ускорение
точки в основном движении.
Третий способ — применение теоремы сложения уско-
рений. Рассмотрим движение звена п манипулятора, при-
соединенного к неподвижному основанию с помощью
кинематической цепи, содержащей вращательные и посту-
пательные кинематические пары.
Найдем абсолютное линейное ускорение точки С звена
номера п как ускорение точки в сложном движении,
принимая переносным движение звена и — 1.
В этом случае абсолютное ускорение точки С звена и
может быть записано в виде
аСп
где — переносное ускорение точки С звена и; —
относительное ускорение точки звена л; а„к — кориолисово
ускорение точки звена п.
Переносное ускорение точки С звена п есть абсолютное
ускорение точки С звена п — 1, с которой в данный
момент времени совпадает точка С звена л, т. е. можно
записать
On — On— 1.
Относительное ускорение точки С звена п характеризует
движение точки С звена п в подвижной системе координат
звена л — 1. Если кинематическая пара, образуемая звеном
п со звеном п — 1,— поступательная, то относительное
ускорение точки С звена п
где sn — относительное перемещение звена п в поступа-
тельной паре номера л; ё„ — орт, определяющий направле-
ние поступательного перемещения пары л.
Если кинематическая пара, образуемая звеном и со
звеном л — 1, вращательная, то относительное ускорение
точки С звена л определяется как ускорение во враща-
тельном движении вокруг оси пары л, т. е.
&п Би, л — 1 X Тп + (0и,и—1 X ((Ой,и—1 X Gi).
1
Здесь (ои,и-1, £п>и—1 — соответственно векторы относи-
тельной угловой скорости и относительного углового
ускорения звена п во вращательном движении по отно-
шению к звену /1—1, т. е.
И— 1 -- фи^’и, &П,П—1 ---- фп^и,
где фи — угол поворота звена п относительно звена п — 1
в кинематической паре п.
Кориолисово ускорение точки С звена п определяется
с учетом того, что переносным является движение звена
п — 1, т. е
где с5и -1 — абсолютная угловая скорость звена п — 1;
Й,п-1 — относительная скорость точки С звена п в ее дви-
жении в системе звена п — 1:
в случае поступательной пары п
в случае вращательной пары п
&п,п—1 — ^п.п—1 X Гп Ф«СИ X Гп.
Переносное ускорение точки С звена п при наличии
многозвенной кинематической цепи находится так же, как
ускорение точки в сложном движении. В этом случае
можно записать
где аеп _ 1, -1, а„_ х — соответственно переносное, относи-
тельное и кориолисово ускорения точки С звена п — 1.
Так же, как и для точки звена и, для точки звена
п — 1 можно записать
Для поступательной пары п — 1 имеем
Л ~
Для вращательной пары п — 1
&п— 1 — Ей— 1,и — 2 X Гп—1,п • 1,и —2 X 1,и —2 X Гп — !,«/»
где соп-1<п-2, £п-1,и-2 — соответственно относительные угло-
вая скорость и угловое ускорение звена п — 1 по отношению
к звену п — 2; f„_ i,п — радиус-вектор, определяющий поло-
жение точки С звена и. относительно центра вращатель-
ной пары п — 1.
3 Механика промышл. роботов, кн. I
65
Кориолисово ускорение а*-! определяется абсолютной
угловой скоростью звена п — 2 и относительной скоростью
точки С звена п — 1 по отношению к звену п — 2:
— 26и_2 х г£-1,и-2.
Здесь «„-г — абсолютная угловая Скорость звена и — 2;
Й -1, п - 2 — относительная скорость точки С звена п — 1 по
отношению к звену и — 2.
Можно продолжить процесс составления выражений
ускорений точки С до тех пор, пока переносное ускорение
этой точки не будет ускорением точки С первого звена.
. Суммируя выражения для составляющих ускорений точки
С, принадлежащей различным звеньям, получим (если все
кинематические пары' вращательные)
п
®Сп = £ Pfc.fc-1 Х 4n + X (<»*,*-1 X 4Л)] +
k = 1
п
~Ь 2 C0fc х (cOfc.fc—1 х г^л).
к= 1
Приведем несколько примеров определения линейных
ускорений точек звеньев манипуляторов.
Анализ ускорений манипулятора с двумя вращательными
и одной поступательной парами (рис. 2.17, а). Определим
линейное ускорение точки D, используя теорему сложения
Рис. 2.17
66
1
ускорений. За обобщенные координаты примем относи-
тельные перемещения <pt, ф2, Фз в кинематических парах
А, В и С соответственно. Заданы также обобщенные ско-
рости фх, ф2, s3 и обобщенные ускорения фХ, ф2, *$з-
Вначале за переносное движение примем движение зве-
на 2. Тогда абсолютное ускорение точки D как ускорение
точки в сложном движении может быть записано в виде
(рис. г 2.17,6)
aD3 = «з = «з + «з + Лз,
где аез — переносное ускорение точки D звена 3; «з —
относительное ускорение точки £>; а3 — кориолисово уско-
рение точки D.
Относительное ускорение точки D звена 3 определяется
движением в поступательной паре С и равно
«з = s3k3i
(2.123)
где кз = орт оси z3 системы Bx3y3z3, связанной со звеном 3.
Кориолисово ускорение точки D звена 3 определяется
переносной угловой скоростью звена 2 и относительной
линейной скоростью точки D3 в системе координат звена 2,
т. е.
(2.124)
где П2 ~ абсолютная угловая скорость звена 2; V32 - от-
носительная скорость точки D3 в системе координат звена 2.
Абсолютная угловая скорость звена 2 (П2) равна вектор-
ной сумме относительных угловых скоростей во вращатель-
ных парах:
Й2 = C0J + о2 = ф!k + (p2fi. (2.125)
Относительная скорость г^2 определяется скоростью
движения в поступательной паре С:
^32 — 53^3-
(2.126)
С учетом (2.125) и (2.126) выражение для ускорения
(2.124) получает вид
п3 = 2(ф!£1 + ф2й) х s3k3 = 2(ф1х38тф21?2 - ф25з/2). (2.127)
\ Переносное ускорение точки D звена 3 (п3) может быть
представлено, в свою очередь, как ускорение точки в слож-
ном движении, если принять за переносное движение
звена 1. В этом случае можно записать
<Я3 — Я2 — П2 + ^2 + П2,
(2.128)
1
где d2 — абсолютное ускорение точки D звена 2; а2 —
переносное ускорение точки D звена 2, это ускорение равно
ускорению точки D звена 7, т. е.
= dp (2.129)
Точка D звена 1 совершает вращательное движение
вокруг оси вращательной пары Л, следовательно, уско-
рение di складывается из нормального и касательного
ускорений точки D в этом движении
dj = d"j + <?i. ’ (2.130)
Наиболее простые выражения имеют проекции этого
ускорения на оси системы Bx^iZ^. В проекциях на эти
оси можно записать
di = ii-(p1s3sin<p2; (2.131)
di = Ji • (pis3 sin <p2, (2.132)
или так:
di = Ji • <PiS3 sin <p2 + i'i • (piS3sin (p2. (2.133)
Относительное ускорение точки D звена 2 (d2) опреде-
ляется движением во вращательной паре В. Это ускорение
наиболее просто выражается в проекциях на оси звена 2
как ускорение во вращательном движении:
d^ = d^" + d2 = — (р 2s3k2 — (p2Sjj2.
Кориолисово ускорение точки D звена 2
(2.134)
«2 = х vr21, (2.135)
где Q] — абсолютная угловая скорость звена 7, равная угло-
вой скорости во вращательной паре А, т. е.
Й1 = ФХ (2.136)
Относительная скорость точки D звена 2 в системе
координат звена 1 (ff2l) определяется движением во враща-
тельной паре В:
(2.137)
ч
С учетом формул (2.136), (2.137) кориолисово ускорение
d2 = 2ф1£ х (p2Sjj2 = 2(pi<p2j2cos(p2.
Полное ускорение точки D звена 3 определяется вектор-
ной суммой его составляющих, определяемых формулами
(2.123), (2.127), (2.133), (2.134), (2.138). Суммируя проекции на
одноименные оси, получим
(2.138)
1
«D3 = «3 = *2 (ф1$3 sin ф2 + Ф153 sin ф2 + 2ф1ф2 cos ф2) +
+ j2( —Фг^з — Фг5з)
£г($з - Ф2^з) +Лф2$з8тф2.
(2.139)
Полученная формула позволяет найти абсолютное уско-
рение точки звена манипулятора и показывает его зависи-
мость от обобщенных положений, скоростей и ускорений.
Анализ ускорений манипу-
лятора с одной вращательной
и двумя поступательными па-
рами (рис. 2.18). Определим
линейное ускорение точки D
выходного звена 3. Задан-
ными будем считать законы
изменения обобщенных ко-
ординат фь s2, s3.
Вначале за переносное
движение примем движение
звена 2. Тогда абсолютное
ускорение точки D как уско-
рение точки в сложном дви-
Рис. 2.18
жении запишется так:
Рассуждая аналогично предыдущему примеру, запишем:
относительное ускорение точки D
кориолисово ускорение
n3 = 2Q2 * ^321
где = ф^!; иг32 = «3Г3.
Подставляя выражения относительной скорости и угловой
скорости звена 2 в выражение кориолисова ускорения,
получим
а$ = 2ф1&1 х s3i3 = 2ф183/3.
Переносное ускорение точки D (а3) может быть также
представлено как ускорение в сложном движении, если при-
нять переносным движение звена 1. В этом случае можно
записать
«3 = «2 = «2 + «2 + «2-
Переносное ускорение точки D звена 2 — это ускорение
точки звена 1
ае2 = Ji = fl” + = -ф1$3Г3 + Ф1«а/з-
69
1
Относительное ускорение аг2 точки D звена 2 опреде-
ляется движением в поступательной кинематической паре В
«2 — ^2^3«
Кориолисово ускорение точки D звена 2
t^2 ~ х t'21 = 2cpiki х s2ki = 0.
Геометрически суммируя все составляющие ускорения
точки Р, получим
«D = s2k3 + С*з ~ Ф1$з) h + (Ф1$з + ^ФхЗз)^.
Полученная формула показывает, что ускорение точки D
звена 3 определяется не только ускорениями в кинемати-
ческих парах, но и обобщенными координатами и
скоростями.
2,7. Ускорения высоких порядков
Для удовлетворения высоких требований к точности про-
мышленных роботов важное значение имеют не только
значения ускорений, которые позволяют найти силы инерций,
действующие на звенья, но и законы изменения ускорений
во времени, определяющие законы изменения сил инерции.
Ускорения высоких порядков изучались в основополагающих
работах О. П. Сомова, Н. Е. Жуковского и др.
Рассмотрим вопрос о линейных ускорениях 2-го поряд-
ка, о так называемом рывке манипулятора. Ускорение
2-го порядка, например, точки С есть производная по
времени от вектора ускорения точки С:
(2.140)
Дифференцируя по времени первый член (2.140), получим
п п п п
(2.141)
Дифференцируя по времени второй член выражения
(2.140), найдем
70
п п п п п
п п п п
(2.142)
Объединяя (2.141) и (2.142), получим
вектора b ускорения 2-го порядка
выражение для
(2.143)
Последнее выражение показывает, что ускорение 2-го
порядка точки манипулятора зависит от обобщенных
координат, определяющих значения коэффициентов
а также обобщенных скоростей и
ускорений.
Определим ускорение 2-го порядка (рывок) схвата мани-
пулятора (точки D) с тремя степенями свободы, рабо-
тающего в цилиндрической системе координат. Для этого
воспользуемся векторным методом и теоремой сложения
ускорений.
В проекциях на подвижные оси Cx2y2z2, связанные со
звеном 2 манипулятора, ускорение схвата точки D имеет
вид
О — S3I2 4“ (5зФ1 4" 2ф1$з)/2 4" *S2k2 •
(2.144)
Орты ?2» J2, к2 определены в подвижной системе
Ox2y2z2, поэтому их производные по времени равны
х i2 — Ф1 к2 х i2 — Ф1У2>
2
х /г “ "’ФЛ,
х к2 = — <Pi&2 х к2 — 0.
(2.145)
2
71
Рывок схвата этого манипулятора находим с учетом
формул (2.144) и (2.145) *
.* da ... * .. di2 z..........
—= s3i2 + s3 — + ($зФ1 + $зФ1 +
... * .. . . dj2 . -
+ 2ф!53 + 2<biS3)j2 4- (53ф! + 2cp1s3) —4-*s2fc2, (2.146)
at
откуда
b = С$з - «зФ1Ф1 - 2<PiS3) i2 4- O’scpi + 9iS3 +
+ 3s3$i)j2 4" *s2k2. (2.147)
Для определения характера изменения моментов сил
инерции звеньев важное значение имеет также изучение
углорых ускорений 2-го порядка.
Угловое ускорение 2-го порядка определим как произ-
водную по времени от вектора абсолютного углового
ускорения звена, т. е.
(2.148)
Пример: определение углового ускорения 2-го порядка для
выходного звена манипулятора, работающего в сферической
системе координат звена 3 (см. рис. 2.17, а). Продифферен-
цируем по времени выражение углового ускорения звена 3.
Угловое ускорение этого звена, очевидно, равно угловому
ускорению звена 2 и имеет вид
ёог — .. (ф1&1 4” Фг^г) — Ф1&1 4- ф2*2 4- ф2
at at
di2 * • - * • ..
_.= (Oj x i2 = ф^ x i2 = ф^’з; (2.149)
^02 = Ф1^1 4* ф2*2 4- Ф1Ф/2.
Абсолютные угловые ускорения звеньев 2 и 3 опреде-
ляются как суммы трех слагаемых векторов. Первые два
слагаемые представляют собой относительные угловые
ускорения звена 2 во вращении вокруг осей с ортами
1*2 и kl9 а третий член представляет собой аналог
кориолисова ускорения точки звена 2.
Дифференцируя по времени выражение для е02, получим
Ф1Ф21 “ (ф2
72
•ее ъ еее м е • • ее» ее»
= Ф1&1 4- ф2г*2 4- ф2ф J2 4- (Ф1Ф2 + Ф1ф2)/2 -
1
Уравнение показывает, что угловое ускорение 2-го поряд-
ка звена 2 рассматриваемого манипулятора определяется
обобщенными скоростями и ускорениями 1-го и 2-го поряд-
ков и не зависит от обобщенной координаты <р2-
2.8. Автоматизированные методы
кинематического анализа манипуляторов
(прямая задача)
Автоматизированные методы кинематического анализа на
ЭВМ необходимы ввиду чрезвычайной трудоемкости и гро-
моздкости решения задач кинематики для многозвенных
манипуляторов.
Методы и алгоритмы автоматизированного кинемати-
ческого анализа на ЭВМ появились в связи с разработкой
методов автоматизированного анализа динамики манипу-
ляторов [11] и были основаны на векторных представле-
ниях. Методы различаются способами формализации
структуры механизма и построены на основе известных мето-
дов кинематического анализа.
С каждым звеном манипулятора связывается система
координат с началом в центре масс звена. Каждому
шарниру манипулятора i ставится в соответствие единич-
ный вектор е4, направленный вдоль оси шарнира. Положе-
ния центров масс звеньев относительно центра шарнира
определяются векторами Гу.
Особенность одного из методов состоит в осуществлении
на ЭВМ операции сборки; это — операция совмещения векто-
ров et и е\ осей элементов шарнира i, принадлежащих
звеньям i — 1 и i, и определения проекций векторов
Гу и ортов системы звена i на неподвижные оси.
Рассмотрим процесс сборки на примере присоединения
звена i к звену i — 1, положение которого известно в
неподвижной системе координат, с помощью вращательной
пары.
Условие сборки можно записать в виде
Ct = Ae'i’, сц = А а'}.
(2.150)
Здесь А — неизвестная матрица перехода от внутренней
системы звена к неподвижной; и а- — нормализованные
компоненты векторов rt и rj в плоскости, нормальной
орту q: эти векторы находятся по формулам
at = - r„/l г„ |; a'i = rn/\ |,
(2.151)
1
где fn, f'n — составляющие векторов rif rj, перпендикулярные
орту et.
Для определения матрицы А необходимо добавить еще
третье уравнение
Ь = АН, (2.152)
где b — ё х а; Ь'=ёха.
Уравнения (2.150) и (2.152) дают линейную систему
девяти уравнений относительно девяти элементов матри-
цы А.
Определение положений манипулятора. В работе [16] для
решения прямой задачи о положениях манипулятора исполь-
зуется формула конечного поворота Родриго
F = rcosq 4- (1 — cosq)(e г)ё + (ё х г)sing, (2.153)
где ё — орт оси конечного поворота; q — угол поворота;
F и г — векторы до и после поворота.
При расчете положения кинематической цепи манипуля-
тора осуществляются конечные повороты в каждом шарнире
J. При этом все векторы звеньев с индексами, большими j,
получают конечные повороты, а их новое положение
находится по формуле (2.153).
Если рассматривается перемещение в поступательной
паре, то проекции всех векторов остаются постоянными,
кроме вектора г^. Его изменение равно перемещению
qjej в поступательной паре.
Определение скоростей и ускорений при автоматизиро-
ванном расчете [И] ведется последовательно: от звеньев
с меньшим номером к звеньям с большим номером. При
этом движение звена i — 1 рассматривается как переносное,
а движение звена i по отношению к i — 1 — как относи-
тельное.
В случае поступательной пары i угловая скорость
звена i и линейная скорость точки звена i находятся по
формулам
— Vt-i — G>j-i x + q^t.
В случае вращательной кинематической пары i
- (Df-i + q^;
Vt = Vi-1 — Of-i X Fj-ij + x riti.
74
1
При решении задачи об ускорениях используется теорема
сложения ускорений. Угловое ускорение звена i опреде-
ляется на основе формулы
£отн
^пер
отн*
Если звено i соединено со звеном i —
поступательной кинематической пары, то
£>-- О •
i 19
с помощью
в случае вращательной пары
-1 4- 4- _ i х е;.
Линейное ускорение точки звена i определяется для
поступательной пары следующим образом:
at = j 4- х -Ь со£ х (о\ х rf).
В работе [37] предложен метод и алгоритм авто-
матизированного кинематического анализа манипулятора с
вращательными, поступательными и цилиндрическими кине-
матическими парами.
Структура манипулятора задается в виде матрицы М
размером 1 х и, где п — число подвижных звеньев. Звенья
нумеруются в порядке возрастания номеров, начиная с не-
подвижного, которому присваивается номер — 0.
Матрица структуры манипулятора имеет вид строки и
составляется по правилу:
J 0, если к-я пара вращательная;
Mk — \ 1, если к-я пара поступательная;
t 2, если к-я пара цилиндрическая.
Геометрия манипулятора характеризуется следующей
системой обозначений (рис. 2.19):
ось z неподвижной системы координат направлена по
оси первой кинематической пары, точка Ао — начало не-
подвижной системы координат; Л2л-ь A2fc — концы общих
перпендикуляров к осям к-й и (к + 1)-й кинематических пар
(к — 1, 2,...,и — 1);
h2k-i = A 2ft-iX2fc — кратчайшее расстояние между осями
к-й и (к 4- 1)-й кинематических пар (к = 1, 2,...,и — 1);
h2k_2 — А2к-^2к-1 — расстояние от точки А2к_2 до точки
А2к_ь измеряемое вдоль оси к-й пары (к = 1,...,и);
орты ет направлены от точки Ат к точке • Am+i
(т = ®, 1,...,2и — 2);
1
Рис. 2.19
орты e2*-i (& = 1, 2,...,n —1) направлены по линии
кратчайшего расстояния между осями к-й и (к + 1)»й пар
от точки Л2Л-1 к точке Л2Л;
орты ё2к_2 (к = 1,...,и) направлены по оси к-й пары от
точки А2к-2 к точке Л2к_ь если Мк = 0;
постоянные углы ит откладываются от векторов ет_2
и ёт против часовой стрелки, если смотреть с конца
вектора ёт-^
с выходным звеном манипулятора связывается система
координат ось £ которой направлена по оси
п-й пары.
При решении прямой задачи о положениях манипуля-
тора заданными являются значения постоянных параметров
и переменных, определяющих относительные положения
звеньев.
Требуется определить абсолютные положения звеньев,
т. е. их положения в неподвижной системе координат. Для
этого достаточно найти проекции орт еь ё2у...9ё2п-1
на неподвижные оси.
В работе [36] предложено для этого использовать
следующие рекуррентные формулы:
ёк = i; е0 = к;
ёт = em_2cosocm + (ёт^1 х em_2)sinam (т = 1, 2,...,2и - 1).
При этом операция векторного произведения может быть
заменена операцией дифференцирования орта ет-19 т. е.
j? ___ - — 1
- 1 - 1 X €т _ 2 _
dOm-i
С учетом последнего выражения рекуррентные формулы
получают вид
76
1
С-1 — i’i £o — К» £o ~~ji
— ^m- 2 COS4~
sin am;
^tn — 2 sill — 1 COS 0tm
(m = 1, 2,...,2n — 1).
Орты осей г) и £ находятся по формулам
^2п ~ ~~ ^2п- 1 ’ $2п + 1 = ^2и-2-
Рекуррентные формулы позволяют по известным зна-
чениям углов поворота векторов af (i = 1,...,2п — 1) после-
довательно найти орты Ei9 е2, Е2, Е2„-!.
Радиусы-векторы вершин Alt А2,...,А2п+2 ломаной линии
определяются по формуле
rA = fA-i _|_ ак_ ^k_ i (к = 1, 2,...,2п 4- 2).
После этого прямую задачу о положениях манипулято-
ра можно считать решенной.
Прямая задача о скоростях и ускорениях. В работе [37]
скорости и ускорения определяются по следующим рекур-
рентным формулам.
Угловая скорость и угловое ускорение звена к:
&к — + a2k-1^2fc-2>
^к = ^к-1 + a2k-1^2k-2 + &2к- 1 х C2fc-2),
где к = 1, 2,...,и; б0 = £0 — 0; a2k-i> a2*-i ~ соответственно
первая и вторая производные по времени от переменных
параметров.
Линейные скорость и ускорение звена к:
Vi = t 4- i 4- 1 (cofc x et- J;
= di-i 4- hi- 1ci_ i 4- 2hi-1 (wfc x J 4-
4- hi-! [(£fc x di-i) 4- (bfc x (dj* x Cj-i)],
где i = 1, 2,..., 2n 4- 2; к = i/2, если i четное, к — (i — 1)/2, если
i нечетное.
Метод, основанный на рекуррентных формулах, пред-
полагает последовательный расчет искомых величин — от
звена к звену, начиная от неподвижного. Такой подход
удобен для полного кинематического анализа манипулято-
ров с незамкнутой кинематической цепью.
Если же требуется найти кинематические характеристики
движения какого-либо одного звена, то удобнее пользо-
ваться аналитическими выражениями для угловых и ли-
нейных скоростей и ускорений данного звена.
77
1
Глава 3
Метод матриц в кинематике
манипуляторов
Применяя к расчету конкретных видов манипуляторов
специальные приемы, можно добиться экономии объема
вычислений. Ряд таких приемов был продемонстрирован
в предыдущей главе. В отличие от этого в данной главе
излагается метод, который можно применять к расчету
любого манипулятора с поступательными и вращательными
кинематическими парами. Универсальность метода покупает-
ся ценой некоторой избыточности вычислений. Этот
метод развивался параллельно с развитием вычислительной
техники, и он больше приспособлен к расчетам на ЭВМ,
нежели к расчетам вручную. Его освоение требует
свободного обращения со сравнительно новым для теорети-
ческой механики матричным аппаратом. Элементы теории
матриц даны в Приложении II. Перед тем как приступить
к изучению § 3.2, следует прочитать п. 5 — 7 данного
Приложения. Особое внимание следует обратить на расши-
ренные матрицы, описывающие переход из одной системы
координат в другую (В-матрицы сдвигов и вращений) и
на производные от этих матриц (П-матрицы).
3.1. Специальные системы координат
Осью вращательной пары (i, i + 1), составленной из звеньев
i и i + 1, является ось цилиндрического шарнира, жестко
связанная со звеном i, вокруг которой вращается звено
i + 1. Для поступательной. пары (i, i + 1) осью является
любая прямая, параллельная вектору скорости поступатель-
ного движения звена i + 1 относительно звена i.
Пронумеруем все звенья манипулятора от стойки (зве-
но 0) до схвата (звена п) и свяжем с каждым из них
свою систему декартовых координат, выбранную следующим
специальным образом [48]: ось zf идет по оси кинемати-
ческой пары (i, i + 1); начало координат системы i, жестко
связанной со звеном г, лежит на общем перпендикуляре
к осям z,-! и zb либо в точке их пересечения,
если таковая имеется, либо в любой точке оси кинемати-
ческой пары, если ось. zL совпадает с осью или
параллельна ей; ось х, идет по общему перпендикуляру,
проведенному к осям х и zb и направлена от точки
пересечения этого перпендикуляра с осью zi_1 к точке его
пересечения с осью (или в любую сторону по нормали
78
1
к плоскости, содержащей оси z,_i и zb если они пересекают-
ся, или произвольным способом, если zf_i и Zi идут
по одной прямой); ось выбирается по правилу правой
тройки векторов.
Начало координат системы 0, т. е. системы, жестко
связанной со стойкой, может лежать в любой точке оси
пары (0, 1); ось х0 направляется произвольным образом.
Выбор системы п тоже выпадает из общего правила,
так как звено п + 1 отсутствует. Поэтому предлагается
вообразить любого типа пару (и, п + 1) и после этого
выбрать систему по общему правилу. Начало выбранной
таким образом системы называется центром схвата.
3.2. Расширенная матрица перехода
для кинематической пары
Специальный выбор систем координат звеньев манипулято-
ра позволяет с помощью лщпь четырех параметров (а не
шести, как в общем случае) описать переход из одной
системы в другую. Систему i — 1 можно преобразовать
в систему i с помощью поворота, двух сдвигов (перено-
сов) и еще одного поворота, выполняемых в следующем
порядке:
1) поворот системы i— 1 вокруг оси Zf-t на угол 0f
до тех пор, пока ось не станет параллельной оси х£;
2) сдвиг повернутой системы вдоль оси z^t на величину
Si до тех пор, пока оси xt-i и xt не окажутся на од-
ной прямой;
3) сдвиг вдоль оси xt на величину до тех пор, пока
не совпадут начала координат;
4) поворот вокруг оси xt на угол oq до совмещения оси
Zi-i с осью zf.
Каждому из этих элементарных движений соответствует
одна из В-матриц: либо матрица вращения, либо матрица
сдвига (см. Приложение II). Результирующая матрица
перехода, связывающая системы i — 1 и i, является произве-
дением этих матриц:
С учетом (П.39), (П.40), (П.41) после перемножения получаем
— sin 0,- COS CLi
cos 0/ cos a£
sin OQ
r
0
sin©: sin a£ a£cos0i
— cos 0f sina; cLsinO:
cos s(
0 1
(3.2)
1
В соответствии с формулой (П.34) с помощью матрицы
А( можно связать радиусы-векторы одной и той же точки
в системах i и i — 1:
где Rf = [xf, yb zb 1]т — матрица-столбец, определяющая по-
ложение произвольной точки звена i в системе отсчета,
жестко связанной с этим звеном; a Ki-1 = [xi_1, J’/—i,
zf_i, 1]т — матрица-столбец, определяющая положение той
же точки в системе, жестко связанной со звеном i — 1.
В матрицу At входят четыре параметра: 0£, sb ab af.
Для любой кинематической пары три из них должны быть
константами и только один — переменной величиной. Для
вращательной пары переменной величиной является угол
а для поступательной пары — перемещение sb Таким обра-
зом, каждая матрица Л, содержит только одну перемен-
ную величину, которую будем называть в дальнейшем
обобщенной координатой и обозначать qb
Для решения многих задач необходимо знать производ-
ную от Ai по обобщенной координате. Считая, что пара
(i — 1, i) вращательного типа, и поэтому дифференцируя
(3.1) по параметру 0,, играющему
координаты, получаем
и, _ аввр(£,0,) - - г
ВСд(к, Sj)Всд\1, cq)
или с учетом (П.47)
ал.- _
роль обобщенной
(3.4)
Если кинематическая пара (i — 1, i) поступательного типа,
то роль обобщенной координаты играет параметр s, и
(/с, Sf)
или, принимая во внимание (П.49) и (П.50),
Матрицу Аь стоящую справа в (3.5), можно было, согласно
(П.50), не писать. Она записана для возможности объедине-
ния (3.4) и (3.5) в одну формулу:
cL4.. _
(3 6)
80
1
Рис. 3.1
где — это либо £2сД, либо Пвр в зависимости от типа
кинематической пары (i — 1, г).
Пример 3.1. Составить таблицу кинематических пар и
параметров, а также вычислить матрицы At для манипу-
лятора, кинематическая схема которого изображена на
рис. 3.1, а.
Манипулятор имеет пять степеней свободы, которым
соответствуют пять обобщенных координат: $i, 02, s3, 0д, 05.
Специальные системы отсчета выбраны в соответствии с
указаниями и показаны на конструктивной схеме (рис. 3.1,6).
Тип кинематических пар и значения параметров сведены
в табл. 3.1.
Тип пары
Параметры
О
•ч
Кинема-
тическая
пара
П оступательная
Вращательная
П оступательная
Вращательная
Номер
звена
О
—п/2
О
—п/2
•S3
•S4
О
«1
О
О
О
о
1
В соответствии с этой таблицей и формулой (3.2) опре-
деляем матрицы At:
ЯМ»
1 о о
О 1
0 10 0
0 0 1 s3
0 0 0 1
cos04 0 —sin64 0
А
sin04 0 cos 64 О
0—1 0 s4
О 0 0 1 J
COS 65
sin 05
о
О —sin 05 а5 cos 05
О COS 05 n5sin05
-10 О
О
О О
Навык в составлении таблиц, подобных 3.1, совершенно
необходим, поскольку такие таблицы исчерпывающим обра-
зом описывают кинематические схемы манипуляторов и
являются входной информацией для кинематического расчета
на ЭВМ. Что касается матриц Аь то их запись требуется
лишь для тестирования программ, а в данной книге, где
решение примеров в целях обучения проводится вручную,
без них невозможно продолжение кинематического анализа.
3.3. Прямая задача о положениях
Прямая задача кинематики манипуляторов формулируется
так: задана кинематическая схема манипулятора и в некото-
рый момент времени известны значения обобщенных коор-
динат, определяющие положение всех звеньев манипулятора
друг относительно друга. Требуется определить положение
и ориентацию последнего звена манипулятора (схвата) в
1
системе отсчета, связанной со стойкой. Геометрические
размеры звеньев считаются известными.
Задача решается с помощью формулы, аналогичной
(П.35):
Ro = T„R„, (3.7)
где Тп — матрица, равная произведению матриц 4,:
Тп = ЛМг.-.Л. (3.8)
В формуле (3.7) Rn и Ro — матрицы-столбцы размером
4x1, первые три элемента которых — это координаты произ-
вольной точки схвата соответственно в системах п и 0.
Столбцы матрицы Т„ имеют геометрическое толкова-
ние. Первые три элемента первого, второго и третьего
столбцов представляют собой направляющие косинусы соот-
ветственно осей х„, y„, zn в системе 0, а три элемента
четвертого столбца — это координаты х*, у*, z* центра
схвата в той же системе:
п
cos (in, Iq)
cos (i„,7o)
COS (/„До)
cos (/„, i0)
cos (kn, i0)
cos (jnJo) cos(k„Jo)
со8(/иДо) cos(fc„,3c0)
(3.9)
. Таким образом, решение прямой задачи кинематики
манипуляторов сводится к тому, что, задавшись значения-
ми обобщенных координат, вычисляются с помощью (3.8) и
(3.2) значения элементов матрицы Т„, а следовательно,
согласно (3.9), определяются положение и ориентация схвата
в системе координат, жестко связанной со стойкой манипу-
лятора. Если обобщенные координаты заданы не значе-
ниями, а функциями времени, то и элементы матрицы
Т„ — функции времени.
Если требуется определить положение и ориентацию не
схвата, а некоторого промежуточного звена i и радиус-
вектор его точки в системе 0, то следует воспользо-
ваться формулами, аналогичными (3.7) и (3.8):
Ro = TtRb
(3.10)
где
(3.11)
По таким же формулам можно определить положение
и ориентацию некоторого звена I по отношению к звену i:
1
9
b
(3.12)
где
(3.13)
Пример 3.2. Определить положение и ориентацию схвата
манипулятора, кинематическая схема которого изображена
на рис. 3.1, а.
Решение задачи заключается в нахождении 12 элементов
матрицы
(£5)12 (013 (^5)14
(^5)22 (£5)23 (^5)24
(*5)32 (^5)33 (^5)34
О 0 1
(3.14)
которая определяется, согласно (3.8), как произведение
матриц Я,:
— Hj^2^3>14^5.
Перемножив матрицы Аь вычисленные при решении
примера 3.1, получаем искомые величины:
(t5)ij = cos 02 cos 04 cos 05 + sin02sin05 = cos(i5, i0);
(*5)21 ®= sin 02 cos 04 cos 05 — cos 02 sin 05 — cos(i5, jo)>
(i5) 31 = - sin 04 cos 05 = cos (i5,X>); 1
(*5)12 = Cos02sin04 = cos (/5, i0);
(*5)22 = sin02sin04 = cos (/*5, Jo);
(Оз2 = COS 04 = COS (/5,^0);
(G)i з—~ cos 02 cos 04 sin 05 + sin 02 cos 05 = cos (£5, i0); ,
)
(*5)23 = “ sin &2 COS 04 sin 05 — COS 02 COS 05 == COS (£5, Jo);
(*з)зз = sin 04 sin 05 = cos (£5,%));
*
(014 = «5(COS02COS04COS05 + Sm02Sin05) — sin02(s3 4-S4) 4-«1 =
(O24 = a5 (sin 02 cos 04 cos 05 — cos 02 sin 05)
4- cos 02 (s3 4- s4) == yl;
(O34 = ~ <*5 sin 04 cos 05 4- Si 4- s2 - z%.
1
Полученные выражения показывают связь матрицы Г,
определяющей положение и ориентацию схвата, с обобщен-
ными координатами манипулятора.
Задания для самостоятельного выполнения. Определить
в системе 0, жестко связанной со стойкой, положение и
Рис. 3.2
85
1
ориентацию схвата Манипулятора ка^с функции обобщенных
координат и линейных размеров звеньев. Кинематические
схемы манипуляторов приведены на рис. 3.2, а—е. Задание
выполнить в следующем порядке:
определить число степеней свободы манипулятора;
на основе условной кинематической схемы нарисовать
конструктивную кинематическую схему манипулятора и про-
нумеровать на ней звенья манипулятора;
выбрать специальные системы координат, жестко связан-
ные со звеньями манипулятора, и показать их на рисунке;
составить таблицу типов кинематических пар и значений
параметров манипулятора;
вычислить матрицы
определить элементы матрицы Ти;
указать, чему равны координаты центра схвата и
направляющие косинусы осей схвата в системе координат,
связанной со стойкой.
Пример 3.3. Выполнить задание для манипулятора, кине-
матическая схема которого дана на рис. 3.3, а. Манипулятор
1
имеет одну вращательную и две поступательные кинемати-
ческие пары, следовательно, число степеней свободы равно
трем. Конструктивная кинематическая схема изображена на
рис. 3.3,6, там же проставлены номера звеньев.
Специальные системы координат выбираем в соответ-
ствии с указаниями (см. § 3.1). Ось z0 идет по оси враща-
тельной пары (0, 1), т. е. по оси вращения тела /; ось zt
идет по оси пары (1, 2), вдоль которой тело 2 поступательно
перемещается относительно тела 7; ось z2 идет вдоль оси
поступательной пары (2, 3); ось z3s идет так же, как ось z2.
Направление осей х и положения начал координат показаны
на рис. 3.3, а.
Типы пар и значения параметров сведены в табл. 3.2.
Таблица 3.2
Кинема- тическая пара Тип пары Номер звена Значения параметров
0 а 5 а
0,1 1,2 2,3 Вращательная Поступательная » 1 2 3 i ф к о 0 п/2 0 0 s2 S3 ' 0 0 0
Составляем матрицы Ai [в соответствии
(3.2)]:
sin 0
0
01
0 1
sin 0
0
0
cos 0
0
0
0
0
о
о
о
о
о
о
о
о
о
о
о
о
о
о
о
о
о
о
о
Вычисляем элементы
— cos 0
матрицы
0
— sin 9
s3 sin 0
— sin 0
0
0
0
COS0
0
0
(3.16)
87
Координаты центра схвата в системе, связанной со стой-
кой манипулятора, равны [что видно из сопоставления (3.16)
С (3.9)]
х* = — s3 sin 0t; уз = s3 cos Oj; z* = s2.
Направляющие косинусы осей x3 и z3:
cos(Г3 Jo) = -cos0!; cos(f3fjo) = -sinOj; cos(Г3До) = 0;
cos (k3, i0) = — sin 0!; cos (k3, j0) — cos 0j; cos (£3, k0) = 0.
Задание выполнено.
3.4. Обратная задача о положениях
Обратную задачу кинематики можно сформулировать так:
задана кинематическая схема манипулятора и известны
положение и ориентация схвата в системе координат стойки.
Требуется определить значения обобщенных координат, ко-
торые обеспечат заданное положение схвата.
Задать положение схвата, как и любого твердого тела,
можно с помощью шести величин. Обычно три из них —
это координаты центра схвата, еще две — это направляю-
щие косинусы одной из координатых осей схвата и послед-
няя — это один из направляющих косинусов другой коорди-
натной оси схвата. Например, этими шестью величинами
могут быть шесть наддиагональных элементов матрицы Тп:
(012
(О1 з
(61)2 з
(014
(O24
(Оз4
(3.17)
Приравнивая шесть заданных величин соответствующим
элементам матрицы Т„, получим систему шести уравнений
(в общем случае трансцендентных), неизвестными в которых
являются обобщенные координаты.
Если п = 6, т. е. число неизвестных равно числу уравне-
ний, то обычно можно отыскать вполне определенные зна-
чения обобщенных координат.
Если манипулятор имеет больше шести степеней свобо-
[Ы, т. е. число неизвестных превышает число уравнении, то
одному и тому же положению схвата могут соответствовать
различные наборы значений обобщенных координат.
И наконец, если п < 6, то решения не существует, т. е.
за счет меньшего, чем шесть, числа обобщенных координат
1
невозможно получить наперед заданные произвольные поло-
жение и ориентацию схвата.
Однако если требуется лишь попадание центра схвата
в определенную точку пространства, а ориентация схвата
может быть любой, то для этой цели годится манипулятор
с тремя степенями свободы. В этом случае при решении
задачи потребуется составить лишь три уравнения для
отыскания трех неизвестных.
Ниже, при решении обратной задачи кинематики всегда
будем считать, что число неизвестных равно числу степеней
свободы манипулятора.
Пример 3.4. Задан г0 (х0, У о, z0) — радиус-вектор центра
схвата манипулятора, изображенного на рис. 3.3, б, в системе,
жестко связанной со стойкой. Требуется определить значе-
ния обобщенных координат 0Ь s2, s3, соответствующих
заданному положению центра схвата.
Рассматриваемый манипулятор имеет только три степени
свободы, поэтому с помощью 01, s2, $з можно лишь обес-
печить попадание центра схвата в наперед заданную точку
пространства. Что же касается ориентации схвата, то это
всецело зависит от значений 0Ь s2, s3, которые получатся
при решении задачи, т. е. ориентацией нельзя управлять
наперед в данном манипуляторе.
Приравнивая первые три элемента четвертого столбца
матрицы Т3, подсчитанной в примере 3.3 [см. (3.16)], задан-
ным величинам х0, yG, z0, получим систему трех уравнений
относительно 0Ь s2, s3:
х0 = -s3 sin 0Х; jo = S3 cos 0i; z0 — s2f
откуда находим искомые величины:
0i = arctg ( —х0/у0); s2 = z0; s3 = ±j/xo + Уо- (3.18)
Конкретная конструкция манипулятора (рис. 3.3, б) не до-
пускает для $3 отрицательных значений.
Алгоритм решения обратной задачи методом последова-
тельных приближений. Не всегда обратную задачу кинема-
тики удается решить аналитически. При численном решении
используется ЭВМ, что требуют программы численного
решения системы трансцендентных уравнений. Однако уни-
версальную и в то же время надежную программу такого
типа разработать трудно. В данном параграфе дается
простейший алгоритм численного решения системы транс-
цендентных уравнений.
Задача состоит в отыскании значений т обобщенных
координат по заданным т элементам матрицы ТП9 причем
89
1
т С 6. Для простоты изложения будем считать, что мани-
пулятор обладает шестью степенями свободы и т — п = 6.
Тогда искомыми являются все обобщенные координаты
манипулятора, которые обозначим qlf q2, q& Заданными
считаем шесть наддиагональных элементов матрицы Т„
[см. (3.17)]. Значения qt определим методом последователь-
ных приближений.
В нулевом приближении дадим обобщенным координа-
там произвольные начальные значения g(20), ...» q
Опишем один шаг получения уточненных значений обоб-
щенных координат по известным приближенным значениям.
Пусть к — номер шага. Приближенное значение обобщен-
ной координаты qt обозначим а уточненное — q\k) (для
первого шага приближенное значение — q{°\ уточненное — ^1}).
Представим зависимость матрицы Тп от обобщенных коор-
динат в виде отрезка степенного ряда Тейлора, ограничен-
ного линейными членами, с центром разложения в точке
«Г1’:
6
ZAT^~ 1)
- (# -# -1 ’).
(3.19)
Здесь 7бк~— матрица Т6, элементы которой выражены
через приближенные значения обобщенных координат.
Введем обозначение
(3.20)
причем Uij — это матрица 4x4:
(Mij) 12
(Mij)22
(М0’)з2
(3.21)
(wy)i 1
(Wy)21
(му)з1
(Mij)41
(.Щj) 1 з
(мо)гз
(му)зз
(му)43
(wij)14
(Mij)24
(м0')з4
(^ij)44
С учетом (3.21) выражение (3.19) выглядит так:
(3.22)
Одному матричному равенству (3.22) соответствуют
16 скалярных равенств (по числу элементов матрицы Т6).
Так как шесть наддиагональных элементов матрицы Т6
заданы, то соответствующие им равенства образуют систе-
90
1
му линейных уравнений для шести обобщенных координат:
(^б)12
(u6j) 12
6
(3.23)
(^б)гз — (^б)г(з
(к- 1)
= (Гв)й-"
(^б)з4 ~ (^б)з4
(М34* ° (#’ - ”)•
Так как приближенные значения известны, то
система (3.23) позволяет определить уточненные значения
qW. На этом заканчивается к-й шаг метода последователь-
ных приближений. Далее, уточненные значения qp обозна-
чаются qjk~l) и с их помощью по формулам (3.8) и (3.20)
вычисляются матрицы Т£к~" и процедура нахожде-
Так как приближенные значения q]
вычисляются матрицы и
ния qjk} из системы (3.23) Повторяется.
Чем ближе д)
меньше разность (gjk) —
вают, когда разность становится меньше наперед заданной
малой величины.
В случае, когда манипулятор имеет и < 6 степеней сво-
боды и число неизвестных обобщенных координат равно и,
система линейных уравнений, аналогичная (3.23), может быть
записана так:
И
(k-D v M
к qj
(k-1)
тем ближе T6(fc к Т6 и тем
). Поэтому уточнения оканчи-
_____ (f (к - 1)
vp Vln
(3.24)
)
t
I
г
I
1
1
г
где v — номер строки, ар — номер столбца матрицы Тп, на
пересечении которых стоит элемент, значение которого
задано. Очевидно, что количество заданных элементов
должно равняться п.
В некоторых задачах может потребоваться обеспечить
заданное положение не схвата, а какого-либо промежуточ-
ного звена i. В этом случае система (3.24) имеет вид
91
1
(#’ - q<‘ - 1 ’).
(3.25)
Остановимся на нахождении матрицы элементы ко-
торой входят в уравнения (3.25). Согласно (3.20) и с учетом
(3.11) получаем
(Aj А2. • *^j- i^j* • »А$_ 1AJ.
Среди всех A-матриц, стоящих в правой части этого выра-
жения, только одна, а именно А7-, зависит от qjy следова-
тельно,
или, согласно (3.6),
(3.26)
где
QBp(/c), если пара вращательного типа;
ОсД(Ц если пара поступательного типа.
Описанный алгоритм определения обобщенных коорди-
нат методом последовательного приближения легко програм-
мируется.
Задания для самостоятельного выполнения. Определить
значения обобщенных координат, обеспечивающих заданное
положение центра схвата в системе 0, жестко связанной
со стойкой манипулятора. Кинематические схемы приведены
на рис. 3.4, а — е, размеры звеньев и координаты центра
схвата задать самостоятельно таким образом, чтобы центр
схвата в крайнем положении находился не далее чем
в 200 см от начала системы 0. Точность определения
угловых координат — 10~3, линейных — 10"2 см.
Пример 3.5. Выполнить задание применительно к манипу-
лятору, кинематическая схема которого изображена на
рис. 3.3.
Зададим радиус-вектор центра схвата в системе 0: х0 —
= 30 см; у0 = 100 см; z0 = 120 см и определим значения
обобщенных координат 0r, s2, 5з> которые обозначим qu q2,
q3. Первые три элемента четвертого столбца матрицы Т3
должны быть равны заданным величинам;
(^з)14 = 30 см; ^3)24 ~ ЮО см; ^3)34 *= 120 см. (3.27)
92
1
Рис. 3.4
л
1
I
Чтобы написать систему уравнений (3.24), необходимо
вычислить матрицы 1731, (732, U33, которые, в свою оче-
редь, зависят от матриц Ai9 а те — от qu q2, q3. Поэтому
при численном решении задачи на ЭВМ на данном этапе
необходимо задать начальные значения обобщенным коор-
93
I
динатам. Мы же воспользуемся аналитическими выраже-
ниями для элементов матриц At и матриц Th полученными
при решении примера 3.3. Согласно (3.26),
^зз
-^1Осд С i а2а3;
или с учетом (3.15) и (П.50)
— COS Q £
о
О —cos qt
О — sin
О
-^3cos^i
-Цз
О
32
sin <2!
О
О
О
О
О
О
О
О
О
О
О
о
о
о
о
О
О
о
о
го
— sin qt~
cos qt
О
Последний столбец матрицы
элементы
согласно (3.16),
Система (3.24) в данном случае имеет вид
(t3)v4 = (<Г “)v4 + («з“Г 44 (91(Ю - ЧГ °) +
+ °)v4 («/> - ЧГ~ °) + («з'Г Х («3® - «Г «К
(3.28)
имеет
(3.30)
Используя (3.27)—(3.29) и полагая последовательно v = 1,
2, 3, разворачиваем (3.30):
30 = — q£k sin q[k n — q£k l> cos q}k k)(q}k} — q}k n)
+ 0(^-й n)-sing(f l}(q(k}~q{3 n)j
100 = q(3 “1)cos^f-1) — q<k~sin q^k~(q^ — q^k~i})
4- 0(^fc) - q%~ly) 4- cosrf-1)(^fc> - q%~1})',
120 « q^~ + 0 (q™ - q}k~ ”) + 1 (q™ - q«~ *>) +
+ 0(^-^"1>).
(3.31)
Попробуем в качестве начальных значений принять д1(0) =
— О, ^2<0) ~ ^з(0) ~ 0. При этом из (3.31) следует: 30 — 0;
100 = q^t 120 = g2(1). Полученная система не имеет решения.
Этот пример показывает, что не любые начальные значения
годятся. Нетрудно понять, почему выбранный набор началь-
94
зз —
О
О
О
о
О
О
О
О
О
О
О
О
1
ных значений оказался неудачным. При q3 = 0 поворот
звена 1 на угол qt не меняет положения центра схвата
(хотя меняет ориентацию). Нужно избегать подобных «вы-
рожденных» положений механизма. Поэтому положим q^ —
— О, ^2(0) = 0’ <?з(0) = 50. Далее по программе, реализующей
алгоритм метода последовательных приближений, подсчи-
тываем уточненные значения обобщенных координат (ниже
эта работа выполнена вручную). Первый раз система (3.31)
такова:
30= - 50g/1*;
100 = 50 + (з3(1) - 50);
120 = д2(1>,
откуда ^j(1) = —0,60, q2(1) = 120,00, д3(1) = 100,00.
Во втором приближении
30 = -100 sin (- 0,6) - 100 cos (- 0,6) (q}2> + 0,6) -
- sin (-0,6) (q3<2) - 100);
100 = 100 cos (- 0,6) - 100 sin (- 0,6) (q[2} + 0,6) +
+ cos (- 0,6) (<j3'2) - 100);
120 = 120 + (q2'2’ - 120),
откуда q/2) = —0,283, q2<2) = 120,00, q32> = 99,47.
В третьем приближении
30 = -99,47 sin (-0,283) - 99,47 cos (-0,283) (91<3> + 0,283) -
- sin (-0,283) (q3,3)- 99,47);
100 = 99,47 cos (- 0,283) - 99,47 sin (- 0,283) (q/31 + 0,283) +
+ cos (-0,283) (q3'3) - 99,47);
120 = 120 + (q2,3) - 120),
откуда q/3) = —0,292, q23> = 120,00, q3<3) = 104,40.
В четвертом приближении поправки к qt меньше, чем
10“3, к q3 — еще меньше, так что итерации можно окон-
чить. Аналитическое решение примера 3.5 по формулам
(3.18) дает значения: qY = —0,292, q2 = 120,00, q3 — ±104,40.
Значение для q3i полученное численным способом, оказав
лось положительным. Требуемую точность дало третье
приближение. Всегда ли это будет так? Наряду с решением
q3 > 0 система уравнений имеет еще одно решение: q3 < 0.
Это решение невозможно реализовать на манипуляторе,
изображенном на рис. 3.3, б, однако его можно получить
в ходе вычислений при неудачном выборе начального
95
приближения. Существуют начальные приближения, ведущие
к этому нереализуемому решению, и существуют начальные
приближения, ведущие к решению q3 > 0. Граничные между
ними начальные приближения в зависимости от ошибок
округления ведут к тому или иному решению, но итерации
при таких граничных приближениях (й при близких к ним)
сходятся медленно. Наконец, существует, как мы видели,
начальные приближения, при которых система уравнений
несовместна.
Таким образом, выбор начального приближения — важ-
ный этап решения системы уравнений. Желательно, чтобы
начальное приближение было близко к искомому решению.
Тогда итерации будут сходиться чрезвычайно быстро.
3.5. Определение законов изменения
обобщенных координат при движении
точки схвата по заданной траектории
В § 3.5 обратная задача кинематики манипулятора была
сформулирована как одноразовая. Фактически приходится
решать серию подобных задач для разных положений
схвата.
Выполнив одно задание, манипулятор приступает к вы-
полнению следующего. Начало выполнения очередного
задания — это конец выполнения предыдущего. Поэтому
обобщенные координаты в начале выполнения задания
известны. Схват нужно переместить из этого известного
начального положения в конечное, для которого обобщен-
ные координаты неизвестны.' Перемещать схват можно по
разным траекториям. Выбор траектории и закона движения
по этой траектории — сложная оптимизационная задача. Чем
быстрее произойдет перемещение, тем раньше освободится
манипулятор для выполнения новых заданий. Но быстрое
перемещение требует больших усилий. Усилия ограничены
прочностью манипулятора и мощностью его приводных
устройств. Усилия зависят также от выбранной траектории.
Чем она прямее, тем меньше усилия, прикладываемые
к переносимому предмету. Но прямолинейное движение
переносимого груза может требовать очень неравномерных
криволинейных движений ряда массивных звеньев манипу-
лятора, что приведет к появлению больших усилий в неко-
торых кинематических парах. Наконец, перемещения звеньев
друг относительно друга имеют ограничения, из-за чего
не всякая траектория центра схвата может быть осуществле-
на.
96
Примем, что закон движения схвата известен и нужно
найти законы изменения обобщенных координат вдоль
траектории центра схвата.
Пусть т — длительность предстоящего перемещения схва-
та. Разделим т на L малых интервалов, например, одина-
ковой длины, и пусть tt — момент окончания интервала /.
В начале интервала 1, т. е. в момент t0, известны обобщен-
ные координаты, а в конце этого интервала, т. е. в момент
известно положение схвата. Чтобы найти обобщенные
координаты в момент нужно рещить обратную задачу
так, как это было описано в § 3.7. При этом в качестве
начального приближения можно взять значения обобщенных
координат в момент i0. Это — хорошее начальное прибли-
жение и тем лучшее, чем меньше интервал времени (t0, tt).
Если число L выбрано достаточно большим, то начальное
приближение окажется таким хорошим, что достаточно
будет сделать один шаг.
Получив обобщенные координаты для момента tx, можно
использовать их в качестве начального приближения при
решении обратной задачи для момента t2 и т. д., пока
не будет пройдена шаг за шагом вся траектория вплоть
до момента tL — конца общего интервала т. Еще лучше
в качестве начальных значений для моментов tIs начиная
с I — 2, брать величины
^0) (h) = qj (ti~ 0 4- (qj (ti-i) - qj (t^ 2))
(3.32)
При заданной точности, с которой нужно выполнить
вычисления, существует такое значение L, при котором
объем вычислений минимален. При этом требуемая точность
достигается уже на первом приближении. Сделав L больше,
мы должны будем решить больше обратных задач. Сделав
L меньше, мы получим меньше
обратных задач, но зато в каж-
дой из них придется вычислять
второе приближение.
Пример 3.6. Найти законы" из-
менения обобщенных координат
манипулятора (см. рис. 3.3), по-
лагая, что центр схвата движется
равномерно прямолинейно из
положения с -координатами 30,
100, 120 в положение с коор-
динатами — 50, — 50, 60 см
(рис. 3.5).
97
4 Механика промышл. роботов, кн. I
Таблица 3.3
Положим L= 10 и решим обратную задачу, как это было
показано выше. Результаты решения сведены в табл. 33»
В этой таблице даны координаты равноотстоящих точек
на прямой, соединяющей начало и конец траектории центра
схвата, а также обобщенные координаты, полученные в пер-
0 1 2 3 4 5 6 7 8 9 10 I
Рис. 3.6
1
вом, втором и третьем приближениях. Начальные прибли-
жения, начиная с I = 2, рассчитывались по формуле (3.32).
Для q2 уже первое приближение даст точный результат.
Для q3 и 41 в первых и последних точках достаточно
второго приближения, так как qY меняется не очень резко.
В средней части траектории, где qY меняется достаточно
резко, нужно третье приближение.
Для наглядности на рис. 3.6 даны графики изменения
всех трех обобщенных координат (q19 q2 и q3).
При расчете на ЭВМ необходимо следить за тем, как
велико число итераций, обеспечивающих заданную точность.
Если траектория проходит близко от особой точки (в дан-
ном примере х0 = 0, yG — 0), в которой сливаются два ре-
шения (в данном примере q3 = ±]/хо + Уо)> то число ите-
раций будет возрастать. В этом случае для уменьшения
объема вычислений целесообразно уменьшить расчетный
шаг по траектории. Одновременно плохая сходимость ите-
раций — свидетельство того, что траектория выбрана неудач-
но и манипулятор может работать неустойчиво.
3.6. Обобщенные скорости и ускорения
Пусть заданы законы изменения во времени обобщенных
координат манипулятора. Они, в частности, могут быть
получены при решении обратной задачи вдоль траектории
центра схвата, как это описано в предыдущем параграфе.
При достаточно большом значении L, когда изменения
обобщенных координат на интервале tt-i, tl+1 малы, можно
вычислить производные по времени от обобщенных коорди-
нат по формулам конечных разностей. Будем считать, что
на интеравале tt+1 обобщенная координата изменяется
равноускоренно:
Qj (i-i) — Qji + Qji (h-1 “ h) + 4д (h-i “ h)2/2;
(3.33)
““ Qfl + Qji(h+i ~ h) + + i ~ G)2/2,
где qji, qj u+d — значения обобщенной координаты
в моменты th tr+1, a qJt и ^ — обобщенные скорость
и ускорение в момент tb соответствующие обобщенной
координате qj.
Решая систему (3.33) относительно qJt и qJb получаем
формулы для обобщенных скоростей и ускорений:
q# — [<G(i+i)(G~ G-i)2 “ <?j(i-i)(G+i “ G)2 + Qjith + i ~ G)2 —
— (t| — G-i)2]/[(G+i ~ G)(G ~ G-i)(G+i ~~ G-i)]; (3.34)
4*
99
*Qji = 2[<?ja+i)Ui — fj-i)2 + Qjd-iAh+i “ h) “
— fljifa+i — h-i)]/[Ui + i ~ hHh ~~ h-i)(h+i —
(3.35)
При интервалах одинаковой длительности, т. е. когда
— t\- j
(3.36)
формулы (3.34), (3.35) упрощаются:
Qu s (Qj а+1) “ Qj (i-»Wi+1 “ h-1);
Qji = 4(</j(j+i) + Qj(i-\) — ^Qji)/(h+i “ ^-i)2-
(3.37)
(3.38)
Пример 3.7. Определить значения обобщенных скоростей
и ускорений манипулятора, изображенного на рис. 3.5,
считая, что перемещение центра схвата из положения I = О
в положение Z = 10 (см. пример 3.6) происходит равномерно
прямолинейно за время т — 0,1 с. Расчет выполнить для
положения 1—1 (табл. 3.3).
П ©скольку L = 10, интервал (tJ +1 — tt _ i) равен 0,02 с.
Для определения значений обобщенных скоростей и
обобщенных ускорений в положении 1—1 воспользуемся
данными табл. 3.3* и формулами (3.37) и (3.38):
4п = (-0,1974 + 0,2916)/0,02 = 4,71 с"1;
q21 — (108 — 120)/0,02 = —600 см/с;
4з1 = (71,39 - 104,40)/0,02 = -1651 см/с;
Qu «4(-0,197 - 0,292 + 2• 0,253)/0,022 = 176 см/с2;
q21 * 4 (108 + 120-2-114)/0,022 = 0;
5з1 » 4(71,39 + 104,40 - 2 • 87,80)/0,022 = 1900 см/с2.
Если в кинематических парах манипулятора реализовать
найденные значения обобщенных скоростей и ускорений,
схват манипулятора будет двигаться заданным образом.
3.7. Скорости и ускорения точек звеньев
Зная обобщенные скорости и ускорения манипулятора, мож-
но вычислить скорость и ускорение любой точки любого
звена i манипулятора. Если обозначить через vOi, dOi векторы
скорости и ускорения точки звена i в системе координат
звена 0, то, дифференцируя столбец Ro, получаем (в блоч-
ном виде):
* Координата qt рассчитывалась с четырьмя знаками после
запятой, из которых в табл. 3.3 оставлены только три. В рас-
четах используется значение qt с четырьмя знаками после
запятой.
100
1
(339)
Дифференцируя по времени (3.10) и учитывая, что R,
определяет положение интересующей нас точки звена i
в системе координат, жестко связанной со звеном (, и от
времени не зависит, получим
Rq — T'iR-i'y Ro — T^R^.
(3.40)
Сопоставляя (3.39) с (3.40), видим, что для расчета vOi
и aOi достаточно знать первую и вторую производные от
матрицы Tt. Дифференцируя (3.11) и имея в виду (3.20),
получим:
* 1 *
T'i ~ zE VijkQjQk 4" UijQj*
j - 1 k = 1 j —I
(3.41)
(3.42)
где Vijk — вспомогательная матрица 4x4, равная
(3.43)
Дифференцируя (3.26) по координате qk, получаем следую-
щие выражения для матрицы Vijk:
при j > к
Vijk ~ A i /4.2• • • *^к~ 1 ’^4» — 1 ^4»» (3.44)
при j < к
Vijk = Л1А2... Ak-iQkAk-• 'Aj-tCljAj.. .Ai-iAit (3.45)
при j — к
V/jk — ^41^2 • • Aj— Aj. • • ^i~
(3.46)
где
П2сд(к) = 0;
-10 0
0 0 0
0 0 0
(3.47)
что следует из (П.44) и (П.48).
101
1
Пример 3.8. По условию примера 3.7 определить скорость
и ускорение переносимого манипулятором точечного груза,
координаты которого в системе, связанной со схватом,
равны х3 = 0, уз = 0, z3 = 50 см. Расчет выполнить для I = 1
(см. пример 3.6).
По формулам (3.28) и данным табл. 3.3 вычисляем
- 0,2506 0
-0,9681 0
0 0
0 0
-0,9681 -85,00
0,2506 22,00
0 0
0 0
" 0 0 0 0"
0 0 0 0
0 0 0 1
0 0 0 0
0 0 0 0,2506
0 0 0 0,9681
0 0 0 0
0 0 0 0
Подставляя полученные матрицы и значения обобщенных
скоростей из примера 3.7 в формулу (3.41), получаем:
Т3 = t7314i + и32<]2 + U ззЯз —
Г-1,1803 0 -4,5597 -814,09 1
-4,5597 0 1,1803 — 1494,711
0 0 о — 600 I
0 0 0 о
Наконец, умножая Т3 на [0, 0, 50, 1]т, получим, согласно
(3.40), первую производную” от Ro:
Ro*= T3R3 = [-1042,07, -1435,70, -600,00, OJ.
Первые три элемента столбца представляют собой про-
екции скорости г03 интересующей нас точки на оси декар-
товой системы координат, жестко связанной со стойкой
манипулятора. Следовательно, искомая скорость
*
Гоз = ИЮ42,07)2 + (- 1435,70)2 + 6002 ss 1872,7 см/с.
Чтобы вычислить ускорение этой же точки груза, нужно
определить матрицы V3jk (j = 1, 2, 3; к = 1, 2, 3), описывае-
мые формулами (3.44) — (3.47). Используя выражения (3.15),
(3.16), (3.47) и (П.48), (П.17), (П.44) и имея в виду, что пара
(0, 1) — вращательного, а (1, 2) и (2, 3) — поступательного
типа, находим:
102
У311 — ~ ^вр (^) Т3 —
' cos 0 sin qt q3 sin qt
sin^ 0 —cos<7t — q3cosqt
0 0 0 0
0 0 0 0
0,9681 0 - 0,2506 - 22,00 “1
-0,2506 0 -0,9601 -85,00
0 0 0 0
0 0 0 0 _
F312 — ^1Л1О2у42^3 — ^вр (£) (£) ^2^3 —
- ^вр (£) ^iQyx (£) — 0*
0 0 0 — cos^j
0 0 0 — sin#!
0 0 0 0
0 0 0 0
0 0 0 —0,9681
0 0 0 0,2506
0 0 0 0
0 0 0 0
^321 ~ ^312 “ 0;
K322 = ^1^2^2^3 ~ ~
^323 — — -^хОсд (£) Л-хОя (£) A3 — Л1£2одАз — 0;
^331 = K313J
У332 = ^323 ~
Кззз — ^1^2^з^з = ^41 Лг^сд^з — 0-
Учитывая только ненулевые слагаемые; входящие в (3.42),
получим
Т3 = ^211^1 + 2V3i3qiq3 4- (7з151 + ^зз^з,
или
-22,63 0 -175,94 84,39’
-175,94 0 22,63 -71,69
0 0 0 0
0 0 0 О
Умножая Т3 на R3, получим, согласно (3.40), ускорение
интересующей нас точки переносимого груза:
Поз = [-8712,61, 1059,81, 0]т.
103
1
Абсолютное значение ускорения
о03 = |/( - 8712,61 )2 + (1059,81 )2 * 8776,8 см/с2.
Представить точность, с которой был выполнен расчет,
можно с помощью вычисления скорости и ускорения центра
С схвата, для которого заранее известно, что он движется
прямолинейно равномерно, т. е. для него известны скорость
и ускорение
= [ - 800, - 1500, - 600]’; = [0, 0, 0]’,
причем значения скорости вычисляются по разности' коор-
динат в конце и начале пути.
Если же полученные матрицы Т3 и Т3 умножить на
столбец Я3(с) = [0, 0, 0, 1р, то получим результаты, отлич-
ные от указанных:
£<$ = [-814,1, -1494,7, — 600,0р; «^=[84,09, -71,69, О]1.
Ошибки в расчете скорости и ускорения связаны с тем,
что даже в начале траектории между соседними точками
обобщенные координаты q19 q3 изменяются отнюдь не мало
(см. табл. 3.3 или рис. 3.6). Из-за этого конечно-разностные
формулы (3.37) и (3.38) неточны.
Увеличив число L, т. е. разбив траекторию центра схвата
на большее число интервалов, можно получить более точ-
ный результат. При L — 20 обобщенные скорости и уско-
рения, вычисленные в примере 3.7, уточняются следующим
образом: = 4,5799; q^ = 172,34; q3 = —165,2; q3 = 1825,8.
При этих значениях скорость и ускорение центра схвата
равны
v$ = [-803,3, -1498,7, -600р; а$ = [- 1,9, - 15,9, 0р.
Это вполне удовлетворительный по точности результат.
Расчеты обобщенных координат qx и q3 при этом оказа-
лось необходимым делать не менее чем с шестью знача-
щими цифрами, иначе 51 и q3 были бы определены не
с лучшей, а с худшей точностью, чем при L= 10.
Естественно, что расчеты, требующие высокой точности,
следует проводить на ЭВМ.
104
1
3.8. Алгоритм оптимизации
быстродействия манипулятора
Одной из целей управления может быть максимальное
быстродействие манипулятора. Однако мощности двигателей
ограничены, из-за чего ограничены обобщенные ускорения,
I Qi I @i шах* (3.48)
Кроме того, ограничены обобщенные скорости
I Qi I Vi max* (3.49)
Ограничения по скоростям связаны как с большими
силами инерции, вызывающими опасные напряжения в эле-
ментах конструкции, так и с большими силами трения,
преодолеть которые не могут двигатели. В координатах
(vf, cq) имеется допустимая область сложной формы, кото-
рую грубо описывают неравенства (3.48) и (3.49).
Поставим задачу максимального быстродействия для
манипулятора с шестью степенями свободы следующим
образом. Необходимо переместить схват из начального
состояния в конечное за минимальное время, не нарушая
ограничений (3.48) и (3.49). В начальном и конечном состоя-
ниях скорости звеньев манипулятора равны нулю. Состоя-
ние схвата характеризует шестимерный вектор х. Он вклю-
чает три декартовых координаты центра схвата в системе О
и еще три величины, определяющие ориентацию схвата
в этой системе.
Задача в сформулированном виде слишком сложна, ибо
фактически у манипулятора имеются еще ограничения на
обобщенные координаты qt (не все состояния схвата реали-
зуемы). Упростим задачу следующим образом. Будем счи-
тать, что кроме начального и конечного известны про-
межуточные состояния, которые заведомо реализуемы и
далеки от нереализуемых. Пусть таких состояний имеется
L— 1. Тогда имеется последовательность векторов х0,
хь ..., xL, через которые следует провести траекторию
(х0 — начальное, xL — конечное состояния).
Используя метод, изложенный в § 3.9, можно рассчитать
последовательность векторов qt, составленных из обобщен-
ных координат qlh qlb ..., Qu- Через эту последователь-
ность должна пройти траектория манипулятора в простран-
стве обобщенных координат. Задача сводится к определе-
нию моментов времени, в которые проходятся те или иные
точки траектории.
105
1
Выберем параметр s, который монотонно меняется вдоль
траектории. Это может быть одна из обобщенных коорди-
нат или какая-нибудь декартова координата системы 0, или
какая-нибудь криволинейная координата системы, неподвиж-
ной относительно системы 0. По формулам (3.34) и (3.35),
заменив в них моменты времени соответствующими зна-
вычислить первые и вторые
координат по параметру $:
чениями параметра s, можно
производные от обобщенных
(3.50)
где I — номер точки на траектории схвата. Введем также
средние по интервалам первые и вторые производные:
9л.1+1 = 71(9(1+ «и+1); 9(1.1+1 = 71 (9(1 + 9(1+1)- (3.51)
Через эти величины можно выразить обобщенные ско-
рости и обобщенные ускорения. Так как
9( (s2),
то для выбранных точек и интервалов траектории обобщен-
ные скорости и ускорения равны
Л/
. (3.52)
«Ускорения» хм+1 можно выразить через «скорости» sb
пользуясь соотношением
или его конечно-разностным аналогом
При этом вместо (3.52) получим
(3.53)
(3.54)
(3.55)
Ограничения (3.48) и (3.49), накладываемые на обобщен-
ные скорости и ускорения, согласно (3.54) и (3.55), связы-
вают выбор «скоростей» st:
Sl < vj max/(9(|)2 ;
(3.56)
106
j max*
(3.57)
В крайних точках траектории манипулятор покоится, так
что
(3.58)
(3.59)
Следует выбрать sf таким образом, чтобы были выпол-
нены условия (3.56)—(3.59), и при этом время перемещения
было минимально. Время перемещения можно выразить
через те же «скорости» sb так как
(3.60)
Полученная задача о наилучшем выборе величины s}
может быть решена методом динамического программиро-
вания. С этой целью выбирается параметр точности е —
шаг изменения величин sf. Последовательно просматрива-
ются интервалы (sh sl+i), начиная с последнего интервала,
для которого I
две функции к}
— 1. На каждом интервале табулируются
t(sf) и тДй2), причем величина §{ в таб-
лицах меняется с шагом £. Считается, что функция т!+1 ($?+1)
уже известна. Величина — это минимальное время, за
которое манипулятор можно перевести из точки st в конеч-
ное состояние, выполнив условия (3.56), (3.57), (3.59), если
в точке Si задана «скорость»
§t. Величина s2I+lopt ~ это зна-
чение s2+1 при оптимальном движении манипулятора, обес-
печивающем его приход в конечное состояние за время
Для последнего интервала, где Sj+1 не может иметь,
согласно (3.59), никаких иных значений, кроме нулевого,
в таблице тг имеется лишь одна точка, причем tl (0) = 0,
а вторая таблица не нужна.
Таблицы для I < L строятся следующим образом. Аргу-
мент s2 последовательно увеличивается с шагом е, начиная
от нулевого значения. При каждом значении sj в таблицу
107
Tj записывается вначале очень большое значение Далее,
просматривается таблица для функции т1+1 (s2+1) и для каж-
дой точки этой таблицы проверяется выполнение неравенств
(3.56) и (3.57). Если з2 = О и з2+1=0, то все неравенства
безусловно выполняются. При очень большом значении s,
неравенства не выполняются ни при каких значениях з2+1.
Пусть для некоторой пары s2, s2+1 неравенства выпол-
нены. Тогда вычисляется
(*?+1)
(3.61)
Полученное значение сравнивается с ранее записанным
в таблицу tz значением т£(з2). Если новое значение т/4*54®
меньше старого, оно заносится в таблицу, иначе там
сохраняется старое значение. Если таблица tz обновляется,
то одновременно в таблицу з2 + 1 ор| заносится значение в2+ь
для которого величина т"редв оказалась меньше найденной
прежде. В итоге после просмотра всей таблицы т1+1 (з2+1)
для точки з2 находится минимальное значение Т| и одно-
временно наилучшее значение s2+1, при котором этот мини-
мум достигается. Как уже говорилось, при очень больших
з2 неравенства не выполняются, так что табулирование
тДз2) и s2 + lopt(s2) требует конечного числа операций.
Последовательно табулируя функции Tl-i(s2-i)» Ч.-г($1-2)
и т. д., в конце концов получим таблицу Ti (з2). С ее по-
мощью можно построить таблицу to(so). Однако, согласно
(3.58), из этой таблицы нам нужна только одна точка,
so = 0, так что достаточно вычислить т0 (0). Это будет ми-
нимальное время, за которое покоящийся в точке s0 мани-
пулятор можно перевести в точку sL, подойдя к этой точке
с нулевой скоростью.
Чтобы узнать, как должна меняться «скорость» s вдоль
траектории, нужно последовательно просмотреть интервалы
(sb з|+1), начиная с первого, для которого I = 0. В начале
каждого интервала з2 — известная величина (на первом ин-
тервале она равна нулю). Для этого известного значения
s2 нужно выбрать из таблицы s2+ х opt (з2) — наилучшее зна-
чение s2+1 и перейти к следующему интервалу. Так будут
последовательно найдены все наилучшие значения з2, обеспе-
чивающие быстродействие. Одновременно из таблиц тДз2)
будут известны величины xf. Очевидно,
(3.62)
так что известная «скорость» sf должна быть реализована
108
к известному моменту tt. Обобщенные скорости и ускоре-
ния находятся по формулам (3.52).
Расчеты по описанному алгоритму показали следующее.
Если ввести сильные ограничения на скорость и ускорение
одной обобщенной координаты д, и слабые ограничения
по прочим обобщенным координатам, то оптимальный
закон движения слагается из трех этапов: разгон с пре-
дельным ускорением max, движение с предельной ско-
ростью max, предельно быстрое гашение скорости с за-
медлением 5imax- Манипулятор ведет себя в этом случае
как система с одной степенью свободы.
Если ужесточить ограничения на скорость и ускорение
по прочим обобщенным координатам, то характер опти-
мального движения усложнится. Например, был получен
такой закон: разгон с предельным для q3 ускорением
5з max, движение с предельной для qx скоростью qx max, гаше-
ние скорости с предельным замедлением 5з max-
11 о-видимому, оптимальные законы движения компону-
ются из интервалов, на каждом из которых одно из огра-
ничений (на qt или <ji) Для какой-то одной из обобщенных
координат становится действующим (соответствующее не-
равенство превращается в равенство).
3.9. Построение алгоритма движения
промышленного робота в робототехническом
комплексе из условия минимизации
времени перехода*
Полученные выше решения задачи об определении отно-
сительных положений звеньев манипуляторов являются ос-
новой для разработки алгоритма и программы оптимизации
движений промышленных роботов с позиционной системой
управления. Задаче построения оптимальных движений ма-
нипуляторов посвящено значительное число работ [16],
однако задача далеко не исчерпана ввиду разнообразия
постановок и используемых моделей системы и среды.
Особенностями решения данной задачи являются: учет
геометрии технологического оборудования и типа техноло-
гической оснастки, возможность разного захвата детали
и использование перехвата, применение метода матриц
4-го порядка, возможность использования как кинематиче-
ской, так и динамической модели манипулятора.
* Алгоритмы оптимизации движений, излагаемые в этом пара-
графе, разработаны с участием А. П. Хныкина.
109
Исходные данные для расчета оптимальных дви-
жений манипулятора:
матрицы, определяющие расположение оборудования от-
носительно системы координат, связанной с манипулятором;
матрицы, определяющие расположение детали на обору-
довании ;
«векторы подхода», указывающие направления возмож-
ных движений в рабочей зоне станка при загрузке—раз-
грузке ;
ограничения на точки траектории, обусловленные гео-
метрической формой станков, и ограничения на обобщен-
ные координаты манипулятора;
кинематическая или динамическая модель манипулятора;
ограничения на точность позиционирования;
ограничения на число опорных точек;
начальная и конечная точки движения манипулятора.
Задача сводится к поиску промежуточных («опорных»)
точек траектории в n-мерном рабочем пространстве мани-
пулятора. Она решается методом моделирования на ЭВМ
процесса движения манипулятора на основе кинематиче-
ской или динамической модели его механизма в среде
с препятствиями в виде технологического оборудования.
На основе расчетов полного времени перехода для раз-
личных точек сетки в рабочей зоне манипулятора из на-
чальной точки в конечную выбираются «опорные» точки
траектории, соответствующие минимальному полному вре-
мени перехода манипулятора.
Основные методические и теоретические положения, ис-
пользуемые при построении алгоритма, следующие.
С каждой единицей технологического оборудования свя-
зывается система координат Ocxcyczc, а с промышленным
роботом — система координат OpXpypZp. Расположение еди-
ниц технологического оборудования относительно робота
определяется матрицей 4-го порядка.
С деталью, перемещаемой роботом в технологическом
процессе со станка на станок, свяжем систему координат
ОдХдУдЯц- Тогда положение детали на станке может быть
определено матрицей 4-го порядка Мсд.
Положение детали в системе координат робота опреде-
ляется матрицей Мрд, которая строится как произведение
матриц Мсд и Мрс'.
М= м М
рд — ^рс1У1СД*
С каждым звеном исполнительного механизма робота
связывается система координат XjyjZj (/= 1, ..., л), где п —
ПО
1
число основных звеньев исполнительного механизма. Поло-
жение звеньев друг относительно друга определяется мат-
рицей в4-го порядка
• При захвате детали или заготовки манипулятором его
последнее звено — схват — занимает определенное положе-
ние относительно детали, которое может быть охарактери-
зовано также матрицей 4-го порядка Мзл.
Положение детали в системе координат робота может
быть найдено также через матрицы относительного поло-
жения звеньев по формуле
Л/рд AIqjA/i2 ’ • • Мп — 1, Лд.
4
Координаты произвольной точки звена манипулятора
в неподвижных системах могут быть найдены по формуле
[хо] — М01Мц • • • Мк- 1, к
где [х0] — матрица-столбец однородных координат точки
в системе робота; [хк] — матрица-столбец однородных коор-
динат в системе xkykzk звена k.
Геометрические формы технологического оборудования
задаются в виде объединения множеств точек пространства,
ограниченных плоскостями или поверхностями 2-го порядка.
«Векторы подхода», указывающие направления движений
деталей при загрузке — разгрузке станка, задаются в виде
множеств точек, расположенных на соответствующих пря-
мых.
Дадим описание алгоритма оптимизации по быстродей-
ствию промышленных роботов позиционного типа в среде
с препятствиями. Схема алгоритма приведена на рис. 3.7.
Блок 1 — ввод исходной информации, указанной выше.
Блок 2 служит для вычисления значений обобщенных
координат в начальном и конечном положениях схвата.
Блок 3 служит для выбора допустимых значений обоб-
щенных координат в начальном и конечном положениях.
В блоке 4 происходит перебор сочетаний координат
начальной и конечной точек. Эти сочетания определяют
варианты перехода из начальной в конечную конфигурацию.
В блоке 5 задается сетка в конфигурационном простран-
стве. Точки сетки в дальнейшем используются в качестве
«опорных» точек траекторий.
В блоке 6 непосредственно выбираются начальная и
конечная точки конфигурационного пространства. Здесь же
учитываются «векторы подхода». Это выражается в том,
что начальные ц конечные точки на последнем и первом
участках траектории всегда берутся на «векторах подхода».
Ввод исходных
данных
Решение обратной
задачи о поло-
жениях
Выбор допустимых
начальных и конеч-
ных точек
Перебор сочетании
началь ных и конеч-
ных точек
Задание
сетки
Блок
движения
Выбор начальной
и конечной точек
в конфигурацион-
ном пространстве
Блок перебора ха
рактврных точек
ния на характерные
точки
Рис. 3.7
чения
ние
конечную
точку
характерные,*
'^перебраны ?
координапл
опада-
Перебор различных
способов захвата
выбор траектории
с минимальным
временем
способы захвата
перебраны ?
Печать
Останов
В блоке 7 моделируется движение манипулятора, т. е.
рассчитываются изменения обобщенных координат на осно-
ве кинематической или динамической модели.
В случае использования кинематической модели теку-
щие координаты вычисляются по формуле
112
1
где i — номер шага по оси времени; qj — обобщенная ско-
рость по /-й обобщенной координате.
В случае использования
динамической модели в этом
блоке интегрируются уравнения движения манипулятора,
которые составлены заранее. В этом же блоке вычисляются
декартовы координаты характерных точек.
За характерные точки принимаются центры шарниров
манипулятора и наиболее удаленные от схвата точки де-
тали.
Блок 8 организует перебор всех указанных характерных
точек.
В блоке 9 происходит сравнение координат характерных
точек с ограничениями. Ограничения используются двух
видов:
ограничения, накладываемые геометрией станков; огра-
ничения, накладываемые на положения схвата характеристи-
ками манипулятора.
При попадании на ограничения первого типа возвра-
щаемся в блок 6, для того чтобы начать движение от дру-
гой начальной конфигурации или к другой конечной конфи-
гурации.
При отсутствии попадания на ограничения переходим
к блоку 10. В блоке 10 происходит сравнение числа уже
рассмотренных характерных точек с их заданным числом.
Если точки рассмотрены не все, то переход в блок 8, если
все, то — в блок 11.
В блоке 11 происходит сравнение с ограничениями,
накладываемыми на обобщенные текущие координаты. Если
текущие обобщенные координаты попадают на ограничения,
не достигая промежуточной точки, то переход в блок 6 для
выбора новых начальной и конечной конфигураций, в про-
тивном случае переход в блок 12. В блоке 12 проверяется,
достиг ли схват промежуточной точки.
Разработанный алгоритм и программа позволили решить
задачу оптимизации по быстродействию циклограммы про-
мышленных роботов типа «Универсал» и «РПМ-25» в дей-
ствующих технологических процессах.
Применение алгоритма позволило разработать так назы-
ваемые «карты обучения» промышленных роботов с пози-
ционной системой управления. Эти карты представляют
собой сечение рабочей зоны манипулятора с нанесенной
на ней сеткой, в узлах которой указано полное время
перехода из начального положения манипулятора в конеч-
ное. В качестве примера приведена одна из «карт обучения»
робота «Универсал-15» для участка, состоящего из подаю-
Рис. 3.8
щего устройства, токарного (№ 1), фрезерного (№ 2) и
сверлильного (№ 3) станков (рис. 3.8).
«Карты обучения» позволяют выбрать промежуточные
опорные точки траектории, соответствующие минимальному
времени перехода манипулятора из начального положения
в конечное. При использовании для управления роботом
вычислительных машин разработанный алгоритм применя-
ется непосредственно для реализации оптимальных по
быстродействию движений, минуя процесс обучения.
Опыт использования приведенного алгоритма для рас-
чета оптимальных программ движения промышленных ро-
ботов позволил вскрыть возможности повышения его
(алгоритма) быстродействия. Так как разработанный алго-
ритм построен на принципе моделирования движения ма-
нипулятора в среде с препятствиями, то основное машин-
1
ное время уходит на проверки типа «непопадание в пре-
пятствие». Поэтому был предложен алгоритм, особенностью
которого является разделение процессов моделирования
движения и проверки на «непопадание в препятствие», что
позволило существенно увеличить его быстродействие.
Алгоритм состоит из следующих трех основных под-
задач :
идентификация траекторий по времени без учета пре-
пятствий;
оптимизация времени перехода схвата манипулятора из
начальной точки в конечную без учета препятствий с при-
менением метода динамического программирования;
моделирование движения с учетом препятствий.
Схема модифицированного алгоритма оптимизации дви-
жений промышленных роботов представлена на рис. 3.9.
Содержание первой подзадачи, т. е. идентификация траек-
торий по времени, состоит в следующем. На основе кине-
матической или динамической модели манипулятора для
заданных опорных точек вычисляется время перехода из
начальной точки в конечную без учета препятствий.
В блоке 5 создается массив времен перехода манипуля-
тора из начальной точки в конечную.
Для кинематической модели время перехода вычисляется
по формуле
t = max {(<# - q?)/qf}.
В блоке 6 проверяется, все ли точки сетки перебраны.
Если точки сетки перебраны не все, то — переход в блок 7,
если все, то — в блок 8.
В блоке 8 решается вторая подзадача — оптимизация
времени перехода схвата манипулятора из начальной точки
в конечную без учета препятствий с применением метода
динамического программирования.
Конфигурационное пространство манипулятора разбива-
ется на к областей. В каждой к-й области выбирается
точка и-мерного конфигурационного пространства, которую
следует рассматривать как «опорную» точку.
В случае движения с одной «опорной» точкой блок 8
сортирует массив, т. е. располагает элементы массива по
возрастанию.
В блоках 9... 11 осуществляется моделирование с учетом
препятствий и поиск оптимальной траектории. В блоке 9
после каждого шага по времени происходит переход в блок
10, в котором движущаяся точка схвата и характерные
115
1
Рис. 3.9
точки проверяются на попадание в ограничения, наклады-
ваемые геометрической формой оборудования и характери-
стиками манипулятора. В случае попадания на препят-
ствие — переход в блок 8, где выбирается следующая траек-
тория с большим временем перехода.
Если характерные точки не попали на препятствие, то —
переход в блок И, где осуществляется проверка на попада-
ние схвата в конечную точку: в случае непопадания — пере-
116
1
ход в блок 9 для дальнейшего движения по траектории.
Если схват попал в конечную точку, то заданная траекто-
рия и является искомой оптимальной.
Расчеты по данному алгоритму показали значительное
сокращение затрат машинного времени по сравнению с пре-
дыдущим алгоритмом.
3.10. Решения обратных задач
о положениях манипуляторов в явном виде
методом матриц
Решения обратных задач о положениях манипуляторов
в явном виде имеют важное значение как при проектиро-
вании, так и управлении. При проектировании такие реше-
ния позволяют исследовать влияние конструктивных пара-
метров на процесс движения. При управлении результаты
таких решений позволяют построить быстродействующие
алгоритмы управления.
Сложность задачи о положениях связана, как известно,
с ее нелинейностью, поэтому точные решения не всегда
возможны. Однако особенность исполнительных механизмов
промышленных роботов, состоящая в том, что оси соседних
кинематических пар перпендикулярны или параллельны
между собой, позволяет получать в таких случаях явное
решение. После решения задачи о положениях задачи
о скоростях и ускорениях становятся линейными.
Здесь вышеуказанная задача решается с использованием
матриц 4-го порядка.
Рассмотрим примеры применения матричного исчисления
к задаче о положениях многозвенных механизмов манипу-
ляторов.
Особенностью излагаемого метода является выбор сис-
тем координат на соседних звеньях: системы координат
выбираются таким образом, что старая система может
быть совмещена с новой одним винтовым перемещением.
В этом случае матрицы прямого и обратного переме-
щения отличаются лишь некоторыми знаками при элемен-
тах, что облегчает построение уравнений связей между
переменными и постоянными параметрами механизма.
Определение относительных положений звеньев исполни-
тельного механизма промышленного робота «Универсал-50».
Промышленный робот «Универсал-50» — один из первых
промышленных роботов, разработанных в СССР, однако
такую структурную схему имеют многие современные ро-
боты, например американский робот «Unimate».
117
Исполнительный механизм
(рис. 3.10) обладает пятью
степенями свободы и содержит
четыре вращательные и одну
поступательную кинематичес-
кие пары; Оси вращательных
пар В и D параллельны. Сим-
волически его структурную
схему можно записать в виде
В±В±П1В±В, что показы-
вает последовательность распо-
ложения осей соседних пар
(± — знак перпендикулярности).
Определим относительные
положения звеньев механизма
Рис. 3.10
при заданном положении схвата.
Со звеньями механизма свя-
жем системы координат сле-
дующим образом. С непод-
вижным звеном механизма
свяжем систему координат Axyz, направив ось z по оси
вращения пары А.
Со звеном 1 свяжем систему координат fiXi^Zj, напра-
вив ось Zj так же, как ось z, и ось xt — по оси враща-
тельной пары В. Со звеном 2 свяжем систему координат
Bx2y2z2, направив ось х2 так же, как ось хь а ось z2 —
по оси звена 3. Со звеном 3 свяжем систему координат
Dx3j3z3, оси которой z3 и х3 параллельны соответственно
осям z2 и х2. Со звеном 4 свяжем систему координат
Dx4y4z4, ось которой совпадает с осью х3, а ось z4
направлена по оси вращательной пары Е. Со звеном 5
свяжем систему координат Ex5y5z5, ось которой z5 совпа-
дает с осью z4, а ось х5 направлена перпендикулярно
плоскости движения губок схвата 5.
Положения введенных систем координат друг относи-
тельно друга определим следующим образом.
Система Bx^Zi повернута вокруг оси z системы Axyz
на угол <Pi и смещена вдоль этой оси на величину
Система Bx2y2z2 повернута вокруг оси xt системы Вх^у^
на угол <р2. Система Dx3y3z3 смещена вдоль оси z2 системы
Bx2y2z2 на величину s3. Система Dx4y4z4 повернута вокруг
оси х3 системы Dx4y4z4 на величину (р4. Система Ex5y5z5
повернута вокруг оси х3 системы £>x4y4z4 на угол <р5
и смещена вдоль этой оси на величину /5.
Соответствующие матрицы перехода имеют вид:
J
118
1
о
О 1
М01
cos (pt
sm (pi
— sin <Pj
COS ф1
0
COS ф2
— sm (p2
M23 =
cos q>2
01
cos (p4
— sin (p4
sin ф4
0
COS ф4
0
COS ф5
“ Sin ф5
sin ф5
0
COS ф5
0
Матрицу перехода
положений звеньев по
известной:
Mo s при определении
заданному положению
относительных
схвата считаем
0
0
0
0
_ 0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0 1
0
«12
м05 =
«21
«2
«з
L 0
0
0
Для получения расчетных уравнений запишем матричные
уравнения перехода от системы Рх4у424 к системе Axyz
двумя способами. При первом способе от системы Dx4y4z4
переходим последовательно к системам Dx3y3z3i Bx2y2z2,
Bxlylzlf Axyz. При втором способе от системы Dx4y4z4
переходим к системе Ex5y5z5, а затем к системе Axyz.
В первом случае матричное уравнение преобразования
координат имеет вид
[х] — MoiMi2M23M34[x4J.
Во втором слудае
[х] = MQ5M54 [х4],
(3.63)
(3.64)
1
где [х] и [х4] — матрицы-столбцы координат произвольной
точки соответственно в систе&ахAxyz и Dx%y4z4; М54 —
матрица перехода от системы Dx4y4z4 к системе Ех$у5г5<
Эта матрица имеет вид
sin <р5 О О
cos ф5 О О
О 1 — *5
ООО
(3.65)
Приравнивая
правые части уравнений (3.63) и (3.65),
получим
AfoiAf 12^23^34 — ^05^*54*
Подставляя в это матричное уравнение матрицы, пере-
множая их и приравнивая соответствующие элементы в ле-
вой и правой частях, получим следующие уравнения связи
между постоянными и переменными параметрами механиз-
ма:
coscpi = «и cos (р5 — «12 s,n Ф5»
sin Ф1 = «21 cos ф5 — а22 sin ф5;
а31 cos ф5 - а32 sin ф5 = 0;
— sin ф1 cos (фх 4- ф4) = а1Х sin ф5 4- а12 cos ф5;
COS фх COS (ф2 4- ф4) = «21 sin Ф5 + «22 COS Фб^
sin (ф2 4- ф4) = «31 sin ф5 4- а32 cos ф5;
«хз = sin фх sin (ф2 4- ф4);
а2з = -cos фх sin (ф2 4- ф4);
«зз = cos (ф2 4- ф4);
«1 “ «13*5 = S3 sin ф2 sin фх;
«2 ” «23*5 = - S3 Sin Ф2 cos Фх;
(3.66)
«3 - «33*5 = *1 + S3 COS ф2.
Из этих двенадцати уравнений связи необходимо найти
пять обобщенных координат, определяющих относительнее
положение звеньев.
Найдем вначале значение s3, считая его всегда положи-
тельным. Из трех последних уравнений получим
S3 = }/(«! «13*5)2 + («2 “ «23*5)2 + («3 ~ «33*5 “ *1)2«
120
I
Далее, из десятого и одиннадцатого уравнении системы
(3.66) получим
sin ф2 = |/(«1 - «1зЬ)2'+ («2 - «2зУ2/^3-
Из последнего уравнения системы (3.66) имеем
cos ф2 == (а3 - а33/5 - /ОДз.
При известных значениях ф2, s3 из десятого и один-
надцатого уравнений системы (3.66) найдем функцию угла
Ф1
1 ~ «13^5 ,
s3 sin ф2
sin <Pt =
____ «2 — «2зЬ
COS ф» — . .
S3 Sin ф2
Оставшиеся углы ф4 и ф5 легко определяются из первых
уравнений системы (3.66):
sin (p5 = —
COS (p!
cos ф1
sin Ф1
«13
«21
COS Ф5 —*
«12
sin (<p2
cos (ф2
фд) = «31 sin ф5
фд) = («21 sin ф5
a32cos ф5;
а22СО8ф5)СО8ф!.
Полученные выражения решают вопрос об определении
обобщенных координат механизма манипулятора по задан-
ному положению выходного звена.
Если вычислить значения обобщенных координат, кото-
рые будут реализованы системой управления манипулятора,
то его выходное звено будет занимать предписанные поло-
жения в пространстве, т. е. рассмотренная задача решает
вопрос программирования этого манипулятора при пози-
ционном управлении.
Так как рассматриваемый механизм имеет пять степеней
свободы, а не шесть, очевидно, что его выходное звено
не может занимать произвольные положения в простран-
стве, т. е. на его движение наложено одно условие связи.
Это условие связи может быть найдено из последних
уравнений системы (3.66)
tg ф! = — а13/а23 = (oii - а13/5)/(а2 - а23/5). (3.67)
Определение относительных положений звеньев исполни-
тельного механизма промышленного робота ПР-10. Исполни-
тельный механизм промышленного робота ПР-10 (рис. 3.11)
t
•<
i
г
I
I
1
1
Рис. 3.11
обладает пятью степенями свободы и содержит две посту-
пательные и три вращательные кинематические пары. Сим-
волически структуру этого механизма можно записать
в виде В || П 1 П || В ± В (здесь || — знак параллельности
осей пар).
Рассмотрим задачу об определении относительных поло-
жений звеньев механизма при заданном положении схвата.
Как и ранее, со звеньями механизма свяжем подвижные
системы координат. Со звеном 1 свяжем систему коорди-
нат AXiy^i, направив ось по оси вращательной пары Л,
а ось хл параллельно оси звена 3. Со звеном 2 свяжем
систему координат Cx2y2z2, оси которой параллельны соот-
ветствующим осям Zi и хь причем ось z2 проходит через
ось пары D. Со звеном 3 свяжем систему координат
Dx3y3z3, ось х3 которой совпадает с осью х2, а ось z3
параллельна оси z4. Со звеном 4 свяжем систему координат
£>Х4У4?4, ось х4 которой совпадает с осью х3, а ось z4
проходит через ось вращательной пары Е. Со звеном 5
свяжем систему координат Exsy5z5, ось которой совпадает
с осью z4, а ось х5 направлена по оси х5 схвата 5.
Введенные системы координат расположены друг относи-
тельно друга следующим образом. Система Axiy^ повер-
нута относительно оси z системы Axyz на угол Система
Cx2y2z2 смещена по оси zt системы Ax^tZi на величину s2.
Система Dx3y3z3 смещена по оси х2 системы Cx2y2z2 на
величину з3. Система Dx4y4z4 повернута вокруг оси х3
системы Dx3y3z3 на угол <р4. Система Ex5y5z5 повернута
1
вокруг оси z4 на угол ф5 и смещена вдоль
величину /5.
Матрицы 4-го порядка, соответствующие
нию координат между соседними системами,
этой оси на
преобразова-
имеют вид:
" 1
О
О
О
О 0 s3
1 О О
О 1 о
О 0 1
М34 —
о
cos ф4
sin ф4
О
О О
— sin ф4 О
COS ф4 О
О 1
о
о
о
COS ф5
sin ф5
О
' м45 =
О
— sin ф5 О О
cos ф5 О О
О 1 15
О 0 1
Запишем матричное уравнение преобразования координат
от системы Dx4y4z4 к системе Axyz двумя способами.
При первом способе от системы Dx4y4z4 переходим
последовательно к системам Dx3y3z3, Сх2у222, Axyz.
Матричное уравнение для этого случая имеет вид
[х, у, z]T = М01М12М2зМ34[х4у4г4]т.
(3.68)
При втором способе от системы Dx4y4z4 переходим
сначала к системе Exsy5zs, а затем от системы Ex5y5z5
к системе Axyz. Матричное уравнение для этого случая
имеет вид
[xyz]T = М05М54 [x4y4z4]T.
(3.69)
В уравнении матрица Л/05 есть матрица перехода от
системы x5y5z5 к системе xyz. Так как положение схвата
считается заданным, то элементы этой матрицы считаем
известными:
^12 «13 «1
«22 «23 «2
«32 '«33 «3
0 0 1 _
123
1
Матрица М54, может
путем транспонирования
быть получена из матрицы М45
•f
cos (р5
— sin ф5
О
О
sin ф5
cos ф$
О
О
о
о
о
Приравнивая правые
части уравнений (3.68) и (3.69),
получим
^01^12^23^34 — Л^05^54-
(3.70)
Подставляя значения матриц, перемножая их и прирав-
нивая соответствующие элементы полученных матриц в ле-
вой и правой частях, найдем:
«и cos ф5 — «12 sin Ф5 = cos Ф1; «13 == sin ф! sin ф4;
«21 cos ф5 — «22 sin ф5 = sin Ф1; «2з = —cos ф1 sin ф4;
«31 cos ф5 - «32 sin ф5 = 0; «33 - cos ф4;
. (3.71)
«11 Sin ф5 4- «12 COS ф5 = —Sin ф! COS ф4; «1 — «13/5 = S3 COS Ф1;
«21 sin ф5 4- «22 COS ф5 = COS ф1 COS ф4; «2 — «23^5 ~ s3 SM Ф1J
«31 sin ф5 4- «32 cos ф5 = sin ф4; «3 — «33/5 = $2,
где afj (i, j = 1, 2, 3) — известные элементы матрицы М05;
af (i — 1, 2, 3) — координаты начала системы Cx5y5z5 в сис-
теме Axyz.
Из уравнений (3.71) находим пять переменных парамет-
ров следующим образом:
s2 = «з - а3з^: «з = l/(«i - «isW2 + («2 ~ «гз^)2;
cos ф4 = a?3; 51Пф4 = a^/sn^; созф1 = («! - «13/5)/5з;
sin Ф1 = («2 - а23/5)/$з;
cos ср —
COS Ф1
sin Ф1
/Д; sin ф5 =
COS Ф1
sin Ф1
«11' «12
«21 а22
*
Неиспользованные уравнения служат для проверки полу-
ченных значений обобщенных координат.
Рассматриваемый механизм имеет пять степеней свобо-
ды, а пространственное движение твердого тела общего
вида определяется шестью независимыми параметрами,
124
1
следовательно, на движение выходного звена механизма
наложено одно общее условие связи. Это условие связи
имеет вид
tgФ1 = -«1з/а2з = ~(«2 - «гзМЛа] ~ а13/5).
(3.72)
Последнее условие должно учитываться при решении
вопроса о возможности применения рассматриваемого ро-
бота в заданной технологической операции.
Определение относительных положений звеньев исполни-
тельного механизма промышленного робота «Универсал-5»
(рис. 3.12). Символически структуру этого механизма можно
записать в виде В || П || В 1 П || В 1 В. Исполнительный ме-
ханизм робота (рис. 3.12) обладает шестью степенями сво-
боды и содержит две поступательные и четыре вращатель-
ные кинематические пары.
Со звеньями механизма свяжем системы координат сле-
дующим образом. Со стойкой свяжем систему координат
Oxyz, направив ось z по оси вращательной пары; со зве-
ном 1 свяжем систему координат Ox^y^z^ направив ось Zi
так же, как ось z, а ось Xi по направлению кратчайшего
расстояния между осями вращательных пар 0 и В. Со зве-
ном 2 свяжем систему координат Bx2y2z2, направив ось х2
параллельно оси хь а ось z2 параллельно оси zY. Со звеном
3 свяжем систему координат Bx3y3z3, направив z3 так же,
как ось z2, а ось х3 по поступательной паре С.
Со звеном 4 свяжем систему координат Dx4y4z4, напра-
вив ось х4 так же, как ось х3, а ось z4 параллельно оси
z3. Со звеном 5 свяжем систему координат Dx5y5z5, напра-
вив ось х5 по оси вращательной пары Е, а ось у5 так же,
как ось у4, по оси вращательной пары D. Со звеном 6
125
положения
свяжем систему координат Ex6y6z6, направив ось х6 так же,
как ось х5, а ось z6 перпендикулярно плоскости движения
губок захвата.
Матрицы, определяющие относительные
звеньев, имеют
Г COS фх
О
О
О
О
А/qi =
-sin фг
cos фх
О
о
о
; М12 —
о
о
о
о
о
М2з —
COS Фз
sin фз
О
О
- sin фз
cos фз
о
о
о
о
О
> ^34
м45 =
COS ф$
О
- sin ф5
О
1 О
О
О
о
о
о
_ о
cos ф6
sin ф6
О
о
о
о
О
sin ф5
о
COS ф5
О
О
- sin фб
cos ф6
О
о
о
о
lO
о
о
О
о
о
Положение схвата относительно неподвижной
координат определяется матрицей
системы
а12
«1
А/об
О
О
О
Матричное уравнение связи имеет вид
A^01M12Af23Af34Af45A<f56 = Mq&
Это уравнение можно переписать в другом виде
^01^12^2зА^34 = М0бМб5М54, (3.74)
где М65 и М54 — матрицы, обратные матрицам М56 и М45,
(3.73)
1
II О О — /б
О cos (ре sin ф6 О
О — sin ф6 созфб О
0 0 0 1
«М|
Icos ф5 О
О 1
п
sm ф5 О
О О
— sin ф5
О
COS ф5
О
О
О
О
Перемножая матрицы в левой и правой частях уравне-
ния (3.74) и приравнивая соответствующие элементы, полу-
чим систему переменных параметров:
«и cos ф5 4- sin ф5 (а12 sin ф6 4- a'13cos ф6) = соз(ф1 4- ф3);
а21 cos ф5 4- sin ф5 (а22 sin ф6 4- а23 cos ф6) = sin (ф! 4- Ф3);
a3i cos ф5 4- sin ф5 (а32 sin ф6 4- а33 cos ф6) — 0;
а12 cos ф6 - «1з sin ф6 = -sin (ф1 4- ф3);
а2 cos ф6 — а23 sin ф6 = cos (Ф1 4- ф3);
а32 cos ф6 - а33 sin ф6 = 0;
(3.75)
— «13 sin ф5 4- cos ф5 (а12 sin ф6 4- ai3 cos ф6) = 0;
— a2i sin ф5 4- cos ф5 (а22 sin ф6 4- а23 cos ф6) = 0;
- а31 sin ф5 4- cos ф5 (а32 sin ф6 4- а33 cos ф6) = 1;
«1 - ^6«i 1 ~ s4 cos (ф! 4- ф3) 4- г cos ф1;
а2 ~ *б«21 = «4 sin (Ф1 4- Фз) 4-,г sin Ф1;
«3 — /б«31 — S2-
Полученные 12 уравнений содержат шесть искомых пере-
менных параметров ф15 s2, ф3, s4, ф5, ф6. Для их опреде-
ления запишем:
из шестого уравнения (3.75)
tg Фо — «зг/«зз9
из третьего уравнения
«32 sin ф6 4- «зз cos ф6 = —«31 ctg ф5.
Подставляя это выражение в девятое уравнение, получим
sin ф5 =
127
1
Из четвертого уравнения системы (3.75) найдем
sin (ф! 4- фз) = —(«12 cos фб - an sin фб).
Из последнего уравнения системы (3.75) имеем
s2 = «з - 16а31.
Из десятого и одиннадцатого уравнений системы (3.75)
найдем переменные ф3 и s4. Возводя в квадрат и склады-
вая эти уравнения, получим квадратное уравнение для
определения s4:
а2 + s4 — 2«iS4 cos (ф1 + фз) 4- а2 — 2а2$4 sin (ф1 + ф3) — г2 =»0,
откуда
s4 = «1 cos (фi + фз) + а2 sin (фt + ф3) ±
± }/[«! cos (ф1 +' фз) 4- а2 sin (ф! 4- ф3)]2 - а2 - а2 4- г2.
Определив s4, угол ф1 найдем из уравнения
s4 — а2 — а2 — г2 — 2п?1 cos Ф1 — 2ra2 sin ф! — 0,
ГДе Hi = «1 - «11/6; «2 = «2 - «21^6-
Ввиду того что для определения углов ф6, ф|, Ф1 4- Фз
приводится лишь по одному выражению их тригонометри-
ческих функций, решение будет неоднозначным. Поэтому для
выбора окончательных значений переменных параметров
следует проверить все независимые уравнения при найден-
ных значениях переменных параметров.
Таких независимых параметров уравнений в данном
случае шесть. Это два первых, четвертое и третье урав-
нения системы (3.75).
Особенностью данного манипулятора является возмож-
ность обеспечения поступательного перемещения объекта
путем координации только двух углов ф! и ф3.
Определение относительных положений звеньев исполни-
тельного механизма промышленного робота «Универсал-15».
Исполнительный механизм этого робота обладает шестью
степенями свободы и содержит две поступательные и че-
тыре вращательные пары (рис. 3.13). Структурная схема
механизма символически записывается в виде П ± В ± В ±
1 П 1 В 1 В.
Рассмотрим задачу об определении относительных по-
ложений звеньев манипулятора при заданных положениях
схвата. С подвижными звеньями механизма свяжем системы
координат аналогично тому, как это было сделано для
механизма робота «Универсал-50».
128
1
Матрицы 4-го порядка, соответствующие преобразованию
координат между соседними системами, имеют вид
ЛТо1 —
1 О 0 Si
0 10 0
0 0 10
_ 0 0 0 1_
Г 1 о
Icos ф2 — sin
sin <р2 cos ф2
О О
О О
О 01 Г 1 О
О cos фз
О sin фз
_ О О
— sin фз О
cos фз О
М34 =
'О о
о о
о о
1 $4
1
о
о
о
о
COS фз
sin ф5
О
о
— sin ф5
COS фз
О
О
О
О
1
О О
О О
О
м
м
О
О 1
L О О
(cosф6 — sinф6 0 01
мпф6 cos ф6 О О
О О и6
О 0 0 1_
Механика промышл. роботов, кн. 1
129
1
Положение звена 6 считаем заданным, а элементы
матрицы М06 известными. Составим матричное уравнение
связи в виде
Л/06 = Мо1М12М2зЛ/з4М45М56,
(3.76)
или
М12 ^01 ^06 ~ -Л'/23-^Г34-^45-^Г56’
Перемножая матрицы в левой и правой частях урав-
нения и приравнивая соответствующие элементы, получйм
систему двенадцати уравнений относительно шести неиз-
вестных :
(3.77)
COS ф2«11
COS ф2^12
COS ф2«13
sin Ф2ОС21 = cos ф6;
sin ф2а22 = — sin ф6;
sin ф2а23 = 0;
-sin ф2ап
-ЯПф2«12
-sin ф2а13
cos ф2а21 — 0;
COS Ф2«22 — COS ф6 COS (ф3
COS ф2«23 = -sin(93
Ф5);
Ф5);
(3.78)
a32 = cos ф6 sin (фз + ф5);
а33 = cos (фз + ф5);
(cq — Si) cos ф2 + а2 sin Ф2 — 0;
— («j - sO sin ф2 + a2 cos ф2 = /6 sin (ф3 + ф4) - s4 sin ф3;
(«з - 12) = k cos (фз + Фб) + s4 cos фз,
где %• (i, j = 1, 2, 3) — известные элементы матрицы M06.
Из этих уравнений найдем переменные параметры меха-
низма по формулам
cos (ф3
а2
os ф2 + а2 sin ф2;
а2 cos ф2 + 16 (фз
1211/2.
Фа)]
(3.79)
+ [(аз — 12) -
cos фз - [(а3
Последние уравнения еще не определяют однозначно
значения обобщенных координат манипулятора, поэтому
для их нахождения следует использовать и другие уравне-
ния системы (3.78).
h) ~~ ^6 COS (Фз
Фз)] «4 1-
130
i
г
1
f
1
I
1
Г лава 4
Метод винтов и дуальных матриц
в кинематике манипуляторов
Классическая теория винтов создана Р. Боллом, винтовое
исчисление разработано А. П. Котельниковым и Э. Штуди
и развито в работах И. И. Занчевского, Д. И. Зейлигера,
Ф. М. Диментберга и др. [21, 22].
4.1. Сведения из классической теории
винтов и винтового исчисления
Известно, что произвольная система^ скользящих векторов
может быть приведена к вектору V и моменту К0, оси
которых коллинеарны. Такой геометрический образ назы-
вают винтом. Тогда можно записать
= рК, (4.1)
где р — скалярный множитель. Этот множитель называется
.параметром винта.
Винт нулевого параметра, модуль которого равен еди-
нице, будем называть единичным винтом, он же
является единичным скользящим вектором.
Если параметр винта равен нулю, то винт эквивалентен
одному скользящему вектору, если вектор равен нулю, то
винт эквивалентен паре.
Относительный момент двух винтов U и V
(4.2)
где V0, <7° — моменты винтов относительно^ какой-либо
точки; V, U — соответственно векторы винтов V и U. Отно-
сительный момент двух винтов представляет инвариант,
не зависящий от выбора точки пространства.
Два винта, относительный момент которых равен нулю,
называются взаимными.
Плюккеровы координаты скользящего вектора и винта.
Пусть в системе прямоугольных декартовых координат
задан скользящий вектор V в точке Л(х, у, z). Тогда
момент этого вектора относительно начала равен
т (F) = б А х К = (yVg - zVy) i + (zVx - xVJj + (xVy - yVx) k.
Скалярные множители перед ортами i, j, к, представляю-
щие моменты вектора V относительно осей х, у, z, назы-
вают плюккеровыми координатами скользя-
щего вектора.
Аналогично вводятся плюккеровы координаты винта.
Пусть задан винт V в точке А (х, у, z), т. е. известны его
вектор V и момент V0. Шесть величин — проекции вектора
V и момента V0 на оси: Vx, Vy, К, V?, V®, V°z — называ-
ются плюккеровыми координатами винта.
Плюккеровы координаты, винта произвольной системы
скользящих векторов есть проекции главного вектора и
главного момента системы на координатные оси.
Параметр винта через плюккеровы координаты
вании работы [13] может быть записан так:
на осно-
(4.3)
построе-
Символ Клиффорда. Комплексный вектор. При
нии винтового исчисления используются операции, анало-
гичные операциям векторного исчисления. В этом случае
винт представляется в виде некоторого комплексного век-
тора с использованием дуальных чисел.
У. Клиффордом введена операция, с помощью которой
винт V может быть представлен в виде комплексного
вектора
V = V + ё V °, (4.4)
где 6 — символ Клиффорда. В операциях с комплексными
векторами, содержащими символ е, этот символ играет роль
числа, обладающего свойством е2 — 0.
Введение комплексного вектора с множителем е приводит
к тому, что результаты операций над винтами систем
скользящих векторов оказываются независимыми от точки
приведения. Кроме того, комплексными векторами можно
оперировать как дуальными числами, имея в виду свойство
символа 6.
Введение и использование дуальных чисел явилось одним
из важных этапов создания винтового исчисления, в кото-
ром операции над винтами проводятся по формулам,
тождественным формулам векторного исчисления, однако
объектами операций являются уже не векторы, а винты,
представляемые в виде комплексных векторов [13].
Комплексный угол между двумя осями. Комплекс-
ным углом между двумя осями с единичными винтами
ёг и ё2 называется фигура, образованная этими осями
и отрезком прямой между ними, пересекающей эти оси
под прямым углом (рис. 4.1). Линия кратчайшего расстоя-
132
1
ния между прямыми называется
осью комплексного угла.
Комплексный угол между дву-
мя осями характеризуется углом а,
на который нужно повернуть век-
тор ё2 до совмещения его направ-
ления с направлением вектора ёъ
и кратчайшим расстоянием между
осями /1.
Комплексному углу можно поставить в соответствие
винт, равный
Л = е12 (ос + e/i),
где е12 — единичный винт или скользящий единичный век-
тор, определяющий ось комплексного угла; е — символ
Клиффорда.
Для комплексного угла принимается обычное правило
знаков векторной алгебры, т. е. угол а считается поло-
жительным, если наблюдатель, смотрящий навстречу век-
тору е12, видит поворот против часовой стрелки.
Величина h считается положительной, если от вектора
ё2 к вектору ё1 перемещение происходит в положительном
направлении е12.
Сложение винтов. Винт V называется суммой винтов
И2,...,К„, если его вектор равен сумме векторов
слагаемых винтов, а момент относительно любой точки
пространства равен сумме моментов слагаемых винтов
относительно этой же точки. Запись
п
означает, что
п
где V — момент винта V; Q — вектор винта F; — мо-
менты слагаемых винтов; — векторы слагаемых винтов.
Из формул (4.4) следует, что плюккеровы координаты
суммы винтов равны сумме плюккеровых координат сла-
гаемых винтов.
Механический смысл операции сложения винтов — сло-
жение бесконечно малых векторных перемещений твердого
тела при одновременном его вращении и перемещении
133
относительно нескольких осей. Такие мгновенные векторные
перемещения складываются как комплексные векторы.
Это означает, что, во-первых, складываются проекции
векторов угловых скоростей вращения тела вокруг осей.
При этом находятся проекции главного вектора системы —
вектора абсолютной угловой скорости тела.
Во-вторых, складываются проекции линейных скоростей
движения тела вдоль осей и моменты векторов угловых
скоростей относительно точки приведения и получаются
проекции главного момента системы — вектора абсолютной
угловой скорости полюса.
Таким образом, в данном случае складываются плюк-
керовы координаты слагаемых винтов.
Дуальные углы Эйлера и дуальные матрицы. Как известно,
относительное положение двух систем координат с общим
началом может быть определено тремя углами Эйлера:
ф, 0. Аналогично, относительное положение двух коор-
динат в пространстве может быть определено тремя
дуальными углами Эйлера ф, ф, 0, причем
ф = \|/ + еф0; ф = ф + Еф0; 0 = 0 + е0о,
этим углам соответствуют три винтовых перемещения
вокруг соответствующих осей.
Винтовым перемещениям относительно осей х, у, z со-
1 ответствуют дуальные матрицы, по виду аналогичные
матрицам чистых поворотов. Дуальная матрица, соответ-
ствующая винтовому перемещению вокруг оси z, имеет вид
cos \|/ sin ф О
— sin cos ф О
О 0 1_
а матрицы, соответствующие винтовым перемещениям
вокруг осей х и у,
Г1 0
[ф] = I 0 cos ф
Lo — sin ф
соответственно имеют вид
0
sin ф
cos ф
Г cos р 0 — sin р
[Й = о I о
|_sin р 0 cos р —
Винтовому перемещению вокруг оси х на угол 0 соот-
ветствует дуальная матрица вида
1
Тогда результирующая дуальная матрица для общего
пространственного перемещения может быть найдена как
произведение дуальных матриц последовательных винтовых
перемещений
м = м [ё] [ф].
Правила арифметических
действий с дуальными матри-
цами совпадают с соответствующими правилами для веще-
ственных матриц.
Дуальная матрица преобразования составляющих дуаль-
ного вектора сохраняет свойства ортогональности, поэтому
ее обращение идентично транспонированию. Свойства орто-
гональности значительно упрощают операции с дуальными
матрицами.
Ввиду того что матрицу преобразования координат
можно рассматривать как оператор движения, матрицы
3-го порядка комплексного поворота можно называть вин-
товыми матрицами, так как они соответствуют винтовому
перемещению тела относительно некоторой оси на комплекс-
ный угол.
Применение винтовых матриц 3-го порядка несколько
сокращает число операций при анализе механизмов по
сравнению с методом матриц 4-го порядка.
Тригонометрические функции дуальных углов выра-
жаются так:
sin а — sin а + е а° cos а;
cos а = cos а — е а° sin а;
tga = tga + еа°(1 + tg2a).
Кроме того, любое дуальное число А = (а 4- ша0) может
быть представлено в виде
е р) — аеер.
Проекции винта на оси прямоугольной системы координат.
Для винта V— Ve, образующего с осями дуальные углы а,
Р, Y, дуальные компоненты определяются формулами
Vx = iTcos a; Vy = jKcos p; Vg = kVcos y,
где V— дуальный модуль винта.
Эти формулы аналогичны формулам для проекций век-
тора, кроме того, они показывают, что любой винт можно
представить как совокупность трех ортогональных винтов.
Винты Vx, Vy, Vg называются базисными.
135
Группы винтов. Группы винтов были введены в меха-
нику А. П. Котельниковым и рассматривались Ф. М. Ди-
ментбергом [14]. Понятие групп винтов, как будет показано
ниже, может быть использовано для описания кинемати-
ческих свойств манипуляторов.
Рассмотрим линейную комбинацию винтов
И
где at = 1, ...» п — вещественные числа; Vt — винты. Винты
Vi называют линейно независимыми, если нельзя
подобрать п таких вещественных чисел аь которые бы, не
будучи все равны нулю одновременно, удовлетворяли бы
равенству
X ai^i ~ О’ (4.5)
4=1
Линейная комбинация п линейно независимых винтов
называется и-членной группой винтов. Винты назы-
ваются основными винтами группы.
Как было сказано, любой винт может быть представлен
в виде суммы трех базисных винтов, следовательно, любую
группу винтов можно представить, используя всего три оси.
А. П. Котельниковым предложена следующая символика
для классификации групп винтов: на первом месте ставится
арабская цифра, показывающая число членов в группе, на
втором — римская цифра, обозначающая число осей в группе;
на третьем — арабская цифра, показывающая число момент-
ных членов в группе.
Трехосная группа винтов в общем случае является
шестичленной, содержащей три векторных числа и три мо-
ментных числа, следовательно, она обозначается так: 6, III, 6.
)
t
I
[
2
1
1
4.2. Группы кинематических винтов
манипуляторов
Выходное звено манипулятора в каждый момент времени
обладает некоторой абсолютной линейной скоростью по-
люса V и угловой скоростью £2, которые могут быть
приведены к одному кинематическому винту. Этот винт
называется кинематическим винтом манипу-
лятора.
136
1
Пусть манипулятор содержит вращательные и посту-
пательные кинематические пары. Тогда абсолютная угловая
скорость выходного звена
где cofc — вектор относительной угловой скорости в шарнире
к; qk — обобщенная скорость в шарнире к; ёк — орт шар-
нира к.
Абсолютная скорость полюса выходного звена может
быть определена по формуле
ж
где индекс i — номер вращательной, а индекс j — номер
поступательной пары; rin — радиус-вектор, проведенный из
центра шарнира i к полюсу выходного звена п.
Векторы У on и в общем случае составляют между
собой угол а. Приведем их к одному винту. Этот винт
представляет собой сумму кинематических винтов пар:
где et — единичные винты осей кинематических пар; X; —
скалярные коэффициенты.
Плюккеровы координаты кинематического винта вы-
ходного звена определяются по правилу сложения винтов,
т. е. плюккеровы координаты суммы винтов равны сумме
плюккеровых координат винтов.
Если кинематическая цепь содержит п вращательных
или т поступательных пар, то винт скоростей выходного
звена принадлежит и-членной группе винтов.
Действительно, векторы Vj линейных скоростей являются
свободными, поэтому при их приведении к выходному
звену они могут быть заменены одним вектором, а
направления осей скользящих векторов относительных угло-
вых скоростей не изменяются.
Рассмотрим некоторые открытые кинематические цепи
переносных движений манипуляторов.
Кинематическая цепь с двумя поступательными и одной
вращательной парами. Приведем векторы относительных ли-
нейных и угловых скоростей к некоторой точке схвата.
137
Получим
вектор линеинои скорости полюса:
угловой скорости звена
и вектор
^з — •
Совокупность векторов С13 и У3 эквивалентна одному
винту с параметром
(4.6)
где г — главный момент винта; г — главный вектор винта.
Для данного случая:
г — ;
г° = о>1 х r13 + К + V3 = (O1S3Y3 + *2&з + Узк
Тогда на основе формулы (4.6) получим
р — V2/cox.
(4.7)
Таким образом, параметр кинематического винта этого
манипулятора не зависит от скорости в поступательной
паре, а определяется скоростями сох и V2.
Согласно классификации [21], группа кинематических
винтов манипулятора ВПП относится к одноосной группе
типа (1, I, 0).
Кинематическая цепь с двумя вращательными и одной
поступательной парами (рис. 4.2). Кинематические винты этого
манипулятора образуют двухосную группу. Определим тип
этой группы.
В качестве осей основных винтов возьмем оси Bx2y2z2>
связанные со звеном 2. Приведем векторы относительных
линейных и угловых скоростей к точке В. Получим:
F V3 sin ф3Гх,
векторы относительных угловых скоростей
вектор относительной
(4.8)
4
t
ч
I
г
J
I
1
— юх "4“ (ох —
Ув = Уз = Уз cos
где ©1 и <о2
во вращательных парах А и В; V3
линейной скорости в поступательной паре С.
Вектор есть главный вектор системы скоростей, а
VB — главный момент. Параметр кинематического винта
_ V3&i cos фз
р _ _
К3со2 sin фз
<о!
Таким образом, параметр кинематического винта этого
манипулятора зависит не только от угловых и линейных
(4.9)
138
1
Рис. 4.3
относительных скоростей, но и от угла между звеньями
1 и 2.
На основании формул (4.5) можно записать выражение
для группы винтов этого манипулятора в виде
у= (Ю1 + g V3 cosxp3) fci + (со2 + е V3 sin (p3) ir,
(4.10)
где
fci и — единичные винты.
Такая группа винтов, согласно классификации [21], явля-
ется двухосной четырехчленной группой типа (4, II, 2).
Кинематическая цепь с тремя вращательными парами
(рис. 4.3). Оси вращательных кинематических пар А и В
перпендикулярны, а оси пар В и С параллельны.
В качестве основных винтов выберем единичные винты
осей системы координат Bx^Zi, связанной со звеном 1.
Ось zr этой системы совпадает с осью вращательной
пары А. Ось Вх! совпадает с осью вращательной пары В и,
следовательно, параллельна оси пары С.
Главный вектор системы скоростей в точке В
= d>i + со2 + ®3 = coifci 4- (со2 4- со3) ;
На основе последних формул можно представить выра-
жение для группы винтов этого манипулятора в виде
V = (Oifcj 4- (а>2 4- а>3) ц, (4.11)
где fcj й ц — единичные винты осей zx и системы
Вх^у^.
Такая группа является двухосной двучленной группой
типа (2, II, 0).
1
Кинематическая цепь с
[вумя вращательными и двумя
поступательными парами (рис. 4.4). Оси вращательных пар
А и D перпендикулярны. В качестве основных винтов
системы примем единичные винты системы координат
Приводя в точку С все векторы скоростей, получим
главный вектор системы
= С01 + «4 = (iiiki
(O4.i1
и главный момент
Vc = V2ki 4- V3ii.
На основании последних формул можно записать выра-
жение для группы кинематических винтов:
У= (©! 4-е V2) к{ 4- (со4 4- g V3) .
Такая группа является двухосной четырехчленной груп-
пой винтов типа (4, II, 2).
Параметр кинематического винта манипулятора в любой
момент времени равен
Pi = (®i^2 + (ОдИзИсо? 4- coj).
Параметр кинематического винта этого манипулятора не
зависит от конфигурации манипулятора, а определяется
лишь угловыми и линейными скоростями в кинематических
парах.
Широкое распространение для выполнения движения
инструмента цилиндрической формы получили промыш-
ленные роботы с пятью степенями свободы.
Кинематическая цепь с пятью вращательными парами
(рис. 4.5). Оси вращательных пар В и С манипулятора
параллельны в любой момент времени.
Со звеньями манипулятора свяжем системы координат
(i = 1, 2, ..., 5), как показано на рис. 4.5. Оси
Ряс. 4.4
140
(i = 2, 3, 5) направлены по осям вращательных пар
В, С, Е. Скользящие векторы угловых скоростей
(i = 1,..., 5) — винты нулевого параметра — коллинеарны ор-
там соответствующих осей и заданы в системах координат,
связанных со звеньями
со2 = co2i2; «з = ®3i3; cb4 = со4£4; <о5 = w5i5.
Абсолютная угловая скорость выходного звена 5 есть
главный вектор систем угловых скоростей
Запишем этот вектор в осях системы x3y3z3, связанный
со звеном 3. Для этого все векторы спроецируем на
оси системы Cx3y3z3, получим
[<°1]з — L3i [coj i —
О
О
COS (ф2 + ф3)
- sin (ф2 + Фз)
о
sin (ф2 + фз)
cos (ф2 + фз) _
о
О
(01
О
©! sin (ф2
О»! COS (ф2
Фз)
Фз)
где L31 — матрица перехода от системы Ах^у^^ к системе
Cx3y3z3; [со^з — матрица-столбец координат вектора cbj в
системе Cx3y3z3; [coi]i — матрица-столбец координат векто-
ра в системе Ах^у^.
Аналогично,
со2
О
. 0.
[со3]з =
[с04]з =
о
_С04 -
I (05 COS ф4
[со 5] 3 = 1 со 5 sin ф4
о
Вектор абсолютной угловой скорости
Й5 — (С02 + СОз + СО5 COS ф4) 1*3 + [cot sin (ф2 + Фз)
-I- ю5 sin ф4]Уз + [cot cos (ф2 + Фз) + со4] к3.
1
Найдем главный момент системы угловых скоростей
относительно точки £, т. е. линейную скорость этой точки.
Составляющие этой скорости
Ve = Oz sin <р2 + h sin (<р2 + Фз)] <Мз “
- Рз (®3 + °>2) + ?2®2 cos Фз] Уз + h sin фз©2^3 •
' Здесь р£ = £ Vi9 где Vt — составляющие линейной скорости
точки Е звена 5 при движении только в шарнире номера i.
Тогда винт звена 5 в любой момент времени может
быть представлен в виде
. у5 = j3 [(cd2 4- ©3 4- ©5 cos ф4) + е (l2 sin ф2 4-
+ l3 sin (ф2 4- фз) ©2)] - у 3 [<»i sin (ф2 + фз) +
4- ©5 sin ф4 + е (/3 (со 2 + 03) + /2о2 cos ф3)] 4-
+ к3 [юх cos (ф2 + фз) + о4 + е (/3 sin ф3ю2)].
Эта формула показывает, что кинематический винт ма-
нипулятора принадлежит к трехосной, шестичленной группе
типа (6, III, 3).
Кинематическая цепь с шестью вращательными парами
(см. рис. 2.4). Запишем главный вектор системы угловой
скорости в координатах системы x4y4z4, связанной со
звеном 4:
Q = i4 (ю6 cos ф 5
+ w4)
у4 (©х sin ф4
©е sin ф5) +
/с4 (®>5 + ©1 COS ф4) .
Главный момент системы относительных скоростей в
кинематических парах относительно точки Е равен линейной
скорости точки Е
VE = Г3Г4 4- ©xs3 cos ф4у4 - ©xs3 sin ф4й4.
В комплексном виде кинематический винт манипулятора
V = Г4 (©6 cos ф5
©4 4- е V3) + у4 [(юх sin ф4 4- ю6 sin Ф5)
4- е юл cos ф4] — fc4 [(©х cos ф4 + со5) — e©xs3 sin ф4].
Последняя формула показывает, что кинематический винт
манипулятора принадлежит трехосной шестичленной группе
типа (6, III, 3).
Определим параметр кинематического винта манипуля-
тора
р = [(о6 cos ф5 + ©4) Г3 4- (©1 sin ф4 4- со6 sin ф5)смз созф4 4-
(шх cos ф4 4- ©5) ©1S3 sin ф4]/[(©4 cos фз 4- ©4)2 4-
(©! sin ф4 4- ©б sin ф5)2 4- (©1 cos ф4 4- ©s)2] •
142
Выражение для параметра кинематического винта мани-
пулятора показывает, что он зависит от конфигурации
манипулятора, а именно, от углов <р4 и ф5.
4.3. Метод дуальных матриц
Покажем применение матриц с дуальными элементами для
решения задач анализа кинематики манипуляторов. Дуаль-
ные матрицы нашли применение для решения задач о
положениях и скоростях в пространственных механизмах.
Пример: решение прямой задачи о скоростях методом
дуальных матриц [76]. Рассмотрим незамкнутую кинемати-
ческую цепь с тремя винтовыми кинематическими парами,
оси которых скрещиваются (рис. 4.6). Кинематические цепи
с вращательными и поступательными парами могут быть
получены из данной, если положить параметр винта р = О
в случае вращательных и р == оо в случае поступательных
пар.
Единичные винты кинематических пар обозначены el9 е2,
е3 и орты общих перпендикуляров к осям — h± и й2. Кон-
фигурации звеньев 1 и 2 определяются дуальными углами
«! = «! + eht; d2 = а2 + eh2,
где е — символ Клиффорда, обладающий свойством е2 = 0.
Шаги винтовых пар plt р2, Рз считаем постоянными
и заданными.
143
1
Положения звеньев: 1 — относительно стойки 0, 2 отно-
сительно 1 и 3 относительно 2 — будем определять дуаль-
ными углами 0Ь 02, :
где 0, (i = 1, 2, 3) — поворот звена i относительно звена
i — 1; 0? (i = 1, 2, 3) — смещение звена i относительно звена
Относительные угловые и линейные скорости вращения
в шарнирах 01э 02, 0з и г2» гз считаем заданными. Звено
3 имеет три степени свободы. В каждый момент
времени его движение можно описать винтом, имеющим
мгновенную ось е, угловую скорость вращения вокруг этой
оси Q и линейную скорость перемещения по этой оси V.
Можно принять V= h£l, где h — мгновенный шаг кинема-
тического винта. Это мгновенное состояние можно характе-
ризовать винтом в виде
Й=П(1
+ eh)e.
Задача состоит в определении винта V, т. е. положения
оси ё винта, значений угловой Q и линейной V скоростей
или шага h методом винтов.
Положение оси можно определить двумя дуальными
углами:
еФ°).
Для определения углов найдем проекции винта V и винтов
0Ь 02» 0з на оси какой-либо системы координат и при-
равняем их. Для этого удобно ввести две системы коор-
динат J и Н:~
система J (i2, к2 х i2, fc2);
система Н (i, к2 х i, fc2).
Выразим дуальную скорость звена 3 в системе J,
используя винтовые матрицы перехода, имеем
_ ЯУ _ ____.. ___
[И/ = [6г] Е«1]
(4.12)
где винтовые матрицы имеют вид, аналогичный матрицам
чистого поворота вокруг соответствующих осей:
144
I 1 о о
[aj = 0 сщ sdf (i = 1, 2);
L 0 — saf caf J
[C02 S02 0
— S02 C02 0
0 0 1_
Подставляя выражения матриц в уравнение (4.12), по-
лучим дуальные проекции винта V в системе J
sdi
sdi
cdi
s02
c02 - 03sa2
4-02 + 03ca3
(4.13)
Найдем проекции винта скорости V3 на оси системы Н,
которая повернута относительно системы J на дуальный
угол 8 вокруг оси х. С одной стороны, имеем
s8
с8
0
0
-
[У зЪ
С другой стороны, имея в виду выражение дуальной
скорости,
У= Q(1 + eh)k. (4.14)
Тогда проекции на ось системы Н можно записать так:
1 0
Перемножая матрицы в правых частях (4.12) и приравни-
вая соответствующие элементы матриц в выражении [У],
получим
01sd1s02fS 4- (0jsdiC02 "" б3$а2) s8 = 0; (4.15)
—(0sdiS02) s8 4- (SjsdjcG^ + 03$аг) c8 = — УсФ; (4.16)
^cdi 4- 02 + 03ca2 = УсФ. (4.17)
145
1
Из (4.15) получим
~ cas§2
tg 5 = --Z-----Z--7?—»
p3sa2 — socirtb
где Дз = бз/Oi - Из [1 + е (&з “ &1)Ъ Из = О3/О1 •
Из (4.16) и (4.17), исключая дуальный модуль скорости V,
находим
p3sa2c6 — saiC (02 + 6)
cdf + р2 + Ц3С&2
\
(4.18)
. где Д2 — 02/0! — ц2 [1 + е(й2 — /н)]; Р2 — ^2/0
Дуальный модуль скорости V можно получить, возводя
в квадрат и складывая (4.16) и (4.17):
V2 = ё? [Й1
2Й2 (cot! + H3ca2) +
2рз [са1са2 - sajsaicOJ + йз + !]•
(4.19)
Мгновенная винтовая ось, как показывают уравнения
(4.18) и (4.19), описывает семейство прямых, зависящих от
двух параметров ц2 и ц3, т. е. конгруэнцию прямых.
В заключение отметим, что метод винтов позволяет
эффективно и компактно описывать кинематические и
динамические свойства твердого тела и систем твердых
тел, моделирующие исполнительные механизмы роботов^
Раздел
Динамика
манипуляторов
Глава 5
Метод кинетостатики
в динамике манипуляторов
Кинетостатический анализ манипуляторов основан на прин-
ципе Даламбера и заключается в определении усилий при-
водов, необходимых для реализации заданного движения
объекта, и сил инерции в кинематических парах, возни-
кающих при выполнении некоторого заданного движения
манипулятора. Рассчитанные усилия приводов позволяют
обоснованно выбрать мощности приводов, а силы и мо-
менты сил инерции звеньев необходимы для последующих
расчетов механической системы на прочность и жесткость.
Метод кинетостатики является также одним из самых
эффективных при построении уравнений движения манипуля-
торов на ЭВМ.
Рассмотрим ряд вопросов, относящихся к кинетоста-
гическому анализу манипуляторов.
5.1. Силы инерции и моменты сил
инерции звеньев
Силы инерции каждого звена приведем к его центру масс
и заменим главным вектором сил инерции звена и глав-
ным моментом сил инерции звена относительно его центра
масс. *
Главный ректор сил инерции звена, как известно, равен
*
К" = — mjdci,
147
где da — ускорение центра масс звена i (i — 1, 2, , и); т,—
масса i-ro звена.
Моменты сил инерции звеньев относительно их центров
масс определим следующим образом. На основании теоремы
об изменении момента количества движения твердого тела
относительно центра масс имеем
(5.1)
где Ка — кинетический момент звена i относительно его
центра масс — точки Cf; Ма — момент внешних сил и реак-
ций связей относительно центра масс звена i.
С другой стороны, на основе принципа Даламбера для
этого же тела можно записать
М*а + Ма = 0.
Сравнивая последние две формулы, получим
Ма =
dkc
~dT'
Используя понятие относительной, или локальной,
изводной, последнее выражение можно представить в
(53)
про-
виде
(5.4)
где d'Kc/dt — локальная производная вектора кинетического
момента, она определяется как вектор, проекции которого
на подвижные оси, связанные с телом, равны производным
от проекций Кс на эти оси; Q — абсолютная угловая
скорость звена.
Проекции момента сил инерции звеньев на подвижные
оси, связанные с телом, имеют вид
(5.5)
Проекции вектора кинетического момента на главнее
центральные оси, связанные с телом,
(5.6)
148
1
где Jb J2, J3 — главные осевые моменты инерции тела;
Qb П2, П3 ~ проекции абсолютной угловой скорости тела
на связанные с ним оси.
Подставляя (5.6) в выражения (5.5) проекций момента сил
инерции, получим:
Л/сл = — [J1П1 4- (J3 — J2) H2Q3] ;
^Су ~ ~ [7гП2 + (J1 ~ Jз) ПгП3] ; (5.7)
Mcz — ~ [J3Q3 4- (J2 ~ A) HiQ2J .
Здесь £>ь Q2, Q3 — производные по времени от проекций
абсолютной угловой скорости звеньев на подвижные оси.
Используя выражение (5.3), можно получить формулу для
момента сил инерции звена следующим образом. Известно,
что кинетический момент твердого тела может быть
выражен в следующем виде [25]:
Кс — Ос®, (5-8)
где 0с — тензор инерции в центре масс; со — вектор угловой
скорости тела.
Производя дифференцирование (5.8) с учетом того, что
тензор определен в подвижной системе координат, получим
Мс = (® х 0с) ® — (0с х ®) со 4- 0С£. (5.9)
Здесь £ — вектор углового ускорения звена.
Тройным произведениям, входящим в выражение (5.9),
ставятся в соответствие произведения матриц. Произведению
(0С х со) со ставится в соответствие произведение матриц
Ососо, где со — кососимметрическая матрица угловой скорости:
—
о
®1
(5.10)
где (01, со2, со3 — проекции угловой скорости звена на оси,
связанные со звеном.
Перемножая матрицы в произведении Осло, получим, что
это произведение равно нулю. Следовательно, момент сил
инерции звена может быть найден по формуле
s= (со х бс)® 4- ёсе. (5.И)
Произведению (со х бс)® ставится в соответствие про-
изведение матриц G)6C®, где ёс — тензор инерции звена в
связанных с ним осях:
I
(5.12)
Для звена i манипулятора момент сил инерции может
быть представлен в виде
х 3С;) + 0С|Д = (d>f х бс.) (bi
1 \ 1
Qk^k *1” J ^ikQk
=1 / fc=1
(5.13)
где == $cfik 5 fii + (d), x 0c) d),..
w
*
<1
5.2. Уравнения движения манипулятора
Для получения дифференциальных уравнений воспользуемся
принципом Даламбера — Лагранжа. К действующим на
звенья механизма силам добавим главные векторы сил
инерции и главные моменты сил инерции звеньев, прило-
женные в их центрах масс, и сообщим системе независимые
150
возможные перемещения в
кинематических парах.
Формирование уравнений
движений сводится к скаляр-
ному перемножению коэф-
фициентов, входящих в выра-
жения моментов и сил инер-
ции звеньев и активных сил,
на векторы возможных пере-
мещений.
При совершении каждого
виртуального перемещения
5ф7, во вращательной или
поступательной паре j все i-e
звенья, для которых i > j, по-
лучают перемещения, а при-
ложенные к ним силы и мо-
менты совершают работу
(рис. 5.1).
Виртуальная работа сил
при виртуальном угловом
перемещении в j-м шарнире
выражается так:
1
М?) 8ф; + (Ff + F") (8ф, X fj,) + М,8фД = О,
(5.14)
где Зфу х г и — перемещение центра масс звена i при вирту-
альном угловом перемещении в шарнире у; 6ф, = 8фД-,
i=i^
fjt = £ 4 + Р ” радиус-вектор, определяющий положение
к = j
центра масс звена i относительно центра шарнира у;
/*’<» М? — соответственно внешние силы и моменты звена i.
Подставляя в (5.14) выражения для Ff и и сокращая
на 6<Pj, получим дифференциальные уравнения движения
механизма в шарнире у:
(5.15)
Индекс у в этом уравнении принимает значения, соот-
ветствующие номерам вращательных шарниров.
Для получения уравнений динамики, соответствующих
движению в поступательных парах, сообщим системе вир-
туальное перемещение последовательно в каждой поступа-
тельной паре и составим суммы виртуальных работ сил.
Виртуальная работа при у-м поступательном виртуальном
перемещении выражается так:
f (Ft + Ff) + ёМ? = 0, (5.16)
• __ •
где — виртуальное перемещение центра масс звена i при
поступательном перемещении в шарнире у.
Подставляя в (5.16) значения сил инерции и сокращая на
получим систему дифференциальных уравнений, опи-
сывающих движения в поступательных парах:
И £ и
Е Е = - Е "vM/ +
i=jfc=i i=j
(5.17)
1
Индекс j в этом уравнении принимает значения, соот-
ветствующие номерам поступательных пар.
Уравнения (5.15) и (5.17) образуют систему линейных
дифференциальных уравнений 2-го порядка со скалярными
коэффициентами. Коэффициенты этих уравнений зависят от
обобщенных координат и при заданных начальных условиях
могут быть найдены по этим формулам.
Поясним на примере использование формулы (5.7).
Главные моменты сил инерции звеньев относительно цент-
ров масс для манипулятора с двумя вращательными и одной
поступательной парами (рис. 5.2, а). Со звеньями манипуля-
тора свяжем системы координат xjiZi (i = 1, 2, 3), как пока-
зано на рис. 5.2, а.
Считаем, что главные центральные оси звеньев парал-
лельны соответствующим осям этих координатных систем.
За обобщенные координаты в данном случае приняты пере-
152
1
менные параметры фъ ф2, Фз, определяющие относительное
положение соседних звеньев.
Запишем выражения проекций абсолютных угловых ско-
ростей звеньев на оси связанных с ними систем координат:
QJX = 0; О2х = ф2; ^зх = <р2;
QJy = 0; Q2y = (pi sin <р2; Q3y = (pi sin ф2;
П12 = ф1; О22 = ф1СО5ф2; Q3z = (pi cos ф2.
Такие же проекции будут иметь векторы угловых ско-
ростей звеньев на главные центральные оси связанной с ними
системы координат.
Найдем производные по времени от этих проекций:
• • •
^lx Oj П2х = П3х ~ (р2 j
Q!y = 0; Q2y = О3у = (pt sin ф2 + Ф1ф2 cos ф2;
Qlz = (pi; Q2z = Q3z = (pi cos ф2 - (pi(p2 sin ф2.
Используя выражения проекций угловых скоростей
звеньев и их производных, составим выражения для проек-
ций моментов сил инерции звеньев относительно их центров
масс на связанные оси, получим (индекс с в уравнениях
моментов сил инерции звеньев для упрощения записи здесь
и в дальнейшем опущен):
для звена 1
М\ = М\ = 0;
М", = «ь
для звена 2
= J(i2)(p2 + (Т{32) - Л2)) (р2 sin ф2 cos ф2;
М1' = Л2) (Ф1 sin ф2 + Ф1(р2 cos ф2) + (Л2) - л2)) Ф1ф2 sin ф2,
где J(2, ~ моменты инерции звена i относительно
осей х, у, z.
5.3. Примеры кинетостатического
анализа манипуляторов
Покажем на примерах решение методом кинетостатики
задач определения реакций связей в кинематических парах
манипулятора при его заданном движении и составление
уравнений динамики.
Для определения реакций в кинематических парах при-
ложим к звеньям манипулятора наряду с действующими
153
1
активными силами силы инерции, приведя их к центрам
масс звеньев и заменив их главным вектором сил инер-
ции Fc и главным моментом сил инерции Мс.
Размыкая поочередно кинематическую цепь манипуля-
тора в каждой кинематической паре, составим уравнения
проекций всех активных сил и сил инерции звеньев сво-
бодной части на оси прямоугольной системы координат,
связанной с последним звеном кинематической цепи, остав-
шейся после отсоединения свободной части, а также уравне-
ния проекций моментов всех сил на эти оси координат.
Полученные уравнения дают возможность определить пять
неизвестных составляющих реакции вращательной кинема-
тической пары: три проекции силы и две проекции
момента на оси, перпендикулярные оси вращения, а также
момент привода вращательной пары.
В поступательной паре находим две проекции силы на
оси, перпендикулярные оси поступательной пары, три проек-
ции момента и силу привода поступательной пары.
Выражения моментов сил относительно осей вращатель-
ных пар и проекций сил на оси поступательных пар при
заданных усилиях приводов представляют собой дифферен-
циальные уравнения движения манипулятора.
Кинетостатика манипулятора с двумя поступательными
и одной вращательной кинематическими парами (рис. 5.3).
Такую схему имеют промыш-
ленные роботы «ПР-10»,
«Циклон-ЗБ» и др.
Вопрос об определении
линейных ускорений центров
хз масс для этого манипулятора
был рассмотрен ранее. Най-
дем силы и моменты сил
инерции звеньев.
Свяжем со звеньями ма-
нипулятора системы коорди-
нат, как это показано на
рис. 5.3. Центры масс звеньев
1 и 2 расположим на оси
вращательной пары Л, а
центр масс звена 3 — на оси
звена 3.
Запишем выражения для
сил и моментов сил инерции
звеньев. Все силы инерции
Рис. 5.3 звена приведем к двум векто-
154
1
рам, приложенным в центре масс: Ff
вектору и моменту сил инерции.
Для манипуляторов, работающих в
системе координат:
главные векторы сил инерции звеньев
F" =0; Fl = -rn2s2ki;
f'S = -m3 [(- Ф1$3 + s3)h + (Ф1$з + 2<p1s3)/1
главные моменты сил инерции звеньев
и М” — главным
цилиндрической
относительно их
центров масс
М^2 = МаСз = -Л ФЛ,
где JZi, Л, J — моменты инерции звеньев 7, 2, 3 отно-
сительно центральных осей звеньев 7, 2, 3, параллельных
оси вращения Zp
При наличии в схвате манипулятора объекта массой
т} и моментом инерции Jzr относительно центральной оси
объекта, параллельной оси zb главный вектор силы инерции
этого тела будет найден по формуле
-7Иг[(-фЬ4 + $з)*1
+ (ф1«4 + 29^3) Я
s2fci],
а главный вектор момента сил инерции
Y Сила F2 направлена вдоль оси z звена 2. Проекции сил
F3 и F” на подвижные оси равны соответственно:
m3<pls3 ~ тз$з и г^гФ154 ~ mi-s3 на ось х3 звена 3;
— m3(piS3 — 2т3ф133 и — mr<p1s4 — 2mr(pis3 на ось у3 звена 3;
— m3s2 и
— mTs2 на ось zt.
Определим реакции в кинематических парах, моменты
и силы приводов. Приложим к звеньям манипулятора
наряду с действующими активными силами главные век-
торы сил инерции и моментов сил инерции. Поочередно
размыкаем кинематическую цепь манипулятора в каждой
кинематической паре, начиная с последней, и составляем
уравнения всех сил и моментов сил, действующих на
свободную часть цепй. Тогда для звена 3 уравнение равно-
весия сил запишется в виде
RE ~ 0,
где G3 и GT — соответственно силы тяжести звена 3 и
объекта; RE — реакция пары Е.
и
155
1
Уравнение моментов сил звена 3 относительно точки Е
+ М? — Гз х S3 — Ff х S4 — G3 х S3 — Gr х s4 4- МЕ = О,
где з= — /2зф1&1; М)? = — Jz Ф1&1; МЕ — момент, воспри-
нимаемый поступательной парой Е.
Спроецируем векторные уравнения сил и моментов на
оси системы координат x3y3z3. Так как поступательная
кинематическая пара С накладывает пять ограничений на
относительное движение звеньев 2 и 3, образующих ее, то
необходимо иметь пять уравнений статики для определения
реакций в этой паре. Уравнение проекции сил на ось у3
4- 2(PiS3) - mr($1s4 4- 2<pis3) 4- REy = 0.
Уравнение проекций сил на ось z3:
— (т3 4- И1Г) s2 — G3 — Gr 4- REz — 0.
Здесь REy и REz — проекции реакции пары Е на оси у3
и z3.
Уравнение моментов относительно оси х3
Л^З + Мхг + МЕх = 0,
где и Мхг — моменты сил инерции масс т3 и тг отно-
сительно оси хз, равные нулю, следовательно, МЕх — 0;
A/jex — проекция момента пары Е на ось хз.
Уравнение моментов относительно оси у3
4- М 4“ т3 S2S3 4~ wir S2S4 4" G3S3 4” Grs4 4- MEy — 0.
Уравнение моментов сил относительно z3
~(Jz3 4- Jzr) (pi - т3 (ф^з 4- 2ф1$*з) s3 - тг (<p1s4 4-
4- 2(pis4) s4 4- ms - 0.
Для определения силы привода поступательной пары Е
спроецируем все силы на ось х3 звена 3. Тогда
-ж3 (s3 - <Р1»з) - (s3 - <pfs4) 4- Qe = 0,
где Qe — сила привода в поступательной паре Е. Это выра-
жение можно рассматривать как первое из трех уравнений
движения рассматриваемого манипулятора под действием
заданных сил.
Для кинематической цепи, состоящей из звеньев 2 и 3,
уравнения проекций сил и моментов сил имеют вид-
уравнение проекций сил на ось х2
-т3 (s3 - <p?s3) - nir (s3 - <p?s4) + RBx = 0;
156
уравнение проекций сил на ось у2
-т3 (ф! 4- 2(piS‘3) - (ф1«4 + 2<Р1«з) + &ву = 0;
уравнение моментов сил относительно оси х2
- [т (ф^з 4- 2ф!$з) + (Ф1«4 + 2ф!$3)] (а2 + Ь2) + МВх = 0;
уравнение моментов сил относительно оси у2
Оз (s3 - фЬз) 4- И1г (s4 - фfs4)] (а2 + Ь2) +
4- т3 s2s3 + m4s2s4 4- G3S3 4- Grs4 4- МВу = 0;
уравнение моментов сил относительно оси z2
(ф^з 4- 2<PiS3) s3 + тг (ф^4 + 2ф!$з)54 4- МВ2 = 0.
Для определения силы привода поступательной пары В
спроецируем все силы на ось z2, тогда
* А
~(т2 4- т3 4- Шг) s2 - G2 - G3 - Gr + QB = 0.
Это уравнение при заданном QB можно рассматри-
вать как второе дифференциальное уравнение движения
манипулятора. Здесь QB — сила привода поступательной
пары В.
Рассмотрим кинематическую цепь, состоящую из звень-
ев 1, 2, 3, разомкнув ее в шарнире А. Запишем уравнения
проекций сил и моментов сил на оси прямоугольной
системы координат:
уравнение проекций сил на ось
-w3 (S3 - Ф1*з) ~ т,(s3 - <PiS4) 4- RAx = 0;
уравнение проекций сил на ось у!
— И1з(ф1^з + 2<p1s3) — тг (cpis4 4- 2фх5з) + RAy = 0;
уравнение проекций сил на ось Zj
—(т2 4- т3 + Шу) s2 — Gi — G2 — G3 — Gr 4- RAz — 0;
уравнение моментов относительно оси хг
Оз (фх^з 4- 2ф! s3) 4- Шг GjSs4 4- гф^)] х
х (s2 4- «2 4- b2) 4- МАх 0;
уравнение- моментов относительно оси уг
- Оз ($3 - Ф1*з) + (s4 - <pfs4)] (s2 4- а2 4- b2)
G3s3 4- Grs4 4- МАу = 0.
Проекции моментов МАх и МАу также могут быть пред-
ставлены в виде моментов пары сил — реакций R'A и RA9
157
1
приложенных по концам образующих цилиндра вращатель-
ной пары.
Для определения момента Мг привода шарнира А
составим уравнение моментов относительно оси z шарнира
~(ЛХ + Л2 + Л3 + Лг) фх - "^3 (<Р1^з + 2<Р1$з)зз -
- шг (Ф1«4 + 2ф1$4) $4 + Afj = 0.
Это выражение есть третье дифференциальное уравне-
ние движения манипулятора.
Кинетостатика манипулятора с двумя вращательными и
одной поступательной кинематическими парами (см. рис. 5.2, а).
Найдем сначала выражения сил и моментов сил инерции
звеньев манипулятора, работающего в сферической системе
координат.
За обобщенные координаты примем относительные пе-
ремещения звеньев в кинематических парах А, В и С -
углы <pb ф2 и линейное перемещение s3 соответственно.
Считаем, что центры масс звеньев — точки Ci9 С2, С3 —
лежат на осях звеньев. Ускорение центра масс звена 1 —
точки — равно нулю. Абсолютное ускорение центров
масс звеньев 2 и 3 было найдено выше.
Запишем выражения для сил и моментов сил инерции
звеньев, приведенных к их центру масс.
Главные векторы сил инерции звеньев рассчитываются
как произведения массы звена на ускорение его центра
масс:
F£ = 0;
= — т2 [(2ф2ф2г2 sin ф2 — ф1г2со8ф2)11 -
- ф?г2 cos ф^ + ф2г2£2 - ф1г2/2];
F3 = -т3 [(2ф!ф2 sin ф2$3 - ф!83 cos ф2 - 2<plts3 cos ф2) -
- Ф1«3 cos фх/i + (ф2$3 + 2ф283) к2 - ф^з/2 + sj3],
где i, j, к — орты осей.
Главный вектор момента сил инерции удобно предста-
вить в виде проекций на оси координат, связанные с соот-
ветствующим звеном.
Запишем выражения проекций абсолютных угловых ско-
ростей звеньев на связанные с ними оси (см. рис. 5.2, б, в):
^1х —
^2х = Ф2 ’
Зх
= 0;
П2у = Ф1 sin Фг!
Г23> = cpt sin <р2;
— ч>!;
0^2^ == Ф2 9
П32 = <Pi cos ф2.
158
1
Найдем производные по времени от этих проекций:
= °; = °;
• •
Q2x = ф2; Q2y = <Pj sin ф2 + Ф1Ф2 cos ф2;
• •
s Ф2; = Ф1 sin Ф2 + Ф1Ф2 cos Ф2;
А
^12 — Ф1 5
^2г = Ф1 COS Ф2 “ Ф1Ф2 Sin Фг5
*
^Зг = Ф1 cos Ф2 - Ф1Ф2 sin Фг-
Используя выражения проекций угловых скоростей звень-
ев и их производные, составим выражения для проекций
моментов сил инерции относительно их центров масс на
связанные со звеньями оси (см. рис. 5.2, г, Э):
для звена 1
— 0; — «^21Ф1-
для звена 2
7У2)ф1 БШф2 cos ф2];
М*2 = - [Jy2 (ф sin ф2 + ф1(р2 cos ф2) + (JX2 - JZ2) фхф2 cos ф2];
М“2 = - [Л2 ($1 cos ф2 - ф!ф2 sin ф2) + (Jy2 - JX2) фХф2 sin ф2];
для звена 3
= - [Л3Ф2 + (Л3 - Jy3) Ф1 Sin ф2 cos ф2];
- 1Лз (Ф1 Sin Ф2 + фхф2 cos ф2) + (JX3 - Л3) Ф1Ф2 cos ф2];
Л^з = - [JZ3 (ф! COS ф2 - ф!ф2 sin ф2) + (Jy3 - JX3) ф!ф2 sin ф2].
Определим реакции в кинематических парах, моменты
и силы приводов. Разомкнем кинематическую цепь мани-
пулятора (см. рис. 5.2, а) в кинематической паре Е и соста-
вим уравнения проекций сил, действующих на звено 3, на
оси х3 и z3 системы координат x3y3z3:
^х3 ”i" ^xr В-Ёх ^3 ( 2ф1ф253
Ф1«з + 2ф1з3 cos ф2)
Шт (-2ф1ф254 ч- ф!$4 + 2ф1§4 cos ф2) + REx = 0;
^za + ^zr “ G3 — Gr-----w3 (ф2$з + 2ф2х3 —
— Ф1$3 sin Ф2 cos Ф2) - Шг ($2s4 + 2ф2$з) - G3 cos ф2 -
— GT cos ф2 — mr<pis4 sin ф2 cos ф2 + REz = 0,
и Rez — проекции реакций пары Е на оси х3
Составим уравнения моментов сил относительно
и z3.
осей
координат:
159
уравнение моментов сил относительно оси х3
+ Л^г - w3 (ф2»з + 2ф2»з) (S3 - S2) -
- (ф2«4 + 2ф254) (s4 - S2) - G3 (§3 - S2) COS ф2 +
Gr (S4 - S2) cos Ф2 4- MEx = 0;
уравнение моментов сил относительно оси у3
уравнение моментов сил относительно оси z3
М? + М”г 4- тз^с^ср^зБтфг - ф^созфг -
«э
2ф!§з cos ф2)(«з - S2) 4- Шт (2<p1<p2s4 sin ф2 -
- ф!54 COS ф2 - 2ф1$3 cos ф2) (s4 - s2) 4- MzE = 0,
где МЕх, МЕу, МЕ2 — проекции момента, воспринимаемого
кинематической парой Е, на оси x3y3z3.
Для определения силы привода поступательной пары Е
спроецируем все силы, действующие на звено 3, включая
силы инерции, на ось у3 звена 3. Получаем
— m3 (s3 — ф1$3 cos2 ф2 — фЬз) — тг (s4 — <p2s4 cos2 ф2 —
— ф2$4) — G3 sin ф2 — Gr sin ф2 4- Q3 = 0.
Это выражение при заданном усилии привода QE пред-
ставляет собой первое из трех дифференциальных уравне-
ний движения манипулятора.
Разомкнем кинематическую цепь в паре В. Составим
уравнения проекций сил, действующих на звенья 2 и 3:
уравнение проекций сил на ось х2
! 1 * х3 Т Л ХГ
Ф1$3 COS ф2 4- 2ф!$3 COS ф2)
$iS4 cos ф2 + 2<p1s3 cos ф2)
&Ех = ™з ( — 2ф1ф253 sin ф2 +
^г(-2ф1ф2з4 sin ф2
т2 (—2ф!ф2г2 sin ф2
сумма проекций сил, действующих на звенья 2 и 3, на
ось у2
REy + tn2 (ф2г2 cos2 ф2 4- ф2г2) + т3 (ф?s3 cos2 ф2
4- <p2s3 - s3) 4- тг (ф?54 cos2 ф2 + <pls4 - s3) 4-
(G2 4- G3 4- Gr) sin ф2 = 0;
160
1
уравнение проекций сил на ось z2
-ТП2 (ф2Г2 + Ф1Г2 S*n Ф2 COS ф2) ~ W3 ($2S3 + 2<p2s3) -
™г (ф2$4 4- Ф1 sin Ф2 COS ф284 + 2ф2$3) —
- (G2 + G3 + Gr) cos ф2 — m3<pis3 sin ф2 cos ф2 + RBz = 0.
Уравнение моментов сил относительно оси z2
М“2 + 4- Щ- + т2 (2ф1ф2г2 sin ф2 - ф^1 cos ф2) +
4- т3 (2фхф283 sin ф2 — фх$3 cos ф2 — 2<pxs3 cos ф2)$3 +
4- Шг (2ф!ф254 sin ф2 — $ts4 cos ф2 — 2ф1Х3 cos фг) s4 + MBz = 0.
Для определения момента привода вращательной кине-
матической пары В запишем уравнение моментов относи-
тельно оси х2 этой пары
(А2 + Л3 + Лг) Фг 4- (Л2 4- JZ3 4- Jzr Jy2
— Jy3 — Jуг) ф2 sin ф2 cos ф2 - m2 (ф2 — ф1 sin ф2 cos ф2) r2 —
- w3 (ф2г2 4- 2ф283 - ф1$3 sin ф2 cos ф2) s3 —
— nir (ф284 + 2ф283 — Ф1$4 sin ф2 cos ф2) s4 4- Мв = 0.
При заданном моменте привода Мв в шарнире В это
выражение представляет собой второе дифференциальное
уравнение движения манипулятора.
Рассмотрим теперь кинематическую цепь, состоящую
из звеньев 1, 2 и 3, нарушив связь в шарнире А.
Составим уравнения проекций сил и моментов относи-
тельно координатных осей xtfiZi.
Уравнение проекций сил на ось Xj будет таким же,
как и на ось х2, уравнения проекций на оси ух и zx будут
отличаться от уравнений проекций на оси у2 и z2 только
проекциями сил тяжести G3 и Gr, поэтому мы их не при-
водим.
Уравнение моментов сил относительно оси Xi
М\ + + М*г - т2 [ф|г2 cos Ф2 (11 + Г2 sin ф2) -
- Ф2^2 (Г2 + h sin ф2) - т3 [ф?53 cos Ф2 (/1 4- s3 sin ф2) -
— (ф2$з 4- 2(p2s3)(s3 + 11 sin Ф2) — s3li С08фг] —
*
Шг(ф154СО8ф2(?1 4- 548Шф2) - (ф254 + 2(p2S3)(s4 +
+ li sin ф2) - *s3li cos ф2] - (G2r2 + G3s3 + Grs4) cos ф2
Max — 0.
6 Механика промышл. роботов, кн. 1
1
Уравнение моментов сил относительно оси yi
+ М®3 + Миуг - т2 (2ф!ф2Г2 sin ф2 “ Ф1^2 cos ф2)(/1
+ r2 sin ф2) - т3 (2ф!ф2«3 sin ф2 - $^3 cos ф2 -
- 2ф|53 cos ф2) (Zt 4- s3 sin ф2) - Юг (2<pi(p2s4 sin ф2 -
— Ф1$4 cos ф2 — 2ф1$з cos ф2) (li + s4 зт*ф2) 4- МАу = 0.
Для определения момента привода вращательной кине-
матической пары А составим уравнение моментов относи-
тельно ОСИ Zi
Ми2} 4- 4- 4- M*ZT 4- т2 (Зс^фг sin ф2 - фг cos ф2) х
х r2cos ф2 4- т3(2ф|ф25з sin ф2 — Ф153 cos ф2 —
— 2ф1$3 cos ф2)з3 cos ф2 4- тг (2ф!ф284 sin ф2 —
- $^4 COS ф2 - 2ф!83 COS ф2) s4 COS ф2 4- MAz — 0.
Последнее выражение при заданном моменте привода
в шарнире А представляет собой третье дифференциаль-
ное уравнение движения манипулятора.
Кинетостатика манипулятора с одной поступательной и
двумя вращательными кинематическими парами. Составим
уравнения движения трехзвенного манипулятора с тремя
степенями свободы (рис. 5.4, а).
Этот манипулятор содержит одну поступательную пару
А и две врашательные В и С, оси которых параллельны.
Со звеньями манипулятора свяжем системы координат, как
показано на рис. 5.4, а. Ось zt направлена по оси звена 1
и совпадает с осью вращательной пары В, ось z2 совпа-
дает с осью z1? а ось х2 направлена по оси звена 2, ось
Рис. 5.4
1
z3 направлена по оси вращательной пары С, а ось хэ
направлена по оси звена 3. Звенья манипулятора будем
считать стержнями, а центры масс будем считать располо-
женными на осях стержней. За обобщенные координаты
примем переменные параметры sb ф2, Фз, определяющие
относительные положения звеньев. Движения звеньев 2 и 3
по оси z и в плоскости Оху являются независимыми,
поэтому их можно рассматривать отдельно.
Рассмотрим вначале ускорения центров масс звеньев 2
и 3. Схема составляющих векторов ускорений центров
масс С2 и С3 в горизонтальной плоскости показана на
рис. 5.4, б.
Можно записать
= 4с + а*с = - 92^2^2 + VirJz + «Л
где г2 — расстояние от центра пары В до центра масс
звена 2; «с2, Пс2 — соответственно нормальное и тангенци-
альное ускорения точки С2; аСз = ас + aCic — +
+ $с3с + ^с3с, где &с — соответственно нормальное и
тангенциальное ускорения точки С; 4с3с, <?с3с — нормальное
и тангенциальное ускорения точки С3 во вращательном
движении вокруг точки С.
В развернутом виде последнее равенство имеет вид
аСз = -qllzh + Ф2У2 - (Ф2 + Фз)2 r3i3 + (ф2 + Фз)га/з+ Sifc.
Здесь г3 — расстояние от центра пары С до центра масс
звена 3; i2, i3, /2, j3, к — соответственно орты осей х2, х3,
у2, Уз, гз5 /2 ~ длина звена 2.
Главные векторы сил инерции звеньев 2 и 3
/2 = -т2бс2; /3 = ~^з«с3-
Главные моменты сил инерции звеньев 2 и 3 относи-
тельно их центров масс
$2 = -Л2Ф2^2; = ~Л3(Ф2 + Фз)^3,
где JZ2, JZ3 — моменты инерции звеньев 2 и 3 относительно
их центров масс.
Для составления уравнений движения манипулятора под
действием усилий Qlf М2, М3 в кинематических парах при-
меним принцип Даламбера.
Для составления уравнения движения манипулятора по
оси z спроецируем на эту ось все внешние силы и силы
инерции звеньев, получим
(mi + т2 + т3) *st = Qi - Gi - G2 - G3i
6* 163
1
где Gi (i= 1, 2, 3) — силы тяжести звеньев; mt (i = 1, 2, 3) —
массы звеньев.
Для получения уравнения движения манипулятора вокруг
оси пары В составим сумму уравнений моментов сил и сил
инерции звеньев 2 и 3 вокруг этой оси, получим
Л2ф2 + Л3 ($2 + Фз) + пг2г1(р2 + W3 [(ф2 4-
4- Фз)2 г3I2 sin Фз + (Ф2 4- Фз) г3 (г3 4-l2 cos ф3)
+ sin фз + ф2?2 (!1 4- r3 cos фз)] = М2,
где 12 — длина звена 2.
Для получения третьего уравнения движения манипуля-
тора составим уравнения моментов всех сил и сил инерции
вокруг оси z3, получим
Ji (фг + Фз) + тз (фг +
Фз) d 4- ^Зф2^2^2 cos Фз
"1зф2^2''з sin Фз = М
3-
Полученные дифференциальные уравнения движения ма-
нипулятора позволяют найти его движение под действием
заданных сил или определить силы, необходимые для
осуществления заданного движения.
Кинетостатика манипулятора с тремя вращательными
кинематическими парами (рис. 5.5, а).
Такую структурную
схему имеют, например, механизмы транспортных движе-
Ъ Рис. 5.5
164
1
ний промышленных роботов «Колер», «ТУР-10», «ASEA»,
«Trallfa» и др. Оси вращательных пар А и В перпенди-
кулярны, оси вращательных пар В и С параллельны. Сим-
волически структуру манипулятора можно записать так:
В1В || В. За обобщенные координаты механизма примем
углы поворота звеньев фь <р2, ф3.
Определим для данного манипулятора главные моменты
и главные векторы сил инерции звеньев в центрах масс
звеньев.
Считаем, что центры масс звеньев располагаются на
осях звеньев. Со звеньями манипулятора свяжем подвиж-
ные системы координат следующим образом. Со звеном 1
свяжем систему координат Лх^?!, направив ось z2 по оси
вращательной пары А и ось х — по оси пары В. Со звеном
2 свяжем систему координат Bx2y2z2t направив ось х2 по
оси пары В, а ось у2 — по оси звена 2; со звеном 3 свя-
жем систему координат Cx3y3z3i направив ось х
шарнира С, а ось у — по оси звена 3.
по оси
Считаем также, что главные центральные оси звеньев
параллельны соответствующим осям xb yti zt (i— 1, 2, 3),
связанным со звеньями.
Как было показано выше, для определения моментов
сил инерции звеньев относительно их центров масс необ-
ходимо знать проекции абсолютных угловых скоростей
звеньев на связанные с ними оси. Эти проекции имеют вид:
Фз);
&2у — Ф18П1ф2;
П3у = ф18ш(ф24-ф3);
Q22 = (PiCos ф2;
О3г = ф! COS (ф2 + фз),
где QiXi &iy, Qis (i = 1, 2, 3) — проекции абсолютной угловой
скорости звеньев на связанные оси.
Производные по времени от проекций угловой скорости
звеньев:
^зх — (фг + Фз);
= 0;
Й2у = Ф1 sin ф2 4- Ф1ф2 cos ф2;
165
Q3r = (pi sin (<p2 + Фз) + Ф1 (Ф1 + Фз) cos (ф2 + Фз);
&2z = Ф1 cos Ф2 - Ф1Ф2 sin Ф2;
П32 = Ф1 cos (ф2 4- Фз) - Ф1 (ф2 + Фз) sin (ф2 + Фз).
Моменты сил инерции звеньев относительно их центров
масс в проекциях на связанные оси:
для звена 2
-М?2 = [Лфг + (Л2 - Jyj Ф? sin Фг COS ф2];
-М?2 = [Jy2 (Ф1 sin ф2 + ф,ф2 cos <p2) + (J„2 - JФ1Ф2 cos <p2];
-M?2 = [J,2 (ф1 cos ф2 - ф!ф2 sin ф2) + (J - JX1) ф!<р2 sin ф2];
для звена 3
4l
~ -^3 = [А3 (Ф2 + Фз) + (Л - J у) (pi sin (ф2 4- Фз) COS (ф2 +
+ Фз)];
- М^3 = [7Уз (Фi sin (ф2 4- Фз) 4- <Pi (ф2 4- фз) COS (ф2 4- ф3)) 4-
+ (Л3 “ Л3) Ф1 cos (ф2 4- Фз)];
-**1 = [Л3 (Ф1 cos (ф2 4- Фз) - Ф1 (ф2 4- фз) sin (ф2 4- Фз)) +
+ (Jy3 “ Л3) (Ф2 + Фз) Ф1 Sin (Ф2 4- Фз)].
Представленные формулы позволяют определить момен-
ты сил инерции звеньев манипулятора, приведенные к
центрам масс звеньев, при заданном движении по обоб-
щенным координатам.
Чтобы определить главные векторы сил инерции звень-
ев, необходимо найти абсолютные ускорения центров масс.
Найдем эти ускорения (рис. 5.5, б, в). Ускорение точки С2 —
центра масс звена 2 — найдем как ускорение точки в слож-
ном движении
где aeC2i агС2, акСг — соответственно переносное, относительное
и кориолисово ускорения точки С2.
Переносное ускорение аеСг представляет собой в данном
случае ускорение точки С2 звена 1 и состоит из нормаль-
ного и касательного ускорений
«с2 = «с2
+ й?2,
где асп. = Ф1Г2созф2; а^. — ф1Г2со8ф2 (г2 — расстояние
центра шарнира В до центра масс звена 2).
от
166
1
Относительное ускорение «с2 также состоит из нормаль-
ного и касательного ускорений
ПС2 - J С2 + Яс2>
где а’?2 = ф1г2; а”2 - ф2г2.
Кориолисово ускорение
dfc2 = 2с51 х vr = 2<p1k1 х ф2г2к2 = ^’гЗфгФгГг sin Фг,
где к19 ic2, i2 — орты осей zlt z2, х2; г2 — ВС.
Абсолютное ускорение центра масс звена 3 — точки D —
определим как сложное, приняв за переносное движение
вращение в шарнире А, а за относительное — вращение
в шарнире В. В этом случае можно записать
aD ~ 4" 4”
Переносное ускорение точки D — это есть
точки D во вращательном движении в шарнире
« D = «о 4- aetD =-ф? [l2 cos ф2 4- r3 cos (ф2 4- Фз)] А
ускорение
А:
- Ф1 Пг cos ф2 4- r3 cos (ф2 + ф3)] it.
Относительное ускорение точки D определяется при
учете вращений в шарнирах В и С. Оно равно
= “фгА/г “ ФгУг “ (фг "V Фз)2 гг/з “ (фг 4" Фз) *з-
Кориолисово ускорение точки D
= 2ф!к1 х [фг^г^г 4" (фг 4" Фз)^з^з] =
= 2 [ф!ф2 sin ф2/2 4- Ф1 (ф2 4- Фз) г3 sin (ф2
Зная все составляющие ускорения точки D и умножив
каждую из них на массу т3, можно получить составляющие
главного вектора сил инерции звена 3.
Для получения первого уравнения движения манипуля-
тора, показанного на рис. 5.5, о, составим уравнения момен-
тов всех сил и сил инерции, действующих на звенья
манипулятора, относительно оси шарнира А, получим
—т2Г2ф1 cos2 ф2 4- 2т2Ф1ф2^г sin Фг cos ф2 — ^з^гФ1 cos2 ф2 —
- 2ш3 [ф1<Мг sin ф2 4- Ф1 (ф2 4- ф3) r3 sin (ф2 4-
Фз)] Ui cos Ф2 4- r3 cos (ф2 4- Фз)] 4-^4-^ cos ф2
MV, sin ф2 4- Ml cos (ф2 4- Фз) 4- AC, sin (ф2 4- Фз) 4- МДВ1 = 0.
Для получения второго уравнения расчленим манипуля-
тор в шарнире В и составим уравнения моментов всех сил
167
и сил инерции, действующих на звенья 2 и 3, относительно
оси В: <.
Ш2ф1Г2 COS Ф2 sin Ф2 + т2(р2г2 + W3<P1^2 COS ф2 sin (р2 +
+ W3/2$2 + П*зФ1 [G COS ф2 + Г3 COS (ф2 4- Фз)] [G Sin ф2 +
+ r3 sin (ф2 + Фз)] + т3 (ф2 + Фз) r3 (r3 + l2 cos ф3) +
+ т3 (ф2 4- Фз) r3l2 sin ф2 - М\ - =
= Мдв2 - P2r2 cos ф2 - Р3 [l2 cos Ф2 + r3 cos (ф2 4- Фз)].
Для получения третьего уравнения составим уравнение
моментов сил и сил инерции, действующих на звено 3,
относительно оси пары С получим
-т3г3 (ф2 + фз) + М”3 + Мдв3 = 0.
В уравнениях движения Мдв { (i = 1, 2, 3) — моменты при-
водов соответственно в шарнирах Л, В, С.
5.4. Динамика манипулятора с учетом
кулоновского трения
в кинематических парах
В алгоритмах анализа динамики манипуляторов обычно
учитывалось лишь вязкое трение в шарнирах. Учет куло-
новского («сухого») трения представляет собой более слож-
ную задачу.
Шарниры пространственных механизмов манипуляторов
находятся, как правило, в условиях перекоса, когда линия
действия сил проходит вне кинематической пары. В этом
случае нормальные реакции в шарнирах и само кулонов-
ское трение могут достигать больших значений.
Силы кулоновского трения определяются нелинейной
зависимостью от скорости, моменты сил кулоновского
трения в кинематической паре выражаются нелинейно через
внешние силы и силы инерции. Это приводит к тому, что
уравнение динамики манипулятора с учетом сил кулонов-
ского трения нелинейны относительно обобщенных ускоре-
ний. Это затрудняет построение эффективных алгоритмов
моделирования динамики манипуляторов в этом случае.
В настоящей работе для решения задач динамики
манипуляторов с учетом кулоновского трения предложен
подход, основанный на линеаризации выражений момента
трения на основе формулы Понселе.
Следует отметить, что данный подход был использован
О. Н. Левитской при анализе динамики дифференциалов
с учетом кулоновского трения. Данный подход позволяет
168
1
получить кусочно-линейную систему относительно обобщен-
ных ускорений.
Рассмотрим предварительно некоторые соотношения
между силами трения и нормальными реакциями враща-
тельного шарнира при общем случае нагружения.
Нормальные силы и силы трения во вращательных парах.
Рассмотрим два случая приведения нормальных сил и сил
трения во вращательной паре. Один из этих случаев назовем
случаем «наличия перекоса», а второй — «отсутствия пере-
коса».
При отсутствии перекоса возникающие на боковой по-
верхности элемента кинематической пары силы могут быть
приведены к одной силе, нормальной боковой цилиндри-
ческой поверхности. Этот случай соответствует прохожде-
нию линии действия силы между опорными элементами
пары.
Во втором случае возникающие на боковой поверхности
реакции могут быть приведены к двум скрещивающимся
силам.
Считаем, что нормальные силы на торце пары приво-
дятся в обоих
случаях к одной осевой силе, направленной
вдоль оси элемента.
Получим векторные формулы для сил трения. Введем
системы координат xkykzk, направив ось zk по оси пары
звена к, а ось хк — по направлению перпендикуляра, про-
веденного из центра пары на ось пары звена к + 1. Тогда
имеет место векторное соотношение
(со х ZV),
(5.18)
где | со | — модуль вектора со; f — коэффициент трения.
Компоненты силы трения по осям х и у получим сле-
дующим образом:
(cofc х Nxi)
[w X (Nx
со (N j - Nyi).
(5.19)
Следовательно, компоненты силы трения равны
coNyi; FyTp
coNJ.
(5.20)
Здесь со — проекция вектора й на ось z; Nx, Ny — проекции
нормальной реакции N на оси х и у.
169
Определим боковые нормальные реакции, возникающие
во вращательной паре. Рассмотрим сначала случай отсут-
ствия перекоса. Уравнения равновесия элемента пары в про-
екциях на оси имеют вид
Rz + Ns - 0; Nx + Rx + FXJP = 0; Ny + Ry + F>p = 0, (5.21)
где Rx, Ryf Ri — проекции главного вектора внешних сил
и сил инерции на оси х, у, z.
Подставляя выражения сил трения из (5.20) в (5.21),
получим
Nx + Rx - ^~Ny = 0; Ny + Ry + ^-Nx = 0. (5.22)
Из (5.21) и (5.22) получим
(5.23)
где n = Jcd/I co |.
Пренебрегая в (5.23) величиной f2, получим
Nz — Rx — nRv; Nv ~ — Rv 4- nRx; N„ — R,.
X Л У 7 У у Л f
(5.24)
Полученные формулы дают выражения проекций нор-
мальной реакции через проекции главного вектора внешних
сил при наличии трения.
Рассмотрим теперь случай перекоса, когда боковые нор-
мальные реакции приводятся к двум нормальным силам
и N2' Найдем проекции этих сил на оси х, у при нали-
чии трения.
Уравнения равновесия элемента пары в проекциях на
оси имеют вид:
Rx + Nix + N2x - nNiy - nN2y = 0;
Ry + Niy + N2y + nNlx + nN2x — 0;
~~ 2y ix F м 2x 0,
+ у N lx - у N2x - n Nly + n N2y = 0,
(5.25)
где L — длина элемента вращательной пары; Мх, Му — про-
екции главного момента внешних сил и сил инерции,
действующих на звено, на оси х, у.
Из последних уравнений найдем проекции нормальных
сил Ni и N2 на оси х, у:
170
1
(5.26)
Полученные выражения для нормальных
реакций во вра-
щательной паре позволяют найти силы трения и моменты
сил трения относительно оси вращения.
Определим моменты сил трения во вращательной паре.
Момент трения во вращательной паре может быть опре-
делен по следующей формуле:
sgn о • £
(5.27)
Мтр = —fN —
круга трения в паре.
Нормальные силы
N = ]/N2x + N2.,
(5.28)
возникающие в парах, зависят от внешних сил и сил инер-
ции, действующих на звенья, т. е. являются функциями
обобщенных ускорений механизма; Nx и Ny выражаются
формулами (5.22) и (5.26) через главные векторы и моменты
внешних сил и сил инерции. Поэтому использование выра-
жений (5.22), (5.26), (5.28) при исследовании динамики мани-
пулятора приводит к системе нелинейных дифференциаль-
ных уравнений относительно обобщенных ускорений.
Для получения приближенного линейного выражения.
момента трения от внешних сил, а следовательно, и от
обобщенных ускорений воспользуемся формулой Понселе,
в соответствии с которой нормальную реакцию можно
представить в виде:
при Nx > Ny
N = ~ 0,960Nx + 0,398Ny = KXNX + K2Nf; (5.29)
171
при Nx < Ny
N = ]/w, + Ny~ 0,3987V, + 0.960N,, =
K2N, + KiNr
Формула Понселе получена из условий наилучшего рав-
номерного приближения и дает максимальную погрешность
не более 4%, что значительно меньше погрешности опре-
деления коэффициента трения и радиуса круга трения.
Дифференциальные уравнения движения манипулятора с
учетом кулоновского трения. Выше было показано, что
главные вектор и момент сил инерции звеньев манипуля-
тора в центре их масс могут быть представлены в виде
(5.30)
где du, Cip — векторные коэффициенты, не зависящие
от обобщенных ускорений системы.
Рассмотрим пространственный механизм манипулятора
с вращательными парами, имеющий незамкнутую кинема-
тическую цепь (см. рис. 5.1).
Со звеньями механизма, как и раньше, свяжем подвиж-
ные системы координат x^z^ направив оси zt по осям
вращательных пар, а оси xt — по направлениям кратчайших
расстояний между ними.
Расчленим кинематическую цепь в s-м шарнире и соста-
вим уравнения моментов всех сил, приложенных к отсоеди-
ненной части цепи, относительно оси вращения пары с уче-
том сил инерции звеньев и сил трения в паре, получим
ИЛИ
(5.31)
Мпр
= 0,
где п — число звеньев манипулятора; aJj и — главные
моменты сил инерции и внешних сил звена к, приведенные
к его центру масс; F%, Ff, — главные векторы сил инерции
и внешних сил звена к, приведенные к его центру масс.
Преобразуем выражение для суммы внешних сил и сил
инерции отсоединенной в s-й паре части цепи:
172
^nsQi
(532)
где ЛП5 —
Преобразуем выражение для моментов внешних сил и
сил инерции отсоединенной в s-й паре части цепи, приве-
денных к центру пары:
ft- £(Л% + Л4И) + f U + Y + =
k —s k —s \ r = s /
и k / и— 1 \ л
где C„s = ~ cki 4" I + £ h ) X ®ki9 ” X Mk ~ ^k
k—si~l \ s=l / k~s
x (fk - bk).
Проекции векторов Rs и Ms на соответствующие оси
получаются путем умножения их на орты этих осей.
Используя выражения (5.32) и (5.33), по формулам (5.24)
и (5.26) найдем проекции нормальных реакций на оси х, у,
а затем, используя формулы (5.27) и (5.29),— выражение для
момента трения
паре.
Выражение для момента сил трения в паре j при от-
сутствии перекоса имеет вид
А/тр
sgn со (aNx 4- bNy),
(5.34)
где
а = къ Ь — к2 при Nx > Ny;
ja = к2, b = к} при Nx < Nr
Подставляя выражения для Nx и Ny из (5.24) и пренеб-
регая членами, содержащими квадрат коэффициента трения,
173
получим
sgn со (— aRx
(5.35)
где
(5.36)
Подставляя (5.36) в (5.35) с учетом (5.32), получим
(537)
где
(5.38)
При перекосе в кинематической паре выражение для
момента трения принимает вид
fd / хт ь хт XT u XT 4
MTp = - — sgn co (atNlx 4- Ь^1у + a2N2x + b2Ar2y),
где
«1 = ki9
— ^2»
al — &2,
C?2 ~ ^2>
при Nlx>Nly;
при < Nly;
при N2x > W2y;
при N2x < N2y.
(5.39)
Подставляя в (5.39) выражения Nlx, Niy, N2x, N2y из (5.26)
и пренебрегая членами, содержащими квадрат коэффициента
трения, получим
где Rx = Rs • is; Мх =MS • is; Ry = As • Js; My = Ms -js.
Уравнение движения манипулятора можно теперь пред-
ставить в виде
174
(Ml + Mns ks) qt + (№„ + N„ • £s) + Mnp = 0,
(5.40)
где s — 1, ..., n; i = 1, ...» n;
при наличии перекоса в шарнире s:
NT = DT- N = D
при отсутствии перекоса в шарнире s:
Система (5.40) является кусочно-линейной относительно
обобщенных ускорений манипулятора и может быть про-
интегрирована на ЭВМ.
5.5. Динамическая модель манипулятора
с упругой связью в схвате
Рассмотрим систему схват—деталь промышленного робота
при наличии упругой связи между ними (рис. 5.6) [3].
Введем следующие системы координат: система Оо*оУого
неподвижна и связана с отверстием; системы O„x„ynzn и
Ои+1хи+1уи+1ги+1 подвижные и связаны соответственно
с геометрическим центром схвата и центром масс детали.
Примем, ’ что в положении равновесия геометрические
центры схвата и детали совпадают и дефомации упругих
элементов равны нулю.
Примем, что линейные и угловые жесткости закрепления
детали в схвате соответственно равны сх = су — cz = с;, с^х —
= с9у — = Сф, сила упругости Fj — ct Лхh где сл — жесткость
i-й упругой связи; Ах-j — деформация упругого элемента
соответствующей координаты,
которая равна смещению х1И-й
оси относительно х,(п+1)-й, т.е.
= Xj(„+1) — xin. Считаем, что
угловые отклонения схвата и
детали от осей неподвижной
системы координат О0х0у0г0
малы, тогда за обобщенные
координаты можно принять
углы поворота этих тел
относительно осей системы
O0x0yQz0.
Составим уравнения дви-
жения схвата манипулятора от-
носительно неподвижной систе-
мы координат. Используя
175
уравнения Эйлера для абсолютно твердого тела, закреплен-
ного в одной точке, и теорему об изменении кинетического
момента твердого тела, запишем для схвата:
*^улфуи RxrJn 4" £ф(фу(и+1) Фул) 4" ^nyt
*^zn<Pzn ^ф(фг(л+1) Фал) 4” Mnz‘
(5.41)
На основе теоремы о движении центра масс запишем:
(5.42)
где фхи, фу„, ф2И — обобщенные координаты, определяющие
угловое положение-.схвата относительно неподвижной сис-
темы координат; фх(и+1), ФУ(И+1), ф2(л+1) “ обобщенные ко-
ординаты, определяющие угловое положение переносимого
объекта относительно неподвижной системы координат;
хл»
2Й — моменты инерции схвата относительно осей
неподвижной системы координат; Rxn, Ryn, Rzn, Mxni Mynt
Mzn — силы и моменты в шарнире п, соединяющем схват
с предыдущим звеном; тп — масса схвата; Ft — сила упру-
гости i-й связи.
Запишем уравнения движения детали относительно не-
подвижной системы координат:
К (л + 1)Фх (Л +1) = ^ф (фх (л +1) Фхл) ,
(л+ 1)фу (л + 1) = £ф(Фу(л+1) Фул)» (5.43)
*^а (л + 1)Фг (н+ 1) = — ^'ф(фа(и+1) ““ Фая)»
^л + 1Ал +1
^л + 1 Ул + 1
^л + 1 ?п + 1
(5.44)
Полученная система дифференциальных уравнений
(5.41)—(5.44) представляет собой динамическую модель упру-
госвязанной системы манипулятор—деталь.
Остановимся на выводе уравнений движения манипуля-
тора, структурная схема которого изображена на рис. 5.7, а,
для чего используем принцип Даламбера—Лагранжа. Для
получения дифференциальных уравнений движения к дей-
ствующим на звенья манипулятора силам добавим главные
векторы сил инерции и главные моменты сил инерции
звеньев, приложенные в их центрах масс, и сообщим систе-
176
ме независимые возможные перемещения. Разорвем пооче-
редно кинематическую цепь манипулятора и составим урав-
нения моментов относительно осей шарниров и уравнения
проекций сил на оси поступательных пар.
Для звена i манипулятора момент сил инерции может
быть представлен в виде
= (со, х Cty + OqE,- = (cbj x 0Cj) со,- 4-
Ci \ i > >
E <hfik + £ Mikqk 4- Mi, (5.45)
=1 / k=l
где Mik = 6aak;
(5-46)
— тензор инерции звена в связанных с ним осях; со{ —
вектор угловой скорости звена i; = bt (qh qt) — векторный
коэффициент, не зависящий от обобщенных ускорений.
При совершении каждого виртуального перемещения 8$j,
8Sj во вращательной или поступательной паре все i-e звенья,
для которых i > j, получают перемещения, а приложенные
к ним силы и моменты совершают работу. Виртуальная
работа при виртуальном угловом перемещении 8ф,- в j-м
шарнире выражается так:
£ [Й? 8<р, + (Pi + Л") (8ф, X fji) + HijSvj
i—j
+ (8<Pj x fj„) + 5<Pj] = 0.
(5.47)
Виртуальная работа при j-м поступательном виртуаль-
ном перемещении 8sf выражается так:
£ [(Pi 4- F?) 4- Qj 8$> 4- Rn 8^ = 0.
(5.48)
Здесь 8(pj х fji — перемещение центра масс звена при вир-
туальном угловом перемещении в шарнире j; 5ф; =
Cj — направляющий вектор оси шарнира j; rfj- — радиус-век-
тор, определяющий положение центра звена i относительно
центра шарнира j. _ г
Подставляя в (5.47) и (5.48) выражения для Ff и
и сокращая на Зф^ и 8sh получим дифференциальные урав-
нения движения манипулятора:
для вращательных пар
4- Mj + R„ (ej х fjn) + Мпё^ = 0, (5.49)
где индекс j принимает значения, соответствующие номе-
рам вращательных пар;
для поступательных пар
k
^i^ik^jQk 4* 4" Qj “I" Rn^j = Q
(5.50)
где индекс j принимает значения, соответствующие номе-
рам поступательных пар.
Представленная в данном разделе математическая мо-
дель и-звенного манипулятора учитывает наличие упругой
связи в схвате и массу упругозакрепленной детали (рис. 5.7, б).
Учет этих факторов в общем случае приводит к появлению
шести дополнительных дифференциальных уравнений 2-го
порядка, описывающих движение упругозакрепленной де-
тали.
5.6. Динамическая модель манипулятора
с контактным взаимодействием
Манипулятор, реализующий силовое воздействие на деталь,
можно представить как систему с наложенной дополни-
тельной связью. Используя принцип Даламбера, можно
построить динамическую модель такого манипулятора, со-
держащую реакцию связи в явном виде.
Выведем уравнения движения и-звенного манипулятора
(плоский случай) (рис. 5.8), используя принцип Даламбера.
Запишем выражения для главного вектора сил инерции
и главного момента сил инерции к-го звена:
- mA*;
(5.51)
178
1
где тк — масса /с-го звена; аск — ускорение центра масс
к-го звена; ёк — тензор инерции звена к; ък — абсолютное
угловое ускорение; — абсолютная угловая скорость.
Выражения для скорости и ускорения центра масс
к-го звена запишем в виде:
п
(5.52)
п п и
Запишем выражения для абсолютной угловой скорости
и абсолютного углового ускорения в виде:
(5.53)
где со, — относительная угловая скорость к-го звена; ef,
es — единичные векторы осей кинематических пар.
Для плоской системы выражения для главного момента
сил инерции Мк и абсолютного углового ускорения £к
к-го звена имеют вид:
— ~~^Ck^ki —
(5.54)
где JCk ~~ момент инерции к-го звена; — относительное
ускорение к-го звена относительно (к — 1)-го звена.
179
Учитывая выражения (5.51)—(5.54), запишем уравнения
равновесия системы на основе принципа Даламбера:
Nnev + Qv = 0;
(5.55)
(Ml + М«)
N (г»п х и)
Ср = 0,
где 6v — обобщенная сила привода в v-й паре; v — номера
поступательных пар; — единичный вектор; — обобщен-
ный момент привода в ц-й паре; ц — номера вращательных
пар; п — единичный вектор нормали к поверхности.
Для плоской системы уравнения (5.55) примут вид:
Ли п п п
п
Fekev - Nnev = Qv;
k = M к —P i=1 к = M
(5.56)
k = p i- 1
n n n
+ N (гри x и) = Sp
d2rck
Уравнения (5.56) описывают движение плоского манипу-
лятора с контактным взаимодействием. В состав аналити-
ческих выражений этих уравнений входит в явном виде
сила N. Такая математическая модель необходима при
синтезе алгоритмов управления промышленными роботами,
выполняющими операции с силовым воздействием: она дает
возможность учесть ограничения на нормальные силы.
Примеры построения динамических моделей промышлен-
ных роботов с упругой связью в схвате. Построение и
обоснование динамических и расчетных моделей конкретных
180
Рис. 5.9
роботов является важной частью задачи аналитического
исследования их характеристик и функциональных возмож-
ностей. Этой задаче посвящены многие работы отечествен-
ных и зарубежных исследователей. Однако желательно
иметь обобщенный подход к построению динамических
моделей, учитывающий возможность использования различ-
ных кинематических схем роботов. Решим задачу построе-
ния динамической модели робота на примере трехзвенного
манипулятора шарнирного типа с учетом упругозакреплен-
ной детали (рис. 5.9, а). Такую кинематическую схему имеют
промышленные роботы РПМ-25, ПУМА, ТУР-10 и др.
Прямое использование уравнений Лагранжа II рода при-
водит к большой громоздкости при выводе уравнений дви-
жения, связанной с дифференцированием выражения кине-
тической энергии. Так как
для данных манипулято-
ров общим является нали-
чие у них схвата и упруго-
закрепленной в нем де-
тали, целесообразно со-
ставить отдельно урав-
нения движения схвата
с деталью и отдельно
оставшейся части кинема-
тической цепи.
Составим уравнение
двухзвенника ABD (рис.
5.10,6) с двумя степенями
свободы и с приложен-
ными к нему управ-
Рис. 5.10
181
1
ляющими моментами Ml9 М2, —Af3 и силами (—FDx),
в шарнире D, где М19 М2, М3 — моменты приводов
в шарнирах А, В, D; Fdx, FDy — компоненты силы Гр,
действующей на схват. Используя метод Лагранжа, можно
записать уравнения двухзвенника ABD'.
Ф1 (Л + 4m2li) + 2<p2m2lil2 cos (<р2 - фх) -
- 2<р2 (фг “ Ф1) *и2М2 sin (ф2 - ФО -
- 2<p1<p2m2Z1Z2 sin (ф2 - фх) = ; (5.57)
Ф2 (Л + w2/i) + 2<Р1Тп21112 cos (ф2 - фх) - 2ф! (<р2 -
- фО m2lil2 sin (ф2 - фх) + 2ф1<р2/1/2т2 sin (ф2 - ф1) = Q2,
;е 61, (З2 ” обобщенные силы; Jx, J2 — соответственно
моменты инерции звеньев АВ и BD относительно их цент-
ров масс; 2/х, 212 ~ соответственно длины звеньев АВ и BD;
т2 — масса звена BD; фь ф2 — углы поворота звеньев в не-
подвижной системе координат Оху.
Обобщенные силы Qi и Q2 в этом случае
61 = Af 1 — Afз — 2/j,Fpx sin ф1 + 2liF[)y cos фх;
Q2 = Af 2 - Af з - 2l2FDx sin ф2 + 2liFDy cos ф2.
(5.58)
Уравнения движения схвата 3 с упругозакрепленной
в нем деталью 4 запишем в неподвижной системе коор-
динат Оху, приняв за обобщенные координаты соответ-
ственно абсолютные координаты центров масс схвата и
детали хс3, ус3 и хс4, ус4 и абсолютные углы поворота этих
тел ф3 и ф4 (рис. 5.9, в). Уравнения движения схвата и де-
тали в неподвижной системе координат Оху запишем на
основании теоремы о движении центра масс:
w3xc3 = Fx + FDx, m3yc3 = Fу + FDy — Рм;
ЛзФз = М - FDyl3 sin ф3 - FDxl3 cos ф3 + сф (ф4 - ф3); (5.59)
m4xc4 = — Fx; m4yc4 = -Fy - Рд; Л4ф = -с4(ф4 - ф3),
где Fx, Fy — проекции силы упругости в неподвижной систе-
ме координат; т3 — масса схвата; Рм — вес манипулятора;
момент инерции схвата относительно горизонтальной
оси вращения; 13 — длина звена 3; т4 — масса детали; Рд —
вес детали; Jc4 — момент инерции детали; сф — коэффициент
жесткости механизма захвата детали.
Считаем, что система координат С3х3у3 связана со охва-
том, ее начало находится в центре масс схвата; система
С4х4у4 связана с деталью, начало ее находится в центре
масс детали 4 (рис. 5.10, в). Примем, что в положении
182
1
равновесия системы координат С3х3у3 и С4х4у4 совпадают.
11 роекции силы упругости, действующей на деталь, наиболее
просто выражаются в системе С3х3у3:
(5.60)
। де сх, су — коэффициенты жесткости закрепления детали
в системе схвата; х£\ y<4 — координаты центра масс дета-
ли в системе координат С3х3у3.
Найдем проекции Fx, Fy сил упругости на неподвижные
оси Оху:
cos ф3
.sin ф3
- Сх*(с4 cos ф3 + суу(с1} sin ф3
- схх^ sin фз - Суу^ cos фз
(5.61)
Заменим в выражении (5.61) координаты центра масс
детали хс(4\ yjp через координаты центра масс схвата и
детали в неподвижной системе координат:
х<1* = хс4 cos Фз + ус4 sin Фз; у$ = -хс4 sin ф3 + ус4 cos ф3,
(5.62)
где хс4 = хс4 - хс3; ус4 = ус4 - ус3.
Подставляя (5.62) в (5.61), найдем
Fx = ~хс4 (сх COS2 Фз + Су sin2 фз) - ус4 (сх sin ф3 cos ф3 -
— Су cos фз sin фз); (5.63)
Fy = -Ус4 (сх sin2 Фз + Су cos2 Фз) - хс4 (сх sin ф3 cos ф3 -
— Су cos фз sin фз).
Ранее было принято, что жесткости закрепления детали
в схвате по осям х и у равны, т. е. сх = с у = с\ с учетом
этого выражения (5.63) примут вид
Fx = - схс4 - —с (хс4 - хс3); Fу - - сус4 = - с {ус4 — ус3)-
(5.64)
Таким образом, динамическая модель системы схват —
деталь может быть описана системой уравнений (5.59), (5.64).
Систему дифференциальных уравнений (5.59) 2-го порядка
можно заменить системой уравнений 4-го порядка, которая
удобна для определения управляющих воздействий. Про-
дифференцируем три последних уравнения системы (5.59)
дважды, получим
т4х$Р = -с(хс4 - хс3); т4у№ = -с(ус4 - ус3);
Л4ф(1У) = “ сФ (ф4 - Фз). (5.65)
183
Решая совместно (5.59) и (5.64), получим:
m4xffl - -схс4 4- с [с (хс4 - хс3) 4- FDx~\/m3,
пг4$р = - сУс4. + с [с (ус4 - ус3) 4- FPy]/m3; (5.66)
Jc4<p(IV) = -сфф4 + сф [Мз + Сф(ф4 - фз) - FDyl3 sin фз -
~ FDxh cos Фз Wз-
В уравнении (5.66) входят активные силы FDx, FDy, ко-
торые определим из выражений (5.58) для обобщенных сил
61 и 62:
[Т^/гСозфг — L2^icos9i — (А/х — М3)/2со8ф2
(М2 - М3)/1со8ф1]/[2/1/28т(ф2 - Ф1)];
(5.67)
FDy = [Мг sin ф2 - L2li sin фх - (Mr - М3) l2 sin ф2
4- (М2 - M3)h 8Шф1]/[2/1/28т(ф2 - фхд,
где Li и Ь2 представляет собой левую часть уравнений (5.57).
Таким образом, математическая модель манипулятора
с упругозакреплеиной деталью описывается системой урав-
нений (5.57), (5.58), (5.66), (5.67).
Построим динамическую модель сборочного промыш-
ленного робота, несущего упругоподвешенную деталь
(рис. 5.10, а). Для описания динамических свойств этого
манипулятора воспользуемся матричной формой представ-
ления уравнений движения, которая является удобной при
решении задач управления движения. В этой форме уравне-
ние движения манипулятора имеет вид
А (</) q + В (q, q) = Q или q = А 1 (q) Q - A 1 (q) В (q, q), (5.68)
где
I 4- J2 + 4m2lj + m2l2+4m2l1l2 cos Фг
A(q)= I 72 + т2/2 + 2т2/1/2со8ф2
L о
J2 4" m2l2 4" 2m2/xl2 cos ф2
m2l2 4* J 2
0
0
4" m2
1И1
Q — [фь Фг» 2з]т — вектор обобщенных координат; q = [фх,
Фг, ^з]т ~ вектор обобщенных ускорений; Q — [М15 М2,2з]Т —
184
матрица обобщенных сил в шарнирах В, D, С;
[-4т2/ik sin Ф2Ф1Ф2 - 2m2lil2 sin ф2ф2
2m2lil2 sin Ф2Ф1
0
Кинематические соотношения механизма запишем в виде
(рис. 5.10, б)
*з = J (<?) q; *з = J(q)q + D (q, q\ (5.69)
где х3 = [х3, у3, z3]T — вектор скорости звена 3; д =
= [<рх, ф2, 23]т — вектор обобщенных скоростей; х3 =
= [*з, Уз, г3]т — вектор ускорений звена 3;
[—2/х sin фх — 2/2 sin (фх+ ф2) — 2Z2 sin (фх+ ф2) 0
2/х cos фх + 2/2 cos (фх + ф2) +2/2 со8(фх+ф2) 0 ;
0 0 1_
1-2/х cos фхфх - 2/2со8(ф1 + ф2)(ф1 + ф2)2
-2/1 sin фхф? - 2/2 sin (ф! + ф2)(Ф1 + Ф2)2
0
Уравнения движения детали
тх4 — с (х3
— х4),
(5.70)
где с — жесткость упругой связи; т — масса детали; х3 —
— [*з, Уз, *з]т — матрица координат схвата; х4 = [х4, у4,
z4]T — матрица координат детали.
Дифференцируя уравнения (5.70), получим
(5.71)
Решая совместно (5.69), (5.7Q) и (5.71), получим
M1X4V — — с [ JA ~1 (2 — В) + D(q, q) — х4].
(5.72)
Уравнение (5.72) описывает динамику движения двух-
массной системы манипулятор — деталь. Из (5.72) найдем
выражение для обобщенных сил в приводах робота, запи-
санные в матричной форме:
Q = -тАс~XJ~ хх]у + В (q, q) - AJ~1 [В (q, q) - х4]. (5.73)
Уравнение (5.72) представляет собой динамическую мо-
дель сборочного промышленного робота М20К с упруго-
закрепленной в схвате деталью, используя которую можно
провести синтез алгоритмов управления роботом на раз-
185
личных этапах сборочного процесса. Формирование управ-
ляющих воздействий на приводы робота должно осуществ-
ляться с учетом выражения (5.73).
5.7. Динамическая модель сборочного
промышленного робота
Рассмотрим задачу сборки гладких цилиндрических соеди-
нений (штифт и отверстие), которые весьма распространены
в машиностроении. Для решения задачи сборки построим
динамические модели сборочного промышленного робота
в. различных фазах сопряжения деталей (плоская задача)
с учетом упругого элемента в схвате.
Запишем уравнения динамики манипулятора, работающе-
го в прямоугольной системе координат, при транспорти-
ровке детали на сборочную позицию (рис. 5.11):
Мх2 = Глупр - сх(х2 - xt)cos^;
Му2 = Лупр - су (у2 - yi) sin р - Рм; (5.74)
ЛиР = FРупр Ср (Р &)>
где х2, у г — координаты геометрического центра схвата
манипулятора в неподвижной системе координат Оху, свя-
занной с отверстием; р — угол между осью схвата манипу-
лятора и осью у неподвижной системы координат; х1? у± —
Рис. 5.11
координаты центра штифта в системе
координат Оху; а — острый угол меж-
ду осями штифта и отверстия;
JM — момент инерции схвата отно-
сительно горизонтальной оси поворо-
та; М — приведенная масса мани-
пулятора; Рм — вес манипулятора; сх,
су, Ср — коэффициенты упругости ме-
ханизма захвата штифта; Fxynp,
Гуупр — управляющие силы по осям
х2 и у2; Групр — управляющий мо-
мент по углу р.
Так как угловая жесткость мани-
пулятора значительно больше угло-
вой жесткости крепления штифта в
схвате, то для декартовой системы
можно считать, что угол р == const,
Р а. Ранее было принято, что
сх — су. С учетом принятых допу-
щений уравнения движения штифта,
186
1
записанные как уравнения плоского движения твердого
тела, будут иметь вид:
njXi — сх(х2 — %i)cos Р;
ту 1 « су (у2 - уi) sin р - Рм; (5-75)
Jd* = са(Р - а),
где тп — масса штифта; Jc — момент инерции штифта отно-
сительно его центра масс.
В процессе сопряжения деталей штифт относительно
отверстия может занять ряд положений (рис. 5.12), разли-
чимых с точки зрения уравнений динамики.
Построим динамическую модель для случая одноточеч-
ного контакта по боковой поверхности штифта
(рис. 5.12, а). Уравнения движения штифта с учетом трения
для случая, изображенного на рис. 5.12, а, имеют вид
mx’i = сх(х2 — хО — (/sin а — cosa)Ni;
™У1 - су(У2 - У1) + (/cosа + sina)Ni - Рм;
Ла = ( -/4- + 4- - h W + са(р - а),
(5.76)
где i/2 — h — j/j cos а — (D/2 — xjsin а; / — коэффициент тре-
ния ; I — длина штифта; d — диаметр штифта; D — диаметр
отверстия; Nx — реакция стенки в точке контакта.
Аналогичные соотношения можно записать для случая,
изображенного на рис. 5.12, б. При этом уравнения движения
штифта примут вид:
mxt = сх(х2 - xj + (/ sin а - cosa)N2;
ШУ1 = су(у2 - У1) + (/cos а + sin a)N2 - Рм; (5.77)
Рис. 5.12
187
Также необходимо учитывать, что на движение штифта
накладывается условие связи вида
(D \ . d
I у + Xi Icos а + yi sm а —
Уравнения движения манипулятора остаются в виде (5.76).
Рассмотрим случай, изображенный на рис. 5.12, в. Он
характеризуется касанием нижнего угла штифта боковой
стенки отверстия. Уравнения движения штифта для этого
случая имеют вид:
— сх(х2 — Xi) — N3;
тУ1 = Су(у2 - У1) + fN3 - Ры;
Г . ( d
Jca — са(Р — а) 4- N31 sm а I -х- +
(5.78)
Запишем уравнения движения штифта для случая,
изображенного на рис. 5.12,г:
ШХ1 = сх(х2 — *i) + N4.;
(5.79)
cos а
sin а
Условие связи имеет вид
—smoc
(5.80)
Для всех других случаев уравнения движения можно
записать аналогичным образом.
5.8. Автоматизированный анализ
инамики манипуляторов
на основе метода кинетостатики
Рассмотрим и-звенный манипулятор, как разомкнутую ки-
нематическую цепь, состоящую из твердых тел, соединен-
ных кинематическими парами 5-го класса.
Для определенности рассмотрим манипулятор, состоящий
только из вращательных пар.
С каждым звеном манипулятора свяжем правую декар-
тову систему координат следующим образом: начало
координат Oi звена расположим в центре шарнира, ось
Ozi направим по оси шарнира, ось 0^ определим как
общий перпендикуляр к осям и О^, имеющим
188
направление от Of-iZf-t к О^, ось 0^ дополняет декарто-
ву систему координат до правой. За нулевую систему коорди-
нат выбираем систему, связанную со стойкой. С характер-
ной точкой схвата манипулятора свяжем прямоугольную
систему координат, оси которой выбраны аналогично.
Обозначим qi(t) угол поворота i-ro звена относительно
(i — 1)-го. Совокупность величин qt(t) в случае вращательных
пар однозначно определяет положение манипулятора в
пространстве. В связи с этим q^t) могут быть названы
обобщенными координатами манипулятора.
В подвижной i-й системе координат известны векторы,
определяющие геометрию звеньев: fi — радиус-вектор центра
масс Ci звена i; bt — радиус-вектор смещения точки О
относительно х по оси; — радиус-вектор точки пересе-
чения осей xt и zt; Rt — ht + bt — радиус-вектор между на-
чалами i-й и (i + 1)-й системами координат.
Известны также конструктивные углы — углы поворота
оси Zi относительно zt-^ и углы начального положения
ql0. Легко видеть, что базисы подвижных систем координат
и базисы инерциальной системы координат можно сделать
коллинеарными, используя матрицы поворота:
— singf 0 | 1 О О
cos qt 0 ; Afai = I 0 cos aB. — sin a
0 1J Lo sinaj cosotf-
где Mqi — матрица поворота осей
Zi до момента, когда оси х£ и
неарными; Mai — матрица поворота
относительно оси х,-! до момента,
OXiyiZi относительно
Xi _ 1 станут колли-
осей Oi_1Xi_1yi_1Zi_i
когда оси zt и
станут коллинеарными.
Преобразование координат векторов, заданных в i-й
системе, в координаты неподвижной системы достигает-
ся умножением на матрицы последовательных поворотов,
начиная с i-й системы координат до системы O0x0)’0z0.
Проекции векторов, заданных в системе координат звена,
в неподвижной системе будут находиться по формулам
[г,] = пмк И',
(5.81)
где [rj — столбец координат вектора _fi в системе
00*оУо2о- Тогда проекции векторов Rci и RQi в неподвиж-
ной системе найдутся по формулам:
[Kci] = [^оД — [*d] + И-
j= i
(5.82)
1
Используя полученные выше выражения, записанные в
векторно-матричной форме, можно определить выражения
для линейных и угловых скоростей и ускорений центров
масс звеньев ct и центров шарниров Ot в зависимости
от первых и вторых производных обобщенных координат
qt по времени, а также геометрических параметров звеньев.
Указанные зависимости выглядят следующим образом:
— Е ЭД»
j-i
где Ц- — абсолютная угловая скорость i-ro звена; Cj —
единичный вектор, орт оси; — первая производная по
времени от обобщенной координаты;
t’Oi = = Е Х
где v0/, v0 а -1) ~~ соответственно линейные скорости центров
i-ro и (i — 1)-го шарниров манипулятора;
= l*oi + Е e/b х и»
где vci — линейная скорость центра масс i-ro звена; ги —
радиус-вектор, имеющий начало в центре шарнира и
конец в центре масс cf;
I S— 1
= Е + Е Е х ^)эд,
J ~ 1 s — 2 1
где Е/ — абсолютное угловое ускорение i-ro звена;
OOi ~ 1) 4" Ej~ 1 х R-i-i. 4" X
линейные ускорения
где aQi и «og-d ~ соответственно
центров i-ro и (i — 1)-го шарниров;
dci = ^O(i- 1) 4" Х Hi 4- X (Q/ х гй),
где dci — линейное ускорение центра
Заменяя в последнем выражении
масс i-ro звена.
d0(i-i), получим
или в более компактной форме
в *> ->
«а = Е
j=i
190
1
I ) I.C
в, - E E x ^sYws + x (^i-i x ^i-i) +
s = 2 j ~ 1
Для построения динамической модели манипулятора
воспользуемся принципом Даламбера: начиная с шарнира с
номером п, фиктивно разрываем кинематическую цепь и
рассматриваем равновесие отсеченной свободной части
манипулятора и движущего момента в шарнире под
действием сил инерции сил тяжести (?$, обобщенной
силы в шарнире Составим суммы моментов для
каждого шарнира с номерами и, п — 1,..., 1; получим
систему п уравнений, линейных относительно вторых произ-
водных обобщенных координат:
Е W + M(Gi) + m^Ff) + Mt = 0.
i ~ 1
Вектор сил инерции, приложенный в центре масс i-ro
звена, определяется так:
г = - niidd.
Заменяя dci9 имеем
*
••
Qj 4~ (5.83)
Момент от силы инерции, приложенный n iicitipc
масс к-го звена, относительно* к-го шарнира он редел иск и
следующим образом:
М (?Г) = i г» X А».
к — п
Заменяя Гки, имеем:
М(АВ)= +
1
191
1
Главный момент сил инерции определяется по форму-
ле (5.11)
Мni — (Qf х 0f) Qf x 0fgf.
Заменяя входящие в эту формулу угловую скорость
и ускорение, получим
Mni — X (5.84)
j = 1
оси шарнира j в системе i; 0, — тензор инерции;
— кососимметрическая матрица, составленная из элементов
вектора
Момент от сил тяжести в шарнире определяется так:
М (Gi) =
х П/
где д — ускорение свободного падения.
Считая известными моменты обобщенных неконсерватив-
ных сил, получим уравнение движения в виде
где A-j и — векторы-столбцы, элементы которых не
зависят от
Разрешая уравнения движения относительно q, можем
записать окончательно уравнения движения манипулятора
192
1
в виде
5 = — [л°]-1в0.
Рассмотренный метод формирования уравнений динами-
ки манипулятора при моделировании на ЭВМ представлен
в виде схемы алгоритма (рис. 5.13). Предполагается, что
известно начальное положение манипулятора и начальные
обобщенные скорости звеньев, т. е. известны qiK и qis,
В блоке 1 вводятся исходные данные: геометрия звеньев
(Л, Ь, г), массы звеньев (тп(), тензоры инерции
начальные условия (q^, шаг интегрирования (At).
В блоке 2 формируются матрицы перехода At из
i-й системы координат в систему координат, коллинеар-
ную нулевой.
Вычисление коэффициентов при
q- в выражении
Формирование матриц, перехода
• 9
Вычисление коэффициентов, не
зависящих от q., в Выражении
Определение координат поло- J
жений центров масс звеньев и шарниром
-//7.
Решение системы линейных. диф-
ференциальных уравнений от-
носительно о^
Печать координат центров
масс звеньев и шарниров
/7----------1—-----------
Интегрирование q-, опреде-
ление q. и q-
Вычисление коэффициентов при
о. В выражении F?
t __
Вычисление коэффициентов, не лг-
висящих от"й}в выражении F-*
Вычисление линейных и угло~
вых скоростей и ускорений
Печать линейных и узловых
скоростей и ускорений
Рис. 5.13
7 Механика промышл. роботов, кн. 1
193
1
J блоке 3 производится пересчет векторов ёь fb Rif
bb hi из собственной системы координат в нулевую.
В блоке 4 по зависимостям (5.81) определяются коор-
динаты положений центров масс звеньев и шарниров.
В блоке 6 по зависимости (5.83) вычисляются вектор-
ные коэффициенты при в выражении сил инерции.
В блоке 7 по зависимости (5.84) вычисляются коэффи-
циенты, не зависящие от qb в выражении сил инерции.
В блоках 8 и 9 по зависимостям (5.86) вычисляются
коэффициенты Ац и В? в выражениях (5.85).
В блоке 10 решается система п линейных уравнений
с п неизвестными. При этом коэффициентами при неизвест-
ных являются элементы векторной матрицы а сво-
бодными членами — Bf.
В блоке 11 проводится интегрирование величин, полу-
ченных в десятом блоке, с заданным шагом интегриро-
вания. Предполагается, что за выбранное время Д/ значение
5* остается неизменным, далее вычисляются и qt для
следующего шага интегрирования.
В блоке 12 вычисляются линейные и угловые скорости
и ускорения.
Описанный выше алгоритм может быть использован
для динамического анализа манипуляторов с различной
кинематической структурой.
Глава 6
Уравнения Лагранжа
и принцип Даламбера
в динамике манипуляторов*
Использование уравнений Лагранжа позволило разработать
эффективные алгоритмы автоматизированного анализа мани-
пуляторов на ЭВМ. Достоинством метода является его
универсальность и удобство учета упругих свойств звеньев.
Прямое использование уравнений Лагранжа для модели-
рования динамики манипуляторов связано с необходи-
мостью вычисления производных от кинетической энергий,
* Обозначения величин в гл. 6, 7 аналогичны принятым
в гл. 3 и Приложении II.
194
1
чго вносит значительные ошибки. В некоторых алгоритмах
нот недостаток устранен путем преобразования исходных
уравнений.
6.1. Кинетическая энергия манипулятора
Кинетическую энергию произвольной точки массы dm,
принадлежащей звену i, запишем в виде произведения
матриц
dR'i = 72ЛоКо<1т, (6.1)
где Ro — производная от Ro по времени t. Так как
Rq — TiRb то в соответствии с (П.1) и (П.5)
R0RJ = tr (T^RfT?), (6.2)
следовательно, кинетическая энергия точки
dKi = 7г tr (TtRiR? Ti) dm. (6.3)
Кинетическую энергию i-ro звена манипулятора запишем
так:
к, = */2 tr f (TtRtR? Tft dm. (6.4)
Так как матрицы и 77 не зависят от координат
точек i-ro звена, то их можно вынести за знак интеграла:
К, = 71 tr [Т, (J адт dm) 77]. (6.5)
Обозначим выражение, стоящее во внутренней скобке, через
Нг и вычислим его:
RtRJ dm
xt
Х}У1 XiZt Xi
УМ yl ytZi yi
ZiXi z^i zl Zi
dm.
(6.6)
xt yt z( 1
Введем следующие обозначения:
jxidm — fxiytdm = ; Jz?dm =
Jx£dm = пцх?; f yt dm = mtyf; fztdm^ m^f; J dm =* mb (6.7)
где пц — масса i-ro звена; x*, yf, zf — координаты центра
тяжести i-ro звена в собственной системе, т. е. в
системе, жестко связанной с i-м звеном; Jjj, ~
элементы тензора инерции, вычисляемые для i-ro звена
195
относительно собственных осей. Подставляя (6.7) в (6.6),
получаем матрицу характеризующую инерцию звена г.
7(i)
**ух
ху
ПЦХ*
Jyy
azy
T(i)
u xz
uyz
j®
щу*
mpc*
т,.у?
KU
(6.8)
Подставляя (6.6) в (6.5), получаем формулу для определе-
ния Кинетической энергии i-ro звена:
= 1/2tr(fiH,-TiT).
(6.9)
Кинетическая- энергия манипулятора равна арифмети-
ческой сумме кинетических энергий всех его звеньев:
к = 72 Е ъ&ндт),
•=1
(6.10)
где п — число звеньев манипулятора.
Пример 6.1. Вычислить кинетическую энергию манипу-
лятора с тремя степенями свободы (см. рис. 3.5). Известны
массы всех звеньев и моменты инерции относительно
главных осей инерции. Принято, что центры тяжести
звеньев лежат на соответствующих осях кинематических
пар, а главные оси инерции совпадают с координатными
осями (хотя это и не соответствует конструктивной схеме,
изображенной на рис. 3.5).
По формуле (6.10) запишем кинетическую энергию звена 1:
Ki = 'Л tr (Л HjTJ).
(6.И)
Согласно (3.40) и (3.26) и с учетом того, что первая
кинематическая пара вращательная, получаем
Т1 == Uuqi = (6.12)
Перемножая матрицы ОвР и
лами (П.48) и (3.15), находим
Alt определяемые форму-
—costh 0 0
— sin^t 0 0
0 0 0
0 0 0.
(6.13)
196
Напишем правую часть выражения (6.11) более подробно:
— sin 41
eos 41
cos 4i —sin 4i
О
О
о о
о о
о о
0 0.
XX
о
о
о
О
7(0
^уу
о
О
о
о
^14
mi _
— sin 4i
-cos 4i
О
О
COSt/i
- sin 41
О
О
О О
О О
О О
О Cl-
о
о
о
о
после перемножения и несложных преобразований получаем
XX
уу, 2
(6.14)
где — момент инерции звена 1 относительно оси
Естественно, выражение для можно было бы записать
сразу, не делая никаких выкладок.1* Однако наша цель
состоит в том, чтобы научить пользоваться именно
общими универсальными формулами, пригодными для лю-
бого звена, в каком бы сложном движении оно ни
находилось, с тем чтобы, во-первых, суметь правильно
запрограммировать эти формулы, а во-вторых, избежать
весьма возможных ошибок при вычислении кинетической
энергии звеньев с большим порядковым номером.
Кинетическая энергия звена 2
К2= 72tr(f2H2TJ). (6.15)
Аналогично предыдущему,
. е. согласно (3.40) и (3.26) и
с учетом того, что вторая кинематическая пара мани-
пулятора является поступательной, получаем
Т2 = 1/2141 + С/2242, (6.16)
причем
&21 = Qsp (^) У^1у^2> U22 ~ -^l^CA(^)-/42* (6.17)
Подставляя (П.44), (П.48) и (3.15) в (6.17), получаем
[sin 41 0 —cos 41 0
-cos4i 0 -sin4i 0
0 0 0 0
0 0 0 0
197
Используя (6.18), (6.16) и (6.8), получаем из (6.15) вы-
ражение для кинетической энергии Звена 2:
К2 = (J® + Л2Д у + т2 = 42> (6.19)
где Jj,2) — момент инерции звена 2 относительно оси у2.
Кинетическая энергия звена 3 манипулятора
K3 = ,/2tr(T3H3T3T), (6.20)
где
^3141 +
U32Q2 + ^зз(7з>
(6.21)
причем
U31 — А^А2А2 —
^21^3>
1732 — (/с) А2А3 — ^22-^3»
U33 — -^1^2^сд (^)-^З’
Используя матрицу Л3, найденную в примере 3.3 [см. фор-
мулу (3.15)], получаем
sin#! 0 —cos#i — #3COS#!
— COS#i 0 — sin#! — #3sin#i
0
0
0
0 0
0 0 0
0 0 0 0
0 0 00
0 0 0 1
0 0 00
0 0 0 — sin#i
0 0 0 cos #t
0 00 0
0 00 0
(6.22)
/
Подставляя (6.22) в (6.21), вычисляем
#isin#i 0 -#icos#i
— #icos#i 0 — (/jsin#!
ООО
ООО
-#l#3COS#i -
-#i#3sin#i +
4г
О
#з sin #1 ”
#3 COS#!
. (6.23)
Меняя в (6.23) строки на столбцы, образуем транспони-
рованную матрицу:
198
1
4i sin q i
0
— qY cosqi
cos - g3singi
-Qicos^! 0 01
0 0 0.
~4i sin qi 0 0 I
-4i^3 sin qi + 4зсоз^! 4г 0j
(6.24)
В соответствии с (6.8) записываем матрицу Н3:
rjg о 0 0 1
О Д3’ о о
О 0 ™3z*
О 0 w3zj m3
(6.25)
След матрицы, полученный в результате перемножения
матриц (6.23), (6.24) и (6.25), равен
tr(T3H3T3) = J^ql 4- J24i + 2m34^3zJ +
(4?d + 41 + 4з).
(6.26)
Подставляя (6.26) в (6.20) и учитывая при этом, что
(6.27)
где — осевой момент инерции звена 3 относительно
оси у3, получаем кинетическую энергию звена 3:
К3 = (J<3) + m3<?i + 2m3q3z3) #- + т3£- + (6.28)
Л 2 2 2
Складывая арифметически Kit К2
кинетическую энергию манипулятора.
и К3> получаем
6.2. Потенциальная энергия
манипулятора
Потенциальная энергия ГЦ i-ro звена манипулятора в поле
сил тяжести
ГЦ — Р
(6.29)
где Pt — сила тяжести звена i; zj — z-я координата центра
тяжести звена i в системе отсчета, связанной со стойкой
манипулятора. В матричной записи формула (6.29) приобре-
тает вид
ГЦ = -т&ЪКТ,
(6.30)
где Rf — матрица-столбец, первые три элемента которой
199
суть декартовы координаты центра тяжести звена i в
собственной системе отсчета, связанной со звеном i; G7 —
матрица-строка следующего вида:
— Е®, 0, —д, 0],
(6.31)
где д — ускорение свободного падения.
Потенциальная энергия манипулятора, имеющего п
звеньев,
(6.32)
Пример 6.2. Вычислить потенциальную энергию манипу-
лятора, изображенного на рис. 3.5, считая, что силы тяжести
звеньев
известны.
Потенциальная энергия манипулятора равна алгебраи-
ческой сумме потенциальных энергий всех его звеньев, т. е.
П = Ih + П2 + П3. (6,33)
Потенциальная энергия первого звена, согласно (6.30),
Hi = — miCFTiRf,
(6.34)
cos^i
0
-sin^
COS^t
о
0
01
0
0
0
то
П1 - mrfiZf
Для вычисления П2 и П3 нужны
Т__, *
з —
(3.15), получаем
. . . т_ - - матрицы Т2 = Л1Л2 и
Л1Л2Л3. Используя для матриц Аъ Л2, А3 выражения
— COS#! 0 —sin#! 0
—sin 0 cos^ о
0 1 0 q2
0 0 0 1
200
—cos0 —sin4i
— sin 4i 0 cos 4i
0 1 0
0 0 0
-4з sin 4i
43cos4i
<12
Тогда потенциальная энергия второго и третьего звеньев
манипулятора
П2 = GTT2R* = P2q2; П3 = &T3R% = РзЯз- (6.35)
Суммируя (6.34) и (6.35), получаем
П = PizJ + P2q2 + P3q3 (6.36)
— потенциальную энергию манипулятора.
6.3. Обобщенные силы
Пусть к i-му звену манипулятору приложены сила Ft и момент
Mi9 причем векторы Ft и записаны в системе i и
проходят через начало координат этой системы. И пусть
никакие другие внешние силы не действуют на манипуля-
тор. Требуется определить обобщенную силу, соответствую-
щую обобщенной координате q^
Как известно [5], обобщенная сила — это скалярная
величина, равная
Qi — 6^4/64/,
(6.37)
где 8Л — элементарная работа приложенных сил на таком
возможном перемещении манипулятора, при котором только
одна его обобщенная координата, а именно qb получает
приращение Sqi9 в то время как все остальные обобщен-
ные координаты сохраняются неизменными.
Если звенья i — 1 и i образуют поступательную пару,
то возможным является поступательное перемещение звена
i вдоль qch Zj пары на величину '84,. В jyroM случае
момент Mt работы не производит, а сила Ft совершает
работу, равную
(6.38)
где — орт оси Zi в системе i; после подстановки (6.38) в
(6.37) получаем
(6.39)
Следовательно, обобщенная сила в этом случае равна
проекции силы Fi на ось кинематической пары.
201
1 и i образуют вращательную пару,
поворот звена i на^угол
В этом случае, наоборот, сила Ff, прило-
Если же звенья i
то возможное перемещение
вокруг оси zt
женная к точке на оси вращения, работы не совершает,
а работа момента равна
5 А = MtzfSqi.
Подставляя 5Л в (6.37), получаем
(6.40)
I
следовательно, обобщенная сила в этом случае равна
проекции момента Mf на ось пары.
Теперь поставим более сложную задачу. Пусть сила
Fo и момент Мо приложены к схвату,
Fo и момент* Мо приложены к схвату, т. е. к звену и,
причем сила Fo проходит через начало системы ц. Однаюр,
в отличие от предыдущей задачи, векторы Fo и Мо
заданы своими проекциями на оси координат системы 0,
а именно известны Fx0, Fy0, Fz0, MxQ, My0, Mz0. Требуется
определить обобщенную силу Qb отнесенную к обобщен-
ной координате qt.
Запишем заданные силу
проекции на оси координат
Ft = TFlF0; М^ТГ'Мо.
и момент в системе i (через
системы 0 в матричном виде:
(6.41)
Здесь
хО
^xi
L О J
О
My0
MSQ
0 J
(6.42)
или в блочном виде
Мо
о ’
Mt
(6.43)
L0
Чтобы найти матрицу Tf1
знать Tt. Матрицу Tf по аналогии с (П.30) запишем в
блочном виде:
входящую в (6.41),
нужно
ч
I
г
1
1
1
О
о ’
мо., ho. I
(6.44)
Обратная матрица
0
(6.45)
202
1
Подставляя (6.44) в (6.41), получаем (в блочной записи)
заданные силу и момент через проекции на координатные
оси системы i:
(6.46)
Для того чтобы свести задачу к предыдущей, нужно
привести заданную систему сил к. началу координат систе-
мы i. Сила Fb хотя и записана уже в системе i, но при-
ложена в начале координат системы и, поэтому при
перенесении ее в центр системы i появляется дополни-
тельный момент, равный
= 4 п х Fb (6.47)
где hitn — радиус-вектор, идущий из начала системы i в
начало системы п, записанный в системе i. Он является
блоком матрицы
(6.48)
Таким образом, при приведении системы сил к центру
системы i получаем главные вектор и момент, равные
соответственно
Ггл = Ff; (6.49)
АГЛ = Mi + fiit „ х Ft, (6.50)
Если кинематическая пара (i — 1, i) поступательная, то
Qi = >£? = FiZ?, (6.51)
а если вращательная, то
Q, = = (М, + х А) г?.
(6.52)
Пример 6.3. Вычислить обобщенные силы, соответ-
ствующие обобщенным координатам манипулятора, изобра-
женного на рис. 3.5, если к схвату приложены момент
Мо и сила Fo, проекции которых заданы в системе 0.
Сила Fo проходит через начало координат системы
ХзУз^з-
Определяем Qx. Поскольку кинематическая пара (0, 1)
вращательная, пользуемся формулой (6.52):
xFJz?. (6.53)
Для вычисления* Fx и потребуется матрица MJ, ь
которую находим с помощью матрицы Тг:
203
1
Т\ = To,i = A —
следовательно,
fcos 4i
sin 4i
0
COS
sin gi
0
— sin 4i 0 0 ”1
cos 4i 0 0 I ГМо.1
о I 0 rL 0
о о d
—sin 4i 0
cos 4i 0
0 1<-
I cos4i sin4o 0 I
-sin4i cos^i 0 I-
L 0 0 1J
(6.55)
Согласно (6.46) с учетом (6.55) и (6.42) получаем
-FxOsin4i
Fy0cos4i
0 J
Myosin 41
Му0 cos 4i
!Mx0cos4i
-MxOsin4!
MZQ
О
Чтобы определить h
(6.48) матрицу Tlt3:
(6.56)
записываем в соответствии с
0
0
о
о
0
0
О
(6.57)
о
о
0
следовательно,
(6.58)
(6.59)
Fy0 sin 4i).
Используя (6.56) и проводя действия, указанные в (6.53)
получаем
Qi = Mzo = 43(^0 cos 41
Определяем 2г* Поскольку кинематическая пара (1,2)
поступательная, пользуемся формулой (6.51).
(6;60)
204
1
Так как [по (6.46)]
то задача сводится к вычислению матрицы
(6.61)
входит в выражение
ho, 2
Сопоставляя (6.62) с
(6.62)
— cos^j 0 —sinQt
— sin 44 0 cos 44
заключаем, что
Leos 44 0
-sing! О
О 1
sin 44
cos^t
следовательно,
I — cos 44 —sin44 0
Мо,2 = | 0 0 1
I— —sin cos <4 О—
(6.63)
О 1 О
0 0 О
Подставляя (6.63) в (6.61),
получаем
а затем результат в (6.60),
-FxOsin4i1
Fyo cos 44 ♦
(6.64)
Аналогично находим Q3:
~ — cosq^ 0 — sin 44
— sin 44 0 cos
О 1 О
L О О О
4з cos q t
205
«мм
—COS#!
-sin^j
О
О — sin (/j-.
О cos^i ;MJ>3 =
1 О
—cosqt
О
— sin qt
— smgt О
0 1
COS#! О
— Fxocosgi -Fyosin^i
Fz0
—FxOsin^! + Fyocosgt—
Q3 = F3z3 = -FxQ sin qt 4- Fyocosq^
(6.65)
Найденные обобщенные силы потребуются в дальней-
шем для решения задач динамики манипуляторов.
6.4. Вывод уравнений Лагранжа II рода
в матричной форме
Как известно [5], уравнение Лагранжа II рода, отнесенное
к обобщенной координате qj9 имеет вид
(6.66)
Если все внешние силы, действующие на манипулятор,
являются потенциальными, то
Qj
дП
Sqj
(6.67)
и уравнение (6.66) записывается так:
^=0
dt 8qj
где L
и потенциальной энергии:
функция Лагранжа, равная
(6.68)
разности кинетической
(6.69)
Считаем, что на манипулятор действуют различные силы,
в том числе и потенциальные (силы тяжести). Предста-
вим обобщенную силу состоящей из двух слагаемых
(6.70)
причем первое слагаемое связано с наличием непотенциаль-
отенциальных. Подставляя (6.70) в (6.66),
ных сил, второе —
получаем
206
(6.71)
(в дальнейшем знак «тильда» над Qj опущен).
Дальнейшие преобразования нацелены на то, чтобы
записать это уравнение в матричном виде, удобном для
программирования. В ходе преобразований потребуется
целый ряд вспомогательных величин, получаемых дифферен-
цированием выражения Т, = AVA2... Д. Эти величины
следующие:
(6.72)
(6.73)
(6.74)
(6.75)
(6.76)
(6.77)
(6.78)
207
1
или
с Z иил при j < »;
dt dqs I 0 при j > i;
(6.79)
<Ша .
-аг*
и
поэтому
к=1
(6.80)
8) т/ = £ £ uikMihk + X
£=11=1 £
(6.81)
Переходим к выполнению операции, указанных в левой
части уравнения (6.71). Частное дифференцирование (6.10)
по qj дает:
яг 1 п / Ат • \ Iй /• ЛТ.Т\
или с учетом (6.77) и (6.78)
Заметим, во-первых, что
(6.82)
(6.83)
поскольку матрица Ht является симметрической [см. (6.8)],
во-вторых, что произведение матриц, стоящее во втором
слагаемом выражения (6.82), можно получить путем транспо-
нирования произведения матриц, стоящего в первом сла-
гаемом :
(6.84)
208
Но операция транспонирования оставляет диагональные
элементы матрицы на своих местах и, следовательно, не
меняет след матрицы, так что
(6.85)
С учетом (6.85) выражение (6.82) упрощается:
(6.86)
Чтобы получить первый член уравнения Лагранжа,
продифференцируем полученное выражение по времени,
имея в виду, что от времени не зависит:
(6.87)
Первое слагаемое этого выражения при использовании
(6.79) и (6.74) преобразуется к виду:
tr qkq+
(6.88)
HiT^
Второе слагаемое при использовании (6.77) и (6.81) стано-
вится таким:
i Uikl) klik
i=jk=il=l
(6.89)
Подставляя (6.88) и (6.89) в (6.87), получаем первый
член уравнения Лагранжа:
209
tr (UtjH.V^q^
i-jk=l
(6.90)
Переходим к преобразованию второго члена в урав-
нении Лагранжа. Дифференцируем (6.10) по обобщенной
координате:
то
(6.91)
и выражение (6.91) упрощается:
(6.92)
Используя (6.75) и (6.72), получаем
tr (UaJH tU £) Щ
(6.94)
i=jk=l
1
Чтобы получить последний член левой части уравнения
Лагранжа, дифференцируем выражение (6.32) по обобщен-
ной координате:
или с
учетом (6.73)
п
- ynii&UijR*.
(6.95)
Подставляя (6.94) и (6.95) в (6.71), получаем
QkQl —
i=jk=l
(6.96)
— уравнение Лагранжа II рода в матричной записи.
6.5. Алгоритмы решения задач динамики
манипуляторов с помощью уравнений
Лагранжа II рода
Первая задача динамики формулируется так: задано
;виже
ние механической системы, требуется определить обобщен-
ные силы, обеспечивающие это движение. Вторая задача
динамики состоит в определении закона движения по
заданным обобщенным силам. В манипуляторе обобщен-
ные силы Qi — это проекции на оси кинематических пар
моментов (если пары вращательные) и сил (если пары
поступательные). Движение определяется изменением во вре-
мени обобщенных координат qi, которыми являются углы
поворота во вращательных и смещения — в поступательных
кинематических парах.
При решении первой задачи обобщенные координаты
qi (t) — известные функции времени. Дифференцируя их, можно
получить qi и Зная qh qi, qh можно вычислить все
величины, входящие в левую часть уравнения (6.96). Следо-
вательно, можно вычислить обобщенные силы Qj. Этим
исчерпывается алгоритм решения первой задачи динамики.
211
Несколько сложнее решается вторая задача. В ней
известными считаются обобщенные силы Qj(t) и значения
обобщенных координат и их скоростей. в начальный
момент to, т. е. qi(to) и 4/ (to). Подставляя в (6.96) величины
#i(to), qi(to) и Qj(to), можно привести это уравнение
к следующему виду:
afcj (to) qk (to) = ₽j (to)
(/ , 2,..., и),
(6.97)
где a*/ (to) и Pj (to) — известные числа. Аргумент to означает,
что эти числа определены для момента to и, следова-
тельно, зависят от выбора этого момента. Фактически они
зависят от ^(to), ^i(to), Qj(to). Из и уравнений (6.97)
можно найти п неизвестных величин: qk(t0); fc=l, 2,...,n.
Решение уравнений удобно записать в матричной форме.
Для этого введем матрицу а размером п х п с элемен-
тами a*j, матрицу-столбец р с элементами Pj и матрицу-
столбец q с элементами qk. Система уравнений (6.97)
примет вид
ctq = Ps (6.98)
а ее решением будет
q — a 1Р = у.
(6.99)
Для вычисления матрицы у, т. е. для обращения матрицы
а и умножения ее на матрицу-столбец р, можно исполь-
зовать стандартные программы матричной алгебры, входя-
щие в стандартное математическое обеспечение современ-
ных ЭВМ.
Найти закон движения манипулятора — значит найти
функции qi (t) или в матричном виде — q (t), где q —
матрица-столбец с элементами qt. Чтобы найти q(t), не-
обходимо проинтегрировать дифференциальное уравнение
(6.99). В этом уравнении матрица у может быть вычислена
для любого момента t, если для этого момента известны
матрицы-столбцы q, q, Q (элементы матрицы q — это qu
а элементы матрицы Q — это Qj). Способ вычисления y(t)
тот же, что и способ вычисления у (to), описанный выше.
Интегрирование дифференциального уравнения может вы-
полняться с использованием одной из многих стандартных
программ. Опишем здесь простейший из применяемых
для этой цели алгоритмов, носящий имя Эйлера.
Будем двигаться по оси времени малыми шагами At,
начиная от t©. На каждом шаге будем считать постоян-
212
^(m+l) = +
(6.100)
ными матрицы q9 q. Тогда на шаге номер т
q™ kt;
Для шага т — 0 правые части (6.100) известны, что
позволяет вычислить и q(i\ После этого делается
следующий шаг (т = 1), на котором правые части (6.100)
также известны, и т. д. В результате q9 q9 q стано-
вятся известными в дискретных точках оси времени,
разделенных интервалами Аг. Чем меньше Аг, тем меньше
погрешность расчета, но выше его трудоемкость.
Для некоторых конкретных манипуляторов задачи
динамики удается решить гораздо проще, чем это было
описано выше, так как человек может учесть особенности
конкретного манипулятора. Для того чтобы ЭВМ могла
учесть конкретные особенности манипулятора так же, как это
делает человек, необходимо составить программу высокой
логической сложности. Если такой программы нет, то
следует работать по «неинтеллектуальной», но зато универ-
сальной программе. «Неинтеллектуальную» программу не-
сложно составить, запрограммировав уравнения (6.96) и расчет
всех входящих в эти уравнения матриц. Соответствующие
формулы имеются в данной главе. По такой программе
можно решать задачи динамики для манипуляторов с
любым числом звеньев, последовательно связанных между
собой и образующих произвольно чередующиеся вращатель-
ные и поступательные кинематические пары.
Пример 6.4. Для манипулятора с тремя степенями
свободы (см. рис. 3.5) определить в начальный момент
to = 0 обобщенные ускорения при следующих данных:
qi Go) — Q2 Go) — Яз (to) — 0; Qi (0) — q2 (0) — qz (0) — 0;
Qi Go) — Ю; Qi Go) — 30; Q3G0) — Ю;
mi = m2
= m3 — 1; = J<2) = J<3)
Pi = P2 = P3 = 10; xf = yf = zf — 0; i = 1, 2, 3.
Равенство нулю координат центров тяжести обеспечи-
вается специальными противовесами, не показанными на
рис. 3.5.
Составляем уравнения Лагранжа II рода:
а аь 6L л
--------лГ" = С1»
at dqi dtfi
213
1
d IL cL
At 8q2 6q2
A BL 6L _
dt cq3 8q3 3
где L — К — П. Кинетическая энергия рассматриваемого
манипулятора, согласно (6.14), (6.19), (6.20) и с учетом того,
что = О,
К = К1 + К2 + К3 = '/2 + J'2’ + J'3’ + m^Dql
(тг + т3) ql + m3q3],
потенциальная энергия, согласно (6.36),
П = (Р2 + Рз) q2,
следовательно,
-^= (Л*’ + Л2’ + Л31 + тэ«Й51; ("12 +
dqi dq2
= m3q3qi.
Подставляя полученные выражения в уравнения Лагран-
жа II рода, после дифференцирования по времени получим
(Лп + J(y2} + 43) + тз£з)31 + 2тз0з414з = Si;
(m2 + m3)q2 + Р2 4- Рз = Q2',
т3‘4з - m3q3qi = Q3,
Эти уравнения при t = 0 выглядят так:
3§t = 10; 2q2 = 10; 5з - Ю,
или
qi Go) ~ 3,3; q2 Go) = 5; qs Go) = Ю.
(6.101)
По алгоритму, который изложен выше, нужно было бы
вычислить матрицы, входящие в уравнение (6.96), построить
матрицы а и р и найти матрицу у. Но при расчете
«вручную» проще было получить конечный результат так,
как это сделано.
Рассмотренный пример наглядно демонстрирует, что с
помощью уравнений Лагранжа II рода можно вычислить
обобщенные ускорения, необходимые для расчета траекто-
рии схвата манипулятора.
214
Задания для самостоятельного выполнения. Составить
уравнения Лагранжа II рода для манипуляторов, кинема-
тические схемы которых изображены на рис. 6.1, а—е.
Считать заданными обобщенные силы, массы звеньев, их
размеры и моменты инерции. Центры тяжести звеньев
Рис. 6.1
215
считать совмещенными с началами связанных со звеньями
специальных систем отсчета, что может быть обеспечено
надлежащими противовесами.
6.6. Определение реакций в кинематических
парах (в матричной форме)
Для расчета звена манипулятора на прочность необхо-
димо
знать внешние силы, приложенные к звену, силы
реакций в кинематических парах и в соответствии с
принципом Даламбера силы инерции. Уравнения Лагран-
жа II рода построены таким образом, чтобы реакции
в парах из них исключались. Нет в них и сил инерции.
Уравнения Лагранжа II рода позволили по заданным
силам отыскать обобщенные координаты, скорости и
ускорения. Зная их, можно найти матрицы Г, Т, Т,
определяющие положение, скорость и ускорение любой точ-
ки звена манипулятора. Это открывает путь к расчету
сил инерции.
Для расчета сил реакций в парах удобно применить
метод множителей Лагранжа, который в сочетании с
принципом возможных перемещений приводит к уравнениям
Лагранжа I рода. Из этих уравнений определяются
множители Лагранжа, представляющие собой в данном
случае силы реакций в кинематических парах.
С целью автоматизации силовых расчетов на ЭВМ будем
использовать специальные матрицы сил размером 4x4.
Матрица сил. Моментом к-го порядка некоторой вели-
чины называют интеграл от произведения декартовой
координаты в /с-й степени на распределение этой вели-
чины. Например, моменты инерции — это моменты 2-го
порядка от распределения масс; статический момент и
масса — это соответственно моменты 1-го и нулевого порядка
от того же распределения. Главный вектор системы сил —
это момент нулевого порядка от распределения сил:
Г™ = JdF.
(6.102)
Проекции главного момента системы сил составляются
из моментов 1-го порядка для проекций сил. Например,
проекция главного момента на ось z — это разность
двух моментов 1-го порядка:
Ms = f х dFy - f у dFx. (6.103)
Матрица сил, которую обозначим через Ф, строится из
моментов нулевого и 1-го порядка для проекций сил:
216
1
fxdFx JydFx JzdFx JdFx
fxdFy fydFy fzdFy JdFy
fxdF? fydFz JzdF2 fdFz
0 0 0 0
(6.104)
Здесь FXi Fy, Fz — проекции силы F на координатные
оси, а х, у, z — координаты точки приложения силы F.
Используя введенные выше матрицы-столбцы F для сил
(элементы Fx, Fy, Fz, 0) и матрицы-столбцы R для радиу-
сов-векторов точек их приложения (элементы х, у, z, 1),
можно представить матрицу Ф в виде
Ф = f dFRT.
(6.105)
Нетрудно убедиться, что после перемножения матриц,
стоящих под интегралом, формула (6.105) приобретает
вид (6.104).
Покажем, как, зная матрицу Ф, вычислить проекции
главного вектора и главного момента системы сил на
координатные оси. Введем в рассмотрение матрицы
0 0 0 0
0 0 0 0
0 0 0 1
_0 0 0 0
(6.106)
Матрицы ПсД (£) и ПвР(Ц фигурировавшие в предыдущих
главах,— это соответственно Q(6) и П(3). Если пара
(i — 1, i) поступательная, то П/ = Под (к) = Q(6); если враща-
тельная, то = Пвр (к) = Q(3>. С помощью матриц П(0,
где I = 1,..., 6, легко получить смещение точек при пово-
217
ротах и сдвигах тела. Так, матрица
8R =
(6.107)
описывает смещение точек при повороте тела вокруг оси х
на малый угол 8qi. Матрицы П(2) и П(3) аналогичным
образом помогают описать повороты тела вокруг осей
у и z.
Матрица-столбец 8R = Q(4) R8q4 описывает смещение точек
при сдвиге тела вдоль оси х на расстояние 8q4. Анало-
гично с помощью матриц Q(5) и Q<6) описываются сдвиги
вдоль осей у и z. Объединяя две последние формулы,
запишем
8R = QPR 8qt.
(6.108)
Матрицы Q(i) можно назвать матрицами скоростей,
поскольку поле скоростей точек твердого тела в общем случае
в
получается с помощью матрицы £ Qw8qi. Первые три
i=i
матрицы описывают поля скоростей при вращениях с
единичными угловыми скоростями вокруг координатных
осей, а последние три — при поступательных движениях
с единичными скоростями вдоль тех же осей.
Элементарная работа силы F, приложенной к точке R,
при смещении этой точки на 8R равна
5 А = tr (F8RT).
(6.109)
Соответственно при смещении тела, описываемом с по-
мощью матрицы Q(I), элементарная работа сил опреде-
ляется формулой (6.109), в которой
8RT = (Q(,)R8qt)T = RT (П(0)т5ф,
так что
8А
8qt
tr (J dFRT (^(/)D = tr (Ф (CF)T).
(6.110)
элемен-
Л
При повороте тела вокруг оси х на угол 8qi
тарная работа сил равна 8А = м£лБ#1,
проекция главного момента на ось х. При смещении
тела вдоль оси х на расстояние 8q4 элементарная работа
сил равна 8Л
место при вращениях и сдвигах относительно других
координатных осе]
полагая в формуле (6.110) 1=1, 2, 3, получим проекции
главного момента
где
Аналогичные соотношения имеют
Отсюда ясно, что, последовательно
на оси
218
(6.111)
проекции главного вектора на те же оси:
М™ = tr(<D(O(1))T); Мугл = tr(0(Q(2))T); Мггл = tr(<D(Q(3))T);
F™ = tr(<D(n(4))T); Гугл
= tr (Ф (Q<5))T); F™ = (Ф (Q(6))T).
Если система сил, действующих на звено манипулятора,
состоит из нескольких подсистем, то матрица Ф для всей
системы сил складывается из аналогичных матриц Ф для
подсистем. Например, пусть Ф~ и Ф+ — это матрицы сил,
посредством которых на данное звено действуют соот-
ветственно предыдущее и последующее звенья. В число
этих сил входят силы реакции в парах, а также актив-
ные силы, возбуждаемые включенными в пары двигателями.
Пусть, далее, Фе — матрица прочих внешних сил, дей-
ствующих на звено. К числу этих сил могут относиться
силы тяжести, силы сопротивления обтекающей манипуля-
тор среды и т. п. Наконец, пусть Ф™ — матрица сил
инерции. Тогда матрица полной системы сил и сил
инерции будет равна
(6.112)
Матрицы сил для звена манипулятора. Силы, действую-
щие на звено i манипулятора, можно записать как в
системе 0, так и в системе i. То же относится и к
радиусам-векторам точек звена i, В предыдущем параграфе,
где не вставал вопрос о выборе систем координат,
предполагалось, что силы и радиусы-векторы записаны
в какой-то одной системе отсчета. Матрицы Ф, в которых
силы, действующие на звено i, и радиусы-векторы точек
их приложения записаны в системе 0, будем обозначать
Фо.,. Формула (6.105) дает
<!><„ = J dF^Rb.
(6.113)
(6.113) в (6.111) получим* главный
вектор в системе О.
При подстановке
момент и главный
прочностной расчет звена i удобно проводить в системе i.
Кроме того, и матрицу сил удобнее вычислять, исполь-
зуя координаты точек в системе i. Обозначим через Ф<
мшрицу сил, в которой силы заданы в системе 0, а
координаты точек их приложения — в системе i:
Между
тем
Ф,
fdFo,^.
(6.114)
Покажем, как вычислить главный момент и главный
lu-Riop в системе
i, используя матрицу Ф,. Для этого
1Н'о(»ходимо найти элементарные работы на элементарных
219
1
перемещениях звена i относительно координатных осей
системы i. Смещение точки
можно получить по формуле
Rt при этих движениях
(6.108), заменяя в ней R
на Rf.
SRt = QF>Rfiqt.
(6.115)
Перепишем это смещение в систему 0:
8Ro., = TfiRi = TiQPRfiqt.
(6.116)
Элементарная работа сил на этом перемещении, согласно
(6.109),
М = tr(f dFo^R^) = tr(f dF^Ti^RiY^) =
= tr(fdFo-JRiT(^))T'Tir^I),
или с учетом (6.114)
8А/8^ - 1г(Ф{(П(/))т77).
(6.117)
Таким образом, в соответствии с (6.111) для расчета
проекций главного момента и главного вектора на коор-
динатные оси системы i получаем формулы:
MTxf = tr (Ф£ (П<1 >)т TJ); = 1г(Ф£(П(2))т77);
= нфдгРУл); Fx? = ^(фдп^Ул);
ргл
ГУ*
1г(фдп(5))тл);
tr^(Q{6))T77).
(6.118)
Поскольку между Ф| и Фо-i имеется очевидное соотно-
шение :
(6.119)
для проекций тех же векторов на оси системы 0 получим
согласно (6.111) и с учетом (6.119) расчетные формулы
следующего вида:
М$., = 1г(Фо.((П“>)т) = tr (Ф| 77 (Q< 1>)т);
= tr (Ф<м (П<2>)т) = tr (Ф,-7J (Q<2))T);
Мг& = 1г(Ф<>.,(П<3))т) = tr (Ф, 77 (Q( 3|)т); (6.120)
F&, = tr (Фо.| (П<4>)т) = 1г(Ф(77(П<4>)’);
Ffg., = tr (Фб_. (Q<5>)T) = ^(Ф.ТТ^5’)1);
= tr (Фо.. (П<6’)т) = 1г(Ф(77(а<«П
dTm
Силы - инерции. Элементарная сила инерции равна
г dm, а соответствующая этой силе столбцовая
220
1
матрица может быть записана так:
dF“H = —jRdm.
(6.121)
Матрица сил инерции определяется формулой, анало-
гичной формуле (6.105):
Фин = f dF™R< = - $RR'dm. (6.122)
Используем формулу (6.122) для i-ro звена
тора. Согласно (6.114) и с учетом (6.121)
Ф?н = f dFfflq = - JR^iR- dm,
но Ro.j ==
Т&, следовательно, R^ =
— f TjRiR] dm ==
- Ti f RiR] dm.
Сравнивая (6.123) с (6.6), видим, что
манипуля-
(6.123)
(6.124)
и
При использовании (6.124) и (6.117) получаем
бАин/8^ = -1г(Т^(П(О)т77),
что дает возможность записать формулы
проекций главного момента и главного
инерции в системе i:
МЗИН = -1г(‘ад(П(1>)т77);
= -tr(TfH,(Q<6>)T77).
для расчета
вектора сил
(6.125)
Определение реакций в кинематических парах с по-
мощью принципа Даламбера. Рассмотрим звено i манипу-
лятора. Матрица полной системы сил и
звена i в соответствии с (6.112) равна
ф. = ф - + ф + + Ф? + ФГ
сил инерции
(6.126)
Главный момент и главный вектор такой системы сил
согласно принципу Даламбера равны нулю, поэтому в
соответствии с (6.118)
tr^(Q<°)T77) = 0.
(6.127)
Уравнение (6.127) — это запись принципа Даламбера в
матричной форме применительно к звену i манипулятора.
Считаем, что Ф^ — известная матрица. Это, безусловно,
справедливо для звена п — схвата, так как за ним нет звеньев
и матрица Ф+ равна нулю. Хотелось бы из (6.126) и (6.127)
по известным Ф/, Ф™, Ф? найти Ф,~. К сожалению, этого
221
1
сделать нельзя, так как ненулевых элементов матрицы Ф,"
вдвое больше, чем различных Q(n и, следовательно, чем
имеющихся уравнений (6.127) *. Тем не менее определим Ф,-
из условия
(6.128)
которому соответствуют 12 скалярных уравнений (по числу
ненулевых элементов матрицы ФД Уравнения (6.127) при
этом будут, безусловно, выполнены. Найденная из (6.128)
матрица Ф,- не будет отражать реально действующих в
паре (i — 1, i) сил. Однако главный момент и главный
вектор реально действующих сил с помощью полученной
таким образом матрицы Ф4 будут определяться правильно.
Можно сказать, что найденная Ф4- будет матрицей одной
из возможных систем сил, действующих в паре (i — 1, i) и
согласующихся с уравнениями кинетостатики твердых тел.
Реальную систему сил, действующих в паре (i — 1, i), мето-
дами кинетостатики твердых тел найти невозможно. Чтобы
ее найти, нужно учитывать деформации в парах, т. е. при-
менять методы теории упругости. Исходными для такого
расчета служат главный момент и главный вектор сил,
действующие в паре, которые матрица Ф^ дает правильно.
Учитывая вышесказанное, обозначим найденную из уравне-
ния (6.128) матрицу сил, действующих со стороны звена
i — 1 на звено i, не Ф<“, а (— ХД
Из (6.126) и (6.128) получаем
X/ = Ф, + Ф| + Ф-111. (6.129)
В правую часть (6.129) входит Ф^ — матрица сил, отли-
чающихся только знаком от тех сил, которые действуют
со стороны звена i на звено i + 1 и которые в системе
i + 1 описываются матрицей Ф^р В определение Ф/^i
входит столбцовая матрица К[+1. В системе i матрице
будет соответствовать матрица Rt = Ai+1Ri+1, а матрице
— матрица KJ+1A-+1. Поэтому при переходе от системы
i + 1 к системе i матрица сил Ф[+1 умножается справа
на А]+1. В итоге
(6.130)
* В матрице Ф|“ имеется 16 элементов, но одна строка состоит
из четырех нулей. Следовательно, неизвестных элементов 12, а
уравнений (6.127) всего шесть, столько, сколько степеней свободы
имеет твердое тело.
.222
1
и вместо (6.129) получим
(6.131)
Как и матрицу Ф, , матрицу Ф|+1 в рамках уравнений
механики абсолютно твердых тел найти невозможно. По-
этому заменим ее также матрицей (—Х<+1), которую можно
найти из уравнения, аналогичного (6.128), Ф^1 = 0 и которая
позволяет правильно рассчитать главный момент и главный
вектор сил в паре (i, i + 1). При этом вместо (6.131) получим
Ф?н.
(6.132)
Теперь рассмотрим звено i
и. Оно является последним,
ля него
= 0, поэтому
(6.133)
Найдя Хя, можно перейти к звену п —
и вычислить
фи»
Двигаясь по направлению к стойке, из (6.132) можно
последовательно вычислить А,й_2, ..., После этого для
любой пары (i — 1, i) можно вычислить главный момент и
главный вектор сил, действующих со стороны звена i — 1 на
звено i. Эти векторы удобно вычислить в системе i — 1,
потому что относительное движение тел в паре происходит
по оси пары, совпадающей с осью (или вокруг оси
zf_i). Для вычислений нужно знать матрицу ( —ФДД
Согласно (6.130), она равна Ф^ А]. Вместо матрицы Ф^
используем матрицу (— Xf), эквивалентную матрице Фг с точки
зрения главного момента и главного вектора. Таким обра-
зом, в формулу (6.118) на место Ф/ следует поставить
(-MD- При этом получим
Л/гл (i-1, о
= -tr(Mi(«(1))Tn-i);
(6.134)
/7ГЛ 1, о =: __ tr (MI (П(6))ТТ1- i) .
Здесь ° — проекция на ось х системы i - 1 главного
момента сил, действующих со стороны звена i — 1 на звено
i. Аналогичный смысл имеют прочие величины. Если пара
поступательная, то обобщенной силой является
если вращательная, — то Мтл^!»О. В любом случае обоб-
щенную силу в паре (i— 1, i) можно записать в виде
Qt
(6.135)
1
Разумеется, справедлива и формула
й = 1г(ФГЛМП-1).
(6.136)
Изложенный алгоритм вычисления реакций достаточно
универсален, поскольку может использоваться для самых
различных кинематических схем манипуляторов. Алгоритм
легко программируется для ЭВМ.
Пример 6.5. Определить реакции в кинематических парах
манипулятора, расчет ускорений которого выполнен в при-
мере 6.4. Дополнительно к данным примера 6.4 считаем,
что плоскостные моменты инерции звеньев имеют следую-
щие значения: = JlV = 0,5; = 10; ~ 0,5;
Начнем расчет с вычисления Фе — матрицы внешних сил,
которыми в данном примере являются силы тяжести Pi,
Р2, Рз. Поскольку начала координат систем, жестко свя-
занных со звеньями, совмещены в данном примере с цент-
рами тяжести звеньев, все моменты первого порядка в
формуле (6.104) равны нулю. Отличны от нуля только
моменты нулевого порядка, так что
ООО
ООО
ООО
_000
0
0
—10
о
Для расчета матриц Ф™ по формуле (6.124) нужно знать
7J и Hf. Сначала определим матрицы Hf. По-прежнему
учитывая специальный выбор начал координат, а также
равенство нулю центробежных моментов инерции, согласно
(6.8), получаем:
”0,5 0 0 0
0 0,5 0 0
0 0 10 о
0 0 0 1
Г 0 0 0 0 j
тт 0 0 0 0
«3 — I I
0 0 10
L 0 0 0 1 J
224
0,5
0,5
0
0
0,5
0
0
2
0
о
о
0
1
Переходим к вычислению 7]. Расчет ведем не по общим
формулам (6.80)*, в которые входят матрицы Uod и Uik> а
путем двукратного дифференцирования выражений = Al9
Т2 = AYA2i Т3 — AtA2A3 [см. (3.15) и (3.16)] и подстановки
в получающиеся после дифференцирования матрицы следую-
щих значений обобщенных координат: qr = q2 = q$ — 0;
41 = ^2 = 4з = 0; 5i = 3,3; q2 = 5; q3 = 10 (последние три
величины — результат решения примера 6.4):
0
0
о
о
о
о
о
о
о
о
о
о
о
Г 0
о
о
0
о
о
о
о
о
о
о
10
о
о
о
0
0
о
о
о
о
о
0
0
0 J
Далее,
инерции:
по
формуле (6.124)
вычисляем
матрицы сил
о
о
0.
о
о
о
о
О
О
О
-О
о
о
о
о
о
—10
По формулам (3.15) находим
~ q3 — 0:
матрицы At при qt = q2 =
По этим формулам расчет проводится на ЭВМ.
8 Механика промышл. роботов, кн. I
225
0
0
о
о
о
о
о
0
о
о
о
о
о
о
о
о
0
0
0
о
о
о
о
о
0
0
0
0
о
о
о
о
о
о
о
о
0
0
о
о
о
0
1
Теперь можно воспользоваться уравнениями (6.132) и
(6.133):
3
о
о
о
о
о
10
0J
о
о
о
о
О
-10
— 30
о
о
о
о
о
О
1
-40
О
о
о
о
о
о
О
о
- о
г о
о
о
о
о
о
о
о
Для вычисления реакции нужно
q2 = q3 = 0 эти матрицы
еще знать матрицы Tt.
такие:
При qt
О
О
О
о
о
о
о
о
о
о
о
о
о
о
О
О
О
О
О
Наконец, по формулам (6.134) находим главный момент
сил, действующих на звено i со стороны звена i — 1,
в проекциях на оси системы 0:
1) = -tr(Mi (ПП))Щ).
Полагая Tq = Е и используя (6.106), а также вычисленные
выше матрицы и Aif получим
Мгхл(р’ О = 0.
Аналогично, заменяя П(1) на П(2) и П<3), рассчитываем
остальные проекции главного момента:
ММЬ о = 0;
D = 10.
Последняя величина — не что иное, как обобщенная сила Qv
В примере 6.4 при расчете ускорений была задана обоб-
щенная сила Qi = 10. Здесь же эта величина получена по
заданным в условии задачи ускорениям.
226
Последовательно заменяя в (6.135) П(1> на О(4\ П(5>, П(6>,
f
получим значения главного вектора
F™jo, 1) = о, ° = 10, 0 = 40.
Аналогично находим реакции в других парах:
2) = 0; 2> = 0; М ^"-2| = 6,7;
£тя(1.2) _ 0. ргл(1,2) _ 10. £гл(1,2) _ 30;
адл(2.3) = 0; JW$<2’3> = 3,3; Л/Й(2-” = О;
2^л(2, 3) _ Q. /тгл(2, 3) _ £5* 3) = 10.
Подобные расчеты лучше поручить ЭВМ. Целесообразно
запрограммировать универсальный алгоритм определения
реакций, описанный выше, а рассмотренный пример исполь-
зовать в качестве тестового для отладки программы.
6.7. Уравнение Лагранжа I рода
В этом параграфе излагается способ получения уравнений
(6.133) с использованием неопределенных множителей
Лагранжа.
Если бы звенья манипулятора представляли собой сво-
бодные твердые тела, то для манипулятора можно было бы
написать уравнение
п
8А = tr
I (J (dFg.,
аад зад
(6.137)
являющееся общим уравнением динамики. В этом уравне-
нии 5Ko-i — матрица-столбец возможных перемещений точ-
ки приложения внешней силы и силы инерции при произ-
вольном малом перемещении звена i как свободного твер-
дого тела. Полагая в (6.135) < — 5 (Т^() и 5Ro-, =
= R?8Ti, получим
54 = trГ t (f + dF^W577) = 0,
или с учетом (6.114)
(6.138)
Однако звенья манипулятора — несвободные тела. Они
связаны друг с другом посредством кинематических пар.
8* 227
Пара (i — 1, i) накладывает связь на возможные изменения
матриц TJ-j и 7J:
следовательно,
87J- AI8Ti_i =0. (6.139)
Это соотношение записано при условии, что обобщен-
ная координата qt не варьируется (Л,- = const).
Чтобы записать уравнение (6.138) с учетом наложенных
связей (6.139), воспользуемся методом неопределенных мно-
жителей Лагранжа. Так как уравнения связи записаны для
матриц, то и множители Лагранжа — — будут матрицами.
Каждое уравнение связи умножается на свой множитель
Лагранжа и результат вычитается из выражения, стоящего
под знаком tr в (6.135):
tr £ [(ФГ + ФГ") 877 - \ (877 - ЛТ877-,)] = 0. (6.140)
i = 1
Преобразуем последнее слагаемое в (6.140):
п
С учетом этого преобразования уравнение (6.140) выглядит
так:
tr [X, 4Т877 - ^+1 Ат+1877 + £ (Ф| +
+ Ф"” - X, + Xj+14f+1)8TjT] = 0. (6.141)
Нетрудно видеть, что при выборе X,- в соответствии
с (6.132) и (6.133) в уравнении (6.141) обратятся в нуль
множители при всех 877, i = 1, 2, ..., п. Поскольку стойка
(звено 0) считается неподвижной, 8 То — 0, и уравнение
(6.141) обращается в тождество. Тем самым уравнения
(6.132) и (6.133), полученные выше при непосредственном
использовании принципа Даламбера, оказываются также
следствием применения метода множителей Лагранжа к
общему уравнению динамики.
228
1
Уравнение Лагранжа I рода для i-ro звена манипулятора
можно получить путем замены в (6.132) матрицы сил инер-
ции на выражение (6.124):
Tfli = Ф? + Xr+1XJ+1 - 1й+1 = 0. (6.142)
Если считать ускорения звеньев манипулятора извест-
ными, т. е. считать известной матрицу Те, то из уравнений
(6.142), как и из системы (6.132)—(6.133), можно найти мат-
рицы X* а следовательно, реакции связей.
Для лучшего понимания уравнения (6.142) полезно
сопоставить его с аналогичным уравнением, которое полу-
чится для манипулятора с поступательными парами, ориен-
тированными вдоль единой для всех звеньев оси z с единым
началом отсчета (рис. 6.2). Поставим в соответствие матрице
Ф- сумму проекций на ось z внешних сил, действующих
на звено i, матрице И, — массу звена i, матрице —
ускорение звена i. В данном случае At+i — единичная
матрица и множитель А]+1 можно игнорировать. Матрице
X, поставим в соответствие силу, действующую со стороны
звена i на звено i — 1. Тогда матрице (—М будет соот-
ветствовать сила, с которой звено i — 1 действует на звено i,
а матрице Х, + х — сила, действующая на звено i со стороны
звена i + 1. При таком переосмыслении величин уравнение
(6.142) оказывается ни чем иным, как выражением второго
закона Ньютона, записанным в проекции на ось z.
В отличие от уравнений Лагранжа II рода уравнение
Лагранжа I рода содержит наряду с заданными силами
также и силы реакций. Поэтому уравнения
Лагранжа I рода можно применять двояко. Если
ускорения найдены из других уравнений, напри- |z
мер из уравнений Лагранжа II рода, то их г,
можно использовать для расчета реакций. Так
как уравнение (6.142) фактически не отличается
от уравнений (6.132) —(6.133), то способ расчета
реакций будет тем же, что и при определении ДК
реакций по принципу Даламбера. I I
Однако можно использовать те же уравнения '"Ч I
(6.142) для расчета ускорений. С. этой целью I I
необходимо, во-первых, матрицу записать в XJ.
виде выражения (6.80). Во-вторых, последова- I
тельно переходя от n-го звена к и — 1 и т. д. | I
вплоть до стойки, нужно получить алгебраи- I j I
ческие выражения для всех матриц В. вы-
ражения для войдут как Ф|, так и TfcHfc; °
к — i^...^n. Наконец, используя (6.135), мож- Рис. 6.2
229
1
но получить уравнения, в левые части которых будут
входить обобщенные силы Qb а в правые
скорости и ускорения. Решив эти уравнения относительно
обобщенных ускорений, аналогично тому, как это было
описано в § 6.5, можно затем вернуться к расчету реакций.
После проведения выкладок для Х( получаются следую-
щие выражения:
обобщенные
(6.143)
Подставляя Х( в (6.135) и используя (6.80), получим
Последний сомножитель в (6.144) после проведения выкладок
упрощается:
что позволяет привести (6.144) при попутной замене i на у,
j на к, к на i к виду
и
(6.145)
Это уравнение отличается от (6.96), не считая формы
записи, тем, что предполагает учет действия на звенья
манипулятора произвольной системы внешних сил. Эти силы
представлены в (6.145) матрицами Ф®.
(6.96) предполагается действие только сил тяжести, которые
там учитываются матрицей G1.
В уравнении же
4
f
ч
i
I
1
1
1
6.8. Дополнительные факторы, влияющие
на динамику манипуляторов
Во всех предыдущих параграфах кинематические пары рас-
сматривались только в двух аспектах: как связи, ограничи-
вающие возможности относительного движения звеньев, и
230
1
как источники активных сил, приводящих звенья в отно-
сительное
движение. На самом деле второй
своей технической реализации приводит к тому,
аспект при
что манипу-
лятор в ряде
ной выше его
Реальными
случаев заметно отличается от рассмотрен-
идеализированной модели.
источниками относительного движения
звеньев могут быть электрические, гидравлические, пнев-
матические и другие двигатели. Пожалуй, ближе всего к
идеализированной модели манипуляторы с пневмоприво-
дами. Однако и при этих двигателях может оказаться не-
обходимым учитывать такие побочные эффекты, как жест-
кость шлангов, подводящих воздух, или реакции, возни-
кающие при стравливании отработавшего воздуха.
При гидроприводе масса звена манипулятора зависит от
количества масла, поступившего в цилиндр, т. е. зависит от
обобщенной координаты. Поэтому точный расчет движения
манипулятора с гидроприводом требует применения уравне-
ний динамики тел переменной массы.
При использовании электропривода кинематическая пара
(i — 1, i) оказывается фактически системой твердых тел, дви-
жущихся друг относительно друга. Первым телом этой
системы является ротор электродвигателя, вращающийся
относительно звена i — 1 (статора). Несмотря на малую
массу, ротор может обладать большой кинетической
энергией. Эта кинетическая энергия пропорциональна 4?,
т. е. она как бы присоединяется к кинетической энергии,
связанной с обобщенной скоростью qt. Следующие за
ротором тела — элементы редуктора — движутся с сущест-
венно меньшими скоростями. Впрочем, их кинетическая
энергия также имеет компоненту, пропорциональную qf,
так как они вместе с ротором и звеном i составляют
механическую систему с одной степенью свободы.
Для правильного описания вклада электропривода в
движение манипулятора важно прежде всего учесть вклад
электродвигателя с редуктором в тот член кинетической
энергии, который пропорционален qf. Обозначим его через
diql/l. В первом приближении между той обобщенной силой
Qb которая рассчитывалась выше по формуле (6.135), и той,
которую теперь следует считать обобщенной силой — назо-
существует следующая дифференциальная связь:
вем ее Qh —
(6.146)
Это соотношение получается из энергетического баланса.
Работа обобщенной силы Qt за единицу времени, согласно
определению обобщенной силы, равна Qtqt. Работа, со игр
24
1
создает гироскопический эффект.
шаемая за единицу времени обобщенной силой Qb теперь
имеющей смысл обобщенной реакции, равна Qiqb Разница
идет на повышение кинетической энергии. Приравнивая эту
разницу производной по времени от получим (6.146).
Однако помимо поправки (6.146) при более точном рас-
чете движения манипулятора с электроприводом необходимо
учитывать и другие эффекты. Быстрое вращение ротора
Далее, момент на валу
электродвигателя не является, строго говоря, непосредствен-
но управляемой величиной. Для изменения этого момента
нужно изменить магнитное поле, а оно, так же как и
механическая система, обладает энергией, а следовательно,
инерцией. Фактически нужно изменить токи и связанные с
ними магнитные поля в целой системе устройств, пере-
дающих команду на изменение момента на валу ротора.
Поэтому при более точном расчете электродвигатель
вместе с системой управления его движением оказыва-
ется системой не с одной, а, по крайней мере, с двумя
степенями свободы.
Глава 7
Принцип Гаусса
в динамике манипуляторов
Принцип Гаусса при анализе динамики манипуляторов
позволяет не формировать дифференциальные уравнения
движения, а найти действительные обобщенные ускорения,
минимизируя некоторую квадратичную форму — меру при-
нуждения, зависящую от этих ускорений. Метод применим
к системам с различными видами связей.
7.1. Вычисление функции принуждения
Рассмотрим движущуюся под действием активных сил
систему материальных точек, на которую наложены идеаль-
ные связи. Если бы в некоторый момент связи были
устранены, то точка v с массой mv, на которую действует
активная сила Fv, начала бы двигаться с ускорением,
равным Fv/m\ К. Ф. Гаусс назвал принуждением Zw сле-
дующую меру отклонения истинного движения от свобод-
ного [5]:
(7.1)
232
Принцип Гаусса состоит в том, что в каждый момент
времени истинное движение системы, находящейся под
действием активных сил и подчиненной идеальным связям,
отличается от всех кинематически возможных (т. е. соглас-
ных с теми же связями) движений, совершающихся из той
же начальной конфигурации и с теми же начальными
скоростями, тем свойством, что для истинного движения
мера отклонения от свободного движения, т. е. принуждение,
есть минимум [5]. Математически это означает, что для
истинного движения вариация принуждения равна нулю. т. е.
SZw = 0.
Вариация берется при неизменяемых координатах и ско-
ростях, а варьируются только ускорения:
6Zw = £ wiv
mv
(7.2)
Перепишем (7.1), сохраняя только члены, зависящие от
ускорений:
2
V
(73)
Считая, как и выше, что Rv и Г' — матрицы-столбцы,
соответствующие векторам Г и Г, запишем выражение
(7.1) в матричном виде:
(7.4)
Если какую-либо реакцию связей назвать активной силой
и включить во второе слагаемое выражения (7.4), то прин-
цип Гаусса остается справедливым. Реакции связей можно
не включать в принуждение Zw. Однако запрета на их
включение нет.
Вычислим функцию Zw для манипулятора. Просумми-
руем (7.4) по всем звеньям манипулятора, включая не-
подвижную стойку (звено 0):
Zw = XI -уtr 0 i rifn,) — tr (J дГо>(Ко,»У
I —- 0 L_
233
Так как R^ = Т&, a Rfa — RiT], то
> ss Q
tr [X (f R,R] dm,) 77]- tr [(f dF0,,KD 77] >.
(7.5)
Интеграл, стоящий в первом слагаемом этого выражения,
дает, согласно (6.8), матрицу Интеграл во втором
слагаемом — это матрица сил, действующих на звено i.
Если считать, что реакции включены в Zw, то матрица
сил имеет три слагаемых: Ф®, Ф,
и f 1 .... ..
Ф£ . Таким образом,
Ф»+)77]Г (7.6)
- tr(<DfT7)-tr[(<Dr
tr(7?H,
Согласно (6.130),
j = 0
(7.7)
I = о
что позволяет исключить Ф/ из последнего слагаемого (7.6):
tr (ФГ
Ф|+)
X 1г(Фг+1Л?+1Т7)
< = о
(7.8)
Поскольку звено 0 неподвижно, а звена и + 1 нет. и на
него, следовательно, звено п не действует, так что ST0 — О
и Ф“+1 = 0, в последней сумме выражения (7.8) индекс i
у Ф(" можно заменить на индекс i + 1 (при этом исчезнет
слагаемое с номером 0, появится с номером п + 1). После
подстановки (7.8) в (7.6) получим
1г(7[Н1Т1)-1г(Фе1Тг1)
(7.9)
Между Ti+i и Ti имеется следующая связь:
1+1 “ dt2
dt
234
1
Транспонируя, находим
7?+1 = С ,7] + AI+lQT+lTlql+l + 2AJ, АК. i , +
+ AI+lQ?+lT[rf+l. (7.10)
Используя (7.10) и отбрасывая члены, не содержащие
ускорений, а также члены с То и Ф“+ь равные нулю,
преобразуем (7.9) к виду
tr(TO77) - tr(Of77) - ^(ФГА^Т^)^
(7.Н)
С учетом (6.136) получим (7.11) в другой форме:
Zw —
tr (Т]Н4ТТ) - tr (ФГTI) - Q&
(7.12)
В этом выражении игнорируется движение деталей дви-
гателя и редуктора относительно того звена («статора»),
на котором закреплен корпус двигателя. Из-за высокой ско-
рости вращения и малой массы вклад движущихся дета-
лей («ротора») в функцию Zw определяется членом, соот-
ветствующим . первому слагаемому формулы (7.12):
71 tr (ТфотН.рот 77рОТ), где Tiрот - матрица, характеризующая
положение ротора в системе 0, а Н(рот — матрица инерции
ротора. Анализ этого выражения, проведенный в [39],
показывает, что в нем существенны два слагаемых:
(7.13)
Первое из этих слагаемых можно объединить с первым
членом в (7.12), считая, что Hi учитывает не только
инерцию i-ro звена манипулятора, но и инерцию i-ro ротора,
[ополняя (7.12) слагаемыми (7.13), получим
Zw —
tr(7JHiTJ) - tr (Ф| 77) + ^рот
(7.14)
где
(7.15)
1
Таккак добавление (или вычитание) членов, не содержащих
ускбрений, не влияет на решение задачи,
(745)
таком виде:
то представим
(7.16)
Такое представление дает возможность увидеть при
сопоставлении (7.16) с (7.1), что вклад в функцию при-
нуждения i-ro «ротора» аналогичен вкладу одной материаль-
ной точки: роль ускорения rv играет обобщенное ускорение
5i, роль сил Fv играет обобщенная сила Qb а роль массы
ту — величина dt.
Подставляя (7.16) в (7.14), получим общее выражение для
функции принуждения с учетом «статора» и «ротора»:
(7.17)
Следует помнить, что Я,- — это матрица инерции i-ro звена
в совокупности с i-м «ротором».
7.2. Минимизация принуждения
с учетом связей
Вычислим вариацию принуждения, варьируя в (7.17) только
tr
Wi4i ~ Qi) Sqi.
(7.18)
Поскольку при транспонировании матрицы ее след не
меняется, первое слагаемое в (7.18) можно транспонировать.
При этом ввиду симметричности матрицы Ht оно ста-
новится таким же, как второе слагаемое в (7.18):
(6ВДТТ)Т = TJHISTT = W77. (7.19)
С учетом (7.19) преобразуем (7.18) к виду
5Zw = £ tr [(7iHf — Ф|) 5TJ] + (diit — GO
i= 1
(7.20)
236
1
В этом выражении 87} и 8^
подчинены
условиям связи.
Эти условия связи получим, варьируя (7.10) и заменяя в
них i на i — 1:
— 877 + = 0.
(7.21)
По методу Лагранжа умножим проварьированные урав-
нения (7.21) — уравнения связи — на множители Лагранжа \
и вычтем эти произведения из. вариации принуждения
(7.20). Выбором можно добиться обращения в нуль мно-
жителя при 8TfT. В данном случае X,- — матрицы, и вы-
читать из 8Zw нужно след произведения матриц:
&Z w —
X,) 877 - м?577-1 -
- ЪАЦЦ Ц-,8ф] + (d^ - Q,) &qt. (7.22)
Введем еще один, равный нулю, множитель Лагранжа
Х„ +, = 0. (7.23)
Тогда
поскольку правая сумма отличается от левой равными нулю
слагаемыми Хп+1Л„Т+18ТИТ и Xi^4J8Tq, то
&Zw = £ tr [(7;я, - ФГ + X, - Xi+MI+1) 577
I
i= 1
- МЗДП-1Й’<] + " Qi)bqt.
Множитель при 8TJ равен нулю, если
7]//$ — Ф| + X,- — Xj+itIJ+i = 0j hn+i ~ 0.
(7.24)
(7.25)
Уравнение (7.25) совпадает с уравнением (6.142). Следо-
вательно — множители Лагранжа — совпадают с матри-
цами сил реакций. •
При выполнении (7.25) вариация принуждения принимает
вид
И
8Zw = — £ [tr (XjAJQfT^-i) 4- Qj — dig,] (7.26)
С помощью этого выражения можно получить уравнения
движения. С этой целью следует приравнять вариацию
237
6Zw нулю и
учесть при этом произвольность вариации oqt:
tr (X, Af flf 77-1) + Qi — dj^i = 0.
(7.27)
Уравнение (7.27) эквивалентно системе (6.135) и (6.145),
если иметь в виду, что формула (6.135) определяет теперь
(после учета двигателя с редуктором) не обобщенную силу
Qu а реакцию Qt. Уравнение (7.25) совпадает с уравнением
(6.142). Таким образом, мы снова получили уравнения Лаг-
ранжа I рода. Исключив из них можно получить
уравнения Лагранжа II рода. Однако принцип Гаусса можно
использовать и по-иному, не сводя его к уравнениям
Лагранжа I или II рода. Этому посвящены следующие
параграфы.
73. Алгоритм определения обобщенных
ускорений и реакций
Алгоритм решения второй задачи динамики манипуляторов
с силовым расчетом с помощью принципа Гаусса описан
в [39]. В этом алгоритме для каждого временного интер-
вала Дг обобщенные ускорения qt находятся не обраще-
нием матрицы а, как это было описано в § 6.5, а прямой
минимизацией функции Zw. Эта минимизация ведется ите-
рационно. В ходе итераций уточняются матрицы Tb X,
и обобщенные ускорения qt. С этой целью в «простран-
стве» обобщенных ускорений отыскивается направление,
вдоль которого функция Zw быстро убывает (для этого
вычисляются производные от Zw по 5Д Вдоль этого направ-
ления ускорение qt меняется до точки минимума
функции Zw. После этого отыскивается новое направ-
ление и т. д. При небольших интервалах At обычно
требуется всего лишь одна-две итерации для хорошего
определения qt. На первом интервале, когда нет хорошего
начального приближения для qb итераций требуется больше,
но не более п — числа звеньев манипулятора.
Опишем этот алгоритм подробнее. Согласно (7.26),
градиент функции Zw в «пространстве» ускорений имеет
проекциями величины
—— = - [tr (kAWTI-1) + Qi - d,qi].
Sqt
(7.28)
Наискорейшее убывание функции Zw происходит в
направлении, противоположном градиенту. Однако более
быстрое приближение к точке минимума дает метод сопря-
женных направлений, в котором движение к точке мини-
238
мума на каждом итерационном шаге к идет в направле-
нии, отклоняющемся от направления наискорейшего спуска,
с учетом направления на предыдущем шаге:
dZw
рюд^-о
(7.29)
г де А$к) — i-я проекция вектора, определяющего направление
движения на к-м шаге в «пространстве» обобщенных
ускорений; p(fc) — коэффициент памяти о предыдущем направ-
лении, равный
р<*> =
azw(k)
(7.30)
Движение
минимума функции Zw.
При движении, определяемом формулами (7.29) и (7.30),
обобщенное ускорение i-ro эвена манипулятора вычисляется
так:
по направлению Ag(ft) осуществляется до точки
Si= Sifc) + e(*)A5|k)
(7.31)
(eU) — пока произвольный параметр). При этом движении
меняются величины 7J. Поскольку они линейно связаны с
$j, закон их изменения аналогичен (7.31):
Tt = Tf + £wAt}«
(7.32)
причем АТ(к) — это те изменения матрицы Ть которые полу-
чаются при еЛ) =1, т. е. когда qt изменяется на Д£|*>
Расчет ATjfc) выполняется с помощью соотношений (7.10):
Д77<*> = Л/Т??} 4-ЛЩТ77_1д§|Л); Aflfc) = о. (7.33)
Величины А 7?^ подсчитываются в порядке возрастания
номеров i.
При подстановке (7.31) и (7.32) в (7.17) получим Zw как
функцию от ew:
Zw (е) = Zw (0) + W** + с ,
где
b = £ tr - Ф?) Д77'*>] + (dffl> - Qi) Д^>;
(7.34)
(7.35)
Y tr (ДТИя.ДТр’) + dt .
(7.36)
239
1
Вследствие квадратичной зависимости Zw от £ минимальное
значение функции Zw получится при
е(к) _. — jj/c. (7.37)
Полученные формулы позволяют следующим образом
описать алгоритм расчета обобщенных ускорений и реакций
в кинематических парах.
1. Для некоторого момента t заданы значения обобщен-
ных координат qt и обобщенных скоростей qi9 а также
матрицы Ф| внешних сил и обобщенные силы (2г. Задаем
приближенные значения обобщенных ускорений q\°\ На-
пример, в качестве $0) можно принять ускорения, вы-
численные в предыдущий расчетный момент времени. Если
t — первый из расчетных моментов времени, то можно
положить qt = 0.
2. Вычисляем, матрицы Т|0) по формулам (6.111) в порядке
возрастания номеров i (напомним, что Т}0) — 0).
3. Вычисляем матрицы М0) по формулам (7.25) в порядке
убывания номеров i (напомним, что 1 — 0).
4. Считая, что сделано к шагов для уточнения обоб-
щенных ускорений, сделаем (к +* 1)-й шаг. По формулам
(7.30) и (7.29) вычисляем 0(к) и &q\k). В порядке возраста-
ния номеров i по формуле (7.33) вычисляем А7?(к). В порядке
убывания номеров i по формуле
AXjk) = AM? ! - ATSfc)Hf; АХ?! х = 0, (7.38)
которая следует из (7.25), последовательно вычисляем AXf.
По формулам (7.37), (7.35), (7.36) вычисляем Е(к). По фор-
мулам (7.31) и (7.32) находим уточненные значения обоб-
щенных ускорений qt и матриц Tt:
$k+ = qf} 4- E(k) A*4(k); T[k+n = Tjk) + £(k) A*Mk). (739)
5. Если значение E(k) меньше наперед заданного зна-
чения е, то обобщенные ускорения, а вместе с ними и
матрицы М больше не уточняются. Если итерации окончены,
уравнения (7.25) и (7.27) выполнены, то М — матрицы сил
реакций и можно переходить к выполнению п. 6. Если
е(к) > е, то вычисления повторяются, начиная с п. 2.
6. Выбирается расчетный интервал At и для момента
t + At вычисляются обобщенные скорости и обобщенные
координаты по формулам:
4, (t + At) = 4i (0 + 'ii (О At;
(7.40)
(7.41)
240
При выборе интервала At следует проверять, не является ли
изменение на этом интервале величин qt и qt чрезмерно
большим.
Под чрезмерно большим изменением следует понимать
такое, при котором существенно изменяются матрицы Аь
используемые вместе с обобщенными скоростями qt в расче-
тах по п. 4. Естественно, что определение «существенно»
является нечетким и требования к выбору интервала Аг
зависят от желаемой точности расчета.
7. Если t + Аг меньше конечного момента времени, до
которого следует вести расчет, то повторяются вычисления,
начиная с п. 1, иначе расчет оканчивается.
Рассмотрим простейший манипулятор с поступа-
тельными парами (рис. 6.2). На его примере выше объяс-
нялось значение отдельных членов уравнения Лагранжа
I рода. Теперь на примере этого же манипулятора
продемонстрируем метод итерационного расчета ускорений.
Обозначая скалярные величины теми же буквами, что и
соответствующие им матрицы, запишем принуждение Zw
в виде
п
(7.42)
где Т} — координата начала системы i в системе 0 *; Ht —
масса звена i; Pf — вес звена i**; Qt — обобщенная сила,
а именно, сила, действующая на звено i со стороны
звена i — 1; qi — обобщенная координата, а именно, сдвиг
звена i относительно звена i — 1 вдоль оси zf. Выражение
(7.42) получается из (7.3), если считать поступательно дви-
жущиеся звенья материальными точками, или из (7.17) при
замене матриц скалярными величинами.
Условия связи
(7-43)
после двукратного дифференцирования запишутся так:
(7.44)
* Координатные оси zf всех систем i идут вертикально вверх,
так что Tt — это z-я координата начала системы i в системе О
(см. рис. 6.2).
** Проекция силы тяжести на ось zt равна (—РД
9 Механика промышл. роботов, кн. 1
241
1
Вариацию функции Zw с учетом связей получим, при-
меняя метод множителей Лагранжа:
SZw —
i= i
или, после перегруппировки членов, учитывая, что
6 То в О, X„+i = О,
SZw = f [(TJHi + Pi + К - Xi+1)STi ~ (Xf + Qi)8qJ. (7.45)
i = l
Из (7.45) следует, что вариация функции Zw при произ-
вольных совместимых со связями вариациях 6Т£, 6gf будет
равна нулю, если
Xf = - Tfli - р. + li+i; Хл+j = 0; (7.46)
Х$ = — Qi. (7.47)
Пример 7.1. Вычислить ускорения звеньев манипулятора
с двумя степенями свободы, считая, что оси zf посту-
пательных пар направлены вертикально вверх (рис. 6.2).
ДаНО! Н1 ~ 2 = 1> Р1 = Р2 = Ю> Q1 “ 02 = 30. При t — 0
обобщенные координаты и скорости равны qt = q2 — 0
И Qi ~~ ^2 — 0.
Из уравнений связи (7.43) последовательно, двигаясь от
стойки к схвату, находим = 0; Т2 = 0. В качестве на-
чальных приближений для qt принимаем нулевые значения:
q(P = о, д(20) — 0 (верхний индекс — номер итерации; на дан-
ном этапе расчета он равен нулю). Из дифференциальных
уравнений связи (7.44) последовательно, двигаясь от стойки
к схвату, находим Т(]0) = 0; Т(20) — 0. Из уравнений (7.46)
последовательно, двигаясь от схвата к стойке, вычисляем
Х<20) = -Ю, 1(10)= -20.
Конечно, в данном весьма частном случае можно было
бы сразу найти Xf из (7.47). Однако в общем случае из
(6.135), аналога (7.47), найти X, нельзя.
Теперь найденные значения qi9 Ti9 Х; следует итерационно
уточнять. Для этого на каждой итерации нужно выбрать
направление изменения вектора q с проекциями qlf q2 и
затем сделать оптимальный шаг в этом направлении.
Выбору направления помогает вид функции Zw, подлежащей
минимизации. Рекомендуется изменять q в основном против
градиента функции Zw, учитывая, однако, направление
изменения q на предыдущей итерации (метод сопряженных
направлений). Градиент функции Zw по q равен
242
AZw =
(7.48)
где К Q — столбцовые матрицы с элементами Хь Qi.
Изменение q против градиента — это движение в направ-
лении вектора (Q 4- X). Выгоднее считать приращение уско-
рения пропорциональным вектору Дд(к), идущему вдоль так
называемого сопряженного направления:
Aq(k) = (£<*> + Х(к)) 4- р(к) Д£(к"1);
Х(к))/(6(*~ °
(7.49)
Для нулевой итерации (к = 0) поправка р(к) равна нулю,
так что Д£(0) = б(0) 4- Х(0); Д510) = 10; Aq^ = 20.
Вычисляем поправки к 7}, X, при изменении q в выбран-
ном направлении на Дд(0). Согласно (7.44),
Д7?> = ДТ<к21 4- Д$к); ДТ(ок) = 0, (7.50)
и двигаясь от стойки к схвату, находим ДТ^ = 10;
дН0) = 30. Согласно (7.46),
ДХ<к) = - Д T^Hf 4- ДХ$ i, ДХ$.1=0, (7.51)
и двигаясь от схвата к стойке, находим ДХ(20) = —30,
А Х(г0) = - 40.
Оптимальное смещение — это смешение на sAq вдоль
выбранного направления Aq. При таком смещении функ-
ция Zw изменяется, согласно (7.42), на
ДИи> = £ [Н(ДТ?е2 + (7JH, + ДТ;£ - Qi Д&е].
t = 1
Минимум AZw достигается в полной аналогии с (6.37) при
£ (T^Hi + Pt) ДТ<» - е,.Д?<»
1=1
e<w = - —-------------------------------. (7.52)
f яддт;*')2
Вычисляя по (7.52) оптимальное смещение, находим, что
е(0> = 0,5.
Теперь можно найти значения q(, Th
= д<о* + e'W/” = 5; «V = «Т + е<0’ДЙ°» = 10;
= Т<°> + = 5; Т[п = Т[о> + 8(0)Дт'2°’ = 15;
9* 243
XV> = XV” + e<0) AXV” = - 20 - 0,5 • 40 = - 40;
XV» = XV” + е<0’ДХ<2°> = -10 - 0,5-30= -25.
Следующая итерация дает:
0’1’ = 0,25; ДдУ* = -7,5; AgV‘ = 10; ,Д7?’ = -7,5; ДТ*»’ = 2,5;
AXV” = -2,5; ДХ',” = 5; s'1’ = 2;
5V> = q',1’ + e^AgV* = 5 - 2 • 7,5 = -10; q'22) = 10 + 2 • 10 = 30;
Т?» = 5 - 2-7,5 = -10; T?’ = 15 + 2-2,5 = 20;
XV’ = -40 + 2-5= -30; X*?’= -25 - 2-2,5= -30.
b
Вторая итерация дала точные значения qb Ть Хь чтр
соответствует теории, поскольку в данном случае и = 1.
Правильность ответов проверяется подстановкой их в (7.44),
(7.46), (7.47).
В данном примере взаимодействие звеньев ограничи-
вается передачей активных сил Qb т. е. реакций нет. Поэто-
му найденные можно использовать только для проверки
правильности расчета по формуле (7.47). В общем случае
найденные матрицы можно использовать для расчета
по формуле (6.134) реакций в парах манипулятора.
Сравнение рассмотренных уравнений, описывающих дина-
мику манипулятора. Примененная здесь матричная форма
записи уравнений движения манипулятора позволяет срав-
нительно просто составить программы для ЭВМ. Однако
по числу арифметических операций, выполняемых ЭВМ
в ходе решения первой или второй задачи динамики, эти
программы нельзя считать экономными. Проделав предва-
рительную аналитическую работу, учитывающую особен-
ности конкретного манипулятора, можно составить програм-
му, которая потребует заметно меньшего машинного вре-
мени. С этой точки зрения уравнения Лагранжа I рода,
принцип Даламбера, принцип Гаусса, поскольку-они факти-
чески ведут к одной и гой же системе линейных уравнений j
относительно ускорений и реакций связей, дают возмож- |
ность составить равноценные программы. Поэтому пред-
почтение одного из них есть вопрос привычки к его при-
менению. У принципа Гаусса, казалось бы, есть дополни-
тельное преимущество, поскольку он «подсказывает» поиск
итерационных методов решения получаемых линейных урав-
нений. Подсказка связана с тем, что искомые ускорения
минимизируют принуждение. Поэтому при очередном уточ-
нении ускорений -полезно вычислить производные от при-
нуждения по ускорениям и использовать их для поиска
244
1
поправки к ускорениям (как это было показано в примере 5.1).
Но современная вычислительная математика имеет большой
арсенал подобных приемов итерационного решения, и их
можно применять независимо от того, каким способом
получена система линейных уравнений.
Особняком стоят уравнения Лагранжа II рода. Из них
сознательно исключены реакции связей в расчете на то, что
они не потребуются. Поэтому эти уравнения целесообразно
применять в тех Случаях, когда реакций связей действи-
тельно не представляют интереса.
Глава 8
Обратные задачи динамики
манипуляторов
(определение структуры сил
и постоянных параметров системы)
Под обратной задачей динамики понимается определение
сил и параметров системы по заданным свойствам дви-
жения [19]. Известны классические задачи Ньютона—Берт-
рана об определении сил, под действием которых точка
совершает движение по коническим сечениям, Суслова —
Жуковского об определении силовых функций, допускающих
•заданные интегралы, Мещерского об определении законов
изменения массы, необходимых для осуществления движе-
ния точки по заданной траектории, и др. [12].
Решение обратных задач динамики позволяет определить
основные требования, которые должны быть наложены на
систему, чтобы движение с заданными свойствами было
возможным, а также законы изменения и структуру управ-
ляющих сил.
Если программа движения системы задана аналитически,
то, отождествляя ее со связями, используемыми в анали-
тической механике, можно построить уравнения движения
методом аналитической механики и определить структуру
управляющих сил. Так, русский ученый Я. И. Грдина,
по-видимому, впервые рассматривал целенаправленные дви-
жения живых организмов с позиций аналитической меха-
ники. Он ввел связи определенного вида, содержащие пере-
менные параметры, и для описания движения живых орга-
низмов применил методы аналитической механики. В. В. Доб-
ронравов показал, что управляемые механические системы
245
можно рассматривать как системы с неголономными свя-
зями. Г. В. Коренев рассматривал различные задачи осу-
ществления целенаправленных движений твердого тела и
управляемых манипуляторов [17].
Следует, однако, отметить, что во всех указанных выше
работах при построении уравнений движения структура
механической системы предполагалась известной.
Наиболее общий подход к рассматриваемой задаче мо-
жет быть сделан на основе разработанного Н. П. Еруги-
ным математического метода построения всего множества
дифференциальных уравнений по заданному частному реше-
нию или интегральному многообразию.
В этом случае не требуется делать никаких предполо-
жений относительно структуры системы дифференциальных
уравнений. Вид программы движения, которая в данном
случае принимается за заданное интегральное многообра-
зие, может быть весьма общим.
При этом могут быть учтены дополнительные требова-
ния к системе, например требование устойчивости или
оптимальности заданного движения.
В работах А. С. Галиуллина и его сотрудников [12]
дано развитие и применение метода
циальных уравнений по заданному
обратных задач динамики и показана эффективность этого
метода.
Б. И. Петровым,
боте [38] была показана возможность применения резуль-
татов решения обратных задач динамики для построения
алгоритмов и систем
Обратные задачи динамики манипуляторов были рас-
смотрены в работах [21, 19, 10, 3] с целью определения
управляющих сил, реализующих заданное программное дви-
жение, и построения системы управления. Было показано,
что обратные задачи динамики, получившие свое развитие
в механике, нашли применение и для построения систем
и алгоритмов управления манипуляторами.
Ниже рассмотрим некоторые основные типы обратных
задач динамики манипуляторов.
I
1
г
построения дифферен-
решению к решению
. Д. Крутько, Е. П. Поповым в ра-
управления.
1
/
i
г
1
i
1
1
8.1. Построение уравнений движения
манипулятора по дифференциальной программе
Пусть задана программа движения выходного звена меха-
низма в виде
Фк (^1, • • • > ^1, • • • > Хм) = (8*1)
т; т
т) — обоб-
щенные координаты твердого тела и их производные по
времени.
Одной из важных особенностей обратных задач дина-
мики является то, что решение для управляющих сил на-
ходится обычно в форме зависимости от состояния сис-
темы, т. е. от обобщенных координат и их производных,
в отличие от известной первой задачи динамики, когда
силы определяются в функции времени.
Определение управляющих сил в функции состояния сис-
темы позволяет организовать управление с использованием
сигналов датчиков состояния в режиме обратной связи. Этим
обусловлено важное значение обратных задач динамики для
управляемых систем, в том числе для манипуляционных
роботов.
Построим дифференциальные уравнения движения меха-
низма, для которого программа (8.1) является их частным
решением, что означает, что заданная программа может
быть осуществлена этим механизмом.
Рассмотрим манипулятор с незамкнутой кинематической
цепью, обладающей п степенями свободы (см. рис. 5.1).
Предполагаем, что в шарнирах механизма приложены
движущие моменты Mj (j = 1
звенья, начиная от стойки. Для решения задачи программу
(8.1), заданную в координатах объекта, переведем в форму,
содержащую обобщенные координаты механизма.
Установим связь между обобщенными координатами
механизма qn и обобщенными координатами твер-
дого тела хь используя матричные уравнения связи. Как
было показано выше, имеем
Л1о1^12, ...»
Приравнивая соответствующие элементы матриц в левой
и правой частях уравнения (8.2), получим 12 уравнений
вида
п). Пронумеруем его
(8.2)
= -^Ои-
Чп> •••» ^т) ’
I = 1, . . . , 12.
Из этих двенадцати уравнений независимыми являются
лишь шесть. Разрешив эти уравнения относительно xif най-
дем функции
Xi = xt (q 1
«J
(8.3)
Дифференцируя (8.3) по времени, получим
(8.4)
Используя (8.1), (8.3), (8.4), найдем представление про-
граммы движения тела в виде
Fk ((?!> • • •» Qnt • • • > (?л)>
(8.5)
уравнения динамики манипулятора представим в виде
^s) Qi + Д/ (Qb Qb Cs) — Qp
(8.6)
где cs — постоянные параметры механизма; Qj — управляю-
щие воздействия приводов.
Эти уравнения запишем в форме Коши, обозначив
ki = Рь тогда система (8.6) получает вид
п
Qi = Рь Е (Qi> cs) Pi + В, (qb pb cs) = Uj.
i~ 1
(8.7)
Разрешая линейную систему уравнений (8.7) относительно
pif получим
Л
4i~Pb pt- fi(qb pb cs) + X pb cs)uj. (8.8)
j J
Теперь задача состоит в том, чтобы найти правые части
уравнений (8.8), такие, чтобы выражение программы (8.1)
являлось частным решением этой системы дифференциаль-
ных уравнений.
Используя метод построения дифференциальных уравне-
ний по заданному частному решению, можно записать
условие осуществимости программы вида (8.1)
(8.9)
где Rk (fb qb Pi) — произвольные функции, обращающиеся
в нуль на многообразии Fk (Л = 1, ..., г).
248
1
Система уравнений (8.8) является линейной относитель-
но неизвестных функций::
«I "• [Zi (<?Ь Pl, cj
i
(8.10)
представляющих искомые правые части системы дифферен-
циальных уравнений (8.8).
Число известных искомых функций щ равно и, а число
линейных уравнений — г, причем г < п. Решая полученную
систему уравнений (8.9) относительно неизвестных аь полу-
чим искомое множество систем дифференциальных уравне-
ний:
п
(8.И)
где А — функциональный определитель системы (8.10) по qb
рь не равный нулю в силу независимости функций Fk;
Дц — алгебраическое дополнение элемента определителя А;
Afcs — определитель, полученный из определителя А заменой
его fc-ro столбца s-м столбцом матрицы
_ dpi
.. Построенная система уравнений (8.11) задает основные
условия, налагаемые программой как на постоянные пара-
метры механизма cs, так и на управляющие воздействия,
входящие в функции at (i = 1, ..., г).
Оставшиеся функции Fs (s — г 4- 1, ..., п) являются про-
извольными и вместе с функциями Rk (к = 1, ..., г), обра-
щающимися в нуль на многообразии Fk, могут быть
использованы для выполнения дополнительных условий.
Такими дополнительными условиями могут быть условия
устойчивости, оптимальности, точности и т. д.
Определение управляющих воздействий. Рассмотрим важ-
ный частный случай задачи построения уравнений движе-
ния, задачу определения управляющих воздействий, осу-
ществляющих движение механизма по заданной программе.
В этом случае функции ft(qb ph cs) и коэффициенты
Ьу(^, pb cs) считаем известными и определению подлежат
лишь уравнения Uj.
249.
п) система урав-
Относительно уравнений (/== 1, 2,.
нений (8.9) есть система г линейных уравнений с и не-
известными. Поэтому управляющие воздействия Uj можно
определить, вводя некоторые дополнительные условия опти-
мальности. За условие оптимальности можно принять усло-
вие минимума «принуждения», или нормы Uj, представляю-
щей собой функцию вида [8]
(8.12)
Очевидно, что задача определения Uj в данном случае
представляет задачу на условный экстремум функционала
(8.12) при условиях (8.9). Применяя для решения этой за-
дачи метод неопределенных коэффициентов Лагранжа, по-
лучим следующие выражения для управляющих воздействий
(8.13)
где Rj — произвольные функции, обращающиеся в нуль на
многообразии Fk; — неопределенные множители Лагран-
жа.
Множители Лагранжа определяются из линейной систе-
мы (8.9) после подстановки в нее выражения для Uj из (8.13).
В полученные выражения для управляющих воздействий
входят произвольные функции Rj, обращающиеся в нуль
на программе движения. Из этого следует, что для реали-
зации программного движения необходимым условием явля-
ются не абсолютные величины управляющих сил, а соот-
ношения между ними. Если считать, что движение проис-
ходит по заданной программе точно без отклонений, то
можно положить Rj = 0. В этом случае найдем «номиналь-
ные» силы, необходимые для реализации заданного про-
граммного движения. Однако в действительности всегда
имеет место отклонение от программы движения, например
в начальный момент времени систему практически нельзя
привести точно в состояние, соответствующее программе.
Поэтому, чтобы в действительности обеспечить выполне-
ние заданной программы движения, управляющие силы
следует находить с учетом условий устойчивости. Решение
таких задач будет рассмотрено ниже в § 8.3 —8.5.
Построение уравнений
В || П ± П по заданной траектории с постоянной скоростью.
250
вижения манипулятора типа
I
I
)
г
i
г
1
I
1
1
Определим управляющие усилия и моменты для манипу-
лятора, осуществляющего движение по эллиптической тра-
ектории, произвольно расположенной в пространстве, с по-
стоянной по модулю скоростью.
Дифференциальные уравнения движения такого механиз-
ма имеют вид:
Лф1 + m3s3(pi + 2m353(p1s3 =
(ТИ2 + Wl3) $2 ~ ^2 ^2^2»
m3*s3 - m3s3<pj = и3 - b3s2, (8.14)
где Jg = Ji + J2 + J3 (J 1, J29 J3 “ соответственно моменты
инерции звеньев 1, 2, 3 относительно центральных осей
звеньев, параллельных оси z); т2, т3 — массы звеньев 2, 3;
Фь s2, $з — обобщенные координаты механизма; ult м2, и3 —
управляющие воздействия; b19 Ь2, Ь3 — коэффициенты вяз-
кого трения.
Уравнения (8.14) представим в виде уравнений первого
порядка. Обозначим Ф1 = pi; <Pi = qi9 s2 — р2; s2 — q2\ s3 =
= Рз J = q3.
Получим
9i = Pi > Pi ~
Hi - biPl - 2m3qj - p^
Jz + шз9з
bnwi;
92 = P2; Р2 =
9з = Рз; Рз =
m3
зРз + ^з9зР1
m3
(8.15)
Уравнение эллиптической траектории в декартовых коор-
динатах зададим в виде пересечения кругового цилиндра
и плоскости:
Ах + By + Cz + D - 0; (х - х0)2 + (у - Уо)2 ~
(8.16)
В обобщенных координатах уравнение траектории имеет
вид
Fi = Aq3 cos qi + Bq3 sin q + Cq2 + D = 0;
F2 == qj + qj - 2g3 (x0 cos qi + y0 sin q^ -
- 2^3 (x0 cos qi + y0 sin qi) - r2 - C2 ® 0;
(8.17)
Uz ~ ^2P2
m2
Условие постоянства линейной скорости схвата при
движении по траектории имеет вид
1
v2 = const = C3,
или
_2 • М2Л2
Рз + Р1<Ь “
(8.18)
Два первых уравнения программы, согласно (8.9), дают
следующие два уравнения:
Чк
(8.19)
Дифференцируя (8.19) по времени, получим
dt у dqt
= Rk (fc=l, 2).
(8.20)
Подставляя значение pt из правых частей уравнений (8.15)
в (8.20), получим два уравнения, связывающие и19 и3:
^-(/1 + bllUl) + ^-(t>2 + b22U2) + + ЬЗЭ"3) =
(к = 1, 2).
(8.21)
Третье уравнение получим, используя выражение для F
согласно (8.18):
dF3 . dF
Р3 + др
dF3 .
dpi
dF3 .
дрз Р
48.22)
или
2pl^3 (fl + ^llwll) + 2р2 (fl + ^22и1) + 2р3 (/э 4- Ь33и3) +
+ 2q3plp3 = R3. (8.23)
Частные производные, входящие в уравнения (8.21), (8.22),
имеют вид:
dF
— Aq3 sin qt + Bq3 cos ;
dFj
^3
= A cos (h 4- В sin qi;
1
dF2 dF2
-г—=2g3x0 sin q2-2q3cosqt; -— = 2q2;
dq2
dF
2 (x0 cos qt
Уо sin qi);
dF3 _ dF3
dpi dq2
dF3 „
aFT= P2’
— Zpiqh
t)F3 o
aFT " 2рз'
Из трех линейных уравнений (8.21)—(8.23) легко нахо-
дятся управляющие силы, необходимые для реализации
заданной программы.
8.2. Построение уравнений движения
манипулятора по голономной
программе
Частным случаем рассмотренной выше дифференциальной
программы является конечная голономная программа.
В этом случае в выражение программы входят лишь*
обобщенные координаты объекта. Выражая обобщенные
координаты объекта через обобщенные координаты меха-
низма, получим
Fk(q19 •••, qn) = ® (fc= 1, ..., г; г <и). (8.24)
9
Построим множество систем дифференциальных уравне-
ний механизма, для которого многообразие Fk(q19 ..., qn)
является интегральным. Уравнение будем строить в виде (8.8).
Основное условие осуществимости программы (8.24), опи-
сываемой уравнениями (8.8), получим следующим образом.
Дифференцируя программу (8.24) по времени, получим:
п п
^2[Fk(q19...,qn) V dFk .. V cl / dFk \
/7 / j у ()q. JPi
i = 1 i = 1
Л
(8.25)
1
Подставляя в (8.25) выражение для pt из (8.8), получим
основное условие осуществимости программы
п п
(8.26)
где Rk (Ft, ...» Fr; Fb ...» Fr) - функции, обращающиеся
в нуль на многообразии Fk и Fk.
Из системы (8.26) определяются правые части дифферен-
п
циальных уравнений ft (qb pit cs) + X P*» cs)uj- При on-
J = 1
ределении только управляющих воздействий коэффициенты
fi Рь cs) и Ьц (qb Рь cs) считаем заданными и определению
подлежат лишь управления Uj.
Определение управляющих сил промышленного робота ти-
па В _L В ± П при движении по голономной программе. По-
добную схему имеет механизм транспортных движений оте-
чественных роботов «Универсал-50» и «Универсал-15», а так-
же американского робота «Unimate».
Используя метод Лагранжа, получим уравнение движения
такого механизма. Кинетическая энергия механизма вы-
ражается так:
Т = 7г [Ai + (Jz2 + Аз) cos2 Ф2 + (Jy2 + Аз) sin2 <р2] <pf +
+ 7г (Аг + Аз) Фг + 7г^г/г (ф i sin2 ф2 + <pi) +
+ 7г ($2ф 1 sin ф2 + ф 2s2 + s2) m3, (8.27)
где Ai, Аг, Аз, Jy2> Jy3 ~ моменты инерции звеньев отно-
сительно осей z, у систем, связанных со звеньями 1, 2, 3
в центрах масс; фь ф2, s — обобщенные координаты сис-
темы.
Уравнения движения этого механизма под действием
заданных сил имеют вид:
Ф1 [Ai + (Аг + Аз) cos2 ф2 + (Аг + Аз)sin2 Фг +
+ (m2l2 + w3s2) sin ф2] + ф1ф2 [Аг + Jy3 - Аг ~ Аз +
+ ml2 + W3S2] sin2 ф2 + 2m3s sin2 ф25ф! = — bi<pt; (8.28)
254
Ф2 [(Лс2 + Лз) + mill + w3s2] + 2m3ssq)2 -
- Ф1 [Jy2 + ^уЗ — Л2 - Лз + ™2I2 + W3S2] sin <р2 cos <р2 =
= p2l2 sin Ф2 - 62ф2 + М2п + p3s sin Ф2; (8.28)
m3s — w3<pi sin2 ф2я — m3s<?2 — — pcos ф2 — b3s + Q3n.
Представим систему (8.28) в гамильтоновой форме.
Обозначим ф1=х1; Ф1=х2; фх = х2; ф2 = .х3; ф2 х4;
Фг = ^45 s = х5; s = х6; s = х6. Уравнения движения меха-
низма (8.28) в последних обозначениях получают вид:
Xj — х2; х2 — f2 + Ь2и2\ х3 — х4;
% 4 ~ fl + ^4.114.', Х5 = XgJ Xg — fe 4-
(8.29)
где
1
fl = - —[*2*4\Jy2 + Jy3 - Jzl - Лз + ™2^2 + fH3S2) sin 2x3
al
+ (Pih + p3x5)sinx3];
1
/4 = — ~ [2wi3X5X6X4 — X2 (Jy2 + Jy3 — J21 ~~ J23 +
a2
+ m2l2 + w3xj) sin x3 cos x3];
/б = — 0^3*2 sin2 x3x5 + m3x5x4 - p3 cos x4);
a3
«1 = J2i+ (Jz2 + Лз) cos2 x3 + (Jy3 + Jy2) sin2 x3 +
+ (w2/l + m3xi)sin2x3;
«2 = (JX2 + Лз) + mill + ^3^5; a3 = m3;
*2 = b4 = h4 = u2 = 2i; U4 = Q2; u6 = Q3.
«1 a2 a3
Рассмотрим вопрос об определении управляющих уси-
лий, при которых схват осуществляет движение по парабо-
лической траектории. Уравнение параболы зададим как
пересечение параболоида с плоскостью. Уравнение про-
граммы имеет вид
х2 у2
Л = У - С = 0; F2 = -р- + _ 2z = 0; р > 0; q > 0. (8.30)
Запишем выражение программы в обобщенных коорди-
натах манипулятора
1
s sin <p2 cos — C — 0;
F2 = (tf2s2 cos2 <px + p2s2 sin2 фО sin2 <p2 —
— p2q2 (hr+ s cos <p2) = 0,
(8.31)
где h19 С ~ постоянные.
Условие выполнимости программы имеет вид:
4»
dFj . dF2 , dFi
“Т7 Рз "a P® ~~
ox± ox$ dx$
(8.32)
Заменяя plf p3, pSi получим
(8.33)
Продифференцируем (8.33) по времени и подставим
вместо х2, х4, х6 их выражения из (8.29). Получаем два
уравнения вида
f 12 + ~^~^22U2 + А) + (Ь44П4 + /4) +
1 3
(^66м6 + /б) — &к
(fc — 1, 2)j
(8.34)
Искомые значения управляющих воздействий щ (i = 2, 4,
6) будем искать при условии обращения в минимум их
нормы, т. е. величины
z*w = w2 + u4 + Ug.
(8.35)
Необходимое условие минимума (8.35) при условиях (8.34)
имеет вид
= 2и2 + Xj ^±b22 + Х2 ^-Ъ22 = 0;
vU 2 vXj
dF dF 1 г 2 и л»
ST - 2u* + 11 -^Ьм + “ °’
(8.36)
1
dFt , . dF2 > _ n
-Г 1>66 + ^2 -X-----^66 Ц
ox$ ox$
(836)
где Xt и X2 — неопределенные множители Лагранжа.
Из пяти уравнений (836) и (834) находим и2, и4, иб
и два множителя Лагранжа и Х2.
Управляющие воздействия выражаются следующим об-
разом :
«в =
(837)
Неопределенные коэффициенты и Х2 находятся из двух
линейных уравнений связи (834). Частные производные,
входящие в (8.37), выражаются так:
— Ах5 sin х3 sin Xi — Вх5 sin х3 cos хх;
dFi
X5 COS X3 (Л COS Xi
В sin xt) — Cxs sin x3;
(838)
dFi
= A sin x3 (cos Xi — sin xt) + C cos x3.
Если рассматривать точное движение схвата по задан-
ной программе, то можно в формуле (834) принять Rk = О
(к = 1, 2).
8.3. Определение управляющих сил
при позиционировании промышленного
робота
Рассмотрим задачи построения выражений для управляю-
щих сил, реализующих заданную программу с учетом
условий устойчивости. Вначале рассмотрим случай простей-
шей программы перевода манипулятора в некоторое поло-
жение, определяемое постоянными координатами.
Пусть требуется перевести манипулятор (см. рис. 5.2)
из некоторого произвольного положения, определяемого
257
обобщенными координатами ф1} s2, s3t в некоторое задан-
ное положение, определяемое обобщенными координатами
Уравнения динамики этого манипулятора имеют вид
+ J2 + ^з)Ф1 +fn3sj(pi
2m3ss3<pi = Qt;
(8.39)
m3 s3 - m353cp? = 63,
где Jj (i = 1, 2, 3) — моменты инерции звена i относительно
центральной оси, параллельной оси вращательной пары Л;
т2, тз — массы звеньев 2 и 3; Qj (j — 1, 2, 3) — обобщенные
силы.
Найдем такие управляющие силы Qj, чтобы они обеспе-
чивали изменение каждой из обобщенных координат <рь $2,
s3 в соответствии с законом
<pj — ф* = Cnexnt 4- cl2exi2r;
c22eW;
(8.40)
где Ху (i, j= 1, 2) — отрицательные константы, определяю-
щие характер переходного процесса; су (г, 7=1, 2, 3) —
постоянные, зависящие от начальных условий.
Этот закон движения обеспечивает выполнение постав-
ленного условия вывода манипулятора в заданное положе-
ние в рабочем пространстве.
Дифференцируя дважды (8.40) по времени, получим
<Р1
C12^12^12t\
(8.41)
<Pi
(8.42)
I
I
г
1
I
1
Исключая из систем (8.40), (8.41) члены соеМ (i, j — 1, 2, 3)
и подставляя их в (8.42), получим законы изменения обоб-
щенных ускорений, при которых реализуются законы изме-
нения обобщенных координат вида (8.40):
фП;
Ф1 — Phi
^22) S2
А32) S3
(8.43)
I
Следует отметить, что выражения (8.43) представляют
собой линейные дифференциальные уравнения 2-го порядка,
имеющие решения выбранного вида (8.40).
Таким образом, чтобы система уравнений (8.39) имела
решение вида (8.40), необходимо выбрать управляющие силы
Qj (j = 1, 2, 3), такие, чтобы они систему (8.39) превращали
в систему вида (8.43).
Для этого решим совместно уравнения (8.39) и (8.43)
относительно Qjt получим
Qi — (Jz 4- w3sl)[(Xn + Xi2)<Pi — (ф1 ” Ф1) +2т3553фх;
& = (т2 + w3) [(Х2х + ^22) s2 — ^21X22 (s2 — s*)]; (8.44)
Q3 — ™3 [(X31 4- ^32) S3 “ ^-31^-32 (s3 ~ 5*) ~ ^35зФ1»
где Jx = Ji 4- J2 4- J3.
Прямой подстановкой выражений (8.44) для управляющих
сил в уравнения динамики (8.39) можно показать, что урав-
нения динамики (8.39) при управлениях (8.44) в любой
момент времени эквивалентны системе линейных уравнений
вида (8.43), т. е. имеют асимптотически устойчивое решение
вида (8.40), реализующее заданное условие вывода манипу-
лятора в заданную точку рабочего пространства.
8.4. Определение управляющих сил
при выводе схвата манипулятора
в заданную точку пространства
с заданной скоростью*
Чтобы осуществить движение схвата манипулятора по за-
данной траектории, необходимо вначале вывести его в не-
которую точку этой траектории с вектором линейной ско-
рости, касательным к траектории. Пусть в исходном состоя-
нии схват имел координаты rk(xkykzk), а его линейная ско-
рость была равна нулю. Требуется определить управляю-
щие силы, осуществившие перевод манипулятора в состоя-
ние, в котором схват занял положение rk(xk> ук, zk) с век-
тором скорости vk, причем движение было устойчивым
и носило плавный характер.
Дифференциальное уравнение движения манипулятора
с п степенями свободы имеет вид
* В данном разделе изложены результаты, полученные совмест-
но с Е. В. Письменной
А. В. Кузьмичевым.
259
(8.45)
I Aj (<?j. Cs) 4i + В J (qb 4i) = Qj U = 1»
где cs (s = 1, ..., m) — постоянные параметры механизма.
Это уравнение для решения поставленной задачи удобно
представить в векторной форме
Я й) q + ё (q, q) = & (8.46)
где A (q) — матрица размером п х п; ё (q, q) — вектор раз-
мером 1 хи; q, q, q — векторы размером 1 х и.
Связь обобщенных координат qt с декартовыми коорди-
натами схвата тоже можно представить в векторном виде
х = F (q), (8.47)
где х = [х, у, z]T; F(q) — вектор размером 1 х п.
Выражение (8.47) определяется конкретной схемой мани-
пулятора. Дифференцируя (8.47) дважды по времени, полу-
чим
х = J (q) q; (8.48)
х = J (q) q + p (q, q), (8.49)
где J (q) — матрица Якоби, имеющая вид
L dq J
P(4, q) — вектор размером 1 x n.
Для обеспечения плавности нарастания скорости примем
закон ее изменения в виде
х = Ф (t)(v0 “ ~ v<h (8.50)
где Ф (t) — матрица, имеющая вид
Ф (г) = exp At.
Для устойчивости движения системы все собственные
значения матрицы А должны быть либо отрицательными,
либо иметь отрицательную действительную часть.
Запишем решение дифференциального уравнения (8.49)
в виде
х (t) = А 1 [/ - ф (0] (4 - 4) + 4* + *о-
(8.51)
Так как вектор скорости схвата в начальный момент вре-
мени равен нулю, получим
х (г) = А 1
[/ - Ф (г)] 4
х0.
(8.52)
Используя (8.52), найдем параметры матрицы А из усло-
вия достижения заданной скорости vk в заданной точке гк.
260
для этого подставим значение вектора положения схвата
в точке Мк в выражение (8.52), получим
“ Ф (**)] $к +
vkh
хо.
(8.53)
Учитывая то, что все проекции вектора скорости схвата
должны быть сформированы одновременно, а также свой-
ство экспоненты, можно записать
4/ = ЗЛ”1; ' (8.54)
А = al. (8.55)
Следовательно, задача сводится к отысканию коэффициен-
тов а. Для их нахождения из (8.53) и (8.55) запишем
(8.56)
или
X* = Хо 4- 4^0С.
(8.57)
Последнее выражение позволяет найти значения а, которые
обеспечат условие достижения скорости, близкой к заданной
в данной точке пространства.
Однако эти условия будут справедливы только в случае
идеальной отработки движения. Йо в процессе нормаль-
ного функционирования системы возможно влияние возму-
щений, которые приведут к тому, что схват будет дви-
гаться по смещенной траектории и в результате разовьет
требуемую скорость не в заданной точке хк, а в некоторой
другой точке пространства. Чтобы исключить это нежела-
тельное явление, потребуем, чтобы отклонение от x(t) было
убывающей функцией вида
2
х (0 - (х0 - vkt 4- А~1 (/ - Ф (0) г*) = X ехР (8.58)
i= 1
Выражение (8.58) является решением однородного вектор-
ного дифференциального уравнения 2-го порядка вида
Дх = Kt Д* 4- К2 Лх. (8.59)
Здесь Ki — Bi 4- В2; К2— —BiB2, где Bj и В2 — матрицы,
у которых все собственные значения отрицательные либо
имеют отрицательные действительные части. При этом
обеспечивается устойчивость движения около программного
значения координат; кроме того, обозначено:
Дх = х (() — АФ (t) vk;
Дх = х (г) — (/ + Ф (t)) гк;
1
Дх » X (t) — (Xq + It — A 1 (Ф (t) — I)) V*.
С* учетом (8.59) формулу для ускорения можно предста-
вить следующим образом:
X(t) = Xi Дх
К2 Дх + АФ (t) vk.
(8.60)
Далее, если организовать движение схвата так, чтобы
ускорение изменялось по закону (8.60), то поставленная
задача будет выполненной.
Для этого, решая совместно уравнения (8.48) и (8.46),
получим
9))
(8.61)
Решая совместно (8.60) и (8.61), находим вектор управ-
ляющих моментов
Q = D й) J-1 й) (Ki Ai + К2 Дх) - р (q, ’q) - Н (q, ’q). (8.62)
Выражение (8.62) для управляющих сил Q представляет
собой нелинейную вектор-функцию времени, обобщенных
координат и скоростей, которые обеспечивают выход схва-
та манипулятора в заданную точку пространства с задан-
ным вектором скорости и при котором гарантируется
требуемое свойство движения по скорости и положению.
8.5. Построение алгоритма управления
манипулятором, выполняющим операцию сборки
Проведем синтез алгоритма управления сборочным про-
мышленным роботом на основе полученных выше уравне-
ний динамики для операции сборки гладкого цилиндри-
ческого соединения с гарантированным зазором (схемы и
описания алгоритмов даны в Приложении III). Эту задачу
решим с учетом переменности динамической модели мани-
пулятора в процессе сопряжения деталей и ограничений
на нормальные силы реакций в точках контакта деталей.
Последнее условие важно для сохранения качества поверх-
ностей сопрягаемых деталей в процессе сборки.
Считаем, что манипулятор (схват) может совершать
плоскопараллельное движение в плоскости Оху, а деталь
закреплена в нем с помощью упругих элементов (см.
рис. 5.9). Система координат связана со штифтом,
начало ее совпадает с его центром масс, система С2х2у2
связана со схватом манипулятора, начало ее находится
в геометрическом центре схвата. Система Оху связана
с отверстием. Примем, что в положении равновесия сис-
262
тема С2х2у2 совпадает с системой CiX^. Координаты
центра Сх в системе Оху — xlt ух, координаты центра С2
в системе Оху — х2, y2t угловое положение штифта отно-
сительно отверстия — ос, угловое положение схвата относи-
тельно отверстия — . Динамическая модель системы полу-
чена в гл. 1 на основе уравнений плоского движения
твердого тела. Перепишем уравнения с учетом ранее при-
нятых коэффициентов упругости с, = с,:
уравнения движения схвата манипулятора
Мх2 « Fхупр сх (х2 Xj),
Му2 ~ Fу у пр Су (у2 У1) Рщ»
/дР = Ррупр Са (Р ОС),
(8.63)
уравнения движения штифта для случая, изображенного
на рис. 8.1, а,
тх\ = сх (х2 — + N (sin ос — f cos а);
т*У1 = Су (у2 - Ух) + N(cos а + /sin а) - Рш;
(8.64)
Xi I cos ос
уравнения движения штифта для случая, изображенного
на рис. 8.1, б,
Рис. 8.1
1
TUX j ,=;
- *!) - М
тУ1 = Су (У2 - л) + N -
Л« = с«(Р - «) - N |/ъ
(8.65)
. Допустим, что в процессе «грубого» позиционирования
штифт относительно отверстия занял положение, соответ-
ствующее рис. 8.1, а. Найдем управляющее воздействие,
которое обеспечило бы совпадение центра штифта с осью
отверстия, угловой разворот штифта и ограниченное зна-
чение реакции в точке контакта деталей. При этом должно
обеспечиваться асимптотически устойчивое движение штиф-
та по координатам х и а и устойчивость значения нормаль-
ной реакции. Так как рассматривается условная плоская
модель сборки, примем, что реакция N направлена по
нормали к донышку штифта.
При малом а система уравнений (8.64) примет вид:
тх1 — сх (х2 — Xi) + N (а — /);
тУ1 = су{у2 - yt) + N (!'- fa) - Рш-,
(8.66)
(8.67)
(8.68)
Из (8.67) выразим нормальную силу
N = [myi - су(у2 - Ji) + Рщ]/(1 + /«)•
(8.69)
Подставим найденное выражение для N в (8.66) с уче-
том малости а
mx*i = сх (х2 - Xi) + N (а - /),
(8.70)
где N определяется выражением (2.69).
Далее решим обратную задачу динамики. Системы двух
дифференциальных уравнений 2-го порядка (8.66) и (8.70)
заменим одним уравнением 4-го порядка. Дифференцируя
(8.70) дважды, получим:
тх^ = сх (х2 - х\) + N (а - /) + 2Na + №,
(8.71)
где
(1 + fa) [m j, -
с, (у2 - У i)] - /а [т^-сДуз-уО+Рщ] .
(1 + 2/а)
(8.72)
264
1
д? = <*+Л) [ту- с, (у2 - Ji)] - fa [my
(l + 2/a)
- {(1 +/a) [m yi — су (у 2 - У1)] —/а [ту 2—су (у2—У1)4-Рш]} 2/а.
(8.73)
В выражениях (8.72) и (8.73) члены, содержащие а и /2,
отброшены ввиду малости. Выражая х2 из (8.71) и подстав-
ляя в (8.63), получим искомое уравнение 4-го порядка
Д[тх/,у>
N (а — f) — 2N<i — Na] =
= Лупр - сх (Xi - х2). (8.74)
Найдем такую силу Гхупр, которая уравнение (8.74) сде-
лает эквивалентным линейному дифференциальному урав-
нению 4-го порядка, имеющему асимптотически устойчивое
решение по координате х, сходящееся к программному
значению х1пр = 0. Желаемое значение координаты
X&V)= -fliXi
где Xiпр — программное значение координаты xt; at (i =
= 1, ..., 4) — коэффициенты, обеспечивающие асимптоти-
чески устойчивое решение уравнения (8.75). В результате
решения обратной задачи динамики найдем аналитическое
выражение для управляющей силы
FXynp = —{т [-fliXi - a2x*i - a3x, - (xt - х1пр)1 +
•Mnp),
(8.75)
N (a - /) - 2Na - Na}
(8.76)
Найдем аналитическое выражение для силы управления
по координате р. Преобразовав системы уравнений
и (8.68), запишем
(8.63)
f
I
г
1
1
1
2N(Xi
M - yitx) + N (x\ - yta - 2yitx- м)> =
(8.77)
С,(>’2-У1)+Рш]
M
N
Аналогично уравнение (8.75) примет вид
a«(IV) = — bj a — b2'd — b3a — b4 (a — otnP),
где otnp — программное значение координаты
(Я.7К)
№
1
= 1, ... , 4) — коэффициенты, обеспечивающие асимптоти-
чески устойчивое решение уравнения (8.78).
Из (8.77) с учетом (8.78) получим выражение для силы
управления
м
Cad
— Уха)
N(x\
yxa - 2ytd - yxd)
Для обеспечения постоянства реакции в точке контакта
деталей используем управляющее воздействие по координате
у2. Выразим у2 из второго уравнения системы (8.63)
Су (Уг - >1) - Рм]/М.
(8.79)
L' УУПР
Подставляя (8.80) в (8.73), получим
(8.80)
M
м ••
— ~У\
(8.81)
[myi
Асимптотическую устойчивость силы N обеспечим, вы-
брав управляющую силу Fy упр в выражении (8.81) из
условия обращения (8.81) в линейное дифференциальное
уравнение вида
- d2 (N - N„p), (8.82)
d2 — коэффициенты, обеспечивающие асимптотиче-
программное значение
где d
ское решение уравнения (8.82); N
реакции.
Используя (8.81) и (8.82), найдем выражение для управ-
ляющей силы по координате у
- RiN - d2 (N - N„p) - R] (1 + fa} +
I
I
)
f
i
r
J
1
1
_ М
УУпр Т
+ Су (у2 - У1) + У1М + Рм- (8-83)
Таким образом, описан алгоритм управления (8.76),
(8.79), (8.83), обеспечивающий двухмассной системе с упругой
связью манипулятор — деталь движение согласно заданным
1
условиям (8.75), (8.78), (8.82). Полученные законы управления
позволяют осуществить управление при a < 0. Однако при
осуществлении операции «грубого» позиционирования штифт
может занять другое положение, например изображенное
на рис. 8.1, б. Тогда модель (8.66)—(8.68) и закон управления
(8.76), (8.79), (8.83) становятся другими, поэтому необходимо
изменить модель, положенную в основу построения закона
управления, т. е. принять ее в виде (8.65). В этом случае
с учетом принятых ранее допущений закон управления
примет вид:
f [myi™
«1 - ^2*1 “ 03*1 - 04 (*1 - *1пр)]
су(У2 ~ ьШ + сх(х2 -*i);
РУПР “ Л
b2'd - b3a - b4 (a - cznp)]
Cad
a
N Ai
М
уупр —
Су
/a
+ (Су (у2 - У1) + МУ1 + Рм. (8.84)
Аналогично предыдущему синтезируем закон управления
для случая, изображенного на рис. 8.1, б. Уравнения движе-
ния штифта при этом имеют вид (5.76). Уравнения движе-
ния манипулятора остаются в виде (8.63). Закон управления
имеет вид:
Г" Z ... •• •
^уупр = ---Lm(~01 Я “ 02У1 — 03У1
Vy
+ CyJi — dA — 2dA — ?vai _|_ е iv _
- «4 (j’l - У1Ч,))
M
fd
c2a
xynp —
267
_ A* [cx (x2 - *1) - W^’1] . 2/a [cx (x2 - Xi) - mx’1]
f< — — —— — -+-
1-2/a 1—4/a
* »
В состав полученных законов управления входят коор-
динаты центра масс детали и их производные, вычисление
которых с достаточной точностью численными методами
на ЭВМ требует значительных затрат машинного времени.
Это осуществимо лишь при применении быстродействую-
щих мультипроцессорных систем. При недостаточной мощ-
ности управляющей ЭВМ следует провести упрощение
полученных алгоритмов. Исследование полученных алгорит-
мов на ЭВМ показало, что при малых значениях податли-
вости относительная величина члена, содержащего старшую
производную, мала.
8.6. Обратные задачи динамики
манипуляторов, выполняющих обработку
поверхностей
Построим алгоритм управления манипулятором при об-
работке поверхности, обеспечивающий заданную нормаль-
ную силу в точке контакта и скорость движения инстру-
мента по поверхности (рис. 8.2). Запишем уравнения дина-
мики
tn2'x = Qi + Nni;
(mi + т2) у = -Pi -P2 + Qi + Nnj,
(8.86)
(8.87)
где т2 — массы первого и второго звеньев манипуля-
Рис. 8.2
тора; х, у — координаты центров
масс звеньев в неподвижной систе-
ме координат Оху; Plt Р2 — силы
тяжести первого и второго звеньев;
61, Q2 — силы, развиваемые приво-
дами манипулятора в первом и
втором шарнирах; N — модуль
вектора нормальной силы в точке
контакта инструмента и детали;
п — вектор нормали к поверхности,
п = grad//I grad/1; /(х, у) = 0 -
уравнение кривой, по которой не-
обходимо осуществить движение
инструмента при его перемещении
по поверхности детали; i, j —
единичные орты.
268
Учитывая, что
j; I grad/1
перепишем уравнения (8.86) в виде
&aAf= to
(8.88)
(8.89)
(™1
(8.90)
где G = | grad f |.
Дифференциальное уравнение электроприводов
в виде
«V = (Mi
примем
к2 — постоянные коэффициенты, характери-
(8.91)
где uv — управляющее напряжение электродвигателя в v-m
шарнире; к
зующие электропривод.
Найдем выражение силы Q2 из уравнения (8.89):
т2х.
(8.92)
Продифференцировав (8.92), получим
N
дх
дх
дхду?
дх
/G2
дхду
т2Х.
(8.93)
Решая совместно (8.91)—(8.93), получим
управляющего воздействия:
выражение для
г
. дх
ц = —-_________
дх2
дх ду V
/G
t
I
г
1
I
1
дхду
kitn2x
дх
к2т2х.
(8.94)
Найдем такое управляющее воздействие, которое выра-
жение для N из (8.94) обращает в уравнение вида
1
N = k(N - Nnp); ?. <0, (8.95}
где Nap — программное значение силы реакции.
^ля этого выразим N из (8.94) и приравняем его пра-
вой части выражения (8.95). Выполнив необходимые пре-
образования, получим значение управляющего воздействия
в виде
/G
- NnP) 8х
дх
/ d2f
\дхду
ktm2x
к2т2х.
(8.96)
Учитывая (8.89) и (8.93), запишем выражение для управ-
ляющего напряжения в виде
й = -кД(N - Nnp) ^/G + k2Q2 + к,ё2 + (8.97)
Таким образом, для формирования управляющего воз-
действия, реализующего постоянство силы N, достаточно
использовать лишь один из приводов Q2. Имеющуюся
избыточность по второй координате используем для реали-
зации условия постоянства скорости движения по поверх-
ности. Это условие зададим в виде
Г = У (г - гпр); у < 0; (8.98)
оно имеет смысл обеспечения устойчивого значения скорости
движения одной детали по поверхности другой, равной гпр.
Здесь
V — |/х2 + у2,
где v — модуль скорости;
(8.99)
х, у — проекции скорости на
;екартовы координаты.
Условие (8.98) с учетом (8.99)
можно представить в виде
Y (г - vnp) = xx/v + yy/v.
(8.100)
В уравнении (8.100) у можно заменить, используя урав-
нение движения манипулятора (8.89) по координате у.
Используя (8.100) и (8.89), найдем управляющее воздействие,
реализующее условие (8.98):
7(v--v„p) = xx/v + y\ -Pl- P2 + Q1 + N~-/G)/(mi + tn2)v.
V еу / (8.101)
Продифференцировав последнее выражение по времени
и выполнив необходимые преобразования, получим
270
1
Учитывая дифференциальное уравнение привода (8.91) и
выражения (8.100) и (8.102), можно аналогично предыдущему
записать
На основе (8.103) с учетом (8.89) запишем алгоритм
271
1
Полученные алгоритмы (8.104) и (8.97) вычисления управ-
ляющих воздействий на приводы манипулятора построены
в предположении, что уравнение поверхности известно.
Рассмотрим теперь решение поставленной задачи для случая,
когда уравнение поверхности не задано.
Примем, что а — угол наклона силы N к неподвижной
системе координат
а = arctg (Ny/Nx) или а = arctg (vx/vy), (8.105)
где NXi Ny — проекции силы N, измеряемые силовым дат-
чиком; vx, vy — проекции скорости v на неподвижные оси
координат.
Уравнения движения манипулятора (рис. 8.2) запишутся
в виде:
m2x = Q2 — N cos a; • (8.106)
(wii + m2) у = Qi + N sin a + Pi 4- P2. (8.107)
Из уравнения (8.106) выразим нормальную силу
N = (Q2 — m2x)/cos а
и продифференцируем выражение по времени, получим
N = [(02 — т2х) cos a + a (Q2 — m2x) sin a]/cos2a. (8.108)
Запишем дифференциальное уравнение, обеспечивающее
требуемый вид переходного процесса по силе N:
N = X(N- Nnp); к < 0, (8.109)
где Nnp — заданное значение нормальной силы в точке
контакта деталей.
Найдем такое управляющее воздействие, которое урав-
нение (8.108) обращает в уравнение вида (8.109). Для этого
решим совместно (8.108) и (8.109), получим
1 (N — Nnp) = [(02 — т2*) cos a + a (Q2 — m2x) sin a]/cos2 a.
(8.110)
Проекция скорости на ось х
vx — Vy tg a.
Продифференцируем (8.111) по времени
= [cos2 a (ya + ay) + 2 (a)2 у cos a sin a] ——
COS 01
(8.111)
-2^-+ytga.
cos'4 a
(8.112)
272
Решим систему уравнений (8.110) и (8.112) относительно
х, у, получим:
d(g2 — w2x)tga —
m2
(8.113)
1 (N — Nnp) cos a]
(Q2 ~ w2x) —
m2
2(a)2у
(2yd -j- ay)
sin a cos a cos" a
(8.114)
Из (8.107) имеем
Продифференцируем полученное выражение
sin a
sin2 a
(8.115)
Решая совместно (8.109), (8.114) и (8.115), найдем
л __ А (дг _ дг ) ( sin a tg a + cos a —
a \ m2
m2
(wit
m2) tg a
(62 т2Х) —
m2
_ 2y(a)2l 1
sin a cos a cos2 a
(8.116)
. cos a
m2)y
Наиболее распространенными в производстве являются
плоские поверхности и поверхности, имеющие постоянную
кривизну. Движение по этим поверхностям необходимо
осуществлять с постоянной, заранее заданной скоростью.
При движении по поверхности постоянной кривизны a =
= a> = const. С учетом сказанного запишем закон формиро-
вания управляющего воздействия по координате Qi:
x
a>
cos a
m2 cos a
mi
+ m2
----—cos a
m2
Qitga
- L. “ •
0)
(yn14-m2)tga
a>
(8.117)
10 Механика промышл, роботов, кн. t
1
Входящие в данное выражение значения тригонометриче-
ских функций угла а в этом случае являются известными
константами, значения которых зависят от кривизны
конкретной поверхности.
При движении по плоским поверхностям а = const,
в этом случае закон формирования управляющей силы
примет Вид
Qi = — X (N — Апр) I sin a
т2
01
m2 sin a tg a *
(8.118)
Выражение (8.118) представляет собой дифференциальное
уравнение первого порядка относительно Qi9 из решения
которого находится-, управляющая сила Q19 обеспечиваю-
щая требуемый вид переходного процесса по силе N.
Алгоритм реализуется на микропроцессорных ЭВМ.
Используем вторую координату для обеспечения необ-
ходимой скорости движения по поверхности детали. Диф-
ференциальное уравнение, описывающее требуемый вид
переходного процесса по скорости движения схвата, имеет
вид (8.98). При выполнении условия (8.109) движение схвата
происходит без отрыва его от поверхности с заданным
значением реакции N, поэтому отслеживание траектории при
движении по поверхности не обязательно. Рассмотрим
выполнение второго условия — постоянства скорости. За-
пишем : х = v sin а. Дифференцируя это выражение и учиты-
вая условие (8.98), запишем
у (г — гпр) — (х — ar cos a)/sin а. (8.119)
Выразим х из уравнения (8.106)
х = (Q2 — N cos a)/w2. (8.120)
Решая совместно (8.119) и (8.120), найдем выражение
для силы g2, развиваемой вторым приводом,
в
= т2 sin а [у (у — гпр) + ar ctg а] 4- N cos бс.
Полученные выражения для управляющих сил показы-
вают, что они должны формироваться в зависимости от
параметров движения и усилий в контакте по показаниям
соответствующих датчиков.
8.7. Построение алгоритма управления
движением манипулятора по заданной
траектории
В промышленных роботах масса исполнительного меха-
низма значительно превышает массу груза, поэтому алго-
ритмы управления такими системами необходимо строить
на основе их динамических моделей. Синтез алгоритма
управления автоматического манипулятора следует прово-
дить с учетом нелинейности и многосвязности его дина-
мической модели, что позволяет повысить эффективность
управления.
Уравнение движения исполнительного механизма мани-
пулятора можно представить в виде
Й) =f(4, 4, м), (8.121)
где q, q — обобщенные координаты и их производные по
времени; М — вектор сил и моментов, развиваемый при-
водами. Ускорение схвата манипулятора
х (0 = J (4) q (О + Р (д, 4),
(8.122)
где х (Г) — вектор координат точки схвата в декартовой
системе; J (q) — матрица Якоби, определяемая кинематиче-
ской схемой манипулятора.
Из (8.122) имеем
«(0 = J"1 (4) 4 [* (О - ? («> «)]•
Пусть необходимо осуществить движение из исходной
точки х (0) х (0) в заданную х
траектории. Потребуем, чтобы программное движение было
экспоненциально устойчиво, т. е.
*(со), х*(оо) по прямолинейной
х* — х (Q = + с2^,
(8.123)
где Ci и с2 — постоянные, определяющиеся начальными
условиями. Этому программному движению соответствует
ускорение схвата, выраженное через его координаты и
скорость:
5с* — kix — 1<2 [х* — х(0]. (8.124)
Выражение (8.123) является решением векторного диффе-
ренциального уравнения (8.124), где kr = + а2; к2 — ад;
а, < 0 (i = 1, 2).
Преобразуем уравнение (8.122) с учетом (8.121) к виду
Й) = J (4)f(4, 4$) + (4, «) (8.125)
10*
275
Далее подставим желаемое ускорение х* из уравнения
(8.124) в выражение (8.125)
ktx — к2(х* - х) = J (q)f\ (4 q, М) + Р (q, q)
(8.126)
где /* (q, q, М) — функция обобщенных моментов, реализую-
щая заданную программу движения схвата.
Выразим из (8.126) требуемую вектор-функцию обоб-
щенных сил
7* (q, 4, М) = J"1 (4) [&i х - к2 (х* - х) - Р (q, 4)]. (8.127)
Таким образом, прямолинейное движение (8.123) будет
осуществляться системой (8.121), если вектор-функция обоб-
щенных сил формируется по закону (8.121).
Рассмотрим задачу построения управлений, реализующих
движение схвата по криволинейной траектории, заданной
в виде интегрального многообразия
<р(Х1, х2,..., х„) = 0. (8.128)
Данную задачу будем рассматривать как задачу пост-
роения дифференциальных уравнений манипулятора по
заданному интегральному многообразию так, чтобы выра-
жение (8.128) являлось интегралом этих уравнений, и из
построения уравнений определим искомые управляющие
воздействия, параметры и связи, реализующие движение
системы по заданным условиям [12].
Пусть траектория движения схвата задана в виде
Ф1 (*ь *2» хэ) = 0; <р2 (*1 *2*з) s °- (8.129)
Задача формулируется следующим образом. Необходимо
перевести схват манипулятора из исходной точки х0 в задан-
ную по траектории (8.129) при следующих граничных
условиях: х(0) = х; х(0) = 0; х(оо) = х*; х(со) = 0. Про-
грамму движения по каждой координате выберем в виде*
X* - Х1 (0 = + C'i2eW; *2 - х2 (О = Сг!^21' +
* — ^3 (0 — c31^®31t
(8.130)
где х? (i = 2, 3) — решение системы (8.129) при фиксиро-
ванном х t; Xi (t) (i — 1, 2, 3) — текущие координаты состояния
схвата.
Первое уравнение обеспечивает экспоненциальный закон
перевода схвата из начальной точки в конечную по
координатам х^ Второе и третье уравнения характеризуют
отклонения от траектории по координатам х2 и х3.
Программа вида (8.130) обеспечивает также асимптотиче-
276
скую устойчивость движения схвата по заданной траекто-
рии. Дифференцируя (8.130), найдем соответствующий вектор
ускорений
*1 = —a11a12[xi - хНО] + (an + a12)xi(0;
3c2 = — a2ta22 [x* — x2 (t)] — (a2i + a22) [x* — x2 (f)] 4- x2;
*3 = “«31«32 [X? ~ *3 W] - («31 + «32) [%* “ *3 (t)] + X?,
(8.131)
где значения x*, xf первых И вторых производных могут
быть получены из выражений (8.129).
В результате дифференцирования (8.129) получим
(8.132)
Затем находим xf, xf и xf, xf как решение системы
линейных уравнений (8.132). Заменяя в (8.131) xf, xf, xf
величинами из уравнений (8.132), выразим программное
ускорение схвата х\ (к — 1, 2, 3) через координаты, их первые
производные и х*, значение которого можно заменить,
используя первое уравнение системы (8.131). Таким образом,
искомое ускорение схвата, реализующее программу (8.130),
является функцией скоростей и координат.
Решая совместно уравнения (8.121), (8.122), (8.125), (8.131),
(8.132), найдем программные обобщенные силы в виде
/* («, i М) = J -1 (q) [X (х, х) - Р (q, 4)1. (8.133)
Важно отметить, что полученная функция есть нелиней-
ная функция фазовых координат системы, что позволяет
построить систему управления на основе обратных связей.
Следует заметить, что в отличие от закона управления,
синтезированного на основе кинематического подхода,
законы управления (8.127) и (8.133) учитывают динамиче-
ские характеристики исполнительного механизма манипуля-
тора как многосвязной нелинейной системы.
Разрешив уравнение (8.127), (8.133) относительно мо-
ментов М, которые входят в правую часть выражения
(8.121) аддитивно, получим его выражение через фазовые
координаты системы
2ЙГ* = Й(4, 4,/*). (8.134)
277
1
-x(t)
I
Рис. 8.3
Полученная функция М* должна быть сформирована
приводом на основе задающего воздействия и*. Описание
привода электромеханического типа примем в виде
dM
(8.135)
[fc (Й - q) - (к
к, ка, kv — постоянные.
В результате решения (8.134) и (8.Г35) относительно й
найдем управляющую функцию для приводов при допу-
щении, что гэ мало:
u* = 1 (чХ-ktX - к2 (х* — х) — Р (#14))] +
(ka
где R„, к, км, к^ kv — параметры привода; и* — задаю
шее воздействие привода.
Рис. 8.4
278
Структурная схема ал-
горитма управления, реа-
лизующего движение
манипулятора по задан-
ной траектории, приведена
c
t
I
r
1
1
1
»
Алгоритм управления
ижением плоского мани-
пулятора по заданной
траектории (рис. 8.4). Дви-
жение манипулятора опи-
сывается уравнениями:
1
+ «12^2 —
Мх;
й21#1 + ^22^2
= М2,
где
atl = mJi 4- Ji 4- 4m2/i 4- l2m2 + 4т2Ц12 cos q2 4- J2;
Д12
l2m2 + 2т2Ц12 cos q2 4- J2;
Mi = Mi + 2m2/J2 sin <Ы2 + 4m2M2 sin <Mi<?2 i
o2i —~ ?n2^2 2^n2/i/2 cos q2 4" *^2 >
a22 — ~~ (W2^2 + ^2)9
M2 = M2 4- 4m2ltl2 sin <?24i42 4- 2т21±12 sin ^2^i;
G, h ~~ длины звеньев; ть m2 — массы
приведенные моменты инерции; Mi,
моменты двигателей.
звеньев;
М'2 — приведенные
Тогда функции ft (q9 q9 AS), f2 (q, q9 M), входящие в урав-
нение, имеют вид
fi № 4, М) = (Mia22 - М2а12)/Д/;
A (4 Я, М) = (M2«i 1 - М^О/ДА
(8.136)
где Д/ = Яц&22 а 12^21 •
Из кинематических уравнений манипулятора находим
х\ = - It cos qtql -Xi'qi- l2 cos (gj + q2) (qi + q2)2 -
- I2 sin (gj + g2) g2;
x2 = xi'qi - Z2cos(gi + g2)g2 - itsingf - i2sm(g! + g2)(g2 + g2)2,
(8.137)
откуда определяем матрицу Якоби и кинематические функ-
-2l2 sin (qi 4- q2)~
-2/2cos(gi + g2)J’
cos q!ql - l2 cos (q!
Ч2) («1 + Q2)1;
«)= -Zising,gi
- h sin (g2 + g2) (g2 + g2)2.
(8.138)
Согласно уравнению (8.127) и с учетом (8.136) запишем
выражение требуемого значения функции /♦ (q9 q9 j^*)
1 Mfa22 -M|a12"l i Г 2l2 cos (q2 + q2) xt 1
ДУ -М^Дц — M*a2i Д — 2/2 sin (fli 4" fl2) “~X2
. (8.139)
279
1
Решая (8.139) относительно Aft и AfJ, получим
Af* = + ^12^2» М* ~ ^21^1 4" #22^2,
где
Di = {2l2 cos (qt 4- q2) [&ii%i ~ k12 (xf - Xi) - pt (q, £)] 4-
4- Xi [k21x2 - k22 (xf - x2) - p2 {q, 4)]}/A;
D2 = {-2l2 sin (q± 4- q2) [&ii^i ~ kl2 (xf - XJ - pt (q, g)] -
- x2 [k21x2 - k22 (x2 - x2) - p2 ((?, Й)/А.
Далее находим требуемые значения приведенных момен-
тов Двигателей
M'f = «11^1 4- «12^2 - 2И12М2 sin Q.2Q.2 - 4ш2/1/2 sin g24i<j2 ;
Af'2* — a2iDi 4- a22D2 - 4m2lil2 sin “ 2m2ZxZ2 sin g24i.
(8.140)
Как видно из (8.140), для формирования моментов,
необходимых для реализации программы (8.121), исполь-
зуются уравнения кинематики (8.137) и динамики (8.136)
исполнительных механизмов. Окончательно для управляю-
щих функций привода можно записать
Таким образом, построенные алгоритмы обеспечивают
системе устойчивое движение по заданной траектории.
Управляющие функции выражаются через обобщенные
координаты, что позволяет строить систему управления
манипулятором на основе обратных связей, используя
информацию с датчиков положения и скорости.
Построенный в примере закон управления был иссле-
дован на ЭВМ методом математического машинного моде-
лирования. Проводилось моделирование процесса движения
манипулятора по заданной прямолинейной траектории. При
этом модели приводов приняты линейными в соответствии
с уравнением (8.135). На рис. 8.5, а показаны графики
отклонений точки схвата манипулятора от заданной пря-
молинейной траектории для различных а. Из графиков
видно, что предложенный алгоритм позволяет реализовать
движение по траектории с ошибкой не более 1 % от длин
звеньев при быстродействии около 0,2 с. При использо-
вании кинематического алгоритма отклонение от траектории
оказывается большим на порядок при большем времени
280
1
>
Рис. 8.5
переходного процесса (рис. 8.5,6); на рисунке: кривые 1 и 3 —
для at = а2 = 20; кривые 2 и 4 — для cq — 20, а2 = 40.
Моделирование показало также устойчивое приближение
схвата к заданной траектории (5 = 0) при действии началь-
ных возмущений (рис. 8.5, а).
Приложение I
Сведения из векторной алгебры
Приведем некоторые сведения из векторного исчисления, необхо-
димые при решении задач о положениях манипуляторов.
Произвольный вектор г может быть представлен в виде про-
изведения его модуля г и единичного вектора е, называемого
ортом, т. е.
г = ге
Скалярное произведение двух векторов
rl * r2 = rixr2x 4- Г1уГ2у + Г1гГ2т = XtX2 + yty2 + ZiZ2 ,
где « xit riy = yb rlz = zt (i = 1,2) — проекции векторов на оси
х, у, z. С другой стороны,
Г1 • Г2 = Г1Т2 COS а >
где а — угол между векторами п и г2.
Векторное произведение двух векторов ft и г2
= КУ 1*2 - У2*1) ~RXlZ2 - X2Zi) + £(Х1?2 - х2У1).
Модуль векторного произведения
| х г21 == rjT2 sin а.
Двойное векторное произведение может быть представлено
в виде
а х (Ь х с) = bfa'C) — c(a-b);
(а х Ь) х с — Ь(а-с) — а(с*Ь).
Очевидно, что эти выражения не эквивалентны.
Важное применение последней формулы состоит в выводе
выражения для разложения вектора Ь на два составляющих
вектора, один из которых параллелен, а другой перпендикуля-
рен заданному вектору а. Положив с — а, получим
а х (Ь х о) = b(a-a) — a(a-b) = ba2 — a(d-b).
Решая относительно вектора Ь, найдем
Векторно-скалярное произведение трех векторов
а (Ь х с) = ±Р,
где V — объем параллелепипеда, построенного на этих векторах.
282
1
Если векторы а, b и с образуют левую систему координат, то
следует брать знак плюс, если правую — минус.
Решение векторных уравнений. При решении обратных задач
о положениях манипуляторов часто приходится определять векторы
из векторных уравнений.
Задач а 1. Заданы два векторных уравнения, выражающих
скалярное и векторное произведения: г-а = с
уравнений необходимо определить вектор г.
Для решения используем формулу разложения вектора г на
два составляющих, один из которых параллелен, а другой пер-
пендикулярен заданному вектору а:
f x а — b. Из этих
-2-а+ -
а а
Подставляя в эту формулу выражения для f • а и f х d, полу-
чим решение задачи
- са
f । — ।
а
1
2 *
В проекциях это условие эквивалентно следующей системе
уравнений:
агу
вуХ ~ ахУ = Ь
Из четырех уравнений независимыми являются лишь три
первых.
Задача 2. Определить вектор г из системы уравнений
Решение этой системы имеет вид
f = ad* + РЬ* + ус*,
где а
: b* —
)
1
1
1
Задача 3. Пусть орт с образует
известными векторами з и К, причем
Требуется определить орт с.
Для решения этой задачи можно воспользоваться известной
формулой трех ортов
с = {г0 (cos ф — cos X cos 6)
X j/ [cos (ф — 5) — cos X] [cos X — cos
где f0 и So — орты векторов Rus.
углы 8 И с двумя
X — угол между ними.
s0 (cos 5 — cos ф cos X) ± f0 х s0 х
1
Приложение II
Элементы теории матриц
Определитель (детерминант) квадратной матрицы. Квадратная
матрица порядка п имеет вид
«11 «12 • •• «1л ~
«21 «22 ••• «2л
А =
- «л! «л2 ••• «лл _
Определитель квадратной матрицы обозначается так:
«11 «12 «1л
«21 «22 ••• «2л
«л1 «л2 ••• «лл
или кратко
| А | = det А.
След матрицы. Следом матрицы называется сумма ее диаго-
нальных элементов. След матрицы обозначается символами tr
или sp, например,
п
tr А = X ан> или
i- 1
1. Транспонирование
матрицы
Если дана некоторая прямоугольная матрица А размером т х п,
то транспонированной матрицей АТ называется матрица размером
и х т, которая получается из данной матрицы заменой строк
столбцами. Так, если А = [«<*]£, то А1 — где аа = ак{.
Транспонирование позволяет матрицу-столбец записывать ’ в
ваде транспонированной матрицы-строки:
= [«ь «2, «з]т-
L«3
Имеют место свойства, проверяемые простым подсчетом:
(А + В)Т = ЛТ + ВТ;
(сМ)т = аЛт; (АВ)Т = ВГАТ.
(П.1)
Последнее свойство означает, что при транспонировании про-
изведения матриц следует транспонировать каждую матрицу и
затем их перемножить в обратном порядке.
284
2. Переменные матрицы, их дифференцирование
и интегрирование
Если элементы матрицы — дифференцируемые функции t (например,
функции времени), то существует производная от матрицы по t.
Это — матрица, элементы которой получаются дифференцированием
элементов исходной матрицы:
Цц «12 ••• «1И
• • •
••• С1тп
(П.2)
Имеет место формула
(АВ) = АВ 4- АВ.
Если элементы матрицы — интегрируемые функции i, то су-
ществует интеграл от матрицы. Он представляет собой матрицу,
элементы которой — интегралы от элементов исходной матрицы:
J A (t) di =
f«u(i)dt fa12(t)di ••• f«i„(i)di
f«2i(t)dt f«22(0di ... f«2„(i)dt
f 1 (0 J @m2 (0 • • • f @mn (0
*
Имеют место свойства:
J (A + В) di = J A di + f В di; J aA di = a J A di,
(П.З)
где a — число; А и В — матрицы одного размера, зависящие от
аргумента t.
Если матрица А зависит от t, а матрица X не зависит от г,
то, очевидно,
f АХ dt = (jAdt)X,
т. е. матричные произведения дифференцируются и интегрируются
по тем же правилам, что и произведения скалярных функций.
3. Скалярное произведение
ной записи
векторов
матриц-
в
При заданной системе координат вектор характеризуется своими
проекциями на оси. Эти проекции — набор чисел. Если располо-
жить их столбцом, то получим столбцовую матрицу. Например,
векторы а и В можно записать в виде матриц:
285
1
t 4»
«1
«2
Ниже столбцовые матрицы, имеющие смысл векторов, иногда
будут обозначаться малыми буквами с чертой сверху.
Скалярное произведение векторов а и b
в матричной форме можно записать так:
(П.4)
или
" аг
а2
а2Ь2
J
Существуют и другие способы записи скалярного произведе-
ния векторов с помощью матричных операций, например:
1 **
^1^2 ••• I
tr (ЛВТ) = tr
<*2^2
(П.5)
ИЛИ
(П.6)
В разных случаях могут оказаться удобными разные формы
записи скалярного произведения векторов.
I
I
)
t
I
!
1
I
1
Горизонтальными и вертикальными прямыми матрицы можно раз-
бить на блоки. Каждый блок — это матрица меньшей размер-
ности. Матрицу-блок можно обозначить буквой. Введя буквенные
обозначения для всех блоков матрицы и заменив ими соответ-
ствующие блоки, получим так называемую блочную матрицу.
Например,
286
1
можно записать как блочную матрицу:
йц а12 а13
flu
-а21
а12
а22-
где
л
а21 ~ [й31 ^31] J
а12 —
а13
-а23_
а23 — [^34-] •
0^11 s=
Если имеются две блочные матрицы А и В с такими бло-
ками a,j и Ру, что любую матрицу можно помножить на
любую матрицу Pfcj, то матрицы А и В можно перемножить и
результат записать в виде блочной матрицы D, блоки 5у которой
вычисляются по формуле
(П.7)
Иначе говоря, блоки в этом случае формально эквивалентны
элементам обычной матрицы. Например, записанную выше блоч-
ную матрицу Л, состоящую из шести блоков, можно помножить
на блочную матрицу В, разбитую на блоки так:
Результатом будет блочная матрица
§12
§22
у которой
5ц — (ХцРц + (Х12Р21 + а1зРз1 »
812 — а1хр12 + а12р22 + оЧзРзг»
621 — «21 Pl 1 + а22021 + а2зРз1 >
822 — &21Р12 + а22022 + а23032«
5. Матрицы
поворота
свойства
и
И X
Пусть твердое тело повернулось на угол ф вокруг оси е, прохо-
дящей через начало неподвижной системы координат х, у, z, причем,
если смотреть с конца оси е, поворот совершался против часовой
287
1
стрелки. Если г (0) — радиус-вектор некоторой точки тела до пово-
рота, а г (<р) — радиус-вектор той же точки после поворота, то,
как известно, проекции г (ф) на оси неподвижной системы коорди-
нат связаны с проекциями г(0) на те же оси зависимостью вида:
х (ф) = жцх (0) 4- т12у (0) 4- m13z (0);
У (Ф) = ^21* (0) + т22У (0) 4- w23z (0);
(П.8)
z (ф) = т31х (0) 4- т32у (0) 4- m33z (0),
где ту — коэффициенты, зависящие от направляющих косинусов
оси е и угла ф.
Представляя г (0) и г (ф) как столбцовые матрицы и конструи-
руя из коэффициентов Шу квадратную матрицу М, запишем (П.8)
в виде
«г
г (ф) — М (е, ф)г (0).
(П.9)
Матрица М (е, ф) называется матрицей поворота, причем ё — еди-
ничный вектор, идущий вдоль оси е.
Общий вид матрицы поворота таков:
М (е, ф) =
" COS ф + CiCi
е3 sin ф + с2ех
_-е2 sin ф 4- с3в1
-МПф + с^г
cos ф 4- с2е2
ех sin ф + с3е2
е2 sin ф + схе3
— et sin ф 4- с2е3
cos ф 4- с3е3
(П.10)
где Ci = ei (1 — cos ф); с2 = е2 (1 — cos ф); с3 = е3 (1 — cos ф), причем
elt е2, ез ~ направляющие косинусы вектора е.
В частности, повороты вокруг координатных осей описывают-
ся матрицами:
М (i, ф) =
0 cos ф
-0 sin ф
cos Ф
M(j, ф) =
COS ф
0
_ — sin ф
0 sin ф
1 0
0 cps ф -
(П.11)
(П.12)
cos ф
М (к, ф) —
sin ф
— sin ф 0
cos ф 0
0 1
(П.13)
0
Основные свойства матриц поворота следующие:
матрица, обратная матрице поворота, — это тоже матрица по-
ворота вокруг той же оси на тот же угол, но в отрицательном
направлении; она получается из матрицы поворота посредством
транспонирования, т. е.
М 1 (ё, ф) = М (ё, — ф) = АП (ё, ф);
(П.14)
288
определитель матрицы поворота равен единице, т. е.
| М (е, ф) | = 1;
произведение матриц поворота есть матрица поворота;
каждый столбец матрицы поворота — это соответствующий
орт повернутой системы координат, жестко связанной с вращаю-
щимся телом, в проекциях на оси неповернутой системы;
каждая строка матрицы М — это соответствующий орт непо-
вернутой системы координат в проекциях на оси повернутой
системы;
производная от матрицы поворота по углу поворота
--Iе’ф) = Г (ё) м (ё, ф) = м (ё, Ф) г (ё), (П. 15)
dt
где Г (е) — вспомогательная матрица, зависящая только от поло-
жения оси вращения:
О
Г(ё) =
(П.16)
L—е2 ех 0 ..
При дифференцировании матриц поворота вокруг координат-
ных осей
"О
о
-О
используются частные виды матриц Г:
"0
; Г(£) = 1
; ^(/) =
ООО
L0
О'
о
0-
0 О
0 — 1
1 о _
о
(П.17)
6. Матричная форма перехода из одной системы
координат в другую (расширенные матрицы пе-
рехода)
Пусть имеется последовательность систем координат с номерами
0, 1, ..., к — 1, к, ..., т. Радиус-вектор некоторой точки будем
снабжать нижним индексом — номером системы координат, в кото-
рой он задан своими проекциями. Связь между радиусами-век-
торами 4-1 и 4, указывающими на одну и ту же точку, но
выходящими из начал двух разных систем координат, определя-
ется соотношением вида
4-1 — hk-i,k + Мк_ 1# fc4» (П.18)
где hk_lfk — вектор смещения начала координат системы к относи-
тельно начала координат системы к — I, заданный в системе к — 1;
Мк -1, * — матрица поворота системы к — 1 до совмещения осей
системы к — 1 с осями системы к.
Зная радиус-вектор 4-1 и расположение систем к — 1 и к — 2
друг относительно друга, по формуле, аналогичной (П.18), можно
289
вычислить радиус-вектор rk_2:
4-2 — hk-2,k-l + Мк-2,к~1Гк-1 I
(П.19)
или после подстановки (П.18) в (П.19)
4-2 — hk-2,k + Mk_2,kfki
(П.20)
где hk-2,k — ^к-2,к- 1 + ^к-2,к-l^fc-l,k 9 ^к-2,к ~ ^к-2,к-l^k- 1,»»
В последних выражениях hk_2,k ~это вектор смещения начала
системы к относительно начала системы к — 2, записанный в
системе к — 2, а Мк_2,к —это матрица поворота осей системы
к — 2 до совмещения их с осями системы к.
. Экстраполируя (П.20) до системы с номером 0, получаем
>
4> = ho,к + М0.к4, (П.21)
где hOk — вектор смещения начала системы к относительно начала
системы 0, заданный в системе 0 и равный
ho,к — ho, i + М0>1Ч2 + Л/0>1М1>2й2>3 4-... 4- Мо> 1М1>2Л/2>3...
• • • ^к— 2,к— lhk — 1,>,
(П.22)
a MG,k — матрица поворота системы к относительно системы О,
равная произведению матриц:
Мо,к — Mo, lt 2М2, з... 2,к-i-М*- i.k •
(П.23)
Связь между радиусами-векторами одной и той же точки, за-
писанными в первой и последней системах заданной последова-
тельности, т. е. в системах 0 и т, подчиняется аналогичным
формулам:
4> = h0, т 4- Mo, пгп; (П.24)
ho,m — ho,l + 1^1,2 + ... + Mo, iAfi>2--*^A-2,k-l^fc-l,fc 4" ...
... 4* Мqj 1-^11,2 ♦ ♦ • — 2,т — 1^щ — 1, т » (П.25)
М(),т — М о, jM 2,3 • •• Мк_ itk... Мт-2,т- 1Мт- 1,т-
(П.26)
Точно так же можно связать радиусы-векторы, записанные
в системах кит:
где
(П.27)
+... 4- Л1ь>ь+1МЛ+1>й+2...
h^k,m ^к,к+ 1^к + 1,к + 2 • ♦ 2,т — 1^т — 1,т *
(П.28)
(П.29)
Введем матрицы 4x4, которые будут содержать и М
что позволит упростить формулы перехода из одной системы
координат в другую и, следовательно, упростить вычислительные
290
1
программы *. Сконструируем матрицу Bk_lk из матрицы М
и вектора hk_ltk следующим образом:
в
(П.30)
о
или в развернутом
W12
™22
™32
О
Wn
т21
™31
О
виде
h
™13
™23
™33
О
(П.31)
Матрицу В будем называть расширенной матрицей перехода. С ее
помощью можно преобразовать координаты произвольной точки
при переходе от записи их в системе к к записи в системе к — 1.
Для возможности такого преобразования введем расширенный
вектор Rk в виде столбцовой матрицы 4x1, которая получается
из столбцовой матрицы гк размером 3x1 приписыванием еще
одного элемента, равного единице:
(П.32)
Умножим Вк-i,k на Rk:
J
о
(П.33)
но, согласно (П.18),
так что, применяя понятие расширенного вектора к rk_t,
получим
(П.34)
Формула (П.34) оправдывает название матрицы В как расши-
ренной матрицы перехода, включающей в себя не только поворот,
но и сдвиг одной системы координат относительно другой.
При использовании R и В формулы (П.21), (П.24), (П.27)
соответственно преобразуются
г
!
1
т*
(П.35)
* За удобство программирования приходится платить боль-
шими затратами машинного времени (из-за операций с нулевыми
элементами матриц).
291
1
где
Во
Mo.k h
о
Мо
. О
о. к
к(>,т
кк.т
(П.36)
(П.37)
(П.38)
В преобразованных формулах отсутствует операция сложения.
Для программирования они удобнее, но требуют большего числа
вычислительных операций.
вид
и их производных
Рассмотрим частные случаи взаимного расположения систем к и
к — 1 Пусть система к получается из системы к
нательного перемещения (сдвига) последней вдоль оси
расстояние s. При этом матрица В, которую в этом
обозначим Всд (4 s), согласно (П.31), приобретает вид
— 1 путем
посту-
х на
случае
(П.39)
Две другие матрицы сдвига вдоль координатных осей у и z выглядят
так:
о
$ > Вод (к* s)
О
(П.40)
Если система к может быть
тем поворота последней вокруг
осей на угол ф
вращения:
то матрицу
О
получена из системы к — 1 пу-
одной из своих координатных
В будем называть матрицей
costp
<р) =
cos ф
О
costp
t
•1
1
г
1
1
1
L О
О
о
о
О
о
о
О
о
о
О
О
О
О
О
О О
о о
о о
о
о
О
О О
О
О
О
О
О
О
о
о
о
о
о
о
о
о
COS ф
О
о
о
О
COS ф
sin ф
О
о
COS ф
О
О
(П.41)
о
о
о
1
В выражениях (П.41) угол <р считается положительным, если
поворот системы к — 1 вокруг соответствующей оси до совмещения
с системой к виделся против часовой стрелки.
Производные от матриц В по своим аргументам имеют вид:
О
О
о
о о
о о
о
о
о
о
ds
(П.42)
О
0 0 0 0
0 0 0 1
0 0 0 0
0 0 0 0
_„ вами
0 0 0 0
«^ = йсд(£)= 0 0 0 0
ds 7 0 0 0 1
.0 0 0 0.
'вр G, ф) Овр (0 >
dBBp (/, <р)
d9
— ^вр 0) ®вр ф) — ^вр О’ ф) Овр (/) >
/
dBBp (ft, ф) _
dtp
(к) Ввр (к, ф) — Ввр (fc, ф) Овр (к),
(ПЛЗ)
(П.44)
(П.45)
(П.46)
(П.47)
где (в блочной записи)
О’
0
(П.48)
' Справедливы следующие равенства:
(П.49)
(П.50)
В последней формуле матрица В — это любая матрица перехода
типа (П.30), а Ц.д — любая матрица (П.42)—(П.44).
1
Приложение III
Моделирование позиционирования (описание алгоритма)
Блок 1 — начало программы (рис. П.1). Блок 2 — ввод исходных
данных: масса поступательно движущихся частей манипулятора
М2, масса переносимой детали жесткость упругой связи с,
коэффициенты программной точки позиционирования к, передаточ-
ное число редуктора i, радиус шестерни-рейки г, коэффициенты
Xi, 12j 1з> ^4» шаг печати Р, шаг интегрирования Т, момент
инерции J.
Блок 3 — начало расчета Т = 0, счетчик циклов N = 0.
Блок 4 — расчет приведенной массы поступательно движущих-
ся частей манипулятора
»
Мо = М2 + (i2/r2)Jt.
Начало расчета
ю
ВВод исходных
данных
Расчет приведенной массы
Расчет коэффициентов
ЕЯ
Задание начальных ус-
Расчет произВодных
Расчет ускорения
схдата yt
Расчет значения произ-
водной у*
Рис.
294
Блок 5 — расчет коэффициентов заданного дифференциального
уравнения, описывающего движение детали:
01 — (^1 4* ^2 4” ^-з 4" ^4)5
о4
Блок 6 — задание начальных условий уь ylt уъ у2.
Блок 7 — расчет производных
У1 = Ci (у2 - yi)/Mi; "yi = ci (у2 - yi)/Mi\
*
y|v = — Oi У1 — О2У1 — Я3У1 ” о4 (у — к).
Блок 8 — расчет силы управления
Блок 9 — расчет ускорения схвата
У2 = [F - ci (у2 - У1)]/Мо-
Блок 10 — расчет текущего значения производной
v = Г F ~ <У2 ~ У1) _ 1 _£1_
L м0
Блок 11 — печать значений управляющей силы,
детали и схвата и производных от перемещений.
Блок 13 — обнуление счетчика N = 0.
Блок 14 — переход к новому циклу расчета
Т = Т + Т1.
координат
N = N 4-1,
НИИ
Блок 15 — численное интегрирование ранее полученных значе-
старших производных и $т\
Блок 16 — расчет текущего значения силы управления.
Блоки 17, 18 — расчет текущих значений производных $
У!
Блок 19 — сравнение текущего значения счетчика N с задан-
ным шагом печати.
Блок 20 — печать значений силы управления, координат и
производных от перемещений детали и схвата.
Блок 21 — конец программы.
Моделирование прижима детали к поверхности с заданной
силой (описание алгоритма)
Блок 1 — начало программы (рис. П.2). Блок 2 — ввод исходных
данных: масса поступательно движущихся частей манипулятора
М2, момент инерции Jit передаточное число редуктора i,
радиус шестерни-рейки г, параметры двигателя Дя, I*, Ja,
ко, жесткость упругой связи с2, константы Х3,
км>
сила
прижима К, координаты центра масс детали у2.
295
1
Начало расчета
Т=0. N-D
Ввод исходных данных
и начальных условий
Расчет координат
центра масс штифта
и производных от пере-
мещения
--------1—-------
Расчет приведенной
массы Мо и коэффи-
циентов а0) в0, с0
Ту. I---------------
Расчет коэффициен-
тов дифференциале -
кого уравнения
Расчет силы N и ^произ-
водных N, N
Задание начальных
условий
Расчет коэффициентов
и желаемых
производных x/v, 0(lv
Расчет управляющего
напряжения привода
.3-------1---------
Расчет управляющих
Расчет значения
производной у 2
Рис.
Расчет новых значений
производных x'vf < ?
Численное интегрирование
расчет координат
Ц=0
Коней,
Печать x1fy1f<3ifN
Блок 3 — начало
N = 0.
Блок 4 — расчет
— М2 4- (i2/r2) J t и
Cq =
расчета, время расчета Т = 0, счетчик
приведенной массы манипулятора Мо =
коэффициентов а0 = rrx/(ikM); b0 = kwi/r;
1
Блок 5 — расчет коэффициентов заданного дифференциального
уравнения, описывающего процесс прижима деталей:
Блок 6 — задание начальных условий начальной координаты
схвата у2, скорости у2 и ускорения у2.
Блок 7 — расчет управляющего напряжения привода
►
и = {Х4у2 — ^-5у2 + [(у2 — yi) — (fco — У1)]} +
+ у2 (bo + c0Ci) + а0 [моу2 + сл(у2- уг)].
Блок 8 — расчет текущего значения производной *у2
’у2 = [м - у2(Ь0 + c0Ci) - ао(МоУ2 + ci (у2 - уi))]/(c0М0).
Блок 9 — печать значений управляющего напряжения, коорди-
нат геометрического центра схвата, производных от перемещения.
Блок 10 — расчет времени интегрирования и числа циклов
Т = Т + Tl, N = N + 1.
Блок И — численное интегрирование производной ‘у*
Блок 12 — сравнение текущего значения счетчика N с заданным
значением шага печати Рр
Блок 13 — печать управляющего напряжения, координат схвата
и производных.
Блок 14 — обнуление счетчика N = 0.
Блок 15 — конец программы.
Моделирование сопряжения деталей (описание алгоритма)
Блок 1 — начало программы (рис. П.З). Блок 2 — ввод исходных
данных и начальных значений: длина штифта I, диаметр штифта d,
масса штифта жесткость упругой связи сх, су, сг, коэф-
фициент трения /, масса поступательно движущихся частей
манипулятора М2, конечные значения координат хь уь а, началь-
ные значения координат xt, х2, а, Р и их производные.
Блок 3 — расчет начальных значений координат у1} у2 и их
производных на основе геометрического условия связи
У1 = 1/2 — (d/2 - xt) а;
у2 + У! + Д.
Блок 4 — расчет нормальной силы N в точке контакта дета-
лей и ее производных
N — [М1’У1 _ су(у2 — yt) + Pi];
A' = [(l + /а)(М1‘у - Ct (у2 - ух)) - /«(М/у -
~ с2(у2 - У1) + Л)]/(1 + /а)2;
1
N = [(1 + ra)(Mi/v - - О - -
- c2(y2 ~ У1) + Pi)]/(1 + /a)2 - [(1 + fa)(Mfy -cy(y2- }i)) -
- fa(Miyt -cy(y2- yO + PJ] 2fa.
Блок 5 — расчет коэффициентов дифференциальных уравнений,
описывающих движение штифта:
TJj — —(Xj + %2 "Ь ^-3 ^4)»
D3 —
(Х1Х2Х3 + Хр 2 '4 "Ь Х1Х3Л4 "Ь ^З^З^)»
Da. —
X । А2Х3Х4,
и производных X
IV
X17 = D1*X1 + 1>2*1
+ 03*1
+ £>4 (х - Xfc);
aIV = Di а + D2a + D3a + D4 (a — осД
Блок 6 — расчет управляющих сил приводов
Fx = (—Afix|v + cxXi — N(a — f) — 2Na — Na) + cx(x2 — xt);
^X
F, = [МоГ - (H - Rt)Ks] —+ R. + ₽M + nM2;
Cy
Fp = (—JiO^v + caa + $v3 + 2Kfo4 + Nvs) + ca(P — a\
Ca
где Rit K4, RSt v3t t>4, v5 — коэффициенты.
Блок 7 — расчет новых значений производных x{v, aIV, х2,
К ₽•
Блок 8 — численное интегрирование производных, расчет новых
значений координат штифта и угла Перекоса а.
сравнение текущего числа циклов интегрирования Q
о«
Блок 9
с заданным шагом печати P
Блок 10 — печать значений координат штифта хь уЬе угла
перекоса ос и нормальной силы N точки контакта.
Блок И - обнуление счетчика 6 = 0.
Блок 12 — конец программы.
1
Список литературы
1. Артоболевский И. И. Теория механизмов и машин. М., 1975.
2. Аузинъш Я. П., Слиеде П. Б. Эффективный алгоритм
автоматизации моделирования динамики механизмов на ЭЦВМ //
Вопросы динамики и прочности. Рига, 1980. Вып. 37. С. 5 — 12.
3. Балакирева Т. Н., Воробьев Е. И. Управление движением
сборочного робота с переменной динамической моделью и ограни-
чениями на нормальные силы//Изв. АН СССР. Механика твер-
дого тела. 1986. № 2. С. 85 — 102.
4. Белянин П. Н. Промышленные роботы. М., 1975.
5. Бухгольц Н. Н. Основной курс теоретической механики.
М., 1967.
6. Верещагин А. Ф. Принцип наименьшего принуждения Гаус-
са для моделирования на ЭВМ динамики роботов-манипулято-
ров //Докл. АН СССР. 1975. Т. 220. Вып. № 1. С. 51-53.
7. Виттенбург И. С. Динамика систем твердых тел. М., 1980.
8. Воробьев Е. И. Анализ кинематики пространственных
исполнительных механизмов манипуляторов методом матриц //
Механика машин. 1970. Вып. 28 — 30. С. 30—37.
9. Воробьев Е. И. Алгоритм моделирования динамики меха-
низмов манипуляторов и промышленных роботов//Механика ма-
шин. 1978. Вып. 53. С. 8 — 16.
10. Воробьев Е. И. Построение уравнений программного
движения пространственных механизмов с несколькими степенями
свободы//Машиноведение. 1981. № 5. С. 42 — 46.
11. Вукобратович 'М., Стокич Д. Управление манипуляцион-
ными роботами. М., 1985.
12. Галиуллин А. С. Обратные задачи динамики. М., 1981.
13. Диментберг Ф. М. Теория винтов и ее приложения. М.,
1978.
14. Диментберг Ф. М. Теория пространственных шарнирных
механизмов. М., 1982.
15. Динамика управления роботами/Под ред. Е. И. Юревича,.
М., 1984.
16. Игнатьев М. Б., Кулаков Ф. М., Покровский А. М.
Алгоритмы управления роботами-манипуляторами. Л., 1972.
17. Коренев Г. В. Целенаправленная механика управляемых
манипуляторов. М., 1979.
18. Корендясев А. И., Саламандра Б. Л., Тывес Л. И. К ре-
шению в явном виде обратной задачи о положениях мани-
пуляторов с шестью степенями подвижности // Машиноведение.
1986. № 3. С. 10-21.
19. Крутько Г. Д., Попов Е. П. Построение алгоритмов
управления движением манипуляционных роботов // Докл. АН СССР.
1980. Т. 255. № 1. С. 40-43.
20. Козырев Ю. Г. Промышленные роботы. Справочник. М.,
1983.
299
1
21. Кулаков Ф. М. Супервизорное управление манипуля-
ционными роботами. М., 1980.
22. Кулешов В. С., Лакота Н. А. Динамика систем управ-
ления манипуляторами. М., 1971.
23. Лакота Н. А., Рахманов Е. В., Шведов В. И. Управление
упругим манипулятором по траектории//Изв. АН СССР. Техни-
ческая кибернетика. 1980. № 2. С. 53 — 59.
24. Лебедев П. А. Векторные уравнения взаимозависимости
от кинематических параметров пространственных механизмов // Ма-
шиноведение. 1982. № 4. С. 54 — 58.
25. Лурье А. И. Аналитическая механика. М., 1961.
26. Медведев В. С., Лесков А. Г., Ющенко А. С. Системы
управления манипуляционных роботов. М., 1978.
27. Михайлов С. А., Черноусько Ф. Л. Динамика упругого
манипулятора при‘заданных управляющих моментах или движе-
ниях перемещаемого груза//Изв. АН СССР. Механика твердого
тела. 1984. № 2. С 51-58.
28. Морошкин Ю. Ф. Определение конфигураций механиз-
мов//Докл. АН СССР. 1952. Т. 82. № 4.
29. Морошкин Ю. Ф. О формах основных уравнений геомет-
рии механизмов//Докл. АН СССР. 1953. Т. 91. № 4.
30. Овакимов А. Г. О дополнениях к методу замкнутого
векторного контура при решении задачи о положениях про-
странственных механизмов с несколькими степенями свободы // Ма-
шиноведение. 1969. № 4. С. 12—15.
31. Овакимов А. Г. Аналоги скоростей и ускорений про-
странственных механизмов с несколькими степенями свободы // Ма-
шиноведение. 1969. № 6. С. 51 — 58.
32. Овакимов А. Г. Аналитический метод определения ско-
ростей и ускорений пространственных механизмов с несколькими
степенями свободы//Механика машин. 1971. Вып. 35 — 36.
С. 45-62.
33. Овакимов А. Г. Кинематическое исследование пространст-
венной цепи управляющего механизма манипулятора//Изв. вузов.
Машиностроение. 1971. № 4. С. 58—62.
34. Овакимов А. Г. Обобщенный способ учета инерции
различных схем вращательного привода в уравнениях движе-
ния манипуляторов//Машиноведение. 1979. № 4. С. 25 — 31.
35. Павлов Б. И. Алгоритмизация решения задач кинематики
пространственных механизмов // Исследование динамических систем
на ЭВМ. М., 1982. С. 99-109.
36. Пейсах Э. Е. Анализ положений манипулятора с шестью
степенями свободы//Робототехника. Л., ЛПИ, 1979. Вып. 2.
37. Пейсах Э. Е. Проблема кинематики роботов-манипуля-
торов и система программ «Робот» // Роботы и робототехни-
ческие системы. Иркутск. 1981. С. 22—40.
38. Петров Б. Н„ Крутъко П. Д., Попов Е. П. Построение
алгоритмов управления как обратная задача динамики//Докл.
АН СССР. 1979. Т. 247. № 5. С. 1078-1081.
300
39. Попов Е. П., Верещагин А. Ф., Зенкевич С. Л. Манипу-
ляционные роботы. Динамика и алгоритмы. М., 1978.
40. Попов С. А. Курсовое проектирование по теории ме-
ханизмов и механике машин. М., 1986.
41. Промышленная робототехника / Под ред. Я. А.
М., 1982.
Шифрина.
42. Промышленная робототехника и гибкие автоматизирован-
ные производства / Под ред. Е. И. Юревича. Л., 1984.
43. Рапопорт Г. Н., Солин Ю. В. Применение промышлен-
ных роботов. М., 1985.
44. Робототехника и гибкие автоматизированные производ-
ства/Под ред. И. М. Макарова. М., 1986. Кн. 1—9.
45. Слиеде П. Б. Конструкции, кинематика и динамика
исполнительных механизмов манипуляционных роботов. — М.:
ЦНТО им. С. И. Вавилова, 1986. 59 с.
46. Теория механизмов и машин/Под ред. К. В. Фролова.
М., 1987.
47. Тимофеев А. В. Управление роботами. Л., 1986.
48. Уикер И. Динамика пространственных механизмов//Кон-
струирование и технология машиностроения. М., 1969. № 1.
С. 264-278.
49. Черноусько Ф. Л. Динамика управляемых движений
упругого манипулятора//Изв. АН СССР. Техническая киберне-
тика. 1981. № 5. С. 142-152.
50. Чейс М. А. Векторный метод анализа механизма//Кон-
струирование и технология машиностроения. М., 1963. № 3.
51. Юревич Е. И и др. Устройство промышленных роботов.
Л., 1980.
52. Юревич Е. И. Динамика управления роботами. М., 1984.
53. Pennock G. R., Yang А. Т. Instantaneous Kinematics of Three —
Parameter Motions//Jornal of Mechanisms, Transmissions and
Automation in Design. 1985. No. 2. P. 177-182.
54. lilov L., Wittenburg J. Bewegunqsqleichunqen fur Systeme
starrer Korper mit Gelenken beliebiger Eigenschaften//ZAMM.
1977. 57. P. 137-152.
55. lilov L., Loren M. Dynamic Analysis of Multirigid —
Body System Based on the Gauss Principle//Z A MM. 1982. 62. No. 11.
P. 539-545.
56. Luh J. Y. S.t Walker M. W., Paul R. P. C. Online
computational scheme for mechanical manipulators//Traus. ASME.
J. Dyn. Snst., Meas., and Contr. 1980. 102. No. 2. P. 69 — 76.
57. Scutedeg W. Nichtlineare Bewegungsgreichungen groBer Mehr-
korpersystem//ZAMM. 1981. 61. No. 9. P. 413—420.
Оглавление
Предисловие
Введение
Раздел I. Кинематика манипуляторов
Глава 1. Основные понятия и определения.................... И
1.1. Символическое представление структуры манипуляторов 14
1.2. Задачи кинематики и динамики манипуляторов .... 16
>
2. Векторны
метод кинематического анализа ма-
нипуляторов
2.1. Прямая задача о положениях .... . .—.—.--18
2.2. Обратная задача о положениях . ". г~. Г .... 23
2.3. Прямая задача о скоростях ......................... 42
2.4. Обратная задача о скоростях................... 50
2.5. Угловые ускорения звеньев и их аналоги ..... 61
2.6. Линейные ускорения........................... 63
2.7. Ускорения высоких порядков ........................ 70
2.8. Автоматизированные методы кинематического анализа
манипуляторов (прямая задача)...................... 73
Глава 3. Метод матриц в кинематике манипуляторов . .
3.1.
3.2.
3.3.
3.4.
3.5.
Специальные системы координат..
Расширенная матрица перехода для кинематической
пары ...................
Прямая задача’о положениях.....
Обратная задача о положениях .........
Определение законов изменения обобщенных коорди-
нат при движении точки схвата по заданной траекто-
78
79
82
88
3.6.
3.7.
3.8.
3.9.
рии .............................................
Обобщенные скорости и ускорения............. . • .
Скорости и ускорения точек звеньев...............
Алгоритм оптимизации быстродействия манипулятора
96
99
100
105
Построение алгоритма движения промышленного робо-
та в робототехническом комплексе из условия мини-
мизации времени перехода..........................
3.10. Решения обратных задач о положениях манипуляторов
в явном виде методом матриц...........................
Глава 4. Метод винтов и
(уальных матриц в кинематике
109
117
манипуляторов
131
i
г-
4.1. Сведения из классической теории винтов и винтового
исчисления ..........................................131 1
4.2. Группы кинематических винтов манипуляторов ... 136
4.3. Метод дуальных матриц .... ...............143
302
Раздел 2. Динамика манипуляторов
Глава 5. Метод кинетостатики в динамике манипуляторов
147
5.1. Силы инерции и моменты сил инерции звеньев . . . 147
5.2. Уравнения движения манипулятора.................. 150
5.3. Примеры кинетостатического анализа манипуляторов 153
5.4. Динамика манипулятора с учетом кулоновского трения
в кинематических парах................................ 168
5.5. Динамическая модель манипулятора с упругой связью
в схвате.............................................. 175
5.6. Динамическая модель манипулятора с контактным вза-
имодействием ......................................... 178
. 5.7. Динамическая модель сборочного промышленного ро-
бота ................................................... 186
5.8. Автоматизированный анализ динамики манипуляторов
на основе метода кинетостатики.....................188
Глава 6. Уравнения Лагранжа и принцип Даламбера в ди-
намике манипуляторов ..................................
194
6.1. Кинетическая энергия манипулятора.................195
6.2. Потенциальная энергия манипулятора................199
6.3. Обобщенные силы...................................201
6.4. Вывод уравнений Лагранжа II рода в матричной форме 206
6.5. Алгоритмы решения задач динамики манипуляторов
с помощью уравнений Лагранжа II рода . . . ,. . 211
6.6. Определение реакций в кинематических парах (в матрич-
ной форме).............................................216
6.7. Уравнения Лагранжа I рода.........................227
6.8. Дополнительные факторы, влияющие на динамику ма-
нипуляторов ......................................... 230
Глава 7. Принцип Гаусса в динамике манипуляторов .
232
7.1. Вычисление функции принуждения...................232
7.2. Минимизация принуждения с учетом связей .... 236
7.3. Алгоритм определения обобщенных ускорений и реак-
ций ................................................ 238
Глава
8. Обратные задачи динамики манипуляторов (опре-
деление структуры сил и постоянных параметров си-
стемы)
8.1.
8.2.
8.3.
8.4.
Построение уравнений движения манипулятора по диффе-
ренциальной программе.
Построение уравнений движения манипулятора по го-
лономной программе...............................
Определение управляющих сил при позиционировании
промышленного робота..............................
Определение управляющих сил при выводе схвата мани-
пулятора в заданную точку пространства с заданной ско-
ростью ......................................
245
247
253
257
259
303
8.5. Построение алгоритма управления манипулятором, вы-
полняющим операцию сборки.................262
8.6. Обратные задачи динамики манипуляторов, выполняю-
щих обработку поверхностей................. .268
8.7. Построение алгоритма управления движением манипу-
лятора по заданной траектории............... 275
Приложение!..............-...................282
Приложение II................................284
Приложение III........................•. . . 294
Список литературы ..........................299
Учебное издание
МЕХАНИКА ПРОМЫШЛЕННЫХ РОБОТОВ
В 3 книгах
Книга 1
Евгений Иванович Воробьев
Сергей Александрович Попов
Галина Ивановна Шевелева
Кинематика и динамика
Заведующий редакцией А. В. Дубровский
Редактор Л. Н. Шатунова
Младший редактор Т. Ф. Артюхина
Художник Е. Н. Урусов
Художественный редактор Л. К. Громова
Технический редактор 3. В. Нуждина
Корректор В. В. Кожуткина
ИБ № 7117
Изд. № ОТ-671. Сдано в набор 10.11.87. Поди, в печать 11.05.88.
Формат 84 х 1081/32. Бум. тип. № 2. Гарнитура тайме. Печать высокая.
Объем 15,96 усл. печ. л. 16,17 усл. кр.-отт. 15,08 уч.-изд. л. Тираж 44000 экз.
Зак. № 1259. Цена 55 коп.
Издательство «Высшая школа». 101430, Москва, ГСП-4, Неглинная ул., д. 29/14.
Ордена Октябрьской Революции, ордена Трудового Красного Знамени
Ленинградское производственно-техническое объединение «Печатный Двор»
имени А. М. Горького Союзполиграфпрома при Государственном коми-
тете СССР по делам издательств, полиграфии и книжной торговли. 197136,
Ленинград, П-136, Чкаловский пр., 15.
1