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

МР представляется в пространстве конфигураций (пространстве обобщенных координат) как точка. Функционирование МР должно происходить в пределах ограниченной области X конфигурационного пространства. Будем считать, что область X имеет такой вид, что для любого q О X выполняются неравенства a1 ≤ q ≤ a2, (1) где a1 = (а11, а21, …, ап1) – вектор нижних ограничений на значения обобщенных координат; a2= (а12, а22, …, аn2) – вектор верхних ограничений на значения обобщенных координат; q = (q1, q2, …, qп) – вектор обобщенных координат МР. Таким образом, область X представляет собой гиперпараллелепипед. Все точки, не удовлетворяющие (1), будем считать запрещенными.

Кроме того, следует учитывать, что и внутри X могут присутствовать запрещенные состояния. Во-первых, это те состояния (конфигурации), которые обусловлены конструктивными ограничениями МР, например те, в которых происходит недопустимое взаимопересечение звеньев. Такие запрещенные конфигурации удается вычислить заранее. Во-вторых, запрещенной является та конфигурация, в которой МР налегает на препятствия. В условиях неизвестной среды все такие конфигурации вычислить заранее невозможно. Итак, запрещенной конфигурацией является та конфигурация МР, в которой МР не может находиться в силу конструктивных ограничений либо в силу налегания на препятствие. Перед началом движения информации о запрещенных состояниях в Х нет или она неполна. Точки из Х , про которые нет достоверной информации о том, что они запрещенные, считаем разрешенными.

Рассмотрим теперь точки из B_T . Разрешенной точкой q ^T ∈ B_T будем считать только ту точку, которая удовлетворяет следующим двум критериям: 1) она не является запрещенной в смыслах, описанных в предыдущих абзацах;

2) в нее можно попасть за конечное число шагов из q ⁰, двигаясь в Х по разрешенным состояниям. Точки из B_T , не удовлетворяющие хотя бы одному из двух таких критериев, считаем запрещенными. Поскольку требуется выяснить, достижимо ли множество B_T хотя бы в одной точке, то считаем, что до начала движения у МР ни про одну точку из B_T нет достоверной информации о том, является ли она разрешенной или запрещенной. Теперь сформулируем следующую Задачу управления МР в неизвестной статической среде: даны стартовая конфигурация МР q ⁰ и целевое множество B_T . Требуется предложить алгоритм, который за конечное число шагов либо передвинет МР из q ⁰ в хотя бы одно состояние из множества В_Т , либо выдаст обоснованный ответ о том, что ни одно состояние из целевого множества В_Т не является разрешенным.

К настоящему времени имеются работы по алгоритмам управления динамическими (ДС) и, в частности, робототехническими (РС) системами в известной и неизвестной среде. Имеются хорошие обзоры таких алгоритмов [1; 2]. Предложены алгоритмы, например, алгоритм полного перебора и алгоритм А*, которые за конечное число шагов либо находят путь из q ⁰ в точку из B_T , либо сообщают, что в B_T нет точки, достижимой из q ⁰.

Некоторые алгоритмы планирования в известной среде в принципе могут быть использованы для движения в неизвестной среде. Если мы дискретизируем пространство состояний, то тогда можно будет использовать графовые методы поиска траектории движения ДС [2; 3]. Однако эти алгоритмы имеют одно общее свойство, которое затрудняет их применение для управления ДС в неизвестной среде, заключающееся в том, что они в том или ином объеме требуют осуществлять поиск в ширину, иначе не гарантируется достижение целевой точки [4]. Но при поиске в ширину часто возникает следующая ситуация: предположим, что мы только что закончили рассмотрение вершин, соседних к вершине q и теперь нам надо рассматривать вершины, соседние вершине q′ и вершины q и q′ не являются соседними. Для того, чтобы рассмотреть вершины, соседние к q′, ДС должна сначала передвинуться в q′. Таким образом, вновь возникает задача планирования и осуществления маршрута из q в q′. При планировании же в известной среде ЭВМ просто «переключает свое внимание» от q к q′, которые хранятся в памяти ЭВМ. Необходимость поиска и исполнения путей для многих различных q и q′ делает общую сумму передвижений ДС очень большой [4]. В случае, когда мы планируем путь в известной среде, компьютер просто переключает свое внимание от точки q к точке q′, которые хранятся в памяти компьютера.

В соответствии с классификацией [2] представителями алгоритмов поиска в ширину являются собственно алгоритм поиска в ширину, алгоритм A*, эвристический поиск «первый–лучший», ленивый вероятностный маршрут, динамическое программирование. В методах, основанных на случайном потенциальном поле, алгоритме «Нить Ариадны», быстро исследующих случайных деревьях [2] новые вершины генерируются случайным образом и потому применение таких методов ведет к тем же трудно стям. Подходы, основанные на разделении ячеек, (двунаправленных) графах видимости, диаграммах Вороного [2] сводятся к попеременному построению графа и поиску пути на нем, таким образом, им также присущ вышеописанный недостаток, связанный со множественными механическими перемещениями.

В алгоритме, представленном в настоящей статье, вершины q и q ′ всегда являются соседними, что сокращает количество движений.

Известно также, что алгоритмы поиска в глубину не всегда доводят до цели [4].

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

Предпринимаются попытки разработки создания алгоритмов управления для среды с неизвестными препятствиями. Большинство из них рассматривают двумерные случаи [5].

В [6–9] рассмотрены различные подходы к управлению роботами в двумерном неизвестном пространстве. В [6; 9] представлен подход, основанный на диаграмме Вороного, в [8] – поиск на основе табу-подхода. Поскольку эти подходы требуют попеременного построения графа и поиска пути на нем, они ведут к многочисленным перемещениям робота. В [7] препятствия должны иметь многоугольную форму. Применение методов, предложенных в [6–9] к управлению n -звенным манипуляционным роботом в неизвестной среде, не представлено.

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

В [10] рассмотрен n -мерный случай. Алгоритм основан на решении системы нелинейных уравнений методом Ньютона и потому не может гарантировать достижения целевой позиции.

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

В [11] предлагается алгоритм управления ДС в n -мерном пространстве состояний в присутствии неизвестных запрещенных статических состояний, за конечное число шагов либо переводящий ДС из стартовой точки q ⁰ в целевую q ^T , либо выдающий обоснованное сообщение о том, что q ^T недостижима. Алгоритм обладает таким недостатком: он предполагает, что во множестве Х каждое состояние достижимо из каждого. Данное требование в ряде случаев может входить в противоречие с целью самого алгоритма – за конечное число шагов выяснить достижимость или недостижимость целевого состояния из стартового.

В [4] предложен подход к управлению роботами (в том числе и манипуляционными) в n -мерном пространстве состояний. Суть подхода заключается в том, что робот планирует путь, соединяющий стартовую точку q ⁰ и целевую q ^T , обходящий известные запрещенные состояния, и пытается исполнить данный путь либо до достижения q ^T , либо до столкновения в некоторой точке q ⁿ с ранее неизвестным запрещенным состоянием. В последнем случае планируется новый путь L ( q ⁿ , q ^T ), соединяющий q ⁿ и q ^T и обходящий все известные запрещенные состояния. Показано [4], что задача перевода робота из q ⁰ в q ^T в неизвестной среде сводится к решению конечного числа задач планирования и исполнения пути L ( q ⁿ , q ^T ) (другими словами, к решению конечного числа задач ПИ (планирования в известной среде)). При этом предполагается, что имеется априорное знание о том, что q ^T достижима.

В настоящей статье при решении Задачи мы опирались на подход [4]. Сформулированная Задача нами сводится к задаче исследования достижимости конечного числа N_BT точек q _i ^T , i = 1, 2, …, N_BT. Требование априорного знания о достижимости q _i ^T , i = 1, 2, …, N_BT снято. Сформулированы новые требования к процедуре ПИ, необходимые для решения Задачи .

В [12–15] даются алгоритмы, за конечное число шагов переводящие ДС в среде с неизвестными запрещенными статическими состояниями из q ⁰ в q ^T. Однако при этом предполагалось, что имеется априорная информация о том, что q ^T достижима.

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

• 1.    На множестве B_T выделяем конечное число N_BT точек q _i ^T , i = 1, …, N_BT. . Это те целевые конфигурации, достижимость которых будет исследоваться. В дальнейшем B_T будем рассматривать как список конфигураций q _i ^T , i = 1, …, N_BT . Считаем, что B_T не пополняется и потому N_BT не увеличивается. Считаем, что координаты каждой точки из B_T определяются достоверно.

