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

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

Info

Publication number
RU2725930C1
RU2725930C1 RU2019144628A RU2019144628A RU2725930C1 RU 2725930 C1 RU2725930 C1 RU 2725930C1 RU 2019144628 A RU2019144628 A RU 2019144628A RU 2019144628 A RU2019144628 A RU 2019144628A RU 2725930 C1 RU2725930 C1 RU 2725930C1
Authority
RU
Russia
Prior art keywords
operator
anthropomorphic robot
manipulator
center
vector
Prior art date
Application number
RU2019144628A
Other languages
English (en)
Inventor
Вячеслав Иванович Петренко
Фариза Биляловна Тебуева
Михаил Михайлович Гурчинский
Владимир Олегович Антонов
Игорь Владиславович Стручков
Сергей Сергеевич Рябцев
Елена Аркадьевна Чайка
Николай Юрьевич Унтевский
Original Assignee
федеральное государственное автономное образовательное учреждение высшего образования "Северо-Кавказский федеральный университет"
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by федеральное государственное автономное образовательное учреждение высшего образования "Северо-Кавказский федеральный университет" filed Critical федеральное государственное автономное образовательное учреждение высшего образования "Северо-Кавказский федеральный университет"
Priority to RU2019144628A priority Critical patent/RU2725930C1/ru
Application granted granted Critical
Publication of RU2725930C1 publication Critical patent/RU2725930C1/ru

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls

Abstract

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

Description

Изобретение относится к робототехнике и может быть использовано в системах копирующего управления манипуляторами антропоморфного робота, работающими в паре.
Известен дистанционный манипулятор, защищенный патентом RU №125508, кл. B25J 3/04, 2013 г., структура системы управления которого включает в себя блок механической системы задающего устройства, блок датчиков задающего устройства, блок управления приводами антропоморфного манипулятора. Эта структура позволяет организовать копирующее управления антропоморфным манипулятором.
Существенными признаками аналога, совпадающими с признаками заявляемого изобретения, являются блок механической системы задающего устройства, блок датчиков задающего устройства, блок управления приводами антропоморфного манипулятора.
Недостатком способа копирующего управления является низкая точность выполнения целевых операций, обусловленная тем, что в процессе движения руки оператора нарушается параллельность между звеньями экзоскелета и отделами руки оператора. Данный факт приводит к различиям между значениями углов поворота в суставах руки оператора и измеренными значениями углов поворота в кинематических парах экзоскелета.
Наиболее близким по технической сущности к предлагаемому изобретению (прототипом) является комплекс управления антропоморфным манипулятором (заявка на изобретение RU 2018146275 А), который включает в себя блок механической системы задающего устройства, блок датчиков задающего устройства, блок расчета углов поворота руки оператора, блок управления приводами антропоморфного манипулятора. Эта структура позволяет организовать копирующее управление антропоморфным манипулятором.
Существенными признаками прототипа, совпадающими с признаками заявляемого изобретения, являются блок механической системы задающего устройства, блок датчиков задающего устройства, блок расчета углов поворота руки оператора, блок управления приводами антропоморфного манипулятора.
Недостатком прототипа является низкая точность выполнения целевых операций при непропорциональности антропометрических параметров оператора системы копирующего управления и аналогичных параметров антропоморфного робота. Кроме того, при наличии двух манипуляторов прототип не обеспечивает соответствие относительного положения кистевых звеньев манипуляторов антропоморфного робота относительному положению кистей рук оператора. Данные недостатки обусловлены тем, что в качестве векторов целевых углов поворота манипуляторов антропоморфного робота используются векторы углов поворота в суставах рук оператора. Вследствие того, что в общем случае длины звеньев манипуляторов антропоморфного робота не пропорциональны длинам соответствующих отделов рук оператора, в процессе копирующего управления возможно возникновение различных нежелательных ситуаций. В частности, если при пропорциональности всех прочих звеньев звенья манипуляторов антропоморфного робота, соответствующие предплечьям рук оператора, непропорционально короче, при сведении оператором рук ладонь к ладони между кистевыми звеньями антропоморфных манипуляторов останется зазор, как это показано на фиг. 1. Приведенные недостатки в общем случае снижают точность, а в частных случаях делают невозможным выполнение целевых операций.
Техническим результатом изобретения является расширение функциональных возможностей систем копирующего управления манипуляторами антропоморфного робота с помощью задающего устройства в виде экзоскелета.
Технический результат достигается тем, что в комплекс копирующего управления, содержащий блок механической системы задающего устройства, блок датчиков задающего устройства, блок расчета углов поворота руки оператора, блок управления приводами антропоморфного манипулятора, в котором выходы блока механической системы задающего устройства соединены со входами блока датчиков задающего устройства, выходы которого соединены со входами блока расчета углов поворота руки оператора, а блок датчиков задающего устройства формирует вектор
Figure 00000001
углов поворота в кинематических парах задающего устройства, блок расчета углов поворота руки оператора вычисляет вектор
Figure 00000002
углов поворота в суставах руки оператора, блок управления приводами антропоморфного манипулятора управляет перемещением манипулятора антропоморфного робота в положение, соответствующее вектору
Figure 00000003
целевых углов поворота манипулятора антропоморфного робота, поступающему на входы блока, дополнительно введен блок расчета целевых углов поворота манипулятора антропоморфного робота, при этом выходы блока расчета углов поворота руки оператора соединены со входами блока расчета целевых углов поворота манипулятора антропоморфного робота, выходы которого соединены со входами блока управления приводами антропоморфного манипулятора, блок расчета целевых углов поворота манипулятора антропоморфного робота содержит блок памяти, блок расчета декартовых координат центров суставов руки оператора, блок расчета специализированных обобщенных координат руки оператора, блок расчета декартовых координат центров сочленений манипулятора антропоморфного робота, блок решения обратной задачи кинематики манипулятора антропоморфного робота, входы блока расчета целевых углов поворота манипулятора антропоморфного робота соединены с первыми входами блока расчета декартовых координат центров суставов руки оператора, выходы которого соединены с первыми входами блока расчета специализированных обобщенных координат руки оператора, выходы которого соединены с первыми входами блока расчета декартовых координат центров сочленений манипулятора антропоморфного робота, выходы которого соединены с первыми входами блока решения обратной задачи кинематики манипулятора антропоморфного робота, выходы которого соединены с выходами блока расчета целевых углов поворота манипулятора антропоморфного робота, первые выходы блока памяти соединены со вторыми входами блока расчета декартовых координат центров суставов руки оператора, вторые выходы блока памяти соединены со вторым входами блока расчета специализированных обобщенных координат руки оператора, третьи выходы блока памяти соединены со вторым входами блока расчета декартовых координат центров сочленений манипулятора антропоморфного робота, четвертые выходы блока памяти соединены со вторым входами блока решения обратной задачи кинематики манипулятора антропоморфного робота, блок памяти хранит значения расстояния
Figure 00000004
от центра плечевого сустава руки оператора до центра его локтевого сустава, расстояния
Figure 00000005
от центра локтевого сустава руки оператора до центра его лучезапястного сустава, расстояния
Figure 00000006
от центра лучезапястного сустава руки оператора до центра его кисти, расстояния m' между центрами плечевых суставов оператора, векторов
Figure 00000007
параметров Денавита-Хартенберга, описывающих кинематическую структуру руки оператора, расстояния
Figure 00000008
от центра плечевого сочленения манипулятора антропоморфного робота до центра его локтевого сочленения, расстояния
Figure 00000009
от центра локтевого сочленения манипулятора антропоморфного робота до центра его лучезапястного сочленения, расстояния
Figure 00000010
от центра лучезапястного сочленения манипулятора антропоморфного робота до центра его схвата, расстояния m между центрами плечевых сочленений манипуляторов антропоморфного робота, векторов
Figure 00000011
параметров Денавита-Хартенберга, описывающих кинематическую структуру манипулятора антропоморфного робота, блок памяти также формирует на первых выходах набор
Figure 00000012
параметров Денавита-Хартенберга, описывающих кинематическую структуру руки оператора, формирует на вторых выходах набор
Figure 00000013
антропометрических параметров оператора, формирует на третьих выходах набор
Figure 00000014
конструктивных параметров антропоморфного робота, формирует на четвертых выходах набор
Figure 00000015
параметров Денавита-Хартенберга, описывающих кинематическую структуру манипулятора антропоморфного робота, блок расчета декартовых координат центров суставов руки оператора вычисляет матрицы однородного преобразования
Figure 00000016
для руки оператора согласно представлению Денавита-Хартенберга по формулам:
Figure 00000017
Figure 00000018
Figure 00000019
Figure 00000020
Figure 00000021
Figure 00000022
где
Figure 00000023
- однородная матрица преобразования из j-й в i-ю систему координат,
Figure 00000024
- однородная матрица сложного преобразования между i-й и (i-1)-й системами координат,
Figure 00000025
- однородная матрица элементарного поворота вокруг оси Oz на угол
Figure 00000026
,
Figure 00000027
- однородная матрица перемещения по оси Oz на величину
Figure 00000028
,
Figure 00000029
- однородная матрица перемещения по оси Ох на величину
Figure 00000030
,
Figure 00000031
- однородная матрица элементарного поворота вокруг оси Ох на угол
Figure 00000032
,
Figure 00000033
- i-e элементы векторов
Figure 00000034
,
вычисляет радиус-вектор
Figure 00000035
центра локтевого сустава руки оператора по формуле:
Figure 00000036
вычисляет радиус-вектор
Figure 00000037
центра лучезапястного сустава руки оператора по формуле:
Figure 00000038
вычисляет радиус-вектор
Figure 00000039
центра кисти руки оператора по формуле:
Figure 00000040
формирует на выходах набор
Figure 00000041
радиус-векторов центров суставов руки оператора, блок расчета специализированных обобщенных координат руки оператора вычисляет максимальную длину
Figure 00000042
вытянутой руки оператора по формуле:
Figure 00000043
вычисляет значение относительного расстояния
Figure 00000044
от центра кисти руки оператора до центра плечевого сустава этой же руки по формуле:
Figure 00000045
вычисляет значение относительного расстояния
Figure 00000046
от центра кисти оператора до центра плечевого сустава второй руки оператора по формуле:
Figure 00000047
вычисляет угол α' между плоскостью, образованной центром кисти руки оператора и центрами его плечевых суставов, и фронтальной плоскостью оператора по формуле:
Figure 00000048
где
Figure 00000049
- орт оси Ох,
Figure 00000050
- проекции радиус-вектора
Figure 00000051
на оси Ох, Oy,
вычисляет угол γ' между радиус-вектором
Figure 00000052
и плоскостью yOz по формуле:
Figure 00000053
вычисляет векторное произведение
Figure 00000054
векторов
Figure 00000055
и
Figure 00000056
по формуле:
Figure 00000057
вычисляет вектор
Figure 00000058
получаемый вращением вектора
Figure 00000059
вокруг вектора
Figure 00000060
на угол γ' по формуле:
Figure 00000061
где
Figure 00000062
- матрица поворота вокруг вектора
Figure 00000063
на угол ρ, вычисляемая по формуле:
Figure 00000064
где rx, ry, rz - проекции вектора
Figure 00000065
на оси Ох, Oy и Oz, соответственно,
S(ρ) - синус угла ρ,
С(ρ) - косинус угла ρ,
V(ρ)=1-cos(ρ),
вычисляет радиус-вектор
Figure 00000066
центра окружности, которую описывает центр локтевого сустава при вращении вокруг оси, проходящей через центры плечевого и лучезапястного суставов руки оператора по формуле:
Figure 00000067
вычисляет вектор
Figure 00000068
соединяющий конец радиус-вектора
Figure 00000069
и конец радиус-вектора
Figure 00000070
центра локтевого сустава, по формуле:
Figure 00000071
вычисляет угол β' между плоскостью, образованной центрами плечевого, локтевого и лучезапястного суставов руки оператора, и вертикальной плоскостью, проходящей через центры плечевого и лучезапястного суставов руки оператора по формуле:
Figure 00000072
вычисляет углы Эйлера ϕ', ϑ', ψ' кисти оператора по формулам:
Figure 00000073
Figure 00000074
Figure 00000075
Figure 00000076
где
Figure 00000077
- вектор-столбцы матрицы
Figure 00000078
ATAN2 (x; y) - функция арктангенса, учитывающая квадрант вычисляемого угла,
формирует на выходах набор
Figure 00000079
специализированных обобщенных координат руки оператора, специализированные обобщенные координаты R7, r7, α, β, ϕ, ϑ, ψ манипулятора антропоморфного робота принимаются равными специализированным обобщенным координатам
Figure 00000080
,
Figure 00000081
, α', β', ϕ', ϑ', ψ' руки оператора:
Figure 00000082
блок расчета декартовых координат центров сочленений манипулятора антропоморфного робота вычисляет значение максимального расстояния rm от центра плечевого сочленения до центра схвата манипулятора антропоморфного робота по формуле:
Figure 00000083
вычисляет радиус-вектор
Figure 00000084
центра схвата манипулятора антропоморфного робота по формулам:
Figure 00000085
Figure 00000086
Figure 00000087
Figure 00000088
Figure 00000089
Figure 00000090
где
Figure 00000091
- расстояние от центра схвата манипулятора антропоморфного робота до центра плечевого сочленения второго манипулятора антропоморфного робота,
O7x, O7y, O7z - проекции радиус-вектора
Figure 00000092
на оси Ox, Oy, Oz, соответственно,
вычисляет радиус-вектор
Figure 00000093
центра лучезапястного сочленения манипулятора антропоморфного робота по формулам:
Figure 00000094
Figure 00000095
где С(ρ) - косинус угла ρ,
S(ρ) - синус угла ρ,
вычисляет вектор нормали
Figure 00000096
к плоскости схвата манипулятора антропоморфного робота по формуле:
Figure 00000097
вычисляет радиус-вектор
Figure 00000098
центра вращения центра локтевого сочленения манипулятора вокруг оси, проходящей через центр плечевого сочленения и центр лучезапястного сочленения манипулятора антропоморфного робота по формулам:
Figure 00000099
Figure 00000100
Figure 00000101
Figure 00000102
Figure 00000103
Figure 00000104
где р - полупериметр треугольника, образованного центрами плечевого, локтевого и лучезапястного сочленений манипулятора антропоморфного робота,
λ - отношение, в котором точка K делит вектор
Figure 00000105
вычисляет угол γ между радиус-вектором
Figure 00000106
и плоскостью yOz по формуле:
Figure 00000107
вычисляет векторное произведение
Figure 00000108
векторов
Figure 00000109
и
Figure 00000110
по формуле:
Figure 00000111
вычисляет вектор
Figure 00000112
получаемый вращением вектора
Figure 00000113
вокруг вектора
Figure 00000114
на угол γ, по формуле:
Figure 00000115
вычисляет радиус-вектор
Figure 00000116
по формуле:
Figure 00000117
формирует на выходах набор
Figure 00000118
декартовых координат центров сочленений манипулятора антропоморфного робота, блок решения обратной задачи кинематики манипулятора антропоморфного робота вычисляет матрицы 0Т1, 0Т2, 0Т4, 0Т5, 0Т6 однородного преобразования для манипулятора антропоморфного робота согласно представлению Денавита-Хартенберга по формулам:
Figure 00000119
Figure 00000120
Figure 00000121
Figure 00000122
Figure 00000123
Figure 00000124
где iTj - однородная матрица преобразования из j-й в i-ю систему координат,
i-1Aj - однородная матрица сложного преобразования из t-й в (i-1)-ю систему координат,
Tz,θ(θi) - однородная матрица элементарного поворота вокруг оси Oz на угол θi,
Tz,d(di) - однородная матрица перемещения по оси Oz на величину di,
Tx,a(ai) - однородная матрица перемещения по оси Ох на величину ai,
Tx,α(αi) - однородная матрица элементарного поворота вокруг оси Ох на угол αi,
ai, di, αi, θi - i-e элементы векторов
Figure 00000125
вычисляет координаты узловых точек манипулятора антропоморфного робота в необходимых системах координат по формулам:
Figure 00000126
Figure 00000127
Figure 00000128
Figure 00000129
Figure 00000130
Figure 00000131
где
Figure 00000132
- радиус-вектор точки Oi в j-й системе координат,
0Ti - матрица преобразования из i-й системы координат в нулевую систему координат,
Figure 00000133
- радиус-вектор точки Oi в нулевой системе координат,
Figure 00000134
- координаты вектора нормали
Figure 00000135
в i-й системе координат,
вычисляет вектор
Figure 00000136
целевых углов поворота манипуляторов антропоморфного робота по формулам:
Figure 00000137
Figure 00000138
Figure 00000139
Figure 00000140
Figure 00000141
Figure 00000142
Figure 00000143
Figure 00000144
Figure 00000145
где
Figure 00000146
- ненормированное значение первого целевого угла поворота манипулятора антропоморфного робота,
θi - i-й целевой угол поворота манипулятора антропоморфного робота,
Θ2, Θ4, Θ6 - константы, отдельно определяемые для каждой модели манипулятора антропоморфного робота,
формирует на выходах вектор
Figure 00000147
целевых углов поворота манипуляторов антропоморфного робота.
Теоретическое доказательство наличия причинно-следственной связи между заявляемыми признаками и достигаемым техническим результатом заключается в следующем.
При реализации копирующего управления важной задачей является определение вектора
Figure 00000148
углов поворота исполнительного манипулятора на основе известного вектора
Figure 00000149
углов поворота в суставах руки оператора. В прототипе в качестве вектора
Figure 00000150
целевых углов поворота исполнительного манипулятора используется вектор
Figure 00000151
углов поворота в суставах руки оператора. В общем случае конструктивные параметры антропоморфного робота не пропорциональны аналогичным антропометрическим параметрам оператора. Данный факт обусловливает возникновение при использовании прототипа ситуаций, в которых комплекс копирующего управления функционирует некорректно. Если длины звеньев манипуляторов пропорциональны длинам соответствующих отделов руки оператора, но «ширина плеч» робота, т.е. расстояние между центрами плечевых сочленений, непропорционально больше расстояния между центрами плечевых суставов оператора, при использовании прототипа возникнет ситуация, изображенная на фиг. 1. За счет того, что «ширина плеч» робота больше, при сведении рук оператора ладонь к ладони между кистевыми звеньями манипуляторов останется зазор, т.е. становится невозможным сведение манипуляторов антропоморфного робота «ладонь к ладони». Таким образом, прототип работает корректно только при полной пропорциональности конструктивных параметров манипуляторов антропоморфного робота и антропометрических параметров рук оператора, что требует создания уникального антропоморфного робота под каждого оператора или реализации возможности подстройки конструктивных параметров антропоморфного робота, что значительно усложняет его конструкцию.
Для решения данной проблемы и расширения функциональных возможностей систем копирующего управления манипуляторами антропоморфного робота предлагается осуществлять копирование движения каждой руки оператора не по вектору углов поворота в ее суставах, а по специализированным обобщенным координатам, к которым относятся:
1. Относительное расстояние
Figure 00000152
от центра кисти руки оператора до центра плечевого сустава этой же руки, вычисляемое по формуле:
Figure 00000153
где
Figure 00000154
- расстояние от центра кисти оператора до центра плечевого сустава этой же руки,
Figure 00000155
- максимальное расстояние от центра плечевого сустава руки оператора до центра его кисти,
Figure 00000156
- расстояние от центра плечевого сустава руки оператора до центра его локтевого сустава,
Figure 00000157
- расстояние от центра локтевого сустава руки оператора до центра его лучезапястного сустава,
Figure 00000158
- расстояние от центра лучезапястного сустава руки оператора до центра его кисти.
2. Относительное расстояние
Figure 00000159
от центра кисти оператора до центра плечевого сустава второй руки оператора, вычисляемое по формуле:
Figure 00000160
где
Figure 00000161
- расстояние от центра кисти оператора до центра плечевого сустава второй руки оператора.
3. Угол α' между плоскостью, образованной центром кисти руки оператора и центрами его плечевых суставов, и фронтальной плоскостью оператора.
4. Угол β' между плоскостью, образованной центрами плечевого, локтевого и лучезапястного суставов руки оператора, и вертикальной плоскостью, проходящей через центры плечевого и лучезапястного суставов руки оператора.
5. Углы Эйлера ϕ', ϑ', ψ' кисти оператора.
Для реализации копирующего управления каждым манипулятором антропоморфного робота по специализированным обобщенным координатам необходимо измерение вектора
Figure 00000162
углов поворота в суставах руки оператора, расчет вектора
Figure 00000163
целевых углов поворота манипулятора, и перемещение манипулятора антропоморфного робота в состояние, соответствующее вектору
Figure 00000164
целевых углов поворота.
Измерение углов поворота в суставах руки оператора требует наличия задающего устройства, включающего блок механической системы задающего устройства, блок датчиков задающего устройства и блок расчета углов поворота руки оператора.
Расчет вектора
Figure 00000165
целевых углов поворота манипулятора заключается в вычислении такого вектора углов
Figure 00000166
, при котором значение специализированных обобщенных координат манипуляторов антропоморфного робота будет совпадать со значениями аналогичных специализированных обобщенных координат руки оператора. Для расчета вектора
Figure 00000167
целевых углов поворота требуется наличие специализированного блока расчета углов поворота манипулятора антропоморфного робота. Для расчета вектора
Figure 00000168
необходимо наличие следующих входных данных:
1. Расстояние
Figure 00000169
от центра плечевого сустава руки оператора до центра его локтевого сустава.
2. Расстояние
Figure 00000170
от центра локтевого сустава руки оператора до центра его лучезапястного сустава.
3. Расстояние
Figure 00000171
от центра лучезапястного сустава руки оператора до центра его кисти.
4. Расстояние m' между центрами плечевых суставов оператора.
5. Векторы
Figure 00000172
параметров Денавита-Хартенберга, описывающих кинематическую структуру руки оператора.
6. Расстояние
Figure 00000173
от центра плечевого сочленения манипулятора антропоморфного робота до центра его локтевого сочленения.
7. Расстояние
Figure 00000174
от центра локтевого сочленения манипулятора антропоморфного робота до центра его лучезапястного сочленения.
8. Расстояние
Figure 00000175
от центра лучезапястного сочленения манипулятора антропоморфного робота до центра его схвата.
9. Расстояние m между центрами плечевых сочленений манипуляторов антропоморфного робота.
10. Векторы
Figure 00000176
параметров Денавита-Хартенберга, описывающих кинематическую структуру манипулятора антропоморфного робота.
11. Вектор
Figure 00000177
углов поворота в суставах руки оператора.
Кинематическая схема руки оператора и манипулятора антропоморфного робота приведены на фиг. 2. Системы координат, введенные в соответствии с представлением Денавита-Хартенберга для кинематической схемы, представленной на фиг. 2, представлены на фиг. 3. Центр плечевого сочленения, центр локтевого сочленения, центр лучезапястного сочленения и центр схвата манипулятора антропоморфного робота соответствуют точкам O0, O3, O6, O7 на фиг. 3. Центры суставов руки оператора расположены аналогично центрам сочленений манипулятора антропоморфного робота.
Расчет вектора
Figure 00000178
целевых углов поворота манипулятора антропоморфного робота включает в себя четыре этапа:
1. Расчет декартовых координат центров суставов руки оператора.
2. Расчет специализированных обобщенных координат руки оператора.
3. Расчет декартовых координат сочленения манипулятора антропоморфного робота.
4. Решение обратной задачи кинематики манипулятора антропоморфного робота.
Расчет декартовых координат суставов руки оператора включает в себя следующие действия.
Формирование матриц однородного преобразования для руки оператора согласно представлению Денавита-Хартенберга по формулам:
Figure 00000179
Figure 00000180
Figure 00000181
Figure 00000182
Figure 00000183
Figure 00000184
где
Figure 00000185
- однородная матрица преобразования из j-й в i-ю систему координат (фиг. 3),
Figure 00000186
- однородная матрица сложного преобразования между i-й и (i-1)-й системами координат,
Figure 00000187
- однородная матрица элементарного поворота вокруг оси Oz на угол
Figure 00000188
,
Figure 00000189
- однородная матрица перемещения по оси Oz на величину
Figure 00000190
,
Figure 00000191
- однородная матрица перемещения по оси Ох на величину
Figure 00000192
,
Figure 00000193
- однородная матрица элементарного поворота вокруг оси Ох на угол
Figure 00000194
,
Figure 00000195
- i-e элементы векторов
Figure 00000196
Расчет радиус-вектора
Figure 00000197
центра локтевого сустава руки оператора по формуле:
Figure 00000198
Расчет радиус-вектора
Figure 00000199
центра лучезапястного сустава руки оператора по формуле:
Figure 00000200
Расчет радиус-вектора
Figure 00000201
центра кисти руки оператора по формуле:
Figure 00000202
Расчет специализированных обобщенных координат руки оператора включает в себя следующие действия.
Расчет максимальной длины
Figure 00000203
вытянутой руки оператора по формуле:
Figure 00000204
Расчет значения относительного расстояния
Figure 00000205
от центра кисти руки оператора до центра плечевого сустава этой же руки по формуле:
Figure 00000206
Расчет значения относительного расстояния
Figure 00000207
от центра кисти оператора до центра плечевого сустава второй руки оператора по формуле:
Figure 00000208
Расчет угла α' между плоскостью, образованной центром кисти руки оператора и центрами его плечевых суставов, и фронтальной плоскостью оператора по формуле:
Figure 00000209
где
Figure 00000210
- орт оси Ох,
Figure 00000211
- проекции радиус-вектора
Figure 00000212
на оси Ох, Oy.
Расчет угла γ' между радиус-вектором
Figure 00000213
и плоскостью yOz по формуле:
Figure 00000214
Расчет векторного произведения
Figure 00000215
векторов
Figure 00000216
и
Figure 00000217
по формуле:
Figure 00000218
Расчет вектора
Figure 00000219
получаемого вращением вектора
Figure 00000220
вокруг вектора
Figure 00000221
на угол γ' по формуле:
Figure 00000222
где
Figure 00000223
- матрица поворота вокруг вектора
Figure 00000224
на угол ρ, вычисляемая по формуле:
Figure 00000225
где rx, ry, rz - проекции вектора
Figure 00000226
на оси Ох, Oy и Oz, соответственно,
S(ρ) - синус угла ρ,
С(ρ) - косинус угла ρ,
V(ρ)=1-cos(ρ).
Расчет радиус-вектора
Figure 00000227
центра окружности, которую описывает центр локтевого сустава при вращении вокруг оси, проходящей через центры плечевого и лучезапястного суставов руки оператора по формуле:
Figure 00000228
Расчет вектора
Figure 00000229
соединяющего конец радиус-вектора
Figure 00000230
и конец радиус-вектор
Figure 00000231
центра локтевого сустава, по формуле:
Figure 00000232
Расчет угла β' между плоскостью, образованной центрами плечевого, локтевого и лучезапястного суставов руки оператора, и вертикальной плоскостью, проходящей через центры плечевого и лучезапястного суставов руки оператора по формуле:
Figure 00000233
Расчет углов Эйлера ϕ', ϑ', ψ' кисти оператора по формулам:
Figure 00000234
Figure 00000235
Figure 00000236
Figure 00000237
где
Figure 00000238
- вектор-столбцы матрицы
Figure 00000239
ATAN2 (x; y) - функция арктангенса, учитывающая квадрант вычисляемого угла.
Специализированные обобщенные координаты манипулятора антропоморфного робота принимаются равными специализированным обобщенным координатам руки оператора:
Figure 00000240
Расчет декартовых координат центров сочленений манипулятора антропоморфного робота включает в себя следующие действия.
Расчет значения максимального расстояния rm от центра плечевого сочленения до центра схвата манипулятора антропоморфного робота по формуле:
Figure 00000241
Расчет радиус-вектора
Figure 00000242
центра схвата манипулятора антропоморфного робота по формулам:
Figure 00000243
Figure 00000244
Figure 00000245
Figure 00000246
Figure 00000247
Figure 00000248
где
Figure 00000249
- расстояние от центра схвата манипулятора антропоморфного робота до центра плечевого сочленения второго манипулятора антропоморфного робота,
O7x, O7y, O7z - проекции радиус-вектора
Figure 00000250
на оси Ох, Oy, Oz, соответственно.
Расчет радиус-вектора
Figure 00000251
центра лучезапястного сочленения манипулятора антропоморфного робота по формулам:
Figure 00000252
Figure 00000253
где С(ρ) - косинус угла ρ,
S(ρ) - синус угла ρ.
Расчет вектора нормали
Figure 00000254
к плоскости схвата манипулятора антропоморфного робота по формуле:
Figure 00000255
Расчет радиус-вектора
Figure 00000256
центра вращения центра локтевого сочленения манипулятора вокруг оси, проходящей через центр плечевого сочленения и центр лучезапястного сочленения манипулятора антропоморфного робота по формулам:
Figure 00000257
Figure 00000258
Figure 00000259
Figure 00000260
Figure 00000261
Figure 00000262
где p - полупериметр треугольника, образованного центрами плечевого, локтевого и лучезапястного сочленений манипулятора антропоморфного робота,
λ - отношение, в котором точка K делит вектор
Figure 00000263
Расчет угла γ между радиус-вектором
Figure 00000264
и плоскостью yOz по формуле:
Figure 00000265
Расчет векторного произведения
Figure 00000266
векторов
Figure 00000267
и
Figure 00000268
по формуле:
Figure 00000269
Расчет вектора
Figure 00000270
получаемого вращением вектора
Figure 00000271
вокруг вектора
Figure 00000272
на угол γ по формуле:
Figure 00000273
Расчет радиус-вектора
Figure 00000274
по формуле:
Figure 00000275
Решение обратной задачи кинематики манипулятора антропоморфного робота включает в себя следующие действия.
Формирование матриц однородного преобразования для манипулятора антропоморфного робота согласно представлению Денавита-Хартенберга по формулам:
Figure 00000276
Figure 00000277
Figure 00000278
Figure 00000279
Figure 00000280
Figure 00000281
где iTj - однородная матрица преобразования из j-й в i-ю систему координат,
i-1Ai - однородная матрица сложного преобразования из i-й в (i-1)-ю систему координат,
Tz,θ(θi) - однородная матрица элементарного поворота вокруг оси Oz на угол θi,
Tz,d(di) - однородная матрица перемещения по оси Oz на величину di,
Tx,a(ai) - однородная матрица перемещения по оси Ох на величину ai,
Tx,α(αi) - однородная матрица элементарного поворота вокруг оси Ох на угол αi,
ai, di, αi, θi - i-e элементы векторов
Figure 00000282
Расчет декартовых координаты различных точек в необходимых системах координат:
Figure 00000283
Figure 00000284
Figure 00000285
Figure 00000286
Figure 00000287
Figure 00000288
где
Figure 00000289
- радиус-вектор точки 0i в j-й системе координат,
0Ti - матрица преобразования из i-й системы координат в нулевую систему координат,
Figure 00000290
- радиус-вектор точки Oi в нулевой системе координат,
Figure 00000291
- координаты вектора нормали
Figure 00000292
в i-й системе координат.
Расчет вектора
Figure 00000293
целевых углов поворота манипуляторов антропоморфного робота по формулам:
Figure 00000294
Figure 00000295
Figure 00000296
Figure 00000297
Figure 00000298
Figure 00000299
Figure 00000300
Figure 00000301
Figure 00000302
где
Figure 00000303
- ненормированное значение первого целевого угла поворота манипулятора антропоморфного робота,
ATAN2 (x; y) - функция арктангенса, учитывающая квадрант вычисляемого угла,
θi - i-й целевой угол поворота манипулятора антропоморфного робота,
Θ2, Θ4, Θ6 - константы, отдельно определяемые для каждой модели манипулятора антропоморфного робота.
Таким образом, для реализации расчета вектора
Figure 00000304
целевых углов поворота манипулятора антропоморфного робота на вход блока расчета целевых углов поворота манипулятора антропоморфного робота должен поступать вектор
Figure 00000305
углов поворота руки оператора, блок должен включать в себя блок памяти для хранения параметров
Figure 00000306
блок расчета декартовых координат центров суставов руки оператора, блок расчета специализированных обобщенных координат руки оператора, блок расчета декартовых координат центров сочленений манипулятора антропоморфного робота, блок решения обратной задачи кинематики манипулятора антропоморфного робота.
Перемещение манипулятора антропоморфного робота в состояние, соответствующее вектору
Figure 00000307
целевых углов поворота манипулятора антропоморфного робота требует наличия блока управления приводами.
Таким образом, копирующее управления по специализированным обобщенным координатам с одной стороны позволяет достичь заявленного технического результата, а с другой - требует применения заявленных признаков, что обусловливает причинно-следственную связь между заявляемыми признаками и достигаемым техническим результатом.
На фиг. 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 приведена визуализация вычисленных положений манипуляторов антропоморфного робота, расстояние между плечевыми сочленениями которого непропорционально уже расстояния между плечевыми суставами оператора для положения рук оператора, когда ладони соединены перед собой, при управлении с помощью способа-прототипа. Центры плечевого, локтевого, лучезапястного сочленений и схвата левого манипуляторы обозначены как O0L, O3L, O6L, O7L соответственно. Аналогичные точки правого манипулятора обозначены как O0R, O3R, O6R, O7R.
На фиг. 7 приведена визуализация вычисленных положений манипуляторов антропоморфного робота, расстояние между плечевыми сочленениями которого непропорционально уже расстояния между плечевыми суставами оператора для положения рук оператора, когда ладони соединены перед собой, при управлении с помощью заявляемого способа. Центры плечевого, локтевого, лучезапястного сочленений и схвата левого манипуляторы обозначены как O0L, O3L, O6L, O7L соответственно. Аналогичные точки правого манипулятора обозначены как O0R, O3R, O6R, O7R.
Принцип работы комплекса копирующего управления манипуляторами антропоморфного робота заключается в следующем.
1. Оператор приводит в движение блок механической системы задающего устройства 1.
2. Блок датчиков задающего устройства 2 измеряет вектор
Figure 00000308
углов поворота в кинематических парах задающего устройства.
3. Блок расчета углов поворота руки оператора 3 рассчитывает вектор
Figure 00000309
углов поворота руки оператора.
4. Блок расчета целевых углов поворота манипулятора антропоморфного робота 4 рассчитывает вектор
Figure 00000310
целевых углов поворота манипулятора антропоморфного робота на основе вектора
Figure 00000311
углов поворота руки оператора, антропометрических параметров оператора и конструктивных параметров манипулятора антропоморфного робота следующим образом.
4.1. Блок расчета декартовых координат центров суставов руки оператора 6 вычисляет набор
Figure 00000312
радиус-векторов центров суставов руки оператора на основе вектора
Figure 00000313
углов поворота и параметров Денавита-Хартенберга руки оператора.
4.2. Блок расчета специализированных обобщенных координат руки оператора 7 вычисляет набор
Figure 00000314
специализированных обобщенных координат руки оператора на основе набора
Figure 00000315
декартовых координат центров суставов руки оператора и антропометрических параметров оператора.
4.3. Блок расчета декартовых координат центров сочленений манипулятора антропоморфного робота 8 вычисляет набор
Figure 00000316
декартовых координат центров сочленений манипулятора антропоморфного робота на основе набора
Figure 00000317
специализированных обобщенных координат руки оператора и конструктивных параметров антропоморфного робота.
4.4. Блок решения обратной задачи кинематики манипулятора антропоморфного робота 9 вычисляет вектор
Figure 00000318
целевых углов поворота манипулятора антропоморфного робота на основе набора
Figure 00000319
декартовых координат центров сочленений и параметров Денавита-Хартенберга манипулятора антропоморфного робота.
5. Блок управления приводами антропоморфного манипулятора 5 управляет перемещением манипулятора антропоморфного робота в положение, соответствующее вектору
Figure 00000320
целевых углов поворота манипулятора антропоморфного робота, поступающему на входы блока.
Один из вариантов осуществления изобретения для копирующего управления манипуляторами антропоморфного робота, каждый из которых имеет кинематическую схему, подобную изображенной на фиг. 2, заключается в следующем. В качестве блока механической системы задающего устройства 1 и блока датчиков задающего устройства 2 могут использоваться существующие средства задающих устройств, реализованных в виде экзоскелета (например, дистанционный манипулятор, защищенный патентом RU №125508, кл. B25J 3/04, 2011 г.). В качестве блока управления приводами антропоморфного манипулятора 5 могут использоваться стандартные блоки управления антропоморфными манипуляторами. В качестве блока расчета углов поворота руки оператора 3 и блока расчета целевых углов поворота манипулятора антропоморфного робота 4 может использоваться специализированная ЭВМ или вычислительные системы задающего или исполнительного устройства.
Подтверждение возможности получения технического результата приведено на фиг. 6, 7. Визуализация вычисленных положений манипуляторов антропоморфного робота при управлении с помощью прототипа, приведена на фиг. 6. Рассматривается случай, когда расстояние между центрами плечевых сочленений антропоморфного робота непропорционально меньше расстояния между центрами плечевых суставов оператора. Оператор держит руки перед собой, ладони соединены. Как следует из фиг. 6, статическое положение манипуляторов антропоморфного робота не адекватно положению рук оператора. Более подробное описание рассматриваемого случая приведено в работе [Сычков В.Б. Метод оценки эффективности копирующего управления манипуляторами антропоморфного робота // Системы управления, связи и безопасности. 2019. №3]. При возникновении подобной ситуации в процессе управления с помощью прототипа, возможно повреждение манипуляторов. Визуализация вычисленного положения манипуляторов антропоморфного робота для аналогичного положения рук оператора при управлении с помощью заявляемого изобретения приведено на фиг. 7. Таким образом, с помощью заявляемого изобретения удается достичь технического результата, не обеспечиваемого прототипом.

Claims (98)

  1. Комплекс копирующего управления манипуляторами антропоморфного робота, содержащий блок механической системы задающего устройства, блок датчиков задающего устройства, блок расчета углов поворота руки оператора, блок управления приводами антропоморфного манипулятора, в котором выходы блока механической системы задающего устройства соединены со входами блока датчиков задающего устройства, выходы которого соединены со входами блока расчета углов поворота руки оператора, а блок датчиков задающего устройства формирует вектор
    Figure 00000321
    углов поворота в кинематических парах задающего устройства, блок расчета углов поворота руки оператора вычисляет вектор
    Figure 00000322
    углов поворота в суставах руки оператора, блок управления приводами антропоморфного манипулятора управляет перемещением манипулятора антропоморфного робота в положение, соответствующее вектору
    Figure 00000323
    целевых углов поворота манипулятора антропоморфного робота, поступающему на входы блока, отличающийся тем, что дополнительно введен блок расчета целевых углов поворота манипулятора антропоморфного робота, при этом выходы блока расчета углов поворота руки оператора соединены со входами блока расчета целевых углов поворота манипулятора антропоморфного робота, выходы которого соединены со входами блока управления приводами антропоморфного манипулятора, блок расчета целевых углов поворота манипулятора антропоморфного робота содержит блок памяти, блок расчета декартовых координат центров суставов руки оператора, блок расчета обобщенных координат руки оператора, блок расчета декартовых координат центров сочленений манипулятора антропоморфного робота, блок решения обратной задачи кинематики манипулятора антропоморфного робота, входы блока расчета целевых углов поворота манипулятора антропоморфного робота соединены с первыми входами блока расчета декартовых координат центров суставов руки оператора, выходы которого соединены с первыми входами блока расчета обобщенных координат руки оператора, выходы которого соединены с первыми входами блока расчета декартовых координат центров сочленений манипулятора антропоморфного робота, выходы которого соединены с первыми входами блока решения обратной задачи кинематики манипулятора антропоморфного робота, выходы которого соединены с выходами блока расчета целевых углов поворота манипулятора антропоморфного робота, первые выходы блока памяти соединены со вторыми входами блока расчета декартовых координат центров суставов руки оператора, вторые выходы блока памяти соединены со вторым входами блока расчета обобщенных координат руки оператора, третьи выходы блока памяти соединены со вторым входами блока расчета декартовых координат центров сочленений манипулятора антропоморфного робота, четвертые выходы блока памяти соединены со вторым входами блока решения обратной задачи кинематики манипулятора антропоморфного робота, блок памяти хранит значения расстояния
    Figure 00000324
    от центра плечевого сустава руки оператора до центра его локтевого сустава, расстояния
    Figure 00000325
    от центра локтевого сустава руки оператора до центра его лучезапястного сустава, расстояния
    Figure 00000326
    от центра лучезапястного сустава руки оператора до центра его кисти, расстояния т' между центрами плечевых суставов оператора, векторов
    Figure 00000327
    параметров Денавита-Хартенберга, описывающих кинематическую структуру руки оператора, расстояния
    Figure 00000328
    от центра плечевого сочленения манипулятора антропоморфного робота до центра его локтевого сочленения, расстояния
    Figure 00000329
    от центра локтевого сочленения манипулятора антропоморфного робота до центра его лучезапястного сочленения, расстояния
    Figure 00000330
    от центра лучезапястного сочленения манипулятора антропоморфного робота до центра его схвата, расстояния т между центрами плечевых сочленений манипуляторов антропоморфного робота, векторов
    Figure 00000331
    параметров Денавита-Хартенберга, описывающих кинематическую структуру манипулятора антропоморфного робота, блок памяти также формирует на первых выходах набор
    Figure 00000332
    параметров Денавита-Хартенберга, описывающих кинематическую структуру руки оператора, формирует на вторых выходах набор
    Figure 00000333
    антропометрических параметров оператора, формирует на третьих выходах набор
    Figure 00000334
    конструктивных параметров антропоморфного робота, формирует на четвертых выходах набор
    Figure 00000335
    параметров Денавита-Хартенберга, описывающих кинематическую структуру манипулятора антропоморфного робота, блок расчета декартовых координат центров суставов руки оператора вычисляет матрицы однородного преобразования
    Figure 00000336
    для руки оператора согласно представлению Денавита-Хартенберга по формулам:
  2. Figure 00000337
  3. где
    Figure 00000338
    - однородная матрица преобразования из j-й в i-ю систему координат,
  4. Figure 00000339
    - однородная матрица сложного преобразования между i-й и (i-1)-й системами координат,
  5. Figure 00000340
    - однородная матрица элементарного поворота вокруг оси Oz на угол
    Figure 00000341
  6. Figure 00000342
    - однородная матрица перемещения по оси Oz на величину
    Figure 00000343
  7. Figure 00000344
    - однородная матрица перемещения по оси Ох на величину
    Figure 00000345
  8. Figure 00000346
    - однородная матрица элементарного поворота вокруг оси Ох на угол
    Figure 00000347
  9. Figure 00000348
    - i-e элементы векторов
    Figure 00000349
  10. вычисляет радиус-вектор
    Figure 00000350
    центра локтевого сустава руки оператора по формуле:
  11. Figure 00000351
  12. вычисляет радиус-вектор
    Figure 00000352
    центра лучезапястного сустава руки оператора по формуле:
  13. Figure 00000353
  14. вычисляет радиус-вектор
    Figure 00000354
    центра кисти руки оператора по формуле:
  15. Figure 00000355
  16. формирует на выходах набор
    Figure 00000356
    радиус-векторов центров суставов руки оператора, блок расчета обобщенных координат руки оператора вычисляет максимальную длину
    Figure 00000357
    вытянутой руки оператора по формуле:
  17. Figure 00000358
  18. вычисляет значение относительного расстояния
    Figure 00000359
    от центра кисти руки оператора до центра плечевого сустава этой же руки по формуле:
  19. Figure 00000360
  20. вычисляет значение относительного расстояния
    Figure 00000361
    от центра кисти оператора до центра плечевого сустава второй руки оператора по формуле:
  21. Figure 00000362
  22. вычисляет угол α' между плоскостью, образованной центром кисти руки оператора и центрами его плечевых суставов, и фронтальной плоскостью оператора по формуле:
  23. Figure 00000363
  24. где
    Figure 00000364
    - орт оси Ох,
  25. Figure 00000365
    - проекции радиус-вектора
    Figure 00000366
    на оси Ох, Оу,
  26. вычисляет угол γ' между радиус-вектором
    Figure 00000367
    и плоскостью yOz по формуле:
  27. Figure 00000368
  28. вычисляет векторное произведение
    Figure 00000369
    векторов
    Figure 00000370
    и
    Figure 00000371
    по формуле:
  29. Figure 00000372
  30. вычисляет вектор
    Figure 00000373
    получаемый вращением вектора
    Figure 00000370
    вокруг вектора
    Figure 00000374
    на угол γ' по формуле:
  31. Figure 00000375
  32. где
    Figure 00000376
    - матрица поворота вокруг вектора
    Figure 00000377
    на угол ρ, вычисляемая по формуле:
  33. Figure 00000378
  34. где rx, ry, rz - проекции вектора
    Figure 00000379
    на оси Ох, Oy и Oz, соответственно,
  35. S(ρ) - синус угла ρ,
  36. С(ρ) - косинус угла ρ,
  37. V(ρ) = 1 - cos(ρ),
  38. вычисляет радиус-вектор
    Figure 00000380
    центра окружности, которую описывает центр локтевого сустава при вращении вокруг оси, проходящей через центры плечевого и лучезапястного суставов руки оператора по формуле:
  39. Figure 00000381
  40. вычисляет вектор
    Figure 00000382
    соединяющий конец радиус-вектора
    Figure 00000383
    и конец радиус-вектора
    Figure 00000384
    центра локтевого сустава, по формуле:
  41. Figure 00000385
  42. вычисляет угол β' между плоскостью, образованной центрами плечевого, локтевого и лучезапястного суставов руки оператора, и вертикальной плоскостью, проходящей через центры плечевого и лучезапястного суставов руки оператора по формуле:
  43. Figure 00000386
  44. вычисляет углы Эйлера ϕ',
    Figure 00000387
    , ψ' кисти оператора по формулам:
  45. Figure 00000388
  46. Figure 00000389
  47. Figure 00000390
  48. Figure 00000391
  49. где
    Figure 00000392
    ,
    Figure 00000393
    ,
    Figure 00000394
    ,
    Figure 00000395
    - вектор-столбцы матрицы
    Figure 00000396
  50. ATAN2(x; у) - функция арктангенса, учитывающая квадрант вычисляемого угла,
  51. формирует на выходах набор
    Figure 00000397
    обобщенных координат руки оператора, обобщенные координаты R7, r7, α, β, ϕ,
    Figure 00000398
    , ψ манипулятора антропоморфного робота принимаются равными обобщенным координатам
    Figure 00000399
    ,
    Figure 00000400
    , α', β', ϕ',
    Figure 00000387
    , ψ' руки оператора:
  52. Figure 00000401
  53. блок расчета декартовых координат центров сочленений манипулятора антропоморфного робота вычисляет значение максимального расстояния rm от центра плечевого сочленения до центра схвата манипулятора антропоморфного робота по формуле:
  54. Figure 00000402
  55. вычисляет радиус-вектор
    Figure 00000403
    центра схвата манипулятора антропоморфного робота по формулам:
  56. Figure 00000404
  57. где
    Figure 00000405
    - расстояние от центра схвата манипулятора антропоморфного робота до центра плечевого сочленения второго манипулятора антропоморфного робота,
  58. O7x, O7y, O7z - проекции радиус-вектора
    Figure 00000406
    на оси Ox, Oy, Oz, соответственно,
  59. вычисляет радиус-вектор
    Figure 00000407
    центра лучезапястного сочленения манипулятора антропоморфного робота по формулам:
  60. Figure 00000408
  61. Figure 00000409
  62. где С(ρ) - косинус угла ρ,
  63. S(ρ) - синус угла ρ,
  64. вычисляет вектор нормали
    Figure 00000410
    к плоскости схвата манипулятора антропоморфного робота по формуле:
  65. Figure 00000411
  66. вычисляет радиус-вектор
    Figure 00000412
    центра вращения центра локтевого сочленения манипулятора вокруг оси, проходящей через центр плечевого сочленения и центр лучезапястного сочленения манипулятора антропоморфного робота по формулам:
  67. Figure 00000413
  68. где р - полупериметр треугольника, образованного центрами плечевого, локтевого и лучезапястного сочленений манипулятора антропоморфного робота,
  69. λ - отношение, в котором точка K делит вектор
    Figure 00000414
  70. вычисляет угол γ между радиус-вектором
    Figure 00000415
    и плоскостью yOz по формуле:
  71. Figure 00000416
  72. вычисляет векторное произведение
    Figure 00000417
    векторов
    Figure 00000418
    и
    Figure 00000419
    по формуле:
  73. Figure 00000420
  74. вычисляет вектор
    Figure 00000421
    получаемый вращением вектора
    Figure 00000422
    вокруг вектора
    Figure 00000423
    на угол γ, по формуле:
  75. Figure 00000424
  76. вычисляет радиус-вектор
    Figure 00000425
    по формуле:
  77. Figure 00000426
  78. формирует на выходах набор
    Figure 00000427
    декартовых координат центров сочленений манипулятора антропоморфного робота, блок решения обратной задачи кинематики манипулятора антропоморфного робота вычисляет матрицы
    Figure 00000428
    однородного преобразования для манипулятора антропоморфного робота согласно представлению Денавита-Хартенберга по формулам:
  79. Figure 00000429
  80. где
    Figure 00000430
    - однородная матрица преобразования из j-й в i-ю систему координат,
  81. Figure 00000431
    - однородная матрица сложного преобразования из i-й в (i-1)-ю систему координат,
  82. Tz,θ(θi) - однородная матрица элементарного поворота вокруг оси Oz на угол
  83. Tz,d(di) - однородная матрица перемещения по оси Oz на величину di,
  84. Tx.a(ai) _ однородная матрица перемещения по оси Ох на величину ai,
  85. Tx,α(αi) - однородная матрица элементарного поворота вокруг оси Ох на угол αi,
  86. ai, di, αi, θi - i-e элементы векторов
    Figure 00000432
  87. вычисляет координаты узловых точек манипулятора антропоморфного робота в необходимых системах координат по формулам:
  88. Figure 00000433
  89. где
    Figure 00000434
    - радиус-вектор точки Oi в j-й системе координат,
  90. Figure 00000435
    - матрица преобразования из i-й системы координат в нулевую систему координат,
  91. Figure 00000436
    - радиус-вектор точки Oi в нулевой системе координат,
  92. Figure 00000437
    - координаты вектора нормали
    Figure 00000438
    в i-й системе координат,
  93. вычисляет вектор
    Figure 00000439
    целевых углов поворота манипуляторов антропоморфного робота по формулам:
  94. Figure 00000440
  95. где
    Figure 00000441
    - ненормированное значение первого целевого угла поворота манипулятора антропоморфного робота,
  96. θi - i-й целевой угол поворота манипулятора антропоморфного робота,
  97. Θ2, Θ4, Θ6 - константы, отдельно определяемые для каждого манипулятора антропоморфного робота,
  98. формирует на выходах вектор
    Figure 00000439
    целевых углов поворота манипуляторов антропоморфного робота.
RU2019144628A 2019-12-27 2019-12-27 Комплекс копирующего управления манипуляторами антропоморфного робота RU2725930C1 (ru)

Priority Applications (1)

Application Number Priority Date Filing Date Title
RU2019144628A RU2725930C1 (ru) 2019-12-27 2019-12-27 Комплекс копирующего управления манипуляторами антропоморфного робота

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
RU2019144628A RU2725930C1 (ru) 2019-12-27 2019-12-27 Комплекс копирующего управления манипуляторами антропоморфного робота

Publications (1)

Publication Number Publication Date
RU2725930C1 true RU2725930C1 (ru) 2020-07-07

Family

ID=71509983

Family Applications (1)

Application Number Title Priority Date Filing Date
RU2019144628A RU2725930C1 (ru) 2019-12-27 2019-12-27 Комплекс копирующего управления манипуляторами антропоморфного робота

Country Status (1)

Country Link
RU (1) RU2725930C1 (ru)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4986280A (en) * 1988-07-20 1991-01-22 Arthur D. Little, Inc. Hand position/measurement control system
RU125508U1 (ru) * 2011-11-23 2013-03-10 Александр Фаритович Пермяков Дистанционный манипулятор
RU146552U1 (ru) * 2014-02-06 2014-10-10 Александр Фаритович Пермяков Антропоморфный манипулятор

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4986280A (en) * 1988-07-20 1991-01-22 Arthur D. Little, Inc. Hand position/measurement control system
RU125508U1 (ru) * 2011-11-23 2013-03-10 Александр Фаритович Пермяков Дистанционный манипулятор
RU146552U1 (ru) * 2014-02-06 2014-10-10 Александр Фаритович Пермяков Антропоморфный манипулятор

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
ЮРЕВИЧ Е.И. Управление роботами и робототехническими системами. - СПб: СПбГТУ, 2000. - 171 с. - С. 123-131. *

Similar Documents

Publication Publication Date Title
JP5444209B2 (ja) フレームマッピングおよびフォースフィードバックの方法、装置およびシステム
US9827675B2 (en) Collision avoidance method, control device, and program
JP3207728B2 (ja) 冗長マニピュレータの制御方法
JPH0820894B2 (ja) 産業用ロボツトの動作制御方法
JP2012096338A (ja) ロボット制御装置
US10335946B2 (en) Compositional impedance programming for robots
KR20170016631A (ko) 여자유도 로봇 제어 시스템, 방법, 및 상기 방법을 실행시키기 위한 컴퓨터 판독 가능한 프로그램을 기록한 기록 매체
Khatamian Solving kinematics problems of a 6-dof robot manipulator
JP4054984B2 (ja) ロボットの制御装置および制御方法
JP2019188514A (ja) ロボットを用いた負荷の重量及び重心位置を推定するための装置、方法及びプログラム
Trinh et al. A geometrical approach to the inverse kinematics of 6r serial robots with offset wrists
US20220009101A1 (en) Control device, control method, and non-transitory recording medium
Baron et al. The on-line direct kinematics of parallel manipulators under joint-sensor redundancy
Hanses et al. Hand-guiding robots along predefined geometric paths under hard joint constraints
RU2725930C1 (ru) Комплекс копирующего управления манипуляторами антропоморфного робота
Avizzano et al. An optimal geometric model for clavels delta robot
JPH05345291A (ja) ロボットの動作範囲制限方式
RU2724769C1 (ru) Способ копирующего управления манипуляторами антропоморфного робота
Gliesche et al. Commanding a whole-arm manipulation grasp configuration with one click: Interaction concept and analytic IK method
JP2011245614A5 (ru)
RU2710290C1 (ru) Комплекс управления антропоморфным манипулятором
Lai A fast task planning system for 6R articulated robots based on inverse kinematics
JPH05177563A (ja) マスタスレーブマニピュレータの制御方法
Petrenko et al. Mathematical method of mapping configuration space for manipulator master-slave teleoperation
Mihelj et al. yControl-open architecture controller for Yaskawa Motoman MH5 robot

Legal Events

Date Code Title Description
QB4A Licence on use of patent

Free format text: LICENCE FORMERLY AGREED ON 20210113

Effective date: 20210113