Комплекс копирующего управления манипуляторами антропоморфного робота

Авторы патента:

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

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

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

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

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

Наиболее близким по технической сущности к предлагаемому изобретению (прототипом) является комплекс управления антропоморфным манипулятором (заявка на изобретение RU 2018146275 А), который включает в себя блок механической системы задающего устройства, блок датчиков задающего устройства, блок расчета углов поворота руки оператора, блок управления приводами антропоморфного манипулятора. Эта структура позволяет организовать копирующее управление антропоморфным манипулятором.

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

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

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

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

где - однородная матрица преобразования из j-й в i-ю систему координат,

- однородная матрица сложного преобразования между i-й и (i-1)-й системами координат,

- однородная матрица элементарного поворота вокруг оси Oz на угол ,

- однородная матрица перемещения по оси Oz на величину ,

- однородная матрица перемещения по оси Ох на величину ,

- однородная матрица элементарного поворота вокруг оси Ох на угол ,

- i-e элементы векторов ,

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

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

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

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

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

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

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

где - орт оси Ох,

- проекции радиус-вектора на оси Ох, Oy,

вычисляет угол γ' между радиус-вектором и плоскостью yOz по формуле:

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

вычисляет вектор получаемый вращением вектора вокруг вектора на угол γ' по формуле:

где - матрица поворота вокруг вектора на угол ρ, вычисляемая по формуле:

где r_x, r_y, r_z - проекции вектора на оси Ох, Oy и Oz, соответственно,

S(ρ) - синус угла ρ,

С(ρ) - косинус угла ρ,

V(ρ)=1-cos(ρ),

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

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

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

вычисляет углы Эйлера ϕ', ϑ', ψ' кисти оператора по формулам:

где - вектор-столбцы матрицы

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

формирует на выходах набор специализированных обобщенных координат руки оператора, специализированные обобщенные координаты R₇, r₇, α, β, ϕ, ϑ, ψ манипулятора антропоморфного робота принимаются равными специализированным обобщенным координатам , , α', β', ϕ', ϑ', ψ' руки оператора:

блок расчета декартовых координат центров сочленений манипулятора антропоморфного робота вычисляет значение максимального расстояния r_m от центра плечевого сочленения до центра схвата манипулятора антропоморфного робота по формуле:

вычисляет радиус-вектор центра схвата манипулятора антропоморфного робота по формулам:

где - расстояние от центра схвата манипулятора антропоморфного робота до центра плечевого сочленения второго манипулятора антропоморфного робота,

O_7x, O_7y,O_7z - проекции радиус-вектора на оси Ox, Oy, Oz, соответственно,

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

где С(ρ) - косинус угла ρ,

S(ρ) - синус угла ρ,

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

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

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

λ - отношение, в котором точка K делит вектор

вычисляет угол γ между радиус-вектором и плоскостью yOz по формуле:

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

вычисляет вектор получаемый вращением вектора вокруг вектора на угол γ, по формуле:

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

формирует на выходах набор декартовых координат центров сочленений манипулятора антропоморфного робота, блок решения обратной задачи кинематики манипулятора антропоморфного робота вычисляет матрицы ⁰Т₁, ⁰Т₂, ⁰Т₄, ⁰Т₅, ⁰Т₆ однородного преобразования для манипулятора антропоморфного робота согласно представлению Денавита-Хартенберга по формулам:

где ⁱT_j - однородная матрица преобразования из j-й в i-ю систему координат,

^i-1A_j - однородная матрица сложного преобразования из t-й в (i-1)-ю систему координат,

T_z,θ(θ_i) - однородная матрица элементарного поворота вокруг оси Oz на угол θ_i,

T_z,d(d_i) - однородная матрица перемещения по оси Oz на величину d_i,

T_x,a(a_i) - однородная матрица перемещения по оси Ох на величину a_i,

T_x,α(α_i) - однородная матрица элементарного поворота вокруг оси Ох на угол α_i,

a_i, d_i, α_i, θ_i - i-e элементы векторов

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

где - радиус-вектор точки O_i в j-й системе координат,

⁰T_i - матрица преобразования из i-й системы координат в нулевую систему координат,

- радиус-вектор точки O_i в нулевой системе координат,

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

вычисляет вектор целевых углов поворота манипуляторов антропоморфного робота по формулам:

где - ненормированное значение первого целевого угла поворота манипулятора антропоморфного робота,

θ_i - i-й целевой угол поворота манипулятора антропоморфного робота,

Θ₂, Θ₄, Θ₆ - константы, отдельно определяемые для каждой модели манипулятора антропоморфного робота,

формирует на выходах вектор целевых углов поворота манипуляторов антропоморфного робота.

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

При реализации копирующего управления важной задачей является определение вектора углов поворота исполнительного манипулятора на основе известного вектора углов поворота в суставах руки оператора. В прототипе в качестве вектора целевых углов поворота исполнительного манипулятора используется вектор углов поворота в суставах руки оператора. В общем случае конструктивные параметры антропоморфного робота не пропорциональны аналогичным антропометрическим параметрам оператора. Данный факт обусловливает возникновение при использовании прототипа ситуаций, в которых комплекс копирующего управления функционирует некорректно. Если длины звеньев манипуляторов пропорциональны длинам соответствующих отделов руки оператора, но «ширина плеч» робота, т.е. расстояние между центрами плечевых сочленений, непропорционально больше расстояния между центрами плечевых суставов оператора, при использовании прототипа возникнет ситуация, изображенная на фиг. 1. За счет того, что «ширина плеч» робота больше, при сведении рук оператора ладонь к ладони между кистевыми звеньями манипуляторов останется зазор, т.е. становится невозможным сведение манипуляторов антропоморфного робота «ладонь к ладони». Таким образом, прототип работает корректно только при полной пропорциональности конструктивных параметров манипуляторов антропоморфного робота и антропометрических параметров рук оператора, что требует создания уникального антропоморфного робота под каждого оператора или реализации возможности подстройки конструктивных параметров антропоморфного робота, что значительно усложняет его конструкцию.

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

1. Относительное расстояние от центра кисти руки оператора до центра плечевого сустава этой же руки, вычисляемое по формуле:

где - расстояние от центра кисти оператора до центра плечевого сустава этой же руки,

- максимальное расстояние от центра плечевого сустава руки оператора до центра его кисти,

- расстояние от центра плечевого сустава руки оператора до центра его локтевого сустава,

- расстояние от центра локтевого сустава руки оператора до центра его лучезапястного сустава,

- расстояние от центра лучезапястного сустава руки оператора до центра его кисти.

2. Относительное расстояние от центра кисти оператора до центра плечевого сустава второй руки оператора, вычисляемое по формуле:

где - расстояние от центра кисти оператора до центра плечевого сустава второй руки оператора.

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

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

5. Углы Эйлера ϕ', ϑ', ψ' кисти оператора.

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

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

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

1. Расстояние от центра плечевого сустава руки оператора до центра его локтевого сустава.

2. Расстояние от центра локтевого сустава руки оператора до центра его лучезапястного сустава.

3. Расстояние от центра лучезапястного сустава руки оператора до центра его кисти.

4. Расстояние m' между центрами плечевых суставов оператора.

5. Векторы параметров Денавита-Хартенберга, описывающих кинематическую структуру руки оператора.

6. Расстояние от центра плечевого сочленения манипулятора антропоморфного робота до центра его локтевого сочленения.

7. Расстояние от центра локтевого сочленения манипулятора антропоморфного робота до центра его лучезапястного сочленения.

8. Расстояние от центра лучезапястного сочленения манипулятора антропоморфного робота до центра его схвата.

9. Расстояние m между центрами плечевых сочленений манипуляторов антропоморфного робота.

10. Векторы параметров Денавита-Хартенберга, описывающих кинематическую структуру манипулятора антропоморфного робота.

11. Вектор углов поворота в суставах руки оператора.

Кинематическая схема руки оператора и манипулятора антропоморфного робота приведены на фиг. 2. Системы координат, введенные в соответствии с представлением Денавита-Хартенберга для кинематической схемы, представленной на фиг. 2, представлены на фиг. 3. Центр плечевого сочленения, центр локтевого сочленения, центр лучезапястного сочленения и центр схвата манипулятора антропоморфного робота соответствуют точкам O₀, O₃, O₆, O₇ на фиг. 3. Центры суставов руки оператора расположены аналогично центрам сочленений манипулятора антропоморфного робота.

Расчет вектора целевых углов поворота манипулятора антропоморфного робота включает в себя четыре этапа:

1. Расчет декартовых координат центров суставов руки оператора.

2. Расчет специализированных обобщенных координат руки оператора.

3. Расчет декартовых координат сочленения манипулятора антропоморфного робота.

4. Решение обратной задачи кинематики манипулятора антропоморфного робота.

Расчет декартовых координат суставов руки оператора включает в себя следующие действия.

Формирование матриц однородного преобразования для руки оператора согласно представлению Денавита-Хартенберга по формулам:

где - однородная матрица преобразования из j-й в i-ю систему координат (фиг. 3),

- однородная матрица сложного преобразования между i-й и (i-1)-й системами координат,

- однородная матрица элементарного поворота вокруг оси Oz на угол ,

- однородная матрица перемещения по оси Oz на величину ,

- однородная матрица перемещения по оси Ох на величину ,

- однородная матрица элементарного поворота вокруг оси Ох на угол ,

- i-e элементы векторов

Расчет радиус-вектора центра локтевого сустава руки оператора по формуле:

Расчет радиус-вектора центра лучезапястного сустава руки оператора по формуле:

Расчет радиус-вектора центра кисти руки оператора по формуле:

Расчет специализированных обобщенных координат руки оператора включает в себя следующие действия.

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

Расчет значения относительного расстояния от центра кисти руки оператора до центра плечевого сустава этой же руки по формуле:

Расчет значения относительного расстояния от центра кисти оператора до центра плечевого сустава второй руки оператора по формуле:

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

где - орт оси Ох,

- проекции радиус-вектора на оси Ох, Oy.

Расчет угла γ' между радиус-вектором и плоскостью yOz по формуле:

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

Расчет вектора получаемого вращением вектора вокруг вектора на угол γ' по формуле:

где - матрица поворота вокруг вектора на угол ρ, вычисляемая по формуле:

где r_x, r_y, r_z - проекции вектора на оси Ох, Oy и Oz, соответственно,

S(ρ) - синус угла ρ,

С(ρ) - косинус угла ρ,

V(ρ)=1-cos(ρ).

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

Расчет вектора соединяющего конец радиус-вектора и конец радиус-вектор центра локтевого сустава, по формуле:

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

Расчет углов Эйлера ϕ', ϑ', ψ' кисти оператора по формулам:

где - вектор-столбцы матрицы

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

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

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

Расчет значения максимального расстояния r_m от центра плечевого сочленения до центра схвата манипулятора антропоморфного робота по формуле:

Расчет радиус-вектора центра схвата манипулятора антропоморфного робота по формулам:

O_7x, O_7y, O_7z - проекции радиус-вектора на оси Ох, Oy, Oz, соответственно.

Расчет радиус-вектора центра лучезапястного сочленения манипулятора антропоморфного робота по формулам:

где С(ρ) - косинус угла ρ,

S(ρ) - синус угла ρ.

Расчет вектора нормали к плоскости схвата манипулятора антропоморфного робота по формуле:

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

где p - полупериметр треугольника, образованного центрами плечевого, локтевого и лучезапястного сочленений манипулятора антропоморфного робота,

λ - отношение, в котором точка K делит вектор

Расчет угла γ между радиус-вектором и плоскостью yOz по формуле:

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

Расчет вектора получаемого вращением вектора вокруг вектора на угол γ по формуле:

Расчет радиус-вектора по формуле:

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

Формирование матриц однородного преобразования для манипулятора антропоморфного робота согласно представлению Денавита-Хартенберга по формулам:

где ⁱT_j - однородная матрица преобразования из j-й в i-ю систему координат,

^i-1A_i - однородная матрица сложного преобразования из i-й в (i-1)-ю систему координат,

T_z,θ(θ_i) - однородная матрица элементарного поворота вокруг оси Oz на угол θ_i,

T_z,d(d_i) - однородная матрица перемещения по оси Oz на величину d_i,

T_x,a(a_i) - однородная матрица перемещения по оси Ох на величину a_i,

T_x,α(α_i) - однородная матрица элементарного поворота вокруг оси Ох на угол α_i,

a_i,d_i, α_i, θ_i - i-e элементы векторов

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

где - радиус-вектор точки 0_i в j-й системе координат,

⁰T_i - матрица преобразования из i-й системы координат в нулевую систему координат,

- радиус-вектор точки O_i в нулевой системе координат,

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

