CC BY
43
УДК 519:713
П. К. Лопатин
КОМПЬЮТЕРНАЯ ИМИТАЦИЯ УПРАВЛЕНИЯ СЕМИЗВЕННЫМ МАНИПУЛЯЦИОННЫМ РОБОТОМ В СРЕДЕ С НЕИЗВЕСТНЫМИ ПРЕПЯТСТВИЯМИ
Представлен алгоритм управления манипуляторами в среде с неизвестными препятствиями. Приведены результаты компьютерной имитации движения семизвенного манипулятора в неизвестной среде на основе данного алгоритма.
В настоящее время манипуляционные роботы применяются в самых различных областях человеческой деятельности, в том числе при обслуживании парализованных пациентов. Так, например, робототехническая группа Института автоматизирующей техники (ИАТ) Бременского университета работает над проектом «Friend» [15], целью которого является разработка автономного инвалидного кресла, обслуживающего парализованных пациентов или пациентов, находящихся на излечении. Это кресло в качестве составной части имеет стол, на котором располагаются предметы, нужные пользователю. К основанию стола крепится манипулятор с семью степенями подвижности (рис. 1). Пользователь может попросить подать ему воды, и манипулятор должен передвинуться среди предметов, расположенных на столе (стаканов, бутылок, яблок и т. д.), взять бутылку, налить воды в стакан, вставить в него трубочку и подать пользователю.
Рис. 1
Такая практическая задача порождает фундаментальную теоретическую задачу - разработку алгоритма, который за конечное число шагов передвинул бы манипулятор из стартовой конфигурации в целевую, при этом в рабочей зоне манипулятора могут находиться препятствия, но их расположение, размеры, форма и количество не известны до начала движения. Предполагается, что расположение, размеры, форма и количество препятствий не будут меняться в течение всего времени движения манипулятора. Сенсорная система манипулятора
может получать информацию о расположении препятствий только из небольшой окрестности той конфигурации, в которой манипулятор находится.
Обозначим стартовую конфигурацию как q0 = (дД q20, ..., дл0), а целевую конфигурацию - как qT = (д1т, q2T, ..., q т). Если манипулятор, находясь в некоторой конфигурации q, имеет хотя бы одну общую точку с хотя бы одним препятствием, то такую конфигурацию будем считать запрещенной и точка q, описывающая эту конфигурацию в пространстве конфигураций, также будет считаться запрещенной. Если манипулятор, находясь в некоторой конфигурации q, не имеет ни одной общей точки ни с одним препятствием, то такая конфигурация будет считаться разрешенной и точка q, описывающая эту конфигурацию в пространстве конфигураций, также будет считаться разрешенной. Итак, запрещенные конфигурации будут представать в пространстве конфигураций как точки, но до начала движения манипулятора у системы управления нет информации об этих точках.
Мы должны также учитывать, что вследствие конструктивных ограничений исполненная траектория должна удовлетворять неравенству
а1 < q(t) < а2 (1)
для каждого момента времени t движения, где а1 - вектор нижних ограничений на значения обобщенных координат; а2 - вектор верхних ограничений. Точки, удовлетворяющие неравенству (1), образуют гиперпараллелепипед в пространстве обобщенных координат. Все точки из пространства обобщенных координат, которые не удовлетворяют неравенствам (1), будут квалифицироваться нами также как запрещенные.
Итак, в нашей задаче манипулятор будет представлен как точка, которая должна будет передвинуться во внутренней области гиперпараллелепипеда (1) из точки q0 в точку цт и траектория движения нашей точки не должна будет налегать на запрещенные точки, которые присутствуют внутри гиперпараллелепипеда (1).
К настоящему времени разработано большое количество алгоритмов управления роботами в известной среде [5; 12]. Предложены алгоритмы, гарантирующие нахождение траектории в среде с известными препятствиями при условии, что такая траектория существует [7.9]. Некоторые алгоритмы, посвященные планированию движения в известной среде, могут быть использованы и для планирования движения в неизвестной среде.
Для решения этой задачи могут применяться различные поисковые алгоритмы, описанные в [12; 16]. Однако эти алгоритмы требуют большого количества механичес-
ких перемещений, что делает крайне затруднительным их практическое применение [3]. Известно также, что алгоритмы поиска в глубину не всегда доводят до цели [3].
Для решения нашей задачи может быть использован подход, основанный на автоматическом доказательстве теорем [4], но этот подход требует рассмотрения большого количества вариантов и направлений поиска и поэтому применение этого подхода оказывается неэффективным [2].
Некоторые авторы предлагают использовать методы искусственных потенциалов (см., например, [6]). В этом методе робот представляется в виде заряженной точки, препятствия наделяются отталкивающими потенциалами, а целевая точка - притягивающим потенциалом. Работа метода демонстрируется для известных препятствий, но не указывается, как распоряжаться поступающей информацией об обнаружении ранее неизвестных препятствий. В общем случае нет гарантии того, что траектория, свободная от столкновений, будет найдена [5].
Для методов планирования траектории в среде с наперед известными запрещенными состояниями имеется одна общая трудность: очень трудно заранее собрать полную информацию о расположении препятствий в рабочей зоне манипулятора и представить эту информацию в виде, пригодном для учета при планировании траектории манипулятора. В нашем случае можно будет видеть, что не требуется собирать полную информацию о препятствиях в рабочей зоне манипулятора заранее, манипулятор будет собирать необходимую информацию самостоятельно в ограниченных объемах и в терминах обобщенных координат, что удобно для планирования траектории.
Что касается разработки алгоритмов управления роботами в среде с неизвестными препятствиями, то пока предпринимаются попытки их разработки в основном только для двумерных случаев [14].
Существует алгоритм управления манипуляторами среди неизвестных препятствий, расположенных в трехмерном декартовом пространстве [14]. Манипулятор в этом случае должен иметь не более трех звеньев и последняя кинематическая пара должна быть поступательной. При таких предварительных условиях алгоритм гарантирует достижение цели за конечное число шагов.
В [17] рассмотрен и-мерный случай. Но алгоритм основан на решении системы нелинейных уравнений методом Ньютона и поэтому не может гарантировать достижения цели, так как метод Ньютона не гарантирует нахождения решения.
Постановка задачи и предварительные условия. Рассмотрим задачу управления манипулятором в среде с неизвестными препятствиями в следующей постановке: даны стартовая конфигурация q0 и целевая конфигурация qт, в рабочей зоне манипулятора препятствия могут присутствовать, но перед началом движения у системы управления манипулятором нет информации о наличии препятствий, их количестве, расположении и форме. Требуется сделать так, чтобы манипулятор передвинулся из конфигурации q0 в конфигурацию qт.
Сделаем следующие допущения:
- расположение, формы, размеры и количество препятствий остаются неизменными в течение всего времени движения манипулятора;
- заранее известно, что целевая конфигурация достижима, т. е. известно, что в пространстве обобщенных координат можно найти хотя бы одну линию, соединяющую q0 и цт и эта линия не налегает ни на одну запрещенную точку, находящуюся во внутренней области гиперпараллелепипеда (1);
- результирующая траектория должна удовлетворять неравенству (1) для каждого момента времени, т. е. все движение должно происходить в гиперпараллелепипеде (1);
- манипулятор имеет сенсорную систему, которая при нахождении манипулятора в некоторой конфигурации q, принадлежащей гиперпараллелепипеду (1), позволяет отвечать на вопрос относительно каждой конфигурации, принадлежащей небольшой г-окрестности конфигурации q: налегает ли конфигурация из этой г-окрестности на препятствия или нет? Эта окрестность конфигурации q есть гипершар в пространстве конфигураций с центром в q и радиусом г > 0. Устройство сенсорной системы здесь не рассматривается;
- обозначим множество всех конфигураций из г-ок-рестности точки q через У^). Множество всех запрещенных точек из У^) обозначим как Q(q), множество всех разрешенных точек из У(ц) - как Z(q) (рис. 2).
Алгоритм управления манипуляторами в неизвестной среде. На кафедре информатики и вычислительной техники Сибирского государственного аэрокосмического университета имени академика М. Ф. Решетнева разработан наиболее общий подход к решению задачи управления манипуляционными роботами в среде с неизвестными препятствиями. Решена задача планирования движения и-звенного манипулятора из стартовой конфигурации в целевую, при этом используется информация только из небольшой окрестности текущего положения манипулятора с тем, чтобы манипулятор достиг целевой конфигурации за конечное число шагов.
Имеется алгоритм для движения и-звенного манипулятора в среде с неизвестными препятствиями, который гарантирует достижение целевой конфигурации за конечное число шагов в среде, заполненной произвольным числом статических препятствий, которые имеют произвольные формы, размеры и расположение [13]. Предполагается, что целевая конфигурация достижима. Назовем этот алгоритм базовым.
В работах [1; 3; 13] приведены эвристические алгоритмы, которые, хотя и не гарантируют достижения целевой конфигурации, но при тестировании продемонстрируют высокую работоспособность.
Рассмотрим эвристический алгоритм А3 (далее будем называть его просто алгоритм) управления манипуляторами в среде с неизвестными препятствиями [3; 13]. В алгоритме qn означает текущую конфигурацию манипулятора.
Перед началом работы алгоритма мы открываем стек, в котором будем хранить конфигурации, в которых манипулятор менял свою траекторию. Количество конфигураций, находящихся в настоящее время в стеке, обозначаем через J. Пер ед началом работы алгоритма стек пуст и J = 0.
Шаг 1. Считается, что манипулятор находится в стартовой конфигурации q0: и = 0, J = 0.
Шаг 2. Если целевая конфигурация цт достигнута, то алгоритм заканчивает работу. В противном случае рассчитать траекторию qnqт, соединяющую qn и qт. Траектория цпцт представляет собой конечное множество точек, лежащих на отрезке прямой линии, соединяющем ^ и qт. Запомнить число точек, образующих qnqт в переменной number_of_points.
Шаг 3. Сформировать множества У^п), Q(qn), Z(qn).
Шаг 4. Если в отрезке qnqт нет ни одной точки, которая совпадает хотя бы с одной точкой из и Q(q1), i = 0, 1, .., и, то тогда манипулятор двигается в следующую точку qnqт, и увеличивается на 1, а алгоритм переходит на шаг 5. В противном случае алгоритм переходит на шаг 7.
Шаг 5. Если и = 1, опустошить стек и считать J = 0.
Шаг 6. Если и I number_of_points, то алгоритм переходит на шаг 3. В противном случае алгоритм переходит на шаг 2.
Шаг 7. Добавить qn в стек, увеличить J на 1. Выбрать q е Z(qn) в соответствии с алгоритмом отталкивания от предыстории (АОП) (см. ниже). Манипулятор передвигается в q, и увеличивается на 1. Обозначить q через qn. Алгоритм переходит на шаг 2.
Заметим, что на шаге 5 мы опустошаем стек, потому что и стало равным 1. Это означает, что манипулятор начал двигаться по спланированному маршруту. Мы рассматриваем это как признак выхода манипулятор из тупика.
В алгоритме предварительная траектория выбирается в виде отрезка прямой линии в пространстве обобщенных координат. Это существенно упрощает расчет предварительной траектории, хотя одновременно ведет к тому, что достижение цели не гарантируется. Поэтому алгоритм, приведенный выше, можно дополнить следующим условием: если шаг 7 был выполнен Ы1 раз подряд, то в качестве предварительной траектории должна быть выбрана линия более высокого порядка, которая смогла бы обойти запрещенные точки.
Рассмотрим теперь алгоритм отталкивания от предыстории. Он используется для упрощения выхода из тупика, который может возникнуть в г-окрестности текущей конфигурации.
Алгоритм отталкивания от предыстории основан на следующем эвристическом подходе. В случае если на шаге 4 мы обнаруживаем, что qnqт пересекается с и Q(qi), i = 0, 1, ..., и, это означает, что манипулятор должен двигаться не в следующую точку qnqт, а в другую точку q е Z(qn). В стеке мы храним точки смены траектории манипулятора, около которых были обнаружены препятствия. Поэтому мы должны выбрать такую точку из множества Z(qn), которая лежит как можно дальше от точек из стека и как можно ближе к qт. Перед работой АОП запишем в переменную питЬег_оГ_соп1^ща;юш число конфигураций в Z(qn). Число J, как и прежде, есть число конфигураций в стеке. АОП выглядит следующим образом:
Шаг 1. Считатьj = 1.
Шаг 2. Если j > J, то перейти на шаг 4. В противном случае рассмотреть q из стека.
Шаг 3. Вызвать алгоритм 1 (см. ниже) и получить от него модифицированное Z(qn) и количество конфигураций в нем в переменной number_of_configurations. Увеличить j на 1. Перейти на шаг 2.
Шаг 4. Выбрать из Z(qn) такую конфигурацию q, чье расстояние до qT наименьшее по сравнению с другими конфигурациями из Z(qn).
Алгоритм 1. Этот алгоритм получает от вызвавшей процедуры множество Z(qn), переменную number _of_configurations, q и qn и оставляет в Z(qn) те конфигурации, чье расстояние до q больше, чем расстояние между q и qn. По окончании своей работы алгоритм 1 возвращает в вызвавшую процедуру модифицированное множество Z(qn) и переменную number_of_configurations. Если при завершении своей работы алгоритм 1 обнаруживает, что он выбросил из Z(qn) все конфигурации, то он возвращает Z(qn) и number_of_configurations в то состояние, которое они имели на момент вызова алгоритма 1.
Результаты применения алгоритма. В ходе стажировки автора в Бременском университете на основе алгоритма было разработано программное обеспечение, имитирующее на ЭВМ движение манипулятора в среде с неизвестными препятствиями. Работа созданного программного обеспечения испытывалась для упомянутого в начале статьи семизвенного манипулятора.
Первоначально пользователь разработанного программного обеспечения создает рабочую сцену с помощью программы, разработанной в ИАТ [10].
Пользователь вводит типы, размеры и количество препятствий. Препятствия представляются параллелепипедами. Каждый параллелепипед характеризуется девятью величинами: координатами начала O координатной системы xоЬзУobsZobs,, связанной с препятствием, в базовой координатной системе xgygzg (рис. 3); углами Roll, Pitch, Yaw, характеризующими ориентацию препятствия по отношению к базовой координатной системе; длиной, шириной и высотой препятствия. Звенья манипулятора также считаются параллелепипедами.
После создания рабочей зоны пользователь вводит векторы q0, описывающий стартовую конфигурацию, и цг, описывающий целевую конфигурацию. Каждый вектор
состоит из семи обобщенных координат, величина каждой координаты вводится в радианах. После этого программное обеспечение демонстрирует движение манипулятора среди неизвестных препятствий.
Мы провели эксперименты для различного числа и расположения препятствий, разных стартовых и целевых конфигураций. Результаты некоторых экспериментов приведены в таблице. Движение манипулятора в ходе эк-
Прокомментируем рис. 4. Стартовая конфигурация q0 показана на рис. 4, а, целевая конфигурация цТ - на рис. 4,
з. Манипулятор генерирует траекторию в соответствии с шагом 2 алгоритма, т. е. генерирует конечное множество точек, лежащих на отрезке прямой линии, соединяющем q0 и цТ в пространстве обобщенных координат. После этого манипулятор начинает двигаться по данной траектории. Момент, когда манипулятор встретил препятствие 1, представлен на рис. 4, б. С помощью АОП манипулятор обходит препятствие и переходит в конфигурацию, показанную на рис. 4, в. После этого манипулятор пытается двигаться в направлении qT, но встречает препятствие 2. Манипулятор вновь использует АОП, который перемещает манипулятор в соседнюю конфигурацию.
Заметим, что всякий раз после того как манипулятор передвинулся в конфигурацию, выбранную по АОП, он планирует новую траекторию, ведущую в qT, и пытается ее осуществить. Но и в следующей конфигурации предварительного маршрута манипулятор наталкивается на
сперимента 4 показано на рис. 4. Заметим, что в экспериментах 1.4 стартовая и целевая конфигурации остаются одинаковыми, но в каждый новый эксперимент мы добавляли по одному препятствию: в эксперименте 1 рабочая сцена такая же, как и в эксперименте 4, но препятствия 3, 4, 5 отсутствуют; в эксперименте 2 нет препятствий 4 и 5; в эксперименте 3 отсутствует препятствие 5 (четвертым препятствием на рис. 4, а является пол).
препятствие (сенсорная система дает информацию о разрешенных и запрещенных конфигурациях только в пределах небольшого радиуса вокруг текущей конфигурации). Поэтому в течение определенного промежутка времени манипулятор сталкивается с препятствием.
Наконец АОП переводит манипулятор в конфигурацию, показанную на рис. 4, г, т. е. манипулятор начинает двигаться вверх. Вспомним, что АОП выбирает ту конфигурацию, которая лежит как можно дальше от конфигураций, выбранных по АОП ранее, и которая лежит как можно ближе к цг. С момента, показанного на рис. 4, г, использование АОП заставляет манипулятор двигаться вниз. Этот процесс приводит манипулятор в конфигурацию, в которой он стол-кивается с полом, т. е. препятствием 4 (рис. 4, д). Теперь АОП вновь начал перемещать манипулятор вверх, но уже из конфигурации, показанной на рис. 4, д. В течение этого движения манипулятор приходит в конфигурацию, показанную на рис. 4, е, которая выглядит похожей на конфигурацию, показанную на рис. 4, д, однако из-за разницы в
№ экспе-ремента 0 Ч , рад Ч Т, рад Характеристики препятствий ^ с
№ 0 X 0 , см 0 У 0 , см 0 г 0 , см RoU рад РіїсЬ, рад Yaw, рад Длина,см Ширина, см Высота см
1 1,.57; 1,57; 0; -1,57; 0; -1,57; 0 -1,5 1,5 0 0 0 0 0 7; 7; 1 0 2 8 0 0 0 80 1,6 2 720
2 20 -10 0 0 0 0 34 14 20
2 1,57; 1,57; 0; -1,57; 0; -1,57; 0 1,5 5, 0 0 0 0 -1 1 7; 7; 1 0 2 8 0 0 0 80 01.июн 2 1050
2 20 -10 0 0 0 0 34 14 20
0 3 -24 -10 0 0 0 0 34 14 40
3 1,57; 1,57; 0; -1,57; 0; -1,57; 0 1,5 5, 0 0 0 0 -1 1 7; 7; 1 0 2 8 0 0 0 80 01.янв 2 867
2 20 -10 0 0 0 0 34 14 20
3 -24 -10 0 0 0 0 34 14 40
0 4 -40 -40 -0.2 0 0 0 200 200 0.2
4 1,57; 1,57; 0; -1,57; 0; -1,57; 0 -1,5 1,5 0 0 0 0 7; 7; 1 0 2 8 0 0 0 80 1,6 2 890
2 20 -10 0 0 0 0 34 14 20
3 -24 -10 0 0 0 0 34 14 40
4 -40 -40 -0.2 0 0 0 200 200 0.2
0 5 -6 8 0 0 0 0 2 2 40
значении угла q4 эти конфигурации выглядят по-разному: значение q4 в конфигурации, показанной на рис. 4, е, позволяет АОП выбрать такую конфигурацию, которая находится выше по предыдущей. Наконец манипулятор переходит в конфигурацию, показанную на рис. 4, ж. Обращаем внимание на то, что величина q4 возросла (и это показывает, что для момента, показанного на рис. 4, е, АОП имел возможность найти свободные конфигурации с более большими значениями q4). В заключение манипулятор обходит препятствия и приходит в целевую конфигурацию q^TO. 4, з).
Программное обеспечение для управления семизвенным манипулятором в среде с неизвестными препятствиями было разработано с использованием Microsoft Visual C++ 6.0 и его работа испытывалась на процессоре Intel Pentium II 2.4 GHz.
Таким образом, представлен эвристический алгоритм управления манипуляторами в среде с неизвестными статическими препятствиями. На основе данного алгоритма разработано программное обеспечение, имитирующее на ЭВМ движение манипуляторов. Представлены результаты тестирования этого программного обеспечения на примере управления семизвенным манипулятором в среде с неизвестными препятствиями. Продемонстрирована высокая работоспособность данного алгоритма.
Библиографический список
1. Бернацкий, И. П. Исследование интеллектуальных систем управления манипуляционными роботами в неизвестных экстремальных средах: отчет о НИР (заключ.) / И. П. Бернацкий, П. К. Лопатин; НИИ СУВПТ. Красноярск, 2000.
2. Ефимов, Е. И. Проблема перебора в искусственном интеллекте / Е. И. Ефимов // Изв. АН СССР. Сер: «Тех. кибернетика». 1988. № 2. С. 127-128.
3. Ильин, В. А. Интеллектуальные роботы: теория и алгоритмы / В. А. Ильин; САА. Красноярск, 1995.
4. Тимофеев, А. В. Роботы и искусственный интеллект / А. В. Тимофеев. М.: Наука, 1978.
5. Ahrikhencheikh, C. Optimized-Motion Planning: Theory And Implementation / С. Ahrikhencheikh, A. Seireg. New York: John Wiley & Sons, Inc, 1994.
6. Barraquand, J. Robot Motion Planning: A Distributed Representation Approach / J. Barraquand, J.-C. Latombe // Int. J. ofRob. Res. Vol. 10. 1991. № 6. P. 628-649.
7. Canny, J. The Complexity Of Robot Motion Planning / J. Canny. Cambridge, Massachusetts: The MIT Press, 1988.
8. Collins, G. E. Quantifier Elimination For Real Closed Fields By Cylindrical Algebraic Decomposition / G. E. Collins // Lecture Notes in Computer Science. Vol. 33. New York: Springer-Verlag, 1975. P. 135-183.
9. Donald, B. R. On Motion Planning with Six Degrees of Freedom: Solving the Intersection Problems in Configuration Space / B. R. Donald // Proceedings of the IEEE International Conference on Robotics and Automation. 1985.
10. Feuser, J. Software development for prevention of a robot collision with an environment via mapped virtual reality / J. Feuser, O. Ivlev // 25. Kolloquim der Automatisierungstechnik. Salzhausen, Germany, 2003. P. 34-35.
11. Gilbert, E. G. A fast procedure for computing the distance between complex objects in free space / E. G. Gilbert, D. W. Johnson, S. S. Keerthi // IEEE Journal of Robotics and Automation. Vol. 4. 1988. N° 2. P. 193-203.
12. LaValle, S. M. Planning Algorithms, 1999-2003 [Electronic resorsce] / S. M. LaValle. Electronic data. Режим доступа: http://msl.cs.uiuc.edu/planning. Загл. с экрана.
13. Lopatin, P. K. Algorithm of a manipulator movement amidst unknown obstacles / P. K. Lopatin // Proceedings of the 10th International Conference on Advanced Robotics (ICAR 2001), August 22-25, 2001, Hotel Mercure Buda, Budapest, Hungary. Budapest, 2001. P. 327-331.
14. Lumelsky, V. J. Three-Dimensional Motion Planning In An Unknown Environment For Robot Arm Manipulators With Revolute Or Sliding Joints / V. J. Lumelsky, K. Sun // International. Journal of Robotics and Automation. Vol. 9. 1994. № 4. P. 188-198.
15. A FRIEND for Assisting Handicapped People / C. Martens, N. Ruchel, O. Lang et al. // IEEE Robotics and Automation Magazine. 2001. Mar. P. 57-65.
16. Nilsson, N. J. Problem-Solving Methods in Artificial Intelligence / N. J. Nilsson. New York: McGraw-Hill Book Company, 1971.
17. Yegenoglu, F. On-line Path Planning Under Uncertainty / F. Yegenoglu, A. M. Erkmen, H. E. Stephanou // Proc. 27th IEEE Conf. Decis. and Contr., Austin, Tex., Dec. 7 - 9, 1988. Vol. 2. New York, 1988. P. 1075-1079.
P. K. Lopatin
COMPUTER SIMULATION OF A 7DOF MANIPULATOR CONTROL IN UNKNOWN ENVIRONMENT
In this article is presented an algorithm for manipulators’ control in an environment with unknown obstacles. Given results of the computer simulation of 7DOF manipulator control in unknown environment based on the presented algorithm.
