CC BY
629
УДК 621.38 (62 - 52)
ПОСТРОЕНИЕ ОБОБЩЕННОЙ МАТЕМАТИЧЕСКОЙ МОДЕЛИ МАНИПУЛЯТОРА ПОДВИЖНОГО РОБОТА
Д.П. Лимаренко
Рассмотрено построение обобщенной математической модели манипулятора подвижного робота. Математическая модель манипулятора включает в себя решение обратной задачи кинематики, преобразование угловых положений звеньев в управляющие электрические сигналы, интегрирование динамической модели электроприводов звеньев, решение прямой задачи динамики для манипуляционной системы и задачи о положении рабочей точки манипулятора.
Ключевые слова: манипулятор, математическая модель, обратная задача кинематики, прямая задача динамики.
Анализ, конструкции манипулятора и входящих в них элементов, позволяет представить структурную схему математической модели манипулятора, предназначенной для моделирования функционирования системы управления и исполнительной системы манипулятора, следующим образом на рис. 1, где М - вектор положения точки рабочего органа манипулятора (заданный); Аг - вектор рассогласования положения рабочего органа и точки позиционирования; Aq - вектор задаваемых угловых положений звеньев; AU - вектор входных напряжений на электроприводы; Uo - вектор напряжений обратных связей приводов; АБ - вектор напряжений рассогласований приводов; АМ - вектор моментов приводов; AQ - вектор моментов, приложенных к звеньям; Оґ - вектор силы тяжести; Mg - вектор приведенных моментов от силы тяжести; q - вектор обобщенных координат манипулятора; rs -вектор положения рабочего органа; го - вектор обратной связи в горизонтальной плоскости; Mq - блок решения обратной задачи кинематики; Ми - блок преобразования угла в напряжение; Р - блок динамической модели электропривода; Ко - блок формирования напряжений с фотоимпульсных датчиков; К - блок решения прямой задачи динамики для манипуляционной системы; Мг - блок формирования положения рабочего органа; £ - блок модели системы обратной связи по положению.
В данном роботе система управления является многоконтурной и шестимерной по числу приводов звеньев манипулятора. Ее функционирование заключается в следующем. Входным воздействием для системы является задаваемое положение точки позиционирования - и представляет собой трехкомпонентный вектор Ы. При использовании в горизонтальной плоскости для отрицательной обратной связи системы обратной связи по положению из вектора Ы будет вычитаться вектор го, две компоненты которого (хо, уо) вырабатываются системой обратной связи, а третья zo=zs.
Рис. 1. Структурная схема математической модели шестистепенного
манипулятора
Сформированный таким образом вектор поправок Аг далее должен быть преобразован в шестикомпонентный вектор углов, а затем в шестикомпонентный вектор управляющих воздействий в виде напряжений АП на электроприводы постоянного тока звеньев манипулятора. Непосредственно на модель электропривода Р подается вектор Е, получаемый как разность АП-По , где вектор По формируется как сигнал обратной связи от фотоимпульсных датчиков на предыдущем цикле расчета из вектора положения манипулятора Ад. Вектор выходных моментов АМ, рассчитанный по модели электропривода, поступает на кинематическую модель манипулятора К. Так как звенья манипулятора непрерывно испытывают дополнительное воздействие силы тяжести, то результирующим воздействием на эту модель является вектор моментов АQ, получаемый как сумма управляющих моментов АМ от приводов и приведенных моментов Mg, возникающих от вектора силы тяжести Gí. Путем решения прямой задачи кинематики получается вектор реальных положений звеньев д. По нему рассчитывается пространственное положение рабочего органа rs. Данное реальное положение в горизонтальной плоскости используется в математической модели системы обратной связи по положению Б, с помощью которой вся система управления замыкается для нового расчета положения рабочего органа.
Таким образом, математическая модель манипулятора включает в себя решение обратной задачи кинематики, преобразование угловых положений звеньев в управляющие электрические сигналы, интегрирование динамической модели электроприводов звеньев, решение прямой задачи динамики для манипуляционной системы и задачи о положении рабочей точки манипулятора.
Для шестистепенного манипулятора обратная задача кинематики имеет несколько решений [1]. По аналогии с геометрией человеческой ру-
ки и в зависимости от положения звеньев для шестистепенного манипулятора существует четыре различных решения обратной задачи кинематики для первых трех сочленений и каждому из этих четырех решений соответствует по два возможных решения для последних трех сочленений. При поиске решения в общем виде можно использовать индикаторы конфигурации манипулятора: рука, локоть, запястье. При этом первые два индикатора определяют решение для первых трех сочленений, а второй - для последних трех. Выбор индикаторов, то есть пространственное положение манипулятора при подходе к рабочей точке предварительно задается пользователем. Однако в качестве выбранного решения принимается решение, при котором конфигурация манипулятора способна осуществлять движение по вертикали и испытывает существенное воздействие силы тяжести и взаимное влияние звеньев, то есть напоминает положение вытянутой руки.
Электрические сервоприводы постоянного тока, обеспечивающие силовое воздействие на манипуляционную систему, являются узлами, во многом определяющими точность функционирования манипулятора. С учетом того, что рассматривается моделирование с малыми геометрическими параметрами, необходимо, чтобы динамическая модель электрического сервопривода учитывала такие важные, влияющие на точностные показатели всего комплекса, характеристики как сухое трение в кинематике привода, упругость и вязкость, люфт в зубчатых соединениях [2].
Тогда для выбранного решения обратной задачи кинематики, уравнения связи вектора положения рабочего органа и обобщенных координат
- углов поворота степеней подвижности - представлены ниже:
2 2 q1 = - arccos(sm / -\/((xd-xs) + (yd-ys) ) ) +
V2 2
((xd-xs) + (yd-ys) ));
2 2 2 2 q2 = - arccos((zd - zs - /1) / ((xd-xs) + (yd-ys) + (zd-zs-l\) -sm ) +
+ arccos ((/32-/22-((xd-xs)2 + (yd-ys)2 + (zd-zs-/1)2-sm2)) /
/(-2 • /2 •(xd-xs)2 + (yd-ys)2 + (zd-zs-/1)2 -sm 2));
q3 = -p + arccos((((xd - xs)2 + (yd - ys)2 + (zd - zs - /1)2 - sm2) - /22 - /32) / ' /(-2 • /22 • /32));
q4 = arctan ((ys • cos(q1)-xs • sin(ql)) /
/ (xs • cos(q1) • cos(q2 + q3) + ys • sin(q1) • cos(q2 + q3) + zs • sin(q2 + q3))); q5 = arctan (xs(cos(q1) • cos(q2 + q3) • cos(q4)- sin(q1) • sin(q4) +
+ ys(sin(q1) cos(q2 + q3) cos(q4) + cos(q1) sin(q4)-zs • cos(q4) sin(q2 + q3)))/
/ (xs cos(q1) sin(q2 + q3) + ys sin(q1) sin(q2 + q3) + zs cos(q2 + q3)); q6 = 0;
где д1 -д6 - обобщенные координаты манипулятора; xs, ys, zs - декартовы координаты детали в инструментальной системе координат (ориентация рабочего органа); хй, уй, zd - декартовы координаты рабочей точки в основной системе манипулятора; 11, 12, 13 - длины 1, 2, 3 степеней подвижности манипулятора; sm - расстояние плоскости вращения 3 звена до оси вращения 1 звена.
Величина д6 постоянна для перемещений в горизонтальной плоскости.
Анализ конструкции сервопривода показывает, что сигнал обратной связи по координате формируется с помощью фотоимпулъсных датчиков, расположенных непосредственно на валу вращения электродвигателя. Это позволяет с высокой точностью позиционировать угловое положение двигателя, но каждая степень подвижности приводится в движение через редуктор и имеет механическое сочленение со смежными степенями подвижности. Поэтому вводимая обратная связь охватывает не все звенья в сервоприводе. Данный результат важен с точки зрения последующего включения в систему управления системы адаптации. Ее наличие в качестве датчика обратной связи по положению рабочего органа манипулятора может скомпенсировать погрешности, возникающие и накапливающиеся из-за неучтенных характеристик люфтов и внешних возмущающих моментов.
Анализ системы управления манипулятора, позволяет дополнить структурную схему сервоприводов несколькими звеньями: усилителем мощности, звеном демпфирования привода, звеном квантования по времени, звеном скоростной обратной связи, реализованном в микропроцессорном модуле управления приводом. Наличие звена квантования по времени определяется внутренним микропроцессорным контуром управления сервоприводом и оказывает существенное влияние на динамику привода при перемещениях на малые величины. Звено демпфирования обусловлено конструкцией привода [3].
Таким образом, математическую модель сервопривода можно представить следующей системой дифференциальных уравнений:
ТтйМп/= (Ку(Пм - Кос • Ап - Ку • Ап') - Се) Ст/Яя - Мп;
й2 Ап/2 = (Мп - МИ - Мм/) / Зп;
МV = А1 (Сп - йНу / )(Ап - Ам);
й2 Ам/2 = (Мм - М1) / Зп;
Соответствующая ей структурная схема оказывается достаточно сложной. С учетом введенных нелинейных звеньев она приведена на рис.
2, где Пм - управляющее напряжение; Мп - момент идеального привода; Мм - момент, обусловленный вязкостью и жесткостью редуктора; Mt -момент, обусловленный силой сухого трения; МИ - момент, обусловлен-
81
ный силой вязкого трения; Ап'- угловая скорость идеального привода; Ап
- угол вращения привода; А1 - угол вращения привода с учетом люфта в соединениях; Ам - выходной угол вращения для реального привода; Яя -сопротивление обмотки якоря двигателя; Тт - механическая постоянная времени двигателя; Ст - коэффициент передачи по моменту привода; Се -коэффициент противоЭДС привода; М - момент инерции привода; Сп -коэффициент жесткости привода; Ну - коэффициент вязкого трения редуктора привода; М - момент инерции нагрузки; Ку - коэффициент передачи по угловой скорости; Кос - коэффициент передачи по угловому перемещению; 12 - звено квантования по времени.
Манипулятор является линейной неголономной системой [1]. Поэтому при получении уравнений движения вместо уравнений Лагранжа 2го рода для кинетической энергии используются уравнения Аппеля для энергии ускорений:
эз/ Э " = Qi; I =1 п С1)
где 3 - энергия ускорений, д,"- обобщенное ускорение, Qi - обобщенная сила
В общем виде она записывается аналогично кинетической энергии системы квадратичной формой, но при этом вместо скоростей в формуле присутствуют ускорения. В представлении через обобщенные координаты, скорости и ускорения функция Аппеля принимает следующий вид:
п п п
3 =0,5 • ЕЕ А] (д) • т ] + Е В (д, ~) ■ 'д\ + С (д, (2)
I =1 ]=1 i=1
где д — вектор обобщенных координат, ~ - вектор обобщенных скоростей.
Вычисление коэффициентов Лу(ц), Ві(ц, ц), С(ц, ц) произведено по алгоритму А.Ф. Верещагина [1]. Алгоритм использует переходные системы координат, которые получены операциями сдвига и поворота при движении от основания манипулятора, неподвижно связанного с землей, к рабочему органу (РО) по геометрии манипуляционной системы. Для одного цикла вычислений его можно описать следующим образом. Вектор координат РО в декартовой системе и вектор обобщенных координат имеют текущие рассчитанные значения от предыдущего цикла. Для текущей конфигурации манипулятора рассчитываются компоненты векторов силы тяжести для всех переходных систем координат. Далее при задании вектора обобщенных скоростей в виде набора нулевых компонент за исключением одной, равной единице, производится перебор единичного значения по всем компонентам при остальных, равных нулю. При этом рассчитываются угловые скорости звеньев в переходных системах координат, линейные скорости центров систем координат, линейные скорости центров масс, в связанных с ними системах координат. В результате вычислений формируется вектор возмущающих моментов от силы тяжести.
После этого, при текущем, рассчитанном на предыдущем цикле, значении вектора обобщенных координат восстанавливается значения векторов обобщенных скоростей, угловых скоростей звеньев в переходных системах координат, линейных скоростей центров систем координат. Далее производится перебор наборов компонент вектора обобщенных ускорений от нулевых значений всех компонент до приравнивания каждой компоненты единице и минус единице при остальных, равных нулю. При использовании каждого набора производится расчет угловых ускорений в переходных системах координат, линейных ускорений центров систем координат, линейных ускорений центров масс, в связанных с ними системах координат, энергии ускорений по формуле:
$ = 0,5 ' Е (ші ' асі + еі '[^ сі]' еі - {4 ' ([^ Сі ]' Мі) * Мі} ' еі), і=1
где Ші - масса звена, асі - вектор линейного ускорения центра масс звена, Є - вектор углового ускорения, Мі - вектор угловой скорости, Зсі - тензор инерции звена. Все указанные величины определяются в связанных со звеньями системами координат. Затем на основе приравнивания (1) и (2) получаются коэффициенты Лі], Ві, С.
Последним этапом вычислений является формирование правой части системы из шести уравнений как разницы момента нагрузки со стороны сервопривода, момента от силы тяжести и момента взаимного динамического влияния звеньев для нахождения обобщенных ускорений, и собственно решение этой системы уравнения методом Г аусса с получением новых значений обобщенных ускорений, а затем в результате интегрирова-
ния новых значений обобщенных скоростей и координат манипуляционной системы.
Список литературы
1. Зенкевич С.Л. Основы управления манипуляционными роботами: учебник для вузов / С.Л.Зенкевич,А.С.Ющенко.-2-е изд., испр. и доп. М. : Изд- во МГТУ им. Н.Э.Баумана, 2004. 480с.
2. Чемоданов Б.К. Следящие приводы: В 3 т. Т.2. Электрические следящие приводы/Е.С.Блейз, В.Н.Бродовский, В.А.Введенский и др. / под ред. Б.К. Чемоданова 2-е изд., перераб. и доп. М.: МГТУ им.Н.Э.Баумана, 2003. 878 с.
3. Шенфельд Р. Автоматизированные электроприводы / Р.Шёфельд,
Э.Хабигер; пер.с нем. Л.С. Антиповой и др.; под ред. Ю.А.Борцова . Л.: Энератомиздат, 1985. 464с.
Лимаренко Денис Павлович, канд. техн. наук, доц., denli_73@mail.ru, Россия, Тула, Тульский государственный университет
BUILDING A GENERALIZED MATHEMATICAL MODEL OF THE MOBILE ROBOT MANIPULA TOR
D.P. Limarenko
Considers the construction of a generalized mathematical model of the mobile robot manipulator. A mathematical model of the manipulator includes a solution for the inverse kinematics problem, the conversion of the angular positions of the links in the managing electrical signals, the integration of the dynamic model of an actuator units, the solution of the direct problem for the handling dynamics of the system and the problem of the position of the operating point of the manipulator.
Key words: manipulator, a mathematical model, inverse kinematics problem, direct dynamics problem.
Limarenko Denis Pavlovich, candidate of technical science, docent, denli_73@mail.ru, Russia, Tula, Tula State University
