Зачем всё это
Я сгенерировала показания идеального IMU — без шума и дрейфа — по траектории, заданной ключевыми позами и сферической интерполяцией, и проинтегрировала их обратно. Среднеквадратичная ошибка положения за 10 секунд получилась около 8 метров. Повышение частоты датчика с 50 до 1600 Гц её не уменьшило.
С другой моделью траектории та же проверка при 200 Гц даёт от 10 см до 10⁻¹¹ м — в зависимости от схемы интегрирования. Главное даже не сами числа, а то, что ошибка уменьшается с шагом так, как положено данной схеме. Эта статья о том, откуда такая разница.
Алгоритмы визуально-инерциальной одометрии (VIO) и SLAM — VINS-Mono [1], ORB-SLAM3 [2], OpenVINS [3] — оценивают движение камеры по изображениям и показаниям инерциального блока: акселерометра и гироскопа. Чтобы их сравнивать и отлаживать, нужны данные с точно известной истинной траекторией.
В реальных наборах вроде EuRoC [4] и TUM VI [5] эталон снят внешней системой: захвата движения или, в части серий EuRoC, лазерным трекером, который измеряет только положение. Эталон доступен только в зоне действия этой системы и сам содержит погрешность. К тому же условия записи зафиксированы: нельзя взять ту же запись и отдельно поменять шум датчика или расстояние между камерой и IMU. В синтетических данных траектория задаётся программно, и все параметры меняются независимо.
Синтетика полезна, только если показания датчиков согласованы с заданным движением, а это зависит от того, как именно задана траектория. Это часть моей дипломной работы в НИТУ «МИСИС»: там по той же траектории рендерятся кадры сцены, восстановленной гауссовым сплаттингом. Здесь только инерциальная часть.
Что на самом деле измеряет IMU
Показания акселерометра зависят от второй производной положения, а показания гироскопа — от первой производной ориентации. Из этого следуют все требования к модели движения.
Введу три системы координат: мир W (ось z вверх), IMU B и камера C. Матрица R_XY переводит координаты из системы Y в X, ускорение свободного падения g = (0, 0, −9,81) м/с². Стандартная модель измерений:
Здесь b — медленно уплывающие смещения нуля, η — белый шум. Для разговора о модели движения их можно считать нулевыми: все проблемы ниже возникают и без них.
Акселерометр измеряет не ускорение, а удельную силу: ускорение минус g, повёрнутое в систему датчика. Неподвижный датчик на столе показывает +9,81 м/с² вверх, а падающий — ноль.
Гироскоп измеряет угловую скорость ω. Она связана с производной матрицы поворота соотношением
где [ω]× — кососимметричная матрица векторного произведения. Поэтому, чтобы сгенерировать показания, траекторию нужно уметь дифференцировать: положение дважды, ориентацию хотя бы один раз. Производные при этом должны вести себя как у физического тела, без скачков, которых в движении не было.
И практическая деталь: производные лучше считать аналитически, а не конечными разностями. Иначе погрешность численного дифференцирования смешается с шумом, который мы сами добавляем, и разделить их уже не получится.
Просто соединить точки: что идёт не так
Самый очевидный способ задать движение — взять ключевые позы и соединить их: положение отрезками прямых, поворот сферической линейной интерполяцией кватернионов (SLERP [6]). На анимации это выглядит нормально, но датчики видят другую картину.
Внутри отрезка скорость постоянна и ускорение равно нулю. Акселерометр видит только гравитацию (и центростремительный член, если камера смещена относительно IMU).
В узле скорость меняется мгновенно. Ускорение в этот момент — дельта-функция, и дискретный датчик её не замечает.
Угловая скорость постоянна на отрезке и скачет в узлах — ступеньки на верхнем графике рис. 1.
Интегратор таких показаний поворотов траектории в узлах не видит и навсегда сохраняет начальную скорость. Моя тестовая траектория — восьмёрка 4 × 3 м с колебаниями по высоте, крену, тангажу и рысканию, 21 ключевая поза через 0,5 с, всего 10 с. На ней среднеквадратичная ошибка положения около 8 м при любой частоте IMU. Это не погрешность, которая уменьшается с шагом, а рассогласование показаний с движением.
Очевидная поправка — плавная интерполяция. Для положения берём кубический интерполяционный сплайн с заданными скоростями на концах, в scipy это CubicSpline(t, p, bc_type=((1, v0), (1, vK))). Для ориентации берём SQUAD [7] — сферический аналог кубической интерполяции из компьютерной анимации (понятный разбор есть в документации библиотеки splines [8]). Угловая скорость становится непрерывной, ступеньки исчезают.
Но угловое ускорение α = dω/dt у SQUAD в каждом узле скачет (нижний график, оранжевая пила). По оси x самый большой скачок — 0,11 рад/с² в узле t = 9,5 с, при том что само α здесь не превышает 0,19 рад/с². По модулю вектора скачки доходят до 0,12 рад/с².

Угловое ускорение не входит ни в одну из формул измерений выше. Оно начинает играть роль, когда камера и IMU стоят в разных местах.
Ловушка рычага
Камера и IMU практически всегда стоят в разных точках. Из-за этого угловое ускорение камеры попадает прямо в показания акселерометра.
Ключевые позы естественно задавать для камеры: именно она определяет, что будет в кадре. IMU жёстко закреплён рядом, в точке r в системе камеры, так что p_WB = p_WC + R_WC·r. Дважды продифференцируем и получаем ускорение IMU:
Это та же физика, что у точки на вращающемся диске: центростремительное ускорение ω×(ω×r) и касательное α×r. Угловое ускорение камеры входит в показания акселерометра через векторное произведение α×r, то есть пропорционально длине рычага. Поэтому скачки α у SQUAD превращаются в ступеньки акселерометра.
В моём примере IMU смещён относительно камеры на 8,5 см и повёрнут на 10°. Значения я выбрала сама, ориентируясь на размеры небольшого дрона. На настоящем устройстве смещение берётся из калибровки.

По оси z самая большая ступенька — 6 мм/с² в узле t = 0,5 с, по модулю вектора — до 6,5 мм/с². Сам вклад рычага доходит до 11 мм/с². Это меньше шума одного отсчёта MEMS-акселерометра уровня датчика из EuRoC: при 200 Гц там десятки мм/с². В зашумлённых данных такие ступеньки глазом не увидеть. Но это систематический артефакт: он растёт с длиной рычага, резкостью манёвров и редкостью ключевых поз. И, как будет видно дальше, он ломает проверку согласованности.
Условие гладкости
Утверждение простое, по сути это формализация, но при смещённой камере его легко упустить. Пусть скорость камеры непрерывна, а положение и ориентация дважды непрерывно дифференцируемы на каждом куске некоторого разбиения отрезка времени. Тогда:
Идеальные (без шума) показания IMU непрерывны при всех r тогда и только тогда, когда и положение, и ориентация камеры принадлежат классу C².
Достаточность видна из формулы выше. Необходимость показывается в три шага:
Гироскоп непрерывен, значит ω непрерывна.
При r = 0 акселерометр непрерывен, значит непрерывно ускорение камеры.
Тогда при любом r непрерывно α×r, из чего подстановкой r = e₁, e₂, e₃ получаем непрерывность α.
Непрерывность скорости в условии существенна. У отрезков со SLERP скорость в узлах скачет, ускорение содержит δ-функцию, и показания акселерометра вообще не определены как функции. Эта модель под утверждение не подпадает.
Слова «при всех r» тоже важны. При одном фиксированном r не обязательны ни C² ориентации, ни даже C² положения камеры: скачок ускорения камеры может компенсироваться скачком α×r. Точное условие для фиксированного r — гладкость траектории самого IMU: положение класса C², ориентация класса C¹.
Именно так устроен симулятор OpenVINS [3]: траектория задана кубическим B-сплайном на SE(3) для системы IMU, и в документации прямо сказано, что кубический сплайн выбран ради C² для инерциальных измерений (см. Simulator.cpp, описание класса BsplineSE3 и Python-порт navlie.bspline[9]). Проблема рычага там не возникает.
Почему бы не сделать так же? Мне нужны кадры, отрендеренные гауссовым сплаттингом, а рендер — самая дорогая часть. Если сплайн задан для камеры, однажды отрендеренные кадры можно использовать с разными калибровками камера–IMU: меняются только показания IMU. Если сплайн задан для IMU, при смене калибровки меняется траектория камеры, и рендерить приходится заново. Плата за такой выбор — угловое ускорение в формуле рычага и требование C² к ориентации.
Решение: B-сплайны в R³ и на SO(3)
Нужна модель, в которой положение и ориентация принадлежат C², а все производные считаются по формулам. Оба требования выполняет кубический B-сплайн. Идея не новая. Показания инерциальных датчиков по непрерывной модели движения генерирует, например, IMUSim [18]. B-сплайнами задаёт траекторию пакет калибровки Kalibr [10], а симулятор OpenVINS [3] выбирает кубический сплайн именно ради C². Кумулятивную форму на группе поворотов, о которой ниже, в визуально-инерциальную оценку принесли Lovegrove, Patron-Perez и Sibley [11, 12]. Новое здесь только одно: при смещённой камере C² нужен и ориентации, и это видно в цифрах.
Положение
На каждом отрезке вводим локальный параметр u от 0 до 1. Положение зависит от четырёх контрольных точек:
Скорость и ускорение считаются по той же формуле: вместо столбца (1, u, u², u³) берём (0, 1, 2u, 3u²)/Δt и (0, 0, 2, 6u)/Δt².
Контрольные точки подбираются так, чтобы сплайн прошёл через ключевые положения и имел заданные скорости на концах:
Условия на скорости связывают точки через одну. Из них выражаем крайние точки c₀ и c_{K+2} и подставляем в первое и последнее уравнения. После этого система относительно c₁, …, c_{K+1} становится трёхдиагональной со строгим диагональным преобладанием. Решение единственное и находится прогонкой.
Полученная кривая совпадает с CubicSpline из scipy с теми же краевыми условиями, bc_type=((1, v0), (1, vK)): расхождение 4·10⁻¹⁶ м, машинный ноль. С краевым условием по умолчанию (not-a-knot) кривая будет другой, так что условия на концах нужно задать явно. Получается, что с положением и раньше всё было хорошо. Вся разница между «сплайн + SQUAD» и B-сплайнами — в ориентации.
Ориентация
С поворотами так прямо не получится: взвешенная сумма матриц поворота поворотом не будет. Помогает кумулятивная форма [13]. Сумму точек переписываем через разности соседних точек. Для вращений разность заменяем логарифмом относительного поворота, сложение — умножением:
Здесь Q — контрольные вращения, Exp и Log — переход между вектором поворота и матрицей (формула Родрига). Кумулятивные базисные функции — обычные многочлены:
Производные удобнее всего считать рекуррентно, по одному множителю за шаг. Эти формулы выведены в [14]:
Начальные значения — нулевые, после третьего шага получаем ω и α камеры. На один отсчёт уходит O(1) операций, численное дифференцирование не нужно.
Почему в узлах нет скачков? На стыке отрезков i и i+1 меняется набор контрольных вращений, но значения базиса на концах отрезка равны (1, 5/6, 1/6) при u = 1 и (5/6, 1/6, 0) при u = 0. В конце отрезка i первый множитель даёт Qᵢ·Exp(d₁) = Qᵢ₊₁, а оставшиеся совпадают с началом отрезка i+1. То же верно для первой и второй производных. Численно я сравнила формулу отрезка i при u = 1 с формулой отрезка i+1 при u = 0 во всех узлах. У B-сплайна ω и α совпали точно, а у SQUAD α расходится до 0,12 рад/с².
Осталось выбрать контрольные вращения. Я беру ключевые повороты как есть, а два крайних получаю продолжением первого и последнего поворота:
У такого выбора есть цена: кривая идёт рядом с ключевыми поворотами, но точно через них не проходит. На моей траектории отклонение не превышает 0,36°. Для генерации данных это не мешает: эталоном служит сам сплайн, а не ключевые позы. Если точное прохождение всё же нужно, учтите: неизвестных вращений K+3, а условий K+1, задача недоопределена, и чистый метод Гаусса–Ньютона тут не сработает: матрица JᵀJ вырождена. Подойдут метод Левенберга–Марквардта, два дополнительных условия на ω на концах или готовая интерполирующая C²-кривая из [15].
Код
Ядро генератора занимает полсотни строк на numpy и scipy. Ниже версия для одного момента времени, без векторизации: так проще читать. С полной реализацией она совпадает до 3·10⁻¹⁶.
import numpy as np from scipy.spatial.transform import Rotation as Rot G_W = np.array([0.0, 0.0, -9.81]) def Exp(phi): return Rot.from_rotvec(phi).as_matrix() def Log(R): return Rot.from_matrix(R).as_rotvec() def cum_basis(u): """Кумулятивный базис и его производные по u.""" B = np.array([(5 + 3*u - 3*u**2 + u**3) / 6, (1 + 3*u + 3*u**2 - 2*u**3) / 6, u**3 / 6]) dB = np.array([(3 - 6*u + 3*u**2) / 6, (3 + 6*u - 6*u**2) / 6, u**2 / 2]) ddB = np.array([u - 1, 1 - 2*u, u]) return B, dB, ddB def rotation_spline(Q, t, t0, dt): """R, omega, alpha кумулятивного B-сплайна на SO(3) в момент t.""" i = min(int((t - t0) // dt), len(Q) - 4) u = (t - t0) / dt - i B, dB, ddB = cum_basis(u) dB, ddB = dB / dt, ddB / dt**2 R, om, al = Q[i].copy(), np.zeros(3), np.zeros(3) for j in range(3): d = Log(Q[i + j].T @ Q[i + j + 1]) A = Exp(B[j] * d) w = dB[j] * d om_rot = A.T @ om al = A.T @ al + np.cross(om_rot, w) + ddB[j] * d om = om_rot + w R = R @ A return R, om, al def ideal_imu(R_wc, acc_wc, om_c, al_c, R_bc, r): """Идеальные показания IMU по движению камеры. r -- положение IMU в системе камеры.""" lever = np.cross(al_c, r) + np.cross(om_c, np.cross(om_c, r)) acc_wb = acc_wc + R_wc @ lever R_wb = R_wc @ R_bc.T gyro = R_bc @ om_c accel = R_wb.T @ (acc_wb - G_W) return gyro, accel
Шум добавляется поверх идеальных показаний по стандартной схеме дискретизации [10, 16]. Белый шум имеет дисперсию σ²/h, смещение на каждом шаге получает приращение σ_b·√h·ξ, где ξ — стандартная нормальная величина. Здесь σ — «noise density» и «random walk» из паспорта датчика или из калибровочного файла датасета. Полный код с положением, подбором контрольных точек, тремя моделями и проверкой из следующего раздела лежит в репозитории: github.com/1Bermud/spline-imu-sim.
Одна ловушка, в которую я попала: scipy хранит кватернионы как (x, y, z, w), а EuRoC — как (w, x, y, z). Если забыть переставить компоненты, программа не падает, а просто выдаёт странные повороты. В свежих версиях scipy у Rotation.from_quat и as_quat есть параметр scalar_first=True (в 1.18 проверила), и переставлять руками не нужно.
Как проверить, что генератор не врёт
Лучший тест, который я нашла, — проинтегрировать идеальные показания обратно и сравнить с исходной траекторией. Смотреть надо не на саму ошибку, а на то, как она убывает с ростом частоты. Если показания согласованы с движением, ошибка должна убывать как h^q, где h — шаг, а q — порядок схемы.
Я брала две схемы. Первая — простая схема первого порядка, как в прединтегрировании IMU [16]. Вторая — классический Рунге–Кутта 4-го порядка (РК4). Для него нужны измерения в середине шага. Генератор умеет считать их в любой момент, но фактически это IMU двойной частоты. Для проверки модели это нормально, для имитации реального датчика — нет. Ориентацию РК4 интегрирует как обычную матричную систему Ṙ = R[ω]×, а после каждого шага матрица проецируется обратно на SO(3) через SVD. Такая проекция порядок не портит, что видно по результату для гладкого случая.
Начальное состояние берётся из траектории точно, шума нет, смещения нулевые, частота IMU — от 50 до 1600 Гц. Я проверяла два варианта сетки: моменты измерений попадают точно в узлы сплайна или сетка сдвинута на 0,37h. Сдвиг важно задавать долей шага, а не в секундах. Тогда узел при любом h лежит в одной и той же точке шага. Иначе он каждый раз попадает в другое место, и наблюдаемый порядок скачет от частоты к частоте. Сдвинутая сетка ближе к жизни: у настоящего датчика отсчёты с узлами траектории никак не связаны.

СКО ошибки положения при 200 Гц, типичной частоте IMU в датасетах. В скобках наблюдаемый порядок на интервале 50–200 Гц:
Модель |
Схема 1-го порядка, м |
РК4, сетка сдвинута, м |
РК4, сетка в узлах, м |
|---|---|---|---|
А: отрезки + SLERP |
8,09 (0) |
8,09 (0) |
8,10 (0) |
Б: кубический сплайн + SQUAD |
9,7·10⁻² (1) |
3,9·10⁻⁵ (1) |
3,2·10⁻⁵ (1)* |
В: B-сплайны |
9,7·10⁻² (1) |
7,6·10⁻⁷ (2) |
1,0·10⁻¹¹ (4) |
*Зависит от того, какое значение генератор отдаёт ровно в узле, см. ниже.
Схема первого порядка не отличает хорошую модель от плохой. У Б и В ошибка одинаковая, 9,7 см: её целиком даёт сама схема. При такой проверке проблема SQUAD останется незамеченной.
На сдвинутой сетке модель Б даёт первый порядок. При 200 Гц её ошибка в 50 раз больше, чем у В (3,9·10⁻⁵ против 7,6·10⁻⁷ м), и разрыв растёт с частотой. Причина — ступеньки акселерометра из прошлого раздела: шаг, внутри которого стоит узел, даёт ошибку порядка h. На сетке в узлах картина зависит от реализации. Если генератор отдаёт в узле одно значение (как настоящий датчик), последняя стадия РК4 часто получает значение с другой стороны ступеньки, и порядок снова первый. Если же в конце шага брать предел слева, модель Б даёт 9,4·10⁻¹² м и порядок 3,8. Я проверила и так. Такое занижение ошибки — свойство реализации, а не модели, и на реальных данных оно недоступно.
У модели В порядок зависит от сетки. Если шаги интегрирования не пересекают узлов, порядок полный, 4,0. Если пересекают, он падает до 2,0: у кубического сплайна рывок (третья производная) в узлах разрывен. На практике 0,76 мкм за 10 с — ничто по сравнению с любым реальным шумом.
Порядок сходимости как мера гладкости
Результаты укладываются в простое правило. Пусть у правой части уравнений движения в узлах разрывна m-я производная (m = 0 — скачок самой функции, m = 1 — излом). Тогда шаг, пересекающий узел, даёт локальную ошибку порядка h^(m+1). Узлов конечное число, поэтому РК4 даёт порядок min(4, m + 1). Проверка на сдвинутой сетке:
Что интегрируется |
Что разрывно в узлах |
m |
Ожидается |
Получено |
|---|---|---|---|---|
Положение, модель Б |
ускорение IMU (скачок) |
0 |
1 |
1,02 |
Положение, модель В |
производная ускорения |
1 |
2 |
2,01 |
Ориентация, модель Б |
α, то есть производная ω |
1 |
2 |
2,00 |
Ориентация, модель В |
производная α |
2 |
3 |
3,00 |
Получается, что наблюдаемый порядок сходимости показывает, насколько гладка модель движения. У модели А он равен нулю: показания с движением не согласованы вообще.
Важная оговорка: эксперимент проведён без шума и на одной траектории. Как всё это выглядит на фоне реального шума и дрейфа и как сказывается на настоящем VIO-алгоритме — следующий шаг работы.
Итоги и ограничения
Если вы генерируете синтетический IMU по траектории камеры, задавайте и положение, и ориентацию дважды непрерывно дифференцируемыми кривыми и проверяйте генератор схемой высокого порядка.
Отрезки с SLERP не подходят совсем: акселерометр не видит поворотов траектории, и ошибка не уменьшается с ростом частоты.
Кубический сплайн с SQUAD выглядит гладким, но угловое ускорение в узлах скачет. Через рычаг между камерой и IMU эти скачки попадают в акселерометр и опускают порядок сходимости до первого.
Из трёх моделей только B-сплайн на SO(3) даёт непрерывные показания при любом расположении камеры. Все его производные считаются по формулам.
Проверка обратным интегрированием схемой первого порядка разницы между моделями не видит. РК4 её показывает, а наблюдаемый порядок говорит, какая производная траектории разрывна.
Ограничение модели: она чисто кинематическая, положение и ориентация задаются независимо. У квадрокоптера они связаны: тяга направлена вдоль оси корпуса, и наклон задаётся ускорением. Поэтому угловая скорость зависит от третьей производной положения (рывка), а угловое ускорение — от четвёртой (снэпа). Для физически достижимого полёта положение должно быть гладким хотя бы до C⁴, и здесь стоит смотреть в сторону траекторий с минимальным снэпом [17]. Для теста VIO кинематической модели обычно хватает.
Дальше в дипломе по этой же траектории будут рендериться кадры сцены, восстановленной гауссовым сплаттингом. Получится полный визуально-инерциальный набор с известным эталоном, на котором можно гонять VIO и SLAM. Об этом — в следующий раз.
Литература
Qin T., Li P., Shen S. VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator // IEEE Transactions on Robotics. 2018. Vol. 34, No. 4. P. 1004–1020.
Campos C., Elvira R., Gómez Rodríguez J.J., Montiel J.M.M., Tardós J.D. ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial, and Multimap SLAM // IEEE Transactions on Robotics. 2021. Vol. 37, No. 6. P. 1874–1890.
Geneva P., Eckenhoff K., Lee W., Yang Y., Huang G. OpenVINS: A Research Platform for Visual-Inertial Estimation // ICRA 2020. P. 4666–4672. Код симулятора: Simulator.cpp.
Burri M. et al. The EuRoC Micro Aerial Vehicle Datasets // The International Journal of Robotics Research. 2016. Vol. 35, No. 10. P. 1157–1163.
Schubert D., Goll T., Demmel N., Usenko V., Stückler J., Cremers D. The TUM VI Benchmark for Evaluating Visual-Inertial Odometry // IROS 2018. P. 1680–1687.
Shoemake K. Animating Rotation with Quaternion Curves // ACM SIGGRAPH Computer Graphics. 1985. Vol. 19, No. 3. P. 245–254.
Shoemake K. Quaternion Calculus and Fast Animation // SIGGRAPH 1987 Course Notes No. 10 «Computer Animation: 3D Motion Specification and Control». P. 101–121.
Документация библиотеки splines: Spherical Quadrangle Interpolation (Squad).
Python-порт сплайна из OpenVINS: navlie.bspline.
Furgale P., Rehder J., Siegwart R. Unified Temporal and Spatial Calibration for Multi-Sensor Systems // IROS 2013. P. 1280–1286.
Lovegrove S., Patron-Perez A., Sibley G. Spline Fusion: A Continuous-Time Representation for Visual-Inertial Fusion with Application to Rolling Shutter Cameras // BMVC 2013.
Patron-Perez A., Lovegrove S., Sibley G. A Spline-Based Trajectory Representation for Sensor Fusion and Rolling Shutter Cameras // International Journal of Computer Vision. 2015. Vol. 113, No. 3. P. 208–219.
Kim M.-J., Kim M.-S., Shin S.Y. A General Construction Scheme for Unit Quaternion Curves with Simple High Order Derivatives // SIGGRAPH 1995. P. 369–376.
Sommer C., Usenko V., Schubert D., Demmel N., Cremers D. Efficient Derivative Computation for Cumulative B-Splines on Lie Groups // CVPR 2020. P. 11145–11153.
Kim M.-J., Kim M.-S., Shin S.Y. A C²-Continuous B-spline Quaternion Curve Interpolating a Given Sequence of Solid Orientations // Computer Animation ’95. P. 72–81.
Forster C., Carlone L., Dellaert F., Scaramuzza D. On-Manifold Preintegration for Real-Time Visual-Inertial Odometry // IEEE Transactions on Robotics. 2017. Vol. 33, No. 1. P. 1–21.
Mellinger D., Kumar V. Minimum Snap Trajectory Generation and Control for Quadrotors // ICRA 2011.
Young A.D., Ling M.J., Arvind D.K. IMUSim: A Simulation Environment for Inertial Sensing Algorithm Design and Evaluation // IPSN 2011. P. 199–210.
Sencis
Del