nabbla (nabbla1) wrote,
nabbla
nabbla1

Categories:

Ликбез по кватернионам, часть 17 - лидарная задача

Часть 1 - история вопроса
Часть 2 - основные операции
Часть 3 - запись вращения через кватернионы
Часть 4 - кватернионы и спиноры; порядок перемножения
Часть 5 - практическая реализация поворота
Часть 5 1/2 - введение метрики, "расстояния" между поворотами
Часть 6 - поворот по кратчайшему пути
Часть 6 1/4 - кратчайший поворот в общем случае
Часть 6 2/4 - поворот, совмещающий два направления
Часть 6 3/4 - кватернион из синуса и косинуса угла
Часть 7 - интегрирование угловых скоростей, углы Эйлера-Крылова
Часть 8 - интегрирование угловых скоростей, матрицы поворота
Часть 8 1/2 - ортонормирование матрицы и уравнения Пуассона
Часть 9 - интегрирование угловых скоростей с помощью кватернионов
Часть 10 - интегрирование угловых скоростей, методы 2-го порядка
Часть 10 1/2 - интегрирование с поддержанием нормы
Часть 11 - интегрирование угловых скоростей, методы высших порядков (в разработке)
Часть 12 - навигационная задача
Часть 13 - Дэвенпорт берёт след!
Часть 14 - линейный метод Мортари-Маркли
Часть 15 - среднее от двух кватернионов
Часть 15 1/2 - проверка и усреднение кватернионов
Часть 16 - разложение кватерниона на повороты
Часть 17 - лидарная задача

У данной задачи много имён. В английской литературе встретил Absolute orientation, Point set registration, а также Point cloud registration, scan matching.

Суть вот в чём: имеется твёрдое тело, на котором жёстко расположено N точек. Введена некая система координат, также жёстко связанная с этим твёрдым телом, и даны координаты N точек в этой системе, rk (rkx,rky,rkz), от слова reference - "эталонный".

А далее мы измерили координаты этих N точек в другой системе координат, mk (mkx,mky,mkz), от слова measured - "измеренный".

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

Я столкнулся с этой задачей в связи с юстировкой прибора, с использованием лазерного трекера. Лазерный трекер - это "адская смесь" дальномера и теодолита: наводишь его на определённую точку, он измеряет два угла (азимут и угол места), дальность и автоматически преобразует это в трёхмерные координаты точки, получая точность лучше десятых долей миллиметра. С его помощью можно узнать координаты всех моих уголковых отражателей, а потом координаты неких характерных точек моего прибора, и затем, дважды решив рассмотренную здесь задачу - узнать, что должен показать мой прибор, будучи идеально откалиброванным.

Подозреваю, что где-то в программном обеспечении для лазерного трекера данная задача давным-давно "зашита", вместе с решением, но точно сказать не могу - "зависит от комплектации", а приобретать такой приборчик себе точно не буду по причине его запредельной цены. Пока знаю лишь, что такой наличествует у РКК Энергия, и нынче они практически отказались от более классических измерительных приборов вроде автотеодолита и плоских зеркал, наклеиваемых на различные части космического аппарата.


Ну и для ориентации в пространстве некоего автономного агрегата, имеющего на борту сканирующий лидар или прочие штучки, позволяющие напрямую получать трёхмерные координаты окружающих объектов, данная задача абсолютно необходима!


Математическая формулировка задачи
Дано N "эталонных" векторов rk, k=1..N и N "измеренных" векторов mk. Соответствие между точками известно! То есть точка под номером k имеет "эталонные" координаты rk и "измеренные" mk и никак иначе.

Необходимо найти такой вектор t (от слова translation - параллельный перенос), число s (scale - масштаб) и поворот R() (его мы пока записываем как преобразование, а как именно он выражен - матрицей, кватернионом, углами Эйлера и пр. - пока не важно), чтобы выражения



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



была минимальной, это приведёт к наиболее простым результатам.

Взятие центроидов

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





А также "передвинем начало координат" в эти центроиды, т.е введём новые вектора:





(Приглашается 2born рассказать, как заставить TeX не писать треугольные скобки настолько "врастопырку". UPD. Исправил, СПАСИБО!)

Старые выразятся через новые следующим образом:





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



Мы знаем, что оператор поворота - линейный, поэтому поворот от суммы векторов равен сумме векторов, повёрнутых "по отдельности":



Введём новый вектор t':



С ним, выражение для Θ упростится:



Раскроем квадрат модуля разности, приняв первые 2 слагаемых "за единое целое":



Правое слагаемое - это скалярное произведение. Вектор в левой его части равен НУЛЮ, поскольку новые вектора m'k и r'k взяты "относительно среднего значения", т.е сумма что одних, что других будет нулевой. Благодаря этому у нас остаётся только два слагаемых:



И только правое зависит от вектора t. Поэтому мы можем найти данный вектор, минимизируя правое слагаемое, а именно, приравняв его нулю:



откуда:



Ответ выражается через два усреднённых вектора (центроида) и пока ещё неизвестные нам масштаб и поворот. Но смысл достаточно ясный: как только мы узнаем поворот и масштаб - параллельный перенос найдём как разницу между усреднёнными векторами.

А минимизировать мы теперь должны левое слагаемое:



Нахождение масштаба

Раскроем в этом выражении квадрат модуля:



В третьем слагаемом можно убрать поворот, взяв просто квадраты длин векторов rk, поскольку поворот не меняет длины. И далее, введём обозначения:







Всё это скаляры: сумма квадратов длин измеренных векторов, сумма скалярных произведений одних на повёрнутые другие, и сумма квадратов длин эталонных векторов. S - от слова sum, D - от слова Dot product (скалярное произведение).

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



причём величины Sm, D и Sr не зависят от масштаба s. D зависит от выбранного поворота, но какой бы поворот мы ни выбрали, далее мы можем минимизировать Θ, правильно подобрав s. А именно: Sr>0, то есть у нас получается выпуклая парабола. Точка минимума параболы ax2+bx+c - это -b/2a. В нашем случае:



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

Если теперь подставить s в Θ, получим:



Причём Sr и Sm константы, зависящие только от исходных векторов. От поворота зависит только D. Поэтому остаётся лишь максимизировать D.

Более "симметричный" масштаб

В [данной работе] (которую я тут и излагаю немного по-своему) очень беспокоятся, что если посчитать масштаб по выражению выше, пропадает симметрия между rk и mk: если мы поменяем их местами, то могли бы ожидать вектор параллельного переноса со знаком "минус", обратное значение масштаба и обратный поворот - это было бы логично. Но, вообще говоря, не получаем.

Но если, после нахождения параллельного переноса постараться минимизировать вот такую величину:



то получим следующее выражение:



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





Очень любопытный результат:
- теперь, чтобы найти масштаб, нам необязательно искать поворот - ответ уже выражается только через входные векторы. По сути, "дисперсию измеренных векторов поделить на дисперсию эталонных - и взять квадратный корень".
- очевидно, что если rk и mk поменять местами, то получим попросту обратную величину масштаба, что логично.

А чтобы найти поворот, надо, как и в прошлом случае, максимизировать D.

Нахождение поворота

Итак, нам надо найти максимум выражения D:



Сумма скалярных произведений измеренных векторов с повёрнутыми эталонными должна оказаться максимальной.

Но ведь мы эту задачу уже решали, см. часть 12 - навигационная задача. Там мы начали с "задачи Вахбы", минимизации суммы квадратов ошибок (но там ничего, кроме поворота, и не надо было находить) - и очень быстро привели эту задачу к максимизации суммы скалярных произведений!

Далее в рассмотренной нами работе по сути повторяют результаты Дэвенпорта (см. Дэвенпорт берёт след!), попутно вводя кватернионы и их основные свойства, в количестве, достаточном, чтобы прийти к результату. Напомним: из эталонных и измеренных векторов неким довольно хитрым образом составляется симметричная матрица 4х4, находят её максимальное собственное значение и соответствующий собственный вектор из 4 компонент. Этот "вектор" и будет кватернионом искомого поворота, останется лишь его отнормировать (или изначально находить его с дополнительным условием единичной нормы).

Но я полагаю, что с тем же успехом можно применить линейный метод Мортари-Маркли. Он решает чуть другую задачу (по-другому сформулирован критерий "близости" двух векторов на плоскости) и обладает "особыми точками" - если имеем дело с поворотом на 180°, то придём к вырожденной системе линейных уравнений. Вблизи 180° решение будет, но все коэффициенты уравнений существенно уменьшатся, что может повлечь численную неустойчивость. Но если мы заранее знаем, что угол поворота далёк от 180°, то метод должен отработать на "пять с плюсом". Всё-таки, решить систему из 3 линейных уравнений с 3 неизвестными - немного приятнее, чем искать собственный вектор матрицы 4х4, соответствующий максимальному собственному значению!

Подытожим.

Получив две группы векторов - эталонные rk и измеренные mk, мы:
1. Находим центроиды, т.е среднее арифметическое первой и второй группы векторов:





2. Центрируем векторы относительно их средних:





Исходные векторы больше не нужны.

3. Решаем навигационную задачу для эталонных векторов r'k и измеренных m'k (отцентрированных) любым понравившимся способом: через матрицу Дэвенпорта, линейным методом Мортари-Маркли или как-нибудь ещё, результатом становится поворот в любой удобной нам форме.
4. Находим масштаб s по одной из двух приведённых здесь формул. Наверное, лучше взять вторую:



она не зависит от поворота и симметрична, если эталонные и измеренные векторы поменять местами. А можно и вовсе постановить масштаб единичным, если мы знаем, что масштабирование возникать не может.
5. Зная поворот и масштаб, находим параллельный перенос:





В следующей части (которая в книге "уползёт" в упражнения и ответы к ним) попробую всё это применить "на практике".
Tags: ПЛИС, кватернионы-это просто (том 1), математика, программки, работа, странные девайсы
Subscribe

Recent Posts from This Journal

  • Великая Октябрьская резня бензопилой

    Сегодня прокатился прочистить Абрамцевскую просеку. Как обычно, с приключениями. Выезжал на велосипеде, а вернулся на самокате. Первый раз по этим…

  • Очередная несуразность в единицах измерения

    Когда-то я написал программу PhysUnitCalc - калькулятор, умеющий работать с размерностями. Мне казалось, что я наступил уже на все грабли, которые…

  • Big Data, чтоб их... (3)

    "В предыдущих сериях": мой прибор выдаёт 6 значений: 3 координаты и 3 угла, т.е все 6 степеней свободы твёрдого тела. Причём ошибки измерения этих 6…

  • Post a new comment

    Error

    Anonymous comments are disabled in this journal

    default userpic

    Your IP address will be recorded 

  • 5 comments