Статьи о роботах

Алгоритм поиска пути для роботов

Выбор и описание алгоритма поиска пути, подходящего для использования при ориентировании мобильных роботов по заранее заданной карте местности

Автор: Андрей Маркеев


Язык для описания роботов RoboML

Стандартизация представления данных является важным моментом любой области знаний. RoboML - язык, который решает эту задачу в области робототехники.

Автор: Андрей Маркеев


XML-спецификация RoboML

XML-спецификация языка описания роботов, снабженная подробными русскими комментариями. Детальное ее описание - в следующей статье о RoboML.

Автор: Андрей Маркеев


Робот-охранник

Рассматривается использование робота, сделанного на основе персонального компьютера, в охранных системах.

Автор: Андрей Маркеев


Робот и искусственный интеллект

Подход к использованию технологий искусственного интеллекта в системе управления роботов.

Автор: Андрей Маркеев


Робот для программиста

Общая идея. Краткое описание текущего положения дел в области робототехники, предпосылок для создания описываемого робота, потенциальных возможностей применения и т.д.

Автор: Андрей Маркеев


вернуться к списку статей

Алгоритм поиска пути для роботов

Автор: Маркеев Андрей

Перепечатка материалов статьи
разрешена только с письменного разрешения автора,
и при указании прямой ссылки на его сайт.

Речь в этой статье пойдет о подходе к реализации поиска пути по заранее заданной карте - для мобильных роботов, создаваемых на базе компьютера или ноутбука.

Собственный самодельный робот автора имеет некоторые особенности, связанные с его ориентированием. В частности, карта для робота задается в виде обычного графического файла формата BMP, черным на ней помечены непроходимые препятствия, красным - «опасные» участки, где следует двигаться с повышенной осторожностью (читать - двигаться медленнее, чем обычно). Обычно карта такого рода получается с помощью сканирования плана квартиры или любого другого помещения, и дополнения этого плана красными пометками в местах, где открываются двери, могут располагаться стулья, столы, или - где часто ходят люди.

Вопрос о текущем положении робота, корректировка текущего положения - решается с помощью датчиков обратной связи, а также системы ориентирования по ИК-маякам. Эти моменты подробно описаны на авторском сайте http://robot.paccbet.ru.

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

  1. Необходимо найти кратчайший путь до целевой точки.
  2. Требуется учитывать размеры робота, и не идти туда, куда он не пролезет. Здесь для простоты предполагается, что робот занимает некоторый квадратный или круглый участок, так как в большинстве случаев мобильные роботы не бывают вытянутыми.
  3. Путь должен содержать наименьшее число смен направления, т.к. поворот для робота - это отдельная задача, потребляющая некоторое количество его «жизненных ресурсов» (т.е. заряда аккумуляторов), и занимающая немало времени.
  4. Так как большинство мобильных роботов не могут крутиться на месте, то следует учитывать невозможность осуществления резкого поворота.
  5. Следует соизмерять масштаб карты с командами, подаваемыми на плату управления двигателями.
  6. По возможности нужно избегать попадания в «красные» участки карты.

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

Примечание: Как пример, автор рассматривал код стратегической игры Warcraft 2000.

В данной ситуации автору при разработке собственного робота очень помогла задача, которую он решал, будучи еще студентом, на занятиях по дисциплине «Искусственный интеллект».
Суть задачи в следующем: есть две абстрактных божьих коровки (мужского пола и женского). Для простоты, будем называть самца «быком», а самку - «коровой».
Они расположены на некотором поле, причем на поле есть холмы и ямы. Холмы обозначены красным, ямы черным.
Изначально у быка - 5 жизней.
Если бык залезает на холм - он теряет одну жизнь.
Если падает в яму - теряет две жизни.
При этом, бык имеет размер 8х8 пикселов, и это должно учитываться при его передвижении.
Задача проста - бык должен дойти до коровы, не умерев...

