🎯 Цель: Программируем робота для точного движения по геометрическим фигурам
⭐ Результат: Создаем программы с компенсацией физических факторов
👨🏫 Учитель: Ахметов Рустам
🏫 Школа: ГБОУ № 1362
📅 Дата: 2025-06-14
⏰ Время: 95 минут
Программируем точное движение:
Наш робот должен:
🎪 Это настоящий вызов для программистов роботов!
🟦 Квадрат:
🔺 Треугольник:
♾️ Восьмерка:
Формула для правильного многоугольника: \[\alpha = \frac{360°}{n}\]
где n - количество сторон
Примеры:
Что происходит при повороте:
Факторы, влияющие на инерцию:
Методы компенсации:
1. Снижение скорости перед поворотом:
void turnWithCompensation(int angle) {
setSpeed(50); // Замедляемся
delay(200); // Даем время затормозить
turn(angle); // Поворачиваем
delay(100); // Даем время повернуться
setSpeed(100); // Восстанавливаем скорость
}
2. Поправочные коэффициенты:
int compensatedAngle = targetAngle * 1.05; // Компенсация недоворота
int compensatedTime = targetTime * 0.95; // Компенсация инерции
Центростремительная сила при повороте:
\[F_c = \frac{m v^2}{R}\]где:
Максимальная скорость поворота без проскальзывания:
\[v_{max} = \sqrt{\mu g R}\]где μ - коэффициент трения
Момент инерции для поворота:
\[I = \frac{1}{2} m (a^2 + b^2)\]для прямоугольного робота с размерами a×b
Угловое ускорение:
\[\epsilon = \frac{M}{I}\]где M - момент силы от моторов
Время поворота на угол θ:
\[t = \sqrt{\frac{2\theta}{\epsilon}}\]при равноускоренном движении от покоя
Основные команды движения:
// Движение вперед на заданное расстояние
void moveForward(int distance, int speed) {
// Расчет времени движения
int time = (distance * 1000) / speed; // мс
motors.setSpeed(speed);
motors.forward();
delay(time);
motors.stop();
}
// Поворот на заданный угол
void turnRight(int angle) {
// Калибровочный коэффициент для данного робота
float turnCoeff = 5.5; // мс на градус
int turnTime = angle * turnCoeff;
motors.turnRight();
delay(turnTime);
motors.stop();
}
Алгоритм движения по квадрату:
void drawSquare(int sideLength) {
Serial.println("Начинаем рисовать квадрат");
for (int i = 0; i < 4; i++) {
Serial.print("Сторона ");
Serial.println(i + 1);
// Движение по стороне
moveForward(sideLength, 80);
delay(500); // Пауза для стабилизации
// Поворот на 90 градусов
turnRight(90);
delay(500); // Пауза для стабилизации
}
Serial.println("Квадрат завершен!");
}
void setup() {
Serial.begin(9600);
motors.init();
}
void loop() {
drawSquare(200); // Квадрат со стороной 20 см
delay(5000); // Пауза перед повтором
}
Особенности треугольника:
void drawTriangle(int sideLength) {
Serial.println("Начинаем рисовать треугольник");
for (int i = 0; i < 3; i++) {
Serial.print("Сторона ");
Serial.println(i + 1);
// Движение по стороне
moveForward(sideLength, 70); // Чуть медленнее для точности
delay(700); // Больше времени на стабилизацию
// Поворот на 120 градусов с компенсацией
compensatedTurn(120);
delay(700);
}
Serial.println("Треугольник завершен!");
}
void compensatedTurn(int angle) {
// Снижаем скорость перед поворотом
motors.setSpeed(40);
delay(200);
// Поворот с поправкой на инерцию
turnRight(angle * 1.08); // Компенсация недоворота
delay(300); // Время на завершение поворота
}
Самая сложная фигура:
void drawFigureEight(int radius) {
Serial.println("Начинаем рисовать восьмерку");
// Первый полукруг (правый)
drawSemicircle(radius, true); // по часовой стрелке
// Переход в центр
delay(500);
// Второй полукруг (левый)
drawSemicircle(radius, false); // против часовой стрелки
Serial.println("Восьмерка завершена!");
}
void drawSemicircle(int radius, bool clockwise) {
int steps = 18; // 18 шагов по 10 градусов = 180°
int stepAngle = 10;
int arcLength = (2 * PI * radius) / (360 / stepAngle);
for (int i = 0; i < steps; i++) {
moveForward(arcLength, 60);
delay(100);
if (clockwise) {
turnRight(stepAngle);
} else {
turnLeft(stepAngle);
}
delay(100);
}
}
Adaptive Speed Control (адаптивное управление скоростью):
class TrajectoryController {
private:
float currentSpeed;
float targetSpeed;
float acceleration;
public:
void smoothAcceleration(float target, float accel) {
acceleration = accel;
targetSpeed = target;
while (abs(currentSpeed - targetSpeed) > 0.1) {
if (currentSpeed < targetSpeed) {
currentSpeed += acceleration;
} else {
currentSpeed -= acceleration;
}
motors.setSpeed(currentSpeed);
delay(20); // 50 Hz update rate
}
}
void smoothTurn(int angle, float radius) {
// Расчет оптимальной скорости для поворота
float maxSpeed = sqrt(FRICTION_COEFF * GRAVITY * radius);
smoothAcceleration(maxSpeed, 2.0);
// Выполнение поворота
float arcLength = (PI * radius * angle) / 180.0;
executeArc(arcLength, radius);
// Возврат к нормальной скорости
smoothAcceleration(100, 2.0);
}
};
PID-регулятор для точного позиционирования:
class PIDController {
private:
float kp, ki, kd;
float lastError;
float integral;
public:
PIDController(float p, float i, float d) : kp(p), ki(i), kd(d) {
lastError = 0;
integral = 0;
}
float calculate(float setpoint, float actual) {
float error = setpoint - actual;
integral += error;
float derivative = error - lastError;
float output = kp * error + ki * integral + kd * derivative;
lastError = error;
return output;
}
};
// Использование для коррекции траектории
PIDController anglePID(2.0, 0.1, 0.5);
void correctiveMove(float targetDistance) {
float actualDistance = getDistanceFromSensor();
float correction = anglePID.calculate(targetDistance, actualDistance);
// Применяем коррекцию к скорости моторов
motors.setLeftSpeed(baseSpeed + correction);
motors.setRightSpeed(baseSpeed - correction);
}
Метрики для оценки качества:
1. Точность возврата в исходную точку:
struct Point {
float x, y;
};
Point startPoint = {0, 0};
Point endPoint;
float calculateDeviation() {
return sqrt(pow(endPoint.x - startPoint.x, 2) +
pow(endPoint.y - startPoint.y, 2));
}
2. Отклонение от эталонной траектории:
float maxDeviation = 0;
float currentDeviation;
void checkTrajectory(Point current, Point reference) {
currentDeviation = distance(current, reference);
if (currentDeviation > maxDeviation) {
maxDeviation = currentDeviation;
}
}
Алгоритм автоматической калибровки:
void calibrateMovement() {
Serial.println("Начинаем калибровку...");
// Калибровка прямолинейного движения
float distanceCoeff = calibrateDistance();
// Калибровка поворотов
float turnCoeff = calibrateTurns();
// Сохранение коэффициентов
saveCalibration(distanceCoeff, turnCoeff);
Serial.println("Калибровка завершена!");
}
float calibrateDistance() {
// Движение на известное расстояние
moveForward(1000, 80); // 1 секунда на скорости 80
// Измерение фактического расстояния
float actualDistance = measureDistance();
// Расчет коэффициента
return actualDistance / 80.0; // мм на единицу скорости
}
float calibrateTurns() {
for (int i = 0; i < 4; i++) {
turnRight(90);
delay(500);
}
// Измерение отклонения от исходного направления
float angleError = measureAngleError();
// Коэффициент коррекции
return 1.0 + (angleError / 360.0);
}
Least Squares для калибровки:
Если у нас есть n измерений пар (команда, результат):
\[\min \sum_{i=1}^{n} (y_i - (a \cdot x_i + b))^2\]Решение:
\[a = \frac{n\sum x_i y_i - \sum x_i \sum y_i}{n\sum x_i^2 - (\sum x_i)^2}\] \[b = \frac{\sum y_i - a\sum x_i}{n}\]Adaptive learning алгоритм:
class AdaptiveLearning {
private:
float learningRate = 0.1;
float distanceCoeff = 1.0;
float turnCoeff = 1.0;
public:
void updateFromError(float commandDistance, float actualDistance) {
float error = actualDistance - commandDistance;
float gradient = error / commandDistance;
distanceCoeff -= learningRate * gradient;
// Ограничиваем коэффициент разумными пределами
distanceCoeff = constrain(distanceCoeff, 0.5, 2.0);
}
float getOptimalDistance(float target) {
return target * distanceCoeff;
}
};
Проблема: Резкие повороты создают рывки и неточности
Решение: Сплайн-интерполяция
struct Waypoint {
float x, y;
float heading;
};
class SmoothTrajectory {
public:
void addWaypoint(float x, float y, float heading) {
waypoints.push_back({x, y, heading});
}
void executeSmoothPath() {
for (int i = 0; i < waypoints.size() - 1; i++) {
smoothTransition(waypoints[i], waypoints[i+1]);
}
}
private:
vector<Waypoint> waypoints;
void smoothTransition(Waypoint start, Waypoint end) {
// Bezier curve interpolation
int steps = 20;
for (int i = 0; i <= steps; i++) {
float t = (float)i / steps;
Waypoint current = interpolate(start, end, t);
moveToPoint(current);
}
}
Waypoint interpolate(Waypoint p1, Waypoint p2, float t) {
return {
p1.x + t * (p2.x - p1.x),
p1.y + t * (p2.y - p1.y),
p1.heading + t * (p2.heading - p1.heading)
};
}
};
Интеграция с энкодерами колес:
class EncoderNavigation {
private:
long leftEncoder, rightEncoder;
float wheelDiameter = 65.0; // мм
int encoderPPR = 360; // импульсов на оборот
public:
void updatePosition() {
long leftTicks = getLeftEncoder() - leftEncoder;
long rightTicks = getRightEncoder() - rightEncoder;
float leftDistance = ticksToMM(leftTicks);
float rightDistance = ticksToMM(rightTicks);
// Расчет изменения позиции
float deltaDistance = (leftDistance + rightDistance) / 2.0;
float deltaAngle = (rightDistance - leftDistance) / wheelBase;
// Обновление текущей позиции
currentX += deltaDistance * cos(currentHeading);
currentY += deltaDistance * sin(currentHeading);
currentHeading += deltaAngle;
leftEncoder = getLeftEncoder();
rightEncoder = getRightEncoder();
}
private:
float ticksToMM(long ticks) {
return (ticks * PI * wheelDiameter) / encoderPPR;
}
};
Reinforcement Learning для калибровки:
class QLearning {
private:
float qTable[STATES][ACTIONS];
float learningRate = 0.1;
float discountFactor = 0.9;
float explorationRate = 0.1;
public:
int selectAction(int state) {
if (random(100) < explorationRate * 100) {
return random(ACTIONS); // Исследование
} else {
return getBestAction(state); // Использование знаний
}
}
void updateQ(int state, int action, float reward, int nextState) {
float maxQ = getMaxQ(nextState);
float oldQ = qTable[state][action];
qTable[state][action] = oldQ + learningRate *
(reward + discountFactor * maxQ - oldQ);
}
float calculateReward(float trajectoryError) {
// Чем меньше ошибка, тем больше награда
return 1.0 / (1.0 + trajectoryError);
}
};
Genetic Algorithm для оптимизации параметров:
struct Individual {
float distanceCoeff;
float turnCoeff;
float speed;
float fitness;
};
class GeneticOptimizer {
private:
vector<Individual> population;
int populationSize = 20;
float mutationRate = 0.1;
public:
void evolve() {
// Оценка приспособленности
evaluateFitness();
// Селекция лучших
selection();
// Скрещивание
crossover();
// Мутация
mutation();
}
private:
void evaluateFitness() {
for (auto& individual : population) {
individual.fitness = testTrajectory(individual);
}
}
float testTrajectory(Individual params) {
// Запуск робота с данными параметрами
// Измерение точности траектории
// Возврат обратной величины ошибки
return 1.0 / (1.0 + measureTrajectoryError(params));
}
};
Система баллов:
| Критерий | Отлично (5) | Хорошо (4) | Удовлетв. (3) | Плохо (2) |
|---|---|---|---|---|
| Точность фигуры | ±2 см | ±5 см | ±10 см | >10 см |
| Возврат в исходную точку | ±3 см | ±7 см | ±15 см | >15 см |
| Плавность движения | Без рывков | Малые рывки | Заметные рывки | Сильные рывки |
| Стабильность | 100% успех | 75% успех | 50% успех | <50% успех |
Соревнование 1: “Точность”
Соревнование 2: “Скорость и точность”
Соревнование 3: “Творческая траектория”
Программные навыки:
Инженерные навыки:
Взаимосвязь дисциплин:
Где используются эти принципы:
1. Алгоритм для многоугольника Составьте алгоритм движения робота по правильному пятиугольнику. Рассчитайте:
2. Отчет о практической работе Подготовьте краткий отчет (1-2 страницы):
3. Программа для спирали Разработайте программу для движения робота по спирали с постепенно уменьшающимся радиусом:
4. Исследование влияния скорости Проведите эксперимент:
5. Адаптивный алгоритм Разработайте программу, которая автоматически подстраивает параметры движения:
6. Сложная траектория Создайте программу для движения по произвольной траектории, заданной массивом точек:
Практические результаты:
Новые навыки:
“Точное движение робота - это не просто команды ‘вперед’ и ‘поворот’. Это искусство сочетания математики, физики и программирования для достижения идеального результата!”
Современные применения:
🎯 Сегодня вы изучили основы высокоточного робототехнического управления!
Программирование роботов:
Для школьников:
⭐ Для углубленного изучения:
Симуляторы и среды разработки:
Идеи для развития:
Успехов в программировании точного движения роботов! 🎯🤖✨