КОМПЛЕКС УПРАВЛЕНИЯ АНТРОПОМОРФНЫМ МАНИПУЛЯТОРОМ

Изобретение относится к робототехнике и может быть использовано в системах копирующего управления антропоморфными манипуляторами.

Известен комплекс копирующего управления с задающей рукояткой [Е.И. Юревич «Управление роботами и робототехническими системами». - СПб: СПбГТУ, 2000. - 171 с. - с. 131-133], который включает в себя блок механической системы задающего устройства, блок датчиков задающего устройства, блок решения обратной задачи кинематики, блок управления приводами исполнительного устройства. Данный комплекс позволяет организовать копирующее управление антропоморфным манипулятором.

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

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

Известно устройство для измерения углов поворота в суставах кисти оператора, защищенное патентом US №4986280, кл. А61В 5/103, B25J 19/02, B25J 9/00, G01D 5/14, G01D 5/12, G05B 19/427, G06F 3/00, 1991 г., система управления которого включает в себя блок механической системы задающего устройства, блок датчиков задающего устройства и вычислитель углов поворота в суставах руки оператора. Эта структура позволяет организовать измерение углов поворота в суставах кисти оператора и может быть использована для осуществления копирующего управления антропоморфным захватом манипулятора.

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

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

Известна структура комплекса управления на основе копирующего манипулятора [Е.И. Юревич «Управление роботами и робототехническими системами». - СПб: СПбГТУ, 2000. - 171 с. - с. 123-131], которая предполагает наличие блока механической системы задающего устройства, блока датчиков задающего устройства, блока управления приводами исполнительного манипулятора. Эта структура позволяет организовать копирующее управление антропоморфным манипулятором.

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

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

Устройством, наиболее близким по технической сущности к предлагаемому изобретению (прототипом), является дистанционный манипулятор, защищенный патентом RU №125508, кл. B25J 3/04, 2011 г., из описания которого структура системы управления включает в себя блок механической системы задающего устройства, блок датчиков задающего устройства, блок управления приводами антропоморфного манипулятора. Эта структура позволяет организовать копирующее управления антропоморфным манипулятором.

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

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

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

Технический результат достигается тем, что в комплекс управления, содержащий блок механической системы задающего устройства, блок датчиков задающего устройства, блок управления приводами антропоморфного манипулятора, в котором выходы блока механической системы задающего устройства соединены со входами блока датчиков задающего устройства, введен блок расчета углов поворота руки оператора, при этом выходы блока датчиков задающего устройства соединены со входами блока расчета углов поворота руки оператора, выходы которого соединены со входами блока управления приводами антропоморфного манипулятора, блок расчета углов поворота руки оператора содержит блок памяти, вычислитель декартовых координат суставов руки оператора, блок решения обратной задачи кинематики, первый выход блока памяти соединен с первым входом вычислителя декартовых координат суставов руки оператора, второй выход блока памяти соединен с первым входом блока решения обратной задачи кинематики, второй вход вычислителя декартовых координат суставов руки оператора соединен со входом блока расчета углов поворота руки оператора, а выход соединен со вторым входом блока решения обратной задачи кинематики, выход которого соединен с выходом блока расчета углов поворота руки оператора, на первом выходе блока памяти формируются координаты точки B₁, принятой за начало отсчета, координаты вектора жесткой сцепки B₁C₁, длины частей руки оператора , , , длины звеньев задающего устройства , , , длины сцепок В₃С₃, В₄С₄ и виртуальной сцепки , , , координаты локтевого сустава в заданном начальном положении руки оператора, на втором выходе блока памяти формируются координаты точки В₁, принятой за начало отсчета, длины частей руки оператора , , , вычислитель декартовых координат суставов руки оператора формирует представление Денавита-Хартенберга, рассчитывает декартовы координаты лучезапястного сустава руки оператора по формуле:

где В₃ - радиус-вектор лучезапястного сустава руки оператора в глобальной системе координат;

- матрица преобразования из системы координат, связанной с шестым звеном задающего устройства, в глобальную систему координат, полученная с помощью представления Денавита-Хартенберга; рассчитывает декартовы координаты центра кисти оператора по формуле:

где В₄ - радиус-вектор центра кисти оператора в глобальной системе координат;

- матрица преобразования из системы координат, связанной с седьмым звеном задающего устройства, в глобальную систему координат, полученная с помощью представления Денавита-Хартенберга; рассчитывает декартовы координаты нормали к ладони оператора по формуле:

