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 - лидарная задача
Задачка к части 17

Хочется опробовать это дело на более-менее реальных данных. Имеется у нас такая вот мишень:

IMG20201021153945.jpg

ну, в реальности вместо светоотражающей плёнки будут уголковые отражатели из кварцевого стекла, а вместо ДСП - алюминиевый сплав АМг6. Не знаю, почему именно его у нас так любят - заклёпками, сваркой, штамповкой не балуемся, исключительно всё выфрезеровываем из цельного куска металла, вроде есть и более прочные и лёгкие для таких случаев. Слышал, что он лучше переносит вибрации и удары - не такой хрупкий, как другие. Может, и так. А скорее всего, просто, как начали - так зачем чего-то менять.

Так вот, координаты центров отражателей в "эталонной" системе координат. Её начало расположено примерно там, где пересекаются две карандашные линии. "На нас" (перпендикулярно доске) - ось X, "слева направо" - ось Y, "снизу вверх" - ось Z. Примерно так:

r1 (60; -104; 0),
r2 (60; 104; 0),
r3 (0; -69; 43),
r4 (0; -84; -51),
r5 (0; -158; 9),
r6 (0; 158; 9),
r7 (0; 69; 43),
r8 (0; 84; -51)

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

И давайте мы будем эту хреновину промерять "сбоку":
p4pb14464164.jpg

Лазерный трекер расположится чуть слева. И допустим, мы получили следующие координаты этих 8 точек:

m1 (-1104,1; 439,8; -0,6),
m2 (-896,1; 440,9; 0,5),
m3 (-1068,7; 499; 42,6),
m4 (-1083,4; 500,8; -50,2),
m5 (-1158,7; 500,3; 9,9),
m6 (-841,1; 499; 8,3),
m7 (-931,1; 500,5; 42,8),
m8 (-915,3; 500,7; -50,4)

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

Под катом решение - чему равен поворот, масштаб и параллельный перенос. Если кому хочется попробовать самостоятельно - под кат пока не заглядывайте :)


1. нахождение средних векторов, "центроидов":





Получаем





В первом случае, не округляй мы до целых миллиметров, вообще должны были получить (15;0;0). Ну да ладно.

2. центрирование векторов относительно средних:

r'1 (45; -104; -0,25),
r'2 (45; 104; -0,25),
r'3 (-15; -69; 42,75),
r'4 (-15; -84; -51,25),
r'5 (-15; -158; 8,75),
r'6 (-15; 158; 8,75),
r'7 (-15; 69; 42,75),
r'8 (-15; 84; -51,25)

m'1 (-104,125; -45,425; -0,9625),
m'2 (103,875; -44,225; 0,1375),
m'3 (-68,725; 13,875; 42,2375),
m'4 (-83,425; 15,675; -50,5625),
m'5 (-158,725; 15,175; 9,5375),
m'6 (158,875; 13,875; 7,9375),
m'7 (68,875; 15,375; 42,4375),
m'8 (83,375; 15,675; -50,7625)

3. Решаем навигационную задачу
Воспользуемся линейным методом Мортари-Маркли.

3.1. Строим суммы и разности векторов:

s1 (-59,125; -149,425; -1,2125),
s2 (148,875; 59,775; -0,1125),
s3 (-83,725; -55,125; 84,9875),
s4 (-98,425; -68,325; -101,8125),
s5 (-173,725; -142,825; 18,2875),
s6 (143,875; 171,875; 16,6875),
s7 (53,875; 84,375; 85,1875),
s8 (68,375; 99,675; -102,0125)

d1 (149,125; -58,575; 0,7125),
d2 (-58,875; 148,225; -0,3875),
d3 (53,725; -82,875; 0,5125),
d4 (68,425; -99,675; -0,6875),
d5 (143,725; -173,175; -0,7875),
d6 (-173,875; 144,125; 0,8125),
d7 (-83,875; 53,625; 0,3125),
d8 (-98,375; 68,325; -0,4875)

3.2. Вычисляем вектор v и матрицу M:




Веса ωn мы берём все единичные. Вектор v в таком случае - просто сумма попарных векторных произведений,

v (123,5; -119,7; 201394,8)

Компонента z превышает x,y более чем в 1000 раз! То есть, метод уже практически "догадался", что поворот был осуществлён вокруг оси Z. Но надо ещё выразить его в правильных единицах и устранить "перекрёстные связи", для чего и нужна матрица. Мы уже выписывали в явном виде произведение двух матриц внутри суммы:



Матрица всегда симметричная, поэтому из 9 компонент достаточно вычислять и суммировать 6.

В итоге получаем:



3.3 Решаем систему линейных уравнений:



Получаем

θ (0,004400749; 0,005808585; -0,999901563)

3.4 Находим кватернион поворота

Ненормированный кватернион получается "прибавлением единички", т.е θ - это наша векторная / мнимая часть кватерниона, а единица - скалярная:

Λненорм = 1 + 0,004400749i + 0,005808585j -0,999901563k

А после нормировки выходит:

Λ = 0,70713 + 0,00311i + 0,00411j -0,70706k.

Правильным ответом был бы поворот на -90° вокруг оси Z, т.е ПРОТИВ ЧАСОВОЙ СТРЕЛКИ, если смотреть в ту сторону, куда "направлена стрелочка" оси Z:



Если посчитать "расстояние" между двумя этими кватернионами с помощью метрики, получим 0,59° - такова ошибка наших измерений, "суммарно" по всем трём осям.

Если мы "уберём" шум - ответ получится точным, что, в общем-то, логично.

4. Измерение масштаба:



Получаем s=1,000114392

Когда мы моделировали эти измерения, мы считали, что масштаб единичный. То есть, в оценке масштаба ошиблись на 0,011%, не так уж и плохо. Но если мы априори знаем, что масштабирования происходить не будет, координаты могут отличаться лишь параллельным переносом и поворотом, то лучше вообще отказаться от оценки масштаба и сразу постановить s=1. Это даст чуть лучшую оценку параллельного переноса, а на оценку поворота никоим образом не повлияет.

5. Измерение параллельного переноса:



Подставив найденный нами поворот и масштаб, получаем

t (-999,977; 500,1285; 0,265647)

Неплохо, ведь правильный ответ: (-1000; 500; 0). Усреднение по 8 точкам явно нам помогло снизить ошибку с 1 мм (такую ошибку по модели мы внесли в измерение каждой координаты каждой точки) до менее 0,266.


А ведь работает алгоритм! Прямо-таки чудес не совершает, ошибка в 1 мм это ошибка в 1 мм, а точек здесь не сотни и не тысячи, а всего 8, а избыточности и того меньше. Просто чтобы эта задача однозначно решалась, нужно хотя бы 3 точки. Но всю имеющуюся информацию алгоритм использует по-максимуму!

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

  • Post a new comment

    Error

    Anonymous comments are disabled in this journal

    default userpic

    Your IP address will be recorded 

  • 1 comment