СПОСОБ УПРАВЛЕНИЯ РАБОЧИМ ОРГАНОМ МНОГОСТЕПЕННОГО МАНИПУЛЯТОРА
METHOD FOR CONTROLLING THE WORKING BODY OF A MULTI-STAGE MANIPULATOR

Изобретение относится к робототехнике и может быть использовано при создании контурных систем управления многостепенными манипуляторами.
Известен способ управления движением динамического объекта по траектории, включающий подачу на входы систем каждого канала управления программных воздействий, определяющих требуемое местоположение объекта на траектории в произвольный момент времени, оценку в каждом канале управления текущего отклонения от желаемого, задаваемого сигналами программных воздействий каждого канала, использование в каждом канале соответствующего текущего отклонения для получения корректирующих сигналов управления, уменьшающих величины этих текущих отклонений от сигналов программных воздействий, причем сигналы всех программных воздействий учитывают и используют при формировании в каждом канале управления дополнительных корректирующих сигналов управления для дополнительного уменьшения текущих отклонений от сигналов программных воздействий, причем скорость движения динамического объекта на конкретных участках траектории с помощью соответствующей одновременной коррекции сигналов программных воздействий в каждом канале управления задают максимально возможной обратно пропорциональной значению текущего отклонения местоположения этого объекта от заданного сигналами указанных программных воздействий местоположения на траектории движения объекта, но такой, чтобы при этом значения текущего отклонения местоположения этого объекта от заданного сигналами программных воздействий местоположения на траектории его движения ограничивались величиной, не превышающей заранее заданное для данного динамического объекта допустимое значение (Патент РФ №2406103, кл. G05B15/00. Бюл. №34, 2010).
Недостатком этого способа является то, что программные (желаемые) воздействия на каждый канал управления движением динамических объектов, определяющие предельно возможные скорости их движения по заданным траекториям, корректируются с учетом информации о текущих динамических ошибках в отработке этими объектами заданных программных сигналов, и не учитывают возможные входы некоторых исполнительных элементов в насыщения. Поэтому быстрое устранение указанных ошибок может быть затруднено, поскольку некоторые каналы управления объектами могут находиться в режиме насыщения, не реагируя на поступающие сигналы управления. В результате указанные объекты могут просто неконтролируемо сходить с предписанных им траекторий.
Известен также способ управления движением динамического объекта по траектории, включающий подачу на входы систем каждого канала управления программных воздействий, определяющих требуемое местоположение объекта на траектории в произвольный момент времени, оценку в каждом канале управления текущего отклонения от желаемого, задаваемого сигналами программных воздействий каждого канала, использование в каждом канале соответствующего текущего отклонения для получения корректирующих сигналов управления, уменьшающих величины этих текущих отклонений от сигналов программных воздействий, причем сигналы всех программных воздействий учитывают и используют при формировании в каждом канале управления дополнительных корректирующих сигналов управления для дополнительного уменьшения текущих отклонений от сигналов программных воздействий, причем скорость движения динамического объекта на конкретных участках траектории с помощью одновременной коррекции сигналов программных воздействий в каждом канале управления задают максимально возможную, увеличивая ее до тех пор, пока в наиболее нагруженном в текущий момент времени канале (или каналах) управления динамическим объектом величина(величины) входного сигнала, прямо пропорциональная скорости движения динамического объекта по траектории, еще не вводит соответствующий исполнительный элемент (элементы) наиболее нагруженного канала (каналов) управления в зону насыщения и в зону нелинейности его (их) характеристики, и уменьшая эту скорость прямо пропорционально величине входного сигнала, превышающей по модулю некоторое его (их) предельно допустимое значение (Патент РФ №2480805, кл. G05B 6/00, G05B 13/02, Бюл. №12, 2013).
Недостаток известного способа в том, что его использование для высокоскоростного управления многостепенными манипуляторами (ММ) может привести к входу в насыщение одного или сразу нескольких их электроприводов. В результате точность их движения резко падает. Это может привести к возникновению аварийных ситуаций, поскольку указанные насыщения в изобретении не контролируются.
Известен также способ управления рабочим органом многостепенного манипулятора, заключающийся в том, что его текущее желаемое положение в трехмерном пространстве задают посредством блока формирования траекторий функциями времени, при этом с использованием вычислительного устройства с учетом текущего желаемого положения рабочего органа многостепенного манипулятора формируют текущие желаемые значения всех обобщенных координат этого манипулятора, поступающие на входы всех его исполнительных следящих электроприводов, причем скорость перемещения рабочих органов манипуляторов настраивается с учетом текущих значений входных сигналов (напряжений) и якорных токов электроприводов, обеспечивая ее увеличение или уменьшение с заданным постоянным ускорением при, соответственно, уменьшении или превышении значениями указанных входных сигналов и якорных токов заданных пороговых значений (Филаретов В.Ф., Зуев А.В., Губанков А.С. Управление манипуляторами при выполнении различных технологических операций // М.: Наука, 2018. С. 56-67.).
Этот способ является наиболее близким к заявляемому изобретению и принят за прототип.
Его недостатком является то, что он не позволяет учитывать текущие конфигурации управляемых манипуляторов, когда уменьшение скоростей движения их рабочих органов по траектории может приводить не к уменьшению, а к увеличению указанных сигналов на входах соответствующих электроприводов, например, за счет моментов от сил тяжести в сочленениях манипуляторов. Из-за чего при использовании этого способа электроприводы некоторых степеней подвижности все еще могут входить в насыщения, приводя к резкому уменьшению точности движения манипуляторов. Кроме того, траектории движения рабочих органов ММ, формируемые с помощью этого способа, задаются до начала работы в блоке формирования траекторий обычными функциями времени и не могут быть изменены, когда это потребуется, непосредственно в процессе самого движения.
Задачей заявляемого изобретения является устранение указанных выше недостатков, то есть обеспечение предельно высокой скорости движения рабочих органов ММ, при которой неизменно сохраняется заданная динамическая точность управления, а их усилители мощности и электроприводы неизменно работают в линейной зоне и без входа в режим насыщения, неизменно сохраняя высокую точность движения.
Технический результат изобретения заключается в автоматическом перемещении рабочих органов ММ по пространственным траекториям на предельно высоких скоростях при неизменном сохранении высокой точности управления. Это достигается за счет формирования таких желаемых сигналов, подаваемых на входы каждого следящего электропривода ММ, которые обеспечат работу одного или нескольких электроприводов в преднасыщенном состоянии.
Поставленная задача решается тем, что способ управления рабочим органом многостепенного манипулятора, заключающийся в том, что его текущее желаемое положение в трехмерном пространстве задают посредством блока формирования траекторий функциями времени, при этом с использованием вычислительного устройства с учетом текущего желаемого положения рабочего органа многостепенного манипулятора формируют текущие желаемые значения всех обобщенных координат этого манипулятора, поступающие на входы всех его исполнительных следящих электроприводов, отличается тем, что текущее желаемое положение рабочего органа многостепенного манипулятора задают посредством блока формирования траекторий, который реализует параметрические сплайны с возможностью их коррекции в функции времени, при этом желаемую скорость перемещения рабочего органа по траекториям задают посредством блока формирования скорости с учетом параметров всех следящих электроприводов многостепенного манипулятора и предельных значений их входных сигналов с использованием непрерывно рассчитываемых в нем текущих значений внешних моментных воздействий на эти электроприводы, а также параметров текущих пространственных траекторий и текущей конфигурации многостепенного манипулятора, всю указанную информацию подают на входы блока формирования скорости с выхода блока формирования траекторий и выходов датчиков положения, скорости и ускорения, установленных на выходных валах редукторов всех следящих электроприводов, причем задаваемая желаемая скорость перемещения рабочего органа по траекториям обеспечивает работу хотя бы одного из следящих электроприводов многостепенного манипулятора в линейной зоне и преднасыщенном состоянии, при этом желаемую скорость обнуляют посредством блока формирования скорости при подходе рабочего органа многостепенного манипулятора к концам траекторий или обеспечивают ее ограничение на заданном уровне для соблюдения требований технологического процесса.
Сопоставленный анализ признаков заявляемого способа с признаками аналогов и прототипа свидетельствует о соответствии этого способа критерию "новизна".
При этом отличительные признаки формулы изобретения решают следующие функциональные задачи.
Признак «…текущее желаемое положение рабочего органа многостепенного манипулятора задают посредством блока формирования траекторий, который реализует параметрические сплайны с возможностью их коррекции в функции времени…» позволяет задавать текущее желаемое положение рабочего органа ММ и корректировать его, используя удобный для реализации предложенного способа аппарат параметрических сплайнов. В отличие от использованного в прототипе метода задания текущего желаемого положения, с помощью указанных сплайнов существенно упрощается задание движения по сложным замкнутым пространственным траекториям, а также обеспечивается автоматически непрерывность первых и вторых производных координат в трехмерном пространстве, что необходимо для плавного перемещения рабочего органа по сформированным траекториям.
Признак «…при этом желаемую скорость перемещения рабочего органа по траекториям задают посредством блока формирования скорости с учетом параметров всех следящих электроприводов многостепенного манипулятора и предельных значений их входных сигналов с использованием непрерывно рассчитываемых в нем текущих значений внешних моментных воздействий на эти электроприводы, а также параметров текущих пространственных траекторий и текущей конфигурации многостепенного манипулятора, всю указанную информацию подают на входы блока формирования скорости с выхода блока формирования траекторий и выходов датчиков положения, скорости и ускорения, установленных на выходных валах редукторов всех следящих электроприводов, причем задаваемая желаемая скорость перемещения рабочего органа по траекториям обеспечивает работу хотя бы одного из следящих электроприводов многостепенного манипулятора в линейной зоне и преднасыщенном состоянии…», максимально повышая скорость перемещения рабочего органа по траектории, обеспечивает повышение производительности работы ММ без снижения его точности за счет непрерывного поддержания хотя бы одного из его следящих электроприводов в преднасыщенном состоянии.
Признак «...при этом желаемую скорость обнуляют посредством блока формирования скорости при подходе рабочего органа многостепенного манипулятора к концам траекторий или обеспечивают ее ограничение на заданном уровне для соблюдения требований технологического процесса.» позволяет снижать скорость движения рабочего органа ММ до полной остановки при его подходе к концам траекторий, поддерживая требования обеспечения технологического процесса.
Заявляемое изобретение иллюстрируется чертежами, где на фиг. 1 показана обобщенная схема технологического процесса; а на фиг. 2 - обобщенная схема системы управления ММ. На чертежах показаны: 1 - рабочий орган ММ 2, выполняющий перемещение по пространственной траектории 3; 4 - блок формирования траекторий (БФТ); 5 - вычислительное устройство (ВУ); 6 - следящий электропривод (СЭП) i-ой степени подвижности  7 - блок формирования скорости (БФС).
k,0
k,3
k,1
k,2
k-1,3
k+1,0
i
i
T
*
*
На чертежах введены следующие обозначения: k-номер текущего параметрического сплайна, входящего в состав траекторий 3; τ∈[0,1] - параметр, однозначно определяющий координаты желаемого положения рабочего органа 1 на каждом k-ом сплайне в системе координат (СК) Oxyz, связанной с основанием ММ 2; P и Р - соответственно, начальная и конечная точки k-го сплайна; P и P - вспомогательные (направляющие) точки, определяющие конкретный вид кривой k-го сплайна; P и P - соответственно, конечная и начальная точки k-1-го и k+1-го сплайнов; С - набор коэффициентов при различных степенях параметра τ в параметрических полиномах, описывающих k-ый сплайн; r=[х у z] - вектор, определяющий желаемой положение рабочего органа 1 ММ 2 в СК Oxyz;  - вектор желаемой скорости движения рабочего органа 1 в СК Oxyz; ν - значение желаемой скорости движения рабочего органа 1; q,- значения обобщенных координат, скоростей и ускорений формируемые соответствующими датчиками, установленными в СЭП 6; - желаемые значения q; p/m - сигнал, определяющий режим формирования желаемой скорости ν (разгон или торможение).
ММ 2 (фиг. 1) имеет три переносные поворотные степени подвижности с кинематической схемой типа PUMA. Его ориентирующие степени подвижности, расположенные вблизи рабочего органа 1, не показаны.
В состав СЭП 6 входят электродвигатели постоянного тока с постоянными магнитами, имеющие малые величины индуктивностей якорных цепей, а также вязкого и сухого трения.
Заявленный способ реализуется следующим образом.
k,0
k,1
k,2
k,3
До запуска технологического процесса ММ 2 неподвижен, а в БФТ 4 сохранены координаты точек P, Р Р, Р k-ых (как минимум для k=0) сплайнов Безье третьего порядка в СК Oxyz. С учетом этих координат по известному методу (Роджерс Д., Адамс Дж. Математические основы машинной графики // М.: Мир, 2001. 604 с. ISBN 5-03-002143-4) с использованием БФТ 4 рассчитывают набор коэффициентов С при различных степенях параметра т в параметрических полиномах третьей степени, описывающих эти сплайны.
T
*
После запуска технологического процесса посредством БФТ 4 начинают формировать вектор r=[xyz] (фиг.1), определяющий желаемое положение рабочего органа 1 ММ 2 в СК Oxyz. Движение конца вектора r по k-му сплайну осуществляется с желаемой скоростью ν, рассчитываемой с помощью БФС 7, с использованием параметрических сплайнов по методу, описанному в работе (Filaretov V.F., Gubankov A.S., Gornostaev I.V. The Formation of Motion Laws for Mechatronics Objects Along the Paths with the Desired Speed // Proc. of International Conference on Computer, Control, Informatics and Its Applications. Jakarta, Indonesia. 2016. P. 93-96.). Использование указанных сплайнов позволяет обеспечить задание желаемого движения рабочего органа 1 даже по сложным замкнутым пространственным траекториям.
k,0
k,3
k,1
k,2
*
Это движение начинается в точке P, когда τ=0, и заканчивается в точке P, когда τ=1, а точки P и Р определяют вид кривой сплайна. При переходе желаемого положения рабочего органа 1 на k+1-ый сплайн параметр τ обнуляется, но векторы  в конце k-го сплайна, где τ=1, и в начале следующего k+1-го сплайна, где τ=0, остаются равными, хотя при переходе от k-го к k+1-му сплайну происходит скачкообразное изменение параметра τ с 1 на 0. Это обеспечивается за счет раздельного расчета  на конце k-го и в начале k+1-го сплайнов по уже полученным аналитическим выражениям, в которых точка сопряжения не является точкой разрыва. Это позволяет обеспечить непрерывность первых и вторых производных координат х, у, z по параметру τ на концах каждого сплайна, то есть гарантирует совпадение касательных в точках сопряжения всех сплайнов и равенство радиусов их кривизны в этих точках и необходимо для плавного перемещения желаемого положения рабочего органа 1 по траекториям 3 с заданной желаемой скоростью ν
T
i
С учетом сформированных посредством БФТ 4 параметров вектора r=[xyz] с использованием ВУ 5 по известным аналитическим выражениям (Борисов О.И., Громов B.C., Пыркин А.А. Методы управления робототехническими приложениями: учебное пособие // СПб.: Университет ИТМО, 2016. С. 23-29.) рассчитывают желаемые значения всех обобщенных координат  ММ 2, поступающие на соответствующие входы СЭП 6. В момент запуска технологического процесса обеспечивается равенство всех желаемых значений  их текущим реальным значениям q, поступающим с выходов всех датчиков положения, установленных на выходных валах редукторов соответствующих СЭП 6. Это гарантирует плавность начала движения рабочего органа 1.
*
Для исключения входа любого СЭП 6 ММ 2 системы (фиг. 2) в насыщение, которое может привести к резкому отклонению рабочего органа 1 от траекторий 3, и одновременного поддержания его предельно высокой скорости посредством БФТ 4 обеспечивают режим перемещения желаемого положения рабочего органа 1 по траектории 3, при котором хотя бы один из СЭП 6 всегда находится в преднасыщенном состоянии, то есть работает на пределе своих функциональных возможностей. Для обеспечения этого условия после начала технологического процесса задание текущих значений параметров вектора r обеспечивают с учетом непрерывно изменяемого значения ν формируемого посредством БФС 7 по следующему правилу.
i
i
С учетом текущих значений номера сплайна к, параметра т и набора коэффициентов С с использованием БФС 7 вначале рассчитывают векторы а и  содержащие информацию о направлениях векторов желаемой скорости  и ускорения , а также векторы g первых и матрицы G вторых частных производных  содержащие информацию о кинематических соотношениях, связывающих желаемые скорость  и ускорение  с обобщенными скоростями  и ускорениями
2
3
Затем посредством БФС 7 с учетом сигналов, формируемых на выходах датчиков положения q, q, скорости  и ускорения а также значений массогабаритных параметров звеньев ММ 2 рассчитывают компоненты текущих моментов, возникающих на выходных валах приводов всех сочленений при произвольном движении ММ 2 (Филаретов В.Ф., Зуев А.В., Губанков А.С.Управление манипуляторами при выполнении различных технологических операций // М.: Наука, 2018. С. 15-22.)
i
i
внi
где H - компонента, характеризующая инерционные свойства соответствующей части ММ 2; h - составляющая момента, пропорциональная; M - моментное воздействие на i-ое сочленение ММ 2, учитывающее гравитационные силы и взаимовлияния его степеней подвижности в процессе движения.
i
i
i
*
После этого с учетом уже рассчитанных значений частных производных, входящих в состав g и  векторов а и  компонентов Н, h и  текущего значения желаемой скорости ν, а также параметров СЭП 6 для каждой i-ой степени подвижности ММ 2 с использованием БФС 7 формируют уравнение движения рабочего органа 1 по траектории 3
Где
i
pi
Mi
ωi
yi
i
R - активные сопротивления якорных цепей электродвигателей; суммарные моменты инерции их роторов и приведенных к ним моментов инерции вращающихся частей редукторов; i - передаточные числа редукторов; K и K - соответственно, коэффициенты крутящих моментов и противо-ЭДС; K - коэффициенты усиления усилителей мощности электродвигателей; U - предельно допустимые (максимальные и минимальные) значения входных сигналов (напряжений), при которых i-ый СЭП 6 будет находиться в преднасыщенном состоянии.
i
Затем для всех СЭП 6 и всех предельных значений входных сигналов U по уравнению (1) рассчитывают предельное значение , при котором ни один СЭП 6 не войдет в насыщение, но хотя бы один из них будет находиться в преднасыщенном состоянии. Причем, если задаваемый посредством БФТ 4 сигнал р/m равен 1, то выполняется разгон ММ 2, а если -1, то - торможение.
*
*
В результате при изменении скорости ν с ускорением  будет поддерживаться работа хотя бы одного СЭП 6 в преднасыщенном состоянии, то есть будет обеспечиваться предельно высокая скорость движения ММ 2 при сохранении высокой точности перемещения рабочего органа 1. В отличие от использованного в прототипе решения, такой способ расчета желаемой скорости ν может быть использован даже в тех случаях, когда желаемая траектория движения рабочего органа ММ заранее неизвестна и задается непосредственно в процессе самого движения.
Общая вычислительная сложность описанного способа (включая работу БФТ 4, ВУ 5 и БФС 7) составляет 412 операций сложения, 359 операций умножения (деления) и 145 операций вычисления стандартных функций. Выполнение всех этих операций без проблем можно обеспечивать в реальном масштабе времени с помощью типового контроллера. Поэтому реализация предложенного способа управления рабочим органом ММ не вызывает принципиальных затруднений, поскольку в способе используются только известные аналитические выражения, а также типовые системы и устройства.