Способ оценивания по реперным точкам угловых и пространственных координат объекта в оптико-электронной системе позиционирования

Изобретение относится к способам оптического определения положения и ориентации объекта при помощи оптического устройства и определения угловых направлений на жестко закрепленные на объекте светоизлучающие или светоотражающие метки - маркеры.

Из уровня техники известен способ локации источника излучения (патент RU 2275652, опубликовано 27.04.2006, МПК: G01S 5/16), включающий определение углов-пеленгов источников излучения, которое осуществляют с помощью двух оптико-локационных блоков, каждый из которых содержит сканирующее зеркало, объектив и фотоприемник. В качестве точки отсчета углов-пеленгов используют точку, расположенную на пересечении сканирующего зеркала и оптической оси объектива. При определении углов-пеленгов определяют смещение точек отсчета углов-пеленгов за счет сканирования и определяют изменение расстояния между оптико-локационными блоками, в качестве которого принимают расстояние между точками отсчета углов-пеленгов в оптико-локационных блоках, которое используют при определении координат источников излучения. Источниками излучения выступают реперные излучатели, размещаемые на базовом неподвижном основании, относительно которого производится определение ориентации подвижного объекта. Оптико-локационные блоки определяют направления на отдельные реперные излучатели (углы-пеленги), используя которые и определяют ориентацию подвижных объектов.

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