• 2.    Считаем, что у нас есть процедура ПИ, которая за конечное число шагов либо планирует траекторию из произвольной разрешенной точки q ⁿ ∈ X в произвольную точку q ^T ∈ X cреди сообщенных ей известных запрещен-

• ных состояний, либо выдает сообщение, что qT недостижима. Такие процедуры уже существуют, например алгоритм полного перебора или алгоритм А* [3], которые для любой начальной точки qn и любой целевой qT при заданных известных запрещенных состояниях за конечное число шагов либо находят путь из qn в qT, либо сообщают, что путь из qn в qTнайден быть не может.

• 3.    Расположение препятствий внутри рабочей зоны МР остается неизменным в течение всего времени движения МР.

• 4.    Количество препятствий внутри рабочей зоны МР остается неизменным в течение всего времени движения МР.

• 5.    Все движение, в том числе и результирующая траектория, должно происходить в гиперпараллелепипеде (1).

• 6.    МР имеет сенсорную систему (СС), которая доставляет информацию об r -окрестности текущей точки МР q е X . Текущая точка МР - это та точка, в которой МР в настоящий момент находится. Под r -окрестностью q понимаем гипершар в X с центром в q и радиусом r > 0. Множество всех точек, входящих в r -окрестность точки q , обозначаем Y ( q ). Слова «доставляет информацию об r -окрестности точки q » означают, что относительно каждой точки из Y ( q ) СС определяет, является ли она разрешенной или запрещенной, при этом все запрещенные точки из Y ( q ) заносятся в множество Q ( q ) , а все разрешенные точки из Y ( q ) заносятся в множество Z ( q ). Способ записи множеств Y ( q ), Z ( q ), Q ( q ) может быть разным -в виде формул, списков, таблиц и т д., но мы считаем, что он есть. Устройство сенсорной системы в данной работе не рассматривается.

• 7.    Считаем, что у нас есть программная Процедура1 ( B T , N BT , Q ( q n )). Процедура1 ( B T , N BT , Q ( q n )) получает при вызове множество B_T , количество N_BT точек в множестве B_T , множество Q ( q ⁿ ), полученное при последнем запуске СС. Процедура1 ( B T , N BT , Q ( q n )) выбрасывает из B_T те точки, которые совпадают с точками из Q ( q ⁿ ). После выброса оставшиеся точки в B_T перенумеровываются сплошной нумерацией начиная с 1 и в N_BT записывается число точек, оставшихся в B T после исполнения Процедуры1 ( B T , N BT , Q ( q n )).

• 8.    Считаем, что у нас есть программная Процедура2 ( B T , N BT , q T ). Процедура2 ( B T , N BT , q T ) получает при вызове множество B T , количество N BT точек в множестве B T и точку q ^T . Процедура2 ( B T , N BT , q T ) выбрасывает из B_T точку q ^T . После этого точки перенумеровываются сплошной нумерацией начиная с 1 и в N BT записывается ч исло точек, оставшихся в B_T после исполн ения Процедуры2 ( B T , N BT , q T ).

Ниже приведен Алгоритм решения нашей Задачи . Перед началом движения текущей конфигурацией q c МР является q ", по ходу движения Алгоритм! может вызываться из других текущих конфигураций МР.

Алгоритм. Если N BT =0, то происходит окончание работы алгоритма с сообщением о том, что объект захвачен быть не может. Иначе в качестве q T назначаем первую точку из B_T .

Шаг 1. МР находится в конфигурации qc. n: =0, qn :=qc. Исполнить целевая_точка_запрещенная:=Алгоритм! (qc, qT, BT, NBT) .

Если целевая_точка_запрещенная=НЕТ, происходит успешное окончание работы Алгоритма с сообщением о том, что объект захвачен в точке q T .

Если целевая_точка_запрещенная=ДА, происходит переход на шаг 2.

Шаг 2. Если N BT =0, происходит окончание работы алгоритма с сообщением о том, что объект не может быть захвачен МР ни в одной из целевых конфигураций. Если N_BT № 0, в качестве q T взять первую точку из B T и перейти на шаг 1. Конец Алгоритма.