Такая задача уже гораздо более похожа на ту, которую предстоит решать при реализации поиска пути для робота. Учитывается размер быка, и есть «опасные» зоны, которые необходимо по возможности обходить. Остаются, конечно, нерешенные проблемы (пп. 3-5), однако они не критичны. Соизмерение масштаба карты и управления двигателями должен взять на себя драйвер приводов. Наименьшее количество поворотов достигается «срезанием» пути, алгоритм которого можно позаимствовать из компьютерных игр. Одна из возможных реализаций: между каждыми двумя точками итогового пути проводится линия, и проверяется, не встречает ли эта линия препятствий на карте. Плавность поворотов достигается путем интерполяции полученной линии в местах резких поворотов стандартными математическими методами.

Итак, рассмотрим решение задачи «Быки и коровы».

Корова в принципе стоит на месте, и с ней мы ничего сделать не можем. А а вот бык двигается вперед, опираясь на следующую информацию:

  1. Расстояние до коровы должно сокращаться
  2. Переход на другую подстилку(холм/яма) крайне нежелателен
  3. Проход по ранее посещенным местам нежелателен
  4. Чем ближе до цели и чем дольше бродит жук, тем больше вероятность, что он пойдет на таран

Дальше можно поступить, как поступают разработчики логических игр (шахматы, крестики-нолики и т.д.). А именно, назначить каждому правилу определенный «вес», затем путем сложения этих весов получается общий вес конкретной позиции быка. Из всех возможных вариантов движения (вперед, назад, влево, вправо, по диагонали) - выбирается тот, вес которого наилучший.

Приступим к реализации. Для начала, определим, какие переменные и функции мы должны создать для того, чтобы следовать указанным правилам:

  • Чтобы учесть п. 1 правил, нам нужна функция, которая вычисляет текущее расстояние до коровы, и переменная, в которой мы будем хранить наилучшее расстояние из достигнутых. Кроме того, для реализации тарана - требуется также знать точное местоположение точки, в которой была достигнута минимальная дистанция.
  • Потребуется функция для определения типа подстилки в некоторой точке карты. При этом на тип подстилки нужно проверять все пространство 8х8 точек, которое занимает наш бык. Кроме того, естественно, нужна переменная, которая бы хранила текущую подстилку, по которой движется бык.
  • Требуется учитывать, какие места карты уже были посещены, и сколько раз. Чем меньше раз была посещена та или иная точка, тем более вероятно, что робот туда пойдет. Для этого потребуется создать двумерный массив целых чисел, равный по размеру карте местности.
  • Необходимо вести временной учет длительности хождения быка с момента последнего тарана. Также, нужна переменная, которая определяет, идет ли сейчас бык на таран, или нет.

Создадим объект TBull (бык), его определение будет иметь следующий вид:

  TBull=object
    cowx,cowy:integer;
    top,left:integer;
    width,height:integer;
    lifes:shortint;
    memory:array [0..FieldHeight-1,0..FieldWidth-1]of byte;
    bestdistance:integer;
    distchange:integer;
    stepcount:integer;
    sbt:longint;
    taran:boolean;

    procedure reset;
    procedure go(x,y:integer);
    procedure setcow(x,y:integer);
    procedure step;
    function calcdistance(x,y:integer):integer;
    function calcfloor(x,y:integer;var front:integer):integer;
    function calcrepeat(x,y:integer):integer;
  end;

Опишу неупомянутые до сих пор переменные:
lifes - содержит текущее количество жизней быка.
distchange - определяет изменение дистанции до коровы с момента предыдущего хода.
Загадочная переменная sbt - это та самая переменная, которая содержит количество шагов быка с момента последнего тарана, а значит, определяет и время с момента последнего тарана. Читать это название следует, как аббревиатуру SBT = steps before taran.
Остальные переменные названы, надеюсь, достаточно понятно. Опишу функции и процедуры:
go - процедура, которая перемещает жука в указанную точку
step - шаг робота, основная процедура, в ней реализуется вся логика передвижения робота
calcrepeat - функция, которая подсчитывает, насколько «посещена» данная точка, путем сложения количества посещений всех точек, соответствующих быку.

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

  function TBull.calcdistance(x,y:integer):integer;
  begin
    calcdistance:=round(566 - sqrt(sqr(x-cowx) + sqr(y-cowy)));
  end;

