Робототехника.Фу, Ли, Гонсалес (962794), страница 33
Текст из файла (страница 33)
Если требуется перевести схват манипулятора из положения 1 в положение 2, то, записывая уравнение для точки 1 относительно системы координат этой точки, имеем т "" ' т, = с, (г) Р„. Записывая уравнение для точки 1 в системе координат 2.й точки, получаем Тв 'РТ» = Св (Г) Ры. (4.4-8) Из этих уравнений находим матрицу Рп Ры С (г) С4 (г) Р1~ ( ~рт4) вв »Т», (4.4-7) С помощью выражения (4.4-7) можно найти Ры, зная Рц.
Таким образом, движение из ~'-го положения в (1+ 1)-е можно рассматривать как переход из состояния Та= С44,(г) Р; 4„(""х рт4+,) (4.4-8) в состояние Та= свр,(г) Р;„,„, (""" Тв+,) (4.4-9) Пол [228] предложил простой способ перевода схвата манипулятора из одного заданного положения в другое. Движение охвата осуществлялось как композиция поступательного перемещения, поворота вокруг заданной оси и еще одного поворота вокруг оси инструмента. Первым поворотом обеспечивалось заданное направление вектора подхода. Вторым поворотом достигалась требуемая ориентация схвата. Отсюда получаем (4.4-12) 0(1)=(Р;; 4) (Рсрь;+1) Представляя положение схвата в точках ! и 1+ 1 в виде соот- ветствующих матриц однородных преобразований, имеем Л Рх РА РА ВЛ а' к к ВА ал р р ВЛ ал х к 0 0 ВВ аВ х х Вв ав В В ВВ аВ 0 0 ал и„' и," (4.4-13) 1 в— Р» Рр в ав р ав зв аа Рв [ о о р„, „,АВ=[„' (44-14) 0 Польз ясь равенством (2.2-27) для обращения матрицы Р,,+, у н пРоизвеДЯ Умножение на Р4+ь»»ь полУчаем пл ' зв пл ' ав пл ' (РВ Рл) В.Л ВВ зл аВ ВА ' (РВ РЯ) ал ' зв ал ' аз ал ' (рв Рл) 0 0 1 ПЯ ПВ 0 (1) = 0 (4.4-15) где точка означает скалярное произведение векторов.
Если ведущая функция содержит одно поступательное и два вращательных движения, то и перемещение, и повороты будут прямо пропорциональны л. Если Х изменяется пропорционально времени, то результирующее движение, описываемое функцией 0(1,) представляет собой поступательное движение с постоян1З7 где я.= —, Х ~ [О 1], à — реальное время с момента начала где движения вдоль данного участка траектории и Т вЂ” полное в е движения вдоль данного участка траектории. В ~'-й точке 4 = О, Л = О, 0(0) — единичная матрица размерностью Х и р мя 4 4 и (4.4-1 1) ной линейной скоростью и два вращения с постоянными угловыми скоростями. Поступательное движение описывается матрицей однородного преобразования Е(Л), задающей движение т л вдоль прямой, соединяющей точки Р~ и Р,л.ь Г1ервое вра аельное движение задается матрицей Йл(Л), описывающей раз- 1 Щ ворот вектора подхода из положения Р; в положение Р;„ь Второе вращательное движение описывается матрицей Р(в(Л), задающей изменение ориентации схвата от положения Р; к положению Рны за счет врапьения вокруг оси инструмента.
В результате ведущая функция может быть представлена в виде (4.4-16) в (л) = !. (Л) Кд (л) Кв (л), где 1. (Л) = (4.4-17) л'ф (лв) + с (ле) — з рсфу (лв) — сфя(ле) 0 !1А (л) С (Лф) — 5 (Лф) 0 0 5 (Лф) С (Лф) 0 0 0 0 1 0 0 0 0 1 (4.4-19) где )л (Я.О) =! — соз (ЛО), С(ЛВ) =-сов(ЛВ), ь(ЛО) = !п (Ле), ': (~4 4.9еьл С (лф) = соз (лф), о (лф) = гп (лф) и Л~ [0, !). Матрица поворота йл(Л) описывает вращение на угол 0 вокруг вектора ориентации, соответствующего положению Рь который повернут иа угол ф вокруг вектора подхода. Матрица Кв(Л) описывает вращение на угол ф вокруг вектора подхода, соответствующего положению Р;~ь Перемножая матрицы (4.4-! 7) — (4.4-19), получаем дп до да г!р !я(л) = 0 0 0 1 (4.'4-21~ 198 1 О 0 Лх 0 1 0 Лу 0 0 1 Лг 0 0 0 1 — яфсф (лв) С2фр (ле) + с (я.е) — яфю(ле) 0 СфЯ (Я.О) 0 вчем(ле) о С (Я.О) 0 0 1 (4.4-18) где ч(л )[уф[ай(ле)+с(ле))+с(лф)[ — зфсф (лв)) ч(Лф)[ ЗфСфр(ЛЕ))+С(ЛО)[С'фГ(ЛЕ)+С(ЛВ)[ .с Р„,р) [ Сф8(ЛЕ)[+ С(ЛЕ) [ — Зф8 (Лв)) д!а = офо (ЛО), др = Я.У с (лв) ла и г!п = до Х ба.
Применяя метод обратных преобразований к равенству (4.4-16), можно определить х, у, х. Для этого нужно умножить обе ча- сти равенства (4,4-16) справа на Кв (Л) Кд'(Я) и приравнять соответствующие элементы векторов положения х=ил ' (Рв Рл). У=ад '(Рв РА) г=а, (рв-рд). Умножая обе части равенства (4.4-16) справа на Кв (Л) и слева на 1.— '(Л), найдем из условия равенства соответствующих эле- ментов третьего столбца значения 0 и ф; ф = агс1н ~ — и ( ф ~( и, ал'ав 0 « ' Е ~ ~и. (4.4-24) Чтобы найти ф, умножим обе части (4,4-16) слева последова- тельно на !. '(Л) и Кл (Л), и, приравняв соответствующие эле- менты, получим лф= — лфсф[л(ЛО) (пл пв)+ [С'фу(Я.В)+С(ЛВ)) (з, пв)— — офЯ (Я.О) (пд пв), (4.4-25) сф = — сфсфр (лв) (п„з,) + [с'фр (ле)+ с(ле)) (зл з,)— †.ьфо (Ле) (а зв), (4.4-26) ф = агс1д ~ — ], — и (ф ~ (и. (4 4-27) Движение между двумя участками траектории.
Часто необ- ходимо обеспечить движение манипулятора вдоль последова- тельности прямолинейных участков (это диктуется выполняе- 199 гдв у 0 у е ) 4~ †. (4.4-31) )(у) = %9)- (4.4-32) (4.4-33) (4,4-28) хвс ХВА у О Увс УВА ЬС= авс о авд Овл Фвл трвс (4.4-34 ) (4.4-35) (4.4-36) 20! (4.4-29) (4.4-30) мой манипулятором задачей или наличием препятствий на его пути). Для того чтобы избежать скачков по скорости при переходе с одного участка траектории на другой, между участками нужно обеспечить движение с ускорением или торможением. Такое движение можно реализовать, начиная изменение скорости за время т до точки перехода между участками и поддерживая ускорение постоянным, пока не пройдет т единиц времени после перехода границы участков (рис. 4.5), Если Рмс 4.5.
Движение между двумя прямолинейными участками траектории. ускорение поддерживается постоянным от момента — т до мо- мента т, то его величина, обеспечивающая заданное изменение скорости и положения, определяется формулой ц (г) = — „', ~ЛС вЂ”,' + ЛВ~, где — и<!<Г, и ЛС и Л — векторы, элементами которых являются прираше. ния декартовых координат и углов между точками В и С и между точками В и А соответственно. В соответствии с равенством (4.4-28) скорость и положение на интервале — т < Г < т определяются равенствами т) (Г) = —,' [Лсф+ ЛВ1 Д вЂ” ~,', е((() = ~~ЛС вЂ” + ЛВ~ У вЂ” 2ЛВ1 Л + ЛВ, При ч < ~ <Т движение задается равенствами п=ЛС)„д=О, где Т' Отметим, что, как и выше, Х представляет собой нормированное время, изменяющееся на интервале [О, 1].
Следует помнить, однако, что нормирующий множитель различен для разных участков траектории. Для движения от А к В и к С определим ф на интервале — т < г < т следующим образом: тр = (трвс — трав) у + фдв где флв и фнс определены для движения от А к В и от В к С формулой (4.4-23). Тем самым ф будет изменяться от трда к фвс. Итак, для описания движения из положения Р, в положение Рны надо в соответствии с равенствами (4.4-!6) — (4,4-27) вычислить ведущую функцию 0(Х); далее следует определить Те(Х) по формуле (4.4-!0) и с помощью программы решения обратной задачи кинематики вычислить соответствующие значения присоединенных координат. При необходимости применяется квадратическая интерполяция траектории между полученными точками в пространстве присоединенных координат.
Пример. Двигаясь по прямолинейной траектории в декартовом пространстве, манипулятор должен переместить болт из обоймы в одно из отверстий бруска, изображенного на рис, 46 Запишите все необходимые матричные уравнения, задающие движение манипулятора вдоль штриховой линии, обеспечивающее выполнение поставленной задачи. Для обозначения промежуточных точек траектории можно использовать буквы. Решение. Пусть Р,— узловые точки траектории, через которые должен пройти охват манипулятора (!=0, 1, 2, 3, 4, 5).
Матричные уравнения будут иметь следующий вид: в точке Ро [ВАЗЕ] [Тб] [Е) = [!й(1Т] [Р,), в точке Г,: [ВАВЕ) [Т6) [Е) = [ВО] [Р.,), в точке Р,: [ВАЬЕ] [Тб] [Е] = [ВО) [Р ), х Абсолюмная гиглгемк то вкьп) Рис. 4.6. [Тб]; [Ро] задана относительно системы [1Х1Т); [Р~), [Ро! н [Ро] заданы относительно [ВО]; [Ро] и [Ро] заданы относительно [ВК). Для описания движения нз Ро и Р~ воспользуемся двойной индексацией, чтобы записать равенство (4.4-34) в системе координат Рь В системе координат Ро имеем [Тб) = [ВАЗЕГ [1ЯТ) [Роо] [Е] ° ' (4.4-40) Записывая это равенство в системе Рь получаем [Т61 = [ВАКЕ) [ВО] [РоД [Е1 Из ранено~в (4.4-40) и (4.4-41) находим [Ро~) = [ВО] [1Ы!Т] [Роо] ' ' (4 4 42) (4.4-41) 202 в точке Р,: [ВАЗЕ] [Т6]1Е] = [ВО] [Р,], (4.4-37) в точке Р4.' [ВАЗЕ] )Т6) [Е) = [ВК] [Ро], (4.4-38) в точке Р,: [ВАЗЕ] [Тб] [Е]= [ВК] [Ро], (4.4-39) где [ВА$Е], [ПЧ1Т], [В01, [ВК1, !Тб), 1Е), !Ро1, [Р~), 1Ро), [Ро), [Ро], [Ро] — матрицы однородных преобразований, имеющие размерность 4Х 4.
Матрицы [ВАКЕ], [1Х1Т], [ВО] и [ВК! заданы относительно абсолютной системы координат; [Тб) задана относительно базовой системы; [Е] задана относительно Таким образом, переход из положения Ро в положение Р, вдоль прямолинейной траектории означает изменение конфигурации манипулятора с [Т6] = [ВАЗЕ] [В01 [Ро~] [Е) на [Тб] = [ВАЗЕ) ' [ВО] [Р,Д [Е] (4.4-43а) (4.4-436) Переход из точки Рг в Роы для 1= 1, 2, 3, 4 осуществляется аналогично.
4.4.2. Планирование прямолинейных траекторий с использованием кватернионов Предложенный Полом способ построения прямолинейных траекторий опирается на описание заданных положений схвата манипулятора матрицами однородных преобразований. Такое представление заданных положений удобно и наглядно. Однако оперирование с матрицами предъявляет относительно высокие требования к объему памяти, используемой для их хранения в вычислителе, и к количеству выполняемых арифметических операций.
Кроме того, матричное описание операций вращения является существенно избыточным, что может привести к вычислительной неустойчивости. Вместе с тем было показано, что представление вращений с помощью кватернионов позволяет задать более эффективное и более равномерное движение. Тэйлор [282] предложил два способа построения прямолинейных участков траектории между узловымн точками. Первый из пнх, называемый управлением в декартовых координатах, является усовершенствованием метода Пола и отличается от него тем, что для описания вращений используются кватернионы. Этот способ прост и обеспечивает более равномерное угловое движение.
![](https://s.studizba.com/z.php?f=/templates/si/i/noavatar.png&w=100&h=100&t=1"){kind=avatar}
