Способ косвенного измерения углов поворота в суставах руки оператора
Владельцы патента RU 2700118:
Федеральное государственное автономное образовательное учреждение высшего образования "Северо-Кавказский федеральный университет" (RU)
Изобретение относится к системам управления и может быть использовано при создании задающих устройств для систем копирующего управления, реализованных в виде экзоскелета с нежесткими креплениями к телу оператора. Предложен способ косвенного измерения углов поворота в суставах руки оператора, основанный на использовании для измерения экзоскелета с кинематической схемой, аналогичной кинематической схеме руки человека, звенья которого располагаются вдоль отделов руки оператора, закрепленного на теле оператора с помощью жестких и нежестких креплений, измерении углов поворота θ в кинематических парах экзоскелета. Дополнительно измеряют lB1-B2 - длину плеча оператора, lB2-B3 - длину предплечья оператора, рассчитывают векторы значений декартовых координат плечевого сустава В1; лучезапястного сустава В3, центра кисти оператора В4, локтевого сочленения экзоскелета С2 путем решения прямой задачи кинематики, рассчитывают значения проекций вектора B1B3, соединяющего плечевой и лучезапястный суставы оператора, рассчитывают расстояние между плечевым и лучезапястным суставами оператора lB1-B3, рассчитывают полупериметр p треугольника, образовываемого плечевым, локтевым и лучезапястным суставами оператора, рассчитывают расстояние lK2-B2 от центра окружности K2, образовываемой виртуальным вращением локтевого сустава оператора вокруг оси, проходящей через плечевой и лучезапястный суставы, и локтевым суставом оператора по формуле:
рассчитывают расстояние lB1-K2 между плечевым суставом и центром окружности K2 по формуле: рассчитывают координаты точки K2 по формуле:
где λ - отношение, в котором точка K2 делит отрезок B1B3, рассчитывают два варианта
и
вектора значений декартовых координат локтевого сустава путем решения системы уравнений:
где xK2, yK2, zK2 - декартовы координаты точки K2; An, Bn, Cn, Dn - коэффициенты плоскости, перпендикулярной отрезку В1В3 и проходящей через точку K2; xB1B3, yB1B3, zB1B3 - значения проекций вектора B1B3 на оси декартовой системы координат; xC2, yC2, zC2 - декартовы координаты сочленения С2; lB2-С2 - эффективная длина нежесткого крепления, значение которой подбирается из условия минимизации погрешности косвенного измерения углов поворота в суставах руки оператора, выбирают один из рассчитанных вариантов
и
вектора декартовых координат локтевого сустава из условия наименьшего расстояния до прошлого известного положения локтевого сустава
где n - номер текущей итерации косвенного измерения углов поворота в суставах руки оператора, формируют вектор углов поворота в суставах руки оператора путем решения обратной задачи кинематики для известных декартовых координат суставов руки оператора. Изобретение обеспечивает повышение точности измерения углов поворота в суставах руки оператора. 3 ил.
Изобретение относится к системам управления и может быть использовано при создании задающих устройств для систем копирующего управления, реализованных в виде экзоскелета с нежесткими креплениями к телу оператора.
Известен способ прямых измерений углов поворота оператора в отдельном суставе, используемый в устройстве, защищенном патентом US №4436099, кл. A61B 5/103, G01D 5/36, G01D 5/26, A61B 5/10, 1984 г. Суть способа заключается в измерении углов поворота в суставе оператора с помощью устройства, повторяющего кинематическую схему сустава. При движении оператора оси кинематических пар экзоскелета совпадают с осями сустава оператора, что обуславливает высокую точность совпадения углов поворота в суставах оператора и измеренных углов поворота в кинематических парах экзоскелета.
Признаком аналога, совпадающим с признаком заявляемого способа, является измерение углов поворота в кинематических парах экзоскелета.
Недостатком, препятствующим получению в этом аналоге технического результата, достигаемого в изобретении, является то обстоятельство, что для реализации данного способа необходимо использование экзоскелетных конструкций, сильно сковывающих движение оператора.
Известен также способ косвенного измерения углов поворота в суставах конечности оператора, используемый в устройстве, защищенном патентом US №4986280, кл. A61B 5/103, B25J 19/02, B25J 9/00, G01D 5/14, G01D 5/12, G05B 19/427, G06F 3/00, 1991 г. Суть способа заключается в том, что для измерения углов поворота в суставах конечности оператора используют экзоскелет с кинематической избыточностью. Измеряют антропометрические показатели оператора. Измеряют углы поворота в кинематических парах экзоскелета. Вычисляют углы поворота в суставах конечности оператора на основе геометрического решения обратной задачи кинематики.
Признаками аналога, совпадающими с признаками заявляемого способа, являются использование экзоскелета для измерений, измерение антропометрических показателей оператора, измерение углов поворота в кинематических парах экзоскелета, вычисление углов поворота в суставах конечности оператора на основе геометрического решения обратной задачи кинематики.
Недостатками, препятствующим достижению в этом способе технического результата, обеспечиваемого заявляемым способом, являются ограниченные функциональные возможности. В частности, данный способ плохо применим для измерения углов поворота в суставах такой кинематически сложной части тела оператора, как рука. Это обусловлено тем, что для реализации способа необходимо использование экзоскелета с высокой степенью кинематической избыточности, что сильно усложняет техническую реализацию подобного экзоскелета.
Наиболее близким по технической сущности к заявляемому способу (прототипом) является способ косвенного измерения углов поворота в суставах руки оператора, используемый в дистанционном манипуляторе, защищенном патентом RU №125508, кл. B25J 3/04, 2013 г. Этот способ заключается в том, что для измерения используют экзоскелет с кинематической схемой, аналогичной кинематической схеме руки человека, звенья которого располагаются вдоль отделов руки оператора, закрепленный на теле оператора с помощью жестких и нежестких креплений, измеряют углы поворота в кинематических парах экзоскелета, формируют вектор углов поворота в суставах оператора из измеренных значений соответствующих углов поворота в кинематических парах экзоскелета.
Признаками способа-прототипа, совпадающими с признаками заявляемого способа, являются использование для измерения углов поворота в суставах руки оператора экзоскелета с кинематической схемой, аналогичной кинематической схеме руки человека, звенья которого располагаются вдоль отделов руки оператора, закрепленного на теле оператора с помощью жестких и нежестких креплений, измерение углов поворота в кинематических парах экзоскелета.
Недостатком, препятствующим достижению в способе-прототипе технического результата, обеспечиваемого заявляемым способом, является низкая точность измерений углов поворота в суставах руки оператора. Она обусловлена тем обстоятельством, что в процессе движения руки оператора нарушается параллельность между звеньями экзоскелета и отделами руки оператора. Данный факт приводит к различиям между значениями углов поворота в суставах руки оператора и измеренными значениями углов поворота в кинематических парах экзоскелета.
Это обстоятельство вызывает существенную рассинхронизацию движений объекта копирующего управления и руки оператора при использовании способа-прототипа в задающем устройстве.
Техническим результатом данного изобретения, является повышение точности измерения углов поворота в суставах руки оператора.
Технический результат достигается тем, что в известном способе-прототипе косвенного измерения углов поворота в суставах руки-оператора, используемом в дистанционном манипуляторе, защищенном патентом RU №125508, кл. B25J 3/04, 2013 г., дополнительно измеряют антропометрические данные оператора, рассчитывают декартовы координаты его суставов, имеющих жесткое крепление с экзоскелетом, путем решения прямой задачи кинематики, рассчитывают декартовы координаты локтевого сустава оператора, формируют вектор углов поворота в суставах руки оператора путем решения обратной задачи кинематики.
Для достижения технического результата в известном способе косвенного измерения углов поворота в суставах руки оператора, основанном на использовании для измерения экзоскелета с кинематической схемой, аналогичной кинематической схеме руки человека, звенья которого располагаются вдоль отделов руки оператора, закрепленного на теле оператора с помощью жестких и нежестких креплений, измерении углов поворота в кинематических парах экзоскелета, дополнительно измеряют следующие антропометрические параметры: 
– длину плеча оператора, 
– длину предплечья оператора, рассчитывают векторы значений декартовых координат плечевого сустава 
, лучезапястного сустава 
, центра кисти оператора 
, локтевого сочленения экзоскелета 
путем решения прямой задачи кинематики, рассчитывают значения проекций вектора 
, соединяющего плечевой и лучезапястный суставы оператора, рассчитывают расстояние между плечевым и лучезапястным суставами оператора 
, рассчитывают полупериметр 
треугольника, образовываемого плечевым, локтевым и лучезапястным суставами оператора, рассчитывают расстояние 
от центра окружности 
, образовываемой виртуальным вращением локтевого сустава оператора вокруг оси, проходящей через плечевой и лучезапястный суставы, и локтевым суставом оператора по формуле:
,
рассчитывают расстояние 
между плечевым суставом и центром окружности по формуле:
,
рассчитывают координаты точки по формуле:
,
,
где 
– отношение, в котором точка делит отрезок ,
рассчитывают два варианта 
и 
вектора значений декартовых координат локтевого сустава, путем решения системы уравнений:
где 
, 
, 
– декартовы координаты точки ;
, 
, 
, 
– коэффициенты плоскости, перпендикулярной отрезку , и проходящей через точку ;
, 
, 
– значения проекций вектора на оси декартовой системы координат;
, 
, 
– декартовы координаты сочленения ;
– эффективная длина нежесткого крепления, значение которой подбирается из условия минимизации погрешности косвенного измерения углов поворота в суставах руки оператора,
выбирают один из рассчитанных вариантов и вектора декартовых координат локтевого сустава из условия наименьшего расстояния до прошлого известного положения локтевого сустава 
, где 
– номер текущей итерации косвенного измерения углов поворота в суставах руки оператора, формируют вектор углов поворота в суставах руки оператора путем решения обратной задачи кинематики для известных декартовых координат суставов руки оператора.
Сущность изобретения заключается в том, что на основе измеренных углов поворота во вращательных парах экзоскелета, известных конструктивных параметров экзоскелета и антропометрических данных оператора выполняется расчёт декартовых координат суставов оператора, на основании которых выполняется расчёт углов поворота в суставах оператора.
Сущность изобретения поясняется чертежами, на которых приведены:
- на фиг.1 – кинематическая схема руки оператора и экзоскелета, соединенных с помощью жестких и нежестких креплений;
- на фиг. 2 – варианты пространственного расположения локтевого сустава оператора, полученные с помощью заявляемого способа;
- на фиг. 3 – взаимное положение руки оператора и экзоскелета, иллюстрирующее преимущество предлагаемого способа над способом-прототипом.
Представленная на фиг. 1 кинематическая цепь 
моделирует руку оператора, а кинематическая цепь 
– экзоскелет, аналогичный используемому в способе-прототипе. Кинематические пары 
соответствуют плечевому, локтевому и лучезапястному суставам оператора. Т. к. экзоскелет имеет кинематическую схему, аналогичную руке оператора, обозначим кинематические пары 
как плечевое, локтевое и лучезапястное сочленения, соответственно. Рука оператора и экзоскелет соединены с помощью жестких креплений 
и 
между плечевыми и лучезапястными суставами, соответственно. Точка 
соответствует месту крепления экзоскелета к телу оператора, точка – месту присоединения жесткого крепления к кистевому отделу руки оператора, точка 
– месту присоединения жесткого крепления к кистевому звену экзоскелета. Нежесткое крепление 
между суставом 
и сочленением 
представляет собой ремень, связывающий руку оператора и экзоскелет. Данное крепление накладывает на движение экзоскелета условие подобия движению руки оператора.
Введя систему декартовых координат, связанную с точкой , на основе измеренных значений обобщённых координат экзоскелета, его конструктивных параметров, а также антропометрических данных оператора, путем решения прямой задачи кинематики могут быть найдены декартовы координаты точек , и , связанных с рукой оператора. Таким образом, расчёт декартовых координат указанных точек может быть выполнен с помощью известных способов и выходит за рамки заявляемого. Одной из задач, решаемых изобретением, является расчёт декартовых координат локтевого сустава , имеющего нежесткую сцепку с экзоскелетом. Наличие нежесткой сцепки не позволяет найти декартовы координаты локтевого сустава с помощью решения прямой задачи кинематики. Также, декартовы координаты локтевого сустава не могут быть найдены с помощью решения обратной задачи кинематики, вследствие наличия в общем случае бесконечного множества решений.
Входными данными, необходимыми для расчёта с помощью изобретения, являются следующие величины:
- длина плеча оператора ;
- длина предплечья оператора ;
- длина плеча экзоскелета 
;
- длина предплечья экзоскелета 
;
- эффективная длина 
крепления ;
- векторы декартовых координат суставов руки оператора и сочленений экзоскелета , , 
, 
, найденные с помощью решения прямой задачи кинематики.
Длины отделов руки оператора могут быть найдены с помощью антропометрических измерений. Длины звеньев экзоскелета относятся к его конструктивным параметрам и полагаются известными. Эффективная длина представляет собой среднее расстояние между локтевым суставом оператор и локтевым сочленением . Данное расстояние может быть подобрано экспериментально для конкретной реализации экзоскелета из условия минимизации погрешности расчёта декартовых координат локтевого сустава с помощью заявляемого способа. Векторы декартовых координат точек , , , могут быть найдены с помощью решения прямой задачи кинематики.
Расчёт декартовых координат локтевого сустава, соединенного нежестким креплением с эсзоскелетом, с помощью заявляемого способа заключается в следующем.
Необходимо найти расстояние между соседними суставами, декартовы координаты которых известны вследствие их жесткой связи с экзоскелетом:
,
где – расстояние между плечевым и лучезапястным суставами руки оператора;
, 
, 
– декартовы координаты плечевого сустава ;
, 
, 
– декартовы координаты лучезапястного сустава .
Для известных декартовых координат суставов и множество возможных положений неизвестного сустава представляет собой окружность, изображенную на фиг. 2, описываемую точкой при вращении вокруг прямой . Центр окружности может быть получен из треугольника 
путем проведения высоты из точки к стороне .
Длина высоты может быть найдена через площадь треугольника :
где – полупериметр треугольника ;
– длины высоты, проведенной в треугольнике из точки .
Длина отрезка 
может быть найдена по формуле:
где – длина отрезка .
Координаты точки могут быть найдены по формуле деления отрезка в заданном отношении:
где – отношение, в котором точка делит отрезок .
Обозначенная на фиг. 2 окружность возможных положений сустава может быть найдена как пересечение сферы с центром в точке и радиусом , и плоскости, перпендикулярной отрезку , и проходящей через точку :
где , , – декартовы координаты точки ;
, , , – коэффициенты плоскости, перпендикулярной отрезку , и проходящей через точку ;
, , – значения проекций вектора на оси декартовой системы координат.
Предлагаемый способ определения координат локтевого сустава основывается на предположении о том, что с некоторой погрешностью можно считать расстояние между сочленением и суставом постоянным. Тогда выбор положения точки на окружности, описывающей возможные положения локтевого сустава, может быть произведен из условия нахождения общих точек пересечения со сферой с центром в точке и радиусом (фиг. 2). Уравнение данной сферы имеет вид:
,
где , , – декартовы координаты сочленения ;
– эффективная длина нежесткого крепления.
Варианты положения 
и 
локтевого сустава могут быть найдены как точки пересечения описанных выше окружности и сферы.
Примем, что:
,
где , , , , – значения декартовых координат соответствующих векторов и точек.
Варианты положения и локтевого сустава могут быть найдены по формулам:
где 
– промежуточные величины, необходимые для расчета декартовых координат точек и ;
– индексы, принимающие значения, определяемые следующим правилом: 
равен номеру ненулевой компоненты вектора в выражении, 
– номерам оставшихся компонент. Данные динамически определяемые в процессе расчета индексы введены для того, чтобы гарантировать, что при расчёте величин 
, 
и 
не будет деления на ноль.
Выбор положения искомой точки из двух возможных вариантов и может выполняться из условия непрерывности перемещения сустава в процессе движения. Для этого в начале работы оператора с экзоскелетом необходимо выбрать одну из двух рассчитанных точек положения локтевого сустава, как наиболее соответствующую его реальному положению. Данный выбор можно осуществить вручную или с помощью постановки руки в некоторое заранее оговоренное положение, в котором точка может быть выбрана автоматически.
В процессе циклической работы алгоритма расчёта выбор одной из двух точек производится из условия минимума расстояния до точки положения локтевого сустава, найденной на прошлой итерации:
где 
– вектор декартовых координат сустава , вычисляемый на -й итерации;
– номер текущей итерации косвенного измерения углов поворота в суставах руки оператора;
– первый вариант вектора декартовых координат сустава , вычисляемый на -й итерации;
– второй вариант вектора декартовых координат сустава , вычисляемый на -й итерации;
– вектор декартовых координат сустава , вычисленный на
-й итерации;
В соответствии с предлагаемым способом осуществляют следующие действия над задающим устройством и другими материальными объектами.
1. Используют для измерения экзоскелет с кинематической схемой, аналогичной кинематической схеме руки человека, звенья которого располагаются вдоль отделов руки оператора, закрепленного на теле оператора с помощью жестких и нежестких креплений.
2. Измеряют углы поворота в кинематических парах экзоскелета.
3. Измеряют длину плеча оператора .
4. Измеряют длину предплечья оператора .
5. Рассчитывают векторы значений декартовых координат плечевого сустава , лучезапястного сустава , центра кисти оператора , локтевого сочленения экзоскелета путем решения прямой задачи кинематики, например, с помощью представления Денавита-Хартенберга [Denavit J., Hartenberg R. S. A Kinematic Notation for Lower-Pair Mechanisms Based on Matrices, J. Appl. Mech., 77, pp. 215–221, 1995].
6. Рассчитывают значения проекций вектора , соединяющего плечевой и лучезапястный сустав оператора по формуле:
.
7. Рассчитывают расстояние между плечевым и лучезапястным суставами оператора по формуле:
.
8. Рассчитывают полупериметр треугольника, образовываемого плечевым, локтевым и лучезапястным суставами оператора по формуле:
.
9. Рассчитывают расстояние от центра окружности , образовываемой виртуальным вращением локтевого сустава оператора вокруг оси, проходящей через плечевой и лучезапястный суставы и локтевым суставом оператора по формуле:
.
10. Рассчитывают расстояние между плечевым суставом и центром окружности по формуле:
.
11. Рассчитывают координаты точки по формуле:
,
,
где – отношение, в котором точка делит отрезок .
12. Рассчитывают два варианта и вектора значений декартовых координат локтевого сустава, не связанного с экзоскелетом жестким креплением путем решения системы уравнений:
где , , – декартовы координаты точки ;
, , , – коэффициенты плоскости, перпендикулярной отрезку , и проходящей через точку ;
, , – декартовы координаты точки ;
, , – декартовы координаты точки .
, , – декартовы координаты сочленения ;
– эффективная длина нежесткого крепления, подбираемая экспериментально исходя из критерия минимизации погрешности косвенного измерения углов поворота в суставах руки оператора.
Выбирают один из рассчитанных вариантов и вектора декартовых координат локтевого сустава из условия наименьшего расстояния до прошлого известного положения локтевого сустава , где – номер текущей итерации косвенного измерения углов поворота в суставах руки оператора.
13. Формируют вектор углов поворота в суставах руки оператора путем решения обратной задачи кинематики для известных декартовых координат суставов руки оператора, например, с помощью решения, изложенного в [Petrenko, V.I. Calculating rotation angles of the operator's arms based on generalized coordinates of the master device with following anthropomorphic manipulator in real time / V.I. Petrenko, F.B. Tebueva, V.B. Sychkov, V.O. Antonov, M.M. Gurchinsky // International Journal of Mechanical Engineering and Technology (IJMET). – 2018. – Vol. 9, Issue 7 (2018). – pp. 447-461].
Заявляемый способ, как и способ-прототип, обеспечивают измерение углов поворота в суставах руки оператора. Однако заявляемый способ позволяет достичь большей точности измерений при использовании того же устройства для проведения измерений. Погрешность способа-прототипа складывается из методической погрешности и погрешностей, обусловленных гибкостью креплений и руки оператора. Методическую погрешность способа-прототипа можно наблюдать на примере взаимного положения руки оператора и экзоскелета, изображенного на фиг. 3. Вследствие того, что в общем случае длины звеньев экзоскелета не совпадают с длинами отделов руки оператора, также отличаются углы поворота в кинематических парах экзоскелета и суставах оператора. Очевидно, что измеренный угол поворота в локтевом сочленении экзоскелета 
отличается от угла поворота в локтевом суставе 
. Предлагаемый способ позволяет практически полностью исключить методическую погрешность способа-прототипа с помощью расчета реального положения суставов руки оператора в декартовых координатах и нахождения углов поворота в суставах руки оператора путем решения обратной задачи кинематики.
Все необходимые действия для реализации заявляемого способа могут быть выполнены на основе тех же средств, что используются в способе-прототипе.
Способ косвенного измерения углов поворота в суставах руки оператора, основанный на использовании для измерения экзоскелета с кинематической схемой, аналогичной кинематической схеме руки человека, звенья которого располагаются вдоль отделов руки оператора, закрепленного на теле оператора с помощью жестких и нежестких креплений, измерении углов поворота θ в кинематических парах экзоскелета, отличающийся тем, что дополнительно измеряют lB1-B2 - длину плеча оператора, lB2-B3 - длину предплечья оператора, рассчитывают векторы значений декартовых координат плечевого сустава В1; лучезапястного сустава В3, центра кисти оператора В4, локтевого сочленения экзоскелета С2 путем решения прямой задачи кинематики, рассчитывают значения проекций вектора B1B3, соединяющего плечевой и лучезапястный суставы оператора, рассчитывают расстояние между плечевым и лучезапястным суставами оператора lB1-B3, рассчитывают полупериметр p треугольника, образовываемого плечевым, локтевым и лучезапястным суставами оператора, рассчитывают расстояние lK2-B2 от центра окружности K2, образовываемой виртуальным вращением локтевого сустава оператора вокруг оси, проходящей через плечевой и лучезапястный суставы, и локтевым суставом оператора по формуле:
рассчитывают расстояние lB1-K2 между плечевым суставом и центром окружности K2 по формуле:
рассчитывают координаты точки K2 по формуле:
где λ - отношение, в котором точка K2 делит отрезок B1B3, рассчитывают два варианта 
и 
вектора значений декартовых координат локтевого сустава, путем решения системы уравнений:
где xK2, yK2, zK2 - декартовы координаты точки K2;
An, Bn, Cn, Dn - коэффициенты плоскости, перпендикулярной отрезку В1В3, и проходящей через точку K2;
xB1B3, yB1B3, zB1B3 - значения проекций вектора B1B3 на оси декартовой системы координат;
xC2, yC2, zC2 - декартовы координаты сочленения С2;
lB2-С2 - эффективная длина нежесткого крепления, значение которой подбирается из условия минимизации погрешности косвенного измерения углов поворота в суставах руки оператора,
выбирают один из рассчитанных вариантов 
и 
вектора декартовых координат локтевого сустава из условия наименьшего расстояния до прошлого известного положения локтевого сустава 
где n - номер текущей итерации косвенного измерения углов поворота в суставах руки оператора, формируют вектор углов поворота в суставах руки оператора путем решения обратной задачи кинематики для известных декартовых координат суставов руки оператора.