Расчет вектора целевых углов поворота манипуляторов антропоморфного робота по формулам:

где - ненормированное значение первого целевого угла поворота манипулятора антропоморфного робота,

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

θ_i - i-й целевой угол поворота манипулятора антропоморфного робота,

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

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

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

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

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

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

На фиг. 3 изображены системы координат, введенные в соответствии с представлением Денавита-Хартенберга для кинематической схемы, представленной на фиг. 2.

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

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

На фиг. 5 приведена структурная схема блока расчета целевых углов поворота манипулятора антропоморфного робота. Блок расчета целевых углов поворота манипулятора антропоморфного робота 4 содержит блок расчета декартовых координат центров суставов руки оператора 6, блок расчета специализированных обобщенных координат руки оператора 7, блок расчета декартовых координат центров сочленений манипулятора антропоморфного робота 8, блок решения обратной задачи кинематики манипулятора антропоморфного робота 9 и блок памяти 10.

Входы блока расчета целевых углов поворота манипулятора антропоморфного робота 4 соединены с первыми входами блока расчета декартовых координат центров суставов руки оператора 6, выходы которого соединены с первыми входами блока расчета специализированных обобщенных координат руки оператора 7, выходы которого соединены с первыми входами блока расчета декартовых координат центров сочленений манипулятора антропоморфного робота 8, выходы которого соединены с первыми входами блока решения обратной задачи кинематики манипулятора антропоморфного робота 9, выходы которого соединены с выходами блока расчета целевых углов поворота манипулятора антропоморфного робота 4, первые выходы блока памяти 10 соединены со вторыми входами блока расчета декартовых координат центров суставов руки оператора 6, вторые выходы блока памяти 10 соединены со вторым входами блока расчета специализированных обобщенных координат руки оператора 7, третьи выходы блока памяти 10 соединены со вторым входами блока расчета декартовых координат центров сочленений манипулятора антропоморфного робота 8, четвертые выходы блока памяти 10 соединены со вторым входами блока решения обратной задачи кинематики манипулятора антропоморфного робота 9.

На фиг. 6 приведена визуализация вычисленных положений манипуляторов антропоморфного робота, расстояние между плечевыми сочленениями которого непропорционально уже расстояния между плечевыми суставами оператора для положения рук оператора, когда ладони соединены перед собой, при управлении с помощью способа-прототипа. Центры плечевого, локтевого, лучезапястного сочленений и схвата левого манипуляторы обозначены как O_0L, O_3L, O_6L, O_7L соответственно. Аналогичные точки правого манипулятора обозначены как O_0R, O_3R, O_6R, O_7R.

На фиг. 7 приведена визуализация вычисленных положений манипуляторов антропоморфного робота, расстояние между плечевыми сочленениями которого непропорционально уже расстояния между плечевыми суставами оператора для положения рук оператора, когда ладони соединены перед собой, при управлении с помощью заявляемого способа. Центры плечевого, локтевого, лучезапястного сочленений и схвата левого манипуляторы обозначены как O_0L, O_3L, O_6L, O_7L соответственно. Аналогичные точки правого манипулятора обозначены как O_0R, O_3R, O_6R, O_7R.

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

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

2. Блок датчиков задающего устройства 2 измеряет вектор углов поворота в кинематических парах задающего устройства.

3. Блок расчета углов поворота руки оператора 3 рассчитывает вектор углов поворота руки оператора.

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

4.1. Блок расчета декартовых координат центров суставов руки оператора 6 вычисляет набор радиус-векторов центров суставов руки оператора на основе вектора углов поворота и параметров Денавита-Хартенберга руки оператора.

4.2. Блок расчета специализированных обобщенных координат руки оператора 7 вычисляет набор специализированных обобщенных координат руки оператора на основе набора декартовых координат центров суставов руки оператора и антропометрических параметров оператора.

4.3. Блок расчета декартовых координат центров сочленений манипулятора антропоморфного робота 8 вычисляет набор декартовых координат центров сочленений манипулятора антропоморфного робота на основе набора специализированных обобщенных координат руки оператора и конструктивных параметров антропоморфного робота.

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

5. Блок управления приводами антропоморфного манипулятора 5 управляет перемещением манипулятора антропоморфного робота в положение, соответствующее вектору целевых углов поворота манипулятора антропоморфного робота, поступающему на входы блока.

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