Еще одна вспомогательная функция, которая наглядно показывает, как отрабатывается размер робота, а также принципы обработки BMP-изображения - calcfloor.

function TBull.calcfloor(x,y:integer;var front:integer):integer;
var i,j:integer;
    t:array[0..2]of byte;
    f:byte;
begin
  f:=0;
  for i:=0 to 2 do t[i]:=0;

  for i:=x-GodCowWidth div 2 to x+GodCowWidth div 2-1 do
    for j:=y-GodCowHeight div 2 to y+GodCowHeight div 2-1 do
      case (Form1.imgMap.Canvas.Pixels[i,j]) of
        clRed:inc(t[1]);
        clBlack:inc(t[2]);
      else
        inc(t[0]);
      end;

  if (t[0]>t[1])and(t[0]>t[2]) then f:=0;
  if (t[1]>t[0])and(t[1]>t[2]) then f:=1;
  if (t[2]>t[1])and(t[2]>t[0]) then f:=2;

  // front - процент вредных примесей в текущей подстилке
  front:=round((GodCowWidth*GodCowHeight-t[f])/(GodCowWidth*GodCowHeight)*100);

  calcfloor:=f;
end;

Отработка размера быка всегда сопровождается циклической обработкой следующего вида:

  for i:=x-GodCowWidth div 2 to x+GodCowWidth div 2-1 do
    for j:=y-GodCowHeight div 2 to y+GodCowHeight div 2-1 do

Обратите внимание, координаты x и y считаются координатами центра быка. GodCowWidth и GodCowHeight - это константы, равные ширине и высоте быка в пикселах. В случае робота, нужно будет обмерить его максимальные значения для ширины и длины, соизмерить с масштабом карты, и внести в настройки программы управления робота. Причем, для упрощения, и если робот имеет более-менее круглую или квадратную форму, желательно, чтобы в программе он принимался за квадрат. Иначе - если робот сильно вытянут - алгоритм придется усложнять, вводя понятие текущего угла поворота и пересчитывать форму при каждом изменении этого угла.

Наконец, перейдем к рассмотрению кода основной процедуры, реализующей алгоритм поиска пути для быка.

procedure TBull.step;
var i,j:integer; // переменные цикла
    t, // вычисляемый общий показатель позиции ("вес")
    p, // (pain) повреждение, которое получит бык
    d, // (distance) вычисляемая дистанция
    front, // вычисляемое количество примесей в подстилке
    my_f, // (my_floor) на какой подстилке жук находится в данный момент
    my_d // (my_distance) текущее расстояние до коровы
    :integer;
    best:record // лучшая из найденных позиций
      t,x,y,pain:integer;
    end;

Первым делом - быстрая проверка, не нашли ли мы уже добычу (корову).

  if (abs(cowx-(left+GodCowWidth div 2))<=GodCowWidth) and
     (abs(cowy-(top+GodCowHeight div 2))<=GodCowHeight) then
   exit;

Далее инициализируем переменные текущего состояния. Лучшей позиции для перемещения пока не найдено, пока что присваиваем ей отрицательное значение.

  best.t:=-maxint;
  my_f:=calcfloor(left+GodCowWidth div 2,top+GodCowHeight div 2,front);
  my_d:=calcdistance(left+GodCowWidth div 2,top+GodCowHeight div 2);

Далее - основной цикл. Как Вы помните, жук имеет размер 8x8 пикселов, а его координаты - x и y, расположены в центре этого квадрата 8х8. Для небольшого упрощения задачи, мы просматриваем все точки внутри быка, и находим из них самую лучшую. Таким образом, перемещение осуществляется быстрее, чем если бы мы просматривали квадратик в 3х3 пиксела, и одновременно гарантируется, что жук не проскочит узкие щели на карте (благодаря тому, что известен размер жука).
Основное условие внутри цикла позволяет нам избежать «уползания» жука за край карты.

  for i := left to left+GodCowWidth do
    for j := top to top+GodCowHeight do
      if (i > GodCowWidth div 2) and (i < FieldWidth-GodCowWidth div 2-1) and
         (j > GodCowHeight div 2) and (j < FieldHeight-GodCowHeight div 2-1) then begin