где С₃ - радиус-вектор лучезапястного сочленения задающего устройства в глобальной системе координат;

N - вектор нормали к ладони оператора в глобальной системе координат;

рассчитывает расстояние между плечевым и лучезапястным суставами руки оператора по формуле:

где - расстояние между плечевым и лучезапястным суставами руки оператора;

х_В1, у_В1, z_B1 - декартовы координаты плечевого сустава В₁;

x_B3, y_B3, z_B3 - декартовы координаты лучезапястного сустава В₃; рассчитывает радиус окружности, описываемой точкой В₂ при вращении вокруг прямой B₁B₃:

где р - полупериметр треугольника B₁B₂B₃;

- расстояние между плечевым и лучезапястным суставами руки оператора;

- длина предплечья руки оператора;

- длина плеча руки оператора;

- радиус окружности, описываемой точкой В₂ при вращении вокруг прямой B₁B₃;

рассчитывает длину отрезка B₁K₂ соединяющего плечевой сустав В₁ и центр K₂ окружности, описываемой точкой В₂ при вращении вокруг прямой B₁B₃, по формуле:

где - длина отрезка B₁K₂;

рассчитывает координаты точки K₂ по формуле:

где λ - отношение, в котором точка K₂ делит отрезок B₁B₃;

рассчитывает коэффициенты A_n, B_n, C_n, D_n плоскости, перпендикулярной отрезку В₁В₃, и проходящей через точку K₂:

где х_В1ВЗ, у_В1ВЗ, z_B1B3 - декартовы координаты вектора В₁В₃;

x_K2, y_K2, z_K2 - декартовы координаты точки K₂;

выполняет замену:

рассчитывает варианты положения В_2а и B_2b локтевого сустава В₂ по формулам:

где а, b, с, r, m, d, , h, q, s, t - промежуточные величины, необходимые для расчета декартовых координат точек В_2а и B_2b;

i, j, k - индексы, принимающие значения, определяемые следующим правилом: индекс i равен номеру ненулевой компоненты вектора B₁B₃ в выражении, j и k - номерам оставшихся компонент;

выбирает положение искомой точки В₂ из двух возможных вариантов В_2а и B_2b по формуле:

где - радиус-вектор декартовых координат сустава В₂, вычисляемый на n-й итерации;

n - номер текущей итерации косвенного измерения углов поворота в суставах руки оператора;

- первый вариант радиус-вектора декартовых координат сустава В₂, вычисляемый на n-й итерации;

- второй вариант радиус-вектора декартовых координат сустава В₂, вычисляемый на n-й итерации;

- радиус-вектор декартовых координат сустава В₂, вычисленный на (n-1)-й итерации;

блок решения обратной задачи кинематики формирует представление Денавита-Хартенберга руки оператора, рассчитывает декартовы координаты различных точек в необходимых системах координат:

где - координаты точки B_i в j-й системе координат представления Денавита-Хартенберга руки оператора;

T_i - матрица преобразования из j-й системы координат в глобальную систему координат;

B_i - координаты точки B_i в глобальной системе координат;

Nⁱ - координаты вектора нормали N в i-й системе координат;

рассчитывает углы поворота руки оператора по формулам:

где - ненормированное значение первого угла поворота руки оператора;

atan2(x,y) - функция арктангенса, учитывающая квадрант вычисляемого угла;

θ_i - i-й угол поворота руки оператора;

Θ₂, Θ₄, Θ₆ - константы, отдельно определяемые для каждой модели задающего устройства;

формирует вектор углов поворота руки оператора по формуле:

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

Рассмотрим кинематическую схему связанных между собой руки оператора и задающего устройства (фиг. 1). Плечевой, локтевой и лучезапястный суставы руки оператора, а также центр его кисти обозначены как В₁, В₂, В₃, В₄, соответственно. Аналогичные точки задающего устройства, имеющего кинематическую схему аналогичную кинематической схеме руки оператора, обозначены как С₁, С₂, С₃, С₄, соответственно. Точка O - неподвижная относительно тела оператора точка крепления задающего устройства. Непрерывными линиями на фиг. 1 обозначены жесткие физически существующие связи между точками. Пунктирными линиями обозначены жесткие связи, не существующие, но обусловленные другими жесткими физическими связями. Полыми точками обозначены кинематические пары руки оператора и задающего устройства. Заполненными точками обозначены жесткие соединения звеньев.

Рука оператора и задающее устройство сцеплены в нескольких местах жесткими сцепками. Жесткая сцепка В₁ОС₁ предотвращает взаимное движение плечевого сустава оператора В₁ и плечевого сочленения задающего устройства C₁. Сцепка В₃В₄С₄С₃ объединяет кистевые звенья оператора и задающего устройства для полного контроля над движением задающего устройства. Также существует гибкая сцепка, представленная на фиг. 1 как кинематическая цепь В₅С₅. Данная цепь удерживает звенья задающего устройства в положении, параллельном частям руки оператора. В противном случае для изображенного на фиг. 1 положения руки возможна ситуация, когда локтевой сустав направлен вниз, а локтевое сочленение задающего устройства - вверх.

Пусть известны:

- координаты точки В₁ = (0; 0; 0), принятой за начало отсчета;

- координаты вектора жесткой сцепки B₁C₁ = (х_В1С1; y_B1C1; z_B1C1);

- длины частей руки оператора , , ;

- длины звеньев задающего устройства , , ;

- длины сцепок В₃С₃, В₄С₄ и виртуальной сцепки , , ;

- вектор измеренных значений обобщенных координат задающего устройства:

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

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

1. Задающее устройство представляется в виде эквивалентной кинематической цепи, состоящей из семи звеньев и семи кинематических пар пятого класса.

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

3. Формируются матрицы преобразования из i-й системы координат, связанной с i-м звеном в глобальную систему координат.

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

где В₃ - радиус-вектор лучезапястного сустава руки оператора в глобальной системе координат;

В₄ - радиус-вектор центра кисти оператора в глобальной системе координат;

С₃ - радиус-вектор лучезапястного сочленения задающего устройства в глобальной системе координат;

N - вектор нормали к ладони оператора в глобальной системе координат;

- матрица преобразования из системы координат, связанной с шестым звеном задающего устройства, в глобальную систему координат;

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

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

Необходимо найти расстояние между соседними суставами, декартовы координаты которых известны вследствие их жесткой связи с задающим устройством:

где - расстояние между плечевым и лучезапястным суставами руки оператора;

х_В1, у_В1, z_B1 - декартовы координаты плечевого сустава В₁;

x_B3, y_B3, z_B3 - декартовы координаты лучезапястного сустава В₃.

Для известных декартовых координат суставов В₁ и В₃ множество возможных положений неизвестного сустава В₂ представляет собой окружность, изображенную на фиг. 2, описываемую точкой В₂ при вращении вокруг прямой В₁В₃. Центр окружности K₂ может быть получен из треугольника B₁B₂B₃ путем проведения высоты из точки В₂ к стороне B₁B₃.

Длина высоты может быть найдена через площадь треугольника B₁B₂B₃:

где р - полупериметр треугольника В₁В₂В₃;

- длина высоты, проведенной в треугольнике B₁B₂B₃ из точки В₂.

Длина отрезка B₁K₂ может быть найдена по формуле:

где - длина отрезка B₁K₂.

Координаты точки K₂ могут быть найдены по формуле деления отрезка в заданном отношении:

где λ - отношение, в котором точка K₂ делит отрезок В₁В₃.

Обозначенная на фиг. 2 окружность возможных положений сустава В₂ может быть найдена как пересечение сферы с центром в точке K₂ и радиусом , и плоскости, перпендикулярной отрезку B₁B₃, и проходящей через точку K₂:

где x_K2, y_K2, z_K2 - декартовы координаты точки K₂;

A_n, B_n, C_n, D_n - коэффициенты плоскости, перпендикулярной отрезку В₁В₃, и проходящей через точку K₂;

x_B1B3, y_B1B3, z_B1B3 - значения проекций вектора B₁B₃ на оси декартовой системы координат.

Предлагаемый способ определения координат локтевого сустава основывается на допущении о том, что в целях расчета положения локтевого сустава можно считать расстояние между сочленением С₂ и суставом В₂ постоянным. Тогда выбор положения точки В₂ на окружности, описывающей возможные положения локтевого сустава, может быть произведен из условия нахождения общих точек пересечения со сферой с центром в точке С₂ и радиусом (фиг. 2). Уравнение данной сферы имеет вид:

где х_С2, у_С2, z_C2 - декартовы координаты сочленения С₂;

