Министерство образования Республики Беларусь БЕЛОРУССКИЙ НАЦИОНАЛЬНЫЙ ТЕХНИЧЕСКИЙ УНИВЕРСИТЕТ Кафедра «Робототехнические системы» А.Р. Околов Е.Р. Новичихина Г.С. Свидерский МАТЕМАТИЧЕСКОЕ ОБЕСПЕЧЕНИЕ ПРОМЫШЛЕННЫХ РОБОТОВ Учебно-методический комплекс Часть 1 Минск БНТУ 2012 Министерство образования Республики Беларусь БЕЛОРУССКИЙ НАЦИОНАЛЬНЫЙ ТЕХНИЧЕСКИЙ УНИВЕРСИТЕТ Кафедра «Робототехнические системы» А.Р. Околов Е.Р. Новичихина Г.С. Свидерский МАТЕМАТИЧЕСКОЕ ОБЕСПЕЧЕНИЕ ПРОМЫШЛЕННЫХ РОБОТОВ Учебно-методический комплекс для студентов специальности 1-53 01 06 «Промышленные роботы и робототехнические комплексы» В 2 частях Часть 1 ЛАБОРАТОРНЫЕ РАБОТЫ Минск БНТУ 2012 УДК 007.52:51(076.5) ББК 32.816я7 О-51 Рецензенты: Н.Н. Гурский, С.Н. Павлович Околов, А.Р. О-51 Математическое обеспечение промышленных роботов: учебно-методический комплекс для студентов специально- сти 1-53 01 06 «Промышленные роботы и робототехнические комплексы» : в 2 ч. / А.Р. Околов, Е.Р. Новичихина, Г.С. Свидерский. – Минск : БНТУ, 2012. – Ч. 1 : Лаборатор- ные работы. – 80 с. ISBN 978-985-550-092-7 (Ч. 1). Издание является первой частью учебно-методического комплекса. Цель выполнения лабораторных работ – повторение студентами лекцион- ного материала и закрепление его на практике. Рассмотрены основные во- просы, связанные с математическим описанием динамики и кинематики промышленного робота, планированием и моделированием траектории движения робота, а также решением прямой и обратной задач кинематики и динамики. Учебно-методический комплекс предназначен для студентов, инжене- ров и преподавателей, занимающихся проектированием и эксплуатацией промышленных роботов. УДК 007.52:51(076.5) ББК 32.816я7 ISBN 978-985-550-092-7 (Ч. 1) © Околов А.Р., Новичихина Е.Р, ISBN 978-985-550-093-4 Свидерский Г.С., 2012 © Белорусский национальный технический университет, 2012 3 ЛАБОРАТОРНАЯ РАБОТА № 1 Матрица однородного преобразования Цель работы: изучить матрицы однородного преобразования. 1. Основные положения Матричная и векторная алгебра применяются для описания и представления расположения звеньев манипулятора относительно заданной абсолютной системы координат. Для этого каждому звену определяется связанная с ним система координат, оси которой па- раллельны осям сочленений звеньев. Звенья манипулятора могут совершать вращательное и/или по- ступательное движение относительно абсолютной системы коорди- нат. Для описания вращательного движения связанной системы от- счета относительно абсолютной используется матрица поворота размерностью 3х3, для описания поступательного перемещения центра связанной системы координат относительно центра абсо- лютной системы координат используется вектор положения раз- мерностью 3х1, а для учета совместного поступательного и враща- тельного движения используется матрица однородного преобразо- вания размерностью 4х4. 1.1 Матрица поворота Ориентацию одной системы относительно другой можно задать с помощью матрицы поворота. Рассмотрим две системы координат: систему координат OXYZ с осями OX, OY, OZ и систему OUVW с осями OU, OV, OW. 4 Рисунок 1.1 – две системы координат Начала этих систем совпадают и расположены в точке О. Систе- ма OXYZ фиксирована в трехмерном пространстве и принята за абсолютную система координат, OUVW вращается относительно абсолютной системы OXYZ. Пусть (ix, jy, kz) и (iu, jv, kw) - единичные векторы, направленные вдоль осей систем OXYZ и OUVW соответ- ственно. Матрица поворота определяет положение осей повернутой си- стемы координат относительно абсолютной системы координат. Векторы столбцы этой матрицы задают в системе OXYZ координа- ты единичных векторов в направлении основных осей системы OUVW. Значения элементов матрицы поворота можно определить как: wzvzuz wyvyuy wxvxux kkjkik kjjjij kijiii R , т. е. первый столбец матрицы поворота определяет координаты повернутой оси OU относительно осей абсолютной системы коор- динат OXYZ, второй столбец определяет координаты оси OV в си- О Z V P Y U X W 5 стеме OXYZ, третий столбец определяет координаты оси OZ в си- стеме OXYZ. С помощью матрицы поворота можно связать координаты точки Рxyz, заданные в абсолютной системе координат, с координатами той же точки Рuvw, заданными в повернутой системе координат. w u wzvzuz wyvyuy wxvxux z y x P Pv P kkjkik kjjjij kijiii P P P . Ниже приводится ряд полезных свойств матриц поворота: 1. Каждый столбец матрицы поворота представляет собой еди- ничный вектор в направлении соответствующей оси повернутой системы отсчета, заданный своими координатами относительно аб- солютной системы координат. Каждая строка матрицы поворота представляет собой единичный вектор в направлении соответству- ющей оси абсолютной системы координат, заданный своими коор- динатами относительно повернутой системы отсчета OUVW. 2. Поскольку каждый столбец и строка представляют собой ко- ординаты единочного вектора, длина векторов, определяемых стро- ками и столбцами матрицы поворота, равна 1. 3. Поскольку столбцы (строки) матрицы поворота являются век- торами, составляющими ортонормированный базис, скалярное про- изведение векторов, определяемых двумя различными столбцами (строками), равно нулю. 4. Операция обращения матрицы поворота совпадает с операци- ей транспонирования: R~1=RT и R•RT = I3, где I3 - единичная матри- ца размереностью 3х3. 1.2 Однородные координаты Поскольку трехмерная матрица поворота не несет информации о поступательном перемещении и используемом масштабе, вектор координат Р=(px,py, pz) T в трехмерном пространстве дополняют чет- вертой координатой (или компонентой) так, что он принимает вид 6 Р=(wpx,wpy, wpz, w) T. В этом случае говорят, что вектор Р выражен в однородных координатах. Описание точек трехмерного простран- ства однородными координатами позволяет ввести в рассмотрение матричное преобразования, содержащие одновременно поворот, параллельный перенос, изменение масштаба и преобразование пер- спективы. В общем случае изображение N-мерного вектора вектором раз- мерностью N +1 называется представлением в однородных коорди- натах. При таком представлении преобразование N-мерного векто- ра производиться в (N+1)-мерном пространстве, а физический N- мерный вектор получается делением однородных координат на (N+1)-ю компоненту w. Так, вектор Р=(px,py, pz) T положения в трех- мерном пространстве в однородных координатах представляется расширенным вектором (wpx,wpy, wpz, w) T.Физические координаты связаны с однородными следующим образом: .,, w wp p w wp p w wp p zz y y x x Представление трехмерного вектора положения в однородных координатах не единственно. Например Р1=(w1px,w1py, w1pz, w1) T и Р2=(w2px,w2py, w2pz, w 2) T являются различными однородными пред- ставлениями одного и того же вектора положения Р=(px,py, pz) T . Та- ким образом, четвертую компоненту w вектора однородных коор- динат можно рассматривать как масштабирующий множитель. Если эта компонента равна 1 (w=1), то однородные координаты вектора положения совпадают с его физическими координатами. В робото- технике масштабирующий множитель всегда выбирают равным 1, а в задачах машинной графики он принимает любое положительное значение. 1.3 Однородная матрица преобразования Однородная матрица преобразования представляет собой матри- цу размерностью 4х4, которая преобразует вектор, выраженный в однородных координатах, из одной системы отсчета в другую. Од- нородная матрица преобразования может быть разбита на четыре подматрицы: 7 . 1131 1333 ваниеМасштабироыперспектив аниеПреобразов СдвигПоворот sf PR T Верхняя левая подматрица размерностью 3х3 представляет со- бой матрицу поворота; верхняя правая подматрица размерностью 3х1 представляет собой вектор положения начала координат повер- нутой системы отсчета относительно абсолютной; нижняя левая матрица размерностью 1х3 задает преобразование перспективы; четвертый диагональный элемент является глобальным маштаби- рующим множителем. Однородная матрица преобразования позво- ляет выявить геометрическую связь между связанной системой от- счета OUVW и абсолютной системой OXYZ. Если вектор Р трехмерного пространства выражен в однородных координатах (т.е. Р=(px,py, pz,1) T, то, используя понятие матрицы преобразования, можно сформировать однородную матрицу преоб- разования ROT(k,Q) , задающую преобразование поворота вокруг вектора k на угол Q и имеющую размерность 4х4. Однородная мат- рица поворота получается соответствующим расширением обычной матрицы поворота, имеющей размерность 3х3. Так однородное преобразование, описывающее поворот вокруг оси Х на угол будет иметь вид: . 1000 0cossin0 0sincos0 0001 ),(xRot Поворот вокруг оси Y на угол : 8 1000 0cos0sin 0010 0sin0cos ),(yRot . Поворот вокруг оси Z на угол : 1000 0100 00cossin 00sincos ),(zRot . Эти матрицы размерностью 4x4 называются однородными мат- рицами элементарных поворотов. Верхняя правая подматрица однородной матрицы преобразова- ния, имеющая размерность 3x1, задает параллельный перенос си- стемы координат OUVW относительно абсолютной системы OXYZ на вектор (dx, dy, dz) T 1000 100 010 001 dz dy dx TRANS . Эта матрица размерностью 4x4 называется однородной матри- цей элементарного сдвига. Правая нижняя подматрица однородной матрицы преобразова- ния размерностью 1х1 определяет глобальное преобразование мас- штаба 9 s z y x z y x s 1000 0100 0010 0001 , где s>0. Физические декартовы координаты вектора будут рав- ны: , s x px , s y p y , s z pz 1 s s w . Таким образом, четвертый диагональный элемент однородной матрицы преобразования определяет глобальное сжатие координат, если s>1, и растяжение, если 00)AND(ttp)AND(t<(tp+tpv)) then begin qq:=qp+v*t*k; vv:=v*k; aa:=0; 38 end else begin qq:=qk-1/2*a*k*sqr(tk-t); vv:=a*k*(tk-t); aa:=-a*k; end; Условные обозначения: t – текущий момент времени; tp – время разгона; tpv – время равномерного движения; tk – конечное время проведения расчетов; k – коэффициент пропорциональности; qq – текущее положение; vv – текущее значение скорости; aa – текущее значение ускорения; k – коэффициент пропорциональности; qn – начальное положение; qp – положение после разгона; qk – конечное положение; v – максимальное значение скорости; a – максимальное значение ускорения. 8. Литература 1. К. Фу, Р.Гонсалес, К. Ли. Робототехника. М.: Мир, 1989. 2. М. Шахинпур. Курс робототехники. М.: Мир, 1990. 3. Справочник по промышленной робототехнике: Кн. 1. М.: Ма- шиностроение, 1990 4.А.К.Лапковский. Алгоритмы изображения движущихся тел при параллельном и центральном проецировании. Аксонометрия и компьютеризация изображения. Минск. Наука и техника.,1993. 5.Л.Аммерал. Принципы программирования в машинной графи- ке. Москва. Сол Систем.,1992. 39 ЛАБОРАТОРНАЯ РАБОТА № 5 Планирование прямолинейных траекторий в пространстве декартовых координат Цель работы: изучить планирование прямолинейных траекто- рий в пространстве декартовых координат. 1. Основные положений Начальная, конечная и промежуточные опорные точки траекто- рии могут быть заданы как в обобщенных, так в декартовых коор- динатах. Поэтому планирование траектории может также осу- ществляться также, как в обобщенных, так и в декартовых коорди- натах (см. лаб. раб.№4 - Моделирование движение робота в пространстве обобщенных координат). Известно, что отработка траектории движения манипулятора выполняется в пространстве обобщенных координат. Поэтому если планирование осуществляется в декартовых координатах, то для ее отработки необходимо решать обратную кинематическую задачу о положении манипулятора. Преимущество планирования траектории в пространстве декар- товых координат наглядность и предсказуемость траектории дви- жения конечного звена (схвата) манипулятора. Преимуществами планирования траектории в пространстве обобщенных координат является: - Планируется непосредственно траектория, которую должны отработать приводы звеньев манипулятора; - Планирование траектории требует небольшого количества вычислений, а, следовательно, и машинного времени; - Траекторию легче планировать, так как в этом случае отсут- ствует понятие ориентации. В данной лабораторной работе рассматривается способ линей- ной интерполяции в пространстве декартовых координат. 40 2. Алгоритм линейной интерполяции траектории в декар- товых координатах Для перемещения вдоль прямой линии в декартовых координа- тах с заданной скоростью и изменением ориентации инструмента манипулятора используется линейная интерполяция. Рассмотрим алгоритм линейной интерполяции: Пусть в декартовых системе координат ( 000 zyx ), связанной с основанием манипулятора заданы две точки (рисунок 5.1). Положе- ние и ориентация систем координат первой и второй точки в базо- вой системе координат описывается преобразованиями 1P и 2P со- ответственно. Тогда положение и ориентация системы координат начальной точки описывается преобразованием 3P : 2 1 1 3 3 3 3 1000 PP zaon yaon xaon P zzz yyy xxx . (5.1) Рисунок 5.1 - Манипулятор с заданными двумя точками на основании 41 Необходимо найти такие законы изменения времени элементов преобразования iP3 , чтобы: 1. Рабочая точка инструмента (РТИ) манипулятора двигалась вдоль прямой, соединяющей начало системы координат начальной и конечной точек, с заданной скоростью (формирование элементов столбца положения iii zyx 333 ,, преобразования iP3 ). 2. Изменение ориентаций РТИ выполнялось бы с управляемой угловой скоростью, например, путем ее вращения вокруг фиксиро- ванной в пространстве оси (формирование матрицы вращения пре- образования 3iP ). Если законы формирования преобразования 3iP найдены, то по- ложение и ориентация РТИ манипулятора при ее движении по пря- мой в базовой декартовой системе координат определим как: ii PPP 31 . (5.2) Формирование законов изменения элементов столбца положения преобразования iP3 не вызывает затруднений. На этапе подготовке к интерполяции по заданному значению скорости и допустимому значению ускорения (торможения) найдем времена движения на участках разгона, торможения и равномерного движения. Время и путь разгона: A t p , (5.3) A S p 2 2 . (5.4) Длинна участка равномерного движения равна: pрв SSS 2 . (5.5) 42 Время движения с постоянной скоростью: рв рв S t . (5.6) Время и путь торможения равны времени и пути разгона. Суммарное значение пути определим как: 2 3 2 3 2 3 zyxS . (5.7) Расчет текущих значений пути iS на участке разгона ( ptt0 ) может быть выполнен по формуле: 2 2 1 AtS i . (5.7.1) На участке равномерного движения ( рвtt0 ): tSS pi . (5.7.2) На участке торможения ( тtt0 ): 2 2 1 ttASS тi . (5.7.3) Значение коэффициентов пропорциональности определим как: S x K x 3 , (5.8) 43 S y K y 3 , (5.9) S z K z 3 . (5.10) Тогда расчет текущих значений элементов столбца положения производится по следующим формулам: ixi SKx3 , (5.11) iyi SKy3 , (5.12) izi SKz3 , (5.13) где iS - текущее значение пути. Для определения законов изменения элементов матрицы враще- ния преобразования iP3 воспользуемся методом двух вращений, т.е. переориентацию инструмента будет выполнять при помощи враще- ний вокруг двух осей. Первое вращение на угол необходимо, чтобы перевести ось z системы координат, связанной с РТИ мани- пулятора из начального положения в конечное. Оно выполняется вокруг фиксированной оси 1y . Ось 1y получена путем поворота оси 1y вокруг оси 1z на угол . Второе вращение на угол необ- ходимо, чтобы придать инструменту манипулятора требуемую ори- ентацию. Оно выполняется вокруг оси 2z . Первому вращению соответствует преобразование: 44 1000 0 0 0 2 2 CSSSC SSCVSVCS SCVCSCVS R ii iiii iiii i , (5.14) где .cos1 ,sin ,cos ii ii ii V S C Этому вращению соответствует преобразование: 1000 0100 00 00 ii ii i CS SC R . (5.15) Тогда преобразование iP3 можно представить в виде: iiii RRTP3 , (5.16) где 1000 100 010 001 3 3 3 i i i i z y x T . Элементы левого столбца преобразования iP3 - результат век- торного произведения двух последующих столбцов: 45 .kaoaojaoao ioaao aaa ooo kji aon xyyxzxzz zyzy zyx zyx (5.17) Приравнивая соответствующие элементы матриц в уравнениях (5.1) и (5.16) имеем: , x y a a arctg (5.18) Если оба элемента ya и xa равны нулю, тогда угол не опреде- ляется. Это вырожденных случай. В такой ситуации следует при- дать нулевое значение 0 . 0, 22 z yx a aa acrtg (5.19) , C S arcth , (5.20) где zyx zyx oSSoCVCoVCSC nSSnCVCnVCSS 2 2 . (5.21) Коэффициенты пропорциональности K и K находим как: S K , (5.22) 46 S K . (5.23) Расчет текущих значений углов i и i производим по следую- щим формулам: ii SK , (5.24) ii SK . (5.25) 3. Содержание работы Работа предполагает выполнение следующих заданий: 1. Построить прямолинейную траекторию в декартовых коор- динатах x и y . 2. Построить графики положения, скорости и ускорения в за- висимости от времени. 4.Задание В декартовой системе координат ( 000 zyx ), связанной с основа- нием манипулятора, заданы две точки (рисунок 4.1). Положение и ориентация систем координат начальной и конечной точки в базо- вой системе координат описывается преобразованиями 1P и 2P . Необходимо спланировать прямолинейную траекторию в базо- вой системе координат при движении манипулятора из начальной точки в конечную с заданной скоростью и ускорением A . 47 5. Методика выполнения 1. По заданным значениям матриц однородных преобразований 1P и 2P , скорости и ускорения определить (вручную): - преобразование 3P (1), где 10 1111 1 pRR P TT . - значение суммарного пути S (7), углы ,, (18-20). - коэффициенты пропорциональности zyx KKK ,, (8-10) и KK , (22-23). - параметры участков разгона ( pp St , ), равномерного движе- ния ( pвpвt S, ) и торможения ( mm St , ) (3-6). 2. (на компьютере) - пути iS (7.1-7.3). - элементов столбца положения iii zyx 333 ,, (11-13). - углов ii , (24-25). - элементов преобразования iP3 (16-17). - элементов преобразования iP (2). Программа должна обеспечивать построение: 1. траектории движения РТИ манипулятора в декартовой си- стеме координат (строится по текущим значениям столбца положе- ний преобразования iP ). 2. графиков положения, скорости, ускорения в зависимости от времени (строятся по текущим значениям пути iS , его первой и второй производных). 48 6. Исходные данные № ва- рианта 1 P 2 P A 1 1000 0100 5.005.086.0 4.0086.05.0 1000 0100 8093.037.0 6037.093.0 60 500 0 2 1000 0100 5.0098.021.0 3021.098.0 1000 0100 8024.097.0 5097.024.0 70 650 0 3 1000 0100 5.0086.05.0 205.086.0 1000 0100 8078.091.0 4061.078.0 80 800 0 4 1000 0100 5.0021.098.0 1098.021.0 1000 0100 809.045.0 3045.09.0 75 700 0 5 1000 0100 5.0072.07.0 007.072.0 1000 0100 3097.024.0 2024.097.0 85 950 0 6 1000 0100 3024.097.0 4097.024.0 1000 0100 109.045.0 2045.09.0 80 900 0 49 7. Содержание отчета Отчет должен содержать следующие материалы: 1. Титульный лист 2. Задание 3. Расчеты и результаты вычислений, сделанные в ручную на этапе подготовке. 4. Листинг программы 5. Распечатку исходных данных и результатов вычислений в виде траектории движения РТИ манипулятора и графиков положе- ния, скорости и ускорения в зависимости от времени. 8.Пример реализации Элемент программы реализующей расчет текущих значений по- ложения, скорости и ускорения на участке разгона, равномерного движения и торможения (программа реализована на языке про- граммирования Pascal): if (t>0)AND(ttp)AND(t<(tp+tpv)) then begin ss:=sp+v*t; vv:=v; aa:=0; end else begin ss:=s-1/2*a*sqr(tk-t); vv:=a*(tk-t); aa:=-a; end; 50 Условные обозначения: t – текущий момент времени; tp – время разгона (определяется по формуле (4.3)); tpv – время равномерного движения (определяется по формуле (4.6)); tk – конечное время проведения расчетов; ss – текущее положение; vv – текущее значение скорости; aa – текущее значение ускорения; sp – положение после разгона ((определяется по формуле (4.4)); s – конечное положение (суммарное значение пути, определяется по формуле (4.7)); v – значение скорости (дано по условию); a – значение ускорения (дано по условию). Для вычисления положения РТИ манипулятора в текущий мо- мент в декартовой системе координат необходимо в предыдущей программе добавить расчет элементов столбца положения (форму- лы (4.11) – (4.13)), предварительно рассчитав коэффициенты про- порциональности (формулы (4.8) – (4.10)). 9. Литература 1. К. Фу, Р.Гонсалес, К. Ли. Робототехника. М.: Мир, 1989. 2. М. Шахинпур. Курс робототехники. М.: Мир, 1990. 3. Справочник по промышленной робототехнике: Кн. 1. М.: Ма- шиностроение, 1990 4.А.К.Лапковский. Алгоритмы изображения движущихся тел при параллельном и центральном проецировании. Аксонометрия и компьютеризация изображения. Минск. Наука и техника.,1993. 5.Л.Аммерал. Принципы программирования в машинной графи- ке. Москва. Сол Систем.,1992. 51 ЛАБОРАТОРНАЯ РАБОТА № 6 Моделирование движения робота по дуге окружности в про- странстве декартовых координат Цель работы: изучить моделирование движения робота по дуге окружности в пространстве декартовых координат. 1. Основные положения В тех случаях, когда необходимо, чтобы рабочая точка инстру- мента (РТИ) манипулятора двигалась с сохранением ориентации по дуге окружности, используется круговая интерполяция в декарто- вых координатах. Рассмотрим алгоритм круговой интерполяции: Пусть в декартовых координатах ( 000 ZYX ), связанных с основа- нием манипулятора, заданы три точки. Положение и ориентация систем координат первой, второй и третей точки в базовой системе координат описываются преобразованиями 1T , 2T , 3T соответ- ственно (рисунок 6.1). Необходимо, чтобы РТИ манипулятора двигался с сохранением ориентации по дуге окружности, проходящей через три заданные точки. Ориентация РТИ манипулятора относительно дуги задается в исходной точке траектории. 52 Рисунок 6.1 - Положение и ориентация систем координат первой, второй и тре- тей точки в базовой системе координат Для выполнения круговой интерполяции на этапе подготовки необходимо определить: - определение P , описывающее положение с ориентацией системы координат, начало которой совпадает с центром окружно- сти, лежащей в плоскости xOy указанной системы и проходящей через три заданные точки в пространстве, а ось X проходит через первую заданную точку; - преобразование I , описывающее положение и ориентацию системы координат первой точки в системе координат P ; - угол , на который необходимо повернуть систему коор- динат первой точки до совмещения с началом системы координат третьей точки. Тогда положение и ориентацию РТИ манипулятора в любой точ- ке дуги можно описать выражением: IJPT ii , (6.1) 53 где преобразование iJ есть функция . Преобразование будем искать в виде (рисунок 6.2): 21 PPP . (6.2) Преобразование описывает положение и ориентацию системы координат 111 ppp ZYX в системе координат 000 ZYX . Рисунок 5.2 - Положение и ориентацию системы координат 111 ppp ZYX в си- стеме координат 000 ZYX Начало этой системы 1O находится в точке пересечения нор- мального вектора плоскости окружности, выходящего из начала O системы 000 ZYX с этой плоскостью. Плоскость 111 pp YOX лежит в плоскости окружности, ось 11 pXO проходит через начальную точку дуги 1T , а направление оси совпадает с направлением нормального вектора плоскости окружности. Преобразование 2P переводит си- стему 111 ppp ZYX в систему ppp ZYX . 54 Уравнение, описывающее плоскость в декартовых координатах, имеет вид: 0DzCyBxA . (6.3) Для определения коэффициентов DCBA ,,, воспользуемся следующим математическим заданием уравнения плоскости, про- ходящей через три заданные точки 1111 ,, zyxT , 2222 ,, zyxT и 3333 ,, zyxT , не лежащие одной прямой: 0 1 1 1 1 333 222 111 zyx zyx zyx zyx . (6.4) Тогда коэффициенты DCBA ,,, можно определить следую- щим образом: 333 222 111 33 22 11 33 22 11 33 22 11 1 1 1 1 1 1 1 1 1 zyx zyx zyx D xz xz xz B yx yx yx C zy zy zy A . (6.5) Координаты начала системы координат 111 ppp ZYX в системе 000 ZYX определим по формулам: 55 . , , 2221 2221 2221 CBA DC Z CBA DB Y CBA DA X p p p (6.6) Направляющие косинусы единичного вектора оси 11 pZO , лежа- щего на нормали к плоскости окружности, определим как: . , , 222 222 222 1 1 1 CBA C a CBA B a CBA A a p p p Z Y X (6.7) Где знак у корня выбирается противоположным знаку D . Направляющие косинусы единичного вектора оси 11 pXO найдем по формулам: 56 , , , 11 11 11 1 1 1 m ZZ n m YY n m XX n p Z p Y p X p p p (6.8) где 2 11 2 11 2 11 ppp ZZYYXXm . Направляющие косинусы единичного вектора оси 11 pYO опреде- лим как результат векторного произведения единичных векторов двух других осей: . , , 11111 11111 11111 ppppp ppppp ppppp XYYXZ ZXXZY YZZYX nanao nanao nanao (6.9) Таким образом получаем: 1000 1 1 1 1 111 111 111 pZZZ pYYY pXXX Zaon Yaon Xaon P ppp ppp ppp . 57 Система координат ppp ZYX может быть получена путем парал- лельного переноса системы 111 ppp ZYX в точку, являющуюся цен- тром окружности и имеющую координаты 22 pp YX в системе 111 ppp ZYX , и поворота этой системы на угол вокруг оси 1pZ . По- этому преобразование 2P будем искать в виде: 1000 0100 0 0 1 2 2 22 22 pYY pXX Yon Xon P pp pp . (6.10) Найдем координаты трех точек, заданных преобразованиями 321 ,, TTT в системе координат основания манипулятора, в системе координат 111 ppp ZYX . Для этого определим преобразования, опи- сывающие системы координат этих точек в системе координат 111 ppp ZYX . . , , 3 1 11 2 1 11 1 1 11 TPL TPK TPI (6.11) Из последней системы уравнений получим 58 , , , , ,0 , 333 333 222 222 111 1 1 1 1 1 1 yzayoxny xzayoxnx yzayoxny xzayoxnx y xzayoxnx yyyL xxxL yyyK xxxK I xxxI (6.12) где элементы xaon xxx ,,, и т.д. являются элементами матрицы 1 1P : 1000 10 1111 1 zaon yaon xaon pRR P zzz yyy xxx TT . (6.13) Координаты центра окружности ( ba, ) и ее радиус (R ) найдем, решив систему уравнений: 59 . , , 222 222 222 11 11 11 Rbyax Rbyax Rbyax LL KK II (6.14) Получим: 11 1111 2 2222 II LILL xx ybxyx a , (6.15) 111111 1111 111111 1111111 2 2 222 KILILK LLKI KILILK LLKKILK xxyxxy yxxx xxyxxy xxyxxxx b , (6.16) 22 1 baxR I . (6.17) Преобразование I , описывающие положение и ориентацию си- стемы координат первой точки окружности в системе координат ppp ZYX , определим по формуле 1 1 TPI . (6.18) Как уже отмечалось, любую систему координат с началом на по- лученной дуге окружности и ориентацией, равной ориентации в начальной точке дуги, можно описать уравнением (1). Преобразо- 60 вание iJ описывает поворот системы координат ppp ZYX на угол i вокруг оси ppZO и имеет вид: 1000 0100 00cossin 00sincos ii ii iJ . (6.19) Определим величину углового перемещения (угол ). Найдем координаты LLKK yxyx ,,, промежуточной точки и конечной точки в системе координат ppp ZYX . Для этого определим соответ- ствующие преобразования. 3 1 2 1 TPL TPK , (6.20) Искомые величины найдем по формулам, аналогичным (6.12). Тогда L L x y arctg , (6.21) где . Преобразования IP, и угол определяют, соответственно, центр окружности, исходную точку сегмента траектории и угловые перемещения (путь) на сегменте. 2. Содержание работы Работа предполагает выполнение следующего задания: - Построить траекторию движения робота с круговой интер- поляцией траектории в декартовых координатах X и Y . 61 3. Задание В декартовой системе координат заданы три точки. Положение и ориентация систем координат первой, второй и третей точки в ба- зовой системе координат 000 ZYX описываются преобразованиями 321 ,, TTT . Необходимо спланировать траекторию в базовой си- стеме координат при движении РТИ манипулятора по дуге окруж- ности из начальной точки в конечную с заданной скоростью и ускорением A . 4. Методика выполнения работы 1. По известным матрицам преобразования 321 ,, TTT опреде- лить (вручную): - коэффициенты DCBA ,,, (6.5); - координаты 111 ppp zyx начала системы координат 111 ppp ZYX в системе 000 ZYX (6.6); - направляющие косинусы 111 ,, ppp ZYX aaa (6.7), 111 ,, ppp ZYX nnn (6.8), 111 ,, ppp ZYX ooo (6.9); - элементы обратной матрицы 1 1P (6.13); - координаты трех точек в системе координат 111 ppp ZYX (6.12); - координаты центра окружности ( ba, ) и ее радиус R (6.15- 6.17); - преобразование P (6.2); - преобразование I (6.18); - преобразование L (6.20); - угол (6.21). 62 2. Для выполнения движения необходимо рассчитать (вручную) параметры участков разгона ( ppt , ), торможения ( mmt , ), рав- номерного движения ( pвpвt , ) по методике, приведенной в лабо- раторной работе №2 (2.6-2.9). Вместо пути q необходимо исполь- зовать значение угла , вместо и A необходимо использовать R и R A . 3. Составить программу расчета текущих значений на участках разгона, равномерного движения и торможения: - угла i (6.23-6.25); - элементов преобразования iJ (6.19); - элементов преобразования iT (6.1). 4. Программа должна обеспечивать построение траектории дви- жения РТИ манипулятора в декартовой системе координат YX , по текущим значениям столбца положения преобразования iT . 2 2 1 t pi , (6.23) 2tpрвi , (6.24) 2 2 1 ttmmi , (6.25) 63 5. Исходные данные № вар . 1T 2 T 3 T A 1 1000 0100 5.005.086.0 4086.05.0 1000 0 5.1 5 1000 0 1 8.5 70 650 0 2 1000 0100 5.0098.021.0 3021.098.0 1000 0 5.2 5 1000 0 2 3.6 75 700 0 3 1000 0100 5.0086.05.0 205.086.0 1000 0 5.3 5 1000 0 3 7.6 85 950 0 4 1000 0100 5.0021.098.0 1098.021.0 1000 0 5.4 5 1000 0 9.3 7 70 700 0 5 1000 0100 5.0072.07.0 007.072.0 1000 0 5.5 5 1000 0 3 4.9 75 800 0 6 1000 0100 5.007.072.0 1072.07.0 1000 0 5.6 5 1000 0 5 9 65 700 0 64 № вар . 1T 2 T 3 T A 7 1000 0100 8093.037.0 6037.093.0 1000 0 7 7 1000 0 1.7 5.7 60 500 0 8 1000 0100 804.097.0 5097.024.0 1000 0 6 7 1000 0 7 8.8 70 650 0 9 1000 0100 8078.061.0 4061.078.0 1000 0 5 7 1000 0 7.5 9 80 800 0 10 1000 0100 809.045.0 3045.09.0 1000 0 4 7 1000 0 6 5.10 65 600 0 65 6. Содержание отчета Отчет должен содержать следующие материалы: 1. Титульный лист. 2. Задание. 3. Расчеты и результаты вычислений, сделанных вручную на этапе подготовки. 4. Листинг программы. 5. Распечатку исходных данных и результатов вычисления в форме траектории движения робота в декартовой системе коорди- нат YX , 10. Пример реализации Аналогично примеру, рассмотренному в лабораторной работе №5 - Планирование прямолинейных траекторий в пространстве де- картовых координат. 11. Литература 1. К. Фу, Р.Гонсалес, К. Ли. Робототехника. М.: Мир, 1989. 2. М. Шахинпур. Курс робототехники. М.: Мир, 1990. 3. Справочник по промышленной робототехнике: Кн. 1. М.: Ма- шиностроение, 1990 4.А.К.Лапковский. Алгоритмы изображения движущихся тел при параллельном и центральном проецировании. Аксонометрия и компьютеризация изображения. Минск. Наука и техника.,1993. 5.Л.Аммерал. Принципы программирования в машинной графи- ке. Москва. Сол Систем.,1992. 66 ЛАБОРАТОРНАЯ РАБОТА № 7 Планирование сглаженных траекторий Цель работы: изучить планирование сглаженных траекторий. 1. Основные положения В предыдущих лабораторных работах, посвященных планирова- нию траекторий, рассматривался только один прямолинейный сег- мент в декартовых или обобщенных координатах (задавались толь- ко начальная и конечная точки траектории). Однако во многих слу- чаях технологическая задача требует указания дополнительных точек, через которые должен пройти манипулятор при следовании из начального в конечное положение и желаемая траектория зада- ется последовательностью дискретных точек. Эти точки называют- ся промежуточными или узловыми. Соединяя между собой каждые две соседние точки, получаем много сегментную (многоугольную) кривую линию, которая может использоваться для описания всей желаемой траектории. Типичная задача перемещения объекта манипулирования пока- зана на рисунке 7.1. 67 Рисунок 7.1 - Типичная задача перемещения Манипулятор движется к объекту, захватывает его, поднимает и перемещает его на новое заданное место. Желаемая траектория движения охвата описывается при этом последовательностью дис- кретных точек 543210 PPPPPP в виде многоугольной линии. Кроме того, желаемая траектория может быть описана и в пространстве обобщенных координат в виде многосегментной кри- вой. Для того, чтобы манипулятор двигался по траектории плавно во время перехода от одного сегмента к другому, концы соседних сег- ментов должны изгибаться в близи узловых точек. Функции, спи- сывающие участки перехода от одного прямолинейного сегмента к другому, должны состыковываться друг с другом таким образом, чтобы выполнялись определенные граничные условия непрерывно- сти, гарантирующие плавное движение манипулятора. Полная желаемая траектория, таким образом, может быть описа- на последовательностью состыкованных (сопряженных) функций и она должна планироваться специальным образом в окрестностях узловых точек с целью плавного сопряжения сегментов. Существуют различные методы определения траектории на участке сопряжения сегментов. В данной лабораторной работе мы рассмотрим три из них. 68 2. Линейная интерполяция со сглаживанием отрезков В настоящее время наиболее широко распространена линейная интерполяция со сглаживанием отрезков. В этом случае движение между узловыми точками jP на участках ],[ 13)1( jj tt происходит по прямой линии (см. рисунок 7.2). Для обеспечения непрерывности скорости ускорения переход с одной прямой на другую в течение интервалов сопряжения ],[ 31 jj tt происходит по кривой, описывае- мой степенной функцией (полиномом). Полином может быть вто- рой, третьей ли четвертой степени в зависимости от требований условия непрерывности скорости и ускорения. Рисунок 7.2 - Линейная интерполяция со сглаживанием отрезков Таким образом, график положения будет описываться последо- вательностью полиномов первой и второй степени, которые стыку- ются между собой с соблюдением соответствующих условий не- прерывности. Достоинством этого метода является точное следование вдоль прямых линий при планировании в декартовой системе координат, недостатком является то, что желаемая траектория (с учетом сопря- жении) не проходит точно через промежуточные точки (см. рисунок 7.2). 3. Метод интерполяции по формуле Лагранжа 69 Теоретически можно провести непрерывную и гладкую кривую точно через последовательность узловых точек, используя формулу , интерполяции Лагранжа: , ))...()(( ))...()(( ))...()(( ))...()(( ))...()(( ))...()(( )( 110 110 1 12101 20 0 02010 21 n nnnn n n n n n y xxxxxx xxxxxx y xxxxxx xxxxxx y xxxxxx xxxxxx xy   (6.1) где nx - значения аргумента в опорных точках траекто- рии. ny - значения функции в этих точках. В этом случае мы получаем высокую плавность траектории, без каких либо скачков скорости и ускорения (см. рисунок 7.3), однако при этом методе степень полинома растет пропорционально числу точек, что увеличивает время расчетов и колебательность. Рисунок 7.3 - Метод интерполяции по формуле Лагранжа 70 Кроме того, желаемую траекторию между узловыми точками очень трудно предугадать. По вышеперечисленным причинам ме- тод интерполяции по формуле Лагранжа не нашел практического применения. 4. Сплайн-интерполяция Осуществить интерполяцию траектории можно с помощью ку- сочно-степенных полиномов - сплайнов. Сплайн-интерполяция поз- воляет достаточно точно описать произвольную пространственную кривую, без значительного увеличения количества запоминаемых точек. Сущность сплайн-интерполяции заключается в представлении траектории между узловыми точками с помощью кривой, описыва- емой степенными полиномами третьей, четвертой или пятой степе- ни вида. .)( 0 k n n ntqtq При этом коэффициенты полиномов выбираются так, чтобы обеспечить непрерывность функций положения, скорости и ускоре- ния при, переходе с одного сегмента на другой. Последователь- ность степенных функций, сцепленных друг с другом, представля- ют желаемую траекторию, которая обеспечивает плавное движение манипулятора (см. рисунок 7.4). 71 Рисунок 7.4 - Сплайн-интерполяция Порядок полинома, выбранного для представления желаемой траектории, зависит от спецификации манипуляционной задачи. Когда заданы положения и скорость в начальные и конечные мо- менты времени, то этим четырем условиям удовлетворяет полином третьей степени с четырьмя неизвестными параметрами. Если в эти моменты времени также задано ускорение, тогда необходимо удо- влетворение 6 условий и соответственно необходим полином 5 сте- пени с шестью неизвестными параметрами. 5. Содержание работы Работа предполагает выполнение следующего задания: Построить графики положения, скорости и ускорения в зависи- мости от времени при планировании сглаженной траектории. 6. Методика выполнения работы 1. При планировании траектории методом линейной интерпо- ляции с сопряжением отрезков рассмотрим рисунке 7.5: Рисунок 7.5 - Планирование траектории методом линейной интерполяции с со- пряжением отрезков 72 Для интервала ассасс ttt положение, скорость и ускорение вычисляются по следующим формулам: , 3 .1 , 1 225.1 ,22 2 1 2 1 2 1 acc acc acc acc acc t hB T t Cq t BhhB T t Cq BBhBhhB T t Cq   где ,BCC . 2 , acc acc t tt h BAB При accacc tttT1 траектория формируется по следующим формулам: . ,0 , , 1 1 T t h q T C q BChq   . 73 Таким образом, описанный метод предполагает циклическое ис- пользование указанных выше математических выражений для 2-х интервалов. При acctTt 1 необходимо выполнить следующее переопределение параметров в рассмотренных формулах: время. начальное , , ,: ,: положение, текущее: ,: 21 acctt BAB BCC DC CB xA TT 2. Для интерполяции траектории полиномами можно воспользо- ваться интерполяционной формулой Лагранжа, имеющей следую- щий вид: . ))...()(( ))...()(( ... ))...()(( ))...()(( ))...()(( ))...()(( )( 110 110 1 12101 20 0 02010 21 n nnnn n n n n n y xxxxxx xxxxxx y xxxxxx xxxxxx y xxxxxx xxxxxx xy где nx - значение аргумента в опорных точках траектории ny - значение функции в этих точках. 74 3. При выполнении сплайн-интерполяции траектории рассмот- рим i -ый участок траектории (см. рисунок 6.6), который описыва- ется следующим полиномом: 01 2 2 3 3 KKKKS , где . , 22 , 233 , , , , 1 3 11 3 2 11 2 11 10 1 1 iim m iimimi m mimiii i i i ii ttt t SStt K t ttSS K K SK tt ttt Рисунок 7.6 - i -ый участок траектории 75 7. Варианты заданий к лабораторной работе Вариант 1 Осуществить планирование траектории методом линейной ин- терполяции с сопряжением отрезков. Таблица 7.1 - Исходные данные № вар. Время acct 0 t 1 t 2 t 3 t 4 t 1 0,1 0 1,0 1,4 2,0 5,1 2 0,15 0 1,0 2,0 3,0 3,7 3 0,2 0 1,0 2,0 4,0 11,0 4 0,25 0 1,0 4,0 12,0 18,0 5 0,3 0 1,0 7,5 8,4 12,0 6 0,35 0 8,0 10,0 12,0 13,0 № вар. Положение 0q 1 q 2 q 3 q 4 q - 1 100 200 400 600 700 2 800 400 250 200 100 3 100 200 400 100 400 4 100 400 200 800 200 5 100 800 400 200 400 6 800 400 200 100 800 Вариант 2 Осуществить планирование траектории с помощью интерполя- ционной формулы Лагранжа. Таблица 7.2 - Исходные данные № вар. Время 0t 1 t 2 t 3 t 4 t 1 0 1,0 1,4 2,0 5,1 2 0 1,0 2,0 3,0 3,7 3 0 1,0 2,0 4,0 11,0 4 0 1,0 4,0 12,0 18,0 5 0 1,0 7,5 8,4 12,0 6 0 8,0 10,0 12,0 13,0 76 № вар. Положение 0q 1 q 2 q 3 q 4 q 1 100 200 400 600 700 2 800 400 250 200 100 3 100 200 400 100 400 4 100 400 200 800 200 5 100 800 400 200 400 6 800 400 200 100 800 Вариант 3 Выполнить сплайн-интерполяцию траектории. Таблица 7.3 - Исходные данные № вар. Время Положение 0t 1 t 2 t 3 t 4 t 0 q 1 q 2 q 3 q 4 q 1 0 1,0 1,4 2,0 5,1 100 200 400 600 700 2 0 1,0 2,0 3,0 3,7 800 400 250 200 100 3 0 1,0 2,0 4,0 11,0 100 200 400 100 400 4 0 1,0 4,0 12,0 18,0 100 400 200 800 200 5 0 1,0 7,5 8,4 12,0 100 800 400 200 400 6 0 8,0 10,0 12,0 13,0 800 400 200 100 800 № вар. Скорость - 0 1 2 3 4 - 1 0 300 500 200 0 2 0 -50 -25 -27 0 3 0 50 -50 4 0 4 0 20 28 2 0 5 0 33 -50 2 0 6 0 -12 -22 150 0 8. Содержание отчета Отчет должен содержать следующие материалы: 1. Титульный лист 2. Листинг программы, содержащий фамилию, номер группы и вариант задания. 77 3. Распечатку исходных данных и результатов вычислений в форме графиков положений, скорости и ускорений. 9. Примеры реализации программы 1 Метод интерполяции по формуле Лагранжа( язык программи- рования С#): double[,] lagQR = new double[3, p1 + p2 + p3]; time[] - массив времени с определенным временем кванто- вания //Присвоить координаты Y for (int j = 0; j < 3; j++) { for (int i = 0; i < p1 + p2 + p3; i++) { lagQR[j, i] = ((time[i] - t[1]) * (time[i] - t[2]) * (time[i] - t[3])) / ((t[0] - t[1]) * (t[0] - t[2]) * (t[0] - t[3])) * arr[j, 0] + ((time[i] - t[0]) * (time[i] - t[2]) * (time[i] - t[3])) / ((t[1] - t[0]) * (t[1] - t[2]) * (t[1] - t[3])) * arr[j, 1] + ((time[i] - t[0]) * (time[i] - t[1]) * (time[i] - t[3])) / ((t[2] - t[0]) * (t[2] - t[1]) * (t[2] - t[3])) * arr[j, 2] + ((time[i] - t[0]) * (time[i] - t[1]) * (time[i] - t[2])) / ((t[3] - t[0]) * (t[3] - t[1]) * (t[3] - t[2])) * arr[j, 3]; } } return lagQR; 2 Сплайн-интерполяция (языки программирования С#): public double[,] Spline(double[,] arr) { TimeIntervals(); double[,] splineQR = new double[3, p1 + p2 + p3]; double tau, tm, k0, k1, k2, k3; 78 double[,] mass = OZK(); int min = 0; int max = 80; for (int j = 1; j < 4; j++) { if (j == 1) { min = 0; max = p1; } if (j == 2) { min = p1-1; max = p1+p2; } if (j == 3) { min = p1+p2-1; max = p1+p2+p3; } for (int i = min; i < max; i++) { for (int k = 0; k < 3; k++) { tau = time[i] - t[j - 1]; tm = t[j] - t[j - 1]; k0 = mass[k, j - 1]; k1 = speed[j - 1]; k2 = (3 * mass[k, j] - 3 * mass[k, j - 1] - 2 * speed[j - 1] * tm - speed[j] * tm) / (tm * tm); k3 = (2 * mass[k, j - 1] - 2 * mass[k, j] + speed[j] * tm + speed[j - 1] * tm) / (tm * tm * tm); splineQR[k, i] = k3 * tau * tau * tau + k2 * tau * tau + k1 * tau + k0; } } } return splineQR; } 10. Литература 1. К. Фу., Р. Гонсалес, К. Ли. Робототехника. M. Мир.,1989 2. М. Шахинпур. Курс робототехники. M. Мир., 1990 3. Справочник по промышленной робототехнике:Кн.1–М.: Машиностроение,1990. 79 Содержание Лабораторная работа № 1 Матрица однородного преобразования………………………………3 Лабораторная работа № 2 Методы описания ориентации объекта в пространстве……………15 Лабораторная работа № 3 Виды изображения объёмных тел в координатах экрана монитора, алгоритмы изображения движущихся тел………………………………………………………...……………21 Лабораторная работа № 4 Моделирование движение робота в пространстве обобщенных координат………………………………………………30 Лабораторная работа № 5 Планирование прямолинейных траекторий в пространстве декартовых координат………………….…………..39 Лабораторная работа № 6 Моделирование движения робота по дуге окружности в пространстве декартовых координат…………………………….51 Лабораторная работа № 7 Планирование сглаженных траекторий……………………………..66 80 Учебное издание ОКОЛОВ Андрей Ромуальдович НОВИЧИХИНА Елена Романовна СВИДЕРСКИЙ Геннадий Сигизмундович МАТЕМАТИЧЕСКОЕ ОБЕСПЕЧЕНИЕ ПРОМЫШЛЕННЫХ РОБОТОВ Учебно-методический комплекс для студентов специальности 1-53 01 06 «Промышленные роботы и робототехнические комплексы» В 2 частях Часть 1 ЛАБОРАТОРНЫЕ РАБОТЫ Подписано в печать 16.10.2012. Формат 60×84 1/16. Бумага офсетная. Ризография. Усл. печ. л. 4,65. Уч.-изд. л. 3,64. Тираж 50. Заказ 1308. Издатель и полиграфическое исполнение: Белорусский национальный технический университет. ЛИ № 02330/0494349 от 16.03.2009. Пр. Независимости, 65. 220013, г. Минск.