Подтверждение возможности получения технического результата приведено на фиг. 6, 7. Визуализация вычисленных положений манипуляторов антропоморфного робота при управлении с помощью прототипа, приведена на фиг. 6. Рассматривается случай, когда расстояние между центрами плечевых сочленений антропоморфного робота непропорционально меньше расстояния между центрами плечевых суставов оператора. Оператор держит руки перед собой, ладони соединены. Как следует из фиг. 6, статическое положение манипуляторов антропоморфного робота не адекватно положению рук оператора. Более подробное описание рассматриваемого случая приведено в работе [Сычков В.Б. Метод оценки эффективности копирующего управления манипуляторами антропоморфного робота // Системы управления, связи и безопасности. 2019. №3]. При возникновении подобной ситуации в процессе управления с помощью прототипа, возможно повреждение манипуляторов. Визуализация вычисленного положения манипуляторов антропоморфного робота для аналогичного положения рук оператора при управлении с помощью заявляемого изобретения приведено на фиг. 7. Таким образом, с помощью заявляемого изобретения удается достичь технического результата, не обеспечиваемого прототипом.

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

где - однородная матрица преобразования из j-й в i-ю систему координат,

- однородная матрица сложного преобразования между i-й и (i-1)-й системами координат,

- однородная матрица элементарного поворота вокруг оси Oz на угол

- однородная матрица перемещения по оси Oz на величину

- однородная матрица перемещения по оси Ох на величину

- однородная матрица элементарного поворота вокруг оси Ох на угол

- i-e элементы векторов

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

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

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

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

где - орт оси Ох,

- проекции радиус-вектора на оси Ох, Оу,

вычисляет угол γ' между радиус-вектором и плоскостью yOz по формуле:

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

вычисляет вектор получаемый вращением вектора вокруг вектора на угол γ' по формуле:

где - матрица поворота вокруг вектора на угол ρ, вычисляемая по формуле:

где r_x, r_y, r_z - проекции вектора на оси Ох, Oy и Oz, соответственно,

S(ρ) - синус угла ρ,

С(ρ) - косинус угла ρ,

V(ρ) = 1 - cos(ρ),

вычисляет углы Эйлера ϕ', , ψ' кисти оператора по формулам:

где , , , - вектор-столбцы матрицы

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

формирует на выходах набор обобщенных координат руки оператора, обобщенные координаты R₇, r₇, α, β, ϕ, , ψ манипулятора антропоморфного робота принимаются равными обобщенным координатам , , α', β', ϕ', , ψ' руки оператора:

вычисляет радиус-вектор центра схвата манипулятора антропоморфного робота по формулам:

O_7x, O_7y, O_7z - проекции радиус-вектора на оси Ox, Oy, Oz, соответственно,

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

где С(ρ) - косинус угла ρ,

S(ρ) - синус угла ρ,

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

λ - отношение, в котором точка K делит вектор

вычисляет угол γ между радиус-вектором и плоскостью yOz по формуле:

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

вычисляет вектор получаемый вращением вектора вокруг вектора на угол γ, по формуле:

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

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

где - однородная матрица преобразования из j-й в i-ю систему координат,

- однородная матрица сложного преобразования из i-й в (i-1)-ю систему координат,

T_z,θ(θ_i) - однородная матрица элементарного поворота вокруг оси Oz на угол

T_z,d(d_i) - однородная матрица перемещения по оси Oz на величину d_i,

T_x.a(a_i) ^_ однородная матрица перемещения по оси Ох на величину a_i,

T_x,α(α_i) - однородная матрица элементарного поворота вокруг оси Ох на угол α_i,

a_i, d_i, α_i, θ_i - i-e элементы векторов

где - радиус-вектор точки O_i в j-й системе координат,

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

- радиус-вектор точки O_i в нулевой системе координат,

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

вычисляет вектор целевых углов поворота манипуляторов антропоморфного робота по формулам:

где - ненормированное значение первого целевого угла поворота манипулятора антропоморфного робота,

θ_i - i-й целевой угол поворота манипулятора антропоморфного робота,

Θ₂, Θ₄, Θ₆ - константы, отдельно определяемые для каждого манипулятора антропоморфного робота,

формирует на выходах вектор целевых углов поворота манипуляторов антропоморфного робота.

Похожие патенты: