Рух робота до точки із заданими координатами

Вітаємо вас, шановні хабравчане!
Наш науковий колектив, який носить назву Студентського конструкторського бюро кафедри СУиИ Університету ІТМО, продовжує розробку курсів з робототехніки, і хоче поділитися одним з останніх проектів на Lego NXT.

Раніше ми публікували курс «Практична робототехніка»NXT. Зараз цей курс використовується для навчання студентів на кафедрі, і на майданчику «Відкрита освіта». Так само публікувалися фрагменти цього курсу з докладним описом дій для ідентифікації моделі двигуна і розрахунку регулятора для робота Segway.
В цей раз було вирішено реалізувати об'їзд перешкод роботом з диференціальним приводом. Конструкція робота досить проста: два колеса з двигунами, гіроскоп і пара ультразвукових датчиків. Для оцінки пройденого відстані використовуються енкодери на валу двигуна, для орієнтації робота, вимірюється гіроскопом його кутова швидкість і розраховується кут повороту, а відстань до перешкоди вимірюється ультразвуковими далекомірами.


Варто відзначити, що робот переміщається за рахунок руху двох окремо керованих коліс. А така кінематична схема має певне математичне опис:

де – відповідні кутові швидкості обертання коліс, x, y – відповідні координати, R – радіус колеса, B – відстань між колесами.

В принципі рух в точку з заданими координатами можна вирішити використанням П-регулятора, але на практиці цього буде недостатньо. Хоча в моделі, представленої нижче все працює просто чудово:


Задача ставиться наступним чином: робот повинен досягти заданих координат .
При вирішенні завдань навігації мобільних роботів використовуються два основних підходи.
  • Глобальний – визначення абсолютних координат пристрою при русі по довгих маршрутах. Траєкторія вибирається ще до початку руху на основі отриманої інформації.
  • Локальний – визначення координат пристрою по відношенню до деякої (зазвичай стартової) точці. Планування задає лише невеликий відрізок траєкторії, в кінцевій точці якого вибирається подальша траєкторія.
Існує безліч методів локальної навігації, наприклад, такі як:
Зупинимося докладніше на останньому методі. Пошукавши в мережі інформацію про способи управління подібним роботом, можна знайти на статті професора кафедри теоретичної кібернетики Спбду Матвєєва А. С. і професора Савкіна А. В. з Університету Нового Південного Уельсу, Сідней, Австралія про алгоритм навігації робота серед рухомих і деформівних перешкод. У статті автори стверджують, що найбільш придатним методом для вирішення нашої задачі є, власне, метод тангенціального уникнення, описаний професором Федерального університету Эспиринту-Санту, Віторія, Бразилія Маріо Сарцинелли, з яким ми мали честь познайомитися під час поїздки нашої команди в Бразилії.

Для початку опишемо математичну модель, що описує навігацію робота до мети, в полярних координатах:


Фактично, робот може управлятися за допомогою значень кутової та лінійної швидкості , тому потрібно знайти такі їх значення, щоб виконувалася умова поставленої задачі . Для цього в статті пропонується скористатися апаратом функції Ляпунова. Це буде квадратична функція, що включає в себе відстань до цілі і курсової кут:


Похідна по часу повинна бути не позитивна для того, щоб відстань до цілі і курсової кут не зростали. Похідна виглядає наступним чином:


Висловивши похідну через математичну модель, запропоновану вище, отримуємо:


Ця похідна негативно визначена, якщо ми виберемо в якості керуючого впливу наступні значення швидкостей:


Все це дозволить роботу досягти своєї мети, але поки лише у відсутності перешкод. Для уникнення перешкод необхідно внести поправку до обчислення курсового кута:

де – коефіцієнт пропорційної складової курсового кута – відстань до перешкоди, – мінімальна відстань до перешкоди.

З допомогою вищевикладеного методу було реалізовано рух із точки в точку з об'їздом перешкоди. Результати представлені на відео, похибка обумовлена прослизанням коліс.
Також ми додали інтегральну складову для розрахунку лінійної швидкості, щоб компенсувати тертя моторів. На відео нижче видно, що робот досить точно досягає мети в околиці 3 сантиметри.
Нижче наведено приклад підсумкової програми на сі-подібному мовою NXC:
Код програми
#define RAD_WHEEL 0.028 // радіус коліс
#define DEG2RAD PI / 180
#define LEFT OUT_C
#define RIGHT OUT_B
#define GYRO S1
#define LIMIT 20 // граничне відстань до перешкоди
#define K_I 50 // коефіцієнт інтегральної складової лінійної швидкості
#define K_P 0.1 // коефіцієнт пропорційної складової курсового кута
#define ERROR 0.03 // точність досягнення цільової точки

