устройство контурного управления манипуляционным роботом

Формула изобретения

1. Устройство контурного управления манипуляционным роботом, содержащее блок механической системы, входы которого связаны с выходами блока исполнительных механизмов, а первые и вторые выходы соответственно с первыми и вторыми входами блока датчиков; планировщик траекторий в пространстве внешних координат, отличающееся тем, что в него введены блок вычисления нелинейных элементов, блок вычисления функциональной матрицы коэффициентов управлений, блок вычисления решения прямой задачи кинематики, блок вычисления якобиана решения прямой задачи кинематики, блок вычисления производной по времени от якобиана решения прямой задачи кинематики, блоки вычисления якобиана желаемых траекторий, блок вычисления производной по времени от якобиана желаемых траекторий, блок вычисления векторного коэффициента, блоки обращения матриц, блок транспонирования, блоки перемножения, блоки суммирования, причем первые выходы блока датчиков, соответствующие обобщенным координатам, связаны с первыми входами планировщика траекторий в пространстве внешних координат, со вторыми входами блока вычисления нелинейных элементов, первыми входами блока вычисления функциональной матрицы коэффициентов управлений, первыми входами блока вычисления якобиана решения прямой задачи кинематики, первыми входами блока вычисления производной по времени от якобиана решения прямой задачи кинематики, первыми входами блока вычисления решения прямой задачи кинематики, а вторые выходы блока датчиков, соответствующие обобщенным скоростям, соединены со вторыми входами планировщика, с первыми входами блока вычисления нелинейных элементов, вторыми входами второго блока вычисления якобиана желаемых траекторий, со вторыми входами блока вычисления производной по времени от якобиана желаемых траекторий, вторыми входами блока вычисления производной по времени от якобиана решения прямой задачи кинематики, третьи входы которого, вместе со вторыми входами блока вычисления решения прямой задачи кинематики, вторыми входами блока вычисления якобиана решения прямой задачи кинематики, вторыми входами блока вычисления функциональной матрицы коэффициентов управлений, третьими входами блока вычисления нелинейных элементов соединены с седьмыми выходами планировщика, задающими массогабаритные параметры робота, а первые выходы планировщика траекторий в пространстве внешних координат соединены со вторыми входами восьмого блока перемножения, первыми входами пятого блока перемножения, первыми входами шестого блока перемножения и первыми входами четырнадцатого блока перемножения, вторые входы которого подключены к шестым выходам планировщика, вторые выходы которого соединены с третьими входами восьмого блока перемножения, первыми входами седьмого блока перемножения и вторыми входами пятого блока перемножения, третьи выходы планировщика, задающие коэффициенты квадратичных форм при квадратах координат, соединены с первыми входами первого блока вычисления якобиана желаемых траекторий, первыми входами блока вычисления производной по времени от якобиана желаемой траектории и первыми выходами первого блока перемножения, четвертые выходы планировщика, задающие коэффициенты квадратичных форм при координатах, подключены ко вторым входам первого блока вычисления якобиана желаемых траекторий и первым входам второго блока перемножения, вторые входы которого вместе с третьими входами первого блока перемножения и входами блока транспонирования соединены с выходами блока вычисления решения прямой задачи кинематики, а выходы блока транспонирования соединены с третьими входами первого блока вычисления якобиана желаемых траекторий и вторыми входами первого блока перемножения, выходы которого подключены к первым входам первого блока суммирования, ко вторым входам которого подключены выходы второго блока перемножения, а к третьим входам - пятые выходы планировщика траекторий, задающие свободные константы квадратичных форм; выходы четырнадцатого блока перемножения соединены со вторыми входами блока вычисления векторного коэффициента, первые входы которого соединены с выходами первого блока суммирования, а его выходы - со вторыми входами пятого блока суммирования; выходы блока вычисления производной по времени от якобиана решения прямой задачи кинематики соединены со вторыми входами четвертого блока перемножения, первые входы которого вместе со вторыми входами шестого блока перемножения и четвертыми входами восьмого блока перемножения подключены к выходам второго блока вычисления якобиана желаемых траекторий, первые входы которого вместе со вторыми входами седьмого блока перемножения соединены с выходами первого блока вычисления якобиана желаемых траекторий, третьи входы второго блока вычисления якобиана траекторий вместе с пятыми входами восьмого блока перемножения, вторыми входами девятого блока перемножения, вторыми входами третьего блока перемножения и третьими входами блока вычисления производной по времени от якобиана желаемых траекторий подключены к выходам блока вычисления якобиана решения прямой задачи кинематики, а выходы блока вычисления производной по времени якобиана желаемых траекторий соединены с первыми входами третьего блока перемножения, выходы которого подключены ко вторым входам второго блока суммирования, первые входы которого соединены с выходами четвертого блока перемножения, а выходы - со вторыми входами десятого блока перемножения, первые входы которого связаны с выходами пятого блока перемножения, а выходы - со вторыми входами четвертого блока суммирования, первые входы которого подключены к выходам девятого блока перемножения, а выходы - к первым входам одиннадцатого блока перемножения, на вторые входы которого подключены вторые выходы блока датчиков, а его выходы являются первыми входами пятого блока суммирования; выходы седьмого блока перемножения соединены с первыми входами третьего блока суммирования, вторые входы которого соединены с выходами шестого блока перемножения, а выходы являются первыми входами девятого блока перемножения, выходы блока вычисления функциональной матрицы коэффициентов управлений соединены со входами первого блока обращения матриц и первыми входами восьмого блока перемножения, выход которого является входом второго блока обращения матриц, а выходы второго блока обращения матриц - первыми входами тринадцатого блока перемножения, вторые входы которого соединены с выходами пятого блока суммирования, а выходы - со вторыми инверсными входами шестого блока суммирования, выходы которого подключены ко входам блока исполнительных механизмов, а первые инверсные входы - к выходам двенадцатого блока перемножения, первые входы которого связаны с выходами блока вычисления нелинейных элементов, а вторые входы - с выходами первого блока обращения матриц.

