Вопросы для собеседования по воплощённому ИИ, набор 2, с ответами

RoboticsX (июнь 2024)

⭐️ Первое собеседование: с ментором (около 35 минут)

• Рассказ о прошлых проектах: общее знакомство с участием кандидата в проектах и разбор контекста.
• Вопросы кандидата: какие конкретные задачи могут быть поручены на стажировке.

⭐️ Второе собеседование: с руководителем группы (около 1 часа)

Интервьюер — выпускник NTU с докторской степенью, специалист по классическому управлению. Вопросы очень детальные, ниже они разбиты по проектам.

1. Проект по планированию с обучением с подкреплением

• Получение данных: как получен датасет симуляции?
• Устойчивость алгоритма: может ли возникнуть застревание в локальном оптимуме (например, в тупике, откуда невозможно выбраться)?
• Моделирование в симуляции: почему в Gazebo выбрана дифференциальная модель (Differential Model)?

2. Проект VLN (навигация по языковым инструкциям)

• Техническая реализация: конкретная схема трёхмерного переотображения (3D Remapping).
• Оценка производительности: как работает модель на практике?
• Развёртывание на реальном роботе: может ли модель работать на настоящем роботе? Если да, какова скорость инференса и работы?

3. Проект по манипуляциям роботом

• Управление всем телом (WBC): какие задачи реализованы в WBC?
• Определение систем координат: почему базе (Base) задаются опорные координаты?
• Сравнение подходов: пробовали ли чисто сквозной метод имитационного обучения (End-to-End Imitation Learning)?
• Сбор данных: как собирались экспертные траектории (Expert Trajectories)?
• Датчики: зачем установлен шестикомпонентный датчик силы и момента (F/T Sensor)?

4. Проект мобильного робота

• Вычислительная эффективность: как обеспечивается скорость глобального планирования пути на большой реальной сцене при очень маленьких ячейках сетки (Grid Cell)?
• Прогностическое управление (MPC):
  • какая математическая модель используется в MPC?
  • как построены ограничения по ускорению и по динамике?
  • как заданы горизонт предсказания (Prediction Horizon) и шаг (Step Size)?
• Качество отслеживания: учитывается ли при отслеживании траектории угол?
  • наблюдались ли колебания или отставание от заданных значений?
• Логика обхода препятствий: возможны ли случаи, когда робот «срезает дугу» или не может построить план в плотной толпе (динамическая среда)?

⭐️ Третье собеседование: с HR (около 15 минут)

• Процесс: объяснить, почему восстанавливалась связь с HC и процесс перезапускался.
• Личное: рассказать о собственном опыте и интересующих направлениях стажировки.
• Информация о выходе на работу: подтвердить дату начала и длительность стажировки.

Ответы

1. Как получен датасет симуляции?

• Основной принцип: получение датасета симуляции по сути означает построение марковского процесса принятия решений (MDP) в физическом движке (например, Gazebo или Isaac Sim). Запускаются экспертная стратегия (например, A* или DWA) или стратегия случайного исследования, и собираются полные данные о переходах состояний при взаимодействии агента со средой.

• Математический вывод: датасет $D$ для опыта обучения с подкреплением определяется так:

  $$D=\{(s_t,a_t,r_t,s_{t+1},d_t){\}}_{t=1}^N$$

  где $r_t=\mathcal{R}(s_t,a_t)$ — немедленная награда, а $d_t\in \{0,1\}$ — признак завершения эпизода.

• Практический псевдокод (Python — конвейер сбора данных в симуляции):

  class SimulationEnv:
      def collect_data(self, episodes, steps_per_episode):
          dataset = []
          for ep in range(episodes):
              state = self.reset() # Инициализация случайной сцены
              for step in range(steps_per_episode):
                  action = self.expert_policy(state) # Вызов A* или DWA для получения оптимального действия
                  next_state, reward, done, info = self.step(action) # Шаг физического движка
                  dataset.append((state, action, reward, next_state, done))
                  state = next_state
                  if done: break
          return dataset
  

2. Может ли возникнуть застревание в локальном оптимуме (например, в тупике)?

• Основной принцип: да, это возможно. В обучении с подкреплением, опирающемся на текущее наблюдение, или в методе искусственного потенциального поля (APF) внутри U-образных препятствий (тупиков) очень легко возникает особая точка, в которой силы притяжения и отталкивания полностью компенсируют друг друга, и робот замирает.

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

  $$F_{total}=-\nabla U_{att}(q)-\nabla U_{rep}(q)$$

  Когда $\nabla U_{att}(q)=-\nabla U_{rep}(q)$, то $F_{total}=0$, и система попадает в локальный минимум.

• Практический псевдокод (Python — внутреннее любопытство ICM как награда):

  def compute_intrinsic_reward(state, next_state, action, forward_model):
      # Чем больше ошибка MSE между предсказанным и реальным признаком, тем более незнакомой является среда, и тем выше награда за исследование
      pred_next_feat = forward_model(state, action)
      actual_next_feat = feature_extractor(next_state)
      intrinsic_reward = torch.mse_loss(pred_next_feat, actual_next_feat)
      return intrinsic_reward.item() * scale_factor
  

Решения проблемы локального оптимума (тупика)

A. Уровень классических алгоритмов: улучшение APF

Когда робот обнаруживает, что $F_{total}\approx 0$, но до цели ещё далеко, можно применить следующие математические приёмы:

1. Введение виртуальной целевой точки (Virtual Target Point):

   Вблизи выхода из тупика временно задаётся виртуальный источник притяжения, который меняет распределение потенциала и выводит робота из U-образной зоны.

2. Случайное возмущение / имитация отжига (Random Perturbation):

   На позицию робота кратковременно подаётся случайная сила $F_{noise}$, которая нарушает равновесие сил.

   $$F_{actual}=F_{att}+F_{rep}+\eta \cdot \text{rand}(-1,1)$$

   $\eta$: коэффициент возмущения.

B. Уровень обучения с подкреплением: борьба с «жадностью» и «близорукостью»

Помимо ICM (внутреннего любопытства), упомянутого в коде, есть и другие основные средства:

1. Внутренняя награда и исследование (Exploration Bonus):

   • ICM (Intrinsic Curiosity Module): как описано выше, ошибка предсказания используется как награда. Чем сильнее робот «не понимает» состояние внутри тупика, тем больше он стремится его исследовать, и тем выше шанс случайно выбраться.
   • RND (Random Network Distillation): предсказывается выход фиксированной случайной сети, и сложность предсказания используется для поощрения исследования.

2. Механизм памяти (Memory-based RL):

   Для обработки последовательных состояний используются LSTM или GRU.

   • Принцип: если робот может «запомнить», что он уже долго кружит в небольшой области, это отражается в представлении состояния, и политика учится выбирать неповторяющиеся действия.
   • Математическое выражение: состояние $s_t$ больше не является одним кадром облака точек, а скрытым состоянием $h_t=\text{LSTM}(s_t,h_{t-1})$.

C. Уровень системной инженерии: иерархическое планирование (промышленный стандарт)

Это самое распространённое решение в индустрии (например, в RoboticsX Tencent или в доставке Meituan): Global Planner + Local Planner.

• Основная логика:

  • Глобальное планирование (Global): A*, JPS или RRT* строят на глобальной карте последовательность опорных точек (Waypoints), которая теоретически полностью обходит U-образный тупик.
  • Локальное планирование (Local): RL или MPC отвечают только за отслеживание этих локальных точек.

• Математическая гарантия:

  Локальная целевая точка $q_{sub\_goal}$ постоянно обновляется, чтобы поле притяжения всегда было направлено к выходу из тупика, а не к цели за стеной.

🚀 Практический код: выход из локального оптимума со случайным возмущением

Если на собеседовании попросят на месте написать, как улучшить APF, можно показать этот код, в котором реализованы обнаружение состояния и возврат после возмущения:

import numpy as np

def compute_apf_action(robot_pos, goal_pos, obstacles):
    # 1. Вычисляем стандартное поле потенциалов
    f_att = k_att * (goal_pos - robot_pos)
    f_rep = np.zeros(2)
    for obs in obstacles:
        dist = np.linalg.norm(robot_pos - obs)
        if dist < d_min:
            f_rep += k_rep * (1/dist - 1/d_min) * (1/dist**2) * (robot_pos - obs) / dist
    
    f_total = f_att + f_rep
    
    # 2. Обнаружение локального минимума (Local Minima Detection)
    # Если суммарная сила очень мала, но цель ещё далеко, значит система достигла точки равновесия
    if np.linalg.norm(f_total) < 1e-3 and np.linalg.norm(goal_pos - robot_pos) > threshold:
        print("Detected Local Minima! Applying Perturbation...")
        # 3. Применяем касательное возмущение или случайную силу
        f_total = np.array([-f_att[1], f_att[0]]) + np.random.normal(0, 0.5, 2)
        
    return f_total

• threshold: допустимая погрешность до конечной точки.
• [-f_att[1], f_att[0]]: поворот силы притяжения на 90 градусов, чтобы попытаться «скользнуть» вдоль края препятствия.

Итог для ответа на собеседовании:

«Для решения проблемы локального оптимума я обычно использую архитектуру иерархического планирования. Глобальный планировщик (например, A*) исключает тупики на топологическом уровне, а затем для обработки препятствий в динамической среде применяется обучение с подкреплением на основе механизма любопытства (ICM) или локальный планировщик со случайным возмущением. Такая комбинация учитывает и глобальную корректность, и локальную устойчивость.»

3. Почему в Gazebo выбрана дифференциальная модель (Differential Model)?

• Основной принцип: дифференциальная модель — реальная физическая конструкция большинства внутренних мобильных шасси (AGV, роботы-пылесосы). Она умеет вращаться на месте при наличии неголономного ограничения (Non-holonomic constraint), и математически легко напрямую связать линейную и угловую скорость, которые выдаёт высокоуровневый планировщик, со скоростями двух моторов.

• Математический вывод: матрица обновления глобального пространства состояний (с учётом колёсной базы $L$ и радиуса колеса $R$):

  $$\left[\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right]=\left[\begin{array}{cc}\cos \theta & 0\\ \sin \theta & 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}\frac{R}{2}({\omega }_R+{\omega }_L)\\ \frac{R}{L}({\omega }_R-{\omega }_L)\end{array}\right]$$

  1. В чём принцип дифференциальной модели?

  Дифференциальный привод основан на независимом управлении. Он состоит из двух соосных ведущих колёс, и направление движения робота меняется за счёт разности скоростей (Differential) двух колёс.

  Физическая конструкция

  • Два колеса на одной оси: ведущие колёса установлены на одной линии.
  • Независимый привод: каждое колесо приводится в движение собственным мотором, и им можно задавать разные скорости ${\omega }_L$ и ${\omega }_R$.
  • Опорные колёса: обычно есть 1–2 шарообразных опорных колеса (Caster Wheel), которые обеспечивают равновесие и не влияют на кинематику.

  Основные формулы кинематики

  Пусть линейная скорость центра робота равна $v$, угловая скорость $\omega$, радиус колеса $R$, колёсная база $L$:

  $$v=\frac{v_R+v_L}{2}=\frac{R({\omega }_R+{\omega }_L)}{2}$$

  $$\omega =\frac{v_R-v_L}{L}=\frac{R({\omega }_R-{\omega }_L)}{L}$$

  ---

  2. Как реализуется поворот на месте?

  Математически поворот на месте (Rotation in place) означает: линейная скорость равна 0, а угловая — нет.

  Математический вывод

  Чтобы выполнить поворот на месте, согласно приведённым формулам полагаем $v=0$:

  $$v=\frac{v_R+v_L}{2}=0⟹v_R=-v_L$$

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

  Угловая скорость в этом случае

  $$\omega =\frac{v_R-(-v_R)}{L}=\frac{2v_R}{L}$$

  Регулируя $v_R$, можно точно задавать скорость поворота. Эта способность позволяет дифференциальному роботу в тупике развернуться на 180° на месте, а не выполнять несколько манёвров задним ходом, как автомобиль (Three-point turn).

  ---

  3. Глубже: неголономное ограничение (Non-holonomic Constraint)

  Это частый каверзный вопрос на собеседованиях.

  Почему это неголономное ограничение?

  Проще говоря, «неголономное ограничение» означает: нельзя мгновенно переместиться в любом направлении.

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

  $$\dot{x}\sin \theta -\dot{y}\cos \theta =0$$

  Смысл этой формулы: в любой момент времени поперечная (боковая) скорость робота равна 0. Он не может, как краб, сместиться боком.

  Где проявляется «гибкость» дифференциальной модели?

  Хотя у неё есть неголономное ограничение (нельзя двигаться боком), она умеет поворачивать с малым радиусом и даже с нулевым радиусом.

  • Обычный автомобиль (рулевая трапеция Аккермана): минимальный радиус поворота ограничен углом передних колёс, и на месте развернуться нельзя.

  • Дифференциальная модель: управляя $v_L$ и $v_R$, она может менять радиус поворота $r$ от $0$ (поворот на месте) до $\infty$ (движение по прямой).

    $$r=\frac{v}{\omega }=\frac{L}{2}\cdot \frac{v_R+v_L}{v_R-v_L}$$

  ---

  🚀 Глубокий вопрос интервьюера: если есть ограничение, почему модель хорошо работает в Gazebo?

  Можно ответить так:

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

  1. Простое разделение переменных: её якобиан (Jacobian) — аналитическое решение, и отображение от высокоуровневых $(v,\omega )$ к моторам нижнего уровня не требует сложных обратных тригонометрических функций.
  2. Устойчивость к препятствиям: возможность поворачивать на месте существенно упрощает логику «выхода из тупика» в локальном планировании, и в узких внутренних сценах Gazebo такой робот застревает реже, чем модель с четырьмя колёсами.
  3. Физическая устойчивость: по сравнению со сложной физической симуляцией рулевых тяг Аккермана дифференциальная модель требует смоделировать только два фрикционных диска, и устойчивость симуляции (сходимость решателя ODE) выше.»

4. Конкретная схема трёхмерного переотображения (3D Remapping).

• Основной принцип: по внутренним параметрам камеры 2D-пиксели RGB-D и значения глубины обратно проецируются в 3D-облако точек в системе координат камеры, затем с помощью внешних параметров от SLAM (дерево TF) переводятся в глобальную систему координат, и наконец методом взвешенного скользящего среднего объединяются в TSDF (усечённое знаковое расстояние) или воксельную карту.

• Математический вывод: вычисление точки $P_w$ в мировой системе координат:

  $$P_w=T_{wc}\cdot P_c=\left[\begin{array}{cc}{R}_{wc}& t_{wc}\\ 0^T& 1\end{array}\right]\left[\begin{array}{c}\frac{(u-c_x)Z}{f_x}\\ \frac{(v-c_y)Z}{f_y}\\ Z\\ 1\end{array}\right]$$

Часть 1. Подробный разбор параметров формулы

Формулу можно разбить на две части: обратная проекция (пиксель $\to$ камера) и преобразование координат (камера $\to$ мир).

1. Левая часть: целевые координаты

• $P_w$: 3D-точка в мировой системе координат (World Frame), обычно записывается как $[X_w,Y_w,Z_w,1{]}^T$. Единица в конце нужна, чтобы получить однородные координаты для умножения матриц.

2. Средняя часть: матрица внешних параметров (Extrinsic Matrix)

• $T_{wc}$: матрица преобразования из системы координат камеры в мировую систему координат (Transformation Matrix), относится к группе $SE(3)$.
• $R_{wc}$: матрица поворота $3\times 3$, описывающая ориентацию камеры относительно мира.
• $t_{wc}$: вектор переноса $3\times 1$, описывающий положение оптического центра камеры в мировой системе координат.
• $0^T$ и $1$: дополнение, превращающее матрицу в $4\times 4$.

3. Правая часть: 3D-точка в системе координат камеры (обратная проекция)

Вектор $[X_c,Y_c,Z_c,1{]}^T$ выводится из модели точечной камеры (Pinhole Model):

• $Z$: значение глубины пикселя (берётся напрямую из карты глубины RGB-D камеры).
• $u,v$: горизонтальная и вертикальная координаты пикселя на изображении (единица: пиксель).
• $c_x,c_y$: координаты главной точки (Principal Point), то есть точки пересечения оптической оси с плоскостью изображения, обычно близкой к центру кадра.
• $f_x,f_y$: фокусные расстояния (Focal Length), показывают, сколько пикселей занимает единица длины на плоскости изображения.
• $\frac{(u-c_x)Z}{f_x}$: координата $X$ в системе камеры, найденная по теореме о подобии треугольников.
• $\frac{(v-c_y)Z}{f_y}$: координата $Y$ в системе камеры, найденная по теореме о подобии треугольников.

Часть 2. Схема переотображения 3D и слияния TSDF

В реальных роботах (например, в проектах RoboticsX) камера движется, поэтому одна и та же точка объекта в каждом кадре попадает в разное место изображения. Поэтому для построения карты нужны следующие шаги:

1. Генерация облака точек: для каждого пикселя карты глубины с помощью внутренних параметров камеры вычисляется 3D-точка в системе камеры.
2. Выравнивание координат: с помощью текущей позы $T_{wc}$ от SLAM все точки переводятся в единую мировую систему координат.
3. Слияние в вокселях (TSDF):
   • Пространство делится на маленькие кубики (воксели).
   • Для каждого вокселя вычисляется расстояние до ближайшей поверхности объекта (SDF).
   • Взвешенное слияние: значение вокселя обновляется методом скользящего среднего, отфильтровывающим случайный шум датчика глубины.

Часть 3. Полная демонстрация реализации (Python + NumPy)

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

import numpy as np

class VoxelMap:
    def __init__(self, resolution=0.05):
        self.res = resolution
        # Для простоты разреженная воксельная сетка моделируется словарём: {(x,y,z): [tsdf_val, weight]}
        self.grid = {}

    def update_tsdf(self, world_points, max_dist=0.1):
        """
        Упрощённое слияние: точки записываются в воксели и обновляется вес
        """
        for pt in world_points:
            # Дискретизация пространства: находим индекс соответствующего вокселя
            idx = tuple((pt[:3] / self.res).astype(int))
            
            # Имитация взвешенного слияния (скользящее среднее)
            if idx not in self.grid:
                self.grid[idx] = [1.0, 1] # [начальное TSDF, начальный вес]
            else:
                curr_val, weight = self.grid[idx]
                # Логика слияния нового наблюдения (упрощённо)
                new_weight = weight + 1
                self.grid[idx] = [curr_val, new_weight]

def remapping_demo():
    # 1. Имитация внутренних параметров камеры (Intrinsic)
    fx, fy = 525.0, 525.0
    cx, cy = 319.5, 239.5
    
    # 2. Имитация одного кадра глубины (Depth Image) 640x480
    # Предполагаем, что глубина всех пикселей равна 2.0 м
    depth_img = np.ones((480, 640)) * 2.0
    
    # 3. Имитация позы камеры от SLAM (Extrinsic T_wc)
    # Предполагаем, что камера сдвинулась по оси X на 1 м, матрица поворота — единичная
    T_wc = np.eye(4)
    T_wc[0, 3] = 1.0 

    # --- Начало основных вычислений ---
    
    # Генерация сетки координат пикселей
    u, v = np.meshgrid(np.arange(640), np.arange(480))
    
    # Шаг 1: обратная проекция в систему координат камеры P_c
    z = depth_img
    x_c = (u - cx) * z / fx
    y_c = (v - cy) * z / fy
    
    # Построение однородных координат [N, 4]
    points_c = np.stack([x_c, y_c, z, np.ones_like(z)], axis=-1).reshape(-1, 4)
    
    # Шаг 2: преобразование в мировую систему координат P_w = T_wc * P_c
    # Матричная операция: (4,4) * (4, N) -> после транспонирования (N, 4)
    points_w = (T_wc @ points_c.T).T
    
    print(f"Успешно переотображено {len(points_w)} точек в мировую систему координат")
    print(f"Пример первых 5 точек:\n{points_w[:5, :3]}")

    # Шаг 3: слияние в карту
    my_map = VoxelMap()
    my_map.update_tsdf(points_w)
    print(f"Обновление карты завершено, число активных вокселей: {len(my_map.grid)}")

if __name__ == "__main__":
    remapping_demo()

Ключевые дополнения:

• Оптимизация производительности: в реальных роботах точек очень много, поэтому обработка обычно выполняется параллельно на GPU (CUDA) или с помощью готовых функций Open3D.

• Усечение TSDF: в реальном TSDF задаётся параметр trunc_margin (например, 10 см), и обновляются только воксели в этом диапазоне. Так экономится память и сохраняется точность поверхности.

• Практический псевдокод (C++ — взвешенное слияние TSDF в вокселях):

  Этот код делится на три математические стадии: вычисление SDF, усечение и нормировка (TSDF) и взвешенное слияние обновления.

  ---

  1. Вычисление знакового расстояния (SDF)

  Код: float sdf_measured = measured_depth - voxel->depth_from_camera;

  Математическая запись:

  $$sdf=d_{meas}-d_{vox}$$

  • $d_{meas}$ (measured_depth): реальное измеренное датчиком (например, RGB-D камерой) значение глубины для текущего пикселя.
  • $d_{vox}$ (depth_from_camera): евклидово расстояние от центра текущего обрабатываемого вокселя до оптического центра камеры или его проекция на оптическую ось.
  • Смысл:
    • Если $sdf>0$: воксель находится за поверхностью объекта (в затенённой области).
    • Если $sdf<0$: воксель находится перед поверхностью объекта (в свободном пространстве).
    • Если $sdf\approx 0$: воксель находится на поверхности объекта.

  ---

  2. Усечение и нормировка (TSDF)

  Код: float tsdf_new = max(-trunc_margin, min(trunc_margin, sdf_measured)) / trunc_margin;

  Математическая запись:

  $$tsd{f}_{new}=\frac{clamp(sdf,-\mu ,\mu )}{\mu }$$

  • $\mu$ (trunc_margin): расстояние усечения. Нас интересует только очень небольшая область у поверхности, данные далеко от неё (слишком большие положительные или отрицательные значения) не нужны для реконструкции.
  • Нормировка: деление на $\mu$ сжимает значения $tsd{f}_{new}$ в диапазон $[-1,1]$.
    • $1$: воксель явно находится перед поверхностью.
    • $-1$: воксель явно находится за поверхностью (обычно за этим пределом обновление прекращается, чтобы не проникать сквозь объект).

  ---

  3. Накопленное взвешенное слияние (Weighted Fusion)

  Это важнейшая часть кода — инкрементальное обновление, по сути это онлайн-взвешенное скользящее среднее.

  Код: voxel->tsdf = (voxel->weight * voxel->tsdf + weight_new * tsdf_new) / (voxel->weight + weight_new);

  Математическая запись:

  $$F_i(\mathbf{x})=\frac{W_{i-1}(\mathbf{x})F_{i-1}(\mathbf{x})+w_i(\mathbf{x})f_i(\mathbf{x})}{W_{i-1}(\mathbf{x})+w_i(\mathbf{x})}$$

  • $F_i(\mathbf{x})$ (voxel->tsdf): итоговое значение TSDF вокселя $\mathbf{x}$ после слияния $i$-го кадра.
  • $W_{i-1}(\mathbf{x})$ (voxel->weight): суммарный накопленный вес за первые $i-1$ кадров.
  • $f_i(\mathbf{x})$ (tsdf_new): новое значение TSDF, наблюдаемое в текущем $i$-м кадре.
  • $w_i(\mathbf{x})$ (weight_new): вес текущего наблюдения (обычно задаётся по достоверности или расстоянию до датчика; в простом варианте равен $1$).

  Затем обновляется вес:

  $$W_i(\mathbf{x})=W_{i-1}(\mathbf{x})+w_i(\mathbf{x})$$

  ---

  💡 Зачем это нужно?

  1. Снижение шума: в одном кадре карты глубины есть случайный шум. При усреднении по нескольким кадрам положительные и отрицательные ошибки взаимно компенсируются, и поверхность, соответствующая изоповерхности $tsdf=0$, становится гладкой.
  2. Инкрементальное вычисление: не нужно хранить все исходные облака точек, достаточно поддерживать сетку вокселей и обновлять её при каждом новом кадре, поэтому потребление памяти относительно постоянно.
  3. Извлечение поверхности: после слияния алгоритм Marching Cubes ищет точки с $tsdf=0$ и восстанавливает гладкую трёхмерную сетку (Mesh).

  void updateVoxel(Voxel* voxel, float measured_depth, float weight_new) {
      float sdf_measured = measured_depth - voxel->depth_from_camera;
      float tsdf_new = max(-trunc_margin, min(trunc_margin, sdf_measured)) / trunc_margin;
  
      // Скользящее взвешенное среднее: фильтрует шум датчика
      voxel->tsdf = (voxel->weight * voxel->tsdf + weight_new * tsdf_new) / (voxel->weight + weight_new);
      voxel->weight += weight_new;
  }
  

5. Каков реальный результат работы?

• Основной принцип: не полагаемся на субъективное описание, а используем стандартный количественный показатель области VLN — SPL. Он учитывает одновременно долю успешной навигации и оптимальность пути и строго наказывает стратегию, которая «добирается до цели, но бесконечно кружит на месте».

• Математический вывод:

  $$SPL=\frac{1}{N}\sum _{i=1}^N{S}_i\frac{l_i}{\max (p_i,l_i)}$$

  где $S_i\in \{0,1\}$ — признак успешности, $l_i$ — кратчайшее расстояние по эксперту, $p_i$ — фактически пройденный путь.

6. Может ли модель работать на реальном роботе? Если да, какова скорость инференса и работы?

• Основной принцип: полностью может. Стандартная схема развёртывания на реальном роботе такова: модель PyTorch экспортируется в ONNX, затем с помощью движка TensorRT выполняется инференс в INT8 или FP16 на периферийной вычислительной платформе (например, Jetson Orin), и задержка снижается с 30 мс примерно до 5 мс.

• Практический псевдокод (C++ — выделение памяти TensorRT и асинхронный инференс):

  void doInference(IExecutionContext& context, float* input, float* output, int batchSize) {
      void* buffers[2];
      cudaMalloc(&buffers[0], batchSize * INPUT_SIZE * sizeof(float)); // Видеопамять для входа
      cudaMalloc(&buffers[1], batchSize * OUTPUT_SIZE * sizeof(float)); // Видеопамять для выхода
  
      // Host To Device
      cudaMemcpy(buffers[0], input, batchSize * INPUT_SIZE * sizeof(float), cudaMemcpyHostToDevice);
  
      // Асинхронный инференс TensorRT
      context.enqueue(batchSize, buffers, stream, nullptr);
  
      // Device To Host
      cudaMemcpy(output, buffers[1], batchSize * OUTPUT_SIZE * sizeof(float), cudaMemcpyDeviceToHost);
  }
  

7. Какие задачи реализованы в WBC?

• Основной принцип: управление всем телом (WBC) разделяет задачи в многомерном пространстве с большим числом степеней свободы с помощью приоритетов и проекции на нулевое пространство (Null-space Projection). Обычно приоритеты такие: ограничения суставов > баланс центра масс (CoM) > отслеживание траектории концевого эффектора > оптимизация позы.

• Математический вывод: как на кинематическом, так и на динамическом уровне основа — оператор проекции $(I-J_1^{+}{J}_1)$. Например, при кинематическом управлении скоростью задача второго уровня ${\dot{x}}_2$ действует только в нулевом пространстве задачи первого уровня:

  $$\dot{q}=J_1^{+}{\dot{x}}_1+(I-J_1^{+}{J}_1)J_{2|1}^{+}({\dot{x}}_2-J_2{J}_1^{+}{\dot{x}}_1)$$

1. Что такое проекция на нулевое пространство (Null Space Projection)?

Интуитивное понимание:

Представьте, что в правой руке вы держите полную чашку кофе (задача 1: сохранять положение концевого эффектора). Поскольку у руки 7 степеней свободы (избыточность), вы можете, не двигая кистью, свободно покачивать локтем (это движение в нулевом пространстве задачи 1).

Математическое определение:

Для линейного уравнения $J\dot{q}=\dot{x}$ нулевое пространство — это множество всех скоростей суставов $\dot{q}$, удовлетворяющих $J\dot{q}=0$.

Матрица проекции на нулевое пространство $N=(I-J^{+}J)$ переводит любую команду движения суставов в это пространство, так что это движение совершенно не влияет на выполнение основной задачи.

2. Подробно об иерархии четырёх задач WBC

В WBC задачи упорядочены по приоритету, и задачи высокого уровня имеют «абсолютную власть» над задачами низкого уровня.

• Избегание предельных значений суставов (Joint Limit Avoidance):
  • Смысл: защита оборудования. У каждого мотора есть ограничение угла поворота.
  • Важность: наивысший приоритет. Если ради отслеживания траектории сустав перекрутится или ударится о предел, робот может быть физически поврежден, а система управления — мгновенно разойтись.
• Баланс центра масс (CoM Balance):
  • Смысл: удерживать центр масс робота внутри опорного многоугольника.
  • Важность: гарантирует, что робот «не упадёт». Для четвероногих или двуногих роботов, если центр масс теряет устойчивость, даже самый точный захват конечностью не спасёт робота от падения.
• Отслеживание траектории концевого эффектора (End-effector Tracking):
  • Смысл: выполнение реальной работы. Например, рука тянется к объекту, нога ставится в заданную координату.
  • Важность: это «бизнес-логика» робота. При условии безопасности и устойчивости он должен как можно точнее достичь цели.
• Оптимизация позы (Posture Optimization):
  • Смысл: если остаются свободные степени свободы, пусть робот «выглядит изящно» или «экономит энергию».
  • Важность: наименьший приоритет. Обычно используется, чтобы избежать сингулярных поз или чтобы суставы возвращались ближе к среднему положению, оставляя запас для последующих движений.

3. Глубокий разбор формулы

$$\dot{q}=J_1^{+}{\dot{x}}_1+(I-J_1^{+}{J}_1)J_{2|1}^{+}({\dot{x}}_2-J_2{J}_1^{+}{\dot{x}}_1)$$

Эта формула называется иерархической обратной кинематикой (Hierarchical Inverse Kinematics). Она описывает, как вычислить скорости суставов, которые учитывают два уровня приоритетов.

Таблица переменных:

Логика формулы по частям:

1. Первое слагаемое $J_1^{+}{\dot{x}}_1$:

   Полностью выполняем задачу 1. Это решение минимальной нормы для задачи 1.

2. Выражение в скобках $({\dot{x}}_2-J_2{J}_1^{+}{\dot{x}}_1)$:

   Это компенсирующий член. Выполнение задачи 1 уже дало какую-то часть эффекта задачи 2 (то есть $J_2{J}_1^{+}{\dot{x}}_1$), и задача 2 должна лишь добрать оставшуюся разницу.

3. Предшествующий оператор $(I-J_1^{+}{J}_1)$:

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

8. Почему базе (Base) задаются опорные координаты?

• Основной принцип: у мобильного манипулятора (Mobile Manipulator) очень большая избыточность (число степеней свободы системы $n>6$). Опорные координаты базы нужны, чтобы задать локальную систему отсчёта, отделить «куда поехало шасси» от «как двигается манипулятор», предотвратить расходимость решателя обратной кинематики и максимально расширить рабочее пространство манипулятора.

• Математический вывод: глобальная поза концевого эффектора составляется как произведение однородных матриц позы шасси и локальной относительной позы руки:

  $$T_{\text{end\_world}}=T_{\text{base\_world}}(x_b,y_b,{\theta }_b)\cdot T_{\text{end\_base}}(q_{arm})$$

  Фиксация или ограничение $T_{\text{base\_world}}$ понижает размерность сложной связанной задачи SE(3).

1. Почему однородные матрицы можно просто «перемножить»?

Секрет в том, что однородная матрица преобразования (Homogeneous Transformation Matrix) объединяет два разных математических действия — поворот и перенос — в одну операцию матричного умножения.

Цепная реакция преобразования координат

В мобильном манипуляторе, чтобы описать положение концевого эффектора в мировой системе координат, мы фактически совершаем «прыжок между системами координат»:

1. Первый шаг: положение концевого эффектора относительно основания робота (определяется углами суставов руки $q_{arm}$).
2. Второй шаг: положение основания робота относительно мировой системы координат (определяется позой шасси).

Математический вывод

Пусть точка на концевом эффекторе имеет координаты $P_{end}$ в системе концевого эффектора.

• Координаты этой точки в системе основания: $P_{base}=T_{\text{end\_base}}\cdot P_{end}$
• Координаты этой точки в мировой системе: $P_{world}=T_{\text{base\_world}}\cdot P_{base}$

Подставляем первое равенство во второе:

$$P_{world}=T_{\text{base\_world}}\cdot (T_{\text{end\_base}}\cdot P_{end})=(T_{\text{base\_world}}\cdot T_{\text{end\_base}})\cdot P_{end}$$

Видно, что произведение двух матриц преобразования как раз и есть матрица прямого перехода от концевого эффектора к миру:

$$T_{\text{end\_world}}=T_{\text{base\_world}}\cdot T_{\text{end\_base}}$$

Проще говоря: умножение однородных матриц по смыслу эквивалентно композиции преобразований (Composition of Transformations). Пока цепочка систем координат непрерывна, можно продолжать умножать.

2. Что такое SE(3) и что оно означает?

$SE(3)$ — не «задача», а математическое пространство: специальная евклидова группа (Special Euclidean Group).

Расшифровка

• S (Special): специальная. Означает, что определитель матрицы равен 1. Это гарантирует, что преобразование содержит только поворот и перенос, без «зеркального отражения» или масштабирования, объекты не растянутся, не сожмутся и не перевернутся.
• E (Euclidean): евклидова. Означает, что такое преобразование сохраняет евклидовы расстояния между точками пространства. Это так называемое «жёсткое преобразование» — как бы ни двигался робот, его рука не удлиняется.
• (3): трёхмерное пространство.

Математическая форма

Матрица $SE(3)$ обычно является матрицей $4\times 4$:

$$T=\left[\begin{array}{cc}R& t\\ 0& 1\end{array}\right]$$

• $R\in SO(3)$: матрица поворота $3\times 3$, имеет 3 степени свободы.
• $t\in {\mathbb{R}}^3$: вектор переноса $3\times 1$, имеет 3 степени свободы.
• Итого: пространство $SE(3)$ имеет 6 степеней свободы (3 поворота + 3 переноса).

3. Что такое связанная задача SE(3) и зачем её понижать в размерности?

Если соединить трёхстепенное шасси ($x,y,\theta$) и семистепенную руку, суммарно получается 10 степеней свободы. Но наша цель (поза концевого эффектора $T_{\text{end\_world}}$) в пространстве $SE(3)$ требует всего 6 степеней свободы.

Трудность связи (Coupling)

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

• Математическая трудность: при решении обратной кинематики (IK) уравнения недоопределены (неизвестных больше, чем уравнений). Небольшая вибрация шасси усиливается умножением и доходит до концевого эффектора, из-за чего вычисления становятся неустойчивыми, возникают колебания или решение не находится. Это и есть проблема связи SE(3).

Искусство понижения размерности

Фиксируя или ограничивая $T_{\text{base\_world}}$, мы искусственно уменьшаем число переменных:

1. Фиксированное основание: шасси стоит на месте. Тогда $T_{\text{base\_world}}$ становится постоянной матрицей.
2. Понижение размерности задачи: задача обратной кинематики переходит от «10 переменных на 6 целей» к «7 переменным (суставы руки) на 6 целей».

Зачем это делать?

• Вычислительная эффективность: аналитическое или численное решение IK для одной руки гораздо быстрее, чем решение для всего тела с учётом связи.
• Оптимизация манипулируемости: обычно сначала шасси перемещается в «удобное положение» (опорное значение $T_{\text{base\_world}}$), где рука далека от сингулярных поз и обладает наибольшей гибкостью.

💡 Итог

Умножение однородных матриц — это правило цепочки для преобразования координат. $SE(3)$ — математический язык, описывающий позу твёрдого тела робота. А «понижение размерности» нужно, чтобы в сложной избыточной системе найти схему управления, которая одновременно быстра и устойчива.

9. Пробовали ли вы чисто сквозной метод имитационного обучения (End-to-End Imitation Learning)?

• Основной принцип: пробовали поведенческое клонирование (BC). Но у чистого BC есть фатальная проблема — сдвиг ковариат (Covariate Shift): поскольку стратегии восстановления в состояниях вне распределения эксперта не обучались, стоит возникнуть небольшой ошибке, система каскадно разрушается во времени.

• Математический вывод: функция потерь BC обычно записывается как отрицательное логарифмическое правдоподобие или MSE:

  $${\mathcal{L}}_{BC}(\theta )={\mathbb{E}}_{(s,a^{\ast })\sim {\mathcal{D}}_{expert}}\left[||{\pi }_{\theta }(s)-a^{\ast }|{|}_2^2\right]$$

Часть 1. Что конкретно представляет собой сдвиг ковариат?

1. Основное определение

В машинном обучении сдвиг ковариат означает, что распределение входов $P(s)$ в обучающей и тестовой выборках различно, но условная вероятность $P(a|s)$ (то есть логика политики) остаётся той же.

• Во время обучения: агент учится на состояниях из распределения эксперта ${\rho }_{{\pi }^{\ast }}$. Эксперт движется очень уверенно, и его состояния — «учебниковые».
• Во время тестирования: из-за ограничений точности модели неизбежно возникают небольшие ошибки $ϵ$.

2. Накопление ошибок (Compounding Errors)

Это самое опасное. В динамической системе текущее действие $a_t$ влияет на следующее состояние $s_{t+1}$.

1. Робот допустил небольшую ошибку и отклонился от траектории эксперта.
2. Он попал в состояние $s^{\prime }$, которого эксперт никогда не посещал (область вне распределения эксперта).
3. Поскольку в обучающих данных нет данных о $s^{\prime }$, робот начинает наугад выбирать действия.
4. Наугад выбранные действия уводят его ещё дальше, в полностью неизвестную область, и в итоге система каскадно разрушается (Cascade Failure).

3. Математический вывод: квадратичный рост ошибки

Пусть максимальная ошибка одного шага равна $ϵ$.

• В задаче из $T$ временных шагов верхняя граница общей ошибки BC (Total Variation Distance) равна $O(ϵT^2)$.

• Напротив, если система работает внутри распределения, ошибка обычно растёт линейно: $O(ϵT)$.

  Этот квадратичный рост по $T$ — и есть то, как сдвиг ковариат разрушает стабильность системы во времени.

Часть 2. Как решать эту проблему?

В промышленности и академической среде сейчас есть четыре основных подхода к «заплатке» этой уязвимости:

1. Алгоритм DAgger (Dataset Aggregation, самый классический)

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

• Процесс:
  1. Запускаем текущую политику ${\pi }_{\theta }$ и собираем траектории.
  2. Передаём эти траектории эксперту, который отвечает на вопрос: «Если бы я был в этих (отклонившихся) позициях, что бы я сделал?»
  3. Добавляем этот «сборник ошибок» вместе с поправочными действиями эксперта в обучающий набор и переобучаем модель.
• Математическая суть: итерациями распределение обучающих данных ${\rho }_{data}$ постепенно приближается к распределению ${\rho }_{{\pi }_{\theta }}$, в котором агент действительно работает.

2. Инъекция шума и аугментация данных (например, DART)

Принцип: когда эксперт собирает данные, на исполнительные механизмы намеренно подаётся небольшой шум.

• Способ: во время работы эксперта к приводам добавляется случайный шум, вынуждающий эксперта выполнять исправляющие действия в «неидеальных» состояниях.
• Эффект: в обучающих данных появляется много примеров того, как восстанавливаться после отклонения.

3. Изменение цели предсказания (Action Chunking / Diffusion Policy)

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

• Логика: предсказывая фрагмент траектории (Action Chunk), можно использовать временную корреляцию между действиями, чтобы нивелировать случайные отклонения на отдельном шаге. Сейчас самый популярный метод — Diffusion Policy: он с помощью генеративной модели описывает сложное распределение действий и значительно повышает устойчивость к сдвигу.

4. Состязательное имитационное обучение (например, GAIL)

Принцип: не учиться напрямую действиям, а учиться «действовать как эксперт».

• Логика: вводится дискриминатор, который определяет, совершал ли эксперт данную пару состояние-действие $(s,a)$. Через обучение с подкреплением агент выравнивается с экспертом по всему распределению траекторий, а не только по покадровому соответствию.

Часть 3. Сводное сравнение

10. Как собирались экспертные траектории (Expert Trajectories)?

• Основной принцип: сбор разнородных данных из нескольких источников. Используются телеуправление (VR-контроллеры или SpaceMouse), кинестетическое обучение (перетаскивание с компенсацией гравитации) или традиционные планировщики на основе OMPL. Собранные данные должны содержать проприоцепцию, визуальный поток и управляющие команды, выровненные по высокочастотным временным меткам.

• Практический псевдокод (Python — построение стандартного датасета HDF5):

  import h5py
  import numpy as np
  
  def save_expert_trajectory(states_obs, actions, joint_positions, file_path="expert.h5"):
      with h5py.File(file_path, 'w') as f:
          f.create_dataset('obs_images', data=np.array(states_obs), compression="gzip")
          f.create_dataset('actions', data=np.array(actions))
          f.create_dataset('proprioception', data=np.array(joint_positions))
  

11. Зачем устанавливать шестикомпонентный датчик силы/момента (F/T Sensor)?

• Основной принцип: оценка силы контакта на конце только по току двигателей сильно зависит от трения в редукторе. F/T-датчик, установленный на фланце, даёт высокоточную обратную связь по внешней силе контакта, которая используется для импедансного или адмиттансного управления. Благодаря этому робот проявляет «податливость» в задачах с жёстким контактом, например при вставке в отверстие или протирке, и не врезается.

• Математический вывод: адмиттансное управление (Admittance Control) преобразует силу контакта в динамическую коррекцию опорной траектории:

  $$M_d(\ddot{x}-{\ddot{x}}_d)+B_d(\dot{x}-{\dot{x}}_d)+K_d(x-x_d)=F_{ext}$$

Эта формула описывает, как мы хотим, чтобы концевой эффектор робота вёл себя при внешнем воздействии — как физическая система с заданными массой, пружиной и демпфером.

Формула:

$$M_d(\ddot{x}-{\ddot{x}}_d)+B_d(\dot{x}-{\dot{x}}_d)+K_d(x-x_d)=F_{ext}$$

Расшифровка переменных:

• $x,\dot{x},\ddot{x}$: фактические позиция, скорость и ускорение концевого эффектора.
• $x_d,{\dot{x}}_d,{\ddot{x}}_d$: желаемая (номинальная) траектория, запланированная роботу.
• $e=x-x_d$: ошибка положения.
• $M_d,B_d,K_d$: заданные человеком виртуальные параметры (масса, демпфирование, жёсткость).
• $F_{ext}$: внешняя сила контакта, измеряемая шестикомпонентным датчиком силы.

Логика вывода:

1. Вход и выход: в адмиттансном управлении входом является сила ($F_{ext}$), выходом — поправка перемещения ($x$).
2. Физическое моделирование: когда датчик измеряет $F_{ext}$, алгоритм не управляет силой напрямую, а вычисляет: «Если бы мой концевой эффектор был телом массой $M_d$, соединённым с пружиной жёсткости $K_d$ и демпфером $B_d$, и на него действовала бы сила $F_{ext}$, каким должно быть перемещение $x$?»
3. Коррекция траектории: решая это дифференциальное уравнение второго порядка, получаем новую опорную позицию $x_{ref}$ и передаём её низкоуровневому контроллеру положения (например, PID).

3. Импедансное управление и адмиттансное управление: в чём принципиальная разница?

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

Импедансное управление (Impedance Control) и адмиттансное управление — «две стороны одной медали». В динамике роботов основная идея импедансного управления такова: моделировать концевой эффектор робота как управляемую систему «пружина — демпфер — масса».

В отличие от адмиттансного управления (сила → смещение), логика импедансного управления такова: отклонение положения → выходная сила/момент.

1. Целевое динамическое уравнение

Мы хотим, чтобы при внешнем возмущении концевой эффектор робота вёл себя в соответствии с характеристиками следующей линейной системы второго порядка:

$$M_d(\ddot{x}-{\ddot{x}}_d)+B_d(\dot{x}-{\dot{x}}_d)+K_d(x-x_d)=f_{ext}$$

Для упрощения вывода введём ошибку $e=x-x_d$ (разность фактического и желаемого положения); тогда целевое уравнение динамики принимает вид:

$$M_d\ddot{e}+B_d\dot{e}+K_{d}e=f_{ext}$$

Смысл переменных:

• $x,\dot{x},\ddot{x}$: фактическая поза, скорость и ускорение концевого эффектора в пространстве задачи (Task Space).
• $x_d,{\dot{x}}_d,{\ddot{x}}_d$: заданная желаемая траектория.
• $M_d,B_d,K_d$: заданные человеком желаемая инерция, желаемое демпфирование и желаемая жёсткость.
• $f_{ext}$: внешняя сила, действующая со стороны среды на концевой эффектор.

2. Вывод на основе линеаризации обратной связью

Для типичного манипулятора с $n$ степенями свободы уравнение динамики в пространстве суставов имеет вид:

$$M(q)\ddot{q}+C(q,\dot{q})\dot{q}+g(q)=\tau +J^T{f}_{ext}$$

где:

• $M(q)$: матрица инерции.
• $C(q,\dot{q})$: члены центробежных и кориолисовых сил.
• $g(q)$: член силы тяжести.
• $\tau$: моменты приводов суставов.
• $J$: якобиан.

Шаг 1: переход в пространство задачи

Используя связь якобиана $\dot{x}=J\dot{q}$, дифференцируем и получаем $\ddot{x}=J\ddot{q}+\dot{J}\dot{q}$, затем выражаем $\ddot{q}$:

$$\ddot{q}=J^{-1}(\ddot{x}-\dot{J}\dot{q})$$

Шаг 2: вычисление управляющего момента $\tau$

Чтобы робот вёл себя как целевая система, мы проектируем закон управления $\tau$ так, чтобы он компенсировал нелинейные члены динамики самого робота (гравитацию, кориолисовы силы и т. д.).

Пусть желаемое ускорение концевого эффектора равно ${\ddot{x}}_{ref}$. По принципу линеаризации обратной связью закон управления обычно записывается так:

$$\tau =M(q)J^{-1}({\ddot{x}}_{ref}-\dot{J}\dot{q})+C(q,\dot{q})\dot{q}+g(q)-J^T{f}_{ext}$$

Шаг 3: подстановка импедансных характеристик

Подставляя целевое уравнение $M_d\ddot{e}+B_d\dot{e}+K_{d}e=f_{ext}$, выражаем нужное опорное ускорение ${\ddot{x}}_{ref}$:

$$\ddot{x}={\ddot{x}}_d+M_d^{-1}(f_{ext}-B_d\dot{e}-K_{d}e)$$

Это выражение подаётся на вход контроллера. В итоге концевой эффектор робота проявит «жёсткость», заданную $K_d$, и «плавность», заданную $B_d$.

3. Физическая сущность импедансного управления

Импедансное управление по сути регулирует динамическую жёсткость робота.

• Член жёсткости $K_d(x-x_d)$: создаёт возвращающую силу, как у пружины. Чем больше $K_d$, тем «жёстче» робот и тем труднее сместить его с целевой позиции.
• Член демпфирования $B_d(\dot{x}-{\dot{x}}_d)$: рассеивает энергию и предотвращает колебания. Чем больше $B_d$, тем «вязче» движение, и тем стабильнее робот при касании объекта.
• Член инерции $M_d\ddot{e}$: определяет чувствительность робота к ускорению, то есть «насколько тяжёлым он ощущается».

💡 Подводные камни при внедрении в промышленности

1. Точность управления моментом: импедансное управление зависит от точного выходного момента ($\tau$). Если сила трения редуктора велика или контур тока отвечает медленно, эффект импедансного управления заметно ухудшается. Поэтому роботы вроде Franka Panda со встроенными датчиками момента в суставах заметно превосходят обычные промышленные манипуляторы в силовом управлении.
2. Граница устойчивости: если $K_d$ задана очень большой, а период управления слишком длинный (например, всего 100 Гц), из-за ошибки дискретизации система может потерять устойчивость и начать дрожать. В высокопроизводительных задачах робототехники обычно требуется частота управления 1000 Гц (1 мс) и выше.

12. Как обеспечивается скорость глобального планирования пути на большой реальной сцене при очень маленьких ячейках сетки (Grid Cell)?

• Основной принцип: на большой карте высокого разрешения пространство поиска традиционного A* взрывается. Промышленность ускоряет вычисления «тремя приёмами»: 1. Weighted A* с коэффициентом раздувания; 2. многоуровневая пирамида сеток; 3. поиск точек прыжка (JPS), отбрасывающий симметричные избыточные пути.

• Математический вывод: эвристическая функция стоимости Weighted A* (нарушает гарантию глобальной оптимальности, но взамен даёт скорость спуска, как у DFS):

  $$f(n)=g(n)+ϵ\cdot h(n)(ϵ>1)$$

Этот вопрос — очень классический вопрос собеседования по навигации и планированию пути роботов (Path Planning). Если вы занимаетесь воплощённым ИИ или управлением роботами, то независимо от того, управляете ли вы манипулятором, двигающимся в полости, полной препятствий (например, в хирургической полости), или четвероногим роботом, который ищет дорогу среди огромных завалов, эта задача — инженерная вершина, которую нужно преодолеть.

Сначала сделаем эту «катастрофическую сцену» конкретной:

Представьте, что робот должен найти путь в огромном складе размером 100 м × 100 м. Чтобы не задеть тонкие кабели на полу, вы задали точность сетки (Grid Cell) 1 см.

Это значит, что в карте 100 миллионов ячеек!

Если использовать традиционный A*, он будет, как круги на воде, очень мучительно расширяться от центра и пересчитывать десятки миллионов клеток. Когда он наконец найдёт идеальный путь, будет уже поздно (а память системы может просто переполниться).

Чтобы решить проблему «взрыва пространства поиска», промышленность выработала «три приёма». Разберём по очереди их физический смысл и остроумие:

Первый приём: Weighted A* с коэффициентом раздувания (превращаем «круги на воде» в «копьё»)

Здесь применяется формула, которую вы привели. Сначала посмотрим, как работает традиционный A*.

1. Рассудительный, но негибкий традиционный A*

Формула традиционного A*:

$$f(n)=g(n)+h(n)$$

• $g(n)$: уже понесённая реальная стоимость (сколько шагов реально пройдено от старта до текущей клетки $n$).
• $h(n)$: предполагаемая стоимость до цели (эвристика, например прямое расстояние от текущей клетки до конечной точки).
• Традиционный A* требует, чтобы эти два веса были 1:1. Из-за этого он крайне «справедлив»: стоит ему отклониться немного вперёд, как рост $g(n)$ вызывает сожаление, и он тут же разворачивается, чтобы проверить клетки рядом со стартом. В результате зона поиска на карте выглядит как расширяющаяся круглая волна, в которой проверяется огромное количество бессмысленного бокового пространства.

2. «Безрассудная» философия Weighted A*

Промышленность нашла грубое, но эффективное решение: умножить эвристику $h(n)$ на коэффициент раздувания $ϵ>1$ (например, $ϵ=5$):

$$f(n)=g(n)+ϵ\cdot h(n)(ϵ>1)$$

• Резкое изменение физического смысла: теперь веса предполагаемой стоимости до цели $h(n)$ в 5 раз больше!
• Почему он начинает спускаться в глубину, как DFS? Когда алгоритм стоит на перекрёстке, хотя шаг в сторону (рост $g$) в целом может казаться разумнее, благодаря $ϵ$ каждый шаг в направлении цели (уменьшение $h$) даёт пятикратный выигрыш в формуле!
• В результате алгоритм становится очень «жадным» и «опрометчивым». Он больше не пробует во все стороны, как волна, а действует как копьё (или, как DFS, идёт до конца одной дороги) и яростно пронзает пространство в направлении цели!

3. Цена и выгода (инженерный компромисс)

• Чем пожертвовали? Нарушено «гарантированное глобальное оптимальное решение (Admissibility)». Поскольку алгоритм слишком жадный, на U-образном препятствии он может с разбегу уйти в тупик, и в итоге найденный путь может быть на 5% длиннее абсолютно кратчайшего.
• Что получили? Скорость поиска выросла экспоненциально (возможно, в десятки и даже сотни раз). В реальных задачах воплощённого ИИ десять миллисекунд на поиск субоптимального пути, длиннее на 5%, гораздо лучше пяти секунд на поиск идеального пути. Это и есть мудрость промышленности.

Второй приём: многоуровневая пирамида сеток («сначала скоростная трасса, потом просёлочные дороги»)

Копьё быстро, но если оно бьёт по 100 миллионам клеток по 1 см, всё равно тяжело. Что делать? Подражать тому, как человек читает карту.

Если вы едете на машине из Пекина в Гуанчжоу, вы не станете сразу смотреть на карту с максимальной детализацией, чтобы выбрать каждую улицу.

1. Верхний уровень (низкое разрешение, например, клетки 1 м): сначала грубо делим карту на крупные клетки по 1 м. На этой крошечной карте размером в несколько десятков клеток выполняем один прогон A*, и он сразу очерчивает «макро-коридор» от старта до финиша (например: сначала скоростная трасса Цзинцзин-Гандун-Аомэнь, затем переход на магистраль Даган).
2. Нижний уровень (высокое разрешение, клетки 1 см): с учётом этого «макро-коридора» поиск высокого разрешения категорически не имеет права выходить за пределы коридора и искать в других ненужных областях! Ему разрешено тщательно искать путь только внутри этого узкого ограниченного региона, аккуратно обходя мелкие камни и кабели на полу.

Благодаря пирамидальной структуре пространство поиска сокращается от исходной площади склада 100 м × 100 м до тонкой трубы, и вычислительные затраты резко падают по экспоненте.

Третий приём: поиск точек прыжка JPS (телепортация, отбрасывающая симметричную избыточность)

(Jump Point Search, JPS.) Это оптимизация для сеточных карт — очень хитрая и при этом умная.

1. «Навязчивость» традиционного A* и симметричная избыточность

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

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

Жёсткий традиционный A* впадает в навязчивость: он вычисляет и сравнивает все эти тысячи одинаковых путей, сжигая огромный объём вычислительных ресурсов.

2. Правило «телепортации» JPS

JPS очень умён: он вводит правило, понятное человеческой интуиции: если впереди ровное поле, иди вперёд, не открывая глаз, пока не встретишь «точку, где надо принять решение»!

• Он пускает лучи по сетке: если впереди сплошное свободное пространство, он не добавляет пройденные клетки в список на проверку (Open List).
• Он продолжает «прыгать (Jump)», пока луч не упрётся в препятствие или не обнаружит «вынужденного соседа» (Forced Neighbor) — то есть место у края препятствия, где без остановки и поворота пришлось бы врезаться в стену или сделать большой крюк.
• Только тогда он записывает эту ключевую точку поворота как узел.

Итог: на открытых участках традиционный A* идёт пешком и вычисляет 1000 раз, а JPS как будто играет с порталом: он сразу «выпрыгивает» из старта к углу коридора, пропуская 999 промежуточных шагов, и вычислительная нагрузка на них равна нулю!

Итог по этой линии:

Перед катастрофическим сценарием из 100 миллионов мелких клеток промышленные роботы выживают так:

Многоуровневая пирамида сокращает 100 миллионов клеток до 10 тысяч; внутри этих 10 тысяч клеток JPS пропускает скучные вычисления на ровных участках; наконец, в действительно сложной местности, где требуется тщательный расчёт, Weighted A* с коэффициентом раздувания, как копьё, пробивает путь!

Эти «три приёма» — прекрасный пример того, как сочетать математический предел алгоритма и физические ограничения реального мира.

13. Какая математическая модель используется в MPC?

• Основной принцип: используется линейная нестационарная модель (LTV-MPC) на основе кинематической ошибки. В точках опорной траектории нелинейная система раскладывается в ряд Тейлора первого порядка (с якобианом) и дискретизируется, поэтому задача управления превращается в стандартную задачу квадратичного программирования (QP).

• Математический вывод: модель обновления пространства состояний:

  $$\Delta x_{k+1}=A_k\Delta x_k+B_k\Delta u_k$$

  где $A_k=\frac{\partial f}{\partial x}{|}_{x_r,u_r}$ и $B_k=\frac{\partial f}{\partial u}{|}_{x_r,u_r}$ — якобианы системы.

В воплощённом ИИ, когда вы отдали макрокоманду пути, то, как манипулятор или четвероногий робот каждую миллисекунду прикладывает усилие, как противостоит инерции и как точно останавливается в заданной точке, определяется MPC. Эта LTV-MPC (линейная нестационарная MPC) — ключевой инструмент, с помощью которого промышленность (например, Boston Dynamics или Tesla Optimus) балансирует между скоростью вычислений и точностью управления.

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

I. Зачем нужна такая сложная математика? (Интуиция опытного водителя)

Представьте, что вы ведёте машину:

• Обычное PID-управление: вы смотрите только на метр перед капотом. Если уходите влево, поворачиваете вправо, если вправо, то влево. Это «реактивное управление», которое заставляет машину вилять по дороге, и быстро ехать так не получится.

• MPC: вы смотрите на 50 метров вперёд. Думаете: «Через 3 секунды впереди поворот, значит, сейчас нужно заранее притормозить и подправить руль».

  Суть MPC: на каждый момент времени предсказывается состояние системы на определённый период вперёд (горизонт предсказания), и вычисляется, какой шаг действия нужно выполнить прямо сейчас.

II. Разбор основной формулы: $\Delta x_{k+1}=A_k\Delta x_k+B_k\Delta u_k$

Реальные роботы (например, хирургические манипуляторы) нелинейны (например, при повороте сустава связь угла и положения концевого эффектора содержит множество $\sin$ и $\cos$). Вычислять нелинейное управление напрямую слишком медленно: даже мощная видеокарта не успеет за управлением с частотой 1000 Гц.

Поэтому мы «ленимся» и используем линеаризацию, то есть то самое разложение в ряд Тейлора, о котором вы спрашивали:

1. Что такое $\Delta x$ и $\Delta u$? (Поиск разницы)

• $x_r,u_r$ (опорная точка): это «идеальный маршрут», который вам даёт алгоритм планирования (например, A*). Например: «сейчас ты должен быть в точке (1,1), со скоростью 0.5».
• $\Delta x_k$ (ошибка состояния): реальность сурова: разница между вашей фактической и идеальной позициями.
• $\Delta u_k$ (приращение управления): дополнительная сила или момент, которые вы прикладываете, чтобы исправить эту разницу.