Известна система целеуказания (патент RU 2216705, опубликовано 20.11.2003, МПК: F41G 3/22, G01B 11/26, G01S 5/16) для шлема пилота, содержащая размещенные на защитном шлеме летчика элементы реперных точек, формирующие данные ориентации защитного шлема, и оптический блок индикации с оптическим узлом вывода изображения индикации в поле зрения летчика, а также размещенные вне шлема и соединенные последовательно средство оптической локации и блок формирования сигналов целеуказания. В данной системе элементы реперных точек, не менее трех, размещены в единой конструктивной сборке с оптическим блоком индикации в плоскости, нормаль к которой ориентирована под известными углами к вектору ориентации шлема и оптическим осям оптического блока индикации, а фотоприемные узлы средства оптической локации размещены в зоне, расположенной вблизи оси симметрии пространственной фигуры, сформированной зоной обзора оператора, например летчика, при изменении положения шлема в процессе функционирования системы. К недостатку системы также следует отнести необходимость применения как минимум двух фотоприемных узлов, вместе образующих стереопару, и как минимум трех реперных излучателей. Показано, что работоспособность системы может быть сохранена, если при априорно известной конфигурации реперов одним из фотоприемных узлов наблюдается всего два репера (статья Глущенко В.Т. Развитие оптико-электронных систем позиционирования нашлемных систем целеуказания и индикации // Оптический журнал. 2012. Т. 79, №12. С. 56-61).

Уменьшить количество оптических блоков позволяет способ определения ориентации и положения объекта в пространстве (патент RU 2431863, опубликовано 20.10.2011, МПК: G01S 5/16). В нем определение положения и ориентации некоторого объекта выполняется при помощи оптического устройства, содержащего по меньшей мере один параллелограмм, жестко связанный с упомянутым объектом, причем оптическое устройство имеет в своем составе оптические средства и электронные средства анализа, дающие возможность определить координаты четырех вершин параллелограмма A'B'C'D' в ортонормированной системе координат с центром в точке О. Принцип действия состоит в определении вершин параллелограмма A'B'C'D' на основе априорного знания характеристик параллелограмма и четырех известных точек четырехугольника ABCD. Последний представляет собой проекцию параллелограмма A'B'C'D' в известную плоскость изображения. Характеристиками параллелограмма A'B'C'D' могут быть, например, его высота, его ширина и координаты одной из его точек. В четырех вершинах параллелограмма A'B'C'D' располагаются реперные излучатели. Совокупность реперных излучателей принято называть кластером. В описании патента также отмечается, что в случае применения в области авиации эти кластеры, размещаемые на шлеме, обычно не располагаются в одной плоскости и представляют собой размещенные на шлеме тетраэдры.

Согласно описанию способа, он предназначен для реализации системы позиционирования шлема пилота, при этом вычисление угловых и пространственных координат шлема выполняется путем решения задачи Perspective-4-Point, Р4Р.

К недостатку способа можно отнести ограничения на линейные и угловые перемещения объекта, поскольку для позиционирования необходимо постоянное наблюдение оптическим устройством всех четырех реперных излучателей. Вторым недостатком является высокая ошибка позиционирования в тех случаях, когда проекция четырех реперных излучателей кластера на плоскость изображения оптического устройства такова, что как минимум две проекции реперов находятся на малом расстоянии друг от друга. При этом влияние ошибок оценивания пиксельных координат центров реперов на погрешность позиционирования резко возрастает. Здесь можно провести аналогию с геометрическим фактором рабочего созвездия спутников при решении задачи позиционирования в сетевых спутниковых радионавигационных системах (ССРНС) - коэффициентом, характеризующим влияние взаимного расположения спутников на погрешность вычисляемого по измеряемым параметрам местоположения (Перов А.И., Харисов В.Н. ГЛОНАСС. Принципы построения и функционирования: 4-е изд. М.: Радиотехника, 2010. 800 с.). Данный коэффициент вычисляется по векторам направляющих косинусов для четверки спутников. Применительно к задаче Р4Р под геометрическим фактором по аналогии будем понимать коэффициент, характеризующий влияние угловых координат четверки реперов в системе координат камеры на погрешность позиционирования объекта.

Альтернативное решение по уменьшению количества оптических блоков рассмотрено в способе определения углового и пространственного положения, реализуемого системой позиционирования шлема пилота (патент CN 101762262 В, опубликован 22.06.2011, МПК: G01C 1/00 (2006.01), G01C 11/00 (2006.01)), по совокупности признаков выбранного в качестве прототипа. Способ предполагает размещение на шлеме пилота как минимум двух кластеров, размещаемых на противоположных (левой и правой) сторонах шлема, каждый из которых состоит из четырех точечных реперных излучателей (реперов) с априорно известными координатами. Их размещают таким образом, что три излучателя образуют равносторонний треугольник, а четвертый излучатель лежит на нормали к плоскости треугольника, восстановленной из его центра тяжести, т.е. точечные излучатели (реперы) кластера не являются компланарными и располагаются в вершинах тетраэдра. Оптический блок (видеокамера) закрепляется сзади относительно шлема и при проецировании в плоскость ее изображения четырех точечных излучателей одного из кластеров отправляет сформированный кадр на вычислитель для обработки с целью определения угловых и пространственных координат кластера, основанного на решении задачи Р4Р.

Как и в патенте RU 2431863, недостатками способа прототипа являются ограничения на линейные и угловые перемещения объекта и высокая ошибка позиционирования при большом значении геометрического фактора четверки точечных излучателей кластера.

Из уровня техники известно, что расширить диапазон линейных и угловых перемещений позиционируемого с помощью оптико-электронной системы объекта можно посредством дополнительной инерциальной системы позиционирования (патент US 7000469 В2, опубликовано 21.01.2006, МПК G01P 15/00 (2006.01); заявка GB 2543917 A, опубликовано 03.05.2017, МПК G01C 21/165, G02B 27/017, G06F 3/011). Инерциальная система позиционирования при этом является основной, а ее показания периодически корректируются по данным от оптико-электронной системы позиционирования с применением, например, фильтра Калмана.

В описании системы позиционирования (патент US 7000469 В2, опубликовано 21.01.2006, МПК G01P 15/00 (2006.01)) в явном виде указано, что оптико-электронная подсистема позиционирования состоит из как минимум одного оптического сенсора и кластера из как минимум трех реперов, излучающих в ультрафиолетовом диапазоне длин волн, а в описании системы позиционирования (заявка GB 2543917 A, опубликовано 03.05.2017, МПК G01C 21/165, G02B 27/017, G06F 3/011) - что оптико-электронная подсистема позиционирования состоит из одного или нескольких оптических сенсоров, в поле зрения которых попадает по крайней мере несколько реперов. При этом алгоритма решения задачи позиционирования по изображениям реперов с помощью оптико-электронной подсистемы ни в одном из двух вышеуказанных изобретений не приводится.

Техническая проблема, решаемая заявляемым изобретением, заключается в отсутствии способа решения задачи позиционирования по информации от единственного оптического устройства (камеры), инерциальной подсистемы позиционирования и кластеру из n=4 реперов, адаптивного к конфигурации проекций реперов в плоскости изображения, в том числе при их частичном закрытии от камеры, при котором дополнительная инерциальная подсистема позиционирования не является основной. В известных изобретениях оптико-электронная система позиционирования является вспомогательной и применяется для коррекции дрейфа сигналов датчиков угловой скорости инерциальной системы.

Технический результат изобретения заключается в разработке алгоритма адаптивного выбора способа решения задачи позиционирования объекта с размещенными на нем минимум n=4 реперами с априорно известными в системе координат объекта координатами с помощью единственной камеры.

Адаптация заключается в том, что в зависимости от количества наблюдаемых реперов кластера 2≤n≤4 и оценки геометрического фактора задача определения угловых и пространственных координат объекта решается либо с применением только оптических средств, либо с применением как оптических средств, так и вспомогательной инерциальной подсистемы позиционирования.

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

- если наблюдаются n=4 репера кластера, то анализируется характеризующий величину геометрического фактора логический показатель А₄, который основан на сравнении минимального углового расстояния между направляющими векторами прямых, проведенных из центра ортонормированной системы координат в центры проекций реперов на плоскость изображения, с пороговым значением p_max, при этом если А₄=1, выполняется поиск решения задачи Р4Р по методу EPnP или RPnP с последующим уточнением по численному алгоритму минимизации ошибки репроекции, а если А₄=0, то выполняется переход к алгоритму решения задачи Р3Р;

- если наблюдаются n=3 репера кластера, либо А₄=0, то аналогичным образом анализируется логический показатель А₃, при этом если А₃=1, выполняется поиск решений задачи Р3Р с последующим уточнением по численному алгоритму минимизации ошибки репроекции, при этом выбор рабочей тройки реперов выполняется либо по критерию максимальной площади треугольника, образованного проекциями тройки реперов на плоскость изображения, либо по критерию минимума суммы углов между нормалями к плоскостям реперов тройки и направлением на центр вышеупомянутой ортонормированной системы координат, а если А₃=0, выполняется переход к алгоритму решения задачи Р2Р;

- если наблюдаются n=2 репера кластера, либо А₄=A₃=0, то аналогичным образом анализируется логический показатель А₂, при этом если А₂=1, выполняется поиск решения задачи Р2Р по информации о крене и тангаже от вспомогательной инерциальной подсистемы позиционирования, размещаемой на камере или объекте позиционирования, при этом выбор рабочей пары реперов выполняется по критерию максимума углового расстояния между направляющими векторами прямых, проведенных из центра ортонормированной системы координат в центры проекций реперов на плоскость изображения, а если А₂=0, то выдается признак невозможности определения угловой координаты курса и пространственных координат объекта с реперными излучателями путем решения задачи PnP;

- если наблюдается n<2 реперов кластера, то выдается признак невозможности определения угловой координаты курса и пространственных координат объекта с реперными излучателями путем решения задачи PnP.

Предположим, что, как и в прототипе, реперы кластера располагаются в вершинах тетраэдра (фиг. 1) и их координаты в системе координат объекта позиционирования априорно известны. На фиг. 1 приняты следующие обозначения: M_i=[X_i,Y_i,Z_i]^T - пространственные координаты реперов, m_i=[u_i,ν_i]^T - пиксельные координаты их проекций в плоскости изображения, ƒ - фокусное расстояние объектива камеры, - знак транспонирования, .

Идентификация проекций реперов, т.е. нахождение соответствия между номером репера и его проекцией на плоскость изображения, выполняется известным образом (например, по аналогии с «Зейналов Р.Ш., Якубенко А.А., Конушин А.С. Оценка траектории движения объекта с использованием инфракрасных маркеров // Цифровая обработка сигналов и ее применение: материалы 14-й междунар. конф. М.: ИПУ РАН, 2012. Т. 2. С. 267-271.» или «Faessler М., Mueggler Е., Schwabe K., Scaramuzza D. A Monocular Pose Estimation System based on Infrared LEDs // IEEE Int. Conference on Robotics and Automation (ICRA). Hong Kong, 2014. P. 907-913»).

Рассмотрим различные конфигурации наблюдаемых камерой проекций реперов в зависимости от угловой и пространственной ориентации кластера.

Если при текущем пространственном и угловом положении кластера {M_i}, , расстояния между проекциями реперов m_i, , в плоскости изображения приблизительно одинаковы (фиг. 1), то задача определения пространственных координат реперов в системе координат камеры решается применением одного из известных алгоритмов решения задачи Р4Р - EPnP (Lepetit V., Moreno-Noguer F., Fua P. An Accurate O(n) Solution to the PnP Problem // International Journal of Computer Vision. 2009. Vol. 81, No. 2. P. 155-166) или RPnP (Li S., Xu C., Xie M. A Robust O(n) Solution to the Perspective-n-Point Problem // IEEE Trans. on Pattern Analysis and Machine Intelligence. 2012. Vol. 34, Is. 7. P. 1444-1450) с последующей оптимизацией с применением численного алгоритма минимизации ошибки репроекции, например, Гаусса-Ньютона или Левенберга-Марквардта (Lu С.-Р., Hager G.D., Mjolsness Е. Fast and globally convergent pose estimation from video images // IEEE Transactions on Pattern Analysis and Machine Intelligence. 2000. Vol. 22, No. 6. P. 610-622).

Если пространственная ориентация реперов кластера такова, что расстояние между проекциями хотя бы одной пары реперов много меньше, чем расстояния между каждым из них и проекциями остальных реперов (фиг. 2), то ошибка решения задачи Р4Р методами EPnP или RPnP, даже с последующей численной оптимизацией, резко возрастает из-за большого геометрического фактора.

В теории ССРНС геометрический фактор Г рассчитывают по формуле

Г={tr[(HH^T)^-1]}^0,5,

где tr(⋅) - операция вычисления следа матрицы, Н - матрица направляющих косинусов,

α_i, β_i, γ_i - соответственно углы между осями X, Y, Z системы координат камеры и линией визирования i-го репера.

Показателем плохого (большого) геометрического фактора Г является близость к нулю угла ϕ_ij между векторами, соединяющими проекции реперов с номерами i и j, , с главной точкой камеры. Данный критерий эквивалентен близости к единице скалярного произведения векторов и , где - однородные пиксельные координаты проекции i-го репера, а ||⋅|| - знак два-нормы.

Таким образом, критерий принятия решения А о значении геометрического фактора (А=1 - геометрический фактор хороший, А=0 - плохой) и, следовательно, о выборе алгоритма Р4Р можно записать в виде:

где i≠j, порог

a ϕ_min - минимальное значение угла между линиями визирования соседних реперов, при котором при заданной, нормированной к фокусному расстоянию камеры, среднеквадратической ошибке (СКО) оценивания пиксельных координат центров их проекций σ_н в результате решения задачи Р4Р обеспечивается допустимая ошибка оценивания угловых и пространственных координат объекта. Для известной конфигурации реперных кластеров зависимость ϕ_ij(σ_н) может быть получена, например, в результате имитационного моделирования решения задачи PnP в заданном рабочем объеме.

В случае А=0 осуществляется переход к решению задачи Р3Р - оценке пространственных и угловых координат реперов по информации от одной камеры, наблюдающей n=3 репера с априорно известными в системе координат объекта координатами. При этом выбор рабочего треугольника в области изображения может осуществляться либо по критерию максимальной площади (Алпатов Б.А., Балашов О.Е., Степашкин А.И. // Вестник Рязанского государственного радиотехнического университета. 2013. №3 (45). С. 19-23), либо по критерию минимума суммы углов между нормалями к плоскостям тройки рабочих реперов и направлением на главную точку камеры (Павлов О.В., Холопов И.С. Сравнительный анализ двух алгоритмов выбора реперного светодиодного кластера в задаче навигации по оптическим меткам // Вестник Рязанского государственного радиотехнического университета. 2017. №61. С. 13-18). Например, при выборе тройки реперов по критерию максимальной площади рабочего треугольника в примере на фиг. 2 в качестве рабочих будут выбраны реперы с номерами 2, 3 и 4.

Задача Р3Р в общем случае имеет 4 решения (Haralick R.M., Lee C.-N., Ottenberg K., Nolle M. Review and Analysis of Solutions of the Three Point Perspective Pose Estimation Problem // International Journal of Computer Vision. 1994. Vol. 13, No. 3. P. 331-356.). Как правило, два решения соответствуют положительным значениям глубины реперов в системе координат камеры, и еще два - отрицательным. По этой причине последние два решения игнорируются.

Выбор истинного решения задачи Р3Р из первых двух может быть основан на ограничениях на возможные значения координат реперов в системе координат камеры. Например, в нашлемных авиационных системах целеуказания такими ограничениями выступают геометрические размеры кабины. Если же ограничения отсутствуют либо оба решения задачи Р3Р им удовлетворяют, то выбор номера решения задачи Р3Р j при переходе от Р4Р к Р3Р, а также при последующих решениях Р3Р, основывается на критерии минимума суммы квадратов расстояний между вычисленными (текущими) координатами реперов и их предыдущими значениями , i=1, 2, 3:

Переход к решению Р3Р также осуществляется в том случае, если в поле зрения камеры остаются только n=3 репера (фиг. 3).

Если в поле зрения камеры попадают всего два реперных излучателя (задача Р2Р, фиг. 4), то определение угловой и пространственной ориентации (три угловых и три пространственных координаты кластера реперов - всего 6 неизвестных) по информации только от оптико-электронной системы позиционирования невозможно. В то же время известно (Kniaz V.V. Robust vision-based pose estimation algorithm for an UAV with known gravity vector // The International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences. Volume XLI-B5. Prague, 2016. P. 63-68; Kukelova Z., Bujnak M., Pajdla T. Closed-form solutions to the minimal absolute pose problems with known vertical direction // ACCV. 2010. Vol. 2. P. 216-229), что при наличии оценок углов крена ϕ и тангажа θ либо камеры, либо кластера, оставшиеся 4 неизвестных (курс ψ и три пространственных координаты) могут быть найдены путем решения задачи Р2Р. Такое решение сводится к полиному 2-го порядка относительно переменной q=tg(0,5ψ). Вычисляемые корни данного полинома дают два значения курса:

ψ₁=α,

ψ₂=α-sign(α)⋅180°,

где -90°≤а<90°, a sign(α) - функция знака:

Решение с курсом ψ₂ исключается, поскольку, как правило, объект позиционирования на рабочей длине волны реперов непрозрачен, а ширина диаграммы светимости 2Δ_α, например, светодиодных реперов, на практике ограничена значениями 120°…140°.

Алгоритм решения Р2Р выбирается также в том случае, если при решении задач Р3Р и Р4Р для геометрического фактора вычислен логический признак А=0 (фиг. 5). При этом рабочая двойка реперов с номерами i и j из наблюдаемых n≤4 реперов выбирается по критерию максимума угла между направляющими векторами прямых v_i и v_j, i≠j, что эквивалентно критерию минимума их скалярного произведения:

По критерию (4) на фиг. 5 в качестве рабочих будут выбраны реперы с номерами 3 и 4.

Оценки крена и тангажа, необходимые для решения задачи Р2Р, могут быть получены по информации от подсистемы инерциальной навигации на основе трехосных микроэлектромеханических сенсоров (MEMS), дополнительно устанавливаемых либо на камеру, либо на объект позиционирования. Взаимная калибровка по определению углового рассогласования между системами координат сенсоров подсистемы инерциальной навигации и системой координат камеры либо объекта позиционирования может быть выполнена известным образом (например, по аналогии со способом калибровки, описанном в патенте US 10061382 В2, опубликован 28.08.2018, МПК: G06F 3/01 (2006.01), G06T 7/73 (2017.01), G02B 27/01 (2006.01)).

Если при решении задачи Р2Р для пары наблюдаемых реперов вычислен логический признак А=0, т.е. выполняется неравенство

v_i⋅v_j≥p_max,

то принимается решение о невозможности оценки трех пространственных и трех угловых координат объекта позиционирования.

Схема алгоритма оценивания угловых и пространственных координат в оптико-электронной системе позиционирования по n≤4 реперным точкам приведена на фиг. 6, где приняты следующие обозначения:

- Р4Р+LM(GN) - решение задачи Р4Р с последующей оптимизацией по алгоритму Левенберга-Марквардта или Гаусса-Ньютона;

- Р3Р+LM(GN) - решение задачи Р3Р с последующей оптимизацией по алгоритму Левенберга-Марквардта или Гаусса-Ньютона;

- Р2Р - решение задачи Р2Р;

- A_k - вычисление логического признака А по (1) при выбранной в качестве рабочей совокупности из k=2, 3 или 4 реперов для решения задач Р2Р, Р3Р и Р4Р соответственно.

Обязательным условием для решения задач PnP является компенсация дисторсии, вносимой объективом камеры.

По оцененным пространственным координатам n≥3 реперов кластера выполняется оценка параметров евклидова преобразования - параллельного переноса (смещения) и поворота, определяемых соответственно вектором t и матрице поворота R (Umeyama S. Least-Squares Estimation of Transformation Parameters Between Two Point Patterns // IEEE Trans, on Pattern Analysis and Machine Intelligence. 1991. Vol. 13, Is. 4. P. 376-380). Углы Эйлера, определяющие текущую угловую ориентацию объекта, вычисляются по элементам матрицы поворота R:

крен - ϕ=atan2(-R₂₃,R₂₂),

тангаж - θ=arcsin(R₂₁),

курс - ψ=atan2(-R₃₁, R₁₁),

кроме θ=±90°, когда

ψ=atan2(-R₁₃, R₃₃).

Для оценки параметров евклидова преобразования над реперным кластером первоначально находятся центры масс множеств и , , n≥3:

,

и центрируются координаты реперов в системе координат объекта и камеры соответственно:

Далее, используя SVD разложение матрицы

,

находят матрицу поворота

и вектор параллельного переноса

.

При n=2 оценки крена и тангажа получают по информации от инерциальной подсистемы позиционирования, а курс и пространственные координаты - непосредственно из решения задачи Р2Р.

Если система позиционирования установлена на подвижном объекте (например, система позиционирования нашлемной системы целеуказания и индикации), то показания инерциальной подсистемы позиционирования будут определяться как изменением углового положения собственно объекта позиционирования (шлема пилота), так и самого носителя НСЦИ. Поэтому в таких системах позиционирования при решении задачи Р2Р необходимо компенсировать крен и тангаж, полученные от инерциальной подсистемы позиционирования, по данным от навигационной системы подвижного объекта.