- эффективная длина нежесткого крепления.

Варианты положения В_2а и B_2b локтевого сустава В₂ могут быть найдены как точки пересечения описанных выше окружности и сферы. Примем, что:

где B₁B₃, K₂, С₂, В_2а, B_2b - значения декартовых координат соответствующих векторов и точек.

Варианты положения В_2а и B_2b локтевого сустава В₂ могут быть найдены по формулам:

где а, b, с, r, m, d, , h, q, s, t - промежуточные величины, необходимые для расчета декартовых координат точек В_2а и B_2b;

i, j, k - индексы, принимающие значения, определяемые следующим правилом: индекс i равен номеру ненулевой компоненты вектора В₁В₃ в выражении, индексы j и k - номерам оставшихся компонент. Данные индексы, динамически определяемые в процессе расчета, введены для гарантии, что при расчете величин а, b и с не будет деления на ноль.

Выбор положения искомой точки В₂ из двух возможных вариантов В_2а и B_2b может выполняться из условия непрерывности перемещения сустава в процессе движения. Для этого в начале работы оператора с задающим устройством необходимо выбрать одну из двух рассчитанных точек положения локтевого сустава, как наиболее соответствующую его реальному положению. Для этого оператор должен выполнить постановку руки в некоторое заданное начальное положение, в котором декартовы координаты локтевого сустава соответствуют координатам, записанным в блоке памяти.

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

где - радиус-вектор декартовых координат сустава В₂, вычисляемый на n-й итерации;

n - номер текущей итерации косвенного измерения углов поворота в суставах руки оператора;

- первый вариант радиус-вектора декартовых координат сустава В₂, вычисляемый на n-й итерации;

- второй вариант радиус-вектора декартовых координат сустава В₂, вычисляемый на n-й итерации;

- радиус-вектор декартовых координат сустава В₂, вычисленный на (n-1)-й итерации.

На основе декартовых координат суставов руки оператора, а также центра его кисти и нормали к ладони могут быть найдены углы поворота руки оператора путем решения обратной задачи кинематики, например, с помощью решения, предложенного в работе [Petrenko, V.I. Calculating rotation angles of the operator's arms based on generalized coordinates of the master device with following anthropomorphic manipulator in real time / V.I. Petrenko, F.B. Tebueva, V.B. Sychkov, V.O. Antonov, M.M. Gurchinsky // International Journal of Mechanical Engineering and Technology (IJMET). - 2018. - Vol. 9, Issue 7 (2018). - pp. 447-461].

Рассчитанные углы поворота руки оператора ближе к реальным значениям углов поворота руки оператора, чем измеренные значения углов поворота задающего устройства, используемые в прототипе в качестве приближенного значения углов поворота руки оператора. Вывод формул (17)÷(29), а также результаты испытаний, подтверждающие повышение точности расчета углов поворота руки оператора при их применении приведены в [Petrenko V.I., Tebueva F.B., Sychkov V.B., Antonov V.O., Gurchinsky M.M. Calculating rotation angles of the operator's arms based on generalized coordinates of the master device with following anthropomorphic manipulator in real time. International Journal of Mechanical Engineering and Technology, Volume 9, Issue 7, July 2018, Pages 447-461].

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

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

Выходы блока механической системы задающего устройства 1 соединены со входами блока датчиков задающего устройства 2. Выходы блока датчиков задающего устройства 2 соединены со входами блока расчета углов поворота руки оператора 3. Выходы блока расчета углов поворота руки оператора 3 соединены с входами блока управления приводами антропоморфного манипулятора 4.

Блок расчета углов поворота руки оператора 3 (см. фиг. 4) содержит блока памяти 5, вычислитель декартовых координат суставов руки оператора 6 и блок решения обратной задачи кинематики 7.

Первый выход блока памяти 5 соединен с первым входом вычислителя декартовых координат суставов руки оператора 6. Второй выход блока памяти 5 соединен с первым входом блока решения обратной задачи кинематики 7. Второй вход вычислителя декартовых координат суставов руки оператора 6 соединен с входом блока расчета углов поворота руки оператора 3. Выход вычислителя декартовых координат суставов руки оператора 6 соединен со вторым входом блока решения обратной задачи кинематики 7, выход которого соединен с выходом блока расчета углов поворота руки оператора 3.

Принцип работы комплекса управления заключается в следующем (фиг. 3).

Рука оператора приводит в движение блок механической системы задающего устройства 1. Положение блока механической системы задающего устройства 1 в пространстве обобщенных координат определяется блоком датчиков задающего устройства 2 и на его выходе формируется вектор обобщенных координат задающего устройства θ', размерностью семь, равной количеству основных степеней подвижности руки оператора. Блок расчета углов поворота руки оператора 3 на основе входного вектора обобщенных координат задающего устройства θ' и хранящихся внутри блока параметров задающего устройства и руки оператора выполняет расчет углов поворота руки оператора θ. Рассчитанные значения углов поворота руки оператора θ обрабатываются блоком управления приводами антропоморфного манипулятора 4.

Процесс расчета углов поворота руки оператора блоком расчета углов поворота руки оператора 3 заключается в следующем.

На первом выходе блока памяти 5 формируются значения параметров, необходимых для расчетов декартовых координат суставов руки оператора:

- координаты точки В₁ = (0; 0; 0), принятой за начало отсчета;

- координаты вектора жесткой сцепки B₁C₁ = (х_В1С1; y_B1C1; z_B1C1);

- длины частей руки оператора , , ;

- длины звеньев задающего устройства , , ;

- длины сцепок В₃С₃, В₄С₄ и виртуальной сцепки , , ;

- координаты локтевого сустава в начальном положении рассчитанные на предыдущей итерации расчета.

На втором выходе блока памяти 5 формируются значения параметров, необходимых для решения обратной задачи кинематики:

- координаты точки В₁ = (0; 0; 0), принятой за начало отсчета;

- длины частей руки оператора , , .

На основе параметров, хранимых в блоке памяти 5 (фиг. 4) и измеренных значений углов поворота задающего устройства θ', поступающих от входов блока расчета углов поворота руки оператора 3, вычислитель декартовых координат суставов руки оператора 6 рассчитывает декартовы координаты суставов руки оператора, центра его кисти и нормали к его ладони.

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

где В₃ - радиус-вектор лучезапястного сустава руки оператора в глобальной системе координат;

В₄ - радиус-вектор центра кисти оператора в глобальной системе координат;

С₃ - радиус-вектор лучезапястного сочленения задающего устройства в глобальной системе координат;

N - вектор нормали к ладони оператора в глобальной системе координат;

- матрица преобразования из системы координат, связанной с шестым звеном задающего устройства, в глобальную систему координат, полученная с помощью представления Денавита-Хартенберга;

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

Для расчета декартовых координат локтевого сустава используют следующую методику.

Находят расстояние между плечевым и лучезапястным суставами, декартовы координаты которых известны вследствие их жесткой связи с задающим устройством:

где - расстояние между плечевым и лучезапястным суставами руки оператора;

х_В1, у_В1, z_B1 - декартовы координаты плечевого сустава В₁;

x_B3, y_B3, z_B3 - декартовы координаты лучезапястного сустава В₃.

Рассчитывают радиус окружности, описываемой точкой В₂ при вращении вокруг прямой B₁B₃ (см. фиг. 2):

где р - полупериметр треугольника В₁В₂В₃;

- радиус окружности, описываемой точкой точкой В₂ при вращении вокруг прямой B₁B₃.

Рассчитывают длину отрезка B₁K₂ соединяющего плечевой сустав В₁ и центр K₂ окружности, описываемой точкой B₂ при вращении вокруг прямой В₁В₃, по формуле:

где - длина отрезка B₁K₂.

Рассчитывают координаты точки K₂ по формуле:

где λ - отношение, в котором точка K₂ делит отрезок B₁B₃.

Рассчитывают коэффициенты A_n, B_n, C_n, D_n плоскости, перпендикулярной отрезку В₁В₃, и проходящей через точку K₂:

где х_В1В3, у_В1В3, z_B1B3 - декартовы координаты вектора В₁В₃;

x_K2, y_K2, z_K2 - декартовы координаты точки K₂.

Варианты положения В_2а и B_2b локтевого сустава В₂ могут быть найдены как точки пересечения описанных выше окружности и сферы. Примем, что:

где B₁B₃, K₂, С₂, В_2а, B_2b - значения декартовых координат соответствующих векторов и точек.

Варианты положения В_2а и B_2b локтевого сустава В₂ могут быть найдены по формулам:

где а, b, с, r, m, d, , h, q, s, t - промежуточные величины, необходимые для расчета декартовых координат точек В_2а и B_2b;

i, j, k - индексы, принимающие значения, определяемые следующим правилом: индекс i равен номеру ненулевой компоненты вектора В₁В₃ в выражении, j и k - номерам оставшихся компонент. Данные индексы, динамически определяемые в процессе расчета, введены для гарантии, что при расчете величин а, b и с не будет деления на ноль.

Выбор положения искомой точки В₂ из двух возможных вариантов В_2а и B_2b может выполняться из условия непрерывности перемещения сустава в процессе движения. Для этого в начале работы оператора с задающим устройством необходимо выбрать одну из двух рассчитанных точек положения локтевого сустава, как наиболее соответствующую его реальному положению. Для этого оператор должен выполнить постановку руки в некоторое заданное начальное положение, в котором декартовы координаты локтевого сустава соответствуют координатам, записанным в блоке памяти.

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

где - радиус-вектор декартовых координат сустава В₂, вычисляемый на n-й итерации;

n - номер текущей итерации косвенного измерения углов поворота в суставах руки оператора;

- первый вариант радиус-вектора декартовых координат сустава В₂, вычисляемый на n-й итерации;

- второй вариант радиус-вектора декартовых координат сустава В₂, вычисляемый на n-й итерации;

- радиус-вектор декартовых координат сустава В₂, вычисленный на (n-1)-й итерации.

На основе формируемых декартовых координат суставов руки оператора В₁, В₂, В₃, центра его кисти В₄ и нормали к его ладони N на выходе вычислителя декартовых координат 6, блок решения обратной задачи кинематики 7 рассчитывает углы поворота руки оператора.

Для этого формируют представление Денавита-Хартенберга руки оператора.

Рассчитывают декартовы координаты различных точек в необходимых системах координат:

где - координаты точки B_i в j-й системе координат представления Денавита-Хартенберга руки оператора;

T_i - матрица преобразования из j-й системы координат в глобальную систему координат;

B_i - координаты точки B_i в глобальной системе координат;

Nⁱ - координаты вектора нормали N в j-й системе координат.

Рассчитывают углы поворота руки оператора по формулам:

где - ненормированное значение первого угла поворота руки оператора;

atan2(x,y) - функция арктангенса, учитывающая квадрант вычисляемого угла;

θ_i - i-й угол поворота руки оператора;

Θ₂, Θ₄, Θ₆ - константы, отдельно определяемые для каждой модели задающего устройства.

Формируют вектор углов поворота руки оператора по формуле:

Таким образом, реализуемый заявляемым комплексом процесс управления антропоморфным манипулятором заключается в следующем:

1) Оператор приводит в движение механическую систему задающего устройства.

2) Измеряют обобщенные координаты 0' задающего устройства.