Алгоритм! получает значения в следующем формате: Алгоритм! ( q ⁿ , q T , B T , N BT )ипосвящен выяснению вопроса о том, является ли точка q T достижимой из q ⁿ в неизвестной среде.

Алгоритм1. Шаг 1. МР находится в q ⁿ (назовем ее «точка смены маршрута). СС доставляет информацию о Y ( q n ), Z ( q n ), Q ( q n ).

Шаг 2. Выполняется nbt :=Процедура1 (BT NBT, Q(qn ||.

Если N_BT =0, происходит присвоение целевая_точка_ запрещенная:=ДА и возврат в алгоритм. Если N BT ^ 0, проверяем, оказалась ли выброшенной из B_T точка q ^T . Если да, то происходит присвоение целевая_точка_запрещен-ная:=ДА и возврат в алгоритм, если нет (т е. q T осталась в B T ) - то происходит переход на шаг 3.

Шаг 3. Вызывается процедура ПИ с задачей сгенерировать линию L(qn, qT), удовлетворяющую следующим условиям:

• -    L ( q ⁿ , q T ) соединяет q ⁿ и q ^T ;

  n

  
  

• -    L ( q ⁿ , q T ) не налегает на множество U Q ⁽ q ) , т е. ни на одну из запрещенных точек;

• -    L ( q ⁿ , q T ) удовлетворяет конструктивным ограничениям (1).

Результатов работы процедуры ПИ может быть два:

• -    ПИ возвращает сгенерированную траекторию и Алгоритм! переходит на шаг 4;

• -    ПИ сообщает о том, что L ( q ⁿ , q T ) сгенерирована быть не может, т. е. q ^T является недостижимой. В этом случае выполняется

NBT: = Процедура 2 (BT, NBT, qT), производится присвоение: целевая_точка_запрещен-ная:=ДА и происходит возврат в Алгоритм.

Шаг 4. МР начинает движение по L ( q ⁿ , q ^T ). Исходов движения может быть два:

• -    МР попадает в некоторую точку q T е B T . В этом случае происходят присвоения q T := q T и целевая_точка_зап-рещенная:=НЕТ и выполняется возврат в Алгоритм ;

• -    МР придет в некоторую точку q *, следующая за которой является запрещенной. В этом случае выполняем n := n + 1 , q ⁿ := q * и Алгоритм! переходит на шаг 1. Конец Алгоритма1 .

Теорема . Исполняя Алгоритм , МР решит Задачу за конечное число шагов.

Доказательство. Алгоритм выясняет достижимость конечного числа точек из BT. Выяснение достижимости в отношении каждой из точек qTеBT осуществляется путем исполнения Алгоритма!. Отсюда видно, что исполнение Алгоритма сводится к конечному числу вызовов Алгоритма!. Поэтому, чтобы доказать, что Алгоритм будет исполнен за конечное число шагов, тре- буется показать, что исполнение Алгоритма1 для произвольных qn и qT будет осуществлено за конечное число шагов.

Алгоритм1 посвящен выяснению вопроса о том, является ли точка q ^T достижимой в неизвестной среде из точки смены маршрута q ⁿ или нет. В Алгоритме1 , когда МР находится в точке q ⁿ , n = 0, 1, 2, …, происходит запуск СС и запуск процедуры ПИ. Если в результате исполнения этих действий точка q ^T будет определена как запрещенная (в силу налегания на препятствие, либо в силу недостижимости), произойдет возврат в алгоритм и для исследования достижимости будет назначена другая точка q ^T ∈ B_T . Если в результате исполнения этих действий точка q ^T не будет определена как запрещенная, происходит генерация линии L ( q ⁿ , q ^T ) и МР начнет исполнять эту траекторию. Исполнение этой траектории может иметь два исхода: либо МР, не встретив на своем пути запрещенных точек, достигнет q ^T (и тогда произойдет успешное окончание работы Алгоритма ) либо МР придет в точку q ⁿ , n = 1, 2, …, следующая за которой будет запрещенной. Покажем, что все точки смены маршрута q ⁿ , n = 0, 1, 2, … будут различными и их число будет конечным.

Покажем, что все точки смены маршрута различны. Предположим, что МР сменил маршрут, находясь в точке q ^s , а потом, находясь в точке q ^p , вновь сменил маршрут, т. е. s <  p . Покажем, что q ^s ≠ q ^p . Предположим сначала, что q ^s= q ^p и тогда Q ( q ^s ) =Q ( q ^p ). Поскольку МР сменил маршрут, находясь в точке q ^s , то он сгенерировал маршрут, не налегающий в том числе и на Q ( q ^s ). Но поскольку МР сменил маршрут в точке q ^p , то это означает, что его маршрут налег на Q ( q ^p )= Q ( q ^s ) (при этом q ^s = q ^p является центром r -окрестности точки q ^s = q ^p и следующая за ней точка является запрещенной), т. е. Q ( q ^p )= Q ( q ^s ) не было известным. Получили противоречие. Отсюда видно, что все точки смены маршрута различны.

Покажем теперь, что число точек смены маршрута конечно. Предположим, что, наоборот, оно бесконечно. Все точки смены маршрута должны удовлетворять ограничениям (1). Это означает, что последовательность этих точек ограничена. Согласно теореме Больцано–Вейерш-трасса из этой последовательности можно извлечь сходящуюся подпоследовательность q ⁱ , i = 1, 2, … В соответствии со свойством Коши сходящихся последовательностей для любого e можно найти такой номер s , что все точки q ⁱ, i >  s , будут лежать в e-окрестности точки q ^s . Возьмем e <  r . Рассмотрим произвольную точку смены маршрута q ⁱ , расположенную в e -окрестности точки q ^s . Так как МР, находясь в q ⁱ , сменил свой маршрут, то это означает, что маршрут налег на множество Q ( q ^s ) ( q ⁱ и ее соседние точки принадлежат Q ( q ^s )). Отсюда надо сделать вывод, что множество Q ( q ^s ) не было учтено при генерации маршрута, приведшего в q ⁱ , что невозможно при исполнении предписаний Алгоритма1 . Таким образом, если принять, что число точек смены маршрута бесконечно, то неизбежно возникнет ситуация, которая не может наступить при исполнении предписаний Алгоритма1 . Следовательно, число точек смены маршрута конечно.

Итак, число точек смены маршрута qn, n = 0, 1, 2, … конечно и они все различны. В каждой точке qn осуществляется запуск СС и вызов процедуры ПИ, генерирую- щей L(qn, qT). В результате выполнения этих действий либо получаем информацию о том, что qT является запрещенной, либо нет. Если получаем, выяснение вопроса о достижимости qT заканчивается выводом о недостижимости qT. Если нет, происходит попытка исполнения траектории L(qn, qT). Если и в последней точке смены маршрута qn точка qT не была квалифицирована как запрещенная, то будет сгенерирована L(qn, qT), эта траектория будет исполнена и qT будет достигнута.

Таким образом, показано, что МР, исполняя Алго-ритм1 за конечное число шагов либо достигнет точки q ^T , либо сделает вывод о том, что q ^T недостижима. Алгоритм сводится к исполнению Алгоритма1 конечное число раз. Отсюда видно, что, исполняя Алгоритм , МР решит задачу за конечное число шагов. Теорема доказана .

Замечание 1. Мы уже говорили, что Алгоритм1 сводится к генерации и исполнению конечного числа пути L ( q ⁿ , q ^T ). Алгоритм сводится к исполнению конечного числа раз Алгоритма1 . Отсюда следует вывод о том, что Задача сводится к решению конечного числа задач ПИ планирования и исполнения пути в среде с известными запрещенными состояниями.

Замечание 2. При первом вызове Алгоритма1 q ^c = q ⁰ исследуется достижимость первой точки из B_T , назовем ее q ₁ ^T . При последующих вызовах Алгоритма1 исследуется достижимость точек q_i^T ∈ B_T , i = 2, 3, …, N_BT и, вообще говоря, q ^c ≠ q ⁰. Но, поскольку МР прибыл в q ^c из q ⁰ по непрерывно следующим одна за другой разрешенным точкам, то вывод о достижимости/недостижимости q_i ^T ∈ B_T , i = 2, 3, …, N_BT из q ^c ≠ q ⁰ будет квалифицироваться как вывод о достижимости/недостижимости точки q_i ^T ∈ B_T , i = 2, 3, …, N_BT из q ⁰.