Дополнение 01. Калибровка «рука — глаз» (Hand-Eye Calibration)

1.1 Определение калибровки «рука — глаз»

Калибровка «рука — глаз» (Hand-Eye Calibration) — фундаментальная и ключевая задача в прикладном машинном зрении робота. Её цель — согласовать системы координат визуальной системы и робота, то есть определить относительное положение и ориентацию камеры и робота.

Когда мы хотим, чтобы робот с помощью зрения схватил предмет, нам нужно знать три соотношения:

1. Взаимное положение концевого эффектора и основания робота.
2. Взаимное положение камеры и концевого эффектора.
3. Взаимное положение и ориентацию объекта относительно камеры.

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

В зависимости от способа установки камеры различают два варианта:

1. Камера на конце манипулятора — «глаз в руке» (Eye in hand).
2. Камера установлена вне манипулятора, например на основании робота — «глаз вне руки» (Eye to hand).

1.2 Математическая модель калибровки «рука — глаз»

Для обоих вариантов — «глаз в руке» и «глаз вне руки» — уравнение решается одинаково: $AH=HB$. Сначала определим системы координат:

$F_b$: система координат основания (Base Frame), закреплённая на основании манипулятора; это глобальная система отсчёта для движения руки.

$F_e$: система координат концевого эффектора (End-Effector Frame), закреплённая на захвате или инструменте.

$F_c$: система координат камеры (Camera Frame), связанная с оптическим центром камеры; это система отсчёта визуального восприятия.

$F_t$: система координат калибровочной мишени (Calibration Target Frame), связанная с мишенью — шахматной доской или плоскостью с точками.

Отношения между системами координат обычно записывают однородной матрицей преобразования (жёсткое преобразование) $T$:

$$T_j^i=\left[\begin{array}{cc}{R}_j^i& t_j^i\\ 0& 1\end{array}\right]$$

Здесь $R\in SO(3)$ — матрица поворота (специальная ортогональная группа), а $t\in {\mathbb{R}}^3$ — вектор переноса в трёхмерном пространстве. Верхний и нижний индексы $T$ показывают, между какими двумя системами координат выполняется преобразование. Например:

$T_c^e$ — преобразование из системы координат камеры в систему координат концевого эффектора. Оно же задаёт позу камеры в системе концевого эффектора; в случае «глаза в руке» именно эта матрица и является искомой.

1.2.1 Eye In Hand («глаз в руке»)

Когда камера закреплена на конце манипулятора, преобразование между камерой и концевым эффектором постоянно. Такой вариант называют «глаз в руке». Для калибровки мишень неподвижно закрепляют, а манипулятор перемещают в разные позиции. С камерой на конце руки делают снимки мишени из разных положений.

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

$$T_t^b=T_{e1}^b{T}_{c1}^{e1}{T}_t^{c1}=T_{e2}^b{T}_{c2}^{e2}{T}_t^{c2}$$

Преобразуем это равенство:

$$(T_{e2}^b{)}^{-1}{T}_{e1}^b{T}_{c1}^{e1}{T}_t^{c1}=T_{c2}^{e2}{T}_t^{c2}$$

$$(T_{e2}^b{)}^{-1}{T}_{e1}^b{T}_{c1}^{e1}=T_{c2}^{e2}{T}_t^{c2}(T_t^{c1}{)}^{-1}$$

$$T_{e1}^{e2}{T}_{c1}^{e1}=T_{c2}^{e2}{T}_{c1}^{c2}$$

$$T_{e1}^{e2}{T}_c^e=T_c^e{T}_{c1}^{c2}$$

$$AH=HB$$

Здесь $T_c^e$ — именно та матрица $H$, которую нужно найти.

1.2.2 Eye To Hand («глаз вне руки»)

Когда камера установлена вне манипулятора, взаимное положение камеры и концевого эффектора меняется вместе с движением руки. Такой вариант называют «глаз вне руки». Для калибровки мишень закрепляют на конце манипулятора, а робот с мишенью фотографируется неподвижной камерой. Для точного результата обычно нужно более 10 снимков.

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

