Планирование траектории манипулятора для движущейся цели

Галемов Руслан Тахирович аспирант, кафедра робототехники и технической кибернетики, Сибирский Федеральный Университет 660041, Россия, Красноярский край, г. Красноярск, проспект Свободный, 79 Galemov Ruslan Takhirovich graduate student, Department of Robotics and Technical Cybernetics, Siberian Federal University 660041, Russia, Krasnoyarskii krai, g. Krasnoyarsk, prospekt Svobodnyi, 79 galemovruslan@gmail.com Другие публикации этого автора

Масальский Геннадий Борисович кандидат технических наук профессор, кафедра робототехники и технической кибернетики, Сибирский федеральный университет 660041, Россия, Красноярский край, г. Красноярск, проспект Свободный, 79 Masalsky Gennadiy Borisovich PhD in Technical Science Professor, Department of Robotics and Technical Cybernetics, Siberian Federal University 660041, Russia, Krasnoyarskii krai, g. Krasnoyarsk, prospekt Svobodnyi, 79 gmasalsky@sfu-kras.ru

Введение

Можно выделить три подхода к решению задачи слежения рабочим органом манипулятора за движущейся целью: построение траектории через инвертированный Якобиан, планирование маршрута в пространстве состояний и интеллектуальные подходы к решению обратной задачи кинематики. Метод инвертированного Якобиана является одним из самых распространенных способов решения обратной задачи кинематики (ОЗК) манипулятора ^[1],[2]. Он используется в системах управления, основанных на отслеживании цели средствами технического зрения ^[3]-^[10].

Планирование маршрута в пространстве состояний использует алгоритмы поиска пути. Здесь широко применяется метод быстрорастущих случайных деревьев ^[11]. Один из подходов к решению задачи построения траектории заключается в нахождении целевых конфигураций манипулятора с помощью грубого решения ОЗК и построения пути от начальной до целевой конфигурации, используя быстрорастущие случайные деревья ^[12],[13]. Другой подход основан на поиске пути с использованием эвристик, направляющих поиск к более предпочтительной цели ^[14],[15]. Именно этот подход используется для слежения за движущейся целью ^[16].

Нейронные сети приобрели широкое распространение в решении ОЗК и планирования пути. Они могут обучаться как на моделях, так и во время работы манипулятора. С их помощью строятся траектории в пространстве конфигураций по требуемым положениям в рабочем пространстве. Классическое обучение с обратным распространением ошибки, когда на вход подавался вектор положений рабочего органа манипулятора, а на выходе получался вектор обобщенных координат, показывало большое время обучения и большую ошибку ^[17]. Поэтому были предложены работающие параллельно нейронные сети ^[18], среди которых выбиралось лучшее решение. Также для каждой обобщенной координаты использовались независимые нейронные сети ^[19]. Другой подход подразумевал изменение алгоритмов обучения, так в ^[20] использовался метод электромагнетизма ^[21]. В ^[22] предложено обучать нейронную сеть на небольшом связанном участке рабочего пространства и записывать веса сети в таблицу поиска. Количество таблиц равно количеству участков и выбор таблицы обусловлен текущим положением цели.

Среди интеллектуальных алгоритмов, используемых для планирования пути, также распространены генетические алгоритмы (ГА). В ^[22] показана способность ГА решать ОЗК. В ^[23] сравниваются способы кодирования «особей» обобщенных координат при планировании пути. Результаты показали, что при бинарном кодировании траектория имеет частые выбросы обобщенных координат, тогда как числовое кодирование дает гладкую траекторию. В ^[24] представлено применение обратной связи в генетическом алгоритме. При таком подходе популяция состоит не из конфигураций манипулятора, а из смещений рабочего органа на основе которых получают смещения обобщенных координат. В ^[25] показан подход, сочетающий генетический алгоритм с алгоритмом имитации отжига, который настраивает вероятности скрещивания и мутации.

В данной статье рассматривается задача слежения рабочего органа манипулятора за движущейся целью. Это достигается путем решения обратной задачи кинематики в каждый момент времени. Решение ОЗК производится комбинированным поисковым методом (КПМ), представляющим из себя комбинацию генетического алгоритма и симплексного поиска. Поскольку цель движется непрерывно, то слежение за движущейся целью можно рассматривать как задачу оптимизации с дрейфующим экстремумом. Здесь предлагается модификация КПМ, способная решать обратную задачу кинематики с движущейся целью.

Прямая и обратная задачи кинематики манипулятора

Решениепрямой задачи кинематики(ПЗК) для манипулятора, при известной структуре, есть расчет изменения координат рабочего органа (РО) относительно координат основания в рабочем пространстве при известных углах поворота его сочленений. В общем виде эта задача выглядит следующим образом:

, (1)

где q – вектор обобщенных координат (углов поворота в звеньях) манипулятора; – матрица размерности , здесь r – вектор, размерности , положения РО рабочем пространстве ; – матрица размерности , где , , – векторы, размерности , составляющие локальную систему координат РО манипулятора в рабочем пространстве . Компоненты матрицы Т представлены на рисунке 1. Обозначим и степени свободы РО на перемещение и вращение соответственно. Величина зависит от конструкции манипулятора. Для планарного манипулятора т. к. РО может двигаться вдоль двух осей и вращаться вокруг одной оси. Максимальное значение , тогда РО может перемещаться и вращаться по трем осям Декартовой системы координат.

Рис. 1 – Рабочее пространство манипулятора

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

Таблица 1– Параметры Денавита–Хартенберга

Эти параметры используются для формирования матриц трансформации. Положение и ориентация системы координат i-го сочленения по отношению к (i-1)-му сочленениюописывается матрицей трансформации:

, (2)

где , .

Решение прямой задачи кинематики находится из трансформации n-го сочленения по отношению к основанию манипулятора

. (3)

Матрица Т извлекается из .

Обратная задачи кинематики(ОЗК) служит для поиска углов в сочленениях манипулятора q при известной матрице T РО и выводится из (1)

. (4)

Решение ОЗК напрямую связано с основной целью манипулятора – расположением рабочего органа с определенной позицией и ориентацией. Трудность решения ОЗК заключается в том, что сложно аналитически выразить из . Кроме того, для манипуляторов с числом степеней свободы , для точек a и b при задача (1) может давать . Поэтому существует более одного решения (4), т.е. функция многоэкстремальная. При этом эти решения могут сильно отличаться друг от друга.

Решение обратной задачи кинематики комбинированным поисковым методом

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

, (5)

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

(6)

, (7)

, (8)

, (9)

где , , расстояния от текущего (для момента времени t) положения РО манипулятора до цели по осям , и соответственно; , – угол между осью системы координат цели и соответствующей осью системы координат РО манипулятора; положения и ориентации цели обозначаются подстрочным индексом «ц», а индексом «м» обозначаются текущие положения и ориентации манипулятора; – штрафной коэффициент , отражающий влияние расстояния до цели на значение .

Комбинированный поисковой метод представляет собой генетический алгоритм в сочетании с симплексным поиском (СП). На каждом шаге генетического алгоритма создается популяция особей, каждая особь представляет собой точку в пространстве поиска. Так как решением ОЗК для манипулятора с n степенями является вектор , то каждая особь КПМ является вектором . Здесь номер вершины, S – число особей в данной популяции. Аналогичное представление имеют вершины симплекса в КПМ.

Рис. 2 – Представление особи(вершины) в КПМ при решении ОЗК

Поиск происходит по следующему алгоритму:

1) создается k-я популяция генетического алгоритма объемом S;

2) вычисляется целевая функция , для каждой вершины из ;

3) особи в популяции сортируются по увеличению (поиск минимума) целевой функции ;

4) первые особей назначаются центрами для симплексов;

5) производится симплексный поиск из всех центров. Результаты каждого симплексного поиска записывают в матрицу H размерности (рис.2);

6) –строка H, сравнивается с лучшей особью популяции, и если , то ;

7) ;

8) проверка условий останова: , где – максимальное число шагов алгоритма: если условия не выполнены переход к п.1, иначе переход в п.9;

9) вывод результата.

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

Задачу (5) запишем в виде:

, (10)

где

, (11)

, (12)

.

Здесь , ; – весовые коэффициенты штрафов, изменение которых влияет на точность и гладкость решения. Штраф увеличивается при приближении к позиционным ограничениям ^[26].

Целевая функция (10) отражает вертикальный дрейф экстремума функции, обусловленный изменением во времени ограничения (6). Идентификацию параметров дрейфа реализуем за счет дополнительного (повторного) эксперимента в каждой текущей вершине КПМ. Для этого аппроксимируем функциональное ограничение на интервале выражением вида

, (13)

где – евклидова норма, вектор оценок дрейфа цели вдоль осей OX, OY, OZ в j-й вершине; – интервал времени между повторными измерениями положения цели в j-й вершине КПМ; <2, 1> – номер эксперимента.

Для поиска лучшей вершины КПМ рассчитаем компенсированное (с учетом дрейфа цели) значение целевой функции

, (14)

где – время вычисления в j-й вершине; для ГА, и , для СП. Компенсация влияния дрейфа для ГА проводится после проведения экспериментов во всех S вершинах, а в СП на каждом шаге поиска для n+1 вершин текущего симплекса относительно s-ой вершины. Временная диаграмма оценки и компенсации представлена на рис.3.

Рис. 3 – Интервалы времени при оценке и компенсации дрейфа

Вектор оценок (13), рассчитывается на основе вектора текущих положений звеньев и вектора расстояний от рабочего органа до цели по осям OX, OY и OZ соответственно .

Обозначим вектор оценок изменения расстояний ; – время повторного измерения вектора в вершине j. Тогда оценка скорости дрейфа в момент времени tj равна:

(15)

(16)

(17)

(18)

Здесь – матрица весовых коэффициентов, – единичная матрица размерности . Выражения (15)-(18) рассчитываются для каждой вершины КПМ. Для расчета (13) в ГА используется оценка , полученная в последней вершине S, в СП используется оценка , полученная в отраженной вершине. Матрица весов имеет постоянные большие значения на главной диагонали исходя из предположения, что скорость дрейфа может меняться в широком диапазоне значений на протяжении всего времени оценки.

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

;

1) создается популяция размером S:

• • • выбор родителей;
    • скрещивание;
    • мутации;

2) для рассчитываем:

• • • значение целевой функции (10);
    • изменение расстояния (16) по каждой оси: , , ;
    • время (17);
    • вектор параметров (15);

3) для вычисляется и целевая функция КПМ (14);

4) особи популяции сортируются в порядке возрастания ;

5) из выбираются первые особей, которые становятся центрами симплексных поисков; , – счетчик шагов l-го СП, ;

6) для l-го симплекса производятся процедуры:

• • • отражение;
    • растяжение;
    • сжатие;

7) для s-й вершины l-го симплекса, где s–индекс отраженной вершины симплекса:

• • • значение целевой функции (10);
    • изменение расстояния (16) по каждой оси: , , ;
    • измерение времени (17) и ;
    • обновляется вектор параметров (15);

8) для вычисляется значение целевой функции КПМ (14);

9) ;

10) проверка условий останова , – максимальное число шагов l-го СП:

• • • если условия не выполнены переход к п.6;
    • заполняем матрицу , , где – результат l-го СП;

11) результаты работы симплексных поисков: матрица H размером , обновленный вектор ;

12) каждая строка изH, сравнивается с лучшей особью из популяции:

• • • вычисляется и ;
    • если , то ;

13) ;

14) проверка условий останова ГА : если условия не выполнены переход к п.1;

15) вывод результата.

Экспериментальные исследования

Эксперименты проводились на моделях манипулятора в среде MATLAB. Эксперимент 1: Рассмотрим трехзвенный манипулятор, работающий в плоскости декартовой системы координат и имеющий число степеней свободы и (рис.4).

Рис. 4 – Кинематика трехзвенного плоского манипулятора

Данный манипулятор не является избыточным , поэтому ОЗК имеет малое () число решений. Параметры Денавита-Хартенберга для манипулятора представлены в таблице 2

Таблица 2 – Параметры трехзвенного плоского манипулятора

В таблице 3 представлены параметры КПМ, обеспечивающие максимальное быстродействие и точность поиска. Поскольку для плоского манипулятора трёх вращательных степеней свободы достаточно для обеспечения положения и ориентации рабочего органа, то вес установлен соответствующим образом. Выбор значений весов , , из (10) обусловлен необходимостью уменьшения (6) до нуля, поэтому намного больше остальных. Вес имеет наименьшее значение поскольку (12) должно иметь значительное влияние на целевую функцию только вблизи позиционных ограничений. Вес получается из условия равенства суммы весов единице.

Таблица 3 – Параметры поиска для эксперимента 1

Движение цели происходило по траекториям, представленным в таблице 4. Траектория 1 – прямая в плоскости с фиксированной координатой xи рабочим органом, ориентированным в сторону положительного направления оси x0. Траектория 2 – лемниската Бернулли в плоскости с рабочим органом, ориентированным в сторону положительного направления оси x0.

Таблица 4–Траектории для испытания плоского манипулятора

Построенные траектории движения рабочего органа представлены на рис. 5 и рис. 6.

а)

б)

в)

г)

Рис. 5– Результаты решения ОЗК для траектории 1: а) заданная и пройденная траектории; б) ошибки положения; в) траектории обобщенных координат; г) ошибки ориентации

а)

б)

в)

г)

Рис. 6– Результаты решения ОЗК для траектории 2: а) заданная и пройденная траектории; б) ошибки положения; в) траектории обобщенных координат; г) ошибки ориентации

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

Эксперимент 2: Рассмотрим трехзвенный манипулятор, работающий в пространстве (рис. 7). Поскольку число степеней свободы манипулятора () меньше числа степеней свободы рабочего пространства , то проведем задачу оптимизации по положению, игнорируя ориентацию рабочего органа. В таблице 5 приведены параметры кинематики манипулятора.

Таблица 5 – Параметры трехзвенного манипулятора

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

Таблица 6 – Параметры поиска для эксперимента 2

Рис. 7– Кинематика трехзвенного пространственного манипулятора

Траектории для испытания схожи с траекториями для эксперимента 1. Траектория 1 – прямая в плоскости параллельной , траектория 2 – лемниската Бернулли, расположенная в плоскости параллельной .

Таблица 7–Траектории для испытания пространственного манипулятора

Результаты эксперимента на рис. 8 и рис. 9.

а)

б)

в)

Рис. 8– Результаты решения ОЗК для траектории 1: а) заданная и пройденная траектории; б) ошибки положения; в) траектории обобщенных координат

а)

б)

в)

Рис. 9– Результаты решения ОЗК для траектории 2: а) заданная и пройденная траектории; б) ошибки по осям координат; в) траектории обобщенных координат

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

Эксперимент 3: Рассмотрим семизвенный манипулятор, представленный на рис.10. Такая конструкция является избыточной в кинематическом смысле (n>m). Это значит, что он способен обеспечить произвольную ориентацию рабочего органа в рабочей области. Такие манипуляторы имеют большое число решений ОЗК.

Рис. 10 – Кинематическая схема семизвенного манипулятора

Кинематические параметры указаны в таблице 8.

Таблица 8 – Параметры семизвенного манипулятора

Поскольку размерность пространства и число экстремумов возросло по сравнению с экспериментами 1 и 2, то эксперимент 3 проводился с большим объемом популяции и количеством симплексов на популяцию. Поскольку конструкция позволяет отслеживать ориентацию рабочего органа, то вес установлен в 0.5. Параметры поиска представлены в таблице 9.

Таблица 9 – Параметры поиска для эксперимента 3

Для этого эксперимента были использованы траектории из эксперимента 2. Результаты эксперимента на рис. 11 и рис. 12.

а)

б)

в)

г)

Рис. 11– Результаты решения ОЗК для траектории 1: а) заданная и пройденная траектории; б) ошибки положения; в) траектории обобщенных координат; г) ошибки ориентации

а)

б)

в)

г)

Рис. 12– Результаты решения ОЗК для траектории 2: а) заданная и пройденная траектории; б) ошибки положения; в) траектории обобщенных координат; г) ошибки ориентации

Несмотря на большое число потенциальных решений ОЗК, алгоритм строит траекторию рабочего органа с ошибкой менее 0.01 м и плавными траекториями обобщенных координат.

По результатам экспериментов наблюдается успешное решение ОЗК для не избыточных и избыточных манипуляторов. Видно, что решение получается с ошибкой, не превышающей 0.01м, кроме случаев, когда обобщенные координаты приближаются к граничным значениям. Такая точность достигается за одну итерацию КПМ благодаря глобальным свойствам ГА и эффективности СП.

Заключение

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

Рассчитанные траектории плавные и не выходят за допустимые границы. Плавность траектории в пространстве обобщенных координат связана с использованием целевой функции (5) , которая позволяет КПМ выбрать из множества решений ОЗК только те, которые требуют минимальных перемещений.

Возможность выбора из набора вариантов дает КПМ преимущество по сравнению с классическими итерационными алгоритмами решения ОЗК и другими алгоритмами, основанных на одном критерии поиска. Это преимущество выражается в возможности получения глобального решения основанного на сумме критериев (10) за несколько итераций поиска. Данный алгоритм прост для реализации и пригоден для параллельного вычисления.