Вычисляем характеристики и «вес» для данной точки:

        d:=calcdistance(i,j);
        t:=round(d*2*(sbt/200+1));
        p:=calcfloor(i,j,front);
        if (p=0)and(my_f<>0) then
          if (d-my_d<4) then
            t:=t-1000
          else
            t:=t-100;
        if p=my_f then p:=0;
        if front>45 then t:=t-100;
        t:=t+(2-p)*1000;
        t:=t+calcrepeat(i,j);

Отдельно обрабатывается таран: если в данный момент жук находится в состоянии тарана, и при этом разница между дистанцией до коровы в данной точке, и лучшей дистанцией, которую достигал жук, минимальна (не превышает 10 пикселов), то вес текущей позиции увеличивается, при этом компенсируется операция t:=t+(2-p)*1000 из предыдущего фрагмента кода, которая значительно уменьшает вес тех позиций, которые предполагают переход на другую подстилку.

        if (taran)and(bestdistance-d<=10) then
          t:=t+2000*(p+1);

Если позиция оказалась наилучшей, запомним ее характеристики.

        if t>best.t then begin
          best.t:=t;
          best.x:=i;
          best.y:=j;
          best.pain:=p;
        end;

На этом основной цикл завершен, осталось собственно совершить переход, а также реализовать дополнительную логику тарана. Ведь на текущий момент, мы таран обработали, но при каких условиях жук переходит в состояние тарана, и как это делает - пока не указали.
Сначала рассмотрим переход:

      end; // окончание основного цикла

  if best.t<>-maxint then begin
    if not taran then inc(sbt); // если мы не находимся в состоянии тарана - увеличим нетерпение быка
    inc(stepcount); // увеличиваем общее число шагов
    i:=left+GodCowWidth div 2;
    j:=top+GodCowHeight div 2;
    distchange:=calcdistance(best.x,best.y)-my_d; // определяем, насколько изменилась дистанция до коровы
    go(best.x,best.y); // собственно, переходим на новую позицию

Нужно ранить жука, в случае, если он перешел на другую подстилку. В случае реализации поиска пути для робота, сам факт ранения, конечно, не потребуется. Однако кроме просто отсчета жизней, данный фрагмент реализует сброс состояния «таран». Иными словами, жук идет на таран только до первого перехода, а затем снова переходит в обычное состояние поиска.

    if best.pain>0 then begin
      dec(lifes,best.pain);
      TimerEnabled:=false;
      Application.MessageBox(PChar('Ай, больно! Осталось жизней: '+inttostr(lifes)),'Божий бык',0);
      TimerEnabled:=true;
      taran:=false;
    end;

Вычисляем необходимость тарана. Если идем на таран - память посещений клеток очищается.

    if not taran and(sbt>500) then begin
      TimerEnabled:=false;
      Application.MessageBox('Надоело бродить! Пойду-ка на таран...','Божий бык',0);
      TimerEnabled:=true;
      taran:=true;
      sbt:=0;
      for i:=0 to FieldWidth-1 do // очищаем память
        for j:=0 to FieldHeight-1 do
          memory[i,j]:=0;
    end;
    // отображаем текущее состояние быка
    Form1.StatusBar1.Panels[0].Text:='Жизней: '+inttostr(lifes);
    Form1.StatusBar1.Panels[1].Text:='Шагов: '+inttostr(stepcount);
  end; // конец процедуры

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

Я рассмотрел алгоритм поиска пути и его реализацию достаточно подробно, специально для того, чтобы указать на богатство выбора решений.
Ведь на сегодняшний день функция передвижения - весьма специфичная вещь для каждого робота. Кроме карты местности, учитываются еще показания разнообразных датчиков. Также, Вы можете захотеть построить/достроить карту местности на основе показаний датчиков - это тоже очень интересная задача, связанная уже с самообучением. А от такого простейшего рода самообучения, прямой шаг и к более сложным понятиям из области искусственного интеллекта.
Простор фантазии не ограничен, так что, отбросьте шаблоны, и пишите сами :)

Ссылки по теме статьи