$$T_t^e=T_b^{e1}{T}_c^b{T}_{t1}^c=T_b^{e2}{T}_c^b{T}_{t2}^c$$

Преобразуем:

$$T_b^{e1}{T}_c^b{T}_{t1}^c=T_b^{e2}{T}_c^b{T}_{t2}^c$$

$$(T_b^{e2}{)}^{-1}{T}_b^{e1}{T}_c^b{T}_{t1}^c=T_c^b{T}_{t2}^c$$

$$(T_b^{e2}{)}^{-1}{T}_b^{e1}{T}_c^b=T_c^b{T}_{t2}^c(T_{t1}^c{)}^{-1}$$

$$(T_{e2}^b{T}_b^{e1})T_c^b=T_c^b(T_{t2}^c{T}_c^{t1})$$

$$AH=HB$$

1.3 Решение уравнения $AH=HB$

Это важная часть робототехники: с 1980-х годов академическое сообщество много исследует калибровку «рука — глаз» и создало множество методов решения уравнения $AH=HB$. Наиболее распространён пошаговый подход: уравнение раскладывают на части, сначала с помощью свойств матрицы поворота находят вращение, а затем подставляют найденное вращение в уравнение для переноса и вычисляют перенос.

Среди классических двухшаговых алгоритмов — метод Цая — Ленца (Tsai-Lenz), который переводит матрицу поворота в вектор поворота, и метод Парка (Park), который опирается на свойства группы Ли для матриц поворота (присоединённое представление). Ниже рассмотрим метод Парка.

1.3.1 Метод Парка: нахождение поворота

Все три переменные исходного уравнения — однородные матрицы преобразования (поворот и перенос записаны в одной матрице 4×4), описывающие переход между двумя системами координат:

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

Здесь $R\in SO(3)$ — матрица поворота, $t\in {\mathbb{R}}^3$ — вектор переноса.

Преобразуем исходное уравнение:

$$\begin{array}{l}AH=HB\\ \left[\begin{array}{cc}{\theta }_A& b_A\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{\theta }_X& b_X\\ 0& 1\end{array}\right]=\left[\begin{array}{cc}{\theta }_X& b_X\\ 0& 1\end{array}\right]\left[\begin{array}{cc}{\theta }_B& b_B\\ 0& 1\end{array}\right]\end{array}$$

Приравнивая части произведений матриц, получаем:

$$\begin{array}{rl}{\theta }_A{\theta }_X& ={\theta }_X{\theta }_B\\ {\theta }_A{b}_X+b_A& ={\theta }_X{b}_B+b_X\end{array}$$

Сначала решаем первое уравнение, которое содержит только матрицы поворота:

$$\begin{array}{rl}{\theta }_A{\theta }_X& ={\theta }_X{\theta }_B\\ {\theta }_A& ={\theta }_X{\theta }_B{\theta }_X^T\end{array}$$

Матрицы поворота образуют группу SO(3), которая является группой Ли. Каждой группе Ли соответствует алгебра Ли — линейное пространство низкой размерности, которое служит касательным пространством в окрестности единицы группы. Группа Ли и её алгебра связаны экспоненциальным и логарифмическим отображениями:

Для матрицы поворота $R$ и соответствующего элемента алгебры $\Phi$ связь записывается так:

$$R=\exp ({\Phi }^{\wedge })=\exp [\Phi ]$$

Здесь символ $[]$ обозначает операцию $\wedge$, то есть преобразование вектора в кососимметричную матрицу (векторное произведение).

Для SO(3) выполняется свойство присоединённого представления:

$$\begin{array}{rl}{\theta }_A& ={\theta }_X{\theta }_B{\theta }_X^T\\ \exp [\alpha ]& ={\theta }_X\exp [\beta ]{\theta }_X^T\\ \exp [\alpha ]& =\exp [{\theta }_X\beta ]\\ \alpha & ={\theta }_X\beta \end{array}$$

При нескольких наблюдениях задача сводится к следующей задаче наименьших квадратов:

Здесь $\alpha$ и $\beta$ — алгебры Ли соответствующих поворотов; это трёхмерные векторы, которые можно рассматривать как точки в трёхмерном пространстве. Тогда задача эквивалентна задаче регистрации облаков точек:

Решение этой задачи в смысле наименьших квадратов:

где:

1.3.2 Метод Парка: нахождение переноса

Найдя матрицу поворота, подставляем её во второе уравнение:

$$\begin{array}{rl}{\theta }_A{b}_X+b_A& ={\theta }_X{b}_B+b_X\\ {\theta }_A{b}_X-b_X& ={\theta }_X{b}_B-b_A\\ ({\theta }_A-I)b_X& ={\theta }_X{b}_B-b_A\\ C{b}_X& =D\end{array}$$

Здесь $C$ и $D$ известны. Так как матрица $C$ не обязательно обратима, преобразуем уравнение:

$$\begin{array}{rl}C{b}_X& =D\\ C^{T}C{b}_X& =C^{T}D\\ b_X& =(C^{T}C{)}^{-1}{C}^{T}D\end{array}$$

Так находится перенос.

При нескольких наблюдениях:

Итоговое решение:

$$\begin{array}{r}H=\left[\begin{array}{cc}{\theta }_X& b_X\\ 0& 1\end{array}\right]\end{array}$$

1.4 Методы калибровки «рука — глаз» в OpenCV и рекомендации

По принципу работы все алгоритмы калибровки «рука — глаз» делятся на три класса: независимые замкнутые решения, одновременные замкнутые решения и итерационные методы.

a. Независимые замкнутые решения (separable closed-form solutions): вращение и перенос находятся по отдельности.

Недостаток: ошибка вычисления вращения $R_x$ переходит в вычисление переноса $t_x$.

b. Одновременные замкнутые решения (simultaneous closed-form solutions): вращение и перенос находятся одновременно.

Недостаток: из-за шума найденная матрица вращения $R_x$ может оказаться не ортогональной. Поэтому её необходимо ортогонализировать, но соответствующий перенос при этом не пересчитывается, что приводит к ошибкам.

c. Итерационные методы (iterative solutions): вращение и перенос уточняются итеративно с помощью методов оптимизации.

Недостаток: вычислительные затраты могут быть большими, так как обычно используются сложные процедуры оптимизации. Кроме того, с ростом числа уравнений $n$ разница между итерационным и замкнутым решениями обычно уменьшается. Поэтому перед применением этого метода нужно решить, оправданы ли вычислительные затраты ради точности итерационного решения.

1.4.1 Интерфейс калибровки «рука — глаз» в OpenCV

OpenCV реализует в основном первые два класса методов. По умолчанию используется метод TSAI. Методы PARK и HORAUD также относятся к независимым решениям. ANDREFF и DANIILIDIS — одновременные замкнутые решения. По результатам экспериментов на одних и тех же данных считается, что среди независимых решений метод TSAI даёт наибольшую ошибку.

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

1.4.2 Рекомендации по калибровке

Новичкам при первой калибровке стоит учесть следующее:

1. Функция calibrateCamera даёт матрицы $R$ и $t$ от мишени к камере, но её внутренние параметры отличаются от тех, что используются при захвате, что вносит ошибку.
2. Направление углов калибровочной мишени перевёрнуто. По умолчанию нумерация идёт слева направо, но иногда она идёт справа налево, и системы координат оказываются несогласованными.
3. Мишень слишком мала и перемещается только в центре — из-за этого будут неточны края.
4. При калибровке нужно поворачивать мишень.
5. Снимков слишком мало: нужно 10 и более.
6. Некоторые камеры работают некорректно; для RGB-D нужно внимательно проверять матрицу преобразования.

1.5 Оценка качества калибровки «рука — глаз»

Литература

1. Алгоритм калибровки «рука — глаз» Цая — Ленца (A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration)
2. Руководство по калибровке «рука — глаз» для роботов (Mech-Mind)
3. Заметки по калибровке, часть 4: подробно о калибровке «рука — глаз»
4. Camera Calibration and 3D Reconstruction (документация OpenCV)
5. 3D Vision Workshop: калибровка «рука — глаз» (с кодом на OpenCV)