2. Матрица $A_k$: «инерция» системы и её самопроизвольное развитие

$$A_k=\frac{\partial f}{\partial x}{|}_{x_r,u_r}$$

• Смысл: если я не трогаю рычаг управления ($u$ не меняется), а просто из-за текущей ошибки положения ($x$ отклонилась), во что превратится эта ошибка через секунду?
• Интуитивно: она описывает физические свойства робота. Например, манипулятор под действием тяжести опускается, или из-за инерции продолжает движение вперёд. $A_k$ показывает, как ошибка самопроизвольно распространяется во времени.

3. Матрица $B_k$: «чувствительность» действия

$$B_k=\frac{\partial f}{\partial u}{|}_{x_r,u_r}$$

• Смысл: если я увеличу ток двигателя на единицу ($u$ немного меняется), на сколько изменятся моё положение и скорость ($x$)?
• Интуитивно: это «чувствительность руля». В разных позах одинаковый момент даёт разный эффект. $B_k$ в реальном времени показывает: сейчас, если вы подтолкните робота, он сдвинется на сколько.

III. Разложение в ряд Тейлора первого порядка (якобиан): превращаем «кривую» в «прямую»

То, что вы назвали «разложением Якоби», — по сути, принцип «локальной аппроксимации кривой прямой».

• Физическая интуиция: закон движения робота — очень сложная кривая.
• Математическая операция: мы смотрим только на текущий шаг (первый порядок разложения). На очень коротком промежутке времени считаем, что этот маленький кусок кривой — прямая линия.
• Результат: эта операция превращает страшное нелинейное уравнение в простое матричное умножение. Матричные умножения в компьютере выполняются очень быстро, поэтому MPC может реагировать за миллисекунды.

IV. Итоговый вид: сведение к задаче квадратичного программирования (QP)

Конечная цель «большого босса» (целевая функция управления): одновременно обеспечить малую ошибку ($\Delta x\to 0$) и экономию усилий ($\Delta u$ не слишком большое, иначе моторы не выдержат).

Поэтому MPC сводит задачу к поиску оптимального $\Delta u$, при котором минимизируется следующая функция стоимости:

$$J=\sum ({\text{ошибка}}^2\times Q+{\text{сила управления}}^2\times R)$$

• Почему это называется «квадратичным программированием (QP)»? Потому что в формуле есть квадратичные члены (Quadratic).
• Почему это легко вычислить? Потому что задача QP в математике — выпуклая оптимизация, которая имеет единственное, определённое глобальное оптимальное решение, и в промышленности есть очень зрелые алгоритмы (например, OSQP и qpOASES), которые находят ответ за несколько сотен микросекунд.

Итог:

1. Опорная точка $(x_r,u_r)$: «идеальный сценарий», заданный целевым планированием навигации.
2. Матрицы якобиана $A_k,B_k$: «снимок» физических свойств робота (инерция и чувствительность) в данный момент.
3. Модель предсказания: используя матрицы $A$ и $B$, предсказываем, где окажется робот через 10 шагов, если сейчас приложить усилие определённым образом.
4. Квадратичное программирование: среди десятков возможных вариантов выбираем серию действий, которая требует наименьших усилий и наиболее близка к идеальному сценарию.
5. Скользящая оптимизация: выполняем только первый шаг из полученной серии, а в следующую миллисекунду заново снимаем данные, заново предсказываем и заново вычисляем (это и есть так называемое управление со скользящим горизонтом).

14. Как строятся ограничения по ускорению и по динамике?

• Основной принцип: в решателе MPC (например, OSQP) предел возможностей исполнительного механизма переводится в строгую матрицу линейных неравенственных ограничений. Ограничение по ускорению по сути ограничивает скорость изменения управления между двумя соседними временными шагами.

• Математический вывод:

  $$\Delta u_{min}\le \Delta u_k\le \Delta u_{max}$$

  Раскрывая в конкретные ограничения линейного и углового ускорения:

  $$a_{min}\le \frac{v_k-v_{k-1}}{\Delta t}\le a_{max},-{\alpha }_{max}\le \frac{{\omega }_k-{\omega }_{k-1}}{\Delta t}\le {\alpha }_{max}$$

15. Как задаются горизонт предсказания (Prediction Horizon) и шаг (Step Size)?

• Основной принцип: это компромисс между вычислительной мощностью и безопасностью управления. Шаг $\Delta t$ определяет частоту управления (обычно совпадает с шасси: 0.05–0.1 с), горизонт предсказания $N_p$ определяет дальность взгляда вперёд. Главное правило: общее время предсказания $T_p=N_p\times \Delta t$ должно быть больше времени, которое нужно роботу для торможения на максимальной скорости.

• Математический вывод: в функции стоимости первый член интегрирует ошибку состояния на $N_p$ шагах, второй — штрафует приращения управления на $N_c$ шагах:

  $$J=\sum _{i=1}^{N_p}\Vert {\mathbf{x}}_{k+i|k}-{\mathbf{x}}_{ref}{\Vert }_Q^2+\sum _{i=0}^{N_c-1}\Vert \Delta {\mathbf{u}}_{k+i|k}{\Vert }_R^2$$

Одна и та же математическая переменная играет совершенно разные роли в разных местах алгоритма.

$\Delta u$ По определению это одна и та же величина (приращение управления $\Delta u$), но для оптимизатора (solver) её смысл и способ обработки совершенно различны.

Представьте MPC как «гонку в кандалах»: $\Delta u$ здесь одновременно и «кандалы», и «штрафной балл».

1. В вопросе 14: $\Delta u$ — «жёсткое ограничение» (физический предел)

В контексте ограничения ускорения $\Delta u$ обозначает физический предел исполнительных механизмов (моторов, гидроцилиндров).

• Смысл: «Насколько максимум может увеличиться ток моего мотора за одну миллисекунду?» или «С какой максимальной скоростью вращается мой сервопривод?»

• Как строится: в решателях вроде OSQP она записывается как ограничение-неравенство (Constraints).

• Математическая интуиция:

  $$\Delta u_{min}\le u_k-u_{k-1}\le \Delta u_{max}$$

  Это как поставить робота на «ограничитель». Если вычисленное $\Delta u$ выходит за пределы, решатель принудительно возвращает его на границу.

• Физическое последствие: если не задать это ограничение, MPC может ради максимально быстрого достижения цели выдать «сверхмгновенную» команду; это приведёт к перегоранию моторов, поломке шестерён или к тому, что система потеряет управление, поскольку физически не может обеспечить такое ускорение.

2. В вопросе 15: $\Delta u$ — «штрафной балл» (стремление к плавности)

В функции стоимости $J$ $\Delta u$ нужно для того, чтобы движение робота было «изящным и плавным».

• Смысл: «Ты можешь нажимать на газ сколько угодно сильно, но за каждое резкое нажатие я снимаю баллы в твоём общем счёте».

• Как строится: это квадратичный член стоимости (Quadratic Cost), его вес задаётся матрицей $R$.

• Математическая интуиция:

  $$J=\cdots +\sum \Vert \Delta u_k{\Vert }_R^2$$

  • Если $R$ задать большим, робот станет очень «пассивным», и движения будут медленными и плавными.
  • Если $R$ задать маленьким, робот станет очень «грубым»: пока не достигнут физических пределов (ограничения из вопроса 14), он будет часто и сильно менять действия.

• Физическое последствие: без этого штрафного члена даже при наличии физических ограничений движения робота могут напоминать «припадок» — постоянные колебания у границы (так называемая высокочастотная вибрация). Это не только расходует энергию, но и ускоряет механический износ.

3. Сводное сравнение: два лица одного $\Delta u$

4. Дополнительные пояснения о горизонте предсказания ($N_p$) и шаге ($\Delta t$)

Горизонт $N_p$ и шаг $\Delta t$ из вопроса 15 определяют, насколько далеко вперёд видит этот «мозжечок».

• Шаг $\Delta t$ (частота дискретизации):

  Если $\Delta t$ слишком большой (низкая частота), робот реагирует медленно и легко врезается в стену; если $\Delta t$ слишком мал (высокая частота), вычислительная нагрузка взрывается. В воплощённом ИИ обычно его согласуют с частотой низкоуровневых приводов (например, 20–100 Гц).

• Горизонт предсказания $N_p$ (глубина взгляда вперёд):

  Поэтому и упоминается «время торможения на максимальной скорости».

  • Пример: если четвероногий робот мчится со скоростью 2 м/с, а полная остановка занимает 1.5 с, а у вас задано $N_p\times \Delta t$ всего 0.5 с, то робот подобен «близорукому» человеку, который видит лишь на полметра. Когда он увидит обрыв, физически уже невозможно будет остановиться на оставшемся расстоянии.

• Ключевой компромисс:

  Нужно найти баланс между «видеть далеко, но считать медленно» и «считать быстро, но смотреть близоруко». Сейчас в промышленности обычно делают так: управляющий горизонт $N_c$ короткий (например, 2–5 шагов), а горизонт предсказания $N_p$ длинный (например, 20–50 шагов). Так обеспечивается безопасность на дальней перспективе и уменьшается число переменных для оптимизации.

16. Учитывается ли при отслеживании траектории угол?

• Основной принцип: обязательно. Дифференциальная модель работает в пространстве SE(2). Если отслеживать только $(x,y)$, шасси будет боком соскальзывать. Ключевой момент: нельзя напрямую использовать абсолютную ошибку в мировой системе координат, её нужно повернуть в локальную систему координат корпуса робота.

• Математический вывод: матрица преобразования ошибки:

  $$e_k=\left[\begin{array}{c}{e}_x\\ e_y\\ e_{\theta }\end{array}\right]=\left[\begin{array}{ccc}\cos \theta & \sin \theta & 0\\ -\sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{ref}-x\\ y_{ref}-y\\ {\theta }_{ref}-\theta \end{array}\right]$$

