Двухмоторная тележка nxt инструкция по сборке

Занятие 20. Двухмоторная тележка

Соседние файлы в предмете [НЕСОРТИРОВАННОЕ]

• #
• #
• #
• #
• #
• #
• #
• #
• #
• #
• #

Двухмоторная тележка

Последнее время вокруг все больше разговоров об искусственном интеллекте, то там то тут звучат модные термины «нейросети» и «генетические алгоритмы». В прошлых проектах (НейроКачели, НейроБашня и N3uralV1s10n) мы уже создавали простейшие нейронные сети, разобрались с тем что это такое в первом приближении и как они работают. Похоже пришло время сделать тоже самое с генетическими алгоритмами.

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

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

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

Начнем с конструкции робота. Это традиционная двухмоторная тележка на базе LEGO Mindstorms NXT, в передней части установлено 4 датчика освещенности, два из которых (внутренних, подключенных к портам 2 и 3) используются ПИД-регулятором робота для движения по линии, а два внешних(подключенных соответственно к портам 1 и 4) — для контроля срыва с линии в процессе обучения. Инструкцию в формате LEGO Digital Designer можно скачать по ссылке.

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

Алгоритм в общем виде выглядит следующим образом:

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

struct person
{
  // у каждой особи должно быть имя, хотя бы codename
  string name;
  // номер поколения, в котором родилась данная особь
  int generation;
  // энергичность (быстрота) особи
  int speed;
  // скорость реакции
  float reaction;
  // мудрость (память) особи
  float memory;
  // интуиция (проницательность)
  float intuition;
  // степень доминантности особи
  float dominance;
  // пройденный особью путь за отведенный на тестирование промежуток времени
  int path;
};

Теперь создадим первую популяцию на основе этого шаблона, в ней у нас будет 6 особей:

person robot[6];

Генерируем случайным образом свойства особей первой популяции:

for (int i=0;i<6;i++){
  robot[i].name = «GeneLINEr_»+NumToStr(Random(1000));
  robot[i].generation=1;
  // энергичность (быстрота) особи 0..100
  robot[i].speed = Random(70)+30;
  // скорость реакции 0..3
  robot[i].reaction = Random(3000)/1000;
  // мудрость (память) особи 0..0,1
  robot[i].memory = Random(100)/1000;
  // интуиция (проницательность) 0..3
  robot[i].intuition = Random(3000)/1000;
  // степень доминантности особи не понятна до тестирования
  robot[i].dominance = 0;
  // пройденный особью жизненный путь = 0
  robot[i].path = 0;
}

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

В роботе реализована функция ПИД-регулятора движения по линии, принимающий на вход параметры Kp, Ki, Kd, скорость робота, выполняющая 5 секундное движение по линии и возвращающая длину пройденного пути, сглаженного до криволинейной траектории.

long pid(float Pk,float Ik,float Dk,int speed){
  long B=0;
  long C=0;
  long path=0;
  long MC=MotorRotationCount(OUT_C);
  long MB=MotorRotationCount(OUT_B);
  long e = 0;
  int porog=28;
  float ERRo=0;
  float ERR=0;
  float u=0;
  float z1=0;
  float z2=0;
  long tmp=CurrentTick();
  int p;
  int i;
  int d;
  while(CurrentTick()-tmp<=5000){
    MC=MotorRotationCount(OUT_C);
    MB=MotorRotationCount(OUT_B);
    ERR=Sensor(IN_3)-Sensor(IN_2);
    p=Pk*ERR;
    d=Dk*(ERR-ERRo);
    i=Ik*e;
    if(i>10)i=10;
    if(i<-10)i= -10;
    u=p+i+d;
    z1=speed-u;
    z2=speed+u;
    if(speed-u>100)z1=100;
    if(speed-u<-100)z1=-100;
    if(speed+u>100)z2=100;
    if(speed+u<-100)z2=-100;
    OnFwd(OUT_B,z1);
    OnFwd(OUT_C,z2);
    if(Sensor(IN_1)<=porog){
      PlayTone(TONE_C5, MS_500);
      RotateMotorEx(OUT_BC, 35, 100, 100, true, true);
      go_to_line();
      break;
    }
    if(Sensor(IN_4)<=porog){
      PlayTone(TONE_C5, MS_500);
      RotateMotorEx(OUT_BC, 35, 100, -100, true, true);
      go_to_line();
      break;
    }
    ERRo=ERR;
    e+=ERR;
    B=MotorRotationCount(OUT_B)-MB;
    C=MotorRotationCount(OUT_C)-MC;
    if(B>0 && C>0){
      if(B<C){
        path+=B;
      }
      else{
        path+=C;
      }
    }
  }
  Off(OUT_BC);
  return path;
}


void go_to_line(){
  float P=1.0;
  float D=1.0;
  float ERRo=0;
  float ERR=0;
  float u=0;
  while(abs(Sensor(IN_2)-Sensor(IN_3))>5){
    int ERR=Sensor(IN_3)-Sensor(IN_2);
    int u=P*ERR+D*(ERR-ERRo);
    int z1=-u;
    int z2=+u;
    if(z1>100) z1=100;
    if(z1<-100) z1=-100;
    if(z2>100) z2=100;
    if(z2<-100) z2=-100;
    OnFwd(OUT_B,z1);
    OnFwd(OUT_C,z2);
  }

}

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