task main() {
int rotA, rotB, // показання енкодерів
pwmLeft, pwmRight, // ШІМ для керування
distS3, distS2, distMax, kDist, // помилки відстані до перешкоди
currentTime, previousTime, dt, // час
gyroSpeed, gyroOffset, // кутова швидкість з гіроскопа та її калібрування
fileSize = 30640;

float course, courseAngle, bearing, // курс (psi), курсовий кут (alpha), пеленг (theta)
xCoord, yCoord, // координати точки між колесами
delthaX, delthaY, // відхилення
length, intLength, // довжина залишився шляху, інтеграл довжини
path, prevPath, delthaPath, // шлях пройдений точкою між колесами
xRef = -1, yRef = 0, // координати точки призначення
baseSpeed = 0, control = 0, // лінійна і кутова швидкість
k = 20;

string s; // рядок для формування висновку

byte handle;

// ініціалізація датчиків
SetSensorLowspeed(S2);
SetSensorHTGyro(GYRO);

// створення файлу для запису даних, затримка
DeleteFile("data.txt");
CreateFile("data.txt", fileSize, handle);
Wait(50);

// калібрування гіроскопа, кутова швидкість (град/сек)
gyroOffset = SensorHTGyro(GYRO);

course = 0;
previousTime = CurrentTick();

while (true) {
// помилки вимірювання відстані до перешкоди
distS3 = LIMIT - SensorUS(S3);
distS2 = LIMIT - SensorUS(S2);

// визначення положення перешкоди (з якого боку робота, далеко від нього)
if (distS3 < 0 ) distS3 = 0;
if (distS2 < 0 ) distS2 = 0;
if (distS2 > distS3) distMax = distS2;
else distMax = distS3;
kDist = sign(distS2 - distS3);

currentTime = CurrentTick();
dt = currentTime - previousTime;
previousTime = currentTime;

// отримання кутової швидкості (град/сек)
gyroSpeed = SensorHTGyro(GYRO) - gyroOffset;
// розрахунок курсу
course = course + gyroSpeed * PI * dt / 1000.0 / 180.0;

// зняття показань енкодерів з двигунів
rotA = MotorRotationCount(LEFT);
rotB = MotorRotationCount(RIGHT);

// розрахунок шляху, пройденого точкою між колесами
path = (rotA + rotB) * DEG2RAD * RAD_WHEEL / 2;

// шлях пройдений за один цикл програми
delthaPath = path - prevPath;

// збереження попереднього значення довжини шляху
prevPath = path;

// обчислення координати X
xCoord = xCoord + delthaPath * cos(course);

// обчислення координати Y
yCoord = yCoord + delthaPath * sin(course);

// обчислення відхилення по X
delthaX = xRef - xCoord;

// обчислення відхилення по Y
delthaY = yRef - yCoord;

// розрахунок пеленга
bearing = atan2(delthaY, delthaX);

// розрахунок курсового кута
courseAngle = bearing - course - K_P * kDist * distMax;

// розворот по найкоротшому куті
if (abs(courseAngle) > PI) courseAngle = courseAngle - sign(courseAngle) * 2 * PI;

// розрахунок відстані до точки
length = sqrt (delthaY * delthaY + delthaX * delthaX);
intLength = intLength + length * dt / 1000;
if (intLength > 10) intLength = 10;

// розрахунок лінійної швидкості
baseSpeed = 100 * tanh (length) * cos (courseAngle) + K_I * intLength;
if (abs(baseSpeed) > 40) baseSpeed = sign (baseSpeed) * 40;

// розрахунок кутової швидкості
control = k * courseAngle + sin(courseAngle) * baseSpeed / length;

// насичення керуючого впливу
if (abs(control) > 30) control = sign(control)*30;

// перетворення управління з float в integer, ШІМ у відсотках
pwmLeft = baseSpeed + control;
// перетворення управління з float в integer, ШІМ у відсотках
pwmRight = baseSpeed - control;

if (abs(pwmLeft) > 100) pwmLeft = sign (pwmLeft) * 100;
if (abs(pwmRight) > 100) pwmLeft = sign (pwmRight) * 100;

// подання управління
OnFwd(LEFT, pwmLeft);
OnFwd(RIGHT, pwmRight);
ClearScreen();

// виведення значень на екран NXT
NumOut(0, 8, distS3);
NumOut(0, 0, distS2);

// створення рядки з даними
s = NumToStr(xCoord) + " " + NumToStr(yCoord) + " " + NumToStr(bearing) + " " + NumToStr(course);

// запис в файл
WriteLnString(handle, s, fileSize);

// вихід при досягненні точки області
if(abs(length) < ERROR) {
Off(OUT_BC);
break;
}
Wait(5);
}
}


Дана задача включена в дисципліну робототехніки для першого і другого курсу нашої кафедри, куди крім неї входять segway, перевернутий маятник на візку, маятник Капіци і трехзвенный маніпулятор. Як бачите, навіть на такому простому обладнанні цілком можна розбирати серйозні робототехнічні завдання.

Джерело: Хабрахабр

0 коментарів

Тільки зареєстровані та авторизовані користувачі можуть залишати коментарі.