Ошибка, которую вы видите на карте, — не та, с которой «руки-ноги» робота могут справиться напрямую. Представьте: вы (робот) с завязанными глазами, а я командую у вас за спиной. Я кричу: «Иди на восток 1 метр!» (ошибка в мировой системе координат). Но вы не знаете, где восток, вы знаете только «перёд, зад, лево, право» (локальная система координат корпуса). Поэтому мне нужно перевести «1 метр на восток» в ваш взгляд: «иди вперёд на 0.7 м и вправо на 0.7 м».

Эта матрица и есть «переводчик».

I. Подготовка: определяем две системы координат

Прежде чем выводить формулу, нарисуем в голове две схемы:

1. Мировая система координат $\{W\}$ (Global Frame): закреплена на земле, с осями $X_W,Y_W$. Опорная точка траектории $(x_{ref},y_{ref},{\theta }_{ref})$ и текущая позиция робота $(x,y,\theta )$ определены здесь.
2. Система координат робота $\{R\}$ (Local/Body Frame): привязана к центру робота. Ось $x_R$ всегда указывает строго вперёд, ось $y_R$ — влево.

Зачем нужно преобразование?

Потому что управляющими величинами дифференциального шасси являются линейная скорость $v$ (вдоль $x_R$) и угловая скорость $\omega$ (вращение вокруг центра). Если управлять по ошибке в мировой системе координат, робот будет вести себя как пьяный, который не различает левое и правое, и начнёт сильно скользить.

II. Шаг первый: вычисляем «ошибку с точки зрения мира»

Этот шаг очень прост — просто вычитаем координаты двух точек:

$$\Delta x_W=x_{ref}-x$$

$$\Delta y_W=y_{ref}-y$$

$$\Delta \theta ={\theta }_{ref}-\theta$$

Это показывает, насколько далеко опорная точка от вас, если смотреть на карту. Но направление здесь задано относительно оси $X$ карты, а не относительно передней части робота.

III. Шаг второй: двумерный поворот координат (основной вывод)

Нужно повернуть вектор смещения $\left[\begin{array}{c}\Delta x_W\\ \Delta y_W\end{array}\right]$ из мировой системы координат в ориентацию робота в данный момент.

1. Вспомним стандартную матрицу поворота:

   Если нужно перевести точку из системы робота $\{R\}$ в мировую систему $\{W\}$, матрица поворота $R(\theta )$ равна:

   $$R(\theta )=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]$$

   (Смысл этой матрицы: повернуть локальные оси координат на угол $\theta$ и отобразить их в глобальную систему.)

2. Обратная операция (из глобальной в локальную):

   Сейчас у нас глобальная ошибка, а нужна локальная. Поэтому нужна обратная матрица к $R(\theta )$. Для матрицы поворота обратная совпадает с транспонированной $R^T$:

   $$R(\theta {)}^{-1}=R(\theta {)}^T=\left[\begin{array}{cc}\cos \theta & \sin \theta \\ -\sin \theta & \cos \theta \end{array}\right]$$

3. Физический смысл построчно:

   • Первая строка $(\cos \theta ,\sin \theta )$: проецирует глобальную ошибку на направление прямо перед роботом (ось $x_R$).
   • Вторая строка $(-\sin \theta ,\cos \theta )$: проецирует глобальную ошибку на боковое направление (ось $y_R$).

IV. Шаг третий: сборка итоговой матрицы

Для ошибки угла $\Delta \theta$, поскольку мы рассматриваем двумерную плоскость (пространство SE(2)), изменение угла не зависит от переноса, и его проекция всегда взаимно однозначна (1 к 1).

Объединяем двумерную матрицу поворота 2×2 и единицу для угла — получается 3×3 матрица, которую вы видели:

$$\left[\begin{array}{c}{e}_x\\ e_y\\ e_{\theta }\end{array}\right]=\underset{\text{отображение поворота}}{\underbrace{\left[\begin{array}{ccc}\cos \theta & \sin \theta & 0\\ -\sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]}}\left[\begin{array}{c}{x}_{ref}-x\\ y_{ref}-y\\ {\theta }_{ref}-\theta \end{array}\right]$$

V. Почему нужно делать именно так? (Интуитивный итог)

• $e_x$ (продольная ошибка): показывает, насколько ещё нужно ехать вперёд (или назад). Её устраняют линейной скоростью $v$.
• $e_y$ (поперечная ошибка): показывает, насколько вы сместились влево или вправо от заданной траектории. Важный момент: у дифференциального робота нет боковых моторов, поэтому он не может идти боком, как краб.
• Как устранить $e_y$? Робот должен сначала повернуться на угол с помощью угловой скорости $\omega$, превратив поперечную ошибку $e_y$ в продольную $e_x$, и только потом ехать вперёд.

Если не использовать эту матрицу:

Решатель MPC или PID будет пытаться выдать «боковую силу», чтобы устранить $e_y$, но дифференциальное шасси физически не может её выполнить, и система начнёт сильно колебаться или вращаться на месте.

17. Наблюдались ли колебания или отставание от заданных значений?

• Основной принцип: наблюдались. Колебания обычно вызываются неудачным распределением весов MPC (слишком малое $R$ делает действия слишком агрессивными) или тем, что задержка связи не учтена в модели состояния. Отставание (Lag) возникает, когда кривизна опорной траектории превышает максимальное физическое ускорение шасси.

• Практический псевдокод (C++ — компенсация задержки системы через очередь):

  // Используя кинематическую модель, заранее интегрируем вперёд команды управления, отправленные, но ещё не выполненные, чтобы компенсировать задержку связи
  State predictDelayCompensatedState(State current_state, deque<ControlCommand>& past_cmds) {
      State virtual_state = current_state;
      for (const auto& cmd : past_cmds) {
          virtual_state.x += cmd.v * cos(virtual_state.theta) * dt;
          virtual_state.y += cmd.v * sin(virtual_state.theta) * dt;
          virtual_state.theta += cmd.omega * dt;
      }
      return virtual_state; // Передаём компенсированное состояние в MPC
  }
  

I. Разбор явления: что такое колебания и задержка?

1. Колебания (Oscillation) — «пьяный водитель»

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

   • Причина A (слишком малое $R$): помните функцию стоимости MPC? $R$ — штраф за резкость действий. Если $R$ слишком мало, главный «босс» (контроллер) считает, что «сила ничего не стоит», и выдаёт грубые команды. Робот резко поворачивает влево, понимает, что перестарался, и резко поворачивает вправо — и в итоге получается колебание.
   • Причина B (задержка системы): самая коварная. Датчик говорит: «Ты на 1 см левее». Но из-за задержки связи это данные 50 мс назад, то есть вы уже давно вернулись к центру. Вы повернули влево по старой информации, и теперь реально смещены вправо. Это как в игре с большой задержкой: вы нажали мышь, а персонаж сдвинулся через полсекунды, и в результате вы постоянно делаете ошибочные действия.

2. Отставание (Lag) — «толстяк, который не может бежать быстро»

   Опорная траектория задаёт 2 м/с, а робот может ехать только 1.5 м/с; или поворот слишком крутой, и робот не успевает повернуть.

   • Главная причина: физический предел. Алгоритм математически вычислил идеальную окружность, но моторы или сила трения не обеспечивают нужного центростремительного ускорения. В собеседованиях это часто называют «динамической невыполнимостью» (Dynamic Feasibility).

II. Основной алгоритм: компенсация системной задержки (часть кода)

C++-код на рисунке предлагает промышленное решение: предсказательную компенсацию задержки (Predictive Delay Compensation).

1. Физическая логика: раз есть задержка, нужно «предвидеть ваши предвидения»

Представьте, что вы целитесь в движущуюся цель. Нельзя целиться в её текущее положение, нужно целиться туда, где она окажется через секунду.

Этот код делает следующее: «Я знаю, что данные приходят с задержкой, поэтому по команд, которые я отправил, вычислю, где вы находитесь сейчас на самом деле, физически».

2. Построчный перевод кода:

State predictDelayCompensatedState(State current_state, deque<ControlCommand>& past_cmds) {
    State virtual_state = current_state; // 1. Берём «устаревшую» позицию с датчика
    for (const auto& cmd : past_cmds) {  // 2. Листаем «записную книжку»: какие команды были отправлены во время задержки
        // 3. Моделируем физику: накладываем эти команды на старое состояние
        virtual_state.x += cmd.v * cos(virtual_state.theta) * dt; 
        virtual_state.y += cmd.v * sin(virtual_state.theta) * dt;
        virtual_state.theta += cmd.omega * dt;
    }
    return virtual_state; // 4. Возвращаем «виртуальное, актуальное» состояние для подачи в MPC
}

• Изюминка: код в компьютере проводит «симулированное путешествие». Поскольку данные датчика устарели, мы используем кинематическую модель (эти несколько сложений) и заранее проходим отрезок времени, который прошёл с задержкой.
• Математическая основа: это простейшая счисление пути (Dead Reckoning). Зная линейную скорость $v$, угловую скорость $\omega$ и шаг времени $dt$, координаты постоянно суммируются.

18. Возможны ли случаи, когда в плотной толпе (динамическая среда) робот «срезает дугу» или не может построить план?

• Основной принцип: алгоритмы раздувания статических препятствий перестают работать при встрече с движущимися людьми. Поскольку люди движутся, свободное пространство (Free Space) может мгновенно сжаться почти до нуля, и алгоритм либо выдаёт исключение (робот «замирает», Freezing Robot), либо строит крайне кружной маршрут.

• Математический вывод: в функцию стоимости локальных планировщиков, таких как DWA, добавляется штрафной член, основанный на относительной скорости динамических препятствий, — время до столкновения (TTC):

  $$Cost(v,\omega )=\cdots +\delta \cdot \frac{1}{\text{TTC}(v,\omega )}$$

  где $\text{TTC}=\frac{||\Delta P||}{||\Delta V||\cos ({\theta }_{rel})}$. Так контроллер при прогнозе возможного столкновения в будущем заранее снижает скорость, вместо того чтобы слепо поворачивать руль и обходить по большой дуге.