for (int i=5;i>=0;i—){
  robot[i].path = pid(robot[i].reaction,
    robot[i].memory,robot[i].intuition,robot[i].speed);
  PlayTone(TONE_A4, MS_500);
}

После испытания всех особей текущего поколения ранжируем их в порядке убывания пройденного за время тестирования пути. Так как время на тест для каждой особи фиксировано — 5 секунд, соответственно у особей убывает и скорость. Будем использовать пузырьковую сортировку и «временную особь», для перестановки пар при ранжировании.

person robot_tmp;

for (int i=0;i<5;i++){
  for (int j=0;j<(5-i);j++){
    if(robot[j].path < robot[j+1].path){
      robot_tmp = robot[j];
      robot[j]=robot[j+1];
      robot[j+1]= robot_tmp;
    }
  }
}

Чем выше у особи рейтинг, тем выше и доминантность данной особи, соответственно тем большую часть свойст данной особи унаследуют ее потомки (выше = ближе к 1):

for (int i=0;i<6;i++){
  robot[i].dominance=i+1;
}

Две особи, самые слабые в популяции (5 и 6 в ранжированном списке) умирают, остальные дают потомство, порождая 6 особей новой популяции, наследующих черты родительских особей. В скрещивании участвуют доминантные признаки особей, давая соотношение унаследованных признаков. Унаследованный признак новорожденного кроме этого подвергается колебанию в 20%, для ускорения эволюции.

person newborn(person male, person female){
  person newburn;
  male.dominance = 1 — (male.dominance/(male.dominance+female.dominance));
  female.dominance = 1 — male.dominance;
  newburn.name = «GeneLINEr_«+NumToStr(Random(1000));
  newburn.generation=male.generation+1;
  if(male.speed>female.speed){
    newburn.speed = male.speed;
  }
  else{
    newburn.speed=female.speed;
  }
  newburn.reaction = male.reaction * male.dominance + female.reaction * female.dominance;
  newburn.reaction =newburn.reaction *((Random(40)+80)/100.0);
  newburn.memory = male.memory * male.dominance + female.memory * female.dominance;
  newburn.memory =newburn.memory *((Random(40)+80)/100.0);
  newburn.intuition = male.intuition * male.dominance + female.intuition * female.dominance;
  newburn.intuition = newburn.intuition *((Random(40)+80)/100.0);
  newburn.path = 0;
  newburn.dominance = 0;
  if(newburn.speed>max_speed){
    max_speed=newburn.speed;
  }
  return newburn;
}

// новая популяция из 6 особей
person robot_next_generation[6];

robot_next_generation[0] = newborn(0,1);
robot_next_generation[1] = newborn(0,2);
robot_next_generation[2] = newborn(0,3);
robot_next_generation[3] = newborn(1,2);
robot_next_generation[4] = newborn(1,3);
robot_next_generation[5] = newborn(2,3);

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

// Мутация
void mutants(){
  int property=Random(3);
  int mutant=Random(5);;
  if(mutant<4){
    switch(property){
      case 0:
        robot_next_generation[mutant].speed = max_speed+5;
        break;
      case 1:
        robot_next_generation[mutant].reaction =robot_next_generation[mutant].reaction *((Random(40)+80)/100.0);
        break;
      case 2:
        robot_next_generation[mutant].memory =robot_next_generation[mutant].memory *((Random(40)+80)/100.0);
        break;
      case 3:
        robot_next_generation[mutant].intuition = robot_next_generation[mutant].intuition *((Random(40)+80)/100.0);
        break;
      default:
        break;
    }
  }
  else{
    switch(property) {
      case 0:
        robot_next_generation[mutant].speed = max_speed+5;
        break;
      case 1:
        robot_next_generation[mutant].reaction = Random(3000)/1000;
        break;
      case 2:
        robot_next_generation[mutant].memory = Random(100)/1000;
        break;
      case 3:
        robot_next_generation[mutant].intuition = Random(3000)/1000;
        break;
      default:
        break;
    }
  }
}

Теперь производим смену популяций. Дети сменяют родителей:

for(int i=0;i<6;i++){
  robot[i]=robot_next_generation[i];
}

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

На этапе ранжирования можно сохранять информацию о свойствах особей в каждой популяции в файл.

byte fh;
int msg_len;
string logstr;


for(int i=0;i<6;i++){
  logstr = robot[i].name;
  logstr = logstr + «;» + NumToStr(robot[i].generation);
  logstr = logstr + «;» + NumToStr(robot[i].reaction) +»;»+NumToStr(robot[i].memory)+»;»+NumToStr(robot[i].intuition);
  logstr = logstr + «;» + NumToStr(robot[i].path)+»;»+NumToStr(robot[i].speed);
  msg_len = StrLen(logstr);
  WriteLnString(fh, logstr, msg_len);
}

По данным, накопленным в файле в процессе работы программы, можно представить ход эволюции в виде наглядного графика (ось X — номер поколения, ось Y — средняя скорость особи):