CC BY
117
Системный анализ и управление
УДК 621.865
Н.В. Ростов
анализ алгоритмов решения обратных задач кинематики в системах управления движением роботов
Рассмотрены формулировки обратных задач кинематики, решаемых в системах управления движением манипуляционных роботов. Обсуждены трудности решения таких задач аналитическими и численными методами. Проанализирована сходимость алгоритмов итерационных методов с регуляризацией на траекториях движения схвата с участками вне его зоны достижимости. Приведен пример итерационного расчета траекторий звеньев трехзвенного робота с вычислением матрицы Якоби по рекурсивному алгоритму.
СИСТЕМЫ УПРАВЛЕНИЯ ДВИЖЕНИЕМ РОБОТОВ; ОБРАТНЫЕ ЗАДАЧИ КИНЕМАТИКИ; МАТРИЦА ЯКОБИ; ИТЕРАЦИОННЫЕ МЕТОДЫ; СХОДИМОСТЬ АЛГОРИТМОВ; РЕГУЛЯРИЗАЦИЯ.
N.V. Rostov
algorithm analysis for solving the inverse kinematic problems in robot motion control systems
The given article covers the general formulations of inverse kinematic problems for robot motion control systems. We have discussed the difficulties how to solve such problems using analytical and numerical methods. We have also analyzed the convergence of iterative algorithms with the regularization on the trajectory with the points outside of the gripper reachability. The example of an iterative calculation of joint trajectories for a 3-link robot using the recursive algorithm for the Jacobi matrix calculation has been presented.
ROBOT MOTION CONTROL SYSTEMS; INVERSE KINEMATIC PROBLEMS; ITERATIVE METHODS; ALGORITHM CONVERGENCE; REGULARIZATION.
При моделировании систем управления движением (СУД) манипуляционных роботов требуется решать с помощью ЭВМ обратные задачи кинематики (ОЗК) для их исполнительных механизмов (ИМ) аналитическими или итерационными методами. Алгоритмы решения таких задач составляют математическую основу разработки сУд роботов.
В теоретической робототехнике для решения озк часто используют алгоритмы на основе аналитических выражений, требующих вычисления обратных тригонометрических и трансцендентных функций. Такие алгоритмы получают непосредственно по
геометрическим моделям кинематики ИМ, либо по векторно-матричным моделям в представлении Денавита—Хартенберга, описывающим кинематику ИМ в однородных координатах [3—9].
однако для роботов со сложной кинематикой аналитические решения озк на заданной траектории схвата могут оказываться ошибочными в особых конфигурациях ИМ (положениях звеньев), а также на границе и вне зоны достижимости схвата из-за вырождаемости матрицы Якоби вследствие понижения ее ранга. В подобных случаях могут быть получены только приближенные решения ОЗК итерационными мето-
дами. Но при применении таких методов необходимо проводить исследование сходимости их алгоритмов.
В предлагаемой статье обсуждаются трудности аналитического решения ОЗК на примере трехзвенного робота с вращательными звеньями, обращается внимание на необходимость регуляризации итерационных алгоритмов для обеспечения их сходимости, описывается рекурсивный алгоритм вычисления матрицы Якоби и анализируется работоспособность итерационных алгоритмов на траекториях схвата, содержащих участки с особыми конфигурациями ИМ.
Общие формулировки ОЗК. В системах позиционного, скоростного и силомомент-ного управления движением роботов решаются разные обратные задачи кинематики.
ОЗК о положениях звеньев. По заданному 6x1 вектору линейных координат положения и угловых координат ориентации схвата
Дс = (хс, Ус, ¿с, Фс, 0с, Ус )Т
вычисляется № 1 вектор обобщенных координат звеньев
д = (?1, д2, ..., )Т = Ф-1^), (1)
где Ф-1(-) — N^1 вектор-функция, обратная 6x1 вектор-функции Ф(д), соответствующей кинематической схеме им робота. задача (1) является наиболее сложной с вычислительной точки зрения, т. к. она требует решения системы нелинейных алгебраических уравнений вида
Ф(д) - Бс = Ж (д) = 0,
где 0 — нулевой 6x1 вектор.
ОЗК о скоростях звеньев. По заданным векторам линейных и угловых скоростей схвата
V,) Т; Пс = (Юф, Юе, Ю )Т
Vc = (V,, Vy
вычисляется Nx1 вектор обобщенных ско ростей звеньев
• • • • (V. Л
д = (дх, д2, ..., дN)т = /_1(д)
ч^ с у
(2)
где / _10 — матрица, обратная (или псевдообратная) матрице Якоби /(д) вектор-функции Ф(д). Задача (2) представляет
собой решение системы линейных алгебраических уравнений вида
• ( V Л /(д)• ч= пс .
ОЗК о силах и моментах в шарнирах звеньев. По заданным векторам проекций силы и момента в схвате
= (Ж,, , ) Т; Мс = (Мф, Ме, Му )Т
определяется шарнирах
вектор обобщенных сил в
6 = (61,62, ..., QN )Т = / Т(д) •
(Л
Ч Мс у
(3)
Выражение (3), требующее вычисления и транспонирования матрицы Якоби, справедливо только для случая идеального им, не имеющего потерь энергии в шарнирах звеньев.
Матрица Якоби в задачах (1)—(3) в зависимости от числа звеньев N робота может быть квадратной или иметь прямоугольный вид:
/ (д) =
дФ(д) д Ж (д)
дд
дд
д Ж (д) ; =
д д,
I = 1,6, ] = 1, N
В более общей постановке задача (1) может формулироваться как задача минимизации квадрата нормы вектора невязки
(д)|| ^ шт,
(4)
и решаться итерационными методами без вычисления матрицы Якоби или градиентными методами, использующими ее численные аппроксимации. однако сходимость алгоритмов таких методов более медленная и при их применении требуется большее число шагов итерационного процесса [2].
В [4] приведен пример итерационного решения озк в формулировке (4) для 6-звенного робота, на программной траектории схвата которого постоянная ориентация схвата задана не углами Эйлера, а вектором направляющих косинусов.
Если же значения вектора обобщенных координат ограничены допустимой обла-
стью q е , то задачи вида (4) должны решаться методами условной минимизации. При этом точные решения ОЗК в принципе могут отсутствовать.
Аналитическое решение ОЗК. Рассмотрим 3-звенный робот ВВВ-типа, работающий в ангулярной системе координат, для которого задача (1) решается неоднозначно и зависит от знака углового положения третьго звена (нижней или верхней конфигурации руки):
ql = аг^ Ус/ХС );
q3 = ± агссоз(С); q2 = аг^^ - Ц) / Д^) + агссо8(О), (5а) где Ц1, Ц2, Ц — длины звеньев;
К*у, = V хС + УС + (,с - Ц)2 ;
С = (Х2 + у2с + (ТС - А)2 -
- Ц - Ц) / (2 Ц 4); о = (хС + уС + (Тс - Ц) +
+ Ц - Ц3) / (2 ьк,).
решение озк можно также получить по другим выражениям [5]:
ql = агс^( УС/ХС );
Е = V1 - С2; ^ = ±агс1Е(Е/С);
q2 = агс^«,С - Ц1) / Д„) -
- аг^
^ Ц3 8Ш q3 ^ Ц + Ц3 соз q3
(5б)
однако решения озк функциями МЛТЪЛБ по выражениям (5а) на траектории схвата с точками вне его зоны достижимости оказываются комплексными числами, не имеющими практического смысла. Для получения вещественных решений ОЗК значения переменных С и О, являющихся косинусами, должны ограничиваться единицей. Решения же ОЗК по выражениям (5б) получаются вещественными в точках и внутри, и вне зоны достижимости схвата, поэтому нет необходимости ограничивать значения переменных С и Е.
Алгоритмы итерационных методов. Рассмотрим алгоритмы численного решения задачи (1), в которых матрица Якоби вычисляется в явном виде.
Алгоритм метода Ньютона. Теоретически в случае квадратной матрицы Якоби, например, когда N = 3 или 6, можно применить следующий алгоритм:
qk+1 = ^ -акГ^к) • Е^), (6)
где ак < 1 — скалярная величина постоянного или переменного шага. При этом на каждом шаге итерационного процесса требуется вычислять обратную матрицу, соответствующую матрице Якоби. Однако в особых конфигурациях ИМ при плохой обусловленности или полной вырождаемости матрицы Якоби из-за понижения ее ранга работоспособность алгоритма (6) теряется, Упрощенный алгоритм метода Ньютона. При замене в алгоритме (6) обратной матрицы I-1 на транспонированную матрицу Якоби
qk+1 = ф -ак1 т^к) • Е), (7)
могут быть получены грубые решения задачи (1), но в особых конфигурациях ИМ сходимость алгоритма (7) также не гарантируется.
Алгоритм метода Гаусса—Ньютона. При решении задач с прямоугольной матрицей Якоби, когда число звеньев избыточно (^ > 6) или недостаточно N < 6) в алгоритме (6) вместо обратной матрицы I 1 необходимо использовать левую I + = (IТ • I)-11Т или правую I += IТ(! • IТ)-1 псевдообратные матрицы [1]. При этом получаем следующий алгоритм:
= qk -аI + (^) • Е(дк).
(8)
Алгоритмы метода Левенберга—Марк-вардта. В алгоритме (8) при вычислении псевдообратных матриц из-за того, что матрицы (Iт • I) и (I • IТ) могут оказываться плохо обусловленными или вырожденными, для обеспечения сходимости итерационных процессов необходимо осуществлять факторизацию на основе матричного разложения или использовать простую регуляризацию:
qk+1 = qk -ак[(IТI + а2кЕ)-11Т]Е); (9) qk+1 = ^^ - ак[IТ(ИТ + а2кЕ)-1-к), (10)
где Е — единичная матрица; а2к — регуля-ризирующий скалярный параметр.
При применении любого из алгоритмов (6)—(10) матрица /(#) может вычисляться либо по получаемым прямым дифференцированием вектор-функции Ф(#) аналитическим выражениям, достаточно громоздким при N > 3, либо по более эффективному рекурсивному алгоритму [9], использующему матрицы преобразования однородных координат и операции пересечения (векторные произведения) столбцов матрицы поворота схвата (п, о, а) — векторов нормали, ориентации и подхода.
Рекурсивный алгоритм вычисления матрицы Якоби. В базовой системе координат робота матрица Якоби осуществляет преобразование вектора обобщенных скоростей звеньев:
(V Л
л
= Со(?) • 2,
где Vс.о и ^с
векторы проекций линей-
ной и угловой скорости схвата. при этом матрица Якоби определяется как произведение Со(2) = (2) • JN(2), где JN(2) - 6xN матрица Якоби преобразования скоростей в системе координат схвата
(V Л
г с.N
ЛГ
= JN (2) • 2;
Су (2) — 6Х6 матрица преобразования линейных и угловых скоростей схвата из ^й в базовую систему координат
(V Л
' с ,0
^^^ п
= Су (2) •
(V л
г с.N
Для вычисления матрицы JN (2) требуются 4x4 матрицы, определяющие положение и ориентацию схвата в у-й системе координат, т. е. относительно (у — 1)-го звена:
и' (2) = Ау (21)Ау+1(22) • • • AN (2N) = 0 0 0 1
где ^, ] = (nN, у, oN, у, aN, у) — матрица поворота с векторами ориентации системы координат схвата; pN, у = ^, у, УN, у, ^, у )Т — вектор положения центра системы координат схвата.
Столбцы матрицы См (2) = J2N ..
... J6N) вычисляются по обратной рекурсии (у' = 6, 5, ..., 1):
=
(^ л
V8у у
'8 у ^
V 03 х1 у
-для у-звена В-типа;
- для у-звена П-типа,
где й.
3x1 вектор, составленный из
7-составляющих векторных произведений
=
( ( PN, у * %, у X ^ (PN, / * ^, / )
(PN,! * aN,! )
N, у
V(pN, у
N,у ^ N, у )г У
(р п ■ — р п Л
Г X, у У, у г у, у X, ]
р о — р о .
р а ■ — р а ■
^ X, у у, у Г у, у X, у
8 у — единичный 3x1 вектор, направленный вдоль оси движения у-го звена
8 у =
( 0 л 0
V1 у
(„ \
V а, у у
0
нулевой 3x1 вектор.
Матрица Jу (2) в блочном представлении имеет следующий вид:
Jу (2) =
( Л
N ,0 03Х3 Л
0 Л
V 3х3 Л■N,0 у
где 03 х 3 — нулевая матрица; Л0(2) — 3x3 матрица поворота с векторами-столбцами нормали, ориентации и подхода, вычисленными в базовой системе координат.
Более детально рекурсивный алгоритм вычисления матрицы Якоби описан в [5].
Анализ сходимости итерационных алгоритмов. Проанализируем процессы итерационного решения ОЗК по алгоритмам (6)—(10) с вычислением матрицы Якоби с использованием рассмотренного выше рекурсивного алгоритма на двух траекториях схвата робота ВВВ-типа (рис. 1). Траектории заданы базовыми точками = {X, У, 7}, первая — в зоне достижимости схвата, вторая — с некоторыми точками вне ее границы.
На рис. 2 и 3 показаны траектории звеньев, полученные для верхней конфигурации робота аналитическими методами (5а) и (5б), а вычисленные по этим траекториям путем решения прямой задачи кинематики соответствующие траектории схвата
= {X, Ус, 2} показаны на рис. 1 сплошными линиями.
На рис. 4 представлены графики модулей определителя матрицы Якоби робота ВВВ-типа, вычисленной по рекурсивному алгоритму для двух вариантов траекторий звеньев (см. рис. 2 и 3). Точно такие же графики получаются при вычислении эле-
ментов матрицы Якоби по аналитическим выражениям. По графикам видно, что для второго варианта траекторий в точках на границе зоны достижимости схвата определитель вырожденной матрицы Якоби принимает нулевое значение.
на первоой траектории схвата решения, получаемые всеми сравниваемыми алгорит-
X, м
9
а
1—I
Он
0-8 0.2 04
Z, м
1.4
1.2 _ х- л»__I -
1
0.8« г г V л
0.6 Л Т ь л т
0.4 \ Л1
0.2 I тШ Г 1
Рис. 1. Заданные траектории схвата 1.5
234 0 2 4 01234
и с с г, С
Рис. 2. Результаты решения ОЗК на первой траектории схвата
К •V Г л г / _____
----¡V--
г к I / 1 к г 1 \ _____
С С
Рис. 3. Результаты решения ОЗК на второй траектории схвата
t, С
Рис. 4. Графики модулей определителя матрицы Якоби
мами (6)—(10), совпадают с траекториями звеньев (см. рис. 2), вычисленными аналитическими методами. При этом для каждого из алгоритмов были настроены значения постоянных параметров ак и а2к, обеспечивающие сходимость процессов решения ОЗК за число итераций ктах < 100.
На второй траектории схвата из-за вырождаемости матрицы Якоби в особых точках сходимость алгоритмов (6) и (8) теряется. Алгоритмы (9) и (10) с регуляризи-рованными левой и правой псевдообратными матрицами наиболее устойчивые. При значениях ак = 2, а2к = 0,1 они дают такие же решения, как на рис. 3, за число итераций ктах < 30. Алгоритм (7), использующий транспонированную матрицу Якоби, сходится, но дает очень грубые решения.
Таким образом, по результатам проведенного исследования можно дать следующие практические рекомендации.
При моделировании и разработке си-
стем управления движением манипуляци-онных роботов целесообразно применять итерационные алгоритмы решения озк. Аналитические решения по выражениям, содержащим обратные тригонометрические функции, в особых конфигурациях ИМ могут оказываться некорректными.
наиболее точные решения озк многозвенных роботов могут быть получены по алгоритмам (9) и (10) с регуляризацией, в которых матрицу Якоби рекомендуется вычислять без численного дифференцирования по рекурсивному алгоритму, использующему кинематические модели в однородных координатах.
При программировании (планировании) траекторий схвата в системах управления движением роботов следует стараться избегать ситуаций, показанных на рис. 1 и 3. Базовые точки должны задаваться в рабочей области робота, определяемой габаритными размерами звеньев и допустимыми диапазонами изменения обобщенных координат.
список литературы
1. Беклемишев Д.В. Дополнительные главы линейной алгебры: учеб. пособие. М.: Наука, 1983. 336 с.
2. Деннис Д., шнабель Р. Численные методы безусловной оптимизации и решения нелинейных уравнений. М.: Мир, 1988. 440 с.
3. зенкевич с.Л., ющенко А.с. Управление роботами. Основы управления манипуляцион-ными роботами: учеб. для вузов. М.: Изд-во МГТУ им. Н.Э. Баумана, 2000. 400 с.
4. игнатова Е.и., Ростов н.В. Кинематические задачи в математическом моделировании роботов // Научно-технические ведомости СПбГПУ. Информатика. Телекоммуникации. Управление. СПб.: Изд-во СПбГПУ, 2010.
№ 2(97). С. 159-164.
5. игнатова Е.и., Лопота А.В., Ростов н.В.
Системы управления движением роботов. Компьютерное проектирование. СПб.: Изд-во СПбГПУ, 2014. 302 с.
6. шахинпур м. Курс робототехники. Пер. с англ. под ред. С.Л. Зенкевича. М.: Мир, 1990. 527 с.
7. Фу к., гонсалес Р., Ли к. Робототехника. Пер. с англ. под ред. В.Г. Градецкого. М.: Мир, 1989. 624 с.
8. юревич Е.и. Основы робототехники: учеб. для вузов. СПб.: БХВ-Петербург, 2005. 416 с.
9. Corke P.I. Robotics Toolbox 9.9 for MATLAB R9. 2014. 286 p.
references
1. Beklemishev D.v. Dopolnitelnyyeglavy lineynoy algebry. Moscow: Nauka Publ., 1983, 336 p. (rus)
2. Dennis D., Shnabel R. Chislennyye metody bezuslovnoy optimizatsii i resheniya nelineynykh uravneniy. Moscow: Mir Publ., 1988, 440 p. (rus)
3. zenkevich S.L., Yushchenko A.S. Upravleniye robotami. Osnovy upravleniya manipulyatsionnymi robotami. Moscow: MGTU im. N.E. Baumana Publ., 2000, 400 p. (rus)
4. Ignatova Ye.I., Rostov N.v. Kinematicheskiye zadachi v matematicheskom modelirovanii robotov, Nauchno-tekhnicheskiye vedomosti SPbGPU. Informatika. Telekommunikatsii. Upravleniye. St. Petersburg: SPbGPU Publ, 2010, No. 2(97), Pp. 159-164. (rus)
5. Ignatova Ye.I., Lopota A.v., Rostov n.v.
Sistemy upravleniya dvizheniyem robotov. Kompyuternoye proyektirovaniye. St. Petersburg: SPbGPU Publ, 2014, 302 p. (rus)
6. Shakhinpur M. Kurs robototekhniki, Per. s angl. pod red. S.L. Zenkevicha. Moscow: Mir Publ., 1990, 527 p. (rus)
7. Fu K., Gonsales R., Li K. Robototekhnika, Per. s angl. pod red. V.G. Gradetskogo. Moscow: Mir Publ., 1989, 624 p. (rus)
8. Yurevich Ye.I. Osnovy robototekhniki. St. Petersburg: BKhV-Petersburg Publ., 2005, 416 p. (rus)
9. Corke P.I. Robotics Toolbox 9.9 for MATLAB R9. 2014, 286 p.
РОсТОВ Ииколай Васильевич — доцент кафедры систем и технологий управления Санкт-Петербургского государственного политехнического университета, кандидат технических наук. 195251, Россия, Санкт-Петербург, ул. Политехническая, д. 29. E-mail: [email protected]
RoSTov, Nicolay v. St. Petersburg Polytechnic University. 195251, Politekhnicheskaya Str. 29, St. Petersburg, Russia. E-mail: [email protected]
© St. Petersburg State Polytechnical University, 2014
