Видеоизмеритель с круговым обзором
Пришло время подтверждать точность работы "изделия". Это значит, нужен ещё один способ измерить взаимное положение "изделия" и оптической мишени, причём все 6 степеней свободы, условно, 3 "поступательных" (вперёд-назад, влево-вправо и вверх-вниз) и 3 "вращательных" (курс, тангаж, крен). Причём должны использоваться только поверенные приборы, внесённые в госреестр средств измерения.
Мы дружно "облизывались" на лазерный трекер, на худой конец, на тахеометр, но дорогущие же они! Просто так, "про запас" (ещё не написав методики подтвеждения точности), их приобретать не стали, обойдясь обычным теодолитом, даже без автоколлимационного объектива.
Хотя и теодолит - штука офигенная, 2 угловые секунды точности - не хухры мухры!
Довольно быстро родилась методика: мы устанавливаем теодолит на отдельный штатив, расположенный примерно на полпути от "изделия" до мишени, но слегка в стороне, чтобы "не мешать". Наводим его на каждый уголковый отражатель мишени, снимая горизонтальный и вертикальный углы. А дальше, по сути, применяем уже написанный "алгоритм ВИПС", чтобы по этим данным определить взаимное положение теодолита и мишени. Остаётся теперь навести теодолит на характерные точки ВИПС (опробовал на днях, вполне получается, хотя поначалу были сомнения - "чёрное на чёрном") - и тем же самым алгоритмом найти взаимное положение теодолита и ВИПС. Вот и всё, задача решена, выкидываем теодолит "из уравнения" - и получаем взаимное положение ВИПС и мишени, измеренное прибором из госреестра.
Всё казалось вполне очевидным, пока меня не попросили всё это строго описать в форме методики. И тут-то попытался это всё провести "вживую" и понял - алгоритм ВИПС предполагал сколько-нибудь узкое поле зрения, и при попытке применить его на теодолите, который может крутиться как угодно, начинаются проблемы. Когда-то я поставил "костыль", чтобы их решить, и этот костыль мне уже успел аукнуться, но записывать его на официальной бумаге, которую прочитают десятки человек (нормоконтроль, метрологи, военпреды, разные службы у заказчика) мне было стыдно, резко захотелось как-то обойтись без него, сделать "как положено"!
В алгоритме ВИПС предполагался "идеальный объектив", камера-обскура с точки зрения положения точек на фотоприёмной матрице. Т.е наша маленькая тонкая линзочка помещена в начало координат, (0;0;0), а фотоприёмная матрица условно располагается на плоскости x=-1. В таком случае, точечный объект, расположенный по координатам (x;y;z) спроецируется следующим образом:
То, что получилось на реальной фотоприёмной матрице, масштабируется в эти безразмерные величины, и после калибровки масштаба всё получается очень недурственно! Ну, есть ещё дисторсия на краях, но это уже дело техники, эти проценты скомпенсировать. Вполне рабочий метод.
Проблемы начинаются, если вдруг наше поле зрения приблизится к 180°, т.е объектив сможет видеть всё полупространство! Теперь лучи, идущие по нормали к оптической оси, будут бесконечно долго приближаться к плоскости x=-1, т.е чтобы реально зафиксировать всё это полупространство, нам нужна бесконечно протяжённая фотоприёмная матрица!
В ситуации с теодолитом, в теории, наше "поле зрения" будет чуточку меньше, чем 180°, как видно по картинке. Но получить "поле зрения" в 170..175° здесь вполне реально, и математика уже начинает "ломаться", ведь tg(87,5°)≈22,9, а производная и вовсе ≈525, т.е во столько раз увеличивается локальный масштаб по одной оси! То есть, если мы в действительности будем брать азимут с теодолита, но заменять его на тангенс, как будто бы там стояла плоская фотоприёмная матрица и объектив без дисторсии, алгоритму "будет казаться", что надо брать те самые точки, проецируемые ближе к "бесконечности", т.к он полагает, что пиксели на фотоприёмной матрице идут равномерно, и именно там мы получим максимально возможную точность.
На практике всё ещё хуже! Азимутальная ось теодолита (говоря простым руским языком: алидада) не имеет чётко обозначенного "нуля". Ноль может выставить оператор в любой момент, но во время наших измерений это можно сделать только ОДИН РАЗ. Нельзя сначала выставить ноль, работая по мишени, а потом, повторно, работая по ВИПС, мы тем самым потеряем "связь" между ними! И только если мы сами ТЩАТЕЛЬНО установим ноль где-то посередине между ВИПСом и мишенью (т.е измерения мишени будут давать углы близкие к 90° по азимуту, а измерения ВИПСа - близкие к -90°), мы уместимся в прокрустово ложе. Но там нет никаких ориентиров, и придумывать какие-то специальные усложнения в методике, только потому что у меня математика сбоит - неправильно это!
Когда-то я сделал костыль: уже получив все показания азимутов (вообще не беспокоясь о положении нуля), я беру среднее арифметическое для направлений на мишени, и ещё одно среднее арифметическое для направлений на ВИПС. Так сказать, "виртуально" изображаю ДВЕ узкопольные камеры, одна смотрит прямой наводкой на мишень, вторая - на ВИПС, и они повёрнуты одна относительно другой на известный угол. Потом, из всех направлений на мишень надо вычесть средний азимут, также как и из всех направлений на ВИПС, ну и под самый конец понадобится дополнительный шаг при сопоставлении координат. Аукнулся этот "костыль" тем, что я в какой-то момент утерял показания среднего азимута (программу дописывал впопыхах, нужно было отчитываться ВЧЕРА), из-за чего не смог подтвердить точность определения курса, хотя показания несколько часов собирали при разных положениях мишени. Ручным теодолитом на 8 точек наводиться, потом эти углы с точностью до секунды в компьютер вбивать из дисплейчика теодолита, то ещё удовольствие.
Для упрощения, поиграемся сначала на игрушечном видеоизмерителе, т.е работающем исключительно на плоскости, изображённой выше. Все точки у нас имеют координаты (anx; any) в системе координат, связанной с мишенью (то есть, эти координаты неизменны), а единственные два сдвига, которые могут здесь происходить - это параллельные переносы по осям X и Y, без вращения, обозначенные tx и ty, соответственно, от слова translation.
В системе координат видеоизмерителя точки будут иметь координаты
но напрямую их измерить мы не можем. Ранее мы полагали, что игрушечный видеоизмеритель получает значение
что как раз очень похоже было на работу узкопольного объектива с плоской фотоприёмной матрицей (в игрушечном измерителе, это фотоприёмная линейка).
Пока что tx, ty неизвестны, но мы знаем некоторое начальное приближение, tx0, ty0. Поэтому мы брали "виртуальную точку"
в этом месте на фотоприёмной линейке мы должны были обнаружить точку, если наше начальное приближение верно. На деле мы получим невязки:
то есть разницу между ожидаемым положением точек и их истинным (измеренным) положением. Чтобы понять, что они значат, мы берём частные производные от gn по всем параметрам сближения, в нашем случае по tx0 и ty0, т.е смотрим, как изменится точка, если эти параметры начать варьировать. Мы получали вектора (а в нормальной задаче: матрицы) Mn:
^2},)
Затем мы получали ненормированную оценку сдвига, вектор 1×2 (это всегда будет вектор с количеством компонент, равным количеству параметров сближения, в нормальном измерителе это 1×6):
И наконец, чтобы её отнормировать, мы составляем матрицу J:
и ответом становится:
Это оценка сдвига, т.е насколько tx, ty отличаются от начального приближения. То есть, напоследок надо будет обновить их значения:
Это одна итерация процесса, но обычно одной итерации вполне хватает, если кадры идут довольно часто, а движения относительно неторопливы.
Теперь рассмотрим ту же задачу, но в случае теодолита
Вся разница, что вместо проекции на плоскость, мы, по сути, делаем проекцию на окружность:
=arg(b_{nx}+ib_{ny}))
Функция atan2 имеется практически во всех математических библиотеках, а по смыслу она эквивалентна аргументу комплексного числа, составленного из этих двух координат, принимая значения от -π до π.
В первом и четвёртом квадрантах (т.е при x>0), atan2 выражается через обычный арктангенс:
=arctg(y/x))
отсюда и пошло его название и странный порядок аргументов, с игреком идущим первым! Разница в том, что atan2 "не сходит с ума", когда x приближается к нулю и достигает его, ну и во втором и третьем квадранте даёт другие значения. По большому счёту, эта функция составлена из нескольких "ветвей", которые аккуратно "склеены" между собой. При грамотной (численно устойчивой) реализации разделение проходит по прямым y=x и y=-x, так что когда |y|>|x|, дробь y/x заменяется на x/y. Мы для QuatCore делали интересный вариант, atan1, у которого два аргумента, как у atan2, но ВНЕЗАПНО от одновременной смены знаков x,y (т.е с переключением между первым и третьим, либо между вторым и четвертым квадрантом) результат измениться НЕ ДОЛЖЕН. Подобное очень удобно для преобразования кватернионов в "обычные углы", чтобы при домножении ответа на 2, получить результат от -π до +π, тогда как обычный atan2 дал бы от -2π до +2π, т.е двойное перекрытие, которое потом нужно убирать. Но это мы отвлеклись.
В принципе, задача вполне решаема в лоб. Точно так же мы теперь для каждой точки находим её "виртуальную проекцию",
)
и невязку:
Далее мы должны взять частные производные gn по tx0 и ty0, составить матрицы частных производных Mn (в нашем случае, это вектора), и далее по накатанной.
Но повсюду лезут костыли.
Когда мы начнём считать невязку, можем, имея очень неплохое начальное приближение, внезапно получить разницу в 2π либо -2π, поскольку оба направления будут вблизи "линии разрыва", где atan2 скачкообразно переходит от +π на -π. Понятно, ничего фатального в этом нет, можно написать проверку на данный случай и при необходимости прибавить либо вычесть 2π.
На деле есть ещё одна подлянка: это "наш" atan2, когда мы виртуально проецируем точку на единичкую окружность, даёт результат от -π до +π. А теодолит, по крайней мере тот, что сейчас у меня, выдаёт азимуты от 0° до 360°, так что надо ещё эту неувязочку убирать. т.е для углов 180°..360° вычесть 360°.
Затем начинается веселье со взятием производной от atan2. Зачастую его приводят в таком виде:
=\left\{\begin{array}{ll}arctg(y/x),&x%3E0,\\\pi+arctg(y/x),&x%3C0,y\ge0\\-\pi+arctg(y/x),&x%3C0,y%3C0\\\frac{\pi}{2},&x=0,y%3E0,\\-\frac{\pi}{2},&x=0,y%3C0,\\\dagger&x=y=0\end{array}\right.)
(последний вариант - "кранты", можно хоть прерывание запускать в таком случае)
Здесь не вполне очевидно, как себя поведёт производная при x=0, смогут ли правильно "склеиться" три выражения, для x=0, x>0 и x<0? Нужно вспоминать первый курс матана и аккуратно считать эту производную чуть ли не по определению. А когда ответ скачкообразно меняется от -π к π (или назад), производная не должна существовать, хотя значение производной "слева" и "справа" одинаковое.
Есть определение atan2 получше, и более соответствующее тому, как оно реализуется в действительности:
=\left\{\begin{array}{ll}arctg(y/x),&x%3E0,|y|%3Cx,\\\frac{\pi}{2}-arctg(x/y),&y%3E0,y%3E|x|,\\\pi+arctg(y/x),&x%3C0,y{\ge}0,|y|%3C|x|,\\-\pi+arctg(y/x),&x%3C0,y%3C0,|y|%3C|x|\\-\frac{\pi}{2}-arctg(x/y),&y%3C0,|y|%3E|x|,\\\dagger,&x=y=0\end{array}\right.)
(завтра вечером здесь будет рисунок, нарисовал но не сохранил на флэшку)
Два выражения слева и справа, в теории, работали бы вплоть до x=0, поэтому изначально мы только этот случай и рассматривали особо. Но на практике, "приближаться к бесконечности" идея сомнительная, поэтому мы два частных случая "малого икса" расширяем в целые квадранты. Зато теперь у нас аргумент арктангенса будет принимать значения от 0 до 1, и такой арктангенс реализовать уже довольно легко.
Для нас важно, что теперь в месте "склейки" на x=0 никаких бесконечностей заведомо не возникает, мы можем взять производную каждой функции и убедиться, что при y=x или y=-x (где нужно от одной переходить к другой) производные совпадают, а именно:
)}{\partial{x}}=-\frac{y}{x^2+y^2},)
)}{\partial{y}}=\frac{x}{x^2+y^2}.)
Но всё ещё остаётся сомнительный с точки зрения матана переход через ось X, при x<0, где ответ скачкообразно меняется на 2π. Можно это обосновать тем, что мы как раз-таки "ручками" будем исправлять одно из значений, чтобы скачка не было, а как только ступенька уходит, производная наконец-то начинает существовать, ровно такая, как указано выше...
В общем, с производной костыли именно в плане обоснования, так-то оно худо-бедно заработает, но всё равно неприятно...
Костыли можно все убрать!
Для этого НЕ БУДЕМ ТОРОПИТЬСЯ БРАТЬ ЭТОТ АРКТАНГЕНС. Мы просто запоминаем про себя, что нас интересует угол, но не интересует длина вектора (bnx, bny). Угол, полученный с теодолита, мы сразу же преобразуем в синус и косинус, т.е вместо fn мы получаем пары чисел (cn, sn). Ещё удобнее будет задействовать комплексные числа. Точку (bnx, bny) представим в виде:
а показания теодолита:
Тогда "комплексную" невязку можно найти так:
Мы ожидаем, что начальное приближение довольно неплохое, так что направления почти совпадают, и при данном умножении получим комплексное число с очень малым аргументом, так что имеем полное право найти его таким способом:
\approx\frac{Im(\hat{h}_n)}{Re(\hat{h}_n)})
это и будет искомая невязка, причём мы избежали всевозможной возни с прибавлением/вычитанием 360°, с изучением "диапазона" теодолита (0°..360°) и "диапазона" atan2 (-180°..180°), довольно изящно.
Мы можем и производной atan2 тогда вообще не касаться. Вместо этого применим производную по направлению. Мы говорим, что если один из параметров сближения варьируется, то точка (bnx, bny) куда-то поползёт. Но нам плевать, если она поползёт вдоль радиуса, нам важно понять, насколько она двинется ПОПЕРЁК, то есть по касательной. Касательную построить очень легко, это вектор (-bny, bnx).
И построим вектор частных производных по одному из параметров сближения, по tx0 например:
}}{\partial{t_{x0}}}=\frac{\partial{\left(\begin{array}{c}a_{nx}+t_{x0}\\a_{ny}+t_{y0}\end{array}\right)}}{\partial{t_{x0}}}=\left(\begin{array}{c}1\\0\end{array}\right))
(да, эти два параметра сближения очень просты, не добавили мы пока поворота)
Найти скалярное произведение с вектором касательной не составляет труда, получим попросту -bny = -any-ty0.
Но вектор касательной не был единичным, поэтому вместо честной производной по направлению мы получили результат, завышенный на длину вектора (-bny, bnx). Кроме того, мы не выполнили проекции исходного вектора на единичную окружность (то есть, тупо, его нормировки, приведения к единичной длине), для чего нужно снова поделить на длину исходного вектора (bnx, bny). Их длина, очевидно, одинакова, так что получим в знаменателе квадрат длины. И, о чудо, мы получаем результат:
Похожие выкладки для частной производной по ty0 дадут:
Это тот же результат, как и при использовании atan2, но нам не понадобилось влезать в тонкости его реализации, рассматривать, как он "сшивается" по квадрантам. И тут уж нет никаких сомнений: при вычислении mn1 и mn2 у нас не может выползти бесконечностей, делений на ноль, ступенек, некорректной проекции "не с той стороны", тут всё чётко. Единственный способ обратить знаменатель в ноль - это расположить мишень аккурат в центр координат измерителя (в нашем случае теодолита), но это очень экзотическая ситуация, хоть и представимая. [но, чёрт возьми, КАК?]можно построить такой объектив, передняя главная точка которого будет "висеть в воздухе" перед объективом, т.е в неё теоретически можно поместить мишень, и вот этот шаг поломает наш алгоритм! Пока ничего такого не ожидается, такой алгоритм с "круговым обзором" нам нужен только для теодолита, а там началом координат является пересечение азимутальной и угломестной оси, в эту точку ни одна мишень заведомо не попадёт.
Если далеко оттащить мишень по оси X, задача сведётся к предыдущей!
То есть, если принять bnx >> bny, мы можем упростить mn1 и mn2:
^2},)
Это хороший признак.
А вот если не делать никаких предположений, мы замечаем приятную симметрию между осями X и Y, которая раньше напрочь отсутствовала! Такой системе действительно не важно, с какой стороны будет приближаться мишень - хоть спереди, хоть сзади, хоть сбоку. И да, для теодолита, у которого направление "вперёд" попросту отсутствует, это очень хорошо.
Пример работы
Мишень состоит из двух точек, имеющих координаты A1(0;1) и A2(0;-1) в системе координат мишени. Она отодвинута от теодолита на вектор (10;2), т.е на 10 единиц "вправо" и на 2 единицы "вверх". То есть, в системе координат теодолита точки имеют координаты B1(10;3) и B2(10;1). Но мы об этом пока не знаем, мы дальномером (а то и рулеткой) измерили примерное расстояние, 10, поэтому ввели начальное приближение t0(10;0).
Теодолит показал нам азимуты f1=16°41'57'' для первой точки и f2=5°42'38'' для второй. [И ещё одна подлянка...]Теодолит отсчитывает азимут по часовой стрелке, если смотреть на него сверху. Мы привыкли в задачах на плоскости отсчитывать углы против часовой... Так что будем считать, что f1 и f2 - это уже с обращённым направлением. Ещё один повод поскорее переходить к синусу и косинусу...
Мы преобразуем это в комплексные числа:
Также в виде комплексных чисел запишем начальные приближения к положению точек:
Посчитаем комплексные невязки:
Значения очень похожие, потому что выглядит всё, ПРЕИМУЩЕСТВЕННО, будто мы просто чуть ошиблись с углом, всё сдвинуто на одно значение.
Найдём невязку "в радианах":
Если бы мы работали напрямую в углах (взяли исходные углы теодолита, а ожидаемые углы нашли бы через арктангенс), мы бы получили 0,1917 и 0,1993, ошибка составляет 1,2% и 1,3%, соответственно. Для такого метода это крохи, мы всё ж начали с очень кривого приближения, аж 11°. Если захочется, сделаем ещё вторую итерацию.
Теперь находим "матрицы" частных производных, M1 и M2, хотя в нашем случае это вектора 1×2:
^2}{\approx}0{,}009901,)
^2}{\approx}0{,}09901.)
Имеем более сильную реакцию на "поперечный" сдвиг (по оси Y) и в 10 раз более слабую на "продольный", что, в общем, логично.
Найдём ненормированную оценку сдвига:
)
Найдём нормирующую матрицу:
)
Из-за "симметричного" расположения мишени при начальном приближении, матрица получилась диагональной. Обратная к ней:
)
Наконец, находим нормированную оценку сдвига:
)
Смещение по оси Y нашли очень точно, правильный ответ 2 ровно. А вот по оси X должен был получиться строго нолик, но не склалось. Новая оценка положения мишени:
Можно провести вторую итерацию. Обновляем начальные приближения к положению точек:
Посчитаем комплексные невязки:
Уже видно, что невязки упали более чем в 20 раз в сравнении с первой итерацией.
Найдём невязку "в радианах":
В этот раз отличие от "честного вычисления с арктангенсами" составляет 0,004% и 0,0005% для первой и второй точки соответственно.
Повторно находим "матрицы" частных производных:
Найдём ненормированную оценку сдвига:
)
Знак сдвига по оси X правильный, мы переоценили эту координату аж на 0,40 единицы. По оси Y вообще сдвиг должен был быть нулевым, но у нас сильная перекрёстная связь, так что не торопимся с выводами...
Найдём нормирующую матрицу:
)
И обратим её:
)
Находим нормированную оценку сдвига:
)
Внеся данную коррекцию, получаем ответ на второй итерации:
Мы явно стягиваемся к правильному ответу, что не может не радовать. Измерение дальности, как водится, "зашумлено" даже при абсолютно не шумящих данных, в данном случае небольшими ошибками при линеаризации нашей нелинейной задачи. Ничего страшного, данный алгоритм будет исполняться на персональном компьютере, можно работать с 80-битной плавающей точкой, и если так уж захочется, сделать ТРИ, а то и ЧЕТЫРЕ итерации (вот ужас-то какой!), чтобы выйти уже на точность, которую может обеспечить теодолит.
На этом пока всё. Мы ещё не попробовали измерить положение "изделия" относительно теодолита, там как раз интересный случай со ступенькой 2π. Но чуть позже.
Мы дружно "облизывались" на лазерный трекер, на худой конец, на тахеометр, но дорогущие же они! Просто так, "про запас" (ещё не написав методики подтвеждения точности), их приобретать не стали, обойдясь обычным теодолитом, даже без автоколлимационного объектива.
Хотя и теодолит - штука офигенная, 2 угловые секунды точности - не хухры мухры!
Довольно быстро родилась методика: мы устанавливаем теодолит на отдельный штатив, расположенный примерно на полпути от "изделия" до мишени, но слегка в стороне, чтобы "не мешать". Наводим его на каждый уголковый отражатель мишени, снимая горизонтальный и вертикальный углы. А дальше, по сути, применяем уже написанный "алгоритм ВИПС", чтобы по этим данным определить взаимное положение теодолита и мишени. Остаётся теперь навести теодолит на характерные точки ВИПС (опробовал на днях, вполне получается, хотя поначалу были сомнения - "чёрное на чёрном") - и тем же самым алгоритмом найти взаимное положение теодолита и ВИПС. Вот и всё, задача решена, выкидываем теодолит "из уравнения" - и получаем взаимное положение ВИПС и мишени, измеренное прибором из госреестра.
Всё казалось вполне очевидным, пока меня не попросили всё это строго описать в форме методики. И тут-то попытался это всё провести "вживую" и понял - алгоритм ВИПС предполагал сколько-нибудь узкое поле зрения, и при попытке применить его на теодолите, который может крутиться как угодно, начинаются проблемы. Когда-то я поставил "костыль", чтобы их решить, и этот костыль мне уже успел аукнуться, но записывать его на официальной бумаге, которую прочитают десятки человек (нормоконтроль, метрологи, военпреды, разные службы у заказчика) мне было стыдно, резко захотелось как-то обойтись без него, сделать "как положено"!
В алгоритме ВИПС предполагался "идеальный объектив", камера-обскура с точки зрения положения точек на фотоприёмной матрице. Т.е наша маленькая тонкая линзочка помещена в начало координат, (0;0;0), а фотоприёмная матрица условно располагается на плоскости x=-1. В таком случае, точечный объект, расположенный по координатам (x;y;z) спроецируется следующим образом:
То, что получилось на реальной фотоприёмной матрице, масштабируется в эти безразмерные величины, и после калибровки масштаба всё получается очень недурственно! Ну, есть ещё дисторсия на краях, но это уже дело техники, эти проценты скомпенсировать. Вполне рабочий метод.
Проблемы начинаются, если вдруг наше поле зрения приблизится к 180°, т.е объектив сможет видеть всё полупространство! Теперь лучи, идущие по нормали к оптической оси, будут бесконечно долго приближаться к плоскости x=-1, т.е чтобы реально зафиксировать всё это полупространство, нам нужна бесконечно протяжённая фотоприёмная матрица!
В ситуации с теодолитом, в теории, наше "поле зрения" будет чуточку меньше, чем 180°, как видно по картинке. Но получить "поле зрения" в 170..175° здесь вполне реально, и математика уже начинает "ломаться", ведь tg(87,5°)≈22,9, а производная и вовсе ≈525, т.е во столько раз увеличивается локальный масштаб по одной оси! То есть, если мы в действительности будем брать азимут с теодолита, но заменять его на тангенс, как будто бы там стояла плоская фотоприёмная матрица и объектив без дисторсии, алгоритму "будет казаться", что надо брать те самые точки, проецируемые ближе к "бесконечности", т.к он полагает, что пиксели на фотоприёмной матрице идут равномерно, и именно там мы получим максимально возможную точность.
На практике всё ещё хуже! Азимутальная ось теодолита (говоря простым руским языком: алидада) не имеет чётко обозначенного "нуля". Ноль может выставить оператор в любой момент, но во время наших измерений это можно сделать только ОДИН РАЗ. Нельзя сначала выставить ноль, работая по мишени, а потом, повторно, работая по ВИПС, мы тем самым потеряем "связь" между ними! И только если мы сами ТЩАТЕЛЬНО установим ноль где-то посередине между ВИПСом и мишенью (т.е измерения мишени будут давать углы близкие к 90° по азимуту, а измерения ВИПСа - близкие к -90°), мы уместимся в прокрустово ложе. Но там нет никаких ориентиров, и придумывать какие-то специальные усложнения в методике, только потому что у меня математика сбоит - неправильно это!
Когда-то я сделал костыль: уже получив все показания азимутов (вообще не беспокоясь о положении нуля), я беру среднее арифметическое для направлений на мишени, и ещё одно среднее арифметическое для направлений на ВИПС. Так сказать, "виртуально" изображаю ДВЕ узкопольные камеры, одна смотрит прямой наводкой на мишень, вторая - на ВИПС, и они повёрнуты одна относительно другой на известный угол. Потом, из всех направлений на мишень надо вычесть средний азимут, также как и из всех направлений на ВИПС, ну и под самый конец понадобится дополнительный шаг при сопоставлении координат. Аукнулся этот "костыль" тем, что я в какой-то момент утерял показания среднего азимута (программу дописывал впопыхах, нужно было отчитываться ВЧЕРА), из-за чего не смог подтвердить точность определения курса, хотя показания несколько часов собирали при разных положениях мишени. Ручным теодолитом на 8 точек наводиться, потом эти углы с точностью до секунды в компьютер вбивать из дисплейчика теодолита, то ещё удовольствие.
Для упрощения, поиграемся сначала на игрушечном видеоизмерителе, т.е работающем исключительно на плоскости, изображённой выше. Все точки у нас имеют координаты (anx; any) в системе координат, связанной с мишенью (то есть, эти координаты неизменны), а единственные два сдвига, которые могут здесь происходить - это параллельные переносы по осям X и Y, без вращения, обозначенные tx и ty, соответственно, от слова translation.
В системе координат видеоизмерителя точки будут иметь координаты
но напрямую их измерить мы не можем. Ранее мы полагали, что игрушечный видеоизмеритель получает значение
что как раз очень похоже было на работу узкопольного объектива с плоской фотоприёмной матрицей (в игрушечном измерителе, это фотоприёмная линейка).
Пока что tx, ty неизвестны, но мы знаем некоторое начальное приближение, tx0, ty0. Поэтому мы брали "виртуальную точку"
в этом месте на фотоприёмной линейке мы должны были обнаружить точку, если наше начальное приближение верно. На деле мы получим невязки:
то есть разницу между ожидаемым положением точек и их истинным (измеренным) положением. Чтобы понять, что они значат, мы берём частные производные от gn по всем параметрам сближения, в нашем случае по tx0 и ty0, т.е смотрим, как изменится точка, если эти параметры начать варьировать. Мы получали вектора (а в нормальной задаче: матрицы) Mn:
Затем мы получали ненормированную оценку сдвига, вектор 1×2 (это всегда будет вектор с количеством компонент, равным количеству параметров сближения, в нормальном измерителе это 1×6):
И наконец, чтобы её отнормировать, мы составляем матрицу J:
и ответом становится:
Это оценка сдвига, т.е насколько tx, ty отличаются от начального приближения. То есть, напоследок надо будет обновить их значения:
Это одна итерация процесса, но обычно одной итерации вполне хватает, если кадры идут довольно часто, а движения относительно неторопливы.
Теперь рассмотрим ту же задачу, но в случае теодолита
Вся разница, что вместо проекции на плоскость, мы, по сути, делаем проекцию на окружность:
Функция atan2 имеется практически во всех математических библиотеках, а по смыслу она эквивалентна аргументу комплексного числа, составленного из этих двух координат, принимая значения от -π до π.
В первом и четвёртом квадрантах (т.е при x>0), atan2 выражается через обычный арктангенс:
отсюда и пошло его название и странный порядок аргументов, с игреком идущим первым! Разница в том, что atan2 "не сходит с ума", когда x приближается к нулю и достигает его, ну и во втором и третьем квадранте даёт другие значения. По большому счёту, эта функция составлена из нескольких "ветвей", которые аккуратно "склеены" между собой. При грамотной (численно устойчивой) реализации разделение проходит по прямым y=x и y=-x, так что когда |y|>|x|, дробь y/x заменяется на x/y. Мы для QuatCore делали интересный вариант, atan1, у которого два аргумента, как у atan2, но ВНЕЗАПНО от одновременной смены знаков x,y (т.е с переключением между первым и третьим, либо между вторым и четвертым квадрантом) результат измениться НЕ ДОЛЖЕН. Подобное очень удобно для преобразования кватернионов в "обычные углы", чтобы при домножении ответа на 2, получить результат от -π до +π, тогда как обычный atan2 дал бы от -2π до +2π, т.е двойное перекрытие, которое потом нужно убирать. Но это мы отвлеклись.
В принципе, задача вполне решаема в лоб. Точно так же мы теперь для каждой точки находим её "виртуальную проекцию",
и невязку:
Далее мы должны взять частные производные gn по tx0 и ty0, составить матрицы частных производных Mn (в нашем случае, это вектора), и далее по накатанной.
Но повсюду лезут костыли.
Когда мы начнём считать невязку, можем, имея очень неплохое начальное приближение, внезапно получить разницу в 2π либо -2π, поскольку оба направления будут вблизи "линии разрыва", где atan2 скачкообразно переходит от +π на -π. Понятно, ничего фатального в этом нет, можно написать проверку на данный случай и при необходимости прибавить либо вычесть 2π.
На деле есть ещё одна подлянка: это "наш" atan2, когда мы виртуально проецируем точку на единичкую окружность, даёт результат от -π до +π. А теодолит, по крайней мере тот, что сейчас у меня, выдаёт азимуты от 0° до 360°, так что надо ещё эту неувязочку убирать. т.е для углов 180°..360° вычесть 360°.
Затем начинается веселье со взятием производной от atan2. Зачастую его приводят в таком виде:
(последний вариант - "кранты", можно хоть прерывание запускать в таком случае)
Здесь не вполне очевидно, как себя поведёт производная при x=0, смогут ли правильно "склеиться" три выражения, для x=0, x>0 и x<0? Нужно вспоминать первый курс матана и аккуратно считать эту производную чуть ли не по определению. А когда ответ скачкообразно меняется от -π к π (или назад), производная не должна существовать, хотя значение производной "слева" и "справа" одинаковое.
Есть определение atan2 получше, и более соответствующее тому, как оно реализуется в действительности:
(завтра вечером здесь будет рисунок, нарисовал но не сохранил на флэшку)
Два выражения слева и справа, в теории, работали бы вплоть до x=0, поэтому изначально мы только этот случай и рассматривали особо. Но на практике, "приближаться к бесконечности" идея сомнительная, поэтому мы два частных случая "малого икса" расширяем в целые квадранты. Зато теперь у нас аргумент арктангенса будет принимать значения от 0 до 1, и такой арктангенс реализовать уже довольно легко.
Для нас важно, что теперь в месте "склейки" на x=0 никаких бесконечностей заведомо не возникает, мы можем взять производную каждой функции и убедиться, что при y=x или y=-x (где нужно от одной переходить к другой) производные совпадают, а именно:
Но всё ещё остаётся сомнительный с точки зрения матана переход через ось X, при x<0, где ответ скачкообразно меняется на 2π. Можно это обосновать тем, что мы как раз-таки "ручками" будем исправлять одно из значений, чтобы скачка не было, а как только ступенька уходит, производная наконец-то начинает существовать, ровно такая, как указано выше...
В общем, с производной костыли именно в плане обоснования, так-то оно худо-бедно заработает, но всё равно неприятно...
Костыли можно все убрать!
Для этого НЕ БУДЕМ ТОРОПИТЬСЯ БРАТЬ ЭТОТ АРКТАНГЕНС. Мы просто запоминаем про себя, что нас интересует угол, но не интересует длина вектора (bnx, bny). Угол, полученный с теодолита, мы сразу же преобразуем в синус и косинус, т.е вместо fn мы получаем пары чисел (cn, sn). Ещё удобнее будет задействовать комплексные числа. Точку (bnx, bny) представим в виде:
а показания теодолита:
Тогда "комплексную" невязку можно найти так:
Мы ожидаем, что начальное приближение довольно неплохое, так что направления почти совпадают, и при данном умножении получим комплексное число с очень малым аргументом, так что имеем полное право найти его таким способом:
это и будет искомая невязка, причём мы избежали всевозможной возни с прибавлением/вычитанием 360°, с изучением "диапазона" теодолита (0°..360°) и "диапазона" atan2 (-180°..180°), довольно изящно.
Мы можем и производной atan2 тогда вообще не касаться. Вместо этого применим производную по направлению. Мы говорим, что если один из параметров сближения варьируется, то точка (bnx, bny) куда-то поползёт. Но нам плевать, если она поползёт вдоль радиуса, нам важно понять, насколько она двинется ПОПЕРЁК, то есть по касательной. Касательную построить очень легко, это вектор (-bny, bnx).
И построим вектор частных производных по одному из параметров сближения, по tx0 например:
(да, эти два параметра сближения очень просты, не добавили мы пока поворота)
Найти скалярное произведение с вектором касательной не составляет труда, получим попросту -bny = -any-ty0.
Но вектор касательной не был единичным, поэтому вместо честной производной по направлению мы получили результат, завышенный на длину вектора (-bny, bnx). Кроме того, мы не выполнили проекции исходного вектора на единичную окружность (то есть, тупо, его нормировки, приведения к единичной длине), для чего нужно снова поделить на длину исходного вектора (bnx, bny). Их длина, очевидно, одинакова, так что получим в знаменателе квадрат длины. И, о чудо, мы получаем результат:
Похожие выкладки для частной производной по ty0 дадут:
Это тот же результат, как и при использовании atan2, но нам не понадобилось влезать в тонкости его реализации, рассматривать, как он "сшивается" по квадрантам. И тут уж нет никаких сомнений: при вычислении mn1 и mn2 у нас не может выползти бесконечностей, делений на ноль, ступенек, некорректной проекции "не с той стороны", тут всё чётко. Единственный способ обратить знаменатель в ноль - это расположить мишень аккурат в центр координат измерителя (в нашем случае теодолита), но это очень экзотическая ситуация, хоть и представимая. [но, чёрт возьми, КАК?]можно построить такой объектив, передняя главная точка которого будет "висеть в воздухе" перед объективом, т.е в неё теоретически можно поместить мишень, и вот этот шаг поломает наш алгоритм! Пока ничего такого не ожидается, такой алгоритм с "круговым обзором" нам нужен только для теодолита, а там началом координат является пересечение азимутальной и угломестной оси, в эту точку ни одна мишень заведомо не попадёт.
Если далеко оттащить мишень по оси X, задача сведётся к предыдущей!
То есть, если принять bnx >> bny, мы можем упростить mn1 и mn2:
Это хороший признак.
А вот если не делать никаких предположений, мы замечаем приятную симметрию между осями X и Y, которая раньше напрочь отсутствовала! Такой системе действительно не важно, с какой стороны будет приближаться мишень - хоть спереди, хоть сзади, хоть сбоку. И да, для теодолита, у которого направление "вперёд" попросту отсутствует, это очень хорошо.
Пример работы
Мишень состоит из двух точек, имеющих координаты A1(0;1) и A2(0;-1) в системе координат мишени. Она отодвинута от теодолита на вектор (10;2), т.е на 10 единиц "вправо" и на 2 единицы "вверх". То есть, в системе координат теодолита точки имеют координаты B1(10;3) и B2(10;1). Но мы об этом пока не знаем, мы дальномером (а то и рулеткой) измерили примерное расстояние, 10, поэтому ввели начальное приближение t0(10;0).
Теодолит показал нам азимуты f1=16°41'57'' для первой точки и f2=5°42'38'' для второй. [И ещё одна подлянка...]Теодолит отсчитывает азимут по часовой стрелке, если смотреть на него сверху. Мы привыкли в задачах на плоскости отсчитывать углы против часовой... Так что будем считать, что f1 и f2 - это уже с обращённым направлением. Ещё один повод поскорее переходить к синусу и косинусу...
Мы преобразуем это в комплексные числа:
Также в виде комплексных чисел запишем начальные приближения к положению точек:
Посчитаем комплексные невязки:
Значения очень похожие, потому что выглядит всё, ПРЕИМУЩЕСТВЕННО, будто мы просто чуть ошиблись с углом, всё сдвинуто на одно значение.
Найдём невязку "в радианах":
Если бы мы работали напрямую в углах (взяли исходные углы теодолита, а ожидаемые углы нашли бы через арктангенс), мы бы получили 0,1917 и 0,1993, ошибка составляет 1,2% и 1,3%, соответственно. Для такого метода это крохи, мы всё ж начали с очень кривого приближения, аж 11°. Если захочется, сделаем ещё вторую итерацию.
Теперь находим "матрицы" частных производных, M1 и M2, хотя в нашем случае это вектора 1×2:
Имеем более сильную реакцию на "поперечный" сдвиг (по оси Y) и в 10 раз более слабую на "продольный", что, в общем, логично.
Найдём ненормированную оценку сдвига:
Найдём нормирующую матрицу:
Из-за "симметричного" расположения мишени при начальном приближении, матрица получилась диагональной. Обратная к ней:
Наконец, находим нормированную оценку сдвига:
Смещение по оси Y нашли очень точно, правильный ответ 2 ровно. А вот по оси X должен был получиться строго нолик, но не склалось. Новая оценка положения мишени:
Можно провести вторую итерацию. Обновляем начальные приближения к положению точек:
Посчитаем комплексные невязки:
Уже видно, что невязки упали более чем в 20 раз в сравнении с первой итерацией.
Найдём невязку "в радианах":
В этот раз отличие от "честного вычисления с арктангенсами" составляет 0,004% и 0,0005% для первой и второй точки соответственно.
Повторно находим "матрицы" частных производных:
Найдём ненормированную оценку сдвига:
Знак сдвига по оси X правильный, мы переоценили эту координату аж на 0,40 единицы. По оси Y вообще сдвиг должен был быть нулевым, но у нас сильная перекрёстная связь, так что не торопимся с выводами...
Найдём нормирующую матрицу:
И обратим её:
Находим нормированную оценку сдвига:
Внеся данную коррекцию, получаем ответ на второй итерации:
Мы явно стягиваемся к правильному ответу, что не может не радовать. Измерение дальности, как водится, "зашумлено" даже при абсолютно не шумящих данных, в данном случае небольшими ошибками при линеаризации нашей нелинейной задачи. Ничего страшного, данный алгоритм будет исполняться на персональном компьютере, можно работать с 80-битной плавающей точкой, и если так уж захочется, сделать ТРИ, а то и ЧЕТЫРЕ итерации (вот ужас-то какой!), чтобы выйти уже на точность, которую может обеспечить теодолит.
На этом пока всё. Мы ещё не попробовали измерить положение "изделия" относительно теодолита, там как раз интересный случай со ступенькой 2π. Но чуть позже.