Робототехника.Фу, Ли, Гонсалес (962794), страница 30
Текст из файла (страница 30)
В следуюшем разделе мы рассмотрим схемы планирования 4-3-4- траекторий и траекторий, задаваемых кубическими сплайнами. 4.3.1. Расчет 4-3-4-траектории В связи с тем, что для каждого участка траектории требуется определить М траекторий присоединенных переменных, удобно воспользоваться нормированным временем ( е= [О, 1), 177 Это позволяет достичь единообразия уравнений, описывающих изменение каждой из присоединенных переменных на каждом участке траектории, При этом нормированное время будет изменяться от !=О (начальный момент для каждого из участков траектории) до Е = 1 (конечный момент для каждого из участков траектории).
Введем следующие обозначения: Š— нормированное время, Е~ [О, 1[; т — реальное время, измеряемое в секундах; т,— момент (в реальном времени) окончания лчго участка траектории; Е; =т,— т; 1 — интервал реального времени, затрачиваемый на прохождение Е-го участка траектории; т — т, т ~ [т,, т,[; Е ен [О, 1). 2 4 — ! Траектория движения Е-й присоединенной переменной задается в виде последовательности полиномов Ьт(Е).
На каждом участке траектории для каждой присоединенной переменной используемые полиномы, выраженные в нормированном времени, имеют вид Ь, (Е) = амЕ'+ а„г'+ а!2Е" + аиЕ+ а!о (1-й участок), (4.3-2) Ь,(Е) = а.гг~+ а~гЕ'+ ам!+ ог~ (2-й участок), (4.3-3) Ь„(Е) = а„4!~+ а„ага+ авггг+ ав!Е+ аво (последний Участок). (4.3-4) Индекс у переменной, стоящей в левой части каждого из равенств, обозначает номер участка траектории; и-й участок— последний. Индексы в обозначении неизвестных коэффициентов ар имеют следующий смысл: Е-й коэффициент для Ечго участка траектории рассматриваемой присоединенной переменной.
Граничные условия, которым должна удовлетворять выбранная система нолиномов, следующие; 1, Начальное положение = Оо = 0(ЕВ). 2. Значение начальной скорости = и, (обычно нулевое). 3. Значение начального ускорения = ао (обычно нулевое). 4. Положение в точке ухода = О> — — 0(Е,).
5. Непрерывность по положению в момент Е! (т. е. 0 [Е, ) = = 0 [Е, )). 6. Непрерывность по скорости в момент Е, (т. е. и [!~ ) = и[Ел )). 7, Непрерывность по ускорению в момент Ел (т. е. а[!~ )=а[!~ )). 8. Положение в точке = Ог = 8 (Ег). 9. Непрерывность по положению в момент Ег (т, е. 8[Е,)= = 0 [Е,+)). 1О. Непрерывность по скорости в момент Ег (т.
е. и(гг ) = и(Ег )). !73 11. Непрерывность по ускорению в момент Ег (т. е. а [Ег ) = ч= о [Ег )). 12. Конечное положение = Ое — — 0(Ее). 13. Значение конечной скорости = и! (обычно нулевое). 14. Значение конечного ускорения =а! (обычно нулевое). Граничные условия для 4-3-4-траекторий показаны на рис. 4.3. Первую и вторую производные рассматриваемых поли- В(тг) ы В(тгл) В(.-) — В(т+) 42 В(т,) ф В(тг) ъ $.
ф 44 В В В(тл! в(.„) = Вг 4(тл) = тг В(ъ„) = тл тг Релт,галиа Влелттт Рис. 43, Граничные условия для 4-3-4-траеятоияя в пространстве првсоедв- НЕИШах ПЕВЕллЕНЯЫХ. помов относительно реального времени можно представить в следующем виде: 4Елл (Е) Вег (Е) тЕЕ 1 тЕЕлг(Е) и (Е) 4Ет 4ЕЕ 41т т, — т,, оŠ— — = — Ь,(Е), 1=1, 2, тн 1 леал 00 (4.3-5) 4ЕЕ 4Егл; (Е) 1 тггьл (Е] 1 442724 (Е) 1 4Ет (т,. — т,) 4ЕЕ Ет 4ЕЕ е'=1, 2, и. (4.3-6) ЬЕ(Е) =а,414+ а!272+ и, Ег+ он!+ ам, Е ен [О, 1[. (4 3 7) 179 Для описания первого участка траектории используется полипом четвертой степени й, (1) 4а4414 + За,зр+ 2а421 + а44 (4.3-3) (4.3-9) Отсюда следует в;, = п412, (4.3-19) Отсюда имеем аи — — по)4 н Ь,(о) й,(В й,(о) й,(1) и 2 2 (4.3-20) илп (4.3-22) (4.3-23) или (4.3-24) йз (1) 12244+ база+ ао14 а,(!) а, (4.3-15) 2 2 181 С учетом равенств (4.3-5) и (4.3-6) скорость и ускорение на этом участке имеют вид а, 11) 12ангг 4- банг+ 2ан а, 11) — —, 1.
Для 1 = 0 (начальная точка данного участка траектории). Иэ граничных условий в этой точке следует а,о — — Ь, (0) = Оо (Оо — задано), (4.3-10) 144 (О) Г 4ан1'+Заззг'+ 22 21+ азг 11 а 4 (4 3 !1) со — — — ] ]1-О Ь~ (о) Г 122441~+ ба,зг+ 2а~з 3 2азз (4 3 12) во — —— з г 2 1, 1, ~,о 1! аог 4 что позволяет получить а„ = — . 2 Подставляя найденные значения коэффициентов в равенство (4.3-7), получим Ьз (1) = и441~+ и431'+ ~ — ~ 1'+ (оА)1+ Оо, ген (О, !]. (4.3-13) 2.
Для 1=! (конечная точка данного участка траектории). В этой точке мы ослабим накладываемые граничные условия, сняв требование точного прохождения траектории через заданное положение, но оставив условия непрерывности по скорости и ускорению. Эти условия означают, что скорость и ускорение в конце первого участка траектории должны совпадать со скоросгью и ускорением в начале второго участка. В конце первого участка скорость и ускорение соответственно равны о, (1) — аз = йг (1) 4а44+ Зазз+ ао14+ оо14 (4.3-14) Для описания второго участка траектории используется полипом третьей степени Ьг(1) =агзгз+ а221~+ ам1+ иго 1ен!04 !] (4 3-16) 1. Для 1= 0 (точка ухода). Пользуясь равенствами (4.3-5) и (4.3-6) в этой точке, имеем 122(0) =а„=02(0), (4.3-17) йг(О) Г Заззгз+ 2амг+ ам 4 ам (4 3 !6) Оз— 12 12 ]г-о 1з а 412 и, следовательно, аг,= —.
2 Поскольку скорость и ускорение в этой точке должны совпадать соответственно со скоростью и ускорением в конечной точке предыдущего участка траектории, то должны выполняться ра- венства которые соответственно приводят к следующим условиям: Зазз1 + 2ам1+ ам] ! 4ан1'+ За,з1 + 2а,з1+ а ] (4 3-2!) 12 11-2 1 Л-1' г — аг, 4ан За42 аог! 221, + — '+ — + — + — =0 1, 1, багз1+2азз'] ] 12ан!2+базз1+2а,г ~ 1 =~ 2 Ф 2 г о 1 1-1 г — 2агг 12а44 ба~о ао14 — + — '+ — + — =О. 12 2. Для 1=1 (точка подхода). В этой точке скорость и ускорение должны совпадать со скоростью и ускорением в начальной точке следуюпгего участка траектории.
Для рассматриваемой точки имеем 112 (!) = ~2+ иго+ в 4+ и о, (4.3-25) (!) йз(1) ! Заз,12+ 2азгг+ аз' 1 Зазз + 2ам+ ам (4 Ч 26) 12 й-з йз ГВ Г баЫ + 2ам ') бам + 2ам . (4.3-27) 422 (4) 2 12 12 ~, 4 12 хода получаем аггп Ьп( — 1)=а 4 а з+ — — ц11,+О1 — — О,(1), (4.3-35) — 4ап, + Запз аг'2« + о11« (4.3-36) Ьп(-1) [ 12а«41 + вапзг + 2а«2 1 12««4 — еа«о+а!1„ г 1« и и (4.3-37) Условия непрерывности скорости и ускорения в точке подхода записываются следующим образом: 62 (1) 6 ( — 1) и 1и 2 662 (1) 6« (-1) (4.3-38) или «4 пз+ 1 "1«+ зз+ 22+ 21 О (4339) гп 12 22 «4+ «З 1« 23 22 0 (4340) гп гй 2 Приращение присоединенной переменной на каждом из участ- ков траектории можно найти по следующим формулам: 6« 01) ап, о! =— (4.3-33) Отсюда следует ап, = О11«.
а11„ 6« = 01 — О, = lг„(0) — 6« ( — 1) = — а«4 + апз — + огг«, (4.3-43) Далее, (4.3-34) 6« (0) 2апз а! =-7 — = —, Все неизвестные коэффициенты в полиномах, описывающих изменение присоединенной переменной, могут быть определены путем совместного решения уравнений (4.3-4! ), (4.3-22), (4.3-24), (4.3-42), (4.3-39) и (4.3-43), Представляя эту систему уравнений в матричной форме, получим и, следовательно, г агг„ а 2= —, « (4.
3-4 1) у=Сх, 182 16З Для описания последнего участка траектории используется полипом четвертой степени Ьп(1) =а„41'+ апз(о+ а«212+ а„,! + а о 1~ [О 1] (4 3 28) Если в этом равенстве заменить 1 на 1=1 — 1 и рассматривать зависимость от новой переменной 1, тем самым мы произведем сдвиг по нормированному времени: если переменная 1 изменяется на интервале [О, 1], то переменная 1 изменяется на интервале [ — 1, 0]. Равенство (4.3-28) при этом примет вид Ьп(1) =а„,,(4+ апз1з+ а„212+ а«,1+ а о 1 е— = [ 1 0]. (4 3 29) Пользуясь равенствами (4.3-5) и (4.3-6), найдем скорость и ускорение на последнем участке [ 6 (7) 4а.
7 + з".7*+ 2а"г+ ап' (43-30) гп 2« (1) 6 (1) 12а '+аа '+2«" (4331) и 2 и 1. Для 1= 0 (конечная точка рассматриваемого участка траектории), В соответствии с граничными условиями в этой точке имеем 6«(О) =а«,=00 (4.3-32) 2. Для 1= — 1 (начальная точка последнего участка траектории).
В соответствии с граничными условиями в точке под- 6« ( 1) [ 4апг! + За«21 + 2а«21 + апг гп гп А--г аогг 2 бг =Ог — 0«=Ьг(1) — гг (О) =а~ +ам+ 2 + ооГп 62 = 02 — О, = 62(1) — 62 (0) = ам+ азз+ азь (4,3-4!) (4.3-421 где аог! У = [б — 2 — оА, — а,(, — оо, — а„бтн т а)!'„ — аг)л+ о), аб бл+ — — о)1„~ (4.3-46) 0 0 0 О (4.3-36) 4/(л — 12/1„ — 1 а21 а22 аМ~ апо, йл4) ° (4. 3-47) 'Таким образом, задача планирования траектории (для каждой присоединенной переменной) сводится к решению векторного уравнения (4,3-44); т у, = ~~'„с!!)х! ! 1 (4.3-46) из! и Х=С !у. (4.3-49) Структура матрицы С позволяет легко найти неизвестные коэффициенты.
Кроме того, матрица, обратная С, всегда существует, если только интервалы О (при !'=1, 2, и) положительны. Решая уравнение (4.3-49), получаем все неизвестные коэффициен. ты полиномов, описывающих траекторию /'-й присоединенной координаты. Поскольку для полинома, описывающего последний участок траектории, нами была произведена замена, сдвигающая интервал изменения нормированного времени, то после определения коэффициентов ата из уравнения (4.3-49) необходимо произвести обратную замену, состоящую в подстановке 1= !' — 1, в равенстве (4.3-29). В результате получим Ьл (1) = а„чТ + ( — 4ал, + апз)1~ + (ба 4 — Запз + ал) П + + ( — 4ап4+ 3апз — 2ал, + ап,) ! + (ал4 — ал, + й„з — ал, + а„,), 1~ (О, 1). (4 3.60) Окончательный вид полиномов для 4-3-4-траектории, получаемый из решения уравнения (4,3-49), представлен в табл. 4.3. г84 1 1 0 3/1, 4/1, — 1/1, 6/У! 12/12 0 0 0 1 О О 1/(, 0 О 0 0 0 0 Х = (йин й14, 0 0 0 0 — 2/52 0 1 1 2/1, 3/12 2/ге 6/Д 0 0 0 0 — 3/1„ 6/гпз 1 Таблица 4.8.
![](https://s.studizba.com/z.php?f=/templates/si/i/noavatar.png&w=100&h=100&t=1"){kind=avatar}