3) Рассчитывают декартовы координаты плечевого и лучезапястного суставов руки оператора, центра ладони оператора и вектора нормали к ней В₁, В₂, В₃, В₄, N, соответственно.

4) Решают обратную задачи кинематики для формирования сигнала управления антропоморфным манипулятором Э.

5) Осуществляют управление антропоморфным манипулятором на основе сформированного сигнала управления 0.

Описанный алгоритм расчета и уравнения (30)÷(44) соответствуют порядку расчета, приведенному в работе [Petrenko V.I., Tebueva F.B., Sychkov V.B., Antonov V.О., Gurchinsky M.M. Calculating rotation angles of the operator's arms based on generalized coordinates of the master device with following anthropomorphic manipulator in real time. International Journal of Mechanical Engineering and Technology, Volume 9, Issue 7, July 2018, Pages 447-461]. Данный порядок расчета обеспечивает формирование закона управления для копирующего управления антропоморфным манипулятором на основе перерасчета углов поворота задающего устройства в углы поворота руки оператора.

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

Предлагаемое изобретение промышленно применимо. В качестве блоков механической системы 1 и датчиков задающего устройства 2 могут использоваться существующие средства задающих устройств, реализованных в виде экзоскелета (например, дистанционный манипулятор, защищенный патентом RU №125508, кл. B25J 3/04, 2011 г.). В качестве блока управления приводами антропоморфного манипулятора 4 могут использоваться существующие блоки управления антропоморфными манипуляторами. В качестве блока расчета углов поворота руки оператора 3 может использоваться специализированная ЭВМ или вычислительные системы задающего или исполнительного устройства.

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