2. Устройство по п. 1, отличающееся тем, что первый блок вычисления якобиана желаемых траекторий содержит пятнадцатый блок перемножения, седьмой блок суммирования, первый регистр, первый блок умножения на два, входы которого являются третьими входами первого блока вычисления якобиана желаемых траекторий, а выходы - вторыми входами пятнадцатого блока перемножения, первые входы которого являются первыми входами первого блока вычисления якобиана желаемых траекторий, вторые входы которого являются первыми входами седьмого блока суммирования, вторые входы которого соединены с выходами пятнадцатого блока перемножения, а выходы седьмого блока суммирования являются первыми входами первого регистра, вторые входы которого заземлены, а выходы являются выходами первого блока вычисления якобиана желаемых траекторий.

3. Устройство по п.1, отличающееся тем, что второй блок вычисления якобиана желаемых траекторий содержит второй регистр, второй блок транспонирования, шестнадцатый блок перемножения и восьмой блок суммирования, первые входы которого связаны с выходами первого регистра и соединены с первыми входами второго блока вычисления якобиана желаемых траекторий, вторые входы которого связаны с первыми входами шестнадцатого блока перемножения, а третьи входы - со вторыми входами шестнадцатого блока перемножения, выходы которого соединены со входами второго блока транспонирования, выходы которого являются вторыми входами второго регистра, первые входы которого заземлены, а выходы присоединены ко вторым входам восьмого блока суммирования, выходы которого являются выходами второго блока вычисления якобиана желаемых траекторий.

4. Устройство по п.1, отличающееся тем, что блок вычисления производной по времени якобиана желаемых траекторий содержит семнадцатый и восемнадцатый блоки перемножения, третий блок транспонирования, второй блок умножения на два и третий регистр, выходы которого являются выходами блока вычисления производной по времени якобиана желаемых траекторий, первые входы которого соединены с первыми входами восемнадцатого блока перемножения, а вторые и третьи входы - соответственно с первыми и вторыми входами семнадцатого блока перемножения, выходы которого соединены со входами третьего блока транспортирования, выходы которого, посредством второго блока умножения на два подключены ко вторым входам восемнадцатого блока перемножения, входы которого являются первыми входами третьего регистра, вторые входы которого заземлены.

5. Устройство по п.1, отличающееся тем, что блок вычисления векторного коэффициента содержит четвертый регистр и девятый блок суммирования, выходы которого являются выходами блока вычисления векторного коэффициента, вторые входы которого соединены со вторыми входами девятого блока суммирования, а первые входы - с первыми входами четвертого регистра, вторые входы которого заземлены, а выходы являются первыми входами девятого блока суммирования.

Описание изобретения к патенту

Изобретение относится к робототехнике и может быть использовано при разработке систем управления манипуляционными и мобильными роботами, обеспечивающих решение траекторных задач при предъявлении дополнительных требований к контурной (траекторной) скорости.

Общая процедура синтеза законов управления манипуляционными роботами, как известно [Вукобратович M., Стокич Д., Кирчански H. Неадаптивное и адаптивное управление манипуляционными роботами. - M.: Мир, 1989.- 376 с., с. 5-53] , предполагает наличие следующих этапов: планирование желаемой траектории S(t) в пространстве внешних координат, что определяет необходимость наличия блока планировщика траекторий в пространстве внешних координат; решение обратной задачи кинематики и наличие соответствующего блока, обеспечивающего переход из пространства R^m внешних S в пространство Rⁿ обобщенных координат q; формирование номинальных траекторий движения q(t) посредством использования интерполирующих полиномов [Шахинпур M. Курс робототехники.- M. : Мир.- 527 с., с. 217-235]; синтез регуляторов, обеспечивающих заданный характер движения относительно номинальных траекторий. Под планировщиком траекторий в пространстве внешних координат понимается совокупность блоков и модулей, формирующих желаемые траектории движения робота, т.e. в состав блока планировщика траекторий могут быть включены ЭВМ верхнего уровня, пульт оператора, чувствительные устройства и т.п. [Управление роботами от ЭВМ. Под ред. Юревича Е.И.- Л.: Энергия, 1980.- 264 с., с. 78].

Известны алгоритмы управления роботами и соответствующая им структурная схема [Тимофеев A.B. Управление роботами.- Л.: из-во Ленинградского университета, 1986.- 240 с., с. 83-86], которая предполагает наличие блока планировщика траекторий и содержит два вычислительных блока, два блока суммирования и интегратор.

Устройство управления, реализуемое предоставленной структурой, позволяет организовать движение вдоль прямой линии в пространстве внешних координат, соединяющей начальное и конечное положение cхвата, с постоянной скоростью и не требует решения обратной задачи кинематики.

Признаки аналога, общие с заявляемым техническим решением, следующие: блок исполнительных механизмов, блок планировщика траекторий в пространстве внешних координат, блок механической системы, блок датчиков, вычислительные блоки, блоки перемножения и блоки суммирования.

Причины, препятствующие достижению требуемого технического результата, заключаются в следующем: полученные структурные решения не позволяют организовать движение по широкому спектру траекторий или предполагают аппроксимацию сложных траекторий отрезками прямых линий и наличия соответствующего блока аппроксимации, т.е., с одной стороны, движение только по прямым линиям сужает функциональные возможности робота, а введение аппроксиматора в структуру системы управления, с другой стороны, вносит дополнительную погрешность планирования, и, следовательно, отработки заданной траектории.

Известно устройство, основанное на решении обратной задачи кинематики [Вукобратович M., Cтокич Д., Кирчански H. Неадаптивное и адаптивное управление манипуляционными роботами.- М.: Мир, 1989.- 376 с., с. 72-78], структура которого предполагает наличие блока планировщика траекторий в пространстве внешних координат, блока решения обратной задачи кинематики, блока интерполяции, двух дифференциаторов и четырех вычислительных блоков и четырех блоков суммирования.

Структурно-алгоритмические решения, реализованные в данном устройстве, обеспечивают учет нелинейных свойств роботов и важное качественное свойство асимптотической устойчивости номинальной траектории.

Признаки аналога, общие с заявляемым техническим решением, следующие: блок исполнительных механизмов, блок планировщика траекторий в пространстве внешних координат, блок механической системы, блок датчиков, вычислительные блоки, блоки перемножения и блоки суммирования.

Причины, препятствующие достижению требуемого технического результата, заключаются в следующем: устройство также требуeт решения обратной задачи кинематики, наличия аппроксимирующих устройств и интерполяторов заданных траекторий в пространстве внешних координат, что вносит дополнительную погрешность в планирование, и, следовательно, в отработку траекторий.

Известно устройство траекторного управления [Крутько П.Д. Управление исполнительными системами роботов.- М.: Наука, гл. ред. физ.-мат. лит. 1991. - 336 с. , с. 308-313], базирующееся на полной нелинейной модели динамики робота и предполагающее наличие следующих блоков: блока планировщика траекторий в пространстве внешних координат, реализующего планирование движений робота в пространстве внешних (рабочих, базовых) координат, блока решения обратной задачи кинематики, блока аппроксимации, блока интерполяции, блока регуляторов, блока исполнительных механизмов, блока датчиков, блока механической системы робота.

Использование полной динамической модели робота с учетом нелинейных составляющих при реализации прототипа позволяет обеспечить асимптотическую устойчивость в целом планируемых траекторий, что в свою очередь расширяет область фазового пространства робота, в котором обеспечиваются требуемые показатели качества. Наличие блока планировщика траекторий в пространстве внешних координат, блоков датчиков, исполнительных механизмов и механической системы является обязательными элементами любой замкнутой робототехнической системы.

Признаки аналога, общие с заявляемым техническим решением, следующие: блок исполнительных механизмов, блок планировщика траекторий в пространстве внешних координат, блок механической системы, блок датчиков, вычислительные блоки, блоки перемножения и блоки суммирования.

Причины, препятствующие достижению требуемого технического результата, заключаются в следующем: прежде всего это относится к наличию блока решения обратной задачи кинематики (ОЗК), что не всегда возможно в аналитической форме и связано со значительными трудностями [Тимофеев А.В. Управление роботами. - Л.: из-во ЛГУ, 1986.- 240 с., с. 77]. Кроме того, наличие интерполяторов и аппроксиматоров вносит дополнительную погрешность в формируемую траекторию программного движения. Следует отметить, что аппроксимация планируемых траекторий осуществляется только отрезками прямых и дугами окружностей, что снижает функциональные возможности робота.

Задачей изобретения является повышение точности планирования, и, следовательно, отработки траекторий, а также расширение функциональных возможностей робота.

Технический результат, достигаемый при осуществлении предлагаемого устройства, заключается в том, что предлагаемые алгоритмы управления не требуют решения обратной задачи кинематики, которое заменено решением обратной задачи динамики, не требуется наличия аппроксимирующих устройств и интерполяторов в структуре системы управления роботом, что позволяет исключить соответствующие составляющие погрешности задания, связанные с приближенностью расчетов [Бурдаков С.Ф., Дьяченко В.А., Тимофеев А.Н. Проектирование манипуляторов промышленных роботов и роботизированных комплексов.- М.: Высшая школа, 1986. - 246 с., с. 60], и, следовательно, снизить погрешность отработки планируемых траекторий движения, т.е. повысить точность движения робота, а движение робота во всем классе квадратичных форм расширяет его функциональные возможности.

Технический результат достигается тем, что в устройство, содержащее блок механической системы, блок исполнительных механизмов, блок датчиков, планировщик траекторий, введены блок вычисления нелинейных элементов, блок вычисления функциональной матрицы коэффициентов управлений, блок вычисления решения прямой задачи кинематики, блок вычисления якобиана решения прямой задачи кинематики, блок вычисления производной по времени от якобиана решения прямой задачи кинематики, два блока вычисления якобиана желаемых траекторий, блок вычисления векторного коэффициента, блок вычисления производной по времени от якобиана желаемых траекторий, два блока обращения матриц, блок транспонирования, четырнадцать блоков перемножения, шесть блоков суммирования.

Технический результат достигается также тем, что блоки вычисления якобиана желаемых траекторий содержат два блока перемножения, два блока суммирования, два регистра, блок транспонирования, блок умножения на два; блок вычисления производной по времени от якобиана желаемых траекторий содержит два блока перемножения, блок транспонирования, блок умножения на два, регистр, а блок вычисления векторного коэффициента содержит регистр и блок суммирования.

Теоретическое доказательство наличия причинно-следственной связи между заявляемыми признаками и достигаемым техническим результатом приведено ниже.

Пусть уравнения динамики робота представляются в следующей форме [Шахинпур M. Курс робототехники.- М.: Мир, 1990.- 527 с., с. 327]:



где D - невырожденная (nxn)-матрица инерции;

H - (nх1) - вектор кориолисовых и центробежных сил (моментов);

G - (nх1) - вектор гравитационных сил (моментов);

- (nx1) - векторы соответственно обобщенных координат, скоростей и ускорений;

- (nх1) - вектор управляющих сил (моментов);

здесь n - число степеней подвижности робота, n 6.

Пусть матрица D, векторы H и G заданы в аналитической форме и известно решение прямой задачи кинематики [Вукобратович M., Стокич Д., Кирчански H. Неадаптивное и адаптивное управление манипуляционными роботами. М.: Мир, 1989.- 376 с., с. 50]

Y = Ф(q), (2)

где Y - (mx1) - вектор внешних (рабочих) координат, а Ф : Rⁿ ---> R^m есть решение прямой задачи кинематики.

Предположим, что робот является кинематически безызбыточным, т.е. m = n. Математическая модель робота (1) с учетом выражения (2) может быть приведена к форме Коши





Y = Ф(x_2i-1) (3-с)



где F - (nx1)-вектор нелинейных элементов;

B - невырожденная (nхn)-матрица коэффициентов управления.

Предположим, что при решении задач контурного управления, всему множеству желаемых траекторий в пространстве внешних координат R^m можно поставить в соответствие линию пересечения квадратичных форм



где _т - вектор-функция своих аргументов, dim_т = (m-1);

M_j1 - диагональные матрицы задаваемых коэффициентов квадратичных форм при квадратах координат, dimM_j1x = (mхm);

M_j2, M_j3 - векторы задаваемых коэффициентов квадратичных форм при координатах и констант квадратичных форм, соответственно dimM_j2 = (1хm), dimM_j3 = (1х1).

Сформируем вектор-функцию _т, отражающую желаемые требования к траектории движения рабочего органа робота (схват, лазер и т.д.)



где 0 - (1х1)-вектор нулевых элементов.

Пусть скорость движения рабочего органа робота вдоль траектории (4) определяется выражением



M - диагональная (mxm)-матрица постоянных коэффициентов, для контурной задачи - единичная матрица;

V_k - задаваемое значение контурной скорости;

Т - символ транспонирования,

а относительно траектории (4) выражением



Или, введя обозначение якобиана , выражение (7) представим в следующем виде:



Тогда вектор-функция _c, отражающая желаемые требования к скорости движения рабочего органа робота, может быть представлена в виде



Очевидно, что линейная комбинация многообразий (5) и (9)



где A - диагональная определенно положительная (nхn)-матрица задаваемых коэффициентов, являясь линией пересечения многообразий _т и _c, задает желаемую траекторию движения робота в фазовом пространстве внешних координат R^2m: R^m х R^m.

Требование асимптотической устойчивости в целом траектории (10) может быть задано дифференциальным уравнением вида [Колесников A.A. Синергетическая теория управления. М. - Таганрог, 1994.- 344 с., с. 66]



где Т - диагональная определенно положительная матрица задаваемых коэффициентов, dim Т = (nхn).

Подставив выражения (6), (9), (10) в уравнение (11), получим



где 0₁ - (1xn) - вектор нулевых элементов,

Введем матрицу



где t_ii - элементы матрицы T₁.

Тогда выражение (12-а) после несложных преобразований может быть представлено в виде





здесь 0₂ - (n-1)x1 - вектор нулевых элементов или



Введем обозначение якобиана решения прямой задачи кинематики (2) и с учетом замены (3-d) получим



Тогда справедливы следующие соотношения:



где - производная по времени от якобиана решения прямой задачи кинематики.

Кроме того, учитывая выражение (4), имеем



где 0₃ - (1xn)-вектор нулевых элементов,

или









Выражение (15-a) описывает алгоритм вычисления желаемой траектории движения робота в пространстве Rⁿ, выражение (15-c) - алгоритм вычисления в Rⁿ производной по времени от якобиана желаемых траекторий, а выражения (15-b) и (15-c) - алгоритмы вычисления в Rⁿ якобианов желаемых траекторий.

С учетом соотношений (2), (4), (13) и (15) представим уравнение (13) в фазовом пространстве обобщенных координат



где .

Подставив уравнение объекта (3) в выражение (16) и решив его относительно вектора управляющих воздействий , получим следующий алгоритм управления [Pshikhopov V.H. Analytical Synthesis of the Nonlinear Contour Controllers for Manipulating Robots. In Abstracts Book of Int. Confer. "Mathematical Theory of Networks and Systems", July 6-10, 1998, Padova, Italy, TM6]:

= -[k₁]^-1[k₂X_2i+k₃]-B^-1F, (17)





где k₁, k₂ - матричные коэффициенты;

k₃ - векторный коэффициент.

В дальнейшем будем полагать, что робот не находится в вырожденных конфигурациях, т. е. , и что выполняется условие . Можно показать, что последнее условие выполняется на планируемых фазовых траекториях и может нарушаться лишь при определенных начальных условиях [Бойчук Л.M. Синтез координирующих систем автоматического управления.- М.: Энергоатомиздат, 1991. - 160 с., с. 113]. В случае попадания рабочего органа МР в указанные точки, где вырождается управление (17), для вывода рабочего органа робота из этих точек возможно использование известных алгоритмов, не имеющих особенностей в этих точках [Крутько П.Д. Управление исполнительными системами роботов.- М.: Наука, гл. ред. физ.-мат. лит. 1991. - 336 с., с. 278].

Учет нелинейных элементов моделей динамики робота (1), (3) при синтезе алгоритма управления (15), (17) и структуры управления прототипа позволяет расширить область фазового пространства робота, т.е. обеспечить точное отслеживание быстрых траекторий [Вукобратович M., Стокич Д., Kирчански H. Неадаптивное и адаптивное управление манипуляционными роботами.- М.: Мир, 1989. - 376 с., с. 59-60], что является немаловажным фактом при повышении требований к производительности робота. Замыкание робота управлением (15), (17) в силу решения дифференциального уравнения (11) обеспечивает асимптотическую устойчивость в целом планируемых траекторий (10). Аналогичным свойством обладают и объекты, замкнутые устройством управления прототипа.

Полученный алгоритм управления (15), (17) не требует решения обратной задачи кинематики (ОЗК), которое сведено к решению обратной задачи динамики, т.е. к решению алгебраической (относительно ) системы уравнений (11), (16). Исключение блока решения ОЗК упрощает структуру системы управления роботом.

Кроме того, в силу аналитичности задания планируемых траекторий (10), (2), (14), (15) движения робота в фазовом пространстве его обобщенных координат в случае движения его по траекториям, задаваемых квадратичными формами, не требуется наличия аппроксимирующих устройств, а также интерполяторов в структуре системы управления роботом.

Исключение аппроксиматоров и интерполяторов из структуры системы управления роботом позволяет исключить соответствующие составляющие погрешности задания, связанные с приближенностью расчетов [Бурдаков С.Ф., Дьяченко В.А., Тимофеев А. Н. Проектирование манипуляторов промышленных роботов и роботизированных комплексов.- М.: Высшая школа, 1986.- 246 с., с. 60], и, следовательно, отработки планируемых траекторий движения, т.е. повысить точность движения робота.

Следует отметить тот факт, что изменение только лишь коэффициентов квадратичных форм (4), (6) позволяeт изменить желаемые траектории движения робота в заданном классе траекторий и при M = E, где E - единичная матрица, с заданной контурной скоростью. В частном случае при M 0, V_k = 0, M_j2 0, алгоритм контурного управления трансформируется в известный алгоритм позиционного управления в пространстве базовых координат [Крутько П.Д. Управление исполнительными системами роботов.- М.: Наука, 1991. - 336 с., с. 278]. Движение робота во всем классе квадратичных форм расширяет его функциональные возможности по сравнению с прототипом. Причем время движения рабочего органа робота к желаемым фазовым траекториям, т.е. время отработки отклонения от желаемой траектории, определяется величиной элементов матриц T₁ и A и, с одной стороны, определяется энергетическими возможностями робота, с другой - требованиями к технологической задачe [Колесников A. A. Синергетическая теория управления. М. - Таганрог, 1994.- 344 с., с. 99].

Изобретение поясняется чертежами, где на фиг. 1 (1.1, 1.2) представлена блок-схема робота, замкнутого синтезированным устройством контурного управления; на фиг. 2 и 3 - структурные схемы блоков вычисления якобиана планируемых траекторий и его производной; на фиг. 4 - структурная схема блока вычисления векторного коэффициента k₃; на фиг. 5 - возможный вариант реализации блока транспонирования матриц; на фиг. 6 - блок-схема алгоритма функционирования устройства; на фиг. 7 - диаграмма работы устройства.

Устройство контурного управления роботом, представленное на фиг. 1, содержит планировщик траектории в пространстве внешних координат (ПТ) 1, седьмые выходы которого подключены на третьи входы блока 2 вычисления нелинейных элементов (БНЭ), вторые входы блока 3 вычисления функциональной матрицы коэффициентов управления (БКУ), вторые входы блока 4 вычисления якобиана решения прямой задачи кинематики (ЯЗК), третьи входы блока 5 вычисления производной по времени от якобиана решения прямой задачи кинематики (ПЯК) и вторые входы блока 6 вычисления решения прямой задачи кинематики (ПЗК). Третьи nх(n-1) выходов планировщика траекторий в пространстве внешних координат 1, задающие коэффициенты квадратичных форм при квадратах координат, являются первыми входами блока 7 - первого блока вычисления якобиана желаемых траекторий (ЯЖТ) и первыми входами блока 8 вычисления производной во времени якобиана планируемых траектории (ПЯТ), а m выходов блока 6 вычисления решения прямой задачи кинематики являются входами блока транспонирования 9 (ТМ), третьими входами блока перемножения 10 (П), первые входы которого подключены к третьим выходам планировщика траекторий в пространстве внешних координат 1, и вторыми входами блока перемножения 11 (П). Вторые входы блока 5 вычисления производной по времени якобиана решения прямой задачи кинематики (ПЯК) соединены со вторыми входами блока 8 вычисления производной по времени от якобиана желаемых траекторий траектории, вторыми входами блока 12, который является вторым блоком вычисления якобиана желаемых траекторий (ЯЖТ) и первыми входами блока 2 вычисления нелинейных элементов (БНЭ), а вторые входы блока перемножения (П) 13 связаны с выходами блока 7, который является первым блоком вычисления якобиана желаемых траекторий (ЯЖТ). (m-1) выходов блока перемножения (П) 11 являются вторыми входами блока суммирования (С) 14. m выходов блока транспонирования (ТМ) 9 являются третьими входами блока 7 вычисления якобиана желаемых траекторий и вторыми входами блока перемножения (П) 10, (m-1) выходов которого являются первыми входами блока суммирования (С) 14.

(nхn) выходов блока 3 вычисления матрицы коэффициентов управления (БКУ) связаны со входами блока обращения матриц (ОМ) 15 и первыми входами блока перемножения (П) 16, четвертые входы которого являются (nхn) выходами блока ЯЖТ 12.

Первые n выходов планировщика траекторий в пространстве внешних координат 1 связаны со вторыми n входами блока перемножения (П) 16, с первыми n входами блоков перемножения (П) 17 и 18. Вторые n выходов планировщика траекторий в пространстве внешних координат 1 подключены к третьим входам блока перемножениями 16, первым входам блока перемножения 13, вторым входам блока перемножения 18.

(nхn) выходов блока 5 вычисления производной по времени от якобиана решения прямой задачи кинематики являются вторыми входами блока перемножения (П) 19, первыми входами которого являются выходы блока ЯЖТ 12, который является вторым блоком вычисления якобиана желаемых траекторий и своими первыми nхn входами связан с выходами блока 7 - первого блока вычисления якобиана желаемых траекторий, nхn выходов блока 4 вычисления якобиана решения прямой задачи кинематики являются третьими входами блока 12, пятыми входами блока перемножения 16 и третьими входами блока 8 вычисления производной по времени от якобиана желаемых траекторий выходы которого являются первыми входами блока перемножения (П) 20, вторые входы которого связаны с выходами блока 4 вычисления якобиана решения прямой задачи кинематики.

Первые m входов блока 21 вычисления векторного коэффициента (ВК) являются выходами блока суммирования 14, n выходов блока 2 вычисления вектора нелинейных элементов связаны с первыми входами блока перемножения (П) 22, а (nхn) выходов блока перемножения 16 - со входами блока 23 обращения матрицы (ОМ). Первые входы блока суммирования (С) 24 соединены с (nхn) выходами блока перемножения 13, вторые входы - с (nхn) выходами блока перемножения 17, вторые входы которого связаны с выходами второго блока вычисления якобиана желаемых траекторий 12.

Выходы блоков перемножения 19 и 20 соответственно связаны с первыми и вторыми входами блока суммирования (С) 25, а (nхn) первые входы блока перемножения (П) 26 являются выходами блока 23 обращения матриц. Выходы блоков перемножения (П) 27 и 28 соответственно связаны с первыми и вторыми входами блока суммирования (С) 29, nхn выходов которого связаны с первыми входами блока перемножения (П) 30, а выходы блока суммирования 25 являются вторыми входами блока перемножения 28, первые входы которого связаны с n выходами блока перемножения 18. Первые входы блока перемножения 27 связаны с nхn выходами блока суммирования 24, вторые входы - с nхn выходами блока 4 вычисления якобиана решения прямой задачи кинематики, а n выходов блока перемножения 26 связаны со вторыми инверсными входами блока суммирования (С) 31, первые инверсные входы которого подключены к выходам блока перемножения 22, вторые входы которого связаны с выходами блока обращения матрицы 15.

Первые входы блока суммирования (С) 32 связаны с n выходами блока перемножения 30, а вторые входы - с n выходами блока 21 вычисления векторного коэффициента, n выходов блока суммирования 32 связаны со вторыми входами блока перемножения 26, а n выходов блока суммирования 31 являются входами блока исполнительных механизмов (ИМ) 33, а n выходов блока 33 - входами блока 34 механической системы (МС) робота, который своими первыми и вторыми выходами связан соответственно с первыми и вторыми входами блока датчиков 35 (D), вторые выходы которого связаны со вторыми входами блока перемножения 30.

Четвертые nх(n-1) выходы планировщика траекторий в пространстве внешних координат 1, задающие коэффициенты квадратичных форм при координатах, являются вторыми входами блока 7 - второго блока вычисления якобиана планируемых траекторий и первыми входами блока перемножения 11. Пятые (n-1) выходов планировщика траекторий в пространстве внешних координат 1, задающие свободные константы квадратичных форм, являются третьими входами блока суммирования 14, шестые n выходов - вторыми входами блока перемножения (П) 36, первые входы которого связаны с первыми выходами планировщика 1, а его m выходов - со вторыми m входами блока вычисления векторного коэффициента 21.

Первые n выходов блока датчиков 35, соответствующие обобщенным координатам, являются первыми входами планировщика траекторий в пространстве внешних координат 1 и первыми входами блока 6 решения прямой задачи кинематики, первыми входами блока 5 вычисления производной по времени от якобиана решения прямой задачи кинематики, первыми входами блока 4 вычисления якобиана решения прямой задачи кинематики, первыми входами блока 3 вычисления матрицы коэффициентов управления и вторыми входами блока 2 вычисления нелинейных элементов.

Вторые n выходов блока датчиков 35, соответствующие обобщенным скоростям, являются вторыми входами планировщика траекторий в пространстве внешних координат 1 и вторыми входами блока 5 вычисления производной по времени якобиана решения прямой задачи кинематики.

Функциональный блок 7, представленный на фиг. 2, является первым блоком вычисления якобиана желаемых траекторий и содержит блок 37 умножения на 2 (У2), n третьих входов которого связаны с выходами блока транспонирования (ТМ) 9; блок перемножения (П) 38, вторые входы которого связаны с n выходами блока 37, а первые nх(n-1) входов - с третьими выходами планировщика траекторий в пространстве внешних координат 1; блок суммирования (С) 39, на первые входы которого заведены n(n-1) четвертых выходов планировщика траекторий в пространстве внешних координат 1, а на вторые входы n выходов блока перемножения 38. nх(n-1) выходов блока суммирования 39 подаются на первые nх(n-1) входов регистра (Р) 44, вторые n входов которого заземлены, а его nхn выходов являются выходами блока 7 - первого блока вычисления якобиана желаемых траекторий и первыми входами второго блока вычисления якобиана желаемых траекторий 12.

Функциональный блок 12, представленный на фиг. 2, является вторым блоком вычисления якобиана желаемых траекторий и содержит блок перемножения (П) 40, первые nхn входов которого являются третьими входами блока 12 и связаны с выходами блока 4 вычисления якобиана решения прямой задачи кинематики, n вторых входов - со вторыми выходами блока датчиков 35, а n выходов - со входами блока транспонирования (ТМ) 41, выходы которого связаны со вторыми n входами регистра (Р) 42, (n-1)n первых входов которого заземлены. Регистр 42, своими nхn выходами связан со вторыми входами блока суммирования 43, выходы которого являются выходами блока 12.

Функциональный блок 8 вычисления производной по времени от якобиана желаемых траекторий, представленный на фиг. 3, содержит блок перемножения (П) 44, первые (nхn) входов которого связаны с выходами блока 4 вычисления якобиана решения прямой задачи кинематики, вторые n входов - со вторыми выходами блока датчиков 35, а n выходов - со входами блока транспонирования (ТМ) 45, n выходов которого связаны с блоком 46 умножения на 2 (У2). Выходы блока 46 связаны со вторыми входами блока перемножения (П) 47, первые n(n-1) входов которого соединены с третьими выходами планировщика траекторий в пространстве внешних координат 1, а (n-1)n выходов - с первыми входами регистра (Р) 48, вторые n входов которого заземлены. (nхn) выходов регистра 48 являются выходами блока 8 вычисления производной по времени от якобиана желаемых траекторий.

Блок 21 вычисления векторного коэффициента (ВК), представленный на фиг. 4, содержит регистр (Р) 49, первые n-1 входов которого соединены с выходами блока суммирования 14, второй вход заземлен, а его n выходов соединены с первыми входами блока суммирования (С) 50, вторые входы которого соединены с n выходами блока перемножения 36.

Структура блоков суммирования, перемножения и транспонирования матриц (векторов), обращения матриц определяется известными правилами матричного исчисления [Корн Г., Корн Т. Справочник по математике для научных работников и инженеров. М., 1984.- 833 с., с. 392-393]. Например, блок транспонирования матриц может представлять собой коммутатор с жесткими связями между (nхn) входами и (nхn) выходами, как показано на фиг. 5. Нулевые элементы диагональных матриц структурой устройства управления не формируются.

Следует отметить, что структурная реализация блоков 2, 3, 4, 5, 6 однозначно определяется из известной модели (3) для каждого конкретного робота. Так, например, в соответствии с выражениями (3-d) процедура вычисления нелинейных элементов вектора F и матрицы коэффициентов управления В может быть реализована как последовательность элементарных операций над известными векторами G, H и матрицей D (суммирование, умножение, обращение). Структурная схема вычисления векторов G, H и матрицы D приведена в работе [Фу К., Гонсалес P., Ли К. Робототехника.- М.: Мир, 1989.- 624 с., с. 152]. Блок решения прямой задачи кинематики реализует вычисления по выражению (3-c), и в зависимости от модели конкретного робота может быть представлен набором блоков суммирования, блоков перемножения, вычислителей тригонометрических функций (синус, косинус). Кроме того, выражение (3-c) однозначно определяет процедуру вычисления его якобиана и производной от него [Фу К., Гонсалес P., Ли К. Робототехника. - М.: Мир, 1989.- 624 с., с. 152; Корн Г., Корн Т. Справочник по математике для научных работников и инженеров.- М., 1984.- 833 с., с. 395]. В частности, процедура вычисления решения прямой задачи кинематики и его якобиан для шестизвенного робота Пума приведена соответственно на с. 62 и 590-591 в работе [Фу К., Гонсалес P., Ли К. Робототехника.- М.: Мир, 1989, 624 с.].

Блок-схема алгоритма функционирования устройства и диаграмма его работы представлены соответственно на фиг. 6 и 7.

Устройство контурного управления функционирует следующим образом. Планировщик траекторий в пространстве внешних координат 1 (см. фиг. 1) в зависимости от желаемой траектории формирует на своих третьих, четвертых и пятых выходах соответствующие коэффициенты квадратичных форм, являющиеся коэффициентами при квадратах координат - М_j1, при координатах и свободными константами - М_j2, М_j3, соответственно. На n первых и n вторых его выходах формируются диагональные элементы определенно положительных матриц А и Т₁ в силу уравнений (10) и (11), определяющих динамику движения робота к линии пересечения (10) многообразий (5) и (9). Одновременно на n(n-1) третьих, n(n-1) четвертых, (n-1) пятых, n шестых и седьмых выходах планировщика траекторий в пространстве внешних координат 1 формируются коэффициенты, задающие желаемые траектории и требуемую контурную скорость V_k движения вдоль этих траекторий, массогабаритные параметры робота, а также блоком датчиков 35 снимаются показания с n первых и n вторых выходов блока 34 механической системы робота. Сигналы с n первых выходов блока датчиков 35, соответствующие вектору x_2i-1 состояния обобщенных координат, подаются соответственно на вторые выходы блока 2 вычисления нелинейных элементов вектора F, на первые входы блока 3 вычисления матрицы коэффициентов управления В, на первые входы блока 4 вычисления якобиана решения прямой задачи кинематики по выражению (14-а), на первые входы блока 5 вычисления матрицы Г_ф - производной по времени от якобиана решения прямой задачи кинематики по выражению (14-d), на первые входы блока 6 вычисления решения прямой задачи кинематики по выражению (2), где q = x_2i-1.

Следует отметить, что структурная реализация блоков 2, 3, 4, 5, 6 однозначно определяется из известной модели (3) для каждого конкретного робота.

Сигналы со вторых n выходов блока датчиков 35, соответствующие обобщенным скоростям, подаются на первые входы блока 2 вычисления нелинейных элементов, вторые входы блока 5 вычисления производной по времени от якобиана решения прямой задачи кинематики и определяют процедуры соответствующих вычислений.

Блок 7 - первый блок вычисления якобиана желаемых траекторий функционирует в соответствии с выражением (15-в). (mx1) - вектор Y внешних координат, формируемый блоком 6 вычисления решения прямой задачи кинематики и транспонируемый блоком 9, подается на третьи входы блока 7, умножается на 2 посредством блока 37 и одновременно перемножается с (n-1) диагональными (nхn) матрицами M_j1, m=n посредством блока перемножения 38. Формируемые таким образом (n-1) вектор-строки суммируются с (n-1)(1хn)-векторами посредством блока суммирования 39, на выходе которого формируется (n-1)хn элементов ненулевого блока матрицы (15-b), подаваемые на первые входы регистра 44. Нулевые элементы матрицы , входящие в (1хn)-вектор O₃, формируются вторыми заземленными входами регистра 44, (nхn) выходов которого, соответствующих матрице , соединены с первыми входами блока суммирования 43 блока 12 вычисления матрицы .

Вычисления элементов матрицы осуществляются блоком 12, реализующим выражение (15-d). С выходов блока 5 nхn элементов якобиана решения прямой задачи кинематики подаются на первые входы блока перемножения 40, на вторые n входов которого подаются сигналы со вторых выходов блока датчиков 35, а на его выходах формируются (nх1) - вектор (14-b), подаваемый на входы блока транспонирования 41, на выходах которого формируется нижняя вектор-строка матрицы , элементы которой подаются на вторые входы регистра 42, а первые (n-1)n входы которого заземляются, формируя на его соответствующих выходах нулевую подматрицу размерности (n-1)n. Таким образом, на выходах регистра 42 формируются элементы nхn матрицы, подаваемые на вторые входы блока суммирования 43, на выходах которого формируются (nхn) элементов матрицы .

Аналогичным образом, посредством блока 8 и в соответствии с выражением (15-c) формируются элементы матрицы ₁ .

nхn элементов якобиана с выходов блока 4 подаются на первые входы блока перемножения 44, на вторые входы которого подаются сигналы со вторых входов блока датчиков 35. Результат перемножения подается на n входов блока транспонирования 45 с последующим умножением на 2 посредством блока перемножения 46, на n выходов которого подаются сигналы с первых входов блока перемножения 47, nхn-1 вторых входов которого связаны с третьими выходами планировщика траекторий в пространстве внешних координат 1. На выходе блока перемножения 47 формируется (n-1)n элементов ненулевой подматрицы матрицы ₁, которые подаются на первые входы регистра 48, вторые n входов которого заземлены, и на соответствующих выходах блока 48 формируют элементы нулевого вектора O₃.

Блок перемножения 16, на первые входы которого подаются сигналы с выходов блока 3 вычисления матрицы коэффициентов управления В, на вторые входы - с первых выходов планировщика траекторий в пространстве внешних координат 1, на третьи входы - со вторых выходов планировщика траекторий 1, на четвертые входы - с выходов блока 9 вычисления матрицы , на пятые входы - с выходов блока 4 вычисления якобиана , реализует вычисление матричного коэффициента k₁ алгоритма (17), а блок обращения 23 - определение матрицы k^-1.

Перемножение второго выхода планировщика траекторий в пространстве внешних координат 1 и выхода блока 7 - первого блока вычисления якобиана желаемых траекторий (матрицы ) посредством блока перемножения 13, а также первого выхода планировщика траекторий в пространстве внешних координат 1 с выходом блока 12 - второго блока вычисления якобиана желаемых траекторий (матрицы ) посредством блока перемножения 17 с последующим суммированием выходов блоков перемножения 13 и 17 на блоке суммирования 24, выходы которого соединены с первыми входами блока перемножения 27, вторые входы которого соединены с выходами блока 4 вычисления якобиана решения прямой задачи кинематики, позволяет на выходе блока 27 определить первое слагаемое матричного коэффициента k₂ в соответствии с выражением (17).

Второе слагаемое матричного коэффициента k₂, определяемого в алгоритме (17), формируется на выходах блока перемножения 28, на первых входах которого посредством блока перемножения 18 формируется произведение T₁A, а на вторых входах посредством блока суммирования 25 - сумма произведений выхода блока 5 вычисления матрицы Г_ф и выхода блока 12 вычисления матрицы , а также выхода блока 8 вычисления матрицы ₁ и выхода блока 4 вычисления якобиана , реализованных соответственно на блоках перемножения 19 и 20.

Оба слагаемых матричного коэффициента k₂ суммируются на блоке суммирования 29, выходы которого соединены с первыми входами блока перемножения 30, на вторые входы которого заведены вторые выходы блока датчиков 35.

Слагаемые ненулевых элементов вектора _т1 (15-а) реализуются на блоке перемножения 10, на первые входы которого подаются сигналы с третьих выходов планировщика траекторий в пространстве внешних координат 1, на вторые входы - с выходов блока транспонирования 9, на третьи входы - с выходов блока 6 вычисления решения прямой задачи кинематики - вектора Ф (2), а сигналы с его выходов подаются на первые входы блока суммирования 14, вторые входы которого соединены с выходами блока перемножения 11, первые входы которого соединены с четвертыми выходами планировщика траекторий в пространстве внешних координат 1 и выходами блока 6 вычисления вектора Ф (2) - решения прямой задачи кинематики.

Шестые выходы планировщика траекторий в пространстве внешних координат 1, формирующие требования к контурной скорости V_k, являются вторыми входами блока суммирования 14, на первые входы которого подаются сигналы с n первых выходов планировщика траекторий в пространстве внешних координат 1, а сигналы с его выходов подаются на вторые входы блока суммирования 50.

(n-1) - вектор _т(X_2i-1), формируемый на выходе блока суммирования 14, подается на первые входы регистра 49, второй вход которого заземлен, а на его выходе формируется вектор _т1 размерности nх1, n-й элемент которого равен 0. Суммирования вектора _т1 и AV на блоке суммирования 50 позволяет определить векторный коэффициент k₃ из выражения (17).

Произведение матричного коэффициента k₂ на вектор обобщенных скоростей X_2i (блок перемножения 30) подается на первые входы блока суммирования 32, на вторые входы которого подаются сигналы с выходов блока 21 вычисления векторного коэффициента k₃, а сигналы с его выхода подаются на вторые входы блока перемножения 26, первые входы которого являются выходами блока обращения 23.

На выходе блока перемножения 26 формируется первое слагаемое алгоритма (17), которое подается на вторые инверсные входы блока суммирования 31. Первые инверсные входы блока суммирования 31 соединены с выходами блока 22, реализующего перемножение вектора F и матрицы В^-1, определенной посредством блока обращения 15, входы которого соединены с выходами блока 3 вычисления матрицы коэффициентов управления В. Суммирование обоих слагаемых алгоритма (17) на блоке суммирования 31 позволяет сформировать на его n выходах управляющие воздействия, подаваемые на входы блока 33 исполнительных механизмов, выходы которого соединены с n входами блока механической системы робота 34.

Планировщик траекторий в пространстве внешних координат 1, снимая показания с блока датчиков 35, определяет необходимость дальнейшего формирования управляющих сигналов. В случае если технологическая задача не решена, блок планировщика траекторий в пространстве внешних координат 1 повторно формирует набор тех же констант на своих выходах, которые инициализируют работу остальных блоков устройства (см. фиг. 7). В противном случае работа устройства завершается и константы на выходе планировщика траекторий в пространстве внешних координат 1 не формируются или осуществляется переход к решению следующей технологической задачи, т.е. формированию нового набора констант T₁, A, M_j1, M_j2, M_j3, V_k.

Функционирование блоков суммирования, перемножения, обращения и транспонирования осуществляется по известным правилам матричного исчисления [Корн Г., Корн Т. Справочник по математике для научных работников и инженеров. М., 1984.- 833 с., с. 392-393].

Расчет управляющих воздействий по алгоритму (17) осуществляется с частотой не менее 50 Гц [Вукобратович M., Стокич Д., Кирчански H. Неадаптивное и адаптивное управление манипуляционными роботами.- М.: Мир, 1998. - 376 с., с. 338-339] до полного выполнения технологического задания.

Технический результат, достигаемый при осуществлении заявляемого технического решения, заключается в том, что оно позволяет по сравнению с прототипом повысить точность отработки планируемых траекторий, исключить из структуры системы управления блок решения обратной задачи кинематики, расширить функциональные возможности робота. Повышение точности отработки траекторий, планируемых в классе квадратичных форм, обеспечивается за счет исключения из структуры системы управления аппроксиматоров и интерполяторов, вносящих дополнительную погрешность при планировании и, следовательно, при отработке желаемой траектории. Расширение функциональных возможностей робота обусловлено возможностью работы робота во всем классе квадратичных форм с заданной контурной скоростью. Это дает возможность расширить область применения роботов и повысить качество их функционирования.

Представленные алгоритмы контурного управления могут быть реализованы программно с использованием устройств микропроцессорной техники, например высокопроизводительных контроллеров (одного или нескольких) [Вукобратович M. , Стокич Д., Кирчански Н. Неадаптивное и адаптивное управление манипуляционными роботами. М.: Мир, 1998.- 376 с., с. 320-326], в соответствии с блок-схемой алгоритма, представленной на фиг. 6, и диаграммой работы устройства, представленной